diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index 9bd73caa01..fa866ca5c8 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -519,6 +519,36 @@ static void Log_Write_RC(void) DataFlash.Log_Write_RCOUT(); } +static void Log_Write_Baro(void) +{ + DataFlash.Log_Write_Baro(barometer); +} + +struct PACKED log_AIRSPEED { + LOG_PACKET_HEADER; + uint32_t timestamp; + float airspeed; + float diffpressure; + int16_t temperature; +}; + +// Write a AIRSPEED packet +static void Log_Write_Airspeed(void) +{ + float temperature; + if (!airspeed.get_temperature(temperature)) { + temperature = 0; + } + struct log_AIRSPEED pkt = { + LOG_PACKET_HEADER_INIT(LOG_AIRSPEED_MSG), + timestamp : hal.scheduler->millis(), + airspeed : airspeed.get_raw_airspeed(), + diffpressure : airspeed.get_differential_pressure(), + temperature : (int16_t)(temperature * 100.0f) + }; + DataFlash.WriteBlock(&pkt, sizeof(pkt)); +} + static const struct LogStructure log_structure[] PROGMEM = { LOG_COMMON_STRUCTURES, { LOG_ATTITUDE_MSG, sizeof(log_Attitude), @@ -547,6 +577,8 @@ static const struct LogStructure log_structure[] PROGMEM = { "MAG2", "Ihhhhhh", "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ" }, { LOG_ARM_DISARM_MSG, sizeof(log_Arm_Disarm), "ARM", "IHB", "TimeMS,ArmState,ArmChecks" }, + { LOG_AIRSPEED_MSG, sizeof(log_AIRSPEED), + "ARSP", "Iffc", "TimeMS,Airspeed,DiffPress,Temp" }, TECS_LOG_FORMAT(LOG_TECS_MSG), }; @@ -597,6 +629,8 @@ static void Log_Write_Compass() {} static void Log_Write_GPS() {} static void Log_Write_IMU() {} static void Log_Write_RC() {} +static void Log_Write_Airspeed(void) {} +static void Log_Write_Baro(void) {} static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 160bb63c87..0bf6c199fd 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -143,6 +143,7 @@ enum log_messages { LOG_SONAR_MSG, LOG_COMPASS2_MSG, LOG_ARM_DISARM_MSG, + LOG_AIRSPEED_MSG, MAX_NUM_LOGS // always at the end }; diff --git a/ArduPlane/sensors.pde b/ArduPlane/sensors.pde index 7cc6db1ede..5811599743 100644 --- a/ArduPlane/sensors.pde +++ b/ArduPlane/sensors.pde @@ -20,6 +20,9 @@ static void init_barometer(void) static int32_t read_barometer(void) { barometer.read(); + if (should_log(MASK_LOG_IMU)) { + Log_Write_Baro(); + } return altitude_filter.apply(barometer.get_altitude() * 100.0); } @@ -51,6 +54,9 @@ static void read_airspeed(void) { if (airspeed.enabled()) { airspeed.read(); + if (should_log(MASK_LOG_IMU)) { + Log_Write_Airspeed(); + } calc_airspeed_errors(); } }