mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Copter: get_pilot_desired_throttle gets thr_mid argument default
This commit is contained in:
parent
bead957a78
commit
a124001b8b
@ -685,7 +685,7 @@ private:
|
|||||||
float get_look_ahead_yaw();
|
float get_look_ahead_yaw();
|
||||||
void update_throttle_hover();
|
void update_throttle_hover();
|
||||||
void set_throttle_takeoff();
|
void set_throttle_takeoff();
|
||||||
float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid);
|
float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid = 0.0f);
|
||||||
float get_pilot_desired_climb_rate(float throttle_control);
|
float get_pilot_desired_climb_rate(float throttle_control);
|
||||||
float get_non_takeoff_throttle();
|
float get_non_takeoff_throttle();
|
||||||
float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt);
|
float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt);
|
||||||
|
@ -64,7 +64,7 @@ void Copter::drift_run()
|
|||||||
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);
|
||||||
|
|
||||||
// get pilot's desired throttle
|
// get pilot's desired throttle
|
||||||
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in(), 0.0f);
|
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());
|
||||||
|
|
||||||
// Grab inertial velocity
|
// Grab inertial velocity
|
||||||
const Vector3f& vel = inertial_nav.get_velocity();
|
const Vector3f& vel = inertial_nav.get_velocity();
|
||||||
|
@ -105,7 +105,7 @@ void Copter::flip_run()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// get pilot's desired throttle
|
// get pilot's desired throttle
|
||||||
throttle_out = get_pilot_desired_throttle(channel_throttle->get_control_in(), 0.0f);
|
throttle_out = get_pilot_desired_throttle(channel_throttle->get_control_in());
|
||||||
|
|
||||||
// get corrected angle based on direction and axis of rotation
|
// get corrected angle based on direction and axis of rotation
|
||||||
// we flip the sign of flip_angle to minimize the code repetition
|
// we flip the sign of flip_angle to minimize the code repetition
|
||||||
|
@ -11,7 +11,7 @@ bool Copter::stabilize_init(bool ignore_checks)
|
|||||||
{
|
{
|
||||||
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
|
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
|
||||||
if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) &&
|
if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) &&
|
||||||
(get_pilot_desired_throttle(channel_throttle->get_control_in(), 0.0f) > get_non_takeoff_throttle())) {
|
(get_pilot_desired_throttle(channel_throttle->get_control_in()) > get_non_takeoff_throttle())) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
// set target altitude to zero for reporting
|
// set target altitude to zero for reporting
|
||||||
@ -51,7 +51,7 @@ void Copter::stabilize_run()
|
|||||||
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
|
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
|
||||||
|
|
||||||
// get pilot's desired throttle
|
// get pilot's desired throttle
|
||||||
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in(), 0.0f);
|
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());
|
||||||
|
|
||||||
// call attitude controller
|
// call attitude controller
|
||||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||||
|
@ -274,7 +274,7 @@ void Copter::exit_mode(control_mode_t old_control_mode, control_mode_t new_contr
|
|||||||
// smooth throttle transition when switching from manual to automatic flight modes
|
// smooth throttle transition when switching from manual to automatic flight modes
|
||||||
if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors.armed() && !ap.land_complete) {
|
if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors.armed() && !ap.land_complete) {
|
||||||
// this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle
|
// this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle
|
||||||
set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(channel_throttle->get_control_in(), 0.0f));
|
set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(channel_throttle->get_control_in()));
|
||||||
}
|
}
|
||||||
|
|
||||||
// cancel any takeoffs in progress
|
// cancel any takeoffs in progress
|
||||||
|
Loading…
Reference in New Issue
Block a user