AC_WPNav: add angle limits and set from AC's throttle controller

ArduCopter/Attitude.pde

@@ -1075,6 +1075,10 @@ get_throttle_rate(int16_t z_target_speed)
         set_throttle_out(g.throttle_cruise+output, true);
     }
 
+    // limit loiter & waypoint navigation from causing too much lean
+    // To-Do: ensure that this limit is cleared when this throttle controller is not running so that loiter is not left constrained for Position mode
+    wp_nav.set_angle_limit(4500 - constrain((z_rate_error - 100) * 10, 0, 3500));
+
     // update throttle cruise
     // TO-DO: this may not be correct because g.rc_3.servo_out has not been updated for this iteration
     if( z_target_speed == 0 ) {

ArduCopter/system.pde

@@ -448,6 +448,7 @@ static void set_mode(uint8_t mode)
         set_roll_pitch_mode(POSITION_RP);
         set_throttle_mode(POSITION_THR);
         set_nav_mode(POSITION_NAV);
+        wp_nav.clear_angle_limit();     // ensure there are no left over angle limits from throttle controller.  To-Do: move this to the exit routine of throttle controller
         break;
 
     case GUIDED:

libraries/AC_WPNav/AC_WPNav.cpp

@@ -26,7 +26,8 @@ AC_WPNav::AC_WPNav(AP_InertialNav* inav, APM_PI* pid_pos_lat, APM_PI* pid_pos_lo
     _pid_pos_lat(pid_pos_lat),
     _pid_pos_lon(pid_pos_lon),
     _pid_rate_lat(pid_rate_lat),
-    _pid_rate_lon(pid_rate_lon)
+    _pid_rate_lon(pid_rate_lon),
+    _lean_angle_max(MAX_LEAN_ANGLE)
 {
     AP_Param::setup_object_defaults(this, var_info);
 }
@@ -340,8 +341,8 @@ void AC_WPNav::get_loiter_accel_lat_lon(int16_t accel_lat, int16_t accel_lon)
     accel_right = -accel_lat*_sin_yaw + accel_lon*_cos_yaw;
 
     // update angle targets that will be passed to stabilize controller
-    _desired_roll = constrain((accel_right/(-z_accel_meas))*(18000/M_PI), -4500, 4500);
+    _desired_roll = constrain((accel_right/(-z_accel_meas))*(18000/M_PI), -_lean_angle_max, _lean_angle_max);
-    _desired_pitch = constrain((-accel_forward/(-z_accel_meas*_cos_roll))*(18000/M_PI), -4500, 4500);
+    _desired_pitch = constrain((-accel_forward/(-z_accel_meas*_cos_roll))*(18000/M_PI), -_lean_angle_max, _lean_angle_max);
 }
 
 // get_bearing_cd - return bearing in centi-degrees between two positions

libraries/AC_WPNav/AC_WPNav.h

@@ -20,6 +20,7 @@
 #define MAX_LOITER_OVERSHOOT            1000        // maximum distance (in cm) that we will allow the target loiter point to be from the current location when switching into loiter
 #define WPINAV_MAX_POS_ERROR            2000.0f     // maximum distance (in cm) that the desired track can stray from our current location.
 #define WP_SPEED                        500         // default horizontal speed betwen waypoints in cm/s
+#define MAX_LEAN_ANGLE                  4500        // default maximum lean angle
 
 class AC_WPNav
 {
@@ -53,6 +54,15 @@ public:
     /// update_loiter - run the loiter controller - should be called at 10hz
     void update_loiter();
 
+    /// set_angle_limit - limits maximum angle in centi-degrees the copter will lean
+    void set_angle_limit(int32_t lean_angle) { _lean_angle_max = lean_angle; }
+
+    /// clear_angle_limit - reset angle limits back to defaults
+    void clear_angle_limit() { _lean_angle_max = MAX_LEAN_ANGLE; }
+    
+    /// get_angle_limit - retrieve maximum angle in centi-degrees the copter will lean
+    int32_t get_angle_limit() { return _lean_angle_max; }
+
     ///
     /// waypoint navigation
     ///
@@ -145,6 +155,8 @@ protected:
     int32_t     _desired_pitch;         // fed to stabilize controllers at 50hz
     int32_t     _desired_altitude;      // fed to alt hold controller at 50hz
 
+    int32_t     _lean_angle_max;        // maximum lean angle.  can we set from main code so that throttle controller can stop leans that cause copter to lose altitude
+
     // internal variables
     Vector3f    _target;   		        // loiter's target location in cm from home
     Vector3f    _origin;                // starting point of trip to next waypoint in cm from home (equivalent to next_WP)