Copter/Rover: add support for running lua scripts as part of missions

ArduCopter/Copter.cpp

@@ -418,6 +418,32 @@ bool Copter::set_circle_rate(float rate_dps)
     return true;
 }
 
+// returns true if mode supports NAV_SCRIPT_TIME mission commands
+bool Copter::nav_scripting_enable(uint8_t mode)
+{
+    return mode == (uint8_t)mode_auto.mode_number();
+}
+
+// lua scripts use this to retrieve the contents of the active command
+bool Copter::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2)
+{
+    if (flightmode != &mode_auto) {
+        return false;
+    }
+
+    return mode_auto.nav_script_time(id, cmd, arg1, arg2);
+}
+
+// lua scripts use this to indicate when they have complete the command
+void Copter::nav_script_time_done(uint16_t id)
+{
+    if (flightmode != &mode_auto) {
+        return;
+    }
+
+    return mode_auto.nav_script_time_done(id);
+}
+
 #endif // AP_SCRIPTING_ENABLED
 
 

ArduCopter/Copter.h

@@ -653,6 +653,9 @@ class Copter : public AP_Vehicle {
     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 get_circle_radius(float &radius_m) override;
     bool set_circle_rate(float rate_dps) override;
+    bool nav_scripting_enable(uint8_t mode) override;
+    bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2) override;
+    void nav_script_time_done(uint16_t id) override;
 #endif // AP_SCRIPTING_ENABLED
     void rc_loop();
     void throttle_loop();

ArduCopter/mode.h

@@ -390,7 +390,7 @@ class ModeAuto : public Mode {
     bool has_manual_throttle() const override { return false; }
     bool allows_arming(AP_Arming::Method method) const override;
     bool is_autopilot() const override { return true; }
-    bool in_guided_mode() const override { return mode() == SubMode::NAVGUIDED; }
+    bool in_guided_mode() const override { return mode() == SubMode::NAVGUIDED || mode() == SubMode::NAV_SCRIPT_TIME; }
 
     // Auto modes
     enum class SubMode : uint8_t {
@@ -404,6 +404,7 @@ class ModeAuto : public Mode {
         LOITER,
         LOITER_TO_ALT,
         NAV_PAYLOAD_PLACE,
+        NAV_SCRIPT_TIME,
     };
 
     // Auto
@@ -438,6 +439,10 @@ class ModeAuto : public Mode {
     // Go straight to landing sequence via DO_LAND_START, if succeeds pretend to be Auto RTL mode
     bool jump_to_landing_sequence_auto_RTL(ModeReason reason);
 
+    // lua accessors for nav script time support
+    bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2);
+    void nav_script_time_done(uint16_t id);
+
     AP_Mission mission{
         FUNCTOR_BIND_MEMBER(&ModeAuto::start_command, bool, const AP_Mission::Mission_Command &),
         FUNCTOR_BIND_MEMBER(&ModeAuto::verify_command, bool, const AP_Mission::Mission_Command &),
@@ -522,6 +527,9 @@ class ModeAuto : public Mode {
 #endif
     void do_payload_place(const AP_Mission::Mission_Command& cmd);
     void do_RTL(void);
+#if AP_SCRIPTING_ENABLED
+    void do_nav_script_time(const AP_Mission::Mission_Command& cmd);
+#endif
 
     bool verify_takeoff();
     bool verify_land();
@@ -540,6 +548,9 @@ class ModeAuto : public Mode {
     bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
 #endif
     bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
+#if AP_SCRIPTING_ENABLED
+    bool verify_nav_script_time();
+#endif
 
     // Loiter control
     uint16_t loiter_time_max;                // How long we should stay in Loiter Mode for mission scripting (time in seconds)
@@ -582,6 +593,19 @@ class ModeAuto : public Mode {
 
     // True if we have entered AUTO to perform a DO_LAND_START landing sequence and we should report as AUTO RTL mode
     bool auto_RTL;
+
+#if AP_SCRIPTING_ENABLED
+    // nav_script_time command variables
+    struct {
+        bool done;          // true once lua script indicates it has completed
+        uint16_t id;        // unique id to avoid race conditions between commands and lua scripts
+        uint32_t start_ms;  // system time nav_script_time command was received (used for timeout)
+        uint8_t command;    // command number provided by mission command
+        uint8_t timeout_s;  // timeout (in seconds) provided by mission command
+        float arg1;         // 1st argument provided by mission command
+        float arg2;         // 2nd argument provided by mission command
+    } nav_scripting;
+#endif
 };
 
 #if AUTOTUNE_ENABLED == ENABLED

ArduCopter/mode_auto.cpp

@@ -135,7 +135,8 @@ void ModeAuto::run()
         break;
 
     case SubMode::NAVGUIDED:
-#if NAV_GUIDED == ENABLED
+    case SubMode::NAV_SCRIPT_TIME:
+#if NAV_GUIDED == ENABLED || AP_SCRIPTING_ENABLED
         nav_guided_run();
 #endif
         break;
@@ -199,6 +200,31 @@ bool ModeAuto::jump_to_landing_sequence_auto_RTL(ModeReason reason)
     return false;
 }
 
+// lua scripts use this to retrieve the contents of the active command
+bool ModeAuto::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2)
+{
+#if AP_SCRIPTING_ENABLED
+    if (_mode == SubMode::NAV_SCRIPT_TIME) {
+        id = nav_scripting.id;
+        cmd = nav_scripting.command;
+        arg1 = nav_scripting.arg1;
+        arg2 = nav_scripting.arg2;
+        return true;
+    }
+#endif
+    return false;
+}
+
+// lua scripts use this to indicate when they have complete the command
+void ModeAuto::nav_script_time_done(uint16_t id)
+{
+#if AP_SCRIPTING_ENABLED
+    if ((_mode == SubMode::NAV_SCRIPT_TIME) && (id == nav_scripting.id)) {
+        nav_scripting.done = true;
+    }
+#endif
+}
+
 // auto_loiter_start - initialises loitering in auto mode
 //  returns success/failure because this can be called by exit_mission
 bool ModeAuto::loiter_start()
@@ -520,6 +546,12 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
         do_payload_place(cmd);
         break;
 
+#if AP_SCRIPTING_ENABLED
+    case MAV_CMD_NAV_SCRIPT_TIME:
+        do_nav_script_time(cmd);
+        break;
+#endif
+
     //
     // conditional commands
     //
@@ -749,6 +781,12 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
         cmd_complete = verify_nav_delay(cmd);
         break;
 
+#if AP_SCRIPTING_ENABLED
+    case MAV_CMD_NAV_SCRIPT_TIME:
+        cmd_complete = verify_nav_script_time();
+        break;
+#endif
+
     ///
     /// conditional commands
     ///
@@ -904,15 +942,15 @@ void ModeAuto::circle_run()
     }
 }
 
-#if NAV_GUIDED == ENABLED
+#if NAV_GUIDED == ENABLED || AP_SCRIPTING_ENABLED
 // auto_nav_guided_run - allows control by external navigation controller
 //      called by auto_run at 100hz or more
 void ModeAuto::nav_guided_run()
 {
     // call regular guided flight mode run function
     copter.mode_guided.run();
 }
-#endif  // NAV_GUIDED
+#endif  // NAV_GUIDED || AP_SCRIPTING_ENABLED
 
 // auto_loiter_run - loiter in AUTO flight mode
 //      called by auto_run at 100hz or more
@@ -1442,6 +1480,28 @@ void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd)
     gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec", (unsigned)(nav_delay_time_max_ms/1000));
 }
 
+#if AP_SCRIPTING_ENABLED
+// start accepting position, velocity and acceleration targets from lua scripts
+void ModeAuto::do_nav_script_time(const AP_Mission::Mission_Command& cmd)
+{
+    // call regular guided flight mode initialisation
+    if (copter.mode_guided.init(true)) {
+        _mode = SubMode::NAV_SCRIPT_TIME;
+        nav_scripting.done = false;
+        nav_scripting.id++;
+        nav_scripting.start_ms = millis();
+        nav_scripting.command = cmd.content.nav_script_time.command;
+        nav_scripting.timeout_s = cmd.content.nav_script_time.timeout_s;
+        nav_scripting.arg1 = cmd.content.nav_script_time.arg1;
+        nav_scripting.arg2 = cmd.content.nav_script_time.arg2;
+    } else {
+        // for safety we set nav_scripting to done to protect against the mission getting stuck
+        nav_scripting.done = true;
+    }
+}
+#endif
+
+
 /********************************************************************************/
 // Condition (May) commands
 /********************************************************************************/
@@ -1968,4 +2028,17 @@ bool ModeAuto::verify_nav_delay(const AP_Mission::Mission_Command& cmd)
     return false;
 }
 
+#if AP_SCRIPTING_ENABLED
+// check if verify_nav_script_time command has completed
+bool ModeAuto::verify_nav_script_time()
+{
+    // if done or timeout then return true
+    if (nav_scripting.done ||
+        (AP_HAL::millis() - nav_scripting.start_ms) > (nav_scripting.timeout_s * 1000)) {
+        return true;
+    }
+    return false;
+}
+#endif
+
 #endif

Rover/Rover.cpp

@@ -165,7 +165,7 @@ bool Rover::set_target_location(const Location& target_loc)
         return false;
     }
 
-    return control_mode->set_desired_location(target_loc);
+    return mode_guided.set_desired_location(target_loc);
 }
 
 // set target velocity (for use by scripting)
@@ -248,6 +248,32 @@ bool Rover::get_control_output(AP_Vehicle::ControlOutput control_output, float &
     }
     return false;
 }
+
+// returns true if mode supports NAV_SCRIPT_TIME mission commands
+bool Rover::nav_scripting_enable(uint8_t mode)
+{
+    return mode == (uint8_t)mode_auto.mode_number();
+}
+
+// lua scripts use this to retrieve the contents of the active command
+bool Rover::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2)
+{
+    if (control_mode != &mode_auto) {
+        return false;
+    }
+
+    return mode_auto.nav_script_time(id, cmd, arg1, arg2);
+}
+
+// lua scripts use this to indicate when they have complete the command
+void Rover::nav_script_time_done(uint16_t id)
+{
+    if (control_mode != &mode_auto) {
+        return;
+    }
+
+    return mode_auto.nav_script_time_done(id);
+}
 #endif // AP_SCRIPTING_ENABLED
 
 #if STATS_ENABLED == ENABLED

Rover/Rover.h

@@ -274,6 +274,9 @@ class Rover : public AP_Vehicle {
     // set desired turn rate (degrees/sec) and speed (m/s). Used for scripting
     bool set_desired_turn_rate_and_speed(float turn_rate, float speed) override;
     bool get_control_output(AP_Vehicle::ControlOutput control_output, float &control_value) override;
+    bool nav_scripting_enable(uint8_t mode) override;
+    bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2) override;
+    void nav_script_time_done(uint16_t id) override;
 #endif // AP_SCRIPTING_ENABLED
     void stats_update();
     void ahrs_update();

Rover/mode.h

@@ -255,7 +255,7 @@ class ModeAuto : public Mode
     bool is_autopilot_mode() const override { return true; }
 
     // return if external control is allowed in this mode (Guided or Guided-within-Auto)
-    bool in_guided_mode() const override { return _submode == Auto_Guided; }
+    bool in_guided_mode() const override { return _submode == Auto_Guided || _submode == Auto_NavScriptTime; }
 
     // return distance (in meters) to destination
     float get_distance_to_destination() const override;
@@ -271,6 +271,10 @@ class ModeAuto : public Mode
     // start RTL (within auto)
     void start_RTL();
 
+    // lua accessors for nav script time support
+    bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2);
+    void nav_script_time_done(uint16_t id);
+
     AP_Mission mission{
         FUNCTOR_BIND_MEMBER(&ModeAuto::start_command, bool, const AP_Mission::Mission_Command&),
         FUNCTOR_BIND_MEMBER(&ModeAuto::verify_command_callback, bool, const AP_Mission::Mission_Command&),
@@ -294,7 +298,8 @@ class ModeAuto : public Mode
         Auto_RTL,               // perform RTL within auto mode
         Auto_Loiter,            // perform Loiter within auto mode
         Auto_Guided,            // handover control to external navigation system from within auto mode
-        Auto_Stop               // stop the vehicle as quickly as possible
+        Auto_Stop,              // stop the vehicle as quickly as possible
+        Auto_NavScriptTime,     // accept targets from lua scripts while NAV_SCRIPT_TIME commands are executing
     } _submode;
 
 private:
@@ -330,6 +335,10 @@ class ModeAuto : public Mode
     void do_set_home(const AP_Mission::Mission_Command& cmd);
     void do_set_reverse(const AP_Mission::Mission_Command& cmd);
     void do_guided_limits(const AP_Mission::Mission_Command& cmd);
+#if AP_SCRIPTING_ENABLED
+    void do_nav_script_time(const AP_Mission::Mission_Command& cmd);
+    bool verify_nav_script_time();
+#endif
 
     bool waiting_to_start;  // true if waiting for EKF origin before starting mission
     bool auto_triggered;        // true when auto has been triggered to start
@@ -362,6 +371,18 @@ class ModeAuto : public Mode
     uint32_t nav_delay_time_max_ms;  // used for delaying the navigation commands
     uint32_t nav_delay_time_start_ms;
 
+#if AP_SCRIPTING_ENABLED
+    // nav_script_time command variables
+    struct {
+        bool done;          // true once lua script indicates it has completed
+        uint16_t id;        // unique id to avoid race conditions between commands and lua scripts
+        uint32_t start_ms;  // system time nav_script_time command was received (used for timeout)
+        uint8_t command;    // command number provided by mission command
+        uint8_t timeout_s;  // timeout (in seconds) provided by mission command
+        float arg1;         // 1st argument provided by mission command
+        float arg2;         // 2nd argument provided by mission command
+    } nav_scripting;
+#endif
 };