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

MCPosControl: fix invalid setpoint race condition

Browse Source 
Only modify the _setpoint for takeoff when there is a new uORB message
to avoid a race condition where both vel_sp(2) and accel_sp(2) can be
NAN at the same time.
This commit is contained in:
bresch
2021-07-28 11:22:24 +02:00
committed by Daniel Agar
parent 44f12acafe
commit e7a2c1d88e
1 changed files with 17 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/MulticopterPositionControl.cpp
+17 -14
View File
@@ -290,7 +290,7 @@ void MulticopterPositionControl::Run()
if (_vehicle_control_mode.flag_multicopter_position_control_enabled) {
_trajectory_setpoint_sub.update(&_setpoint);
const bool is_trajectory_setpoint_updated = _trajectory_setpoint_sub.update(&_setpoint);
// adjust existing (or older) setpoint with any EKF reset deltas
if (_setpoint.timestamp < local_pos.timestamp) {
@@ -361,22 +361,25 @@ void MulticopterPositionControl::Run()
_vehicle_constraints.want_takeoff,
_vehicle_constraints.speed_up, false, time_stamp_now);
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);
const bool flying = (_takeoff.getTakeoffState() >= TakeoffState::flight);
// make sure takeoff ramp is not amended by acceleration feed-forward
if (!flying) {
_setpoint.acceleration[2] = NAN;
}
if (is_trajectory_setpoint_updated) {
// make sure takeoff ramp is not amended by acceleration feed-forward
if (!flying) {
_setpoint.acceleration[2] = NAN;
}
if (not_taken_off || flying_but_ground_contact) {
// we are not flying yet and need to avoid any corrections
reset_setpoint_to_nan(_setpoint);
Vector3f(0.f, 0.f, 100.f).copyTo(_setpoint.acceleration); // High downwards acceleration to make sure there's no thrust
const bool not_taken_off = (_takeoff.getTakeoffState() < TakeoffState::rampup);
const bool flying_but_ground_contact = (flying && _vehicle_land_detected.ground_contact);
// prevent any integrator windup
_control.resetIntegral();
if (not_taken_off || flying_but_ground_contact) {
// we are not flying yet and need to avoid any corrections
reset_setpoint_to_nan(_setpoint);
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();
}
}
// limit tilt during takeoff ramupup