diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index 93e6638304..c9ec2050c7 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -321,7 +321,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = { MSG_BATTERY_STATUS, }; static const ap_message STREAM_PARAMS_msgs[] = { - MSG_NEXT_PARAM + MSG_NEXT_PARAM, + MSG_AVAILABLE_MODES }; const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = { @@ -649,3 +650,43 @@ void GCS_MAVLINK_Tracker::send_global_position_int() 0, // Z speed cm/s (+ve Down) tracker.ahrs.yaw_sensor); // compass heading in 1/100 degree } + +// Send the mode with the given index (not mode number!) return the total number of modes +// Index starts at 1 +uint8_t GCS_MAVLINK_Tracker::send_available_mode(uint8_t index) const +{ + const Mode* modes[] { + &tracker.mode_manual, + &tracker.mode_stop, + &tracker.mode_scan, + &tracker.mode_guided, + &tracker.mode_servotest, + &tracker.mode_auto, + &tracker.mode_initialising, + }; + + const uint8_t mode_count = ARRAY_SIZE(modes); + + // Convert to zero indexed + const uint8_t index_zero = index - 1; + if (index_zero >= mode_count) { + // Mode does not exist!? + return mode_count; + } + + // Ask the mode for its name and number + const char* name = modes[index_zero]->name(); + const uint8_t mode_number = (uint8_t)modes[index_zero]->number(); + + mavlink_msg_available_modes_send( + chan, + mode_count, + index, + MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD, + mode_number, + 0, // MAV_MODE_PROPERTY bitmask + name + ); + + return mode_count; +} diff --git a/AntennaTracker/GCS_Mavlink.h b/AntennaTracker/GCS_Mavlink.h index a431f5217e..af98ee5a39 100644 --- a/AntennaTracker/GCS_Mavlink.h +++ b/AntennaTracker/GCS_Mavlink.h @@ -30,6 +30,10 @@ protected: void send_nav_controller_output() const override; void send_pid_tuning() override; + // Send the mode with the given index (not mode number!) return the total number of modes + // Index starts at 1 + uint8_t send_available_mode(uint8_t index) const override; + private: void packetReceived(const mavlink_status_t &status, const mavlink_message_t &msg) override; diff --git a/AntennaTracker/mode.h b/AntennaTracker/mode.h index d6a10022dc..bfdcad9ac7 100644 --- a/AntennaTracker/mode.h +++ b/AntennaTracker/mode.h @@ -22,6 +22,7 @@ public: // returns a unique number specific to this mode virtual Mode::Number number() const = 0; + virtual const char* name() const = 0; virtual bool requires_armed_servos() const = 0; @@ -41,6 +42,7 @@ protected: class ModeAuto : public Mode { public: Mode::Number number() const override { return Mode::Number::AUTO; } + const char* name() const override { return "Auto"; } bool requires_armed_servos() const override { return true; } void update() override; }; @@ -48,6 +50,7 @@ public: class ModeGuided : public Mode { public: Mode::Number number() const override { return Mode::Number::GUIDED; } + const char* name() const override { return "Guided"; } bool requires_armed_servos() const override { return true; } void update() override; @@ -66,6 +69,7 @@ private: class ModeInitialising : public Mode { public: Mode::Number number() const override { return Mode::Number::INITIALISING; } + const char* name() const override { return "Initialising"; } bool requires_armed_servos() const override { return false; } void update() override {}; }; @@ -73,6 +77,7 @@ public: class ModeManual : public Mode { public: Mode::Number number() const override { return Mode::Number::MANUAL; } + const char* name() const override { return "Manual"; } bool requires_armed_servos() const override { return true; } void update() override; }; @@ -80,6 +85,7 @@ public: class ModeScan : public Mode { public: Mode::Number number() const override { return Mode::Number::SCAN; } + const char* name() const override { return "Scan"; } bool requires_armed_servos() const override { return true; } void update() override; }; @@ -87,6 +93,7 @@ public: class ModeServoTest : public Mode { public: Mode::Number number() const override { return Mode::Number::SERVOTEST; } + const char* name() const override { return "ServoTest"; } bool requires_armed_servos() const override { return true; } void update() override {}; @@ -96,6 +103,7 @@ public: class ModeStop : public Mode { public: Mode::Number number() const override { return Mode::Number::STOP; } + const char* name() const override { return "Stop"; } bool requires_armed_servos() const override { return false; } void update() override {}; };