aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Leonard Hall 2017-11-12 18:20:51 +10:30 committed by Randy Mackay
parent 1540f98888
commit 93de23e7c4
11 changed files with 17 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
ArduCopter/mode.cpp
View File

@ -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;
}

2
ArduCopter/mode.h
View File

@ -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;

2
ArduCopter/mode_althold.cpp
View File

@ -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());

2
ArduCopter/mode_autotune.cpp
View File

@ -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());

2
ArduCopter/mode_drift.cpp
View File

@ -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());

3
ArduCopter/mode_flowhold.cpp
View File

@ -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) {

2
ArduCopter/mode_land.cpp
View File

@ -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

2
ArduCopter/mode_loiter.cpp
View File

@ -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);

2
ArduCopter/mode_poshold.cpp
View File

@ -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)

2
ArduCopter/mode_stabilize.cpp
View File

@ -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());

2
ArduCopter/mode_stabilize_heli.cpp
View File

@ -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());