diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index b132d80160a5c..09b355b8cafc8 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/Blimp/GCS_Mavlink.cpp @@ -395,7 +395,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 }; static const ap_message STREAM_ADSB_msgs[] = { MSG_ADSB_VEHICLE, @@ -608,3 +609,41 @@ uint8_t GCS_MAVLINK_Blimp::high_latency_wind_direction() const return wrap_360(degrees(atan2f(-wind.y, -wind.x))) / 2; } #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_Blimp::send_available_mode(uint8_t index) const +{ + const Mode* modes[] { + &blimp.mode_land, + &blimp.mode_manual, + &blimp.mode_velocity, + &blimp.mode_loiter, + &blimp.mode_rtl, + }; + + 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/Blimp/GCS_Mavlink.h b/Blimp/GCS_Mavlink.h index 35276b03a46d5..90c4881b2adf4 100644 --- a/Blimp/GCS_Mavlink.h +++ b/Blimp/GCS_Mavlink.h @@ -48,6 +48,10 @@ class GCS_MAVLINK_Blimp : public GCS_MAVLINK uint32_t log_radio_bit() const override { return MASK_LOG_PM; } #endif + // 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/Blimp/mode.h b/Blimp/mode.h index 8a990fbb31b30..8718d47fa144e 100644 --- a/Blimp/mode.h +++ b/Blimp/mode.h @@ -52,6 +52,9 @@ class Mode 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; + virtual bool is_landing() const { return false; @@ -159,6 +162,8 @@ class ModeManual : public Mode return "MANU"; } + Mode::Number number() const override { return Mode::Number::MANUAL; } + private: }; @@ -201,6 +206,8 @@ class ModeVelocity : public Mode return "VELY"; } + Mode::Number number() const override { return Mode::Number::VELOCITY; } + private: }; @@ -244,6 +251,8 @@ class ModeLoiter : public Mode return "LOIT"; } + Mode::Number number() const override { return Mode::Number::LOITER; } + private: Vector3f target_pos; float target_yaw; @@ -286,6 +295,8 @@ class ModeLand : public Mode return "LAND"; } + Mode::Number number() const override { return Mode::Number::LAND; } + private: }; @@ -328,4 +339,7 @@ class ModeRTL : public Mode { return "RTL"; } + + Mode::Number number() const override { return Mode::Number::RTL; } + };