Copter: get-pilot-desired-lean-angles accepts another angle-max

ArduCopter/mode.cpp

@@ -319,24 +319,20 @@ void Copter::Mode::update_navigation()
 
 // get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
 // returns desired angle in centi-degrees
-void Copter::Mode::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max)
+void Copter::Mode::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max, float angle_limit)
 {
-    AP_Vehicle::MultiCopter &aparm = copter.aparm;
-    // sanity check angle max parameter
-    aparm.angle_max = constrain_int16(aparm.angle_max,1000,8000);
-
     // limit max lean angle
-    angle_max = constrain_float(angle_max, 1000, aparm.angle_max);
+    angle_limit = constrain_float(angle_limit, 1000.0f, angle_max);
 
     // scale roll_in, pitch_in to ANGLE_MAX parameter range
-    const float scaler = aparm.angle_max/(float)ROLL_PITCH_YAW_INPUT_MAX;
+    float scaler = angle_max/(float)ROLL_PITCH_YAW_INPUT_MAX;
     roll_in *= scaler;
     pitch_in *= scaler;
 
     // do circular limit
-    const float total_in = norm(pitch_in, roll_in);
+    float total_in = norm(pitch_in, roll_in);
-    if (total_in > angle_max) {
+    if (total_in > angle_limit) {
-        float ratio = angle_max / total_in;
+        float ratio = angle_limit / total_in;
         roll_in *= ratio;
         pitch_in *= ratio;
     }

ArduCopter/mode.h

@@ -36,7 +36,7 @@ protected:
     virtual bool in_guided_mode() const { return false; }
 
     // pilot input processing
-    void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max);
+    static void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max, float angle_limit);
 
     // takeoff support
     bool takeoff_triggered(float target_climb_rate) const;

ArduCopter/mode_althold.cpp

@@ -40,7 +40,7 @@ void Copter::ModeAltHold::run()
 
     // get pilot desired lean angles
     float target_roll, target_pitch;
-    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control->get_althold_lean_angle_max());
+    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max());
 
     // get pilot's desired yaw rate
     float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());

ArduCopter/mode_autotune.cpp

@@ -337,7 +337,7 @@ void Copter::ModeAutoTune::run()
     update_simple_mode();
 
     // get pilot desired lean angles
-    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, copter.aparm.angle_max);
+    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max());
 
     // get pilot's desired yaw rate
     target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());

ArduCopter/mode_drift.cpp

@@ -60,7 +60,7 @@ void Copter::ModeDrift::run()
     }
 
     // convert pilot input to lean angles
-    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, copter.aparm.angle_max);
+    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);
 
     // get pilot's desired throttle
     pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());

ArduCopter/mode_flowhold.cpp

@@ -269,7 +269,8 @@ void Copter::ModeFlowHold::run()
     float angle_max = copter.attitude_control->get_althold_lean_angle_max();
     get_pilot_desired_lean_angles(roll_in, pitch_in,
                                          bf_angles.x, bf_angles.y,
-                                         angle_max);
+                                         angle_max,
+                                         attitude_control->get_althold_lean_angle_max());
     
     if (quality_filtered >= flow_min_quality &&
         AP_HAL::millis() - copter.arm_time_ms > 3000) {

ArduCopter/mode_land.cpp

@@ -99,7 +99,7 @@ void Copter::ModeLand::nogps_run()
             update_simple_mode();
 
             // get pilot desired lean angles
-            get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, copter.aparm.angle_max);
+            get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max());
         }
 
         // get pilot's desired yaw rate

ArduCopter/mode_loiter.cpp

@@ -86,7 +86,7 @@ void Copter::ModeLoiter::run()
 
         // convert pilot input to lean angles
         // ToDo: convert get_pilot_desired_lean_angles to return angles as floats
-        get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, wp_nav->get_loiter_angle_max_cd());
+        get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, wp_nav->get_loiter_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
 
         // process pilot's roll and pitch input
         wp_nav->set_pilot_desired_acceleration(target_roll, target_pitch);

ArduCopter/mode_poshold.cpp

@@ -201,7 +201,7 @@ void Copter::ModePosHold::run()
         return;
     }else{
         // convert pilot input to lean angles
-        get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, copter.aparm.angle_max);
+        get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max());
 
         // convert inertial nav earth-frame velocities to body-frame
         // To-Do: move this to AP_Math (or perhaps we already have a function to do this)

ArduCopter/mode_stabilize.cpp

@@ -44,7 +44,7 @@ void Copter::ModeStabilize::run()
 
     // convert pilot input to lean angles
     // To-Do: convert get_pilot_desired_lean_angles to return angles as floats
-    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
+    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max, aparm.angle_max);
 
     // get pilot's desired yaw rate
     target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());

ArduCopter/mode_stabilize_heli.cpp

@@ -56,7 +56,7 @@ void Copter::ModeStabilize_Heli::run()
 
     // convert pilot input to lean angles
     // To-Do: convert get_pilot_desired_lean_angles to return angles as floats
-    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, copter.aparm.angle_max);
+    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);
 
     // get pilot's desired yaw rate
     target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());