fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 03:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix confusion between trajectory_setpoint and vehicle_local_postion_setpoint

Browse Source
This commit is contained in:
Matthias Grob
2022-10-17 16:18:00 -04:00
committed by Daniel Agar
parent 7c237fca74
commit 75c63aee2a
13 changed files with 91 additions and 157 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/FlightModeManager.cpp
+2 -2
View File
@@ -320,7 +320,7 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
const vehicle_local_position_s &vehicle_local_position)
{
// If the task fails sned out empty NAN setpoints and the controller will emergency failsafe
trajectory_setpoint_s setpoint = FlightTask::empty_setpoint;
trajectory_setpoint_s setpoint = FlightTask::empty_trajectory_setpoint;
vehicle_constraints_s constraints = FlightTask::empty_constraints;
if (_current_task.task->updateInitialize() && _current_task.task->update()) {
@@ -392,7 +392,7 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
}
// Save current setpoints for the next FlightTask
trajectory_setpoint_s last_setpoint = FlightTask::empty_setpoint;
trajectory_setpoint_s last_setpoint = FlightTask::empty_trajectory_setpoint;
ekf_reset_counters_s last_reset_counters{};
if (isAnyTaskActive()) {
src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
+2 -4
View File
@@ -3,9 +3,7 @@
#include <lib/geo/geo.h>
constexpr uint64_t FlightTask::_timeout;
// First index of empty_setpoint corresponds to time-stamp and requires a finite number.
const trajectory_setpoint_s FlightTask::empty_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN};
const trajectory_setpoint_s FlightTask::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN};
const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, false, {}};
const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}};
@@ -21,7 +19,7 @@ bool FlightTask::activate(const trajectory_setpoint_s &last_setpoint)
void FlightTask::reActivate()
{
// Preserve vertical velocity while on the ground to allow descending by stick for reliable land detection
trajectory_setpoint_s setpoint_preserve_vertical{empty_setpoint};
trajectory_setpoint_s setpoint_preserve_vertical{empty_trajectory_setpoint};
setpoint_preserve_vertical.velocity[2] = _velocity_setpoint(2);
activate(setpoint_preserve_vertical);
}
src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp
+2 -3
View File
@@ -138,10 +138,9 @@ public:
const vehicle_trajectory_waypoint_s &getAvoidanceWaypoint() { return _desired_waypoint; }
/**
* Empty setpoint.
* All setpoints are set to NAN.
* All setpoints are set to NAN (uncontrolled). Timestampt zero.
*/
static const trajectory_setpoint_s empty_setpoint;
static const trajectory_setpoint_s empty_trajectory_setpoint;
/**
* Empty constraints.