Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 22:32:11 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

mc_pos_control: handle takeoff and landing thrust limit the same way

Browse Source 
The landing thrust limit was after the position controller and
could be inconsistent with what the takeoff limit did. This
resulted in different thrust values sequentially getting applied
during landing.
This commit is contained in:
Matthias Grob
2020-03-09 10:04:04 +01:00
parent 3049a3d14d
commit 5027c68c0a
1 changed files with 8 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+8 -23
View File
@@ -608,7 +608,11 @@ MulticopterPositionControl::Run()
constraints.speed_up, !_control_mode.flag_control_climb_rate_enabled, time_stamp_now);
constraints.speed_up = _takeoff.updateRamp(_dt, constraints.speed_up);
if (_takeoff.getTakeoffState() < TakeoffState::rampup) {
const bool not_taken_off = _takeoff.getTakeoffState() < TakeoffState::rampup;
const bool flying = _takeoff.getTakeoffState() >= TakeoffState::flight;
const bool flying_but_ground_contact = flying && _vehicle_land_detected.ground_contact;
if (not_taken_off || flying_but_ground_contact) {
// we are not flying yet and need to avoid any corrections
reset_setpoint_to_nan(setpoint);
setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f;
@@ -618,6 +622,9 @@ MulticopterPositionControl::Run()
setpoint.yawspeed = 0.f;
// prevent any integrator windup
_control.resetIntegral();
}
if (not_taken_off) {
// reactivate the task which will reset the setpoint to current state
_flight_tasks.reActivate();
}
@@ -661,13 +668,6 @@ MulticopterPositionControl::Run()
attitude_setpoint.timestamp = time_stamp_now;
_control.getAttitudeSetpoint(attitude_setpoint);
// Part of landing logic: if ground-contact/maybe landed was detected, turn off
// controller. This message does not have to be logged as part of the vehicle_local_position_setpoint topic.
// Note: only adust thrust output if there was not thrust-setpoint demand in D-direction.
if (_takeoff.getTakeoffState() > TakeoffState::rampup) {
limit_thrust_during_landing(attitude_setpoint);
}
// publish attitude setpoint
// It's important to publish also when disarmed otheriwse the attitude setpoint stays uninitialized.
// Not publishing when not running a flight task
@@ -908,21 +908,6 @@ MulticopterPositionControl::start_flight_task()
}
}
void
MulticopterPositionControl::limit_thrust_during_landing(vehicle_attitude_setpoint_s &setpoint)
{
if (_vehicle_land_detected.ground_contact
|| _vehicle_land_detected.maybe_landed) {
// we set the collective thrust to zero, this will help to decide if we are actually landed or not
setpoint.thrust_body[2] = 0.f;
// go level to avoid corrections but keep the heading we have
Quatf(AxisAngle<float>(0, 0, _states.yaw)).copyTo(setpoint.q_d);
setpoint.yaw_sp_move_rate = 0.f;
// prevent any position control integrator windup
_control.resetIntegral();
}
}
void
MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states,
const bool force, bool warn)