diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 63552a88ba..ffd667db81 100644 --- a/ArduCopter/mode.cpp +++ b/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); - if (total_in > angle_max) { - float ratio = angle_max / total_in; + float total_in = norm(pitch_in, roll_in); + if (total_in > angle_limit) { + float ratio = angle_limit / total_in; roll_in *= ratio; pitch_in *= ratio; } diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 6bc6ca481b..9a12eb1d10 100644 --- a/ArduCopter/mode.h +++ b/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; diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp index 70ab379297..77d2a92ad8 100644 --- a/ArduCopter/mode_althold.cpp +++ b/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()); diff --git a/ArduCopter/mode_autotune.cpp b/ArduCopter/mode_autotune.cpp index 1c51592b79..213b4a570f 100644 --- a/ArduCopter/mode_autotune.cpp +++ b/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()); diff --git a/ArduCopter/mode_drift.cpp b/ArduCopter/mode_drift.cpp index 5be83ed3d1..6f32dd85ad 100644 --- a/ArduCopter/mode_drift.cpp +++ b/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()); diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 63f4e1a39f..008d831256 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/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) { diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index c0049391ac..4cf8122818 100644 --- a/ArduCopter/mode_land.cpp +++ b/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 diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 4491b67aa7..b60bd0ac44 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/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); diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index 84d5d07601..58668e2d44 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/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) diff --git a/ArduCopter/mode_stabilize.cpp b/ArduCopter/mode_stabilize.cpp index 6740184d7b..d60f2603db 100644 --- a/ArduCopter/mode_stabilize.cpp +++ b/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()); diff --git a/ArduCopter/mode_stabilize_heli.cpp b/ArduCopter/mode_stabilize_heli.cpp index fb6461acd9..118bbfd74d 100644 --- a/ArduCopter/mode_stabilize_heli.cpp +++ b/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());