From 512f620e6e1e12e99692bddcb5eadc5fdf695441 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 15 Sep 2024 01:05:49 +0100 Subject: [PATCH] Copter: add support for adding a custom mode from scripting --- ArduCopter/Copter.cpp | 34 +++++++++++- ArduCopter/Copter.h | 6 +++ ArduCopter/mode.cpp | 87 +++++++++++++------------------ ArduCopter/mode.h | 27 ++++++++-- ArduCopter/mode_guided.cpp | 12 ++++- ArduCopter/mode_guided_custom.cpp | 11 ++++ 6 files changed, 118 insertions(+), 59 deletions(-) create mode 100644 ArduCopter/mode_guided_custom.cpp diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 6f2146175c..6bb4493597 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -411,7 +411,39 @@ bool Copter::set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_ mode_guided.set_angle(q, ang_vel_body, throttle, true); return true; } -#endif + +// Register a custom mode with given number and names +bool Copter::register_custom_mode(const uint8_t num, const char* full_name, const char* short_name) +{ + // Numbers over 100 are reserved for custom modes + if (num < 100) { + return false; + } + + const Mode::Number number = (Mode::Number)num; + + // Number already registered to existing mode + if (mode_from_mode_num(number) != nullptr) { + return false; + } + + // Find free slot + for (uint8_t i = 0; i < ARRAY_SIZE(mode_guided_custom); i++) { + if (mode_guided_custom[i] == nullptr) { + // Duplicate strings so were not pointing to unknown memory + const char* full_name_copy = strdup(full_name); + const char* short_name_copy = strndup(short_name, 4); + if ((full_name_copy != nullptr) && (short_name_copy != nullptr)) { + mode_guided_custom[i] = NEW_NOTHROW ModeGuidedCustom(number, full_name_copy, short_name_copy); + } + return mode_guided_custom[i] != nullptr; + } + } + + // No free slots + return false; +} +#endif // MODE_GUIDED_ENABLED #if MODE_CIRCLE_ENABLED // circle mode controls diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 9aa2475025..003e61d940 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -684,6 +684,8 @@ private: bool set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs) override; bool set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps, float throttle) override; + // Register a custom mode with given number and names + bool register_custom_mode(const uint8_t number, const char* full_name, const char* short_name) override; #endif #if MODE_CIRCLE_ENABLED bool get_circle_radius(float &radius_m) override; @@ -1029,6 +1031,10 @@ private: #endif #if MODE_GUIDED_ENABLED ModeGuided mode_guided; +#if AP_SCRIPTING_ENABLED + // Custom modes registered at runtime + Mode *mode_guided_custom[5]; +#endif #endif ModeLand mode_land; #if MODE_LOITER_ENABLED diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 5d8fc5a9e4..b5900f8830 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -32,158 +32,141 @@ PayloadPlace Mode::payload_place; // return the static controller object corresponding to supplied mode Mode *Copter::mode_from_mode_num(const Mode::Number mode) { - Mode *ret = nullptr; switch (mode) { #if MODE_ACRO_ENABLED case Mode::Number::ACRO: - ret = &mode_acro; - break; + return &mode_acro; #endif case Mode::Number::STABILIZE: - ret = &mode_stabilize; - break; + return &mode_stabilize; case Mode::Number::ALT_HOLD: - ret = &mode_althold; - break; + return &mode_althold; #if MODE_AUTO_ENABLED case Mode::Number::AUTO: - ret = &mode_auto; - break; + return &mode_auto; #endif #if MODE_CIRCLE_ENABLED case Mode::Number::CIRCLE: - ret = &mode_circle; - break; + return &mode_circle; #endif #if MODE_LOITER_ENABLED case Mode::Number::LOITER: - ret = &mode_loiter; - break; + return &mode_loiter; #endif #if MODE_GUIDED_ENABLED case Mode::Number::GUIDED: - ret = &mode_guided; - break; + return &mode_guided; #endif case Mode::Number::LAND: - ret = &mode_land; - break; + return &mode_land; #if MODE_RTL_ENABLED case Mode::Number::RTL: - ret = &mode_rtl; - break; + return &mode_rtl; #endif #if MODE_DRIFT_ENABLED case Mode::Number::DRIFT: - ret = &mode_drift; - break; + return &mode_drift; #endif #if MODE_SPORT_ENABLED case Mode::Number::SPORT: - ret = &mode_sport; - break; + return &mode_sport; #endif #if MODE_FLIP_ENABLED case Mode::Number::FLIP: - ret = &mode_flip; - break; + return &mode_flip; #endif #if AUTOTUNE_ENABLED case Mode::Number::AUTOTUNE: - ret = &mode_autotune; - break; + return &mode_autotune; #endif #if MODE_POSHOLD_ENABLED case Mode::Number::POSHOLD: - ret = &mode_poshold; - break; + return &mode_poshold; #endif #if MODE_BRAKE_ENABLED case Mode::Number::BRAKE: - ret = &mode_brake; - break; + return &mode_brake; #endif #if MODE_THROW_ENABLED case Mode::Number::THROW: - ret = &mode_throw; - break; + return &mode_throw; #endif #if HAL_ADSB_ENABLED case Mode::Number::AVOID_ADSB: - ret = &mode_avoid_adsb; - break; + return &mode_avoid_adsb; #endif #if MODE_GUIDED_NOGPS_ENABLED case Mode::Number::GUIDED_NOGPS: - ret = &mode_guided_nogps; - break; + return &mode_guided_nogps; #endif #if MODE_SMARTRTL_ENABLED case Mode::Number::SMART_RTL: - ret = &mode_smartrtl; - break; + return &mode_smartrtl; #endif #if MODE_FLOWHOLD_ENABLED case Mode::Number::FLOWHOLD: - ret = (Mode *)g2.mode_flowhold_ptr; - break; + return (Mode *)g2.mode_flowhold_ptr; #endif #if MODE_FOLLOW_ENABLED case Mode::Number::FOLLOW: - ret = &mode_follow; - break; + return &mode_follow; #endif #if MODE_ZIGZAG_ENABLED case Mode::Number::ZIGZAG: - ret = &mode_zigzag; - break; + return &mode_zigzag; #endif #if MODE_SYSTEMID_ENABLED case Mode::Number::SYSTEMID: - ret = (Mode *)g2.mode_systemid_ptr; - break; + return (Mode *)g2.mode_systemid_ptr; #endif #if MODE_AUTOROTATE_ENABLED case Mode::Number::AUTOROTATE: - ret = &mode_autorotate; - break; + return &mode_autorotate; #endif #if MODE_TURTLE_ENABLED case Mode::Number::TURTLE: - ret = &mode_turtle; - break; + return &mode_turtle; #endif default: break; } - return ret; +#if MODE_GUIDED_ENABLED && AP_SCRIPTING_ENABLED + // Check registered custom modes + for (uint8_t i = 0; i < ARRAY_SIZE(mode_guided_custom); i++) { + if ((mode_guided_custom[i] != nullptr) && (mode_guided_custom[i]->mode_number() == mode)) { + return mode_guided_custom[i]; + } + } +#endif + + return nullptr; } diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 5579b51fb7..b9020fe959 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1193,14 +1193,33 @@ private: void set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle); // controls which controller is run (pos or vel): - SubMode guided_mode = SubMode::TakeOff; - bool send_notification; // used to send one time notification to ground station - bool takeoff_complete; // true once takeoff has completed (used to trigger retracting of landing gear) + static SubMode guided_mode; + static bool send_notification; // used to send one time notification to ground station + static bool takeoff_complete; // true once takeoff has completed (used to trigger retracting of landing gear) // guided mode is paused or not - bool _paused; + static bool _paused; }; +#if AP_SCRIPTING_ENABLED +// Mode which behaves as guided with custom mode number and name +class ModeGuidedCustom : public ModeGuided { +public: + // constructor registers custom number and names + ModeGuidedCustom(const Number _number, const char* _full_name, const char* _short_name); + + Number mode_number() const override { return number; } + +protected: + const char *name() const override { return full_name; } + const char *name4() const override { return short_name; } + +private: + const Number number; + const char* full_name; + const char* short_name; +}; +#endif class ModeGuidedNoGPS : public ModeGuided { diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index f7ce7642a0..ba0321a144 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -7,7 +7,7 @@ */ static Vector3p guided_pos_target_cm; // position target (used by posvel controller only) -bool guided_pos_terrain_alt; // true if guided_pos_target_cm.z is an alt above terrain +static bool guided_pos_terrain_alt; // true if guided_pos_target_cm.z is an alt above terrain static Vector3f guided_vel_target_cms; // velocity target (used by pos_vel_accel controller and vel_accel controller) static Vector3f guided_accel_target_cmss; // acceleration target (used by pos_vel_accel controller vel_accel controller and accel controller) static uint32_t update_time_ms; // system time of last target update to pos_vel_accel, vel_accel or accel controller @@ -30,7 +30,15 @@ struct Guided_Limit { float horiz_max_cm; // horizontal position limit in cm from where guided mode was initiated (0 = no limit) uint32_t start_time;// system time in milliseconds that control was handed to the external computer Vector3f start_pos; // start position as a distance from home in cm. used for checking horiz_max limit -} guided_limit; +} static guided_limit; + +// controls which controller is run (pos or vel): +ModeGuided::SubMode ModeGuided::guided_mode = SubMode::TakeOff; +bool ModeGuided::send_notification; // used to send one time notification to ground station +bool ModeGuided::takeoff_complete; // true once takeoff has completed (used to trigger retracting of landing gear) + +// guided mode is paused or not +bool ModeGuided::_paused; // init - initialise guided controller bool ModeGuided::init(bool ignore_checks) diff --git a/ArduCopter/mode_guided_custom.cpp b/ArduCopter/mode_guided_custom.cpp new file mode 100644 index 0000000000..2c3ede7dd7 --- /dev/null +++ b/ArduCopter/mode_guided_custom.cpp @@ -0,0 +1,11 @@ +#include "Copter.h" + +#if MODE_GUIDED_ENABLED && AP_SCRIPTING_ENABLED +// constructor registers custom number and names +ModeGuidedCustom::ModeGuidedCustom(const Number _number, const char* _full_name, const char* _short_name): + number(_number), + full_name(_full_name), + short_name(_short_name) +{ +} +#endif