diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index eddbf23061..4d972a4e26 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -140,8 +140,10 @@ void GCS_MAVLINK_Plane::send_attitude() const omega.z); } -void Plane::send_aoa_ssa(mavlink_channel_t chan) +void GCS_MAVLINK_Plane::send_aoa_ssa() { + AP_AHRS &ahrs = AP::ahrs(); + mavlink_msg_aoa_ssa_send( chan, micros(), @@ -438,7 +440,7 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id) case MSG_AOA_SSA: CHECK_PAYLOAD_SIZE(AOA_SSA); - plane.send_aoa_ssa(chan); + send_aoa_ssa(); break; case MSG_LANDING: plane.landing.send_landing_message(chan); diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index dd7676c594..33da5fc8fa 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -28,6 +28,7 @@ protected: virtual bool in_hil_mode() const override; + void send_aoa_ssa(); void send_attitude() const override; void send_simstate() const override; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 189a129228..64f67a164b 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -765,8 +765,6 @@ private: void send_servo_out(mavlink_channel_t chan); void send_wind(mavlink_channel_t chan); - void send_aoa_ssa(mavlink_channel_t chan); - void Log_Write_Fast(void); void Log_Write_Attitude(void); void Log_Write_Performance();