diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp index e79e7d66d1b2..21155786f5fd 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp @@ -152,7 +152,7 @@ bool FlightTask::_checkTakeoff() bool position_triggered_takeoff = false; if (PX4_ISFINITE(_position_setpoint(2))) { - //minimal altitude either 20cm or what is necessary for correct estimation e.g. optical flow + // minimal altitude either 20cm or what is necessary for correct estimation e.g. optical flow float min_altitude = 0.2f; const float min_distance_to_ground = _sub_vehicle_local_position->get().hagl_min; diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 5fdd58ce3b30..433f0956e53a 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -347,7 +347,7 @@ void FlightTaskManualAltitude::_updateSetpoints() 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; } diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp index 77abc78ab10f..70af19134327 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp @@ -38,7 +38,7 @@ #include "Takeoff.hpp" #include -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); _takeoff_ramp_vz_init = -hover_thrust / velocity_p_gain; diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp index 957562e1b756..d3bb433d20f4 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp @@ -61,7 +61,14 @@ class Takeoff // initialize parameters 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 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. @@ -70,6 +77,15 @@ class Takeoff */ void updateTakeoffState(const bool armed, const bool landed, const bool want_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); TakeoffState getTakeoffState() { return _takeoff_state; } diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 82690e4b04b8..1580aedce1db 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -360,7 +360,7 @@ MulticopterPositionControl::parameters_update(bool force) // set trigger time for takeoff delay _takeoff.setSpoolupTime(_param_mpc_spoolup_time.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) { _wv_controller->update_parameters();