AC_Avoid: avoidance for non-GPS flight modes

libraries/AC_Avoidance/AC_Avoid.cpp

@@ -10,6 +10,21 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("ENABLE", 1,  AC_Avoid, _enabled, AC_AVOID_ALL),
 
+    // @Param: ANGLE MAX
+    // @DisplayName: Avoidance max lean angle in non-GPS flight modes
+    // @Description: Max lean angle used to avoid obstacles while in non-GPS modes
+    // @Range: 0 4500
+    // @User: Standard
+    AP_GROUPINFO("ANGLE_MAX", 2,  AC_Avoid, _angle_max, 1000),
+
+    // @Param: DIST_MAX
+    // @DisplayName: Avoidance distance maximum in non-GPS flight modes
+    // @Description: Distance from object at which obstacle avoidance will begin in non-GPS modes
+    // @Units: meters
+    // @Range: 3 30
+    // @User: Standard
+    AP_GROUPINFO("DIST_MAX", 3,  AC_Avoid, _dist_max, AC_AVOID_NONGPS_DIST_MAX_DEFAULT),
+
     AP_GROUPEND
 };
 
@@ -38,7 +53,7 @@ void AC_Avoid::adjust_velocity(float kP, float accel_cmss, Vector2f &desired_vel
         adjust_velocity_polygon_fence(kP, accel_cmss_limited, desired_vel);
     }
 
-    if ((_enabled & AC_AVOID_USE_PROXIMITY_SENSOR) > 0) {
+    if ((_enabled & AC_AVOID_USE_PROXIMITY_SENSOR) > 0 && _proximity_enabled) {
         adjust_velocity_proximity(kP, accel_cmss_limited, desired_vel);
     }
 }
@@ -52,6 +67,54 @@ void AC_Avoid::adjust_velocity(float kP, float accel_cmss, Vector3f &desired_vel
     desired_vel.y = des_vel_xy.y;
 }
 
+// adjust roll-pitch to push vehicle away from objects
+// roll and pitch value are in centi-degrees
+void AC_Avoid::adjust_roll_pitch(float &roll, float &pitch, float angle_max)
+{
+    // exit immediately if proximity based avoidance is disabled
+    if ((_enabled & AC_AVOID_USE_PROXIMITY_SENSOR) == 0 || !_proximity_enabled) {
+        return;
+    }
+
+    // exit immediately if angle max is zero
+    if (_angle_max <= 0.0f || angle_max <= 0.0f) {
+        return;
+    }
+
+    float roll_positive = 0.0f;    // maximum positive roll value
+    float roll_negative = 0.0f;    // minimum negative roll value
+    float pitch_positive = 0.0f;   // maximum position pitch value
+    float pitch_negative = 0.0f;   // minimum negative pitch value
+
+    // get maximum positive and negative roll and pitch percentages from proximity sensor
+    get_proximity_roll_pitch_pct(roll_positive, roll_negative, pitch_positive, pitch_negative);
+
+    // add maximum positive and negative percentages together for roll and pitch, convert to centi-degrees
+    Vector2f rp_out((roll_positive + roll_negative) * 4500.0f, (pitch_positive + pitch_negative) * 4500.0f);
+
+    // apply avoidance angular limits
+    // the object avoidance lean angle is never more than 75% of the total angle-limit to allow the pilot to override
+    float angle_limit = constrain_float(_angle_max, 0.0f, angle_max * AC_AVOID_ANGLE_MAX_PERCENT);
+    float vec_len = rp_out.length();
+    if (vec_len > angle_limit) {
+        rp_out *= (angle_limit / vec_len);
+    }
+
+    // add passed in roll, pitch angles
+    rp_out.x += roll;
+    rp_out.y += pitch;
+
+    // apply total angular limits
+    vec_len = rp_out.length();
+    if (vec_len > angle_max) {
+        rp_out *= (angle_max / vec_len);
+    }
+
+    // return adjusted roll, pitch
+    roll = rp_out.x;
+    pitch = rp_out.y;
+}
+
 /*
  * Adjusts the desired velocity for the circular fence.
  */
@@ -264,3 +327,62 @@ float AC_Avoid::get_stopping_distance(float kP, float accel_cmss, float speed) c
         return accel_cmss/(2.0f*kP*kP) + (speed*speed)/(2.0f*accel_cmss);
     }
 }
+
+// convert distance (in meters) to a lean percentage (in 0~1 range) for use in manual flight modes
+float AC_Avoid::distance_to_lean_pct(float dist_m)
+{
+    // ignore objects beyond DIST_MAX
+    if (dist_m <= 0.0f || dist_m >= _dist_max || _dist_max <= 0.0f) {
+        return 0.0f;
+    }
+    // objects at 0m cause maximum lean
+    if (dist_m <= 0.0f) {
+        return 1.0f;
+    }
+    // inverted but linear response
+    return 1.0f - (dist_m / _dist_max);
+}
+
+// returns the maximum positive and negative roll and pitch percentages (in -1 ~ +1 range) based on the proximity sensor
+void AC_Avoid::get_proximity_roll_pitch_pct(float &roll_positive, float &roll_negative, float &pitch_positive, float &pitch_negative)
+{
+    // exit immediately if proximity sensor is not present
+    if (_proximity.get_status() != AP_Proximity::Proximity_Good) {
+        return;
+    }
+
+    uint8_t obj_count = _proximity.get_object_count();
+
+    // if no objects return
+    if (obj_count == 0) {
+        return;
+    }
+
+    // calculate maximum roll, pitch values from objects
+    for (uint8_t i=0; i<obj_count; i++) {
+        float ang_deg, dist_m;
+        if (_proximity.get_object_angle_and_distance(i, ang_deg, dist_m)) {
+            if (dist_m < _dist_max) {
+                // convert distance to lean angle (in 0 to 1 range)
+                float lean_pct = distance_to_lean_pct(dist_m);
+                // convert angle to roll and pitch lean percentages
+                float angle_rad = radians(ang_deg);
+                float roll_pct = -sinf(angle_rad) * lean_pct;
+                float pitch_pct = cosf(angle_rad) * lean_pct;
+                // update roll, pitch maximums
+                if (roll_pct > 0.0f) {
+                    roll_positive = MAX(roll_positive, roll_pct);
+                }
+                if (roll_pct < 0.0f) {
+                    roll_negative = MIN(roll_negative, roll_pct);
+                }
+                if (pitch_pct > 0.0f) {
+                    pitch_positive = MAX(pitch_positive, pitch_pct);
+                }
+                if (pitch_pct < 0.0f) {
+                    pitch_negative = MIN(pitch_negative, pitch_pct);
+                }
+            }
+        }
+    }
+}

libraries/AC_Avoidance/AC_Avoid.h

@@ -17,6 +17,11 @@
 #define AC_AVOID_USE_PROXIMITY_SENSOR   2       // stop based on proximity sensor output
 #define AC_AVOID_ALL                    3       // use fence and promiximity sensor
 
+// definitions for non-GPS avoidance
+#define AC_AVOID_NONGPS_DIST_MAX_DEFAULT    10.0f   // objects over 10m away are ignored (default value for DIST_MAX parameter)
+#define AC_AVOID_ANGLE_MAX_PERCENT          0.75f   // object avoidance max lean angle as a percentage (expressed in 0 ~ 1 range) of total vehicle max lean angle
+#define AC_AVOID_NONGPS_P                   1.0f    // p gain used in conversion from distance to lean angle
+
 /*
  * This class prevents the vehicle from leaving a polygon fence in
  * 2 dimensions by limiting velocity (adjust_velocity).
@@ -35,6 +40,14 @@ public:
     void adjust_velocity(float kP, float accel_cmss, Vector2f &desired_vel);
     void adjust_velocity(float kP, float accel_cmss, Vector3f &desired_vel);
 
+    // adjust roll-pitch to push vehicle away from objects
+    // roll and pitch value are in centi-degrees
+    // angle_max is the user defined maximum lean angle for the vehicle in centi-degrees
+    void adjust_roll_pitch(float &roll, float &pitch, float angle_max);
+
+    // enable/disable proximity based avoidance
+    void proximity_avoidance_enable(bool on_off) { _proximity_enabled = on_off; }
+
     static const struct AP_Param::GroupInfo var_info[];
 
 private:
@@ -90,6 +103,16 @@ private:
      */
     float get_margin() const { return _fence.get_margin() * 100.0f; }
 
+    /*
+     * methods for avoidance in non-GPS flight modes
+     */
+
+    // convert distance (in meters) to a lean percentage (in 0~1 range) for use in manual flight modes
+    float distance_to_lean_pct(float dist_m);
+
+    // returns the maximum positive and negative roll and pitch percentages (in -1 ~ +1 range) based on the proximity sensor
+    void get_proximity_roll_pitch_pct(float &roll_positive, float &roll_negative, float &pitch_positive, float &pitch_negative);
+
     // external references
     const AP_AHRS& _ahrs;
     const AP_InertialNav& _inav;
@@ -98,4 +121,8 @@ private:
 
     // parameters
     AP_Int8 _enabled;
+    AP_Int16 _angle_max;        // maximum lean angle to avoid obstacles (only used in non-GPS flight modes)
+    AP_Float _dist_max;         // distance (in meters) from object at which obstacle avoidance will begin in non-GPS modes
+
+    bool _proximity_enabled = true; // true if proximity sensor based avoidance is enabled (used to allow pilot to enable/disable)
 };