diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index efd15d6f2a..ca49cb3ff2 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -311,8 +311,6 @@ public: void Write_Rally(); void Write_Baro(); void Write_Power(void); - void Write_AHRS2(); - void Write_POS(); void Write_Radio(const mavlink_radio_t &packet); void Write_Message(const char *message); void Write_MessageF(const char *fmt, ...); @@ -322,8 +320,6 @@ public: void Write_ESC(uint8_t id, uint64_t time_us, int32_t rpm, uint16_t voltage, uint16_t current, int16_t esc_temp, uint16_t current_tot, int16_t motor_temp, float error_rate = 0.0f); void Write_ServoStatus(uint64_t time_us, uint8_t id, float position, float force, float speed, uint8_t power_pct); void Write_ESCStatus(uint64_t time_us, uint8_t id, uint32_t error_count, float voltage, float current, float temperature, int32_t rpm, uint8_t power_pct); - void Write_Attitude(const Vector3f &targets); - void Write_AttitudeView(AP_AHRS_View &ahrs, const Vector3f &targets); void Write_Current(); void Write_Compass(); void Write_Mode(uint8_t mode, const ModeReason reason); @@ -332,19 +328,13 @@ public: void Write_Command(const mavlink_command_int_t &packet, MAV_RESULT result, bool was_command_long=false); void Write_Mission_Cmd(const AP_Mission &mission, const AP_Mission::Mission_Command &cmd); - void Write_Origin(uint8_t origin_type, const Location &loc); void Write_RPM(const AP_RPM &rpm_sensor); - void Write_Rate(const AP_AHRS_View *ahrs, - const AP_Motors &motors, - const AC_AttitudeControl &attitude_control, - const AC_PosControl &pos_control); void Write_RallyPoint(uint8_t total, uint8_t sequence, const RallyLocation &rally_point); void Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence); void Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter, bool ignored); void Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, float vel_err, uint8_t reset_counter, bool ignored); - void Write_AOA_SSA(AP_AHRS &ahrs); void Write_Beacon(AP_Beacon &beacon); void Write_Proximity(AP_Proximity &proximity); void Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& point); diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 5ea17a010d..e0c0d03167 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -7,9 +7,6 @@ #include #include #include -#include -#include -#include #include #include @@ -447,56 +444,6 @@ void AP_Logger::Write_Power(void) #endif } -// Write an AHRS2 packet -void AP_Logger::Write_AHRS2() -{ - const AP_AHRS &ahrs = AP::ahrs(); - Vector3f euler; - struct Location loc; - Quaternion quat; - if (!ahrs.get_secondary_attitude(euler) || !ahrs.get_secondary_position(loc) || !ahrs.get_secondary_quaternion(quat)) { - return; - } - const struct log_AHRS pkt{ - LOG_PACKET_HEADER_INIT(LOG_AHR2_MSG), - time_us : AP_HAL::micros64(), - roll : (int16_t)(degrees(euler.x)*100), - pitch : (int16_t)(degrees(euler.y)*100), - yaw : (uint16_t)(wrap_360_cd(degrees(euler.z)*100)), - alt : loc.alt*1.0e-2f, - lat : loc.lat, - lng : loc.lng, - q1 : quat.q1, - q2 : quat.q2, - q3 : quat.q3, - q4 : quat.q4, - }; - WriteBlock(&pkt, sizeof(pkt)); -} - -// Write a POS packet -void AP_Logger::Write_POS() -{ - const AP_AHRS &ahrs = AP::ahrs(); - - Location loc; - if (!ahrs.get_position(loc)) { - return; - } - float home, origin; - ahrs.get_relative_position_D_home(home); - const struct log_POS pkt{ - LOG_PACKET_HEADER_INIT(LOG_POS_MSG), - time_us : AP_HAL::micros64(), - lat : loc.lat, - lng : loc.lng, - alt : loc.alt*1.0e-2f, - rel_home_alt : -home, - rel_origin_alt : ahrs.get_relative_position_D_origin(origin) ? -origin : quiet_nanf(), - }; - WriteBlock(&pkt, sizeof(pkt)); -} - void AP_Logger::Write_Radio(const mavlink_radio_t &packet) { const struct log_Radio pkt{ @@ -562,45 +509,6 @@ void AP_Logger::Write_Trigger(const Location ¤t_loc) Write_CameraInfo(LOG_TRIGGER_MSG, current_loc, 0); } -// Write an attitude packet -void AP_Logger::Write_Attitude(const Vector3f &targets) -{ - const AP_AHRS &ahrs = AP::ahrs(); - - const struct log_Attitude pkt{ - LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG), - time_us : AP_HAL::micros64(), - control_roll : (int16_t)targets.x, - roll : (int16_t)ahrs.roll_sensor, - control_pitch : (int16_t)targets.y, - pitch : (int16_t)ahrs.pitch_sensor, - control_yaw : (uint16_t)wrap_360_cd(targets.z), - yaw : (uint16_t)wrap_360_cd(ahrs.yaw_sensor), - error_rp : (uint16_t)(ahrs.get_error_rp() * 100), - error_yaw : (uint16_t)(ahrs.get_error_yaw() * 100), - active : ahrs.get_active_AHRS_type(), - }; - WriteBlock(&pkt, sizeof(pkt)); -} - -// Write an attitude packet -void AP_Logger::Write_AttitudeView(AP_AHRS_View &ahrs, const Vector3f &targets) -{ - const struct log_Attitude pkt{ - LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG), - time_us : AP_HAL::micros64(), - control_roll : (int16_t)targets.x, - roll : (int16_t)ahrs.roll_sensor, - control_pitch : (int16_t)targets.y, - pitch : (int16_t)ahrs.pitch_sensor, - control_yaw : (uint16_t)wrap_360_cd(targets.z), - yaw : (uint16_t)wrap_360_cd(ahrs.yaw_sensor), - error_rp : (uint16_t)(ahrs.get_error_rp() * 100), - error_yaw : (uint16_t)(ahrs.get_error_yaw() * 100) - }; - WriteBlock(&pkt, sizeof(pkt)); -} - void AP_Logger::Write_Current_instance(const uint64_t time_us, const uint8_t battery_instance) { @@ -795,19 +703,6 @@ void AP_Logger::Write_PID(uint8_t msg_type, const PID_Info &info) WriteBlock(&pkt, sizeof(pkt)); } -void AP_Logger::Write_Origin(uint8_t origin_type, const Location &loc) -{ - const struct log_ORGN pkt{ - LOG_PACKET_HEADER_INIT(LOG_ORGN_MSG), - time_us : AP_HAL::micros64(), - origin_type : origin_type, - latitude : loc.lat, - longitude : loc.lng, - altitude : loc.alt - }; - WriteBlock(&pkt, sizeof(pkt)); -} - void AP_Logger::Write_RPM(const AP_RPM &rpm_sensor) { float rpm1 = -1, rpm2 = -1; @@ -824,33 +719,6 @@ void AP_Logger::Write_RPM(const AP_RPM &rpm_sensor) WriteBlock(&pkt, sizeof(pkt)); } -// Write a rate packet -void AP_Logger::Write_Rate(const AP_AHRS_View *ahrs, - const AP_Motors &motors, - const AC_AttitudeControl &attitude_control, - const AC_PosControl &pos_control) -{ - const Vector3f &rate_targets = attitude_control.rate_bf_targets(); - const Vector3f &accel_target = pos_control.get_accel_target(); - const struct log_Rate pkt_rate{ - LOG_PACKET_HEADER_INIT(LOG_RATE_MSG), - time_us : AP_HAL::micros64(), - control_roll : degrees(rate_targets.x), - roll : degrees(ahrs->get_gyro().x), - roll_out : motors.get_roll(), - control_pitch : degrees(rate_targets.y), - pitch : degrees(ahrs->get_gyro().y), - pitch_out : motors.get_pitch(), - control_yaw : degrees(rate_targets.z), - yaw : degrees(ahrs->get_gyro().z), - yaw_out : motors.get_yaw(), - control_accel : (float)accel_target.z, - accel : (float)(-(ahrs->get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f), - accel_out : motors.get_throttle() - }; - WriteBlock(&pkt_rate, sizeof(pkt_rate)); -} - // Write visual odometry sensor data void AP_Logger::Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence) { @@ -909,19 +777,6 @@ void AP_Logger::Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, WriteBlock(&pkt_visualvel, sizeof(log_VisualVelocity)); } -// Write AOA and SSA -void AP_Logger::Write_AOA_SSA(AP_AHRS &ahrs) -{ - const struct log_AOA_SSA aoa_ssa{ - LOG_PACKET_HEADER_INIT(LOG_AOA_SSA_MSG), - time_us : AP_HAL::micros64(), - AOA : ahrs.getAOA(), - SSA : ahrs.getSSA() - }; - - WriteBlock(&aoa_ssa, sizeof(aoa_ssa)); -} - // Write beacon sensor (position) data void AP_Logger::Write_Beacon(AP_Beacon &beacon) { diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 3e0ec991cb..eed0d5ed52 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -121,6 +121,8 @@ const struct MultiplierStructure log_Multipliers[] = { #include #include +#include + // structure used to define logging format struct LogStructure { uint8_t msg_type; @@ -374,28 +376,6 @@ struct PACKED log_Optflow { float body_y; }; -struct PACKED log_AHRS { - LOG_PACKET_HEADER; - uint64_t time_us; - int16_t roll; - int16_t pitch; - uint16_t yaw; - float alt; - int32_t lat; - int32_t lng; - float q1, q2, q3, q4; -}; - -struct PACKED log_POS { - LOG_PACKET_HEADER; - uint64_t time_us; - int32_t lat; - int32_t lng; - float alt; - float rel_home_alt; - float rel_origin_alt; -}; - struct PACKED log_POWR { LOG_PACKET_HEADER; uint64_t time_us; @@ -514,20 +494,6 @@ struct PACKED log_Camera { uint16_t yaw; }; -struct PACKED log_Attitude { - LOG_PACKET_HEADER; - uint64_t time_us; - int16_t control_roll; - int16_t roll; - int16_t control_pitch; - int16_t pitch; - uint16_t control_yaw; - uint16_t yaw; - uint16_t error_rp; - uint16_t error_yaw; - uint8_t active; -}; - struct PACKED log_PID { LOG_PACKET_HEADER; uint64_t time_us; @@ -791,15 +757,6 @@ struct PACKED log_MAV_Stats { // uint8_t state_retry_max; }; -struct PACKED log_ORGN { - LOG_PACKET_HEADER; - uint64_t time_us; - uint8_t origin_type; - int32_t latitude; - int32_t longitude; - int32_t altitude; -}; - struct PACKED log_RPM { LOG_PACKET_HEADER; uint64_t time_us; @@ -807,23 +764,6 @@ struct PACKED log_RPM { float rpm2; }; -struct PACKED log_Rate { - LOG_PACKET_HEADER; - uint64_t time_us; - float control_roll; - float roll; - float roll_out; - float control_pitch; - float pitch; - float pitch_out; - float control_yaw; - float yaw; - float yaw_out; - float control_accel; - float accel; - float accel_out; -}; - struct PACKED log_SbpLLH { LOG_PACKET_HEADER; uint64_t time_us; @@ -887,13 +827,6 @@ struct PACKED log_Rally { int16_t altitude; }; -struct PACKED log_AOA_SSA { - LOG_PACKET_HEADER; - uint64_t time_us; - float AOA; - float SSA; -}; - struct PACKED log_Beacon { LOG_PACKET_HEADER; uint64_t time_us; @@ -1097,20 +1030,6 @@ struct PACKED log_PSC { // @Field: Ver_vel: Vehicle vertical velocity // @Field: Squark: Transponder squawk code -// @LoggerMessage: AHR2 -// @Description: Backup AHRS data -// @Field: TimeUS: Time since system startup -// @Field: Roll: Estimated roll -// @Field: Pitch: Estimated pitch -// @Field: Yaw: Estimated yaw -// @Field: Alt: Estimated altitude -// @Field: Lat: Estimated latitude -// @Field: Lng: Estimated longitude -// @Field: Q1: Estimated attitude quaternion component 1 -// @Field: Q2: Estimated attitude quaternion component 2 -// @Field: Q3: Estimated attitude quaternion component 3 -// @Field: Q4: Estimated attitude quaternion component 4 - // @LoggerMessage: ARM // @Description: Arming status changes // @Field: TimeUS: Time since system startup @@ -1133,19 +1052,6 @@ struct PACKED log_PSC { // @Field: Hfp: Probability sensor has failed // @Field: Pri: True if sensor is the primary sensor -// @LoggerMessage: ATT -// @Description: Canonical vehicle attitude -// @Field: TimeUS: Time since system startup -// @Field: DesRoll: vehicle desired roll -// @Field: Roll: achieved vehicle roll -// @Field: DesPitch: vehicle desired pitch -// @Field: Pitch: achieved vehicle pitch -// @Field: DesYaw: vehicle desired yaw -// @Field: Yaw: achieved vehicle yaw -// @Field: ErrRP: lowest estimated gyro drift error -// @Field: ErrYaw: difference between measured yaw and DCM yaw estimate -// @Field: AEKF: active EKF type - // @LoggerMessage: BARO // @Description: Gathered Barometer data // @Field: TimeUS: Time since system startup @@ -1553,14 +1459,6 @@ struct PACKED log_PSC { // @Field: bodyX: derived velocity, X-axis // @Field: bodyY: derived velocity, Y-axis -// @LoggerMessage: ORGN -// @Description: Vehicle navigation origin or other notable position -// @Field: TimeUS: Time since system startup -// @Field: Type: Position type -// @Field: Lat: Position latitude -// @Field: Lng: Position longitude -// @Field: Alt: Position altitude - // @LoggerMessage: PARM // @Description: parameter value // @Field: TimeUS: Time since system startup @@ -1596,15 +1494,6 @@ struct PACKED log_PSC { // @Field: I2CI: Number of i2c interrupts serviced // @Field: Ex: number of microseconds being added to each loop to address scheduler overruns -// @LoggerMessage: POS -// @Description: Canonical vehicle position -// @Field: TimeUS: Time since system startup -// @Field: Lat: Canonical vehicle latitude -// @Field: Lng: Canonical vehicle longitude -// @Field: Alt: Canonical vehicle altitude -// @Field: RelHomeAlt: Canonical vehicle altitude relative to home -// @Field: RelOriginAlt: Canonical vehicle altitude relative to navigation origin - // @LoggerMessage: POWR // @Description: System power information // @Field: TimeUS: Time since system startup @@ -1650,24 +1539,6 @@ struct PACKED log_PSC { // @Field: Lng: longitude of rally point // @Field: Alt: altitude of rally point -// @LoggerMessage: RATE -// @Description: Desired and achieved vehicle attitude rates -// @Field: TimeUS: Time since system startup -// @Field: RDes: vehicle desired roll rate -// @Field: R: achieved vehicle roll rate -// @Field: ROut: normalized output for Roll -// @Field: PDes: vehicle desired pitch rate -// @Field: P: vehicle pitch rate -// @Field: POut: normalized output for Pitch -// @Field: YDes: vehicle desired yaw rate -// @Field: Y: achieved vehicle yaw rate -// @Field: YOut: normalized output for Yaw -// @Field: YDes: vehicle desired yaw rate -// @Field: Y: achieved vehicle yaw rate -// @Field: ADes: desired vehicle vertical acceleration -// @Field: A: achieved vehicle vertical acceleration -// @Field: AOut: percentage of vertical thrust output current being used - // @LoggerMessage: RCIN // @Description: RC input channels to vehicle // @Field: TimeUS: Time since system startup @@ -1936,8 +1807,6 @@ struct PACKED log_PSC { "BAT", "QBfffffcf", "TimeUS,Instance,Volt,VoltR,Curr,CurrTot,EnrgTot,Temp,Res", "s#vvAiJOw", "F-000!/?0" }, \ { LOG_CURRENT_CELLS_MSG, sizeof(log_Current_Cells), \ "BCL", "QBfHHHHHHHHHHHH", "TimeUS,Instance,Volt,V1,V2,V3,V4,V5,V6,V7,V8,V9,V10,V11,V12", "s#vvvvvvvvvvvvv", "F-0CCCCCCCCCCCC" }, \ - { LOG_ATTITUDE_MSG, sizeof(log_Attitude),\ - "ATT", "QccccCCCCB", "TimeUS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,ErrRP,ErrYaw,AEKF", "sddddhhdh-", "FBBBBBBBB-" }, \ { LOG_MAG_MSG, sizeof(log_MAG), \ "MAG", "QBhhhhhhhhhBI", "TimeUS,I,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOX,MOY,MOZ,Health,S", "s#GGGGGGGGG-s", "F-CCCCCCCCC-F" }, \ { LOG_MODE_MSG, sizeof(log_Mode), \ @@ -1960,10 +1829,6 @@ struct PACKED log_PSC { "OADJ","QBBBBLLLL","TimeUS,State,Err,CurrPoint,TotPoints,DLat,DLng,OALat,OALng", "sbbbbDUDU", "F----GGGG" }, \ { LOG_SIMPLE_AVOID_MSG, sizeof(log_SimpleAvoid), \ "SA", "QBffffB","TimeUS,State,DVelX,DVelY,MVelX,MVelY,Back", "sbnnnnb", "F------"}, \ - { LOG_AHR2_MSG, sizeof(log_AHRS), \ - "AHR2","QccCfLLffff","TimeUS,Roll,Pitch,Yaw,Alt,Lat,Lng,Q1,Q2,Q3,Q4","sddhmDU????", "FBBB0GG????" }, \ - { LOG_POS_MSG, sizeof(log_POS), \ - "POS","QLLfff","TimeUS,Lat,Lng,Alt,RelHomeAlt,RelOriginAlt", "sDUmmm", "FGG000" }, \ { LOG_SIMSTATE_MSG, sizeof(log_AHRS), \ "SIM","QccCfLLffff","TimeUS,Roll,Pitch,Yaw,Alt,Lat,Lng,Q1,Q2,Q3,Q4", "sddhmDU????", "FBBB0GG????" }, \ { LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \ @@ -2006,17 +1871,14 @@ struct PACKED log_PSC { "ISBH",ISBH_FMT,ISBH_LABELS,ISBH_UNITS,ISBH_MULTS }, \ { LOG_ISBD_MSG, sizeof(log_ISBD), \ "ISBD",ISBD_FMT,ISBD_LABELS, ISBD_UNITS, ISBD_MULTS }, \ - { LOG_ORGN_MSG, sizeof(log_ORGN), \ - "ORGN","QBLLe","TimeUS,Type,Lat,Lng,Alt", "s-DUm", "F-GGB" }, \ LOG_STRUCTURE_FROM_DAL \ LOG_STRUCTURE_FROM_NAVEKF2 \ LOG_STRUCTURE_FROM_NAVEKF3 \ +LOG_STRUCTURE_FROM_AHRS \ { LOG_DF_FILE_STATS, sizeof(log_DSF), \ "DSF", "QIHIIII", "TimeUS,Dp,Blk,Bytes,FMn,FMx,FAv", "s--b---", "F--0---" }, \ { LOG_RPM_MSG, sizeof(log_RPM), \ "RPM", "Qff", "TimeUS,rpm1,rpm2", "sqq", "F00" }, \ - { LOG_RATE_MSG, sizeof(log_Rate), \ - "RATE", "Qffffffffffff", "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut", "skk-kk-kk-oo-", "F?????????BB-" }, \ { LOG_RALLY_MSG, sizeof(log_Rally), \ "RALY", "QBBLLh", "TimeUS,Tot,Seq,Lat,Lng,Alt", "s--DUm", "F--GGB" }, \ { LOG_MAV_MSG, sizeof(log_MAV), \ @@ -2090,7 +1952,7 @@ enum LogMessages : uint8_t { LOG_RSSI_MSG, LOG_BARO_MSG, LOG_POWR_MSG, - LOG_AHR2_MSG, + LOG_IDS_FROM_AHRS, LOG_SIMSTATE_MSG, LOG_CMD_MSG, LOG_MAVLINK_COMMAND_MSG, @@ -2104,7 +1966,6 @@ enum LogMessages : uint8_t { LOG_CSRV_MSG, LOG_CESC_MSG, LOG_ARSP_MSG, - LOG_ATTITUDE_MSG, LOG_CURRENT_MSG, LOG_CURRENT_CELLS_MSG, LOG_MAG_MSG, @@ -2121,7 +1982,6 @@ enum LogMessages : uint8_t { LOG_GPS_RAWS_MSG, LOG_ACC_MSG, LOG_GYR_MSG, - LOG_POS_MSG, LOG_PIDR_MSG, LOG_PIDP_MSG, LOG_PIDY_MSG, @@ -2129,7 +1989,6 @@ enum LogMessages : uint8_t { LOG_PIDS_MSG, LOG_DSTL_MSG, LOG_VIBE_MSG, - LOG_ORGN_MSG, LOG_RPM_MSG, LOG_GPA_MSG, LOG_RFND_MSG, @@ -2148,11 +2007,9 @@ enum LogMessages : uint8_t { LOG_MSG_SBPEVENT, LOG_TRIGGER_MSG, - LOG_RATE_MSG, LOG_RALLY_MSG, LOG_VISUALODOM_MSG, LOG_VISUALPOS_MSG, - LOG_AOA_SSA_MSG, LOG_BEACON_MSG, LOG_PROXIMITY_MSG, LOG_DF_FILE_STATS,