diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index c2bbf8b5c4..f17901638d 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -443,7 +443,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = { #endif }; 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[] = { @@ -968,3 +969,47 @@ uint8_t GCS_MAVLINK_Sub::high_latency_tgt_airspeed() const return 0; } #endif // HAL_HIGH_LATENCY2_ENABLED + +// Send the mode with the given index (not mode number!) return the total number of modes +// Index starts at 1 +uint8_t GCS_MAVLINK_Sub::send_available_mode(uint8_t index) const +{ + const Mode* modes[] { + &sub.mode_manual, + &sub.mode_stabilize, + &sub.mode_acro, + &sub.mode_althold, + &sub.mode_surftrak, + &sub.mode_poshold, + &sub.mode_auto, + &sub.mode_guided, + &sub.mode_circle, + &sub.mode_surface, + &sub.mode_motordetect, + }; + + 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/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h index c38ec3f4ab..7dc2d0c1a2 100644 --- a/ArduSub/GCS_Mavlink.h +++ b/ArduSub/GCS_Mavlink.h @@ -37,6 +37,10 @@ protected: uint64_t capabilities() const 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 handle_message(const mavlink_message_t &msg) override; diff --git a/ArduSub/mode.h b/ArduSub/mode.h index 11a6447167..35780cb9c0 100644 --- a/ArduSub/mode.h +++ b/ArduSub/mode.h @@ -72,6 +72,9 @@ public: virtual const char *name() const = 0; virtual const char *name4() const = 0; + // returns a unique number specific to this mode + virtual Mode::Number number() const = 0; + // functions for reporting to GCS virtual bool get_wp(Location &loc) { return false; } virtual int32_t wp_bearing() const { return 0; } @@ -202,6 +205,7 @@ protected: const char *name() const override { return "MANUAL"; } const char *name4() const override { return "MANU"; } + Mode::Number number() const override { return Mode::Number::MANUAL; } }; @@ -224,6 +228,7 @@ protected: const char *name() const override { return "ACRO"; } const char *name4() const override { return "ACRO"; } + Mode::Number number() const override { return Mode::Number::ACRO; } }; @@ -246,6 +251,7 @@ protected: const char *name() const override { return "STABILIZE"; } const char *name4() const override { return "STAB"; } + Mode::Number number() const override { return Mode::Number::STABILIZE; } }; @@ -272,6 +278,7 @@ protected: const char *name() const override { return "ALT_HOLD"; } const char *name4() const override { return "ALTH"; } + Mode::Number number() const override { return Mode::Number::ALT_HOLD; } }; @@ -293,6 +300,7 @@ protected: const char *name() const override { return "SURFTRAK"; } const char *name4() const override { return "STRK"; } + Mode::Number number() const override { return Mode::Number::SURFTRAK; } private: @@ -342,6 +350,8 @@ protected: const char *name() const override { return "GUIDED"; } const char *name4() const override { return "GUID"; } + Mode::Number number() const override { return Mode::Number::GUIDED; } + autopilot_yaw_mode get_default_auto_yaw_mode(bool rtl) const; private: @@ -387,6 +397,7 @@ protected: const char *name() const override { return "AUTO"; } const char *name4() const override { return "AUTO"; } + Mode::Number number() const override { return Mode::Number::AUTO; } private: void auto_wp_run(); @@ -417,6 +428,7 @@ protected: const char *name() const override { return "POSHOLD"; } const char *name4() const override { return "POSH"; } + Mode::Number number() const override { return Mode::Number::POSHOLD; } }; @@ -439,6 +451,7 @@ protected: const char *name() const override { return "CIRCLE"; } const char *name4() const override { return "CIRC"; } + Mode::Number number() const override { return Mode::Number::CIRCLE; } }; class ModeSurface : public Mode @@ -460,6 +473,7 @@ protected: const char *name() const override { return "SURFACE"; } const char *name4() const override { return "SURF"; } + Mode::Number number() const override { return Mode::Number::CIRCLE; } }; @@ -482,4 +496,5 @@ protected: const char *name() const override { return "MOTORDETECT"; } const char *name4() const override { return "DETE"; } + Mode::Number number() const override { return Mode::Number::MOTOR_DETECT; } };