Copter: configurable max lean angle

ANGLE_MAX parameter allows limiting the roll and pitch angles during
manual and auto flight modes to anywhere from 10 to 80 degrees

ArduCopter/ArduCopter.pde

@@ -1300,6 +1300,9 @@ static void super_slow_loop()
     // pass latest alt hold kP value to navigation controller
     wp_nav.set_althold_kP(g.pi_alt_hold.kP());
 
+    // update latest lean angle to navigation controller
+    wp_nav.set_lean_angle_max(g.angle_max);
+
     // log battery info to the dataflash
     if ((g.log_bitmask & MASK_LOG_CURRENT) && motors.armed())
         Log_Write_Current();
@@ -1705,9 +1708,8 @@ void update_roll_pitch_mode(void)
             update_simple_mode();
         }
 
-        // copy control_roll and pitch for reporting purposes
+        // convert pilot input to lean angles
-        control_roll            = g.rc_1.control_in;
+        get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, control_roll, control_pitch);
-        control_pitch           = g.rc_2.control_in;
 
         // pass desired roll, pitch to stabilize attitude controllers
         get_stabilize_roll(control_roll);
@@ -1733,9 +1735,8 @@ void update_roll_pitch_mode(void)
             update_simple_mode();
         }
 
-        // copy pilot input to control_roll and pitch for reporting purposes
+        // convert pilot input to lean angles
-        control_roll            = g.rc_1.control_in;
+        get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, control_roll, control_pitch);
-        control_pitch           = g.rc_2.control_in;
 
         // mix in user control with optical flow
         get_stabilize_roll(get_of_roll(control_roll));

ArduCopter/Attitude.pde

@@ -1,5 +1,30 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+// get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
+// returns desired angle in centi-degrees
+static void get_pilot_desired_lean_angles(int16_t roll_in, int16_t pitch_in, int16_t &roll_out, int16_t &pitch_out)
+{
+    static float _scaler = 1.0;
+    static int16_t _angle_max = 0;
+
+    // return immediately if no scaling required
+    if (g.angle_max == ROLL_PITCH_INPUT_MAX) {
+        roll_out = roll_in;
+        pitch_out = pitch_in;
+        return;
+    }
+
+    // check if angle_max has been updated and redo scaler
+    if (g.angle_max != _angle_max) {
+        _angle_max = g.angle_max;
+        _scaler = (float)g.angle_max/(float)ROLL_PITCH_INPUT_MAX;
+    }
+
+    // convert pilot input to lean angle
+    roll_out = roll_in * _scaler;
+    pitch_out = pitch_in * _scaler;
+}
+
 static void
 get_stabilize_roll(int32_t target_angle)
 {
@@ -7,7 +32,7 @@ get_stabilize_roll(int32_t target_angle)
     target_angle = wrap_180_cd(target_angle - ahrs.roll_sensor);
 
     // limit the error we're feeding to the PID
-    target_angle = constrain_int32(target_angle, -4500, 4500);
+    target_angle = constrain_int32(target_angle, -g.angle_max, g.angle_max);
 
     // convert to desired rate
     int32_t target_rate = g.pi_stabilize_roll.kP() * target_angle;
@@ -23,7 +48,7 @@ get_stabilize_pitch(int32_t target_angle)
     target_angle            = wrap_180_cd(target_angle - ahrs.pitch_sensor);
 
     // limit the error we're feeding to the PID
-    target_angle            = constrain_int32(target_angle, -4500, 4500);
+    target_angle            = constrain_int32(target_angle, -g.angle_max, g.angle_max);
 
     // convert to desired rate
     int32_t target_rate = g.pi_stabilize_pitch.kP() * target_angle;
@@ -114,10 +139,10 @@ get_acro_level_rates()
     int32_t target_rate = 0;
 
     if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
-        if (roll_angle > 4500){
+        if (roll_angle > g.angle_max){
-            target_rate =  g.pi_stabilize_roll.get_p(4500-roll_angle);
+            target_rate =  g.pi_stabilize_roll.get_p(g.angle_max-roll_angle);
-        }else if (roll_angle < -4500) {
+        }else if (roll_angle < -g.angle_max) {
-            target_rate =  g.pi_stabilize_roll.get_p(-4500-roll_angle);
+            target_rate =  g.pi_stabilize_roll.get_p(-g.angle_max-roll_angle);
         }
     }
     roll_angle   = constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE);
@@ -131,10 +156,10 @@ get_acro_level_rates()
     target_rate = 0;
 
     if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
-        if (pitch_angle > 4500){
+        if (pitch_angle > g.angle_max){
-            target_rate =  g.pi_stabilize_pitch.get_p(4500-pitch_angle);
+            target_rate =  g.pi_stabilize_pitch.get_p(g.angle_max-pitch_angle);
-        }else if (pitch_angle < -4500) {
+        }else if (pitch_angle < -g.angle_max) {
-            target_rate =  g.pi_stabilize_pitch.get_p(-4500-pitch_angle);
+            target_rate =  g.pi_stabilize_pitch.get_p(-g.angle_max-pitch_angle);
         }
     }
     pitch_angle  = constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE);

ArduCopter/Parameters.h

@@ -88,7 +88,8 @@ public:
         k_param_sonar_gain,
         k_param_ch8_option,
         k_param_arming_check_enabled,
-        k_param_sprayer,    // 33
+        k_param_sprayer,
+        k_param_angle_max,              // 34
 
         // 65: AP_Limits Library
         k_param_limits = 65,            // deprecated - remove
@@ -299,6 +300,7 @@ public:
     AP_Int8         battery_curr_pin;
     AP_Int8         rssi_pin;
     AP_Int8         wp_yaw_behavior;            // controls how the autopilot controls yaw during missions
+    AP_Int16        angle_max;                  // maximum lean angle of the copter in centi-degrees
 
     // Waypoints
     //

ArduCopter/Parameters.pde

@@ -423,6 +423,13 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Standard
     GSCALAR(arming_check_enabled, "ARMING_CHECK",   1),
 
+    // @Param: ANGLE_MAX
+    // @DisplayName: Angle Max
+    // @Description: Maximum lean angle in all flight modes
+    // @Range 1000 8000
+    // @User: Advanced
+    GSCALAR(angle_max, "ANGLE_MAX",                 DEFAULT_ANGLE_MAX),
+
 #if FRAME_CONFIG ==     HELI_FRAME
     // @Group: HS1_
     // @Path: ../libraries/RC_Channel/RC_Channel.cpp

ArduCopter/config.h

@@ -814,12 +814,11 @@
 //////////////////////////////////////////////////////////////////////////////
 // Stabilize Rate Control
 //
-
+#ifndef ROLL_PITCH_INPUT_MAX
-#ifndef MAX_INPUT_ROLL_ANGLE
+ # define ROLL_PITCH_INPUT_MAX      4500            // roll, pitch input range
- # define MAX_INPUT_ROLL_ANGLE      4500
 #endif
-#ifndef MAX_INPUT_PITCH_ANGLE
+#ifndef DEFAULT_ANGLE_MAX
- # define MAX_INPUT_PITCH_ANGLE     4500
+ # define DEFAULT_ANGLE_MAX         4500            // ANGLE_MAX parameters default value
 #endif
 #ifndef RATE_ROLL_P
  # define RATE_ROLL_P        		0.150f

ArduCopter/motors.pde

@@ -322,6 +322,14 @@ static void pre_arm_checks(bool display_failure)
         }
     }
 
+    // lean angle parameter check
+    if (g.angle_max < 1000 || g.angle_max > 8000) {
+        if (display_failure) {
+            gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check ANGLE_MAX"));
+        }
+        return;
+    }
+
     // if we've gotten this far then pre arm checks have completed
     ap.pre_arm_check = true;
 }

ArduCopter/radio.pde

@@ -20,8 +20,8 @@ static void default_dead_zones()
 static void init_rc_in()
 {
     // set rc channel ranges
-    g.rc_1.set_angle(MAX_INPUT_ROLL_ANGLE);
+    g.rc_1.set_angle(ROLL_PITCH_INPUT_MAX);
-    g.rc_2.set_angle(MAX_INPUT_PITCH_ANGLE);
+    g.rc_2.set_angle(ROLL_PITCH_INPUT_MAX);
 #if FRAME_CONFIG == HELI_FRAME
     // we do not want to limit the movment of the heli's swash plate
     g.rc_3.set_range(0, 1000);

libraries/AC_WPNav/AC_WPNav.cpp

@@ -90,6 +90,7 @@ AC_WPNav::AC_WPNav(AP_InertialNav* inav, AP_AHRS* ahrs, APM_PI* pid_pos_lat, APM
     _vel_last(0,0,0),
     _loiter_leash(WPNAV_MIN_LEASH_LENGTH),
     _loiter_accel_cms(WPNAV_LOITER_ACCEL_MAX),
+    _lean_angle_max_cd(MAX_LEAN_ANGLE),
     _wp_leash_xy(WPNAV_MIN_LEASH_LENGTH),
     _wp_leash_z(WPNAV_MIN_LEASH_LENGTH),
     _track_accel(0),
@@ -162,8 +163,8 @@ void AC_WPNav::init_loiter_target(const Vector3f& position, const Vector3f& velo
     _target_vel.y = velocity.y;
 
     // initialise desired roll and pitch to current roll and pitch.  This avoids a random twitch between now and when the loiter controller is first run
-    _desired_roll = constrain_int32(_ahrs->roll_sensor,-MAX_LEAN_ANGLE,MAX_LEAN_ANGLE);
+    _desired_roll = constrain_int32(_ahrs->roll_sensor,-_lean_angle_max_cd,_lean_angle_max_cd);
-    _desired_pitch = constrain_int32(_ahrs->pitch_sensor,-MAX_LEAN_ANGLE,MAX_LEAN_ANGLE);
+    _desired_pitch = constrain_int32(_ahrs->pitch_sensor,-_lean_angle_max_cd,_lean_angle_max_cd);
 
     // initialise pilot input
     _pilot_vel_forward_cms = 0;
@@ -366,8 +367,8 @@ void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f
     _flags.fast_waypoint = false;
 
     // initialise desired roll and pitch to current roll and pitch.  This avoids a random twitch between now and when the wpnav controller is first run
-    _desired_roll = constrain_int32(_ahrs->roll_sensor,-MAX_LEAN_ANGLE,MAX_LEAN_ANGLE);
+    _desired_roll = constrain_int32(_ahrs->roll_sensor,-_lean_angle_max_cd,_lean_angle_max_cd);
-    _desired_pitch = constrain_int32(_ahrs->pitch_sensor,-MAX_LEAN_ANGLE,MAX_LEAN_ANGLE);
+    _desired_pitch = constrain_int32(_ahrs->pitch_sensor,-_lean_angle_max_cd,_lean_angle_max_cd);
 
     // reset target velocity - only used by loiter controller's interpretation of pilot input
     _target_vel.x = 0;
@@ -615,8 +616,8 @@ void AC_WPNav::get_loiter_acceleration_to_lean_angles(float accel_lat, float acc
     accel_right = -accel_lat*_sin_yaw + accel_lon*_cos_yaw;
 
     // update angle targets that will be passed to stabilize controller
-    _desired_roll = constrain_float(fast_atan(accel_right*_cos_pitch/(-z_accel_meas))*(18000/M_PI), -MAX_LEAN_ANGLE, MAX_LEAN_ANGLE);
+    _desired_roll = constrain_float(fast_atan(accel_right*_cos_pitch/(-z_accel_meas))*(18000/M_PI), -_lean_angle_max_cd, _lean_angle_max_cd);
-    _desired_pitch = constrain_float(fast_atan(-accel_forward/(-z_accel_meas))*(18000/M_PI), -MAX_LEAN_ANGLE, MAX_LEAN_ANGLE);
+    _desired_pitch = constrain_float(fast_atan(-accel_forward/(-z_accel_meas))*(18000/M_PI), -_lean_angle_max_cd, _lean_angle_max_cd);
 }
 
 // get_bearing_cd - return bearing in centi-degrees between two positions

libraries/AC_WPNav/AC_WPNav.h

@@ -143,6 +143,9 @@ public:
     /// get_waypoint_acceleration - returns acceleration in cm/s/s during missions
     float get_waypoint_acceleration() const { return _wp_accel_cms.get(); }
 
+    /// set_lean_angle_max - limits maximum lean angle
+    void set_lean_angle_max(int16_t angle_cd) { if (angle_cd >= 1000 && angle_cd <= 8000) {_lean_angle_max_cd = angle_cd;} }
+
     static const struct AP_Param::GroupInfo var_info[];
 
 protected:
@@ -216,6 +219,7 @@ protected:
     Vector3f    _vel_last;              // previous iterations velocity in cm/s
     float       _loiter_leash;          // loiter's horizontal leash length in cm.  used to stop the pilot from pushing the target location too far from the current location
     float       _loiter_accel_cms;      // loiter's acceleration in cm/s/s
+    int16_t     _lean_angle_max_cd;     // maximum lean angle in centi-degrees
 
     // waypoint controller internal variables
     Vector3f    _origin;                // starting point of trip to next waypoint in cm from home (equivalent to next_WP)