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

Copter: Fix take off altitude

This commit is contained in:
Leonard Hall 2021-05-13 15:10:05 +09:30 committed by Andrew Tridgell
parent 1326dbe2ee
commit caeaf7c047
8 changed files with 41 additions and 118 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/mode.h
View File

@ -157,16 +157,14 @@ protected:
public:
void start(float alt_cm);
void stop();
void get_climb_rates(float& pilot_climb_rate,
float& takeoff_climb_rate);
void do_pilot_takeoff(float& pilot_climb_rate);
bool triggered(float target_climb_rate) const;
bool running() const { return _running; }
private:
bool _running;
float max_speed;
float alt_delta;
uint32_t start_ms;
float take_off_start_alt;
float take_off_alt;
};
static _TakeOff takeoff;

10
ArduCopter/mode_althold.cpp
View File

@ -20,8 +20,6 @@ bool ModeAltHold::init(bool ignore_checks)
// should be called at 100hz or more
void ModeAltHold::run()
{
float takeoff_climb_rate = 0.0f;
// initialize vertical speeds and acceleration
pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
@ -66,14 +64,11 @@ void ModeAltHold::run()
takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
}
// get take-off adjusted pilot and takeoff climb rates
takeoff.get_climb_rates(target_climb_rate, takeoff_climb_rate);
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
// set position controller targets
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate + takeoff_climb_rate, false);
// get take-off adjusted pilot and takeoff climb rates
takeoff.do_pilot_takeoff(target_climb_rate);
break;
case AltHold_Flying:
@ -99,5 +94,4 @@ void ModeAltHold::run()
// call z-axis position controller
pos_control->update_z_controller();
}

9
ArduCopter/mode_flowhold.cpp
View File

@ -227,8 +227,6 @@ void ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stick_input)
// should be called at 100hz or more
void ModeFlowHold::run()
{
float takeoff_climb_rate = 0.0f;
update_height_estimate();
// initialize vertical speeds and acceleration
@ -279,14 +277,11 @@ void ModeFlowHold::run()
takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
}
// get take-off adjusted pilot and takeoff climb rates
takeoff.get_climb_rates(target_climb_rate, takeoff_climb_rate);
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
// call position controller
copter.pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate + takeoff_climb_rate, false);
// get take-off adjusted pilot and takeoff climb rates
takeoff.do_pilot_takeoff(target_climb_rate);
break;
case AltHold_Landed_Ground_Idle:

17
ArduCopter/mode_loiter.cpp
View File

@ -76,7 +76,6 @@ void ModeLoiter::run()
float target_roll, target_pitch;
float target_yaw_rate = 0.0f;
float target_climb_rate = 0.0f;
float takeoff_climb_rate = 0.0f;
// initialize vertical speed and acceleration
pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
@ -120,7 +119,6 @@ void ModeLoiter::run()
pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero
loiter_nav->init_target();
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate);
pos_control->update_z_controller();
break;
case AltHold_Takeoff:
@ -129,21 +127,17 @@ void ModeLoiter::run()
takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
}
// get takeoff adjusted pilot and takeoff climb rates
takeoff.get_climb_rates(target_climb_rate, takeoff_climb_rate);
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
// get take-off adjusted pilot and takeoff climb rates
takeoff.do_pilot_takeoff(target_climb_rate);
// run loiter controller
loiter_nav->update();
// call attitude controller
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate);
// update altitude target and call position controller
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate + takeoff_climb_rate, false);
pos_control->update_z_controller();
break;
case AltHold_Landed_Ground_Idle:
@ -155,7 +149,6 @@ void ModeLoiter::run()
loiter_nav->init_target();
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero
pos_control->update_z_controller();
break;
case AltHold_Flying:
@ -181,9 +174,11 @@ void ModeLoiter::run()
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false);
pos_control->update_z_controller();
break;
}
// call z-axis position controller
pos_control->update_z_controller();
}
uint32_t ModeLoiter::wp_distance() const

10
ArduCopter/mode_poshold.cpp
View File

@ -66,7 +66,6 @@ bool ModePosHold::init(bool ignore_checks)
// should be called at 100hz or more
void ModePosHold::run()
{
float takeoff_climb_rate = 0.0f;
float controller_to_pilot_roll_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls
float controller_to_pilot_pitch_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls
const Vector3f& vel = inertial_nav.get_velocity();
@ -122,20 +121,17 @@ void ModePosHold::run()
takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
}
// get take-off adjusted pilot and takeoff climb rates
takeoff.get_climb_rates(target_climb_rate, takeoff_climb_rate);
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
// get take-off adjusted pilot and takeoff climb rates
takeoff.do_pilot_takeoff(target_climb_rate);
// init and update loiter although pilot is controlling lean angles
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
loiter_nav->update(false);
// set position controller targets
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate + takeoff_climb_rate, false);
// set poshold state to pilot override
roll_mode = RPMode::PILOT_OVERRIDE;
pitch_mode = RPMode::PILOT_OVERRIDE;

9
ArduCopter/mode_sport.cpp
View File

@ -24,8 +24,6 @@ bool ModeSport::init(bool ignore_checks)
// should be called at 100hz or more
void ModeSport::run()
{
float takeoff_climb_rate = 0.0f;
// initialize vertical speed and acceleration
pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
@ -87,14 +85,11 @@ void ModeSport::run()
takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
}
// get take-off adjusted pilot and takeoff climb rates
takeoff.get_climb_rates(target_climb_rate, takeoff_climb_rate);
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
// call position controller
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate + takeoff_climb_rate, false);
// get take-off adjusted pilot and takeoff climb rates
takeoff.do_pilot_takeoff(target_climb_rate);
break;
case AltHold_Landed_Ground_Idle:

15
ArduCopter/mode_zigzag.cpp
View File

@ -287,7 +287,6 @@ void ModeZigZag::manual_control()
{
float target_yaw_rate = 0.0f;
float target_climb_rate = 0.0f;
float takeoff_climb_rate = 0.0f;
// process pilot inputs unless we are in radio failsafe
if (!copter.failsafe.radio) {
@ -330,7 +329,6 @@ void ModeZigZag::manual_control()
pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero
loiter_nav->init_target();
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);
pos_control->update_z_controller();
break;
case AltHold_Takeoff:
@ -339,9 +337,6 @@ void ModeZigZag::manual_control()
takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
}
// get takeoff adjusted pilot and takeoff climb rates
takeoff.get_climb_rates(target_climb_rate, takeoff_climb_rate);
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
@ -351,9 +346,8 @@ void ModeZigZag::manual_control()
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);
// update altitude target and call position controller
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate + takeoff_climb_rate, false);
pos_control->update_z_controller();
// get take-off adjusted pilot and takeoff climb rates
takeoff.do_pilot_takeoff(target_climb_rate);
break;
case AltHold_Landed_Ground_Idle:
@ -365,7 +359,6 @@ void ModeZigZag::manual_control()
loiter_nav->init_target();
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero
pos_control->update_z_controller();
break;
case AltHold_Flying:
@ -385,9 +378,11 @@ void ModeZigZag::manual_control()
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false);
pos_control->update_z_controller();
break;
}
// call z-axis position controller
pos_control->update_z_controller();
}
// return true if vehicle is within a small area around the destination

81
ArduCopter/takeoff.cpp
View File

@ -48,7 +48,7 @@ bool Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
return true;
}
// start takeoff to specified altitude above home in centimeters
// start takeoff to specified altitude above home in centimetres
void Mode::_TakeOff::start(float alt_cm)
{
// indicate we are taking off
@ -56,88 +56,43 @@ void Mode::_TakeOff::start(float alt_cm)
// tell position controller to reset alt target and reset I terms
copter.flightmode->set_throttle_takeoff();
// calculate climb rate
const float speed = MIN(copter.wp_nav->get_default_speed_up(), MAX(copter.g.pilot_speed_up*2.0f/3.0f, copter.g.pilot_speed_up-50.0f));
// sanity check speed and target
if (speed <= 0.0f || alt_cm <= 0.0f) {
return;
}
// initialise takeoff state
_running = true;
max_speed = speed;
start_ms = millis();
alt_delta = alt_cm;
take_off_start_alt = copter.pos_control->get_pos_target_z_cm();
take_off_alt = take_off_start_alt + alt_cm;
}
// stop takeoff
void Mode::_TakeOff::stop()
{
_running = false;
start_ms = 0;
}
// returns pilot and takeoff climb rates
// pilot_climb_rate is both an input and an output
// takeoff_climb_rate is only an output
// has side-effect of turning takeoff off when timeout as expired
void Mode::_TakeOff::get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate)
// do_pilot_takeoff - commands the aircraft to the take off altitude
// climb is cancelled if pilot_climb_rate_cm becomes negative
// sets take off to complete when target altitude is within 1% of the take off altitude
void Mode::_TakeOff::do_pilot_takeoff(float& pilot_climb_rate_cm)
{
// return pilot_climb_rate if take-off inactive
if (!_running) {
takeoff_climb_rate = 0.0f;
return;
}
// acceleration of 50cm/s/s
static constexpr float TAKEOFF_ACCEL = 50.0f;
const float takeoff_minspeed = MIN(50.0f, max_speed);
const float time_elapsed = (millis() - start_ms) * 1.0e-3f;
const float speed = MIN(time_elapsed * TAKEOFF_ACCEL + takeoff_minspeed, max_speed);
Vector3f pos;
Vector3f vel;
Vector3f accel;
const float time_to_max_speed = (max_speed - takeoff_minspeed) / TAKEOFF_ACCEL;
float height_gained;
if (time_elapsed <= time_to_max_speed) {
height_gained = 0.5f * TAKEOFF_ACCEL * sq(time_elapsed) + takeoff_minspeed * time_elapsed;
} else {
height_gained = 0.5f * TAKEOFF_ACCEL * sq(time_to_max_speed) + takeoff_minspeed * time_to_max_speed +
(time_elapsed - time_to_max_speed) * max_speed;
}
pos.z = take_off_alt;
vel.z = pilot_climb_rate_cm;
// check if the takeoff is over
if (height_gained >= alt_delta) {
// command the aircraft to the take off altitude and current pilot climb rate
copter.pos_control->input_pos_vel_accel_z(pos, vel, accel);
// stop take off early and return if negative climb rate is commanded or we are within 0.1% of our take off altitude
if (is_negative(pilot_climb_rate_cm) ||
(take_off_alt - take_off_start_alt) * 0.999f < copter.pos_control->get_pos_target_z_cm() - take_off_start_alt) {
stop();
}
// if takeoff climb rate is zero return
if (speed <= 0.0f) {
takeoff_climb_rate = 0.0f;
return;
}
// default take-off climb rate to maximum speed
takeoff_climb_rate = speed;
// if pilot's commands descent
if (pilot_climb_rate < 0.0f) {
// if overall climb rate is still positive, move to take-off climb rate
if (takeoff_climb_rate + pilot_climb_rate > 0.0f) {
takeoff_climb_rate = takeoff_climb_rate + pilot_climb_rate;
pilot_climb_rate = 0.0f;
} else {
// if overall is negative, move to pilot climb rate
pilot_climb_rate = pilot_climb_rate + takeoff_climb_rate;
takeoff_climb_rate = 0.0f;
}
} else { // pilot commands climb
// pilot climb rate is zero until it surpasses the take-off climb rate
if (pilot_climb_rate > takeoff_climb_rate) {
pilot_climb_rate = pilot_climb_rate - takeoff_climb_rate;
} else {
pilot_climb_rate = 0.0f;
}
}
}
void Mode::auto_takeoff_run()