fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 15:47:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightModeManager: correct setpoint struct initialization

Browse Source 
It is important that setpoints get initialized with NAN and not overwritten
if specifically set by a successful flight task execution. It's then
clear if any setpoints were intentionally and successfully set.
Crucial for the position controller's emergency failsafe and the
seamless setpoint handover to the next flight task.
This commit is contained in:
Matthias Grob
2021-01-17 14:06:30 +01:00
committed by Lorenz Meier
parent 63db61a700
commit df54f938ef
3 changed files with 10 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/FlightModeManager.cpp
+10 -8
View File
@@ -494,13 +494,15 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
}
}
if (_current_task.task->updateInitialize() && _current_task.task->update() && _current_task.task->updateFinalize()) {
// updated
}
// initialize with empty NAN setpoints, if the task fails they get sent out and controller's emergency failsafe kicks in
vehicle_local_position_setpoint_s setpoint = FlightTask::empty_setpoint;
vehicle_constraints_s constraints = FlightTask::empty_constraints;
// setpoints and constraints for the position controller from flighttask or failsafe
vehicle_local_position_setpoint_s setpoint = _current_task.task->getPositionSetpoint();
vehicle_constraints_s constraints = _current_task.task->getConstraints();
if (_current_task.task->updateInitialize() && _current_task.task->update() && _current_task.task->updateFinalize()) {
// setpoints and constraints for the position controller from flighttask
setpoint = _current_task.task->getPositionSetpoint();
constraints = _current_task.task->getConstraints();
}
// limit altitude according to land detector
limitAltitude(setpoint, vehicle_local_position);
@@ -609,8 +611,8 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
}
// Save current setpoints for the next FlightTask
vehicle_local_position_setpoint_s last_setpoint{};
ekf_reset_counters_s last_reset_counters = FlightTask::zero_reset_counters;
vehicle_local_position_setpoint_s last_setpoint = FlightTask::empty_setpoint;
ekf_reset_counters_s last_reset_counters{};
if (isAnyTaskActive()) {
last_setpoint = _current_task.task->getPositionSetpoint();
src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
-1
View File
@@ -6,7 +6,6 @@ constexpr uint64_t FlightTask::_timeout;
// First index of empty_setpoint corresponds to time-stamp and requires a finite number.
const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {}};
const ekf_reset_counters_s FlightTask::zero_reset_counters = {};
const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}};
const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}};
src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp
-5
View File
@@ -150,11 +150,6 @@ public:
*/
static const vehicle_local_position_setpoint_s empty_setpoint;
/**.
* All counters are set to 0.
*/
static const ekf_reset_counters_s zero_reset_counters;
/**
* Empty constraints.
* All constraints are set to NAN.