From deca687d303e17534a2182b3df1c7545d08b508d Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 9 Nov 2024 12:35:04 +0000 Subject: [PATCH] Blimp: add support for `AVAILABLE_MODES` msg --- Blimp/GCS_Mavlink.cpp | 41 ++++++++++++++++++++++++++++++++++++++++- Blimp/GCS_Mavlink.h | 4 ++++ Blimp/mode.h | 14 ++++++++++++++ 3 files changed, 58 insertions(+), 1 deletion(-) diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index 4d91191a02..1eb5985e81 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/Blimp/GCS_Mavlink.cpp @@ -401,7 +401,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, @@ -614,3 +615,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 35276b03a4..90c4881b2a 100644 --- a/Blimp/GCS_Mavlink.h +++ b/Blimp/GCS_Mavlink.h @@ -48,6 +48,10 @@ protected: 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 8a990fbb31..8718d47fa1 100644 --- a/Blimp/mode.h +++ b/Blimp/mode.h @@ -52,6 +52,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; + virtual bool is_landing() const { return false; @@ -159,6 +162,8 @@ protected: return "MANU"; } + Mode::Number number() const override { return Mode::Number::MANUAL; } + private: }; @@ -201,6 +206,8 @@ protected: return "VELY"; } + Mode::Number number() const override { return Mode::Number::VELOCITY; } + private: }; @@ -244,6 +251,8 @@ protected: return "LOIT"; } + Mode::Number number() const override { return Mode::Number::LOITER; } + private: Vector3f target_pos; float target_yaw; @@ -286,6 +295,8 @@ protected: return "LAND"; } + Mode::Number number() const override { return Mode::Number::LAND; } + private: }; @@ -328,4 +339,7 @@ protected: { return "RTL"; } + + Mode::Number number() const override { return Mode::Number::RTL; } + };