diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 398fff24a7..fca945978f 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -811,6 +811,7 @@ static void ahrs_update() if (should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); Log_Write_EKF(); + Log_Write_AHRS2(); } if (should_log(MASK_LOG_IMU)) @@ -895,6 +896,7 @@ static void update_logging1(void) if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); Log_Write_EKF(); + Log_Write_AHRS2(); } if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_IMU)) diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index 9f81b00efc..c7c83e8c42 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -189,6 +189,38 @@ static void Log_Write_Attitude(void) DataFlash.WriteBlock(&pkt, sizeof(pkt)); } +struct PACKED log_AHRS2 { + LOG_PACKET_HEADER; + uint32_t time_ms; + int16_t roll; + int16_t pitch; + uint16_t yaw; + float alt; + int32_t lat; + int32_t lng; +}; + +// Write an AHRS2 packet +static void Log_Write_AHRS2(void) +{ + Vector3f euler; + struct Location loc; + if (!ahrs.get_secondary_attitude(euler) || !ahrs.get_secondary_position(loc)) { + return; + } + struct log_AHRS2 pkt = { + LOG_PACKET_HEADER_INIT(LOG_AHRS2_MSG), + time_ms : hal.scheduler->millis(), + 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 + }; + DataFlash.WriteBlock(&pkt, sizeof(pkt)); +} + struct PACKED log_EKF1 { LOG_PACKET_HEADER; uint32_t time_ms; @@ -676,8 +708,12 @@ static const struct LogStructure log_structure[] PROGMEM = { "ARM", "IHB", "TimeMS,ArmState,ArmChecks" }, { LOG_AIRSPEED_MSG, sizeof(log_AIRSPEED), "ARSP", "Iffc", "TimeMS,Airspeed,DiffPress,Temp" }, - { LOG_EKF1_MSG, sizeof(log_EKF1), "EKF","IccCccccccbbb","TimeMS,Roll,Pitch,Yaw,VN,VE,VD,PN,PE,PE,GX,GY,GZ" }, - { LOG_EKF2_MSG, sizeof(log_EKF2), "EKF","Iccccchhhhhh","TimeMS,AX,AY,AZ,VWN,VWE,MN,ME,MD,MX,MY,MZ" }, + { LOG_AHRS2_MSG, sizeof(log_AHRS2), + "AHR2","IccCfLL","TimeMS,Roll,Pitch,Yaw,Alt,Lat,Lng" }, + { LOG_EKF1_MSG, sizeof(log_EKF1), + "EKF1","IccCffffffbbb","TimeMS,Roll,Pitch,Yaw,VN,VE,VD,PN,PE,PD,GX,GY,GZ" }, + { LOG_EKF2_MSG, sizeof(log_EKF2), + "EKF2","Ibbbcchhhhhh","TimeMS,AX,AY,AZ,VWN,VWE,MN,ME,MD,MX,MY,MZ" }, TECS_LOG_FORMAT(LOG_TECS_MSG), }; diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 04c4e7bcae..33af9d29ba 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -146,6 +146,7 @@ enum log_messages { LOG_AIRSPEED_MSG, LOG_EKF1_MSG, LOG_EKF2_MSG, + LOG_AHRS2_MSG, MAX_NUM_LOGS // always at the end };