ArduCopter: changed signature for get_pilot_desired_lean_angles()

Removes first two redundant input parameters, makes fn non static, makes fn const
This commit is contained in:
Ebin 2018-03-18 16:36:54 +05:30 committed by Randy Mackay
parent 5fbb385c31
commit d8f56b3511
12 changed files with 26 additions and 29 deletions

View File

@ -319,30 +319,32 @@ 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, float angle_limit)
void Copter::Mode::get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const
{
// limit max lean angle
// fetch roll and pitch inputs
roll_out = channel_roll->get_control_in();
pitch_out = channel_pitch->get_control_in();
// limit max lean angle
angle_limit = constrain_float(angle_limit, 1000.0f, angle_max);
// scale roll_in, pitch_in to ANGLE_MAX parameter range
// scale roll and pitch inputs to ANGLE_MAX parameter range
float scaler = angle_max/(float)ROLL_PITCH_YAW_INPUT_MAX;
roll_in *= scaler;
pitch_in *= scaler;
roll_out *= scaler;
pitch_out *= scaler;
// do circular limit
float total_in = norm(pitch_in, roll_in);
float total_in = norm(pitch_out, roll_out);
if (total_in > angle_limit) {
float ratio = angle_limit / total_in;
roll_in *= ratio;
pitch_in *= ratio;
roll_out *= ratio;
pitch_out *= ratio;
}
// do lateral tilt to euler roll conversion
roll_in = (18000/M_PI) * atanf(cosf(pitch_in*(M_PI/18000))*tanf(roll_in*(M_PI/18000)));
roll_out = (18000/M_PI) * atanf(cosf(pitch_out*(M_PI/18000))*tanf(roll_out*(M_PI/18000)));
// return
roll_out = roll_in;
pitch_out = pitch_in;
// roll_out and pitch_out are returned
}
bool Copter::Mode::takeoff_triggered(const float target_climb_rate) const

View File

@ -36,7 +36,7 @@ protected:
virtual bool in_guided_mode() const { return false; }
// pilot input processing
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);
void get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const;
// takeoff support
bool takeoff_triggered(float target_climb_rate) const;

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, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max());
get_pilot_desired_lean_angles(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());

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, attitude_control->get_althold_lean_angle_max());
get_pilot_desired_lean_angles(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());

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, copter.aparm.angle_max);
get_pilot_desired_lean_angles(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());

View File

@ -267,10 +267,7 @@ void Copter::ModeFlowHold::run()
int16_t roll_in = copter.channel_roll->get_control_in();
int16_t pitch_in = copter.channel_pitch->get_control_in();
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,
attitude_control->get_althold_lean_angle_max());
get_pilot_desired_lean_angles(bf_angles.x, bf_angles.y,angle_max, attitude_control->get_althold_lean_angle_max());
if (quality_filtered >= flow_min_quality &&
AP_HAL::millis() - copter.arm_time_ms > 3000) {

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, attitude_control->get_althold_lean_angle_max());
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max());
}
// get pilot's desired yaw rate
@ -226,7 +226,7 @@ void Copter::land_run_horizontal_control()
update_simple_mode();
// convert pilot input to lean angles
Mode::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());
flightmode->get_pilot_desired_lean_angles(target_roll, target_pitch, wp_nav->get_loiter_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
// record if pilot has overriden roll or pitch
if (!is_zero(target_roll) || !is_zero(target_pitch)) {

View File

@ -85,7 +85,7 @@ void Copter::ModeLoiter::run()
update_simple_mode();
// 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, wp_nav->get_loiter_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
get_pilot_desired_lean_angles(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, G_Dt);

View File

@ -197,7 +197,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, attitude_control->get_althold_lean_angle_max());
get_pilot_desired_lean_angles(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)

View File

@ -284,7 +284,7 @@ void Copter::ModeRTL::descent_run()
update_simple_mode();
// 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, wp_nav->get_loiter_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
get_pilot_desired_lean_angles(target_roll, target_pitch, wp_nav->get_loiter_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
// record if pilot has overriden roll or pitch
if (!is_zero(target_roll) || !is_zero(target_pitch)) {

View File

@ -43,8 +43,7 @@ void Copter::ModeStabilize::run()
AP_Vehicle::MultiCopter &aparm = copter.aparm;
// 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, aparm.angle_max);
get_pilot_desired_lean_angles(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());

View File

@ -55,8 +55,7 @@ void Copter::ModeStabilize_Heli::run()
update_simple_mode();
// 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, copter.aparm.angle_max);
get_pilot_desired_lean_angles(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());