Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ArduCopter/mode.cpp
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

2
ArduCopter/mode.h
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;

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

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

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

5
ArduCopter/mode_flowhold.cpp
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) {

4
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, 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)) {

2
ArduCopter/mode_loiter.cpp
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);

2
ArduCopter/mode_poshold.cpp
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)

2
ArduCopter/mode_rtl.cpp
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)) {

3
ArduCopter/mode_stabilize.cpp
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());

3
ArduCopter/mode_stabilize_heli.cpp
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());