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

Revert "MCPosControl: fix invalid setpoint race condition"

Browse Source 
This reverts commit e7a2c1d88e.
This commit is contained in:
Matthias Grob
2022-05-05 13:26:15 +02:00
parent 1f3a78b535
commit d9a2fe5226
1 changed files with 17 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/MulticopterPositionControl.cpp
+17 -20
View File
@@ -348,7 +348,7 @@ void MulticopterPositionControl::Run()
}
}
const bool is_trajectory_setpoint_updated = _trajectory_setpoint_sub.update(&_setpoint);
_trajectory_setpoint_sub.update(&_setpoint);
// adjust existing (or older) setpoint with any EKF reset deltas
if ((_setpoint.timestamp != 0) && (_setpoint.timestamp < vehicle_local_position.timestamp)) {
@@ -451,28 +451,25 @@ void MulticopterPositionControl::Run()
_vehicle_constraints.want_takeoff,
_vehicle_constraints.speed_up, false, vehicle_local_position.timestamp_sample);
const bool flying = (_takeoff.getTakeoffState() >= TakeoffState::flight);
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 (is_trajectory_setpoint_updated) {
// make sure takeoff ramp is not amended by acceleration feed-forward
if (!flying) {
_setpoint.acceleration[2] = NAN;
// hover_thrust maybe reset on takeoff
_control.setHoverThrust(_param_mpc_thr_hover.get());
}
// make sure takeoff ramp is not amended by acceleration feed-forward
if (!flying) {
_setpoint.acceleration[2] = NAN;
// hover_thrust maybe reset on takeoff
_control.setHoverThrust(_param_mpc_thr_hover.get());
}
const bool not_taken_off = (_takeoff.getTakeoffState() < TakeoffState::rampup);
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.timestamp = vehicle_local_position.timestamp_sample;
Vector3f(0.f, 0.f, 100.f).copyTo(_setpoint.acceleration); // High downwards acceleration to make sure there's no thrust
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.timestamp = vehicle_local_position.timestamp_sample;
Vector3f(0.f, 0.f, 100.f).copyTo(_setpoint.acceleration); // High downwards acceleration to make sure there's no thrust
// prevent any integrator windup
_control.resetIntegral();
}
// prevent any integrator windup
_control.resetIntegral();
}
// limit tilt during takeoff ramupup