From ee87257a8dfb595348da61040ebb1479082152e0 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 30 Apr 2021 19:29:51 +0200 Subject: [PATCH] FlightModeManager: move velocity control feedback into FlightTask 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. --- .../flight_mode_manager/FlightModeManager.cpp | 12 ----------- .../flight_mode_manager/FlightModeManager.hpp | 1 - .../tasks/FlightTask/FlightTask.cpp | 20 +++++++++++++++++++ .../tasks/FlightTask/FlightTask.hpp | 3 ++- 4 files changed, 22 insertions(+), 14 deletions(-) diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 0b411221a2..d207dc4e55 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -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; diff --git a/src/modules/flight_mode_manager/FlightModeManager.hpp b/src/modules/flight_mode_manager/FlightModeManager.hpp index 5d59b3e171..b0f9fdf06e 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.hpp +++ b/src/modules/flight_mode_manager/FlightModeManager.hpp @@ -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_sub{ORB_ID(home_position)}; uORB::SubscriptionData _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::SubscriptionData _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp index e986db4616..55c78b5cd5 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp @@ -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 diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp index 257ff8ae2d..3752ba0b8b 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp @@ -186,13 +186,14 @@ public: protected: uORB::SubscriptionData _sub_vehicle_local_position{ORB_ID(vehicle_local_position)}; uORB::SubscriptionData _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 */