Copter: Feature: Ship Landing

ArduCopter/Copter.cpp

@@ -291,11 +291,12 @@ bool Copter::start_takeoff(float alt)
 bool Copter::set_target_location(const Location& target_loc)
 {
     // exit if vehicle is not in Guided mode or Auto-Guided mode
-    if (!flightmode->in_guided_mode()) {
-        return false;
+    if (flightmode->in_guided_mode()) {
+        return mode_guided.set_destination(target_loc);
     }
 
-    return mode_guided.set_destination(target_loc);
+    // check if rtl is active (RTL or auto-rtl)
+    return flightmode->set_target_location(target_loc);
 }
 
 // set target position (for use by scripting)
@@ -445,6 +446,13 @@ bool Copter::has_ekf_failsafed() const
     return failsafe.ekf;
 }
 
+bool Copter::set_velocity_match(const Vector2f &velocity)
+{
+    velocity_match = velocity;
+    velocity_match_time_ms = AP_HAL::millis();
+    return true;
+}
+
 #endif // AP_SCRIPTING_ENABLED
 
 // returns true if vehicle is landing. Only used by Lua scripts

ArduCopter/Copter.h

@@ -488,6 +488,9 @@ class Copter : public AP_Vehicle {
     AC_WPNav *wp_nav;
     AC_Loiter *loiter_nav;
 
+    uint32_t velocity_match_time_ms;
+    Vector2f velocity_match;
+
 #if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
     AC_CustomControl custom_control{ahrs_view, attitude_control, motors, scheduler.get_loop_period_s()};
 #endif
@@ -694,6 +697,7 @@ class Copter : public AP_Vehicle {
     // lua scripts use this to retrieve EKF failsafe state
     // returns true if the EKF failsafe has triggered
     bool has_ekf_failsafed() const override;
+    bool set_velocity_match(const Vector2f &velocity) override;
 #endif // AP_SCRIPTING_ENABLED
     bool is_landing() const override;
     bool is_taking_off() const override;

ArduCopter/mode.h

@@ -80,6 +80,7 @@ class Mode {
     static void takeoff_stop() { takeoff.stop(); }
 
     virtual bool is_landing() const { return false; }
+    virtual bool set_target_location(const Location& location) { return false; }
 
     // mode requires terrain to be present to be functional
     virtual bool requires_terrain_failsafe() const { return false; }
@@ -987,6 +988,7 @@ class ModeGuided : public Mode {
     //             IF false: climb_rate_cms_or_thrust represents climb_rate (cm/s)
     void set_angle(const Quaternion &attitude_quat, const Vector3f &ang_vel, float climb_rate_cms_or_thrust, bool use_thrust);
 
+    bool set_target_location(const Location& dest_loc) override { return set_destination(dest_loc); }
     bool set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool terrain_alt = false);
     bool set_destination(const Location& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
     bool get_wp(Location &loc) const override;
@@ -1149,6 +1151,11 @@ class ModeLand : public Mode {
 
     void set_land_pause(bool new_value) { land_pause = new_value; }
 
+    // Scripting
+#if AC_PRECLAND_ENABLED
+    bool set_target_location(const Location& location) override;
+#endif
+
 protected:
 
     const char *name() const override { return "LAND"; }
@@ -1185,6 +1192,7 @@ class ModeLoiter : public Mode {
 
 #if AC_PRECLAND_ENABLED
     void set_precision_loiter_enabled(bool value) { _precision_loiter_enabled = value; }
+    bool set_target_location(const Location& location) override;
 #endif
 
 protected:

ArduCopter/mode_land.cpp

@@ -149,6 +149,13 @@ void ModeLand::do_not_use_GPS()
     control_position = false;
 }
 
+#if AC_PRECLAND_ENABLED
+bool ModeLand::set_target_location(const Location& location)
+{
+    return copter.precland.set_target_location(location);
+}
+#endif
+
 // set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts
 //  this is always called from a failsafe so we trigger notification to pilot
 void Copter::set_mode_land_with_pause(ModeReason reason)

ArduCopter/mode_loiter.cpp

@@ -213,4 +213,13 @@ int32_t ModeLoiter::wp_bearing() const
     return loiter_nav->get_bearing_to_target();
 }
 
+#if AC_PRECLAND_ENABLED
+
+bool ModeLoiter::set_target_location(const Location& location)
+{
+    return copter.precland.set_target_location(location);
+}
+
+#endif
+
 #endif

ArduCopter/takeoff.cpp

@@ -189,6 +189,12 @@ void Mode::auto_takeoff_run()
     } else {
         Vector2f vel;
         Vector2f accel;
+
+        // If velocity mactch is recent, use it
+        if (AP_HAL::millis()-copter.velocity_match_time_ms <= 1000) {
+            vel = copter.velocity_match * 100;
+        }
+
         pos_control->input_vel_accel_xy(vel, accel);
     }
     pos_control->update_xy_controller();

Tools/scripts/build_options.py

@@ -339,6 +339,7 @@ def __init__(self,
     Feature('Precision Landing', 'PrecLand', 'AC_PRECLAND_ENABLED', 'Enable Precision Landing support', 0, None),
     Feature('Precision Landing', 'PrecLand - Companion', 'AC_PRECLAND_COMPANION_ENABLED', 'Enable Companion-Supported Precision Landing support', 0, "PrecLand"),  # noqa
     Feature('Precision Landing', 'PrecLand - IRLock', 'AC_PRECLAND_IRLOCK_ENABLED', 'Enable IRLock-Supported Precision Landing support', 0, "PrecLand"),  # noqa
+    Feature('Precision Landing', 'PrecLand - Script', 'AC_PRECLAND_SCRIPT_ENABLED', 'Enable Scripted Precision Landing Support', 0, "PrecLand,SCRIPTING"),  # noqa
 
     #    Feature('Filesystem', 'FILESYSTEM_ESP32_ENABLED', 'AP_FILESYSTEM_ESP32_ENABLED', 'Enable ESP32 Filesystem', 0, None),
     # Feature('Filesystem', 'FILESYSTEM_FATFS', 'AP_FILESYSTEM_FATFS_ENABLED', 'Enable FATFS Filesystem', 0, None),

libraries/AC_PrecLand/AC_PrecLand.cpp

@@ -12,6 +12,7 @@
 #include "AC_PrecLand_IRLock.h"
 #include "AC_PrecLand_SITL_Gazebo.h"
 #include "AC_PrecLand_SITL.h"
+#include "AC_PrecLand_Script.h"
 #include <AP_Logger/AP_Logger.h>
 #include <GCS_MAVLink/GCS.h>
 #include <AP_Vehicle/AP_Vehicle_Type.h>
@@ -41,7 +42,7 @@ const AP_Param::GroupInfo AC_PrecLand::var_info[] = {
     // @Param: TYPE
     // @DisplayName: Precision Land Type
     // @Description: Precision Land Type
-    // @Values: 0:None, 1:CompanionComputer, 2:IRLock, 3:SITL_Gazebo, 4:SITL
+    // @Values: 0:None, 1:CompanionComputer, 2:IRLock, 3:SITL_Gazebo, 4:SITL, 5:Scripted
     // @User: Advanced
     AP_GROUPINFO("TYPE",    1, AC_PrecLand, _type, 0),
 
@@ -261,6 +262,11 @@ void AC_PrecLand::init(uint16_t update_rate_hz)
         case Type::SITL:
             _backend = new AC_PrecLand_SITL(*this, _backend_state);
             break;
+#endif
+#if AC_PRECLAND_SCRIPT_ENABLED
+        case Type::SCRIPT:
+            _backend = new AC_PrecLand_Script(*this, _backend_state);
+            break;
 #endif
     }
 
@@ -483,6 +489,16 @@ void AC_PrecLand::handle_msg(const mavlink_landing_target_t &packet, uint32_t ti
     }
 }
 
+// Allow setting target location from script if scripted backend is active
+bool AC_PrecLand::set_target_location(const Location &location)
+{
+    if (_backend != nullptr) {
+        return _backend->set_target_location(location);
+    } else {
+        return false;
+    }
+}
+
 //
 // Private methods
 //

libraries/AC_PrecLand/AC_PrecLand.h

@@ -6,6 +6,7 @@
 
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include <AP_Math/AP_Math.h>
+#include <AP_Common/Location.h>
 #include <stdint.h>
 #include "PosVelEKF.h"
 #include <AP_HAL/utility/RingBuffer.h>
@@ -87,6 +88,9 @@ class AC_PrecLand
     // process a LANDING_TARGET mavlink message
     void handle_msg(const mavlink_landing_target_t &packet, uint32_t timestamp_ms);
 
+    // Allow setting target location from script if scripted backend is active
+    bool set_target_location(const Location &location);
+
     // State of the Landing Target Location
     enum class TargetState: uint8_t {
         TARGET_NEVER_SEEN = 0,
@@ -138,6 +142,9 @@ class AC_PrecLand
 #endif
 #if AC_PRECLAND_SITL_ENABLED
         SITL = 4,
+#endif
+#if AC_PRECLAND_SCRIPT_ENABLED
+        SCRIPT = 5,
 #endif
     };
 

libraries/AC_PrecLand/AC_PrecLand_Backend.h

@@ -7,6 +7,7 @@
 #include "AC_PrecLand.h"
 #include <AP_Math/AP_Math.h>
 #include <AC_PID/AC_PID.h>
+#include <AP_Common/Location.h>
 
 
 class AC_PrecLand_Backend
@@ -42,6 +43,9 @@ class AC_PrecLand_Backend
     // parses a mavlink message from the companion computer
     virtual void handle_msg(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) {};
 
+    // handles a set target location lua script call
+    virtual bool set_target_location(const Location &location) { return false; };
+
     // get bus parameter
     int8_t get_bus(void) const { return _frontend._bus.get(); }
 

libraries/AC_PrecLand/AC_PrecLand_Script.cpp

@@ -0,0 +1,75 @@
+#include "AC_PrecLand_config.h"
+
+#if AC_PRECLAND_SCRIPT_ENABLED
+
+#include "AC_PrecLand_Script.h"
+#include <AP_HAL/AP_HAL.h>
+#include <GCS_MAVLink/GCS.h>
+#include <AP_AHRS/AP_AHRS.h>
+
+// perform any required initialisation of backend
+void AC_PrecLand_Script::init()
+{
+    // set healthy
+    _state.healthy = true;
+    _have_los_meas = false;
+}
+
+// retrieve updates from sensor
+void AC_PrecLand_Script::update()
+{
+    _have_los_meas = AP_HAL::millis()-_los_meas_time_ms <= 1000;
+}
+
+// provides a unit vector towards the target in body frame
+//  returns same as have_los_meas()
+bool AC_PrecLand_Script::get_los_body(Vector3f& ret)
+{
+    if (have_los_meas()) {
+        ret = _los_meas_body;
+        return true;
+    }
+    return false;
+}
+
+// returns system time in milliseconds of last los measurement
+uint32_t AC_PrecLand_Script::los_meas_time_ms()
+{
+    return _los_meas_time_ms;
+}
+
+// return true if there is a valid los measurement available
+bool AC_PrecLand_Script::have_los_meas()
+{
+    return _have_los_meas;
+}
+
+// return distance to target
+float AC_PrecLand_Script::distance_to_target()
+{
+    return _distance_to_target;
+}
+
+bool AC_PrecLand_Script::set_target_location(const Location &location)
+{
+    const Matrix3f body_to_ned = AP::ahrs().get_rotation_body_to_ned();
+
+    Location body_location;
+    if (!AP::ahrs().get_location(body_location)) { // If current location is unknown, return
+        return false;
+    }
+    body_location.change_alt_frame(location.get_alt_frame()); // the alt frame does not matter if it is relative
+
+    const Vector3f body_to_target = -(location.get_distance_NED(body_location));
+    _los_meas_body = body_to_ned.mul_transpose(body_to_target);
+
+    _distance_to_target = _los_meas_body.length();
+    _los_meas_body /= _distance_to_target;
+
+    _los_meas_time_ms = AP_HAL::millis();
+    return true;
+}
+
+//bool AC_PrecLand_Script::set
+
+#endif // AC_PRECLAND_SCRIPT_ENABLED

libraries/AC_PrecLand/AC_PrecLand_Script.h

@@ -0,0 +1,53 @@
+#pragma once
+
+#include "AC_PrecLand_config.h"
+
+#if AC_PRECLAND_SCRIPT_ENABLED
+
+#include "AC_PrecLand_Backend.h"
+#include <AP_Math/AP_Math.h>
+
+/*
+ * AC_PrecLand_Companion - implements precision landing using target vectors provided
+ *                         by lua script using set_target_location while in RTL mode
+ */
+
+class AC_PrecLand_Script : public AC_PrecLand_Backend
+{
+public:
+    // Constructor
+    using AC_PrecLand_Backend::AC_PrecLand_Backend;
+
+    // perform any required initialisation of backend
+    void init() override;
+
+    // retrieve updates from sensor
+    void update() override;
+
+    // provides a unit vector towards the target in body frame
+    //  returns same as have_los_meas()
+    bool get_los_body(Vector3f& ret) override;
+
+    // returns system time in milliseconds of last los measurement
+    uint32_t los_meas_time_ms() override;
+
+    // return true if there is a valid los measurement available
+    bool have_los_meas() override;
+
+    // returns distance to target in meters (0 means distance is not known)
+    float distance_to_target() override;
+
+    //
+    bool set_target_location(const Location &location) override;
+
+private:
+    float               _distance_to_target;    // distance from the camera to target in meters
+
+    Vector3f            _los_meas_body;         // unit vector in body frame pointing towards target
+    uint32_t            _los_meas_time_ms;      // system time in milliseconds when los was measured
+    bool                _have_los_meas;         // true if there is a valid data
+
+};
+
+
+#endif // AC_PRECLAND_SCRIPT_ENABLED

libraries/AC_PrecLand/AC_PrecLand_config.h

@@ -25,3 +25,7 @@
 #ifndef AC_PRECLAND_SITL_GAZEBO_ENABLED
 #define AC_PRECLAND_SITL_GAZEBO_ENABLED (AC_PRECLAND_BACKEND_DEFAULT_ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_SITL)
 #endif
+
+#ifndef AC_PRECLAND_SCRIPT_ENABLED
+#define AC_PRECLAND_SCRIPT_ENABLED AC_PRECLAND_BACKEND_DEFAULT_ENABLED
+#endif