From bd28bdd90769a4333133b054b53cfeaf0d09d86b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 17 Jul 2017 10:46:54 +1000 Subject: [PATCH] GCS_MAVLink: move handling of MAG_CAL command longs up --- libraries/GCS_MAVLink/GCS.h | 2 ++ libraries/GCS_MAVLink/GCS_Common.cpp | 16 ++++++++++++++++ libraries/GCS_MAVLink/GCS_Dummy.h | 1 + .../GCS_MAVLink/examples/routing/routing.cpp | 1 + 4 files changed, 20 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 24c47438a2..3b5eaa35b5 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -220,6 +220,7 @@ protected: virtual bool accept_packet(const mavlink_status_t &status, mavlink_message_t &msg) { return true; } virtual AP_Mission *get_mission() = 0; virtual AP_Rally *get_rally() const = 0; + virtual Compass *get_compass() const = 0; virtual AP_ServoRelayEvents *get_servorelayevents() const = 0; bool waypoint_receiving; // currently receiving @@ -275,6 +276,7 @@ protected: bool telemetry_delayed() const; virtual uint32_t telem_delay() const = 0; + MAV_RESULT handle_command_mag_cal(const mavlink_command_long_t &packet); MAV_RESULT handle_command_long_message(mavlink_command_long_t &packet); private: diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 2ad99f0bf7..2badfeda82 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1835,12 +1835,28 @@ void GCS_MAVLINK::handle_common_mission_message(mavlink_message_t *msg) } } +MAV_RESULT GCS_MAVLINK::handle_command_mag_cal(const mavlink_command_long_t &packet) +{ + Compass *compass = get_compass(); + if (compass == nullptr) { + return MAV_RESULT_UNSUPPORTED; + } + return compass->handle_mag_cal_command(packet); +} + MAV_RESULT GCS_MAVLINK::handle_command_long_message(mavlink_command_long_t &packet) { MAV_RESULT result = MAV_RESULT_FAILED; switch (packet.command) { + case MAV_CMD_DO_START_MAG_CAL: + case MAV_CMD_DO_ACCEPT_MAG_CAL: + case MAV_CMD_DO_CANCEL_MAG_CAL: { + result = handle_command_mag_cal(packet); + break; + } + case MAV_CMD_START_RX_PAIR: result = handle_rc_bind(packet); break; diff --git a/libraries/GCS_MAVLink/GCS_Dummy.h b/libraries/GCS_MAVLink/GCS_Dummy.h index 90a16332d7..3e08606032 100644 --- a/libraries/GCS_MAVLink/GCS_Dummy.h +++ b/libraries/GCS_MAVLink/GCS_Dummy.h @@ -14,6 +14,7 @@ class GCS_MAVLINK_Dummy : public GCS_MAVLINK protected: + Compass *get_compass() const override { return nullptr; }; AP_Mission *get_mission() override { return nullptr; } AP_Rally *get_rally() const override { return nullptr; }; AP_ServoRelayEvents *get_servorelayevents() const override { return nullptr; } diff --git a/libraries/GCS_MAVLink/examples/routing/routing.cpp b/libraries/GCS_MAVLink/examples/routing/routing.cpp index 6415fa90a5..3794e20f24 100644 --- a/libraries/GCS_MAVLink/examples/routing/routing.cpp +++ b/libraries/GCS_MAVLink/examples/routing/routing.cpp @@ -21,6 +21,7 @@ public: protected: uint32_t telem_delay() const override { return 0; } + Compass *get_compass() const override { return nullptr; }; AP_Mission *get_mission() override { return nullptr; } AP_Rally *get_rally() const override { return nullptr; } AP_ServoRelayEvents *get_servorelayevents() const override { return nullptr; }