Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-27 10:17:45 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

mc_pos_control: fix adjusting the wrong setpoint

Browse Source 
There are two local_position_setpoint in the position controller.
One describing the setpoint the task gives to the position controller
and a second one with the output of the position controller. I corrected
the wrong one during takeoff because the new takeoff thrust ramp runs after
the controller and not before.
This commit is contained in:
Matthias Grob
2019-05-16 10:23:06 +01:00
committed by Lorenz Meier
parent ad6eb19f09
commit a9f0981aaf
1 changed files with 7 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+7 -8
View File
@@ -675,21 +675,20 @@ MulticopterPositionControl::run()
_takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, constraints.want_takeoff, local_pos_sp.thrust[2], !_control_mode.flag_control_climb_rate_enabled); _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, constraints.want_takeoff, local_pos_sp.thrust[2], !_control_mode.flag_control_climb_rate_enabled);
local_pos_sp.thrust[2] = _takeoff.updateThrustRamp(local_pos_sp.thrust[2], _dt); local_pos_sp.thrust[2] = _takeoff.updateThrustRamp(local_pos_sp.thrust[2], _dt);
if (_takeoff.getTakeoffState() < TakeoffState::flight) { if (_takeoff.getTakeoffState() < TakeoffState::flight && !PX4_ISFINITE(setpoint.thrust[2])) {
// we set thrust to zero, this will help to decide if we are actually landed or not // we are not flying yet and need to avoid any corrections
setpoint.thrust[0] = setpoint.thrust[1] = 0.0f; // set the horizontal thrust to zero
local_pos_sp.thrust[0] = local_pos_sp.thrust[1] = 0.0f;
// set yaw-sp to current yaw to avoid any corrections // set yaw-sp to current yaw to avoid any corrections
setpoint.yaw = _states.yaw; // TODO: we need a clean way to disable yaw control
setpoint.yawspeed = 0.f; local_pos_sp.yaw = _states.yaw;
local_pos_sp.yawspeed = 0.f;
// prevent any integrator windup // prevent any integrator windup
_control.resetIntegralXY(); _control.resetIntegralXY();
_control.resetIntegralZ(); _control.resetIntegralZ();
} }
if (_takeoff.getTakeoffState() < TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) { if (_takeoff.getTakeoffState() < TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) {
// Keep throttle low when landed and NOT in smooth takeoff
setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f;
setpoint.yaw = _states.yaw;
// reactivate the task which will reset the setpoint to current state // reactivate the task which will reset the setpoint to current state
_flight_tasks.reActivate(); _flight_tasks.reActivate();
} }