From 35c3adb1ad6cf9e3c556da8249e77a4fafd85d9f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 5 May 2016 13:37:16 +1000 Subject: [PATCH] DataFlash: use caller supplied timestamps for sensor logging used to support EK2_LOGGING=1 --- libraries/DataFlash/DataFlash.h | 8 ++++---- libraries/DataFlash/LogFile.cpp | 29 ++++++++++++++++++----------- 2 files changed, 22 insertions(+), 15 deletions(-) diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index 1f6448c25d..f9ca1bbae8 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -105,15 +105,15 @@ public: void StopLogging(); void Log_Write_Parameter(const char *name, float value); - void Log_Write_GPS(const AP_GPS &gps, uint8_t instance); + void Log_Write_GPS(const AP_GPS &gps, uint8_t instance, uint64_t time_us=0); void Log_Write_RFND(const RangeFinder &rangefinder); void Log_Write_IMU(const AP_InertialSensor &ins); - void Log_Write_IMUDT(const AP_InertialSensor &ins); + void Log_Write_IMUDT(const AP_InertialSensor &ins, uint64_t time_us); void Log_Write_Vibration(const AP_InertialSensor &ins); void Log_Write_RCIN(void); void Log_Write_RCOUT(void); void Log_Write_RSSI(AP_RSSI &rssi); - void Log_Write_Baro(AP_Baro &baro); + void Log_Write_Baro(AP_Baro &baro, uint64_t time_us=0); void Log_Write_Power(void); void Log_Write_AHRS2(AP_AHRS &ahrs); void Log_Write_POS(AP_AHRS &ahrs); @@ -131,7 +131,7 @@ public: void Log_Write_Airspeed(AP_Airspeed &airspeed); void Log_Write_Attitude(AP_AHRS &ahrs, const Vector3f &targets); void Log_Write_Current(const AP_BattMonitor &battery, int16_t throttle); - void Log_Write_Compass(const Compass &compass); + void Log_Write_Compass(const Compass &compass, uint64_t time_us=0); void Log_Write_Mode(uint8_t mode, uint8_t reason = 0); void Log_Write_EntireMission(const AP_Mission &mission); diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index a59148a5ea..953fdc1ab8 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -707,12 +707,15 @@ bool DataFlash_Backend::Log_Write_Parameter(const AP_Param *ap, } // Write an GPS packet -void DataFlash_Class::Log_Write_GPS(const AP_GPS &gps, uint8_t i) +void DataFlash_Class::Log_Write_GPS(const AP_GPS &gps, uint8_t i, uint64_t time_us) { + if (time_us == 0) { + time_us = AP_HAL::micros64(); + } const struct Location &loc = gps.location(i); struct log_GPS pkt = { LOG_PACKET_HEADER_INIT((uint8_t)(LOG_GPS_MSG+i)), - time_us : AP_HAL::micros64(), + time_us : time_us, status : (uint8_t)gps.status(i), gps_week_ms : gps.time_week_ms(i), gps_week : gps.time_week(i), @@ -735,7 +738,7 @@ void DataFlash_Class::Log_Write_GPS(const AP_GPS &gps, uint8_t i) gps.speed_accuracy(i, sacc); struct log_GPA pkt2 = { LOG_PACKET_HEADER_INIT((uint8_t)(LOG_GPA_MSG+i)), - time_us : AP_HAL::micros64(), + time_us : time_us, vdop : gps.get_vdop(i), hacc : (uint16_t)(hacc*100), vacc : (uint16_t)(vacc*100), @@ -817,9 +820,11 @@ void DataFlash_Class::Log_Write_RSSI(AP_RSSI &rssi) } // Write a BARO packet -void DataFlash_Class::Log_Write_Baro(AP_Baro &baro) +void DataFlash_Class::Log_Write_Baro(AP_Baro &baro, uint64_t time_us) { - uint64_t time_us = AP_HAL::micros64(); + if (time_us == 0) { + time_us = AP_HAL::micros64(); + } struct log_BARO pkt = { LOG_PACKET_HEADER_INIT(LOG_BARO_MSG), time_us : time_us, @@ -926,7 +931,7 @@ void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins) } // Write an accel/gyro delta time data packet -void DataFlash_Class::Log_Write_IMUDT(const AP_InertialSensor &ins) +void DataFlash_Class::Log_Write_IMUDT(const AP_InertialSensor &ins, uint64_t time_us) { float delta_t = ins.get_delta_time(); float delta_vel_t = ins.get_delta_velocity_dt(0); @@ -935,7 +940,6 @@ void DataFlash_Class::Log_Write_IMUDT(const AP_InertialSensor &ins) ins.get_delta_angle(0, delta_angle); ins.get_delta_velocity(0, delta_velocity); - uint64_t time_us = AP_HAL::micros64(); struct log_IMUDT pkt = { LOG_PACKET_HEADER_INIT(LOG_IMUDT_MSG), time_us : time_us, @@ -1635,14 +1639,17 @@ void DataFlash_Class::Log_Write_Current(const AP_BattMonitor &battery, int16_t t } // Write a Compass packet -void DataFlash_Class::Log_Write_Compass(const Compass &compass) +void DataFlash_Class::Log_Write_Compass(const Compass &compass, uint64_t time_us) { + if (time_us == 0) { + time_us = AP_HAL::micros64(); + } const Vector3f &mag_field = compass.get_field(0); const Vector3f &mag_offsets = compass.get_offsets(0); const Vector3f &mag_motor_offsets = compass.get_motor_offsets(0); struct log_Compass pkt = { LOG_PACKET_HEADER_INIT(LOG_COMPASS_MSG), - time_us : AP_HAL::micros64(), + time_us : time_us, mag_x : (int16_t)mag_field.x, mag_y : (int16_t)mag_field.y, mag_z : (int16_t)mag_field.z, @@ -1663,7 +1670,7 @@ void DataFlash_Class::Log_Write_Compass(const Compass &compass) const Vector3f &mag_motor_offsets2 = compass.get_motor_offsets(1); struct log_Compass pkt2 = { LOG_PACKET_HEADER_INIT(LOG_COMPASS2_MSG), - time_us : AP_HAL::micros64(), + time_us : time_us, mag_x : (int16_t)mag_field2.x, mag_y : (int16_t)mag_field2.y, mag_z : (int16_t)mag_field2.z, @@ -1685,7 +1692,7 @@ void DataFlash_Class::Log_Write_Compass(const Compass &compass) const Vector3f &mag_motor_offsets3 = compass.get_motor_offsets(2); struct log_Compass pkt3 = { LOG_PACKET_HEADER_INIT(LOG_COMPASS3_MSG), - time_us : AP_HAL::micros64(), + time_us : time_us, mag_x : (int16_t)mag_field3.x, mag_y : (int16_t)mag_field3.y, mag_z : (int16_t)mag_field3.z,