diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 96f3872819..9ff41bc679 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -440,6 +440,8 @@ protected: void handle_optical_flow(const mavlink_message_t &msg); + MAV_RESULT handle_fixed_mag_cal_yaw(const mavlink_command_long_t &packet); + // vehicle-overridable message send function virtual bool try_send_message(enum ap_message id); virtual void send_global_position_int(); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 7c27b41818..0559c3e2dc 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -3030,6 +3030,19 @@ void GCS_MAVLINK::handle_optical_flow(const mavlink_message_t &msg) optflow->handle_msg(msg); } + +/* + handle MAV_CMD_FIXED_MAG_CAL_YAW + */ +MAV_RESULT GCS_MAVLINK::handle_fixed_mag_cal_yaw(const mavlink_command_long_t &packet) +{ + Compass &compass = AP::compass(); + return compass.mag_cal_fixed_yaw(packet.param1, + uint8_t(packet.param2), + packet.param3, + packet.param4); +} + /* handle messages which don't require vehicle specific data */ @@ -3752,6 +3765,10 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t return MAV_RESULT_UNSUPPORTED; + case MAV_CMD_FIXED_MAG_CAL_YAW: + result = handle_fixed_mag_cal_yaw(packet); + break; + default: result = MAV_RESULT_UNSUPPORTED; break;