From e2907ba88bd6aea49e43dc6f8f78e9b58fb0c969 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 21 Dec 2018 17:15:40 +1100 Subject: [PATCH] DataFlash: use AHRS_View for RATE log msg --- libraries/DataFlash/DataFlash.h | 2 +- libraries/DataFlash/LogFile.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index ea007e81e7..d940c13295 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -138,7 +138,7 @@ public: const AP_Mission::Mission_Command &cmd); void Log_Write_Origin(uint8_t origin_type, const Location &loc); void Log_Write_RPM(const AP_RPM &rpm_sensor); - void Log_Write_Rate(const AP_AHRS &ahrs, + void Log_Write_Rate(const AP_AHRS_View *ahrs, const AP_Motors &motors, const AC_AttitudeControl &attitude_control, const AC_PosControl &pos_control); diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index f3dcd81a3c..18efbbc31a 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -1621,7 +1621,7 @@ void DataFlash_Class::Log_Write_RPM(const AP_RPM &rpm_sensor) } // Write a rate packet -void DataFlash_Class::Log_Write_Rate(const AP_AHRS &ahrs, +void DataFlash_Class::Log_Write_Rate(const AP_AHRS_View *ahrs, const AP_Motors &motors, const AC_AttitudeControl &attitude_control, const AC_PosControl &pos_control) @@ -1632,16 +1632,16 @@ void DataFlash_Class::Log_Write_Rate(const AP_AHRS &ahrs, LOG_PACKET_HEADER_INIT(LOG_RATE_MSG), time_us : AP_HAL::micros64(), control_roll : degrees(rate_targets.x), - roll : degrees(ahrs.get_gyro().x), + roll : degrees(ahrs->get_gyro().x), roll_out : motors.get_roll(), control_pitch : degrees(rate_targets.y), - pitch : degrees(ahrs.get_gyro().y), + pitch : degrees(ahrs->get_gyro().y), pitch_out : motors.get_pitch(), control_yaw : degrees(rate_targets.z), - yaw : degrees(ahrs.get_gyro().z), + yaw : degrees(ahrs->get_gyro().z), yaw_out : motors.get_yaw(), control_accel : (float)accel_target.z, - accel : (float)(-(ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f), + accel : (float)(-(ahrs->get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f), accel_out : motors.get_throttle() }; WriteBlock(&pkt_rate, sizeof(pkt_rate));