From c600c1a746e34f9aac2f742d3ffc863abb827d01 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 12 May 2015 20:49:17 +0100 Subject: [PATCH] DataFlash: log Gyro and Accel Health for IMUs. Log the health of the various IMUs under GyHlt and AcHlt. (Names are shortened to get inside the string array limit). --- libraries/DataFlash/DataFlash.h | 7 ++++--- libraries/DataFlash/LogFile.cpp | 12 +++++++++--- 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index 98ea2453f7..daba43fd25 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -205,6 +205,7 @@ struct PACKED log_IMU { float accel_x, accel_y, accel_z; uint32_t gyro_error, accel_error; float temperature; + uint8_t gyro_health, accel_health; }; struct PACKED log_RCIN { @@ -571,7 +572,7 @@ Format characters in the format string for binary log messages { LOG_GPS_MSG, sizeof(log_GPS), \ "GPS", "BIHBcLLeeEefI", "Status,TimeMS,Week,NSats,HDop,Lat,Lng,RelAlt,Alt,Spd,GCrs,VZ,T" }, \ { LOG_IMU_MSG, sizeof(log_IMU), \ - "IMU", "IffffffIIf", "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA,Temp" }, \ + "IMU", "IffffffIIfBB", "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA,Temp,GyHlt,AcHlt" }, \ { LOG_MESSAGE_MSG, sizeof(log_Message), \ "MSG", "Z", "Message"}, \ { LOG_RCIN_MSG, sizeof(log_RCIN), \ @@ -606,9 +607,9 @@ Format characters in the format string for binary log messages { LOG_GPS2_MSG, sizeof(log_GPS2), \ "GPS2", "BIHBcLLeEefIBI", "Status,TimeMS,Week,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,T,DSc,DAg" }, \ { LOG_IMU2_MSG, sizeof(log_IMU), \ - "IMU2", "IffffffIIf", "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA,Temp" }, \ + "IMU2", "IffffffIIfBB", "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA,Temp,GyHlt,AcHlt" }, \ { LOG_IMU3_MSG, sizeof(log_IMU), \ - "IMU3", "IffffffIIf", "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA,Temp" }, \ + "IMU3", "IffffffIIfBB", "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA,Temp,GyHlt,AcHlt" }, \ { LOG_AHR2_MSG, sizeof(log_AHRS), \ "AHR2","IccCfLL","TimeMS,Roll,Pitch,Yaw,Alt,Lat,Lng" }, \ { LOG_POS_MSG, sizeof(log_POS), \ diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index e79b4ee22e..8712239667 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -801,7 +801,9 @@ void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins) accel_z : accel.z, gyro_error : ins.get_gyro_error_count(0), accel_error : ins.get_accel_error_count(0), - temperature : ins.get_temperature(0) + temperature : ins.get_temperature(0), + gyro_health : (uint8_t)ins.get_gyro_health(0), + accel_health : (uint8_t)ins.get_accel_health(0) }; WriteBlock(&pkt, sizeof(pkt)); if (ins.get_gyro_count() < 2 && ins.get_accel_count() < 2) { @@ -821,7 +823,9 @@ void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins) accel_z : accel2.z, gyro_error : ins.get_gyro_error_count(1), accel_error : ins.get_accel_error_count(1), - temperature : ins.get_temperature(1) + temperature : ins.get_temperature(1), + gyro_health : (uint8_t)ins.get_gyro_health(1), + accel_health : (uint8_t)ins.get_accel_health(1) }; WriteBlock(&pkt2, sizeof(pkt2)); if (ins.get_gyro_count() < 3 && ins.get_accel_count() < 3) { @@ -840,7 +844,9 @@ void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins) accel_z : accel3.z, gyro_error : ins.get_gyro_error_count(2), accel_error : ins.get_accel_error_count(2), - temperature : ins.get_temperature(2) + temperature : ins.get_temperature(2), + gyro_health : (uint8_t)ins.get_gyro_health(2), + accel_health : (uint8_t)ins.get_accel_health(2) }; WriteBlock(&pkt3, sizeof(pkt3)); #endif