Enhanced attitude control via scripting

ArduCopter/Copter.cpp

@@ -350,15 +350,24 @@ bool Copter::set_target_posvelaccel_NED(const Vector3f& target_pos_ned_m, const
     return mode_guided.set_pos_vel_accel_NEU_m(pos_neu_m, vel_neu_ms, accel_neu_mss, use_yaw, radians(yaw_deg), use_yaw_rate, radians(yaw_rate_degs), yaw_relative);
 }
 
-bool Copter::set_target_velocity_NED(const Vector3f& target_vel_ned_ms)
+bool Copter::set_target_velocity_NED(const Vector3f& target_vel_ned_ms, bool align_yaw_to_target)
 {
     // exit if vehicle is not in Guided mode or Auto-Guided mode
     if (!flightmode->in_guided_mode()) {
         return false;
     }
 
+    // optionally line up the copter with the velocity vector
+    float yaw_rads = 0.0f;
+    if (align_yaw_to_target) {
+        const float speed_sq = target_vel_ned_ms.xy().length_squared();
+        if (copter.position_ok() && (speed_sq > (YAW_LOOK_AHEAD_MIN_SPEED_MS * YAW_LOOK_AHEAD_MIN_SPEED_MS))) {
+            yaw_rads = atan2f(target_vel_ned_ms.y, target_vel_ned_ms.x);
+        }
+    }
+
     const Vector3f vel_neu_ms{target_vel_ned_ms.x, target_vel_ned_ms.y, -target_vel_ned_ms.z};
-    mode_guided.set_vel_NEU_ms(vel_neu_ms);
+    mode_guided.set_vel_accel_NEU_m(vel_neu_ms, Vector3f(), align_yaw_to_target, yaw_rads);
     return true;
 }
 
@@ -413,6 +422,20 @@ bool Copter::set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_
     return true;
 }
 
+// set target roll pitch and yaw angles and roll pitch and yaw rates with throttle (for use by scripting)
+bool Copter::set_target_angle_and_rate_and_throttle(float roll_deg, float pitch_deg, float yaw_deg, float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps, float throttle)
+{
+    Quaternion q;
+    q.from_euler(radians(roll_deg),radians(pitch_deg),radians(yaw_deg));
+
+    // Convert from degrees per second to radians per second
+    Vector3f ang_vel_body { roll_rate_dps, pitch_rate_dps, yaw_rate_dps };
+    ang_vel_body *= DEG_TO_RAD;
+
+    mode_guided.set_angle(q, ang_vel_body, throttle, true);
+    return true;
+}
+
 // Register a custom mode with given number and names
 AP_Vehicle::custom_mode_state* Copter::register_custom_mode(const uint8_t num, const char* full_name, const char* short_name)
 {

ArduCopter/Copter.h

@@ -684,10 +684,11 @@ class Copter : public AP_Vehicle {
     bool set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool is_terrain_alt) override;
     bool set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel) override;
     bool set_target_posvelaccel_NED(const Vector3f& target_pos, const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative) override;
-    bool set_target_velocity_NED(const Vector3f& vel_ned) override;
+    bool set_target_velocity_NED(const Vector3f& vel_ned, bool align_yaw_to_target) override;
     bool set_target_velaccel_NED(const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool relative_yaw) override;
     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;
+    bool set_target_angle_and_rate_and_throttle(float roll_deg, float pitch_deg, float yaw_deg, float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps, float throttle) override;
 
     // Register a custom mode with given number and names
     AP_Vehicle::custom_mode_state* register_custom_mode(const uint8_t number, const char* full_name, const char* short_name) override;

Rover/Rover.cpp

@@ -172,7 +172,7 @@ bool Rover::set_target_location(const Location& target_loc)
 
 #if AP_SCRIPTING_ENABLED
 // set target velocity (for use by scripting)
-bool Rover::set_target_velocity_NED(const Vector3f& vel_ned)
+bool Rover::set_target_velocity_NED(const Vector3f& vel_ned, bool align_yaw_to_target)
 {
     // exit if vehicle is not in Guided mode or Auto-Guided mode
     if (!control_mode->in_guided_mode()) {

Rover/Rover.h

@@ -268,7 +268,7 @@ class Rover : public AP_Vehicle {
 #endif
 
 #if AP_SCRIPTING_ENABLED
-    bool set_target_velocity_NED(const Vector3f& vel_ned) override;
+    bool set_target_velocity_NED(const Vector3f& vel_ned, bool align_yaw_to_target) override;
     bool set_steering_and_throttle(float steering, float throttle) override;
     bool get_steering_and_throttle(float& steering, float& throttle) override;
     // set desired turn rate (degrees/sec) and speed (m/s). Used for scripting

libraries/AP_Motors/AP_Motors_Class.h

@@ -152,6 +152,7 @@ class AP_Motors {
     float               get_throttle_out() const { return _throttle_out; }
     virtual bool        get_thrust(uint8_t motor_num, float& thr_out) const { return false; }
     virtual bool        get_raw_motor_throttle(uint8_t motor_num, float& thr_out) const { return false; }
+    float               get_throttle_in() const { return _throttle_in; }
     float               get_throttle() const { return constrain_float(_throttle_filter.get(), 0.0f, 1.0f); }
     float               get_throttle_bidirectional() const { return constrain_float(2 * (_throttle_filter.get() - 0.5f), -1.0f, 1.0f); }
     float               get_throttle_slew_rate() const { return _throttle_slew_rate; }

libraries/AP_Scripting/docs/docs.lua

@@ -1821,6 +1821,10 @@ function motors:get_forward() end
 ---@return number
 function motors:get_throttle() end
 
+-- get thrust motor input
+---@return number
+function motors:get_throttle_in() end
+
 -- get throttle motor output
 ---@return integer
 ---| '0' # Shut down
@@ -2705,10 +2709,23 @@ function vehicle:set_target_angle_and_climbrate(roll_deg, pitch_deg, yaw_deg, cl
 ---@return boolean -- true if successful
 function vehicle:set_target_rate_and_throttle(roll_rate_dps, pitch_rate_dps, yaw_rate_dps, throttle) end
 
+-- Set vehicle's roll, pitch, and yaw angles and rates with throttle in guided mode
+---@param roll_deg number -- roll angle in degrees
+---@param pitch_deg number -- pitch angle in degrees
+---@param yaw_deg number -- yaw angle in degrees
+---@param roll_rate_dps number -- roll rate in degrees per second
+---@param pitch_rate_dps number -- pitch rate in degrees per second
+---@param yaw_rate_dps number -- yaw rate in degrees per second
+---@param throttle number -- throttle demand 0.0 to 1.0
+---@return boolean -- true on success
+function vehicle:set_target_angle_and_rate_and_throttle(roll_deg, pitch_deg, yaw_deg, roll_rate_dps, pitch_rate_dps, yaw_rate_dps, throttle) end
+
+-- desc
 -- Sets the target velocity using a Vector3f object in a guided mode.
 ---@param vel_ned Vector3f_ud -- North, East, Down meters / second
+---@param align_yaw_to_target? boolean -- optionally align the yaw to the target
 ---@return boolean -- true on success
-function vehicle:set_target_velocity_NED(vel_ned) end
+function vehicle:set_target_velocity_NED(vel_ned, align_yaw_to_target) end
 
 -- desc
 ---@param target_vel Vector3f_ud
@@ -3999,6 +4016,30 @@ function fence:get_margin_breach_time() end
 ---| 8 # Minimum altitude
 function fence:get_breaches() end
 
+-- Returns minimum safe altitude (i.e. alt_min + margin)
+---@return number 
+function fence:get_safe_alt_min() end
+
+-- Returns maximum safe altitude (i.e. alt_max - margin)
+---@return number 
+function fence:get_safe_alt_max() end
+
+-- Returns configured fences
+---@return integer fence_type bitmask
+---| 1 # Maximim altitude
+---| 2 # Circle
+---| 4 # Polygon
+---| 8 # Minimum altitude
+function fence:present() end
+
+-- Returns enabled fences
+---@return integer fence_type bitmask
+---| 1 # Maximim altitude
+---| 2 # Circle
+---| 4 # Polygon
+---| 8 # Minimum altitude
+function fence:get_enabled_fences() end
+
 -- Returns the type bitmask of any fence whose margins have been crossed
 ---@return integer fence_type bitmask
 ---| 1 # Maximim altitude

libraries/AP_Scripting/generator/description/bindings.desc

@@ -360,9 +360,10 @@ singleton AP_Vehicle method set_target_pos_NED boolean Vector3f boolean float -3
 singleton AP_Vehicle method set_target_posvel_NED boolean Vector3f Vector3f
 singleton AP_Vehicle method set_target_posvelaccel_NED boolean Vector3f Vector3f Vector3f boolean float -360 +360 boolean float'skip_check boolean
 singleton AP_Vehicle method set_target_velaccel_NED boolean Vector3f Vector3f boolean float -360 +360 boolean float'skip_check boolean
-singleton AP_Vehicle method set_target_velocity_NED boolean Vector3f
+singleton AP_Vehicle manual set_target_velocity_NED lua_AP_Vehicle_set_target_velocity_NED 2 1
 singleton AP_Vehicle method set_target_angle_and_climbrate boolean float -180 180 float -90 90 float -360 360 float'skip_check boolean float'skip_check
 singleton AP_Vehicle method set_target_rate_and_throttle boolean float'skip_check float'skip_check float'skip_check float'skip_check
+singleton AP_Vehicle method set_target_angle_and_rate_and_throttle boolean float -180 180 float -90 90 float -360 360 float'skip_check float'skip_check float'skip_check float'skip_check
 singleton AP_Vehicle method get_circle_radius boolean float'Null
 singleton AP_Vehicle method set_circle_rate boolean float'skip_check
 singleton AP_Vehicle method set_steering_and_throttle boolean float -1 1 float -1 1
@@ -782,6 +783,7 @@ singleton AP::motors() method get_pitch_ff float
 singleton AP::motors() method get_yaw float
 singleton AP::motors() method get_yaw_ff float
 singleton AP::motors() method get_throttle float
+singleton AP::motors() method get_throttle_in float
 singleton AP::motors() method get_forward float
 singleton AP::motors() method get_lateral float
 singleton AP::motors() method get_spool_state uint8_t
@@ -1048,6 +1050,10 @@ singleton AC_Fence method get_breach_time uint32_t
 singleton AC_Fence method get_margin_breaches uint8_t
 singleton AC_Fence method get_margin_breach_time uint32_t
 singleton AC_Fence method get_breach_distance float uint8_t'skip_check
+singleton AC_Fence method get_safe_alt_min float
+singleton AC_Fence method get_safe_alt_max float
+singleton AC_Fence method present uint8_t
+singleton AC_Fence method get_enabled_fences uint8_t
 
 include AP_Rally/AP_Rally.h depends HAL_RALLY_ENABLED
 

libraries/AP_Scripting/lua_bindings.cpp

@@ -1296,4 +1296,38 @@ int lua_gps_inject_data(lua_State *L)
 
 #endif  // AP_GPS_ENABLED
 
+#if !defined(HAL_BUILD_AP_PERIPH)
+int lua_AP_Vehicle_set_target_velocity_NED(lua_State *L)
+{
+    const int args = lua_gettop(L);
+
+    if (args > 3) {
+        return luaL_argerror(L, args, "too many arguments");
+    } else if (args < 2) {
+        return luaL_argerror(L, args, "too few arguments");
+    }
+
+    AP_Vehicle * ud = check_AP_Vehicle(L);
+    Vector3f & data_2 = *check_Vector3f(L, 2);
+
+    bool yaw_to_target = false;
+
+    if (args == 3) {
+        yaw_to_target = static_cast<bool>(lua_toboolean(L, 3));
+    }
+#if AP_SCHEDULER_ENABLED
+    AP::scheduler().get_semaphore().take_blocking();
+#endif
+    const bool data = static_cast<bool>(ud->set_target_velocity_NED(
+            data_2,
+            yaw_to_target));
+
+#if AP_SCHEDULER_ENABLED
+    AP::scheduler().get_semaphore().give();
+#endif
+    lua_pushboolean(L, data);
+    return 1;
+}
+#endif // !defined(HAL_BUILD_AP_PERIPH)
+
 #endif  // AP_SCRIPTING_ENABLED

libraries/AP_Scripting/lua_bindings.h

@@ -36,3 +36,4 @@ int lua_range_finder_handle_script_msg(lua_State *L);
 int lua_GCS_command_int(lua_State *L);
 int lua_DroneCAN_get_FlexDebug(lua_State *L);
 int lua_gps_inject_data(lua_State *L);
+int lua_AP_Vehicle_set_target_velocity_NED(lua_State *L);

libraries/AP_Vehicle/AP_Vehicle.h

@@ -192,10 +192,11 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks {
     virtual bool set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool is_terrain_alt) { return false; }
     virtual bool set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel) { return false; }
     virtual bool set_target_posvelaccel_NED(const Vector3f& target_pos, const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative) { return false; }
-    virtual bool set_target_velocity_NED(const Vector3f& vel_ned) { return false; }
+    virtual bool set_target_velocity_NED(const Vector3f& vel_ned, bool align_yaw_to_target = false) { return false; }
     virtual bool set_target_velaccel_NED(const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative) { return false; }
     virtual 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) { return false; }
     virtual bool set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps, float throttle) { return false; }
+    virtual bool set_target_angle_and_rate_and_throttle(float roll_deg, float pitch_deg, float yaw_deg, float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps, float throttle) { return false; }
 
     // command throttle percentage and roll, pitch, yaw target
     // rates. For use with scripting controllers