From 179b1cdf56798313d9849e9e727b1fce7d729441 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 15 Nov 2021 21:01:25 +0100 Subject: [PATCH] FlightTaskAuto: merge activate(), reActivate() and update() from FlightTaskAutoLineSmoothVel --- .../tasks/Auto/FlightTaskAuto.cpp | 99 +++++++++++++++++ .../tasks/Auto/FlightTaskAuto.hpp | 2 + .../AutoFollowMe/FlightTaskAutoFollowMe.cpp | 3 +- .../FlightTaskAutoLineSmoothVel.cpp | 104 ------------------ .../FlightTaskAutoLineSmoothVel.hpp | 4 - 5 files changed, 102 insertions(+), 110 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 36ed0f3f67..5219fe4977 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -53,14 +53,49 @@ FlightTaskAuto::FlightTaskAuto() : bool FlightTaskAuto::activate(const vehicle_local_position_setpoint_s &last_setpoint) { bool ret = FlightTask::activate(last_setpoint); + _position_setpoint = _position; _velocity_setpoint = _velocity; _yaw_setpoint = _yaw; _yawspeed_setpoint = 0.0f; _setDefaultConstraints(); + + // Set setpoints equal current state. + _velocity_setpoint = _velocity; + _position_setpoint = _position; + + Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz}; + Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z}; + Vector3f accel_prev{last_setpoint.acceleration}; + + for (int i = 0; i < 3; i++) { + // If the position setpoint is unknown, set to the current postion + if (!PX4_ISFINITE(pos_prev(i))) { pos_prev(i) = _position(i); } + + // If the velocity setpoint is unknown, set to the current velocity + if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); } + + // No acceleration estimate available, set to zero if the setpoint is NAN + if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; } + } + + _position_smoothing.reset(accel_prev, vel_prev, pos_prev); + + _yaw_sp_prev = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw; + _updateTrajConstraints(); + _is_emergency_braking_active = false; + return ret; } +void FlightTaskAuto::reActivate() +{ + FlightTask::reActivate(); + + // On ground, reset acceleration and velocity to zero + _position_smoothing.reset({0.f, 0.f, 0.f}, {0.f, 0.f, 0.7f}, _position); +} + bool FlightTaskAuto::updateInitialize() { bool ret = FlightTask::updateInitialize(); @@ -82,6 +117,70 @@ bool FlightTaskAuto::updateInitialize() return ret; } +bool FlightTaskAuto::update() +{ + bool ret = FlightTask::update(); + // always reset constraints because they might change depending on the type + _setDefaultConstraints(); + + // The only time a thrust set-point is sent out is during + // idle. Hence, reset thrust set-point to NAN in case the + // vehicle exits idle. + + if (_type_previous == WaypointType::idle) { + _acceleration_setpoint.setNaN(); + } + + // during mission and reposition, raise the landing gears but only + // if altitude is high enough + if (_highEnoughForLandingGear()) { + _gear.landing_gear = landing_gear_s::GEAR_UP; + } + + switch (_type) { + case WaypointType::idle: + _prepareIdleSetpoints(); + break; + + case WaypointType::land: + _prepareLandSetpoints(); + break; + + case WaypointType::loiter: + + /* fallthrought */ + case WaypointType::position: + _preparePositionSetpoints(); + break; + + case WaypointType::takeoff: + _prepareTakeoffSetpoints(); + break; + + case WaypointType::velocity: + _prepareVelocitySetpoints(); + break; + + default: + _preparePositionSetpoints(); + break; + } + + if (_param_com_obs_avoid.get()) { + _obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type); + _obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint, + _yawspeed_setpoint); + } + + + _generateSetpoints(); + + // update previous type + _type_previous = _type; + + return ret; +} + bool FlightTaskAuto::updateFinalize() { // All the auto FlightTasks have to comply with defined maximum yaw rate diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index e46d60e791..2ba49077ba 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -88,7 +88,9 @@ public: virtual ~FlightTaskAuto() = default; bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; + void reActivate() override; bool updateInitialize() override; + bool update() override; bool updateFinalize() override; /** diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp b/src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp index dda7374f66..195472465a 100644 --- a/src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp @@ -42,9 +42,8 @@ using namespace matrix; bool FlightTaskAutoFollowMe::update() { - bool ret = FlightTaskAuto::update(); _position_setpoint = _target; matrix::Vector2f vel_sp = _getTargetVelocityXY(); _velocity_setpoint = matrix::Vector3f(vel_sp(0), vel_sp(1), 0.0f); - return ret; + return true; } diff --git a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index 6c044a7a4c..d80aae3e89 100644 --- a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -39,107 +39,3 @@ #include using namespace matrix; - -bool FlightTaskAutoLineSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint) -{ - bool ret = FlightTaskAuto::activate(last_setpoint); - - // Set setpoints equal current state. - _velocity_setpoint = _velocity; - _position_setpoint = _position; - - Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz}; - Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z}; - Vector3f accel_prev{last_setpoint.acceleration}; - - for (int i = 0; i < 3; i++) { - // If the position setpoint is unknown, set to the current postion - if (!PX4_ISFINITE(pos_prev(i))) { pos_prev(i) = _position(i); } - - // If the velocity setpoint is unknown, set to the current velocity - if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); } - - // No acceleration estimate available, set to zero if the setpoint is NAN - if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; } - } - - _position_smoothing.reset(accel_prev, vel_prev, pos_prev); - - _yaw_sp_prev = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw; - _updateTrajConstraints(); - _is_emergency_braking_active = false; - - return ret; -} - -void FlightTaskAutoLineSmoothVel::reActivate() -{ - FlightTaskAuto::reActivate(); - - // On ground, reset acceleration and velocity to zero - _position_smoothing.reset({0.f, 0.f, 0.f}, {0.f, 0.f, 0.7f}, _position); -} - -bool FlightTaskAutoLineSmoothVel::update() -{ - bool ret = FlightTaskAuto::update(); - // always reset constraints because they might change depending on the type - _setDefaultConstraints(); - - // The only time a thrust set-point is sent out is during - // idle. Hence, reset thrust set-point to NAN in case the - // vehicle exits idle. - - if (_type_previous == WaypointType::idle) { - _acceleration_setpoint.setNaN(); - } - - // during mission and reposition, raise the landing gears but only - // if altitude is high enough - if (_highEnoughForLandingGear()) { - _gear.landing_gear = landing_gear_s::GEAR_UP; - } - - switch (_type) { - case WaypointType::idle: - _prepareIdleSetpoints(); - break; - - case WaypointType::land: - _prepareLandSetpoints(); - break; - - case WaypointType::loiter: - - /* fallthrought */ - case WaypointType::position: - _preparePositionSetpoints(); - break; - - case WaypointType::takeoff: - _prepareTakeoffSetpoints(); - break; - - case WaypointType::velocity: - _prepareVelocitySetpoints(); - break; - - default: - _preparePositionSetpoints(); - break; - } - - if (_param_com_obs_avoid.get()) { - _obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type); - _obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint, - _yawspeed_setpoint); - } - - - _generateSetpoints(); - - // update previous type - _type_previous = _type; - - return ret; -} diff --git a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp index c314c02541..c08be22816 100644 --- a/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp +++ b/src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp @@ -47,8 +47,4 @@ class FlightTaskAutoLineSmoothVel : public FlightTaskAuto public: FlightTaskAutoLineSmoothVel() = default; virtual ~FlightTaskAutoLineSmoothVel() = default; - - bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; - void reActivate() override; - bool update() override; };