From 4a1adc69da319e495e0f3e57f1c02fb18b9561e8 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 2 Feb 2024 11:34:22 +1100 Subject: [PATCH] GCS_MAVLink: make compass.mag_cal_fixed_yaw return boolean this method could be used from a transmitter without GCS enabled, for example --- libraries/GCS_MAVLink/GCS_Common.cpp | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index c1f07fea17..ec4ff7eae0 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -3832,19 +3832,24 @@ void GCS_MAVLINK::handle_optical_flow(const mavlink_message_t &msg) #endif -#if COMPASS_CAL_ENABLED +#if AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED /* handle MAV_CMD_FIXED_MAG_CAL_YAW */ MAV_RESULT GCS_MAVLINK::handle_command_fixed_mag_cal_yaw(const mavlink_command_int_t &packet) { Compass &compass = AP::compass(); - return compass.mag_cal_fixed_yaw(packet.param1, - uint8_t(packet.param2), - packet.param3, - packet.param4); + if (!compass.mag_cal_fixed_yaw(packet.param1, + uint8_t(packet.param2), + packet.param3, + packet.param4)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; } +#endif // AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED +#if COMPASS_CAL_ENABLED MAV_RESULT GCS_MAVLINK::handle_command_mag_cal(const mavlink_command_int_t &packet) { return AP::compass().handle_mag_cal_command(packet); @@ -5226,9 +5231,11 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p case MAV_CMD_COMPONENT_ARM_DISARM: return handle_command_component_arm_disarm(packet); -#if COMPASS_CAL_ENABLED +#if AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED case MAV_CMD_FIXED_MAG_CAL_YAW: return handle_command_fixed_mag_cal_yaw(packet); +#endif +#if COMPASS_CAL_ENABLED case MAV_CMD_DO_START_MAG_CAL: case MAV_CMD_DO_ACCEPT_MAG_CAL: case MAV_CMD_DO_CANCEL_MAG_CAL: