diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index bd4e2e8ceb..839b51d8b8 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -274,6 +274,10 @@ private: #endif void Log_Write_Baro_instance(AP_Baro &baro, uint64_t time_us, uint8_t baro_instance, enum LogMessages type); + void Log_Write_IMU_instance(const AP_InertialSensor &ins, + uint64_t time_us, + uint8_t imu_instance, + enum LogMessages type); void backend_starting_new_log(const DataFlash_Backend *backend); diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index bdbe96a67d..d2be518040 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -400,14 +400,12 @@ void DataFlash_Class::Log_Write_Baro(AP_Baro &baro, uint64_t time_us) } } -// Write an raw accel/gyro data packet -void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins) +void DataFlash_Class::Log_Write_IMU_instance(const AP_InertialSensor &ins, const uint64_t time_us, const uint8_t imu_instance, const enum LogMessages type) { - uint64_t time_us = AP_HAL::micros64(); - const Vector3f &gyro = ins.get_gyro(0); - const Vector3f &accel = ins.get_accel(0); + const Vector3f &gyro = ins.get_gyro(imu_instance); + const Vector3f &accel = ins.get_accel(imu_instance); struct log_IMU pkt = { - LOG_PACKET_HEADER_INIT(LOG_IMU_MSG), + LOG_PACKET_HEADER_INIT(type), time_us : time_us, gyro_x : gyro.x, gyro_y : gyro.y, @@ -415,62 +413,34 @@ void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins) accel_x : accel.x, accel_y : accel.y, 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), - gyro_health : (uint8_t)ins.get_gyro_health(0), - accel_health : (uint8_t)ins.get_accel_health(0), - gyro_rate : ins.get_gyro_rate_hz(0), - accel_rate : ins.get_accel_rate_hz(0), + gyro_error : ins.get_gyro_error_count(imu_instance), + accel_error : ins.get_accel_error_count(imu_instance), + temperature : ins.get_temperature(imu_instance), + gyro_health : (uint8_t)ins.get_gyro_health(imu_instance), + accel_health : (uint8_t)ins.get_accel_health(imu_instance), + gyro_rate : ins.get_gyro_rate_hz(imu_instance), + accel_rate : ins.get_accel_rate_hz(imu_instance), }; WriteBlock(&pkt, sizeof(pkt)); +} + +// Write an raw accel/gyro data packet +void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins) +{ + uint64_t time_us = AP_HAL::micros64(); + + Log_Write_IMU_instance(ins, time_us, 0, LOG_IMU_MSG); if (ins.get_gyro_count() < 2 && ins.get_accel_count() < 2) { return; } - const Vector3f &gyro2 = ins.get_gyro(1); - const Vector3f &accel2 = ins.get_accel(1); - struct log_IMU pkt2 = { - LOG_PACKET_HEADER_INIT(LOG_IMU2_MSG), - time_us : time_us, - gyro_x : gyro2.x, - gyro_y : gyro2.y, - gyro_z : gyro2.z, - accel_x : accel2.x, - accel_y : accel2.y, - 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), - gyro_health : (uint8_t)ins.get_gyro_health(1), - accel_health : (uint8_t)ins.get_accel_health(1), - gyro_rate : ins.get_gyro_rate_hz(1), - accel_rate : ins.get_accel_rate_hz(1), - }; - WriteBlock(&pkt2, sizeof(pkt2)); + Log_Write_IMU_instance(ins, time_us, 1, LOG_IMU2_MSG); + if (ins.get_gyro_count() < 3 && ins.get_accel_count() < 3) { return; } - const Vector3f &gyro3 = ins.get_gyro(2); - const Vector3f &accel3 = ins.get_accel(2); - struct log_IMU pkt3 = { - LOG_PACKET_HEADER_INIT(LOG_IMU3_MSG), - time_us : time_us, - gyro_x : gyro3.x, - gyro_y : gyro3.y, - gyro_z : gyro3.z, - accel_x : accel3.x, - accel_y : accel3.y, - 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), - gyro_health : (uint8_t)ins.get_gyro_health(2), - accel_health : (uint8_t)ins.get_accel_health(2), - gyro_rate : ins.get_gyro_rate_hz(2), - accel_rate : ins.get_accel_rate_hz(2), - }; - WriteBlock(&pkt3, sizeof(pkt3)); + + Log_Write_IMU_instance(ins, time_us, 2, LOG_IMU3_MSG); } // Write an accel/gyro delta time data packet