mirror of https://github.com/ArduPilot/ardupilot
Copter: Add control over throttle ramp time during take-off
This commit is contained in:
parent
4a12faea92
commit
4b20a2d5f1
|
@ -1143,6 +1143,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
AP_SUBGROUPINFO(command_model_pilot, "PILOT_Y_", 56, ParametersG2, AC_CommandModel),
|
AP_SUBGROUPINFO(command_model_pilot, "PILOT_Y_", 56, ParametersG2, AC_CommandModel),
|
||||||
|
|
||||||
|
// @Param: TKOFF_SLEW_TIME
|
||||||
|
// @DisplayName: Slew time of throttle during take-off
|
||||||
|
// @Description: Time to slew the throttle from minimum to maximum while checking for a succsessful takeoff.
|
||||||
|
// @Units: s
|
||||||
|
// @Range: 0.25 5.0
|
||||||
|
// @User: Standard
|
||||||
|
AP_GROUPINFO("TKOFF_SLEW_TIME", 57, ParametersG2, takeoff_throttle_slew_time, 2.0),
|
||||||
|
|
||||||
// ID 62 is reserved for the SHOW_... parameters from the Skybrush fork at
|
// ID 62 is reserved for the SHOW_... parameters from the Skybrush fork at
|
||||||
// https://github.com/skybrush-io/ardupilot
|
// https://github.com/skybrush-io/ardupilot
|
||||||
|
|
||||||
|
|
|
@ -667,6 +667,9 @@ public:
|
||||||
AP_Int8 surftrak_mode;
|
AP_Int8 surftrak_mode;
|
||||||
AP_Int8 failsafe_dr_enable;
|
AP_Int8 failsafe_dr_enable;
|
||||||
AP_Int16 failsafe_dr_timeout;
|
AP_Int16 failsafe_dr_timeout;
|
||||||
|
|
||||||
|
// ramp time of throttle during take-off
|
||||||
|
AP_Float takeoff_throttle_slew_time;
|
||||||
};
|
};
|
||||||
|
|
||||||
extern const AP_Param::Info var_info[];
|
extern const AP_Param::Info var_info[];
|
||||||
|
|
|
@ -48,10 +48,13 @@ void Copter::update_land_detector()
|
||||||
} else if (ap.land_complete) {
|
} else if (ap.land_complete) {
|
||||||
#if FRAME_CONFIG == HELI_FRAME
|
#if FRAME_CONFIG == HELI_FRAME
|
||||||
// if rotor speed and collective pitch are high then clear landing flag
|
// if rotor speed and collective pitch are high then clear landing flag
|
||||||
if (motors->get_takeoff_collective() && motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
|
if (!flightmode->is_taking_off() && motors->get_takeoff_collective() && motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
|
||||||
#else
|
#else
|
||||||
// if throttle output is high then clear landing flag
|
// if throttle output is high then clear landing flag
|
||||||
if (motors->get_throttle() > get_non_takeoff_throttle()) {
|
if (!flightmode->is_taking_off() && motors->get_throttle_out() > get_non_takeoff_throttle() && motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
|
||||||
|
// this should never happen because take-off should be detected at the flight mode level
|
||||||
|
// this here to highlight there is a bug or missing take-off detection
|
||||||
|
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
|
||||||
#endif
|
#endif
|
||||||
set_land_complete(false);
|
set_land_complete(false);
|
||||||
}
|
}
|
||||||
|
|
|
@ -52,6 +52,7 @@ void ModeThrow::run()
|
||||||
|
|
||||||
} else if (stage == Throw_Detecting && throw_detected()){
|
} else if (stage == Throw_Detecting && throw_detected()){
|
||||||
gcs().send_text(MAV_SEVERITY_INFO,"throw detected - spooling motors");
|
gcs().send_text(MAV_SEVERITY_INFO,"throw detected - spooling motors");
|
||||||
|
copter.set_land_complete(false);
|
||||||
stage = Throw_Wait_Throttle_Unlimited;
|
stage = Throw_Wait_Throttle_Unlimited;
|
||||||
|
|
||||||
// Cancel the waiting for throw tone sequence
|
// Cancel the waiting for throw tone sequence
|
||||||
|
|
|
@ -57,11 +57,6 @@ bool Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
|
||||||
// start takeoff to specified altitude above home in centimeters
|
// start takeoff to specified altitude above home in centimeters
|
||||||
void Mode::_TakeOff::start(float alt_cm)
|
void Mode::_TakeOff::start(float alt_cm)
|
||||||
{
|
{
|
||||||
// indicate we are taking off
|
|
||||||
copter.set_land_complete(false);
|
|
||||||
// tell position controller to reset alt target and reset I terms
|
|
||||||
copter.pos_control->init_z_controller();
|
|
||||||
|
|
||||||
// initialise takeoff state
|
// initialise takeoff state
|
||||||
_running = true;
|
_running = true;
|
||||||
take_off_start_alt = copter.pos_control->get_pos_target_z_cm();
|
take_off_start_alt = copter.pos_control->get_pos_target_z_cm();
|
||||||
|
@ -72,6 +67,11 @@ void Mode::_TakeOff::start(float alt_cm)
|
||||||
void Mode::_TakeOff::stop()
|
void Mode::_TakeOff::stop()
|
||||||
{
|
{
|
||||||
_running = false;
|
_running = false;
|
||||||
|
// Check if we have progressed far enough through the takeoff process that the
|
||||||
|
// aircraft may have left the ground but not yet detected the climb.
|
||||||
|
if (copter.attitude_control->get_throttle_in() > copter.get_non_takeoff_throttle()) {
|
||||||
|
copter.set_land_complete(false);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// do_pilot_takeoff - controls the vertical position controller during the process of taking off
|
// do_pilot_takeoff - controls the vertical position controller during the process of taking off
|
||||||
|
@ -85,6 +85,24 @@ void Mode::_TakeOff::do_pilot_takeoff(float& pilot_climb_rate_cm)
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (copter.ap.land_complete) {
|
||||||
|
// send throttle to attitude controller with angle boost
|
||||||
|
float throttle = constrain_float(copter.attitude_control->get_throttle_in() + copter.G_Dt / copter.g2.takeoff_throttle_slew_time, 0.0, 1.0);
|
||||||
|
copter.attitude_control->set_throttle_out(throttle, true, 0.0);
|
||||||
|
// tell position controller to reset alt target and reset I terms
|
||||||
|
copter.pos_control->init_z_controller();
|
||||||
|
if (throttle >= 0.9 ||
|
||||||
|
(copter.pos_control->get_z_accel_cmss() >= 0.5 * copter.pos_control->get_max_accel_z_cmss()) ||
|
||||||
|
(copter.pos_control->get_vel_desired_cms().z >= constrain_float(pilot_climb_rate_cm, copter.pos_control->get_max_speed_up_cms() * 0.1, copter.pos_control->get_max_speed_up_cms() * 0.5)) ||
|
||||||
|
(is_positive(take_off_complete_alt - take_off_start_alt) && copter.pos_control->get_pos_target_z_cm() - take_off_start_alt > 0.5 * (take_off_complete_alt - take_off_start_alt))) {
|
||||||
|
// throttle > 90%
|
||||||
|
// acceleration > 50% maximum acceleration
|
||||||
|
// velocity > 10% maximum velocity && commanded climb rate
|
||||||
|
// velocity > 50% maximum velocity
|
||||||
|
// altitude change greater than half complete alt from start off alt
|
||||||
|
copter.set_land_complete(false);
|
||||||
|
}
|
||||||
|
} else {
|
||||||
float pos_z = take_off_complete_alt;
|
float pos_z = take_off_complete_alt;
|
||||||
float vel_z = pilot_climb_rate_cm;
|
float vel_z = pilot_climb_rate_cm;
|
||||||
|
|
||||||
|
@ -96,6 +114,7 @@ void Mode::_TakeOff::do_pilot_takeoff(float& pilot_climb_rate_cm)
|
||||||
(take_off_complete_alt - take_off_start_alt) * 0.999f < copter.pos_control->get_pos_target_z_cm() - take_off_start_alt) {
|
(take_off_complete_alt - take_off_start_alt) * 0.999f < copter.pos_control->get_pos_target_z_cm() - take_off_start_alt) {
|
||||||
stop();
|
stop();
|
||||||
}
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// auto_takeoff_run - controls the vertical position controller during the process of taking off in auto modes
|
// auto_takeoff_run - controls the vertical position controller during the process of taking off in auto modes
|
||||||
|
@ -131,9 +150,7 @@ void Mode::auto_takeoff_run()
|
||||||
}
|
}
|
||||||
|
|
||||||
// aircraft stays in landed state until rotor speed run up has finished
|
// aircraft stays in landed state until rotor speed run up has finished
|
||||||
if (motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
|
if (motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
|
||||||
set_land_complete(false);
|
|
||||||
} else {
|
|
||||||
// motors have not completed spool up yet so relax navigation and position controllers
|
// motors have not completed spool up yet so relax navigation and position controllers
|
||||||
pos_control->relax_velocity_controller_xy();
|
pos_control->relax_velocity_controller_xy();
|
||||||
pos_control->update_xy_controller();
|
pos_control->update_xy_controller();
|
||||||
|
@ -141,7 +158,31 @@ void Mode::auto_takeoff_run()
|
||||||
pos_control->update_z_controller();
|
pos_control->update_z_controller();
|
||||||
attitude_control->reset_yaw_target_and_rate();
|
attitude_control->reset_yaw_target_and_rate();
|
||||||
attitude_control->reset_rate_controller_I_terms();
|
attitude_control->reset_rate_controller_I_terms();
|
||||||
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds());
|
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// aircraft stays in landed state until vertical movement is detected or 90% throttle is reached
|
||||||
|
if (copter.ap.land_complete) {
|
||||||
|
// send throttle to attitude controller with angle boost
|
||||||
|
float throttle = constrain_float(copter.attitude_control->get_throttle_in() + copter.G_Dt / copter.g2.takeoff_throttle_slew_time, 0.0, 1.0);
|
||||||
|
copter.attitude_control->set_throttle_out(throttle, true, 0.0);
|
||||||
|
// tell position controller to reset alt target and reset I terms
|
||||||
|
copter.pos_control->init_z_controller();
|
||||||
|
pos_control->relax_velocity_controller_xy();
|
||||||
|
pos_control->update_xy_controller();
|
||||||
|
attitude_control->reset_rate_controller_I_terms();
|
||||||
|
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0);
|
||||||
|
if (throttle >= 0.9 ||
|
||||||
|
(copter.pos_control->get_z_accel_cmss() >= 0.5 * copter.pos_control->get_max_accel_z_cmss()) ||
|
||||||
|
(copter.pos_control->get_vel_desired_cms().z >= 0.1 * copter.pos_control->get_max_speed_up_cms()) ||
|
||||||
|
( auto_takeoff_no_nav_active && (inertial_nav.get_position_z_up_cm() >= auto_takeoff_no_nav_alt_cm))) {
|
||||||
|
// throttle > 90%
|
||||||
|
// acceleration > 50% maximum acceleration
|
||||||
|
// velocity > 10% maximum velocity
|
||||||
|
// altitude change greater than half auto_takeoff_no_nav_alt_cm
|
||||||
|
copter.set_land_complete(false);
|
||||||
|
}
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -223,8 +264,5 @@ bool Mode::is_taking_off() const
|
||||||
if (!has_user_takeoff(false)) {
|
if (!has_user_takeoff(false)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if (copter.ap.land_complete) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
return takeoff.running();
|
return takeoff.running();
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue