diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index a5234bff58..8d8cf3690e 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -132,7 +132,7 @@ public: void Log_Write_Attitude(AP_AHRS &ahrs, const Vector3f &targets); void Log_Write_AttitudeView(AP_AHRS_View &ahrs, const Vector3f &targets); void Log_Write_Current(); - void Log_Write_Compass(const Compass &compass, uint64_t time_us=0); + void Log_Write_Compass(uint64_t time_us=0); void Log_Write_Mode(uint8_t mode, uint8_t reason); void Log_Write_EntireMission(const AP_Mission &mission); @@ -291,8 +291,7 @@ private: void Log_Write_IMU_instance(uint64_t time_us, uint8_t imu_instance, enum LogMessages type); - void Log_Write_Compass_instance(const Compass &compass, - uint64_t time_us, + void Log_Write_Compass_instance(uint64_t time_us, uint8_t mag_instance, enum LogMessages type); void Log_Write_Current_instance(uint64_t time_us, diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 3b3c3dacb1..38c8bc53d8 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -1452,8 +1452,10 @@ void DataFlash_Class::Log_Write_Current() } } -void DataFlash_Class::Log_Write_Compass_instance(const Compass &compass, const uint64_t time_us, const uint8_t mag_instance, const enum LogMessages type) +void DataFlash_Class::Log_Write_Compass_instance(const uint64_t time_us, const uint8_t mag_instance, const enum LogMessages type) { + const Compass &compass = AP::compass(); + const Vector3f &mag_field = compass.get_field(mag_instance); const Vector3f &mag_offsets = compass.get_offsets(mag_instance); const Vector3f &mag_motor_offsets = compass.get_motor_offsets(mag_instance); @@ -1476,19 +1478,22 @@ void DataFlash_Class::Log_Write_Compass_instance(const Compass &compass, const u } // Write a Compass packet -void DataFlash_Class::Log_Write_Compass(const Compass &compass, uint64_t time_us) +void DataFlash_Class::Log_Write_Compass(uint64_t time_us) { if (time_us == 0) { time_us = AP_HAL::micros64(); } - Log_Write_Compass_instance(compass, time_us, 0, LOG_COMPASS_MSG); + const Compass &compass = AP::compass(); + if (compass.get_count() > 0) { + Log_Write_Compass_instance(time_us, 0, LOG_COMPASS_MSG); + } if (compass.get_count() > 1) { - Log_Write_Compass_instance(compass, time_us, 1, LOG_COMPASS2_MSG); + Log_Write_Compass_instance(time_us, 1, LOG_COMPASS2_MSG); } if (compass.get_count() > 2) { - Log_Write_Compass_instance(compass, time_us, 2, LOG_COMPASS3_MSG); + Log_Write_Compass_instance(time_us, 2, LOG_COMPASS3_MSG); } }