Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
Path: blob/master/ArduSub/Log.cpp
Views: 1798
#include "Sub.h"12#if HAL_LOGGING_ENABLED34// Code to Write and Read packets from AP_Logger log memory5// Code to interact with the user to dump or erase logs67struct PACKED log_Control_Tuning {8LOG_PACKET_HEADER;9uint64_t time_us;10float throttle_in;11float angle_boost;12float throttle_out;13float throttle_hover;14float desired_alt;15float inav_alt;16float baro_alt;17int16_t desired_rangefinder_alt;18int16_t rangefinder_alt;19float terr_alt;20int16_t target_climb_rate;21int16_t climb_rate;22};2324// Write a control tuning packet25void Sub::Log_Write_Control_Tuning()26{27// get terrain altitude28float terr_alt = 0.0f;29#if AP_TERRAIN_AVAILABLE30if (terrain.enabled()) {31terrain.height_above_terrain(terr_alt, true);32} else {33terr_alt = rangefinder_state.rangefinder_terrain_offset_cm * 0.01f;34}35#else36terr_alt = rangefinder_state.rangefinder_terrain_offset_cm * 0.01f;37#endif3839struct log_Control_Tuning pkt = {40LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),41time_us : AP_HAL::micros64(),42throttle_in : attitude_control.get_throttle_in(),43angle_boost : attitude_control.angle_boost(),44throttle_out : motors.get_throttle(),45throttle_hover : motors.get_throttle_hover(),46desired_alt : pos_control.get_pos_target_z_cm() * 0.01f,47inav_alt : inertial_nav.get_position_z_up_cm() * 0.01f,48baro_alt : barometer.get_altitude(),49desired_rangefinder_alt : (int16_t)mode_surftrak.get_rangefinder_target_cm(),50rangefinder_alt : rangefinder_state.alt_cm,51terr_alt : terr_alt,52target_climb_rate : (int16_t)pos_control.get_vel_target_z_cms(),53climb_rate : climb_rate54};55logger.WriteBlock(&pkt, sizeof(pkt));56}5758// Write an attitude packet59void Sub::Log_Write_Attitude()60{61ahrs.Write_Attitude(attitude_control.get_att_target_euler_rad() * RAD_TO_DEG);6263AP::ahrs().Log_Write();64}6566struct PACKED log_Data_Int16t {67LOG_PACKET_HEADER;68uint64_t time_us;69uint8_t id;70int16_t data_value;71};7273// Write an int16_t data packet74UNUSED_FUNCTION75void Sub::Log_Write_Data(LogDataID id, int16_t value)76{77if (should_log(MASK_LOG_ANY)) {78struct log_Data_Int16t pkt = {79LOG_PACKET_HEADER_INIT(LOG_DATA_INT16_MSG),80time_us : AP_HAL::micros64(),81id : (uint8_t)id,82data_value : value83};84logger.WriteCriticalBlock(&pkt, sizeof(pkt));85}86}8788struct PACKED log_Data_UInt16t {89LOG_PACKET_HEADER;90uint64_t time_us;91uint8_t id;92uint16_t data_value;93};9495// Write an uint16_t data packet96UNUSED_FUNCTION97void Sub::Log_Write_Data(LogDataID id, uint16_t value)98{99if (should_log(MASK_LOG_ANY)) {100struct log_Data_UInt16t pkt = {101LOG_PACKET_HEADER_INIT(LOG_DATA_UINT16_MSG),102time_us : AP_HAL::micros64(),103id : (uint8_t)id,104data_value : value105};106logger.WriteCriticalBlock(&pkt, sizeof(pkt));107}108}109110struct PACKED log_Data_Int32t {111LOG_PACKET_HEADER;112uint64_t time_us;113uint8_t id;114int32_t data_value;115};116117// Write an int32_t data packet118void Sub::Log_Write_Data(LogDataID id, int32_t value)119{120if (should_log(MASK_LOG_ANY)) {121struct log_Data_Int32t pkt = {122LOG_PACKET_HEADER_INIT(LOG_DATA_INT32_MSG),123time_us : AP_HAL::micros64(),124id : (uint8_t)id,125data_value : value126};127logger.WriteCriticalBlock(&pkt, sizeof(pkt));128}129}130131struct PACKED log_Data_UInt32t {132LOG_PACKET_HEADER;133uint64_t time_us;134uint8_t id;135uint32_t data_value;136};137138// Write a uint32_t data packet139void Sub::Log_Write_Data(LogDataID id, uint32_t value)140{141if (should_log(MASK_LOG_ANY)) {142struct log_Data_UInt32t pkt = {143LOG_PACKET_HEADER_INIT(LOG_DATA_UINT32_MSG),144time_us : AP_HAL::micros64(),145id : (uint8_t)id,146data_value : value147};148logger.WriteCriticalBlock(&pkt, sizeof(pkt));149}150}151152struct PACKED log_Data_Float {153LOG_PACKET_HEADER;154uint64_t time_us;155uint8_t id;156float data_value;157};158159// Write a float data packet160UNUSED_FUNCTION161void Sub::Log_Write_Data(LogDataID id, float value)162{163if (should_log(MASK_LOG_ANY)) {164struct log_Data_Float pkt = {165LOG_PACKET_HEADER_INIT(LOG_DATA_FLOAT_MSG),166time_us : AP_HAL::micros64(),167id : (uint8_t)id,168data_value : value169};170logger.WriteCriticalBlock(&pkt, sizeof(pkt));171}172}173174struct PACKED log_GuidedTarget {175LOG_PACKET_HEADER;176uint64_t time_us;177uint8_t type;178float pos_target_x;179float pos_target_y;180float pos_target_z;181float vel_target_x;182float vel_target_y;183float vel_target_z;184};185186// Write a Guided mode target187void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target)188{189struct log_GuidedTarget pkt = {190LOG_PACKET_HEADER_INIT(LOG_GUIDEDTARGET_MSG),191time_us : AP_HAL::micros64(),192type : target_type,193pos_target_x : pos_target.x,194pos_target_y : pos_target.y,195pos_target_z : pos_target.z,196vel_target_x : vel_target.x,197vel_target_y : vel_target.y,198vel_target_z : vel_target.z199};200logger.WriteBlock(&pkt, sizeof(pkt));201}202203// @LoggerMessage: CTUN204// @Description: Control Tuning information205// @Field: TimeUS: Time since system startup206// @Field: ThI: throttle input207// @Field: ABst: angle boost208// @Field: ThO: throttle output209// @Field: ThH: calculated hover throttle210// @Field: DAlt: desired altitude211// @Field: Alt: achieved altitude212// @Field: BAlt: barometric altitude213// @Field: DSAlt: desired rangefinder altitude214// @Field: SAlt: achieved rangefinder altitude215// @Field: TAlt: terrain altitude216// @Field: DCRt: desired climb rate217// @Field: CRt: climb rate218219// @LoggerMessage: D16220// @Description: Generic 16-bit-signed-integer storage221// @Field: TimeUS: Time since system startup222// @Field: Id: Data type identifier223// @Field: Value: Value224225// @LoggerMessage: D32226// @Description: Generic 32-bit-signed-integer storage227// @Field: TimeUS: Time since system startup228// @Field: Id: Data type identifier229// @Field: Value: Value230231// @LoggerMessage: DFLT232// @Description: Generic float storage233// @Field: TimeUS: Time since system startup234// @Field: Id: Data type identifier235// @Field: Value: Value236237// @LoggerMessage: DU16238// @Description: Generic 16-bit-unsigned-integer storage239// @Field: TimeUS: Time since system startup240// @Field: Id: Data type identifier241// @Field: Value: Value242243// @LoggerMessage: DU32244// @Description: Generic 32-bit-unsigned-integer storage245// @Field: TimeUS: Time since system startup246// @Field: Id: Data type identifier247// @Field: Value: Value248249// @LoggerMessage: GUIP250// @Description: Guided mode target information251// @Field: TimeUS: Time since system startup252// @Field: Type: Type of guided mode253// @Field: pX: Target position, X-Axis254// @Field: pY: Target position, Y-Axis255// @Field: pZ: Target position, Z-Axis256// @Field: vX: Target velocity, X-Axis257// @Field: vY: Target velocity, Y-Axis258// @Field: vZ: Target velocity, Z-Axis259260// type and unit information can be found in261// libraries/AP_Logger/Logstructure.h; search for "log_Units" for262// units and "Format characters" for field type information263const struct LogStructure Sub::log_structure[] = {264LOG_COMMON_STRUCTURES,265{ LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),266"CTUN", "Qfffffffccfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00BBBBBB" },267{ LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t),268"D16", "QBh", "TimeUS,Id,Value", "s--", "F--" },269{ LOG_DATA_UINT16_MSG, sizeof(log_Data_UInt16t),270"DU16", "QBH", "TimeUS,Id,Value", "s--", "F--" },271{ LOG_DATA_INT32_MSG, sizeof(log_Data_Int32t),272"D32", "QBi", "TimeUS,Id,Value", "s--", "F--" },273{ LOG_DATA_UINT32_MSG, sizeof(log_Data_UInt32t),274"DU32", "QBI", "TimeUS,Id,Value", "s--", "F--" },275{ LOG_DATA_FLOAT_MSG, sizeof(log_Data_Float),276"DFLT", "QBf", "TimeUS,Id,Value", "s--", "F--" },277{ LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),278"GUIP", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ", "s-mmmnnn", "F-000000" },279};280281uint8_t Sub::get_num_log_structures() const282{283return ARRAY_SIZE(log_structure);284}285286void Sub::Log_Write_Vehicle_Startup_Messages()287{288// only 200(?) bytes are guaranteed by AP_Logger289logger.Write_Mode((uint8_t)control_mode, control_mode_reason);290ahrs.Log_Write_Home_And_Origin();291gps.Write_AP_Logger_Log_Startup_messages();292}293294295#endif // HAL_LOGGING_ENABLED296297298