Enhanced attitude control via scripting

ArduCopter/Copter.cpp

@@ -347,16 +347,24 @@ bool Copter::set_target_posvelaccel_NED(const Vector3f& target_pos, const Vector
     return mode_guided.set_destination_posvelaccel(pos_neu_cm, vel_neu_cms, accel_neu_cms, use_yaw, yaw_deg * 100.0, use_yaw_rate, yaw_rate_degs * 100.0, yaw_relative);
 }
 
-bool Copter::set_target_velocity_NED(const Vector3f& vel_ned)
+bool Copter::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 (!flightmode->in_guided_mode()) {
         return false;
     }
 
+    // optionally line up the copter with the velocity vector
+    float yaw_cd = 0.0f;
+    if (align_yaw_to_target) {
+        const float speed_sq = vel_ned.xy().length_squared();
+        if (copter.position_ok() && (speed_sq > (YAW_LOOK_AHEAD_MIN_SPEED * YAW_LOOK_AHEAD_MIN_SPEED))) {
+            yaw_cd = degrees(atan2f(vel_ned.y,vel_ned.x))*100.0f;
+        }
+    }
     // convert vector to neu in cm
     const Vector3f vel_neu_cms(vel_ned.x * 100.0f, vel_ned.y * 100.0f, -vel_ned.z * 100.0f);
-    mode_guided.set_velocity(vel_neu_cms);
+    mode_guided.set_velaccel(vel_neu_cms, Vector3f(), align_yaw_to_target, yaw_cd);
     return true;
 }
 
@@ -389,6 +397,20 @@ bool Copter::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, flo
     mode_guided.set_angle(q, Vector3f{}, climb_rate_ms*100, false);
     return true;
 }
+
+bool Copter::set_target_angle_and_thrust(float roll_deg, float pitch_deg, float yaw_deg, float thrust, bool use_yaw_rate, float yaw_rate_degs)
+{
+    // exit if vehicle is not in Guided mode or Auto-Guided mode
+    if (!flightmode->in_guided_mode()) {
+        return false;
+    }
+
+    Quaternion q;
+    q.from_euler(radians(roll_deg),radians(pitch_deg),radians(yaw_deg));
+
+    mode_guided.set_angle(q, Vector3f{}, thrust, true);
+    return true;
+}
 #endif
 
 #if MODE_CIRCLE_ENABLED

ArduCopter/Copter.h

@@ -678,9 +678,10 @@ 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 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_angle_and_thrust(float roll_deg, float pitch_deg, float yaw_deg, float thrust, bool use_yaw_rate, float yaw_rate_degs) override;
 #endif
 #if MODE_CIRCLE_ENABLED
     bool get_circle_radius(float &radius_m) override;

Rover/Rover.cpp

@@ -174,7 +174,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

@@ -276,7 +276,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

@@ -156,6 +156,7 @@ class AP_Motors {
     float               get_yaw_ff() const { return _yaw_in_ff; }
     float               get_throttle_out() const { return _throttle_out; }
     virtual bool        get_thrust(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

@@ -1517,6 +1517,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
@@ -2384,10 +2388,22 @@ function vehicle:get_circle_radius() end
 ---@return boolean
 function vehicle:set_target_angle_and_climbrate(roll_deg, pitch_deg, yaw_deg, climb_rate_ms, use_yaw_rate, yaw_rate_degs) end
 
+-- desc
+---@param roll_deg number
+---@param pitch_deg number
+---@param yaw_deg number
+---@param thrust number
+---@param use_yaw_rate boolean
+---@param yaw_rate_degs number
+---@return boolean
+function vehicle:set_target_angle_and_thrust(roll_deg, pitch_deg, yaw_deg, thrust, use_yaw_rate, yaw_rate_degs) 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
 ---@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
@@ -3574,6 +3590,30 @@ function fence:get_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
+
 -- desc
 ---@class (exact) stat_t_ud
 local stat_t_ud = {}

libraries/AP_Scripting/generator/description/bindings.desc

@@ -327,8 +327,9 @@ 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 method set_target_velocity_NED boolean Vector3f boolean
 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_angle_and_thrust boolean float -180 180 float -90 90 float -360 360 float'skip_check boolean 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
@@ -713,6 +714,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
@@ -936,6 +938,10 @@ singleton AC_Fence depends AP_FENCE_ENABLED
 singleton AC_Fence rename fence
 singleton AC_Fence method get_breaches uint8_t
 singleton AC_Fence method get_breach_time uint32_t
+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_Filesystem/AP_Filesystem.h depends AP_FILESYSTEM_FILE_READING_ENABLED
 include AP_Filesystem/AP_Filesystem_config.h

libraries/AP_Vehicle/AP_Vehicle.h

@@ -183,9 +183,10 @@ 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 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_angle_and_thrust(float roll_deg, float pitch_deg, float yaw_deg, float thrust, bool use_yaw_rate, float yaw_rate_degs) { return false; }
 
     // command throttle percentage and roll, pitch, yaw target
     // rates. For use with scripting controllers