diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 3375754b49..9d52939a30 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -807,8 +807,10 @@ static void ahrs_update() ahrs.update(); NavEKF.UpdateFilter(); - if (should_log(MASK_LOG_ATTITUDE_FAST)) + if (should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); + Log_Write_EKF(); + } if (should_log(MASK_LOG_IMU)) Log_Write_IMU(); @@ -889,8 +891,10 @@ static void barometer_accumulate(void) */ static void update_logging1(void) { - if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) + if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); + Log_Write_EKF(); + } if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_IMU)) Log_Write_IMU(); diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index 3308cf0aae..b5bbfac70f 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -174,7 +174,7 @@ struct PACKED log_Attitude { uint16_t error_yaw; }; -// Write an attitude packet. Total length : 10 bytes +// Write an attitude packet static void Log_Write_Attitude(void) { struct log_Attitude pkt = { @@ -189,6 +189,37 @@ static void Log_Write_Attitude(void) DataFlash.WriteBlock(&pkt, sizeof(pkt)); } +struct PACKED log_EKF { + LOG_PACKET_HEADER; + uint32_t time_ms; + int16_t roll; + int16_t pitch; + uint16_t yaw; + float alt; + int32_t lat; + int32_t lon; +}; + +// Write an EKF packet +static void Log_Write_EKF(void) +{ + Vector3f euler; + struct Location loc; + NavEKF.getEulerAngles(euler); + NavEKF.getLLH(loc); + struct log_EKF pkt = { + LOG_PACKET_HEADER_INIT(LOG_EKF_MSG), + time_ms : hal.scheduler->millis(), + roll : (int16_t)(100*degrees(euler.x)), + pitch : (int16_t)(100*degrees(euler.y)), + yaw : (uint16_t)wrap_360_cd(100*degrees(euler.z)), + alt : loc.alt*1.0e-2f, + lat : loc.lat, + lon : loc.lng + }; + DataFlash.WriteBlock(&pkt, sizeof(pkt)); +} + struct PACKED log_Performance { LOG_PACKET_HEADER; uint32_t loop_time; @@ -582,6 +613,8 @@ 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_EKF_MSG, sizeof(log_EKF), + "EKF", "IccCfLL", "TimeMS,Roll,Pitch,Yaw,Alt,Lat,Lng" }, TECS_LOG_FORMAT(LOG_TECS_MSG), }; @@ -625,6 +658,7 @@ static void Log_Write_Nav_Tuning() {} static void Log_Write_TECS_Tuning() {} static void Log_Write_Performance() {} static void Log_Write_Attitude() {} +static void Log_Write_EKF() {} static void Log_Write_Control_Tuning() {} static void Log_Write_Camera() {} static void Log_Write_Mode(uint8_t mode) {} diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 55b493048b..21949b5f47 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -144,6 +144,7 @@ enum log_messages { LOG_COMPASS2_MSG, LOG_ARM_DISARM_MSG, LOG_AIRSPEED_MSG, + LOG_EKF_MSG, MAX_NUM_LOGS // always at the end };