Copter: add support for adding a custom mode from scripting

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

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

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;
 }
 
 

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 {
 

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)

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