fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 13:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightModeManager: move velocity control feedback into FlightTask

Browse Source 
This was only handled outside because FlightTaks lived in the
multicopter position controller which produces the data that was
needed but now it doesn't make sense anymore to handle this
subscription separately.

It's better to have it inside the base task to have the data available
on task activation sucht that e.g. Altitude mode can take over smoothly
from Position mode.
This commit is contained in:
Matthias Grob
2021-04-30 19:29:51 +02:00
parent c28533677d
commit ee87257a8d
4 changed files with 22 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/FlightModeManager.cpp
-12
View File
@@ -452,18 +452,6 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
{
_current_task.task->setYawHandler(_wv_controller);
// Inform FlightTask about the input and output of the velocity controller
// This is used to properly initialize the velocity setpoint when onpening the position loop (position unlock)
if (_vehicle_local_position_setpoint_sub.updated()) {
vehicle_local_position_setpoint_s vehicle_local_position_setpoint;
if (_vehicle_local_position_setpoint_sub.copy(&vehicle_local_position_setpoint)) {
const Vector3f vel_sp{vehicle_local_position_setpoint.vx, vehicle_local_position_setpoint.vy, vehicle_local_position_setpoint.vz};
const Vector3f acc_sp{vehicle_local_position_setpoint.acceleration};
_current_task.task->updateVelocityControllerFeedback(vel_sp, acc_sp);
}
}
// If the task fails sned out empty NAN setpoints and the controller will emergency failsafe
vehicle_local_position_setpoint_s setpoint = FlightTask::empty_setpoint;
vehicle_constraints_s constraints = FlightTask::empty_constraints;