fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/flight_mode_manager/FlightModeManager.cpp
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;

1
src/modules/flight_mode_manager/FlightModeManager.hpp
View File

@ -144,7 +144,6 @@ private:
uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)};
uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
uORB::SubscriptionData<home_position_s> _home_position_sub{ORB_ID(home_position)};
uORB::SubscriptionData<vehicle_control_mode_s> _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::SubscriptionData<vehicle_land_detected_s> _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};

20
src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
View File

@ -34,6 +34,7 @@ bool FlightTask::updateInitialize()
_sub_home_position.update();
_evaluateVehicleLocalPosition();
_evaluateVehicleLocalPositionSetpoint();
_evaluateDistanceToGround();
return true;
}
@ -163,6 +164,25 @@ void FlightTask::_evaluateVehicleLocalPosition()
}
}
void FlightTask::_evaluateVehicleLocalPositionSetpoint()
{
vehicle_local_position_setpoint_s vehicle_local_position_setpoint;
// Only use data that is received within a certain timestamp
if (_vehicle_local_position_setpoint_sub.copy(&vehicle_local_position_setpoint)
&& (_time_stamp_current - vehicle_local_position_setpoint.timestamp) < _timeout) {
// Inform 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)
_velocity_setpoint_feedback = matrix::Vector3f(vehicle_local_position_setpoint.vx, vehicle_local_position_setpoint.vy,
vehicle_local_position_setpoint.vz);
_acceleration_setpoint_feedback = matrix::Vector3f(vehicle_local_position_setpoint.acceleration);
} else {
_velocity_setpoint_feedback.setAll(NAN);
_acceleration_setpoint_feedback.setAll(NAN);
}
}
void FlightTask::_evaluateDistanceToGround()
{
// Altitude above ground is local z-position or altitude above home or distance sensor altitude depending on what's available

3
src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp
View File

@ -186,13 +186,14 @@ public:
protected:
uORB::SubscriptionData<vehicle_local_position_s> _sub_vehicle_local_position{ORB_ID(vehicle_local_position)};
uORB::SubscriptionData<home_position_s> _sub_home_position{ORB_ID(home_position)};
uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
/** Reset all setpoints to NAN */
void _resetSetpoints();
/** Check and update local position */
void _evaluateVehicleLocalPosition();
void _evaluateVehicleLocalPositionSetpoint();
void _evaluateDistanceToGround();
/** Set constraints to default values */