forked from Archive/PX4-Autopilot
Takeoff: address @RomanBapst's review comments
This commit is contained in:
parent
1c776f16ec
commit
ea48cd4970
|
@ -347,7 +347,7 @@ void FlightTaskManualAltitude::_updateSetpoints()
|
||||||
|
|
||||||
bool FlightTaskManualAltitude::_checkTakeoff()
|
bool FlightTaskManualAltitude::_checkTakeoff()
|
||||||
{
|
{
|
||||||
// stick is deflected above the middle 15% of the range
|
// stick is deflected above 65% throttle (_sticks(2) is in the range [-1,1])
|
||||||
return _sticks(2) < -0.3f;
|
return _sticks(2) < -0.3f;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -38,7 +38,7 @@
|
||||||
#include "Takeoff.hpp"
|
#include "Takeoff.hpp"
|
||||||
#include <mathlib/mathlib.h>
|
#include <mathlib/mathlib.h>
|
||||||
|
|
||||||
void Takeoff::generateInitialValue(const float hover_thrust, float velocity_p_gain)
|
void Takeoff::generateInitialRampValue(const float hover_thrust, float velocity_p_gain)
|
||||||
{
|
{
|
||||||
velocity_p_gain = math::max(velocity_p_gain, 0.01f);
|
velocity_p_gain = math::max(velocity_p_gain, 0.01f);
|
||||||
_takeoff_ramp_vz_init = -hover_thrust / velocity_p_gain;
|
_takeoff_ramp_vz_init = -hover_thrust / velocity_p_gain;
|
||||||
|
|
|
@ -61,7 +61,14 @@ public:
|
||||||
// initialize parameters
|
// initialize parameters
|
||||||
void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; }
|
void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; }
|
||||||
void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(seconds * (float)1_s)); }
|
void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(seconds * (float)1_s)); }
|
||||||
void generateInitialValue(const float hover_thrust, const float velocity_p_gain);
|
|
||||||
|
/**
|
||||||
|
* Calculate a vertical velocity to initialize the takeoff ramp
|
||||||
|
* that when passed to the velocity controller results in a zero throttle setpoint.
|
||||||
|
* @param hover_thrust normalized thrsut value with which the vehicle hovers
|
||||||
|
* @param velocity_p_gain proportional gain of the velocity controller to calculate the thrust
|
||||||
|
*/
|
||||||
|
void generateInitialRampValue(const float hover_thrust, const float velocity_p_gain);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Update the state for the takeoff.
|
* Update the state for the takeoff.
|
||||||
|
@ -70,6 +77,15 @@ public:
|
||||||
*/
|
*/
|
||||||
void updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff,
|
void updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff,
|
||||||
const float takeoff_desired_vz, const bool skip_takeoff);
|
const float takeoff_desired_vz, const bool skip_takeoff);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Update and return the velocity constraint ramp value during takeoff.
|
||||||
|
* By ramping up _takeoff_ramp_vz during the takeoff and using it to constain the maximum climb rate a smooth takeoff behavior is achieved.
|
||||||
|
* Returns zero on the ground and takeoff_desired_vz in flight.
|
||||||
|
* @param dt time in seconds since the last call/loop iteration
|
||||||
|
* @param takeoff_desired_vz end value for the velocity ramp
|
||||||
|
* @return true if setpoint has updated correctly
|
||||||
|
*/
|
||||||
float updateRamp(const float dt, const float takeoff_desired_vz);
|
float updateRamp(const float dt, const float takeoff_desired_vz);
|
||||||
|
|
||||||
TakeoffState getTakeoffState() { return _takeoff_state; }
|
TakeoffState getTakeoffState() { return _takeoff_state; }
|
||||||
|
|
|
@ -360,7 +360,7 @@ MulticopterPositionControl::parameters_update(bool force)
|
||||||
// set trigger time for takeoff delay
|
// set trigger time for takeoff delay
|
||||||
_takeoff.setSpoolupTime(_param_mpc_spoolup_time.get());
|
_takeoff.setSpoolupTime(_param_mpc_spoolup_time.get());
|
||||||
_takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get());
|
_takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get());
|
||||||
_takeoff.generateInitialValue(_param_mpc_thr_hover.get(), _param_mpc_z_vel_p.get());
|
_takeoff.generateInitialRampValue(_param_mpc_thr_hover.get(), _param_mpc_z_vel_p.get());
|
||||||
|
|
||||||
if (_wv_controller != nullptr) {
|
if (_wv_controller != nullptr) {
|
||||||
_wv_controller->update_parameters();
|
_wv_controller->update_parameters();
|
||||||
|
|
Loading…
Reference in New Issue