From e7faf3c668629384249211d4886d41aa09e9a3e4 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 28 Sep 2019 13:42:15 -0400 Subject: [PATCH 1/2] FlightTasks: switch to uORB::Subscription --- src/lib/FlightTasks/FlightTasks.cpp | 11 -- src/lib/FlightTasks/FlightTasks.hpp | 8 +- .../FlightTasks/tasks/Auto/FlightTaskAuto.cpp | 117 +++++++----------- .../FlightTasks/tasks/Auto/FlightTaskAuto.hpp | 11 +- .../tasks/AutoMapper/FlightTaskAutoMapper.cpp | 4 +- .../AutoMapper2/FlightTaskAutoMapper2.cpp | 8 +- .../tasks/FlightTask/CMakeLists.txt | 1 - .../tasks/FlightTask/FlightTask.cpp | 89 ++++++------- .../tasks/FlightTask/FlightTask.hpp | 14 +-- .../tasks/FlightTask/SubscriptionArray.cpp | 85 ------------- .../tasks/FlightTask/SubscriptionArray.hpp | 112 ----------------- .../tasks/Manual/FlightTaskManual.cpp | 28 ++--- .../tasks/Manual/FlightTaskManual.hpp | 3 +- .../FlightTaskManualAltitude.cpp | 32 ++--- .../FlightTaskManualAltitude.hpp | 3 +- .../FlightTaskManualPosition.cpp | 17 +-- .../FlightTaskManualPosition.hpp | 1 - .../tasks/Offboard/FlightTaskOffboard.cpp | 103 +++++++-------- .../tasks/Offboard/FlightTaskOffboard.hpp | 4 +- .../tasks/Utility/ObstacleAvoidance.cpp | 29 ++--- .../tasks/Utility/ObstacleAvoidance.hpp | 9 +- .../tasks/Utility/ObstacleAvoidanceTest.cpp | 8 -- 22 files changed, 191 insertions(+), 506 deletions(-) delete mode 100644 src/lib/FlightTasks/tasks/FlightTask/SubscriptionArray.cpp delete mode 100644 src/lib/FlightTasks/tasks/FlightTask/SubscriptionArray.hpp diff --git a/src/lib/FlightTasks/FlightTasks.cpp b/src/lib/FlightTasks/FlightTasks.cpp index 7e1a30741ffa..d6355ca75c2f 100644 --- a/src/lib/FlightTasks/FlightTasks.cpp +++ b/src/lib/FlightTasks/FlightTasks.cpp @@ -20,7 +20,6 @@ bool FlightTasks::update() _updateCommand(); if (isAnyTaskActive()) { - _subscription_array.update(); return _current_task.task->updateInitialize() && _current_task.task->update() && _current_task.task->updateFinalize(); } @@ -77,16 +76,6 @@ FlightTaskError FlightTasks::switchTask(FlightTaskIndex new_task_index) return FlightTaskError::NoError; } - // subscription failed - if (!_current_task.task->initializeSubscriptions(_subscription_array)) { - _current_task.task->~FlightTask(); - _current_task.task = nullptr; - _current_task.index = FlightTaskIndex::None; - return FlightTaskError::SubscriptionFailed; - } - - _subscription_array.forcedUpdate(); // make sure data is available for all new subscriptions - // activation failed if (!_current_task.task->updateInitialize() || !_current_task.task->activate(last_setpoint)) { _current_task.task->~FlightTask(); diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index c6a401fc2737..64eedaa6b947 100644 --- a/src/lib/FlightTasks/FlightTasks.hpp +++ b/src/lib/FlightTasks/FlightTasks.hpp @@ -42,7 +42,6 @@ #pragma once #include "FlightTask.hpp" -#include "SubscriptionArray.hpp" #include "FlightTasks_generated.hpp" #include #include @@ -55,7 +54,6 @@ enum class FlightTaskError : int { NoError = 0, InvalidTask = -1, - SubscriptionFailed = -2, ActivationFailed = -3 }; @@ -169,23 +167,21 @@ class FlightTasks }; flight_task_t _current_task = {nullptr, FlightTaskIndex::None}; - SubscriptionArray _subscription_array; - struct task_error_t { int error; const char *msg; }; - static const int _numError = 4; + static constexpr int _numError = 3; /** * Map from Error int to user friendly string. */ task_error_t _taskError[_numError] = { {static_cast(FlightTaskError::NoError), "No Error"}, {static_cast(FlightTaskError::InvalidTask), "Invalid Task "}, - {static_cast(FlightTaskError::SubscriptionFailed), "Subscription Failed"}, {static_cast(FlightTaskError::ActivationFailed), "Activation Failed"} }; + /** * Check for vehicle commands (received via MAVLink), evaluate and acknowledge them */ diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index c46f59c0e84b..1bd524020a46 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp @@ -48,35 +48,6 @@ FlightTaskAuto::FlightTaskAuto() : } -bool FlightTaskAuto::initializeSubscriptions(SubscriptionArray &subscription_array) -{ - if (!FlightTask::initializeSubscriptions(subscription_array)) { - return false; - } - - if (!subscription_array.get(ORB_ID(position_setpoint_triplet), _sub_triplet_setpoint)) { - return false; - } - - if (!subscription_array.get(ORB_ID(home_position), _sub_home_position)) { - return false; - } - - if (!subscription_array.get(ORB_ID(vehicle_status), _sub_vehicle_status)) { - return false; - } - - if (!subscription_array.get(ORB_ID(manual_control_setpoint), _sub_manual_control_setpoint)) { - return false; - } - - if (!_obstacle_avoidance.initializeSubscriptions(subscription_array)) { - return false; - } - - return true; -} - bool FlightTaskAuto::activate(vehicle_local_position_setpoint_s last_setpoint) { bool ret = FlightTask::activate(last_setpoint); @@ -91,6 +62,12 @@ bool FlightTaskAuto::activate(vehicle_local_position_setpoint_s last_setpoint) bool FlightTaskAuto::updateInitialize() { bool ret = FlightTask::updateInitialize(); + + _sub_home_position.update(); + _sub_manual_control_setpoint.update(); + _sub_vehicle_status.update(); + _sub_triplet_setpoint.update(); + // require valid reference and valid target ret = ret && _evaluateGlobalReference() && _evaluateTriplets(); // require valid position @@ -159,17 +136,17 @@ bool FlightTaskAuto::_evaluateTriplets() // Check if triplet is valid. There must be at least a valid altitude. - if (!_sub_triplet_setpoint->get().current.valid || !PX4_ISFINITE(_sub_triplet_setpoint->get().current.alt)) { + if (!_sub_triplet_setpoint.get().current.valid || !PX4_ISFINITE(_sub_triplet_setpoint.get().current.alt)) { // Best we can do is to just set all waypoints to current state and return false. _prev_prev_wp = _triplet_prev_wp = _triplet_target = _triplet_next_wp = _position; _type = WaypointType::position; return false; } - _type = (WaypointType)_sub_triplet_setpoint->get().current.type; + _type = (WaypointType)_sub_triplet_setpoint.get().current.type; // Always update cruise speed since that can change without waypoint changes. - _mc_cruise_speed = _sub_triplet_setpoint->get().current.cruising_speed; + _mc_cruise_speed = _sub_triplet_setpoint.get().current.cruising_speed; if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f)) { // If no speed is planned use the default cruise speed as limit @@ -182,8 +159,8 @@ bool FlightTaskAuto::_evaluateTriplets() // Temporary target variable where we save the local reprojection of the latest navigator current triplet. Vector3f tmp_target; - if (!PX4_ISFINITE(_sub_triplet_setpoint->get().current.lat) - || !PX4_ISFINITE(_sub_triplet_setpoint->get().current.lon)) { + if (!PX4_ISFINITE(_sub_triplet_setpoint.get().current.lat) + || !PX4_ISFINITE(_sub_triplet_setpoint.get().current.lon)) { // No position provided in xy. Lock position if (!PX4_ISFINITE(_lock_position_xy(0))) { tmp_target(0) = _lock_position_xy(0) = _position(0); @@ -200,10 +177,10 @@ bool FlightTaskAuto::_evaluateTriplets() // Convert from global to local frame. map_projection_project(&_reference_position, - _sub_triplet_setpoint->get().current.lat, _sub_triplet_setpoint->get().current.lon, &tmp_target(0), &tmp_target(1)); + _sub_triplet_setpoint.get().current.lat, _sub_triplet_setpoint.get().current.lon, &tmp_target(0), &tmp_target(1)); } - tmp_target(2) = -(_sub_triplet_setpoint->get().current.alt - _reference_altitude); + tmp_target(2) = -(_sub_triplet_setpoint.get().current.alt - _reference_altitude); // Check if anything has changed. We do that by comparing the temporary target // to the internal _triplet_target. @@ -222,7 +199,7 @@ bool FlightTaskAuto::_evaluateTriplets() } else { _triplet_target = tmp_target; - _target_acceptance_radius = _sub_triplet_setpoint->get().current.acceptance_radius; + _target_acceptance_radius = _sub_triplet_setpoint.get().current.acceptance_radius; if (!PX4_ISFINITE(_triplet_target(0)) || !PX4_ISFINITE(_triplet_target(1))) { // Horizontal target is not finite. @@ -237,10 +214,10 @@ bool FlightTaskAuto::_evaluateTriplets() // If _triplet_target has updated, update also _triplet_prev_wp and _triplet_next_wp. _prev_prev_wp = _triplet_prev_wp; - if (_isFinite(_sub_triplet_setpoint->get().previous) && _sub_triplet_setpoint->get().previous.valid) { - map_projection_project(&_reference_position, _sub_triplet_setpoint->get().previous.lat, - _sub_triplet_setpoint->get().previous.lon, &_triplet_prev_wp(0), &_triplet_prev_wp(1)); - _triplet_prev_wp(2) = -(_sub_triplet_setpoint->get().previous.alt - _reference_altitude); + if (_isFinite(_sub_triplet_setpoint.get().previous) && _sub_triplet_setpoint.get().previous.valid) { + map_projection_project(&_reference_position, _sub_triplet_setpoint.get().previous.lat, + _sub_triplet_setpoint.get().previous.lon, &_triplet_prev_wp(0), &_triplet_prev_wp(1)); + _triplet_prev_wp(2) = -(_sub_triplet_setpoint.get().previous.alt - _reference_altitude); } else { _triplet_prev_wp = _position; @@ -249,10 +226,10 @@ bool FlightTaskAuto::_evaluateTriplets() if (_type == WaypointType::loiter) { _triplet_next_wp = _triplet_target; - } else if (_isFinite(_sub_triplet_setpoint->get().next) && _sub_triplet_setpoint->get().next.valid) { - map_projection_project(&_reference_position, _sub_triplet_setpoint->get().next.lat, - _sub_triplet_setpoint->get().next.lon, &_triplet_next_wp(0), &_triplet_next_wp(1)); - _triplet_next_wp(2) = -(_sub_triplet_setpoint->get().next.alt - _reference_altitude); + } else if (_isFinite(_sub_triplet_setpoint.get().next) && _sub_triplet_setpoint.get().next.valid) { + map_projection_project(&_reference_position, _sub_triplet_setpoint.get().next.lat, + _sub_triplet_setpoint.get().next.lon, &_triplet_next_wp(0), &_triplet_next_wp(1)); + _triplet_next_wp(2) = -(_sub_triplet_setpoint.get().next.alt - _reference_altitude); } else { _triplet_next_wp = _triplet_target; @@ -261,7 +238,7 @@ bool FlightTaskAuto::_evaluateTriplets() if (_ext_yaw_handler != nullptr) { // activation/deactivation of weather vane is based on parameter WV_EN and setting of navigator (allow_weather_vane) - (_param_wv_en.get() && _sub_triplet_setpoint->get().current.allow_weather_vane) ? _ext_yaw_handler->activate() : + (_param_wv_en.get() && _sub_triplet_setpoint.get().current.allow_weather_vane) ? _ext_yaw_handler->activate() : _ext_yaw_handler->deactivate(); } @@ -270,13 +247,13 @@ bool FlightTaskAuto::_evaluateTriplets() _yaw_setpoint = _yaw; _yawspeed_setpoint = _ext_yaw_handler->get_weathervane_yawrate(); - } else if (_type == WaypointType::follow_target && _sub_triplet_setpoint->get().current.yawspeed_valid) { - _yawspeed_setpoint = _sub_triplet_setpoint->get().current.yawspeed; + } else if (_type == WaypointType::follow_target && _sub_triplet_setpoint.get().current.yawspeed_valid) { + _yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed; _yaw_setpoint = NAN; } else { - if (_sub_triplet_setpoint->get().current.yaw_valid) { - _yaw_setpoint = _sub_triplet_setpoint->get().current.yaw; + if (_sub_triplet_setpoint.get().current.yaw_valid) { + _yaw_setpoint = _sub_triplet_setpoint.get().current.yaw; } else { _set_heading_from_mode(); @@ -291,16 +268,16 @@ bool FlightTaskAuto::_evaluateTriplets() if (triplet_update || (_current_state != previous_state)) { _updateInternalWaypoints(); - _mission_gear = _sub_triplet_setpoint->get().current.landing_gear; + _mission_gear = _sub_triplet_setpoint.get().current.landing_gear; } if (_param_com_obs_avoid.get() - && _sub_vehicle_status->get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + && _sub_vehicle_status.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { _obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint, _triplet_next_wp, - _sub_triplet_setpoint->get().next.yaw, - _sub_triplet_setpoint->get().next.yawspeed_valid ? _sub_triplet_setpoint->get().next.yawspeed : NAN, - _ext_yaw_handler != nullptr && _ext_yaw_handler->is_active(), _sub_triplet_setpoint->get().current.type); + _sub_triplet_setpoint.get().next.yaw, + _sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : NAN, + _ext_yaw_handler != nullptr && _ext_yaw_handler->is_active(), _sub_triplet_setpoint.get().current.type); _obstacle_avoidance.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt); } @@ -320,15 +297,15 @@ void FlightTaskAuto::_set_heading_from_mode() break; case 1: // Heading points towards home. - if (_sub_home_position->get().valid_hpos) { - v = Vector2f(&_sub_home_position->get().x) - Vector2f(_position); + if (_sub_home_position.get().valid_hpos) { + v = Vector2f(&_sub_home_position.get().x) - Vector2f(_position); } break; case 2: // Heading point away from home. - if (_sub_home_position->get().valid_hpos) { - v = Vector2f(_position) - Vector2f(&_sub_home_position->get().x); + if (_sub_home_position.get().valid_hpos) { + v = Vector2f(_position) - Vector2f(&_sub_home_position.get().x); } break; @@ -372,22 +349,22 @@ bool FlightTaskAuto::_evaluateGlobalReference() // Only update if reference timestamp has changed AND no valid reference altitude // is available. // TODO: this needs to be revisited and needs a more clear implementation - if (_sub_vehicle_local_position->get().ref_timestamp == _time_stamp_reference && PX4_ISFINITE(_reference_altitude)) { + if (_sub_vehicle_local_position.get().ref_timestamp == _time_stamp_reference && PX4_ISFINITE(_reference_altitude)) { // don't need to update anything return true; } - double ref_lat = _sub_vehicle_local_position->get().ref_lat; - double ref_lon = _sub_vehicle_local_position->get().ref_lon; - _reference_altitude = _sub_vehicle_local_position->get().ref_alt; + double ref_lat = _sub_vehicle_local_position.get().ref_lat; + double ref_lon = _sub_vehicle_local_position.get().ref_lon; + _reference_altitude = _sub_vehicle_local_position.get().ref_alt; - if (!_sub_vehicle_local_position->get().z_global) { + if (!_sub_vehicle_local_position.get().z_global) { // we have no valid global altitude // set global reference to local reference _reference_altitude = 0.0f; } - if (!_sub_vehicle_local_position->get().xy_global) { + if (!_sub_vehicle_local_position.get().xy_global) { // we have no valid global alt/lat // set global reference to local reference ref_lat = 0.0; @@ -401,8 +378,8 @@ bool FlightTaskAuto::_evaluateGlobalReference() // check if everything is still finite if (PX4_ISFINITE(_reference_altitude) - && PX4_ISFINITE(_sub_vehicle_local_position->get().ref_lat) - && PX4_ISFINITE(_sub_vehicle_local_position->get().ref_lon)) { + && PX4_ISFINITE(_sub_vehicle_local_position.get().ref_lat) + && PX4_ISFINITE(_sub_vehicle_local_position.get().ref_lon)) { return true; } else { @@ -424,10 +401,10 @@ void FlightTaskAuto::_setDefaultConstraints() Vector2f FlightTaskAuto::_getTargetVelocityXY() { // guard against any bad velocity values - const float vx = _sub_triplet_setpoint->get().current.vx; - const float vy = _sub_triplet_setpoint->get().current.vy; + const float vx = _sub_triplet_setpoint.get().current.vx; + const float vy = _sub_triplet_setpoint.get().current.vy; bool velocity_valid = PX4_ISFINITE(vx) && PX4_ISFINITE(vy) && - _sub_triplet_setpoint->get().current.velocity_valid; + _sub_triplet_setpoint.get().current.velocity_valid; if (velocity_valid) { return Vector2f(vx, vy); diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp index c14371dbb77d..7cafd8872c10 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp @@ -77,7 +77,6 @@ class FlightTaskAuto : public FlightTask FlightTaskAuto(); virtual ~FlightTaskAuto() = default; - bool initializeSubscriptions(SubscriptionArray &subscription_array) override; bool activate(vehicle_local_position_setpoint_s last_setpoint) override; bool updateInitialize() override; bool updateFinalize() override; @@ -99,9 +98,10 @@ class FlightTaskAuto : public FlightTask matrix::Vector3f _next_wp{}; /**< The next waypoint after target (local frame). If no next setpoint is available, next is set to target. */ float _mc_cruise_speed{0.0f}; /**< Requested cruise speed. If not valid, default cruise speed is used. */ WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */ - uORB::SubscriptionPollable *_sub_home_position{nullptr}; - uORB::SubscriptionPollable *_sub_manual_control_setpoint{nullptr}; - uORB::SubscriptionPollable *_sub_vehicle_status{nullptr}; + + uORB::SubscriptionData _sub_home_position{ORB_ID(home_position)}; + uORB::SubscriptionData _sub_manual_control_setpoint{ORB_ID(manual_control_setpoint)}; + uORB::SubscriptionData _sub_vehicle_status{ORB_ID(vehicle_status)}; State _current_state{State::none}; float _target_acceptance_radius{0.0f}; /**< Acceptances radius of the target */ @@ -127,7 +127,8 @@ class FlightTaskAuto : public FlightTask private: matrix::Vector2f _lock_position_xy{NAN, NAN}; /**< if no valid triplet is received, lock positition to current position */ bool _yaw_lock{false}; /**< if within acceptance radius, lock yaw to current yaw */ - uORB::SubscriptionPollable *_sub_triplet_setpoint{nullptr}; + + uORB::SubscriptionData _sub_triplet_setpoint{ORB_ID(position_setpoint_triplet)}; matrix::Vector3f _triplet_target; /**< current triplet from navigator which may differ from the intenal one (_target) depending on the vehicle state. */ diff --git a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp index 173291919e33..f50d95e87e0b 100644 --- a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp +++ b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp @@ -161,9 +161,9 @@ void FlightTaskAutoMapper::_updateAltitudeAboveGround() // We have a valid distance to ground measurement _alt_above_ground = _dist_to_bottom; - } else if (_sub_home_position->get().valid_alt) { + } else if (_sub_home_position.get().valid_alt) { // if home position is set, then altitude above ground is relative to the home position - _alt_above_ground = -_position(2) + _sub_home_position->get().z; + _alt_above_ground = -_position(2) + _sub_home_position.get().z; } } diff --git a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp index ed35fe45c9cd..34fdc1ad4eca 100644 --- a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp +++ b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp @@ -176,9 +176,9 @@ void FlightTaskAutoMapper2::_updateAltitudeAboveGround() // We have a valid distance to ground measurement _alt_above_ground = _dist_to_bottom; - } else if (_sub_home_position->get().valid_alt) { + } else if (_sub_home_position.get().valid_alt) { // if home position is set, then altitude above ground is relative to the home position - _alt_above_ground = -_position(2) + _sub_home_position->get().z; + _alt_above_ground = -_position(2) + _sub_home_position.get().z; } } @@ -199,12 +199,12 @@ bool FlightTaskAutoMapper2::_highEnoughForLandingGear() float FlightTaskAutoMapper2::_getLandSpeed() { bool rc_assist_enabled = _param_mpc_land_rc_help.get(); - bool rc_is_valid = !_sub_vehicle_status->get().rc_signal_lost; + bool rc_is_valid = !_sub_vehicle_status.get().rc_signal_lost; float throttle = 0.5f; if (rc_is_valid && rc_assist_enabled) { - throttle = _sub_manual_control_setpoint->get().z; + throttle = _sub_manual_control_setpoint.get().z; } float speed = 0; diff --git a/src/lib/FlightTasks/tasks/FlightTask/CMakeLists.txt b/src/lib/FlightTasks/tasks/FlightTask/CMakeLists.txt index bc83cc84e571..25a472bc6b46 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/CMakeLists.txt +++ b/src/lib/FlightTasks/tasks/FlightTask/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_library(FlightTask FlightTask.cpp - SubscriptionArray.cpp ) target_include_directories(FlightTask PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp index 6c2d6ed7eded..afaaf8cb267e 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp @@ -9,19 +9,6 @@ const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NA const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}}; const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}}; -bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array) -{ - if (!subscription_array.get(ORB_ID(vehicle_local_position), _sub_vehicle_local_position)) { - return false; - } - - if (!subscription_array.get(ORB_ID(vehicle_attitude), _sub_attitude)) { - return false; - } - - return true; -} - bool FlightTask::activate(vehicle_local_position_setpoint_s last_setpoint) { _resetSetpoints(); @@ -43,6 +30,10 @@ bool FlightTask::updateInitialize() _time = (_time_stamp_current - _time_stamp_activate) / 1e6f; _deltatime = math::min((_time_stamp_current - _time_stamp_last), _timeout) / 1e6f; _time_stamp_last = _time_stamp_current; + + _sub_vehicle_local_position.update(); + _sub_attitude.update(); + _evaluateVehicleLocalPosition(); _checkEkfResetCounters(); return true; @@ -50,40 +41,40 @@ bool FlightTask::updateInitialize() void FlightTask::_initEkfResetCounters() { - _reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter; - _reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter; - _reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter; - _reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter; - _reset_counters.quat = _sub_attitude->get().quat_reset_counter; + _reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter; + _reset_counters.vxy = _sub_vehicle_local_position.get().vxy_reset_counter; + _reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter; + _reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter; + _reset_counters.quat = _sub_attitude.get().quat_reset_counter; } void FlightTask::_checkEkfResetCounters() { // Check if a reset event has happened - if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counters.xy) { + if (_sub_vehicle_local_position.get().xy_reset_counter != _reset_counters.xy) { _ekfResetHandlerPositionXY(); - _reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter; + _reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter; } - if (_sub_vehicle_local_position->get().vxy_reset_counter != _reset_counters.vxy) { + if (_sub_vehicle_local_position.get().vxy_reset_counter != _reset_counters.vxy) { _ekfResetHandlerVelocityXY(); - _reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter; + _reset_counters.vxy = _sub_vehicle_local_position.get().vxy_reset_counter; } - if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counters.z) { + if (_sub_vehicle_local_position.get().z_reset_counter != _reset_counters.z) { _ekfResetHandlerPositionZ(); - _reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter; + _reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter; } - if (_sub_vehicle_local_position->get().vz_reset_counter != _reset_counters.vz) { + if (_sub_vehicle_local_position.get().vz_reset_counter != _reset_counters.vz) { _ekfResetHandlerVelocityZ(); - _reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter; + _reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter; } - if (_sub_attitude->get().quat_reset_counter != _reset_counters.quat) { - float delta_psi = matrix::Eulerf(matrix::Quatf(_sub_attitude->get().delta_q_reset)).psi(); + if (_sub_attitude.get().quat_reset_counter != _reset_counters.quat) { + float delta_psi = matrix::Eulerf(matrix::Quatf(_sub_attitude.get().delta_q_reset)).psi(); _ekfResetHandlerHeading(delta_psi); - _reset_counters.quat = _sub_attitude->get().quat_reset_counter; + _reset_counters.quat = _sub_attitude.get().quat_reset_counter; } } @@ -133,44 +124,44 @@ void FlightTask::_evaluateVehicleLocalPosition() _yaw = NAN; _dist_to_bottom = NAN; - if ((_time_stamp_current - _sub_attitude->get().timestamp) < _timeout) { + if ((_time_stamp_current - _sub_attitude.get().timestamp) < _timeout) { // yaw - _yaw = matrix::Eulerf(matrix::Quatf(_sub_attitude->get().q)).psi(); + _yaw = matrix::Eulerf(matrix::Quatf(_sub_attitude.get().q)).psi(); } // Only use vehicle-local-position topic fields if the topic is received within a certain timestamp - if ((_time_stamp_current - _sub_vehicle_local_position->get().timestamp) < _timeout) { + if ((_time_stamp_current - _sub_vehicle_local_position.get().timestamp) < _timeout) { // position - if (_sub_vehicle_local_position->get().xy_valid) { - _position(0) = _sub_vehicle_local_position->get().x; - _position(1) = _sub_vehicle_local_position->get().y; + if (_sub_vehicle_local_position.get().xy_valid) { + _position(0) = _sub_vehicle_local_position.get().x; + _position(1) = _sub_vehicle_local_position.get().y; } - if (_sub_vehicle_local_position->get().z_valid) { - _position(2) = _sub_vehicle_local_position->get().z; + if (_sub_vehicle_local_position.get().z_valid) { + _position(2) = _sub_vehicle_local_position.get().z; } // velocity - if (_sub_vehicle_local_position->get().v_xy_valid) { - _velocity(0) = _sub_vehicle_local_position->get().vx; - _velocity(1) = _sub_vehicle_local_position->get().vy; + if (_sub_vehicle_local_position.get().v_xy_valid) { + _velocity(0) = _sub_vehicle_local_position.get().vx; + _velocity(1) = _sub_vehicle_local_position.get().vy; } - if (_sub_vehicle_local_position->get().v_z_valid) { - _velocity(2) = _sub_vehicle_local_position->get().vz; + if (_sub_vehicle_local_position.get().v_z_valid) { + _velocity(2) = _sub_vehicle_local_position.get().vz; } // distance to bottom - if (_sub_vehicle_local_position->get().dist_bottom_valid - && PX4_ISFINITE(_sub_vehicle_local_position->get().dist_bottom)) { - _dist_to_bottom = _sub_vehicle_local_position->get().dist_bottom; + if (_sub_vehicle_local_position.get().dist_bottom_valid + && PX4_ISFINITE(_sub_vehicle_local_position.get().dist_bottom)) { + _dist_to_bottom = _sub_vehicle_local_position.get().dist_bottom; } // global frame reference coordinates to enable conversions - if (_sub_vehicle_local_position->get().xy_global && _sub_vehicle_local_position->get().z_global) { - globallocalconverter_init(_sub_vehicle_local_position->get().ref_lat, _sub_vehicle_local_position->get().ref_lon, - _sub_vehicle_local_position->get().ref_alt, _sub_vehicle_local_position->get().ref_timestamp); + if (_sub_vehicle_local_position.get().xy_global && _sub_vehicle_local_position.get().z_global) { + globallocalconverter_init(_sub_vehicle_local_position.get().ref_lat, _sub_vehicle_local_position.get().ref_lon, + _sub_vehicle_local_position.get().ref_alt, _sub_vehicle_local_position.get().ref_timestamp); } } } @@ -194,7 +185,7 @@ bool FlightTask::_checkTakeoff() if (PX4_ISFINITE(_position_setpoint(2))) { // minimal altitude either 20cm or what is necessary for correct estimation e.g. optical flow float min_altitude = 0.2f; - const float min_distance_to_ground = _sub_vehicle_local_position->get().hagl_min; + const float min_distance_to_ground = _sub_vehicle_local_position.get().hagl_min; if (PX4_ISFINITE(min_distance_to_ground)) { min_altitude = min_distance_to_ground + 0.05f; diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp index 37f539f4038b..27e4425e5035 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp @@ -44,7 +44,7 @@ #include #include #include -#include +#include #include #include #include @@ -53,7 +53,6 @@ #include #include #include -#include "SubscriptionArray.hpp" class FlightTask : public ModuleParams { @@ -67,13 +66,6 @@ class FlightTask : public ModuleParams virtual ~FlightTask() = default; - /** - * Initialize the uORB subscriptions using an array - * @param subscription_array handling uORB subscribtions externally across task switches - * @return true on success, false on error - */ - virtual bool initializeSubscriptions(SubscriptionArray &subscription_array); - /** * Call once on the event where you switch to the task * @param state of the previous task @@ -176,8 +168,8 @@ class FlightTask : public ModuleParams protected: - uORB::SubscriptionPollable *_sub_vehicle_local_position{nullptr}; - uORB::SubscriptionPollable *_sub_attitude{nullptr}; + uORB::SubscriptionData _sub_vehicle_local_position{ORB_ID(vehicle_local_position)}; + uORB::SubscriptionData _sub_attitude{ORB_ID(vehicle_attitude)}; /** Reset all setpoints to NAN */ void _resetSetpoints(); diff --git a/src/lib/FlightTasks/tasks/FlightTask/SubscriptionArray.cpp b/src/lib/FlightTasks/tasks/FlightTask/SubscriptionArray.cpp deleted file mode 100644 index 5ff750343546..000000000000 --- a/src/lib/FlightTasks/tasks/FlightTask/SubscriptionArray.cpp +++ /dev/null @@ -1,85 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "SubscriptionArray.hpp" - -#include - -SubscriptionArray::~SubscriptionArray() -{ - cleanup(); -} - -void SubscriptionArray::cleanup() -{ - for (int i = 0; i < _subscriptions_count; ++i) { - delete _subscriptions[i]; - } - - delete[] _subscriptions; - _subscriptions = nullptr; -} - -bool SubscriptionArray::resizeSubscriptions() -{ - const int new_size = _subscriptions_size == 0 ? 4 : _subscriptions_size * 2; - uORB::SubscriptionPollableNode **new_array = new uORB::SubscriptionPollableNode*[new_size]; - - if (!new_array) { - return false; - } - - if (_subscriptions) { - memcpy(new_array, _subscriptions, sizeof(uORB::SubscriptionPollableNode *)*_subscriptions_count); - delete[] _subscriptions; - } - - _subscriptions = new_array; - _subscriptions_size = new_size; - - return true; -} - -void SubscriptionArray::update() -{ - for (int i = 0; i < _subscriptions_count; ++i) { - _subscriptions[i]->update(); - } -} - -void SubscriptionArray::forcedUpdate() -{ - for (int i = 0; i < _subscriptions_count; ++i) { - _subscriptions[i]->forcedUpdate(); - } -} diff --git a/src/lib/FlightTasks/tasks/FlightTask/SubscriptionArray.hpp b/src/lib/FlightTasks/tasks/FlightTask/SubscriptionArray.hpp deleted file mode 100644 index 276ec4cfc01f..000000000000 --- a/src/lib/FlightTasks/tasks/FlightTask/SubscriptionArray.hpp +++ /dev/null @@ -1,112 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file SubscriptionArray.hpp - * - * Simple array that contains a dynamic amount of Subscription instances - * - * @author Beat Küng - */ - -#pragma once - -#include - -class SubscriptionArray -{ -public: - SubscriptionArray() = default; - ~SubscriptionArray(); - - /** - * Get a subscription - * @param meta ORB_ID(topic) - * @param subscription returned subscription (output parameter) - * @param instance topic instance - * @return true on success, false otherwise (subscription set to nullptr) - */ - template - bool get(const struct orb_metadata *meta, uORB::SubscriptionPollable *&subscription, unsigned instance = 0); - - /** - * update all subscriptions (if new data is available) - */ - void update(); - - /** - * update all subscriptions - */ - void forcedUpdate(); - -private: - void cleanup(); - - bool resizeSubscriptions(); - - uORB::SubscriptionPollableNode **_subscriptions{nullptr}; - int _subscriptions_count{0}; ///< number of valid subscriptions - int _subscriptions_size{0}; ///< actual size of the _subscriptions array -}; - - -template -bool SubscriptionArray::get(const struct orb_metadata *meta, uORB::SubscriptionPollable *&subscription, - unsigned instance) -{ - // does it already exist? - for (int i = 0; i < _subscriptions_count; ++i) { - if (_subscriptions[i]->getMeta() == meta && _subscriptions[i]->getInstance() == instance) { - // we know the type must be correct, so we can use reinterpret_cast (dynamic_cast is not available) - subscription = reinterpret_cast*>(_subscriptions[i]); - return true; - } - } - - // resize if needed - if (_subscriptions_count >= _subscriptions_size) { - if (!resizeSubscriptions()) { - subscription = nullptr; - return false; - } - } - - subscription = new uORB::SubscriptionPollable(meta, 0, instance); - - if (!subscription) { - return false; - } - - _subscriptions[_subscriptions_count++] = subscription; - return true; -} diff --git a/src/lib/FlightTasks/tasks/Manual/FlightTaskManual.cpp b/src/lib/FlightTasks/tasks/Manual/FlightTaskManual.cpp index f6d5c2f994bb..89a240887173 100644 --- a/src/lib/FlightTasks/tasks/Manual/FlightTaskManual.cpp +++ b/src/lib/FlightTasks/tasks/Manual/FlightTaskManual.cpp @@ -42,22 +42,12 @@ using namespace matrix; using namespace time_literals; -bool FlightTaskManual::initializeSubscriptions(SubscriptionArray &subscription_array) -{ - if (!FlightTask::initializeSubscriptions(subscription_array)) { - return false; - } - - if (!subscription_array.get(ORB_ID(manual_control_setpoint), _sub_manual_control_setpoint)) { - return false; - } - - return true; -} - bool FlightTaskManual::updateInitialize() { bool ret = FlightTask::updateInitialize(); + + _sub_manual_control_setpoint.update(); + const bool sticks_available = _evaluateSticks(); if (_sticks_data_required) { @@ -72,13 +62,13 @@ bool FlightTaskManual::_evaluateSticks() hrt_abstime rc_timeout = (_param_com_rc_loss_t.get() * 1.5f) * 1_s; /* Sticks are rescaled linearly and exponentially to [-1,1] */ - if ((_time_stamp_current - _sub_manual_control_setpoint->get().timestamp) < rc_timeout) { + if ((_time_stamp_current - _sub_manual_control_setpoint.get().timestamp) < rc_timeout) { /* Linear scale */ - _sticks(0) = _sub_manual_control_setpoint->get().x; /* NED x, "pitch" [-1,1] */ - _sticks(1) = _sub_manual_control_setpoint->get().y; /* NED y, "roll" [-1,1] */ - _sticks(2) = -(_sub_manual_control_setpoint->get().z - 0.5f) * 2.f; /* NED z, "thrust" resacaled from [0,1] to [-1,1] */ - _sticks(3) = _sub_manual_control_setpoint->get().r; /* "yaw" [-1,1] */ + _sticks(0) = _sub_manual_control_setpoint.get().x; /* NED x, "pitch" [-1,1] */ + _sticks(1) = _sub_manual_control_setpoint.get().y; /* NED y, "roll" [-1,1] */ + _sticks(2) = -(_sub_manual_control_setpoint.get().z - 0.5f) * 2.f; /* NED z, "thrust" resacaled from [0,1] to [-1,1] */ + _sticks(3) = _sub_manual_control_setpoint.get().r; /* "yaw" [-1,1] */ /* Exponential scale */ _sticks_expo(0) = math::expo_deadzone(_sticks(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get()); @@ -89,7 +79,7 @@ bool FlightTaskManual::_evaluateSticks() // Only switch the landing gear up if the user switched from gear down to gear up. // If the user had the switch in the gear up position and took off ignore it // until he toggles the switch to avoid retracting the gear immediately on takeoff. - int8_t gear_switch = _sub_manual_control_setpoint->get().gear_switch; + int8_t gear_switch = _sub_manual_control_setpoint.get().gear_switch; if (_gear_switch_old != gear_switch) { _applyGearSwitch(gear_switch); diff --git a/src/lib/FlightTasks/tasks/Manual/FlightTaskManual.hpp b/src/lib/FlightTasks/tasks/Manual/FlightTaskManual.hpp index 2ee0a37f7b12..c0ad8dab8862 100644 --- a/src/lib/FlightTasks/tasks/Manual/FlightTaskManual.hpp +++ b/src/lib/FlightTasks/tasks/Manual/FlightTaskManual.hpp @@ -50,7 +50,6 @@ class FlightTaskManual : public FlightTask virtual ~FlightTaskManual() = default; - bool initializeSubscriptions(SubscriptionArray &subscription_array) override; bool applyCommandParameters(const vehicle_command_s &command) override { return FlightTask::applyCommandParameters(command); }; bool updateInitialize() override; @@ -68,7 +67,7 @@ class FlightTaskManual : public FlightTask bool _evaluateSticks(); /**< checks and sets stick inputs */ void _applyGearSwitch(uint8_t gswitch); /**< Sets gears according to switch */ - uORB::SubscriptionPollable *_sub_manual_control_setpoint{nullptr}; + uORB::SubscriptionData _sub_manual_control_setpoint{ORB_ID(manual_control_setpoint)}; DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, (ParamFloat) _param_mpc_hold_dz, /**< 0-deadzone around the center for the sticks */ diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 8ee135ba3fde..3e13f0debad4 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -41,22 +41,12 @@ using namespace matrix; -bool FlightTaskManualAltitude::initializeSubscriptions(SubscriptionArray &subscription_array) -{ - if (!FlightTaskManual::initializeSubscriptions(subscription_array)) { - return false; - } - - if (!subscription_array.get(ORB_ID(home_position), _sub_home_position)) { - return false; - } - - return true; -} - bool FlightTaskManualAltitude::updateInitialize() { bool ret = FlightTaskManual::updateInitialize(); + + _sub_home_position.update(); + // in addition to manual require valid position and velocity in D-direction and valid yaw return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)) && PX4_ISFINITE(_yaw); } @@ -73,15 +63,15 @@ bool FlightTaskManualAltitude::activate(vehicle_local_position_setpoint_s last_s _constraints.tilt = math::radians(_param_mpc_man_tilt_max.get()); - if (PX4_ISFINITE(_sub_vehicle_local_position->get().hagl_min)) { - _constraints.min_distance_to_ground = _sub_vehicle_local_position->get().hagl_min; + if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_min)) { + _constraints.min_distance_to_ground = _sub_vehicle_local_position.get().hagl_min; } else { _constraints.min_distance_to_ground = -INFINITY; } - if (PX4_ISFINITE(_sub_vehicle_local_position->get().hagl_max)) { - _constraints.max_distance_to_ground = _sub_vehicle_local_position->get().hagl_max; + if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_max)) { + _constraints.max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max; } else { _constraints.max_distance_to_ground = INFINITY; @@ -182,9 +172,9 @@ void FlightTaskManualAltitude::_updateAltitudeLock() } else if (PX4_ISFINITE(_position_setpoint(2)) && apply_brake) { // Position is locked but check if a reset event has happened. // We will shift the setpoints. - if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counter) { + if (_sub_vehicle_local_position.get().z_reset_counter != _reset_counter) { _position_setpoint(2) = _position(2); - _reset_counter = _sub_vehicle_local_position->get().z_reset_counter; + _reset_counter = _sub_vehicle_local_position.get().z_reset_counter; } } else { @@ -273,8 +263,8 @@ void FlightTaskManualAltitude::_respectGroundSlowdown() if (PX4_ISFINITE(_dist_to_bottom)) { dist_to_ground = _dist_to_bottom; - } else if (_sub_home_position->get().valid_alt) { - dist_to_ground = -(_position(2) - _sub_home_position->get().z); + } else if (_sub_home_position.get().valid_alt) { + dist_to_ground = -(_position(2) - _sub_home_position.get().z); } // limit speed gradually within the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2 diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index faddb0dde008..e77687fb964d 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -47,7 +47,6 @@ class FlightTaskManualAltitude : public FlightTaskManual public: FlightTaskManualAltitude() = default; virtual ~FlightTaskManualAltitude() = default; - bool initializeSubscriptions(SubscriptionArray &subscription_array) override; bool activate(vehicle_local_position_setpoint_s last_setpoint) override; bool updateInitialize() override; bool update() override; @@ -111,7 +110,7 @@ class FlightTaskManualAltitude : public FlightTaskManual */ void _respectGroundSlowdown(); - uORB::SubscriptionPollable *_sub_home_position{nullptr}; + uORB::SubscriptionData _sub_home_position{ORB_ID(home_position)}; uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */ float _max_speed_up = 10.0f; diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index b2e1d6f41507..8a51fe29e98e 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -46,15 +46,6 @@ FlightTaskManualPosition::FlightTaskManualPosition() : _collision_prevention(thi } -bool FlightTaskManualPosition::initializeSubscriptions(SubscriptionArray &subscription_array) -{ - if (!FlightTaskManualAltitude::initializeSubscriptions(subscription_array)) { - return false; - } - - return true; -} - bool FlightTaskManualPosition::updateInitialize() { bool ret = FlightTaskManualAltitude::updateInitialize(); @@ -102,11 +93,11 @@ void FlightTaskManualPosition::_scaleSticks() } // scale the stick inputs - if (PX4_ISFINITE(_sub_vehicle_local_position->get().vxy_max)) { + if (PX4_ISFINITE(_sub_vehicle_local_position.get().vxy_max)) { // estimator provides vehicle specific max // use the minimum of the estimator and user specified limit - _velocity_scale = fminf(_constraints.speed_xy, _sub_vehicle_local_position->get().vxy_max); + _velocity_scale = fminf(_constraints.speed_xy, _sub_vehicle_local_position.get().vxy_max); // Allow for a minimum of 0.3 m/s for repositioning _velocity_scale = fmaxf(_velocity_scale, 0.3f); @@ -152,10 +143,10 @@ void FlightTaskManualPosition::_updateXYlock() } else if (PX4_ISFINITE(_position_setpoint(0)) && apply_brake) { // Position is locked but check if a reset event has happened. // We will shift the setpoints. - if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counter) { + if (_sub_vehicle_local_position.get().xy_reset_counter != _reset_counter) { _position_setpoint(0) = _position(0); _position_setpoint(1) = _position(1); - _reset_counter = _sub_vehicle_local_position->get().xy_reset_counter; + _reset_counter = _sub_vehicle_local_position.get().xy_reset_counter; } } else { diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp index 505512faa076..8b051fc5177a 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -49,7 +49,6 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude FlightTaskManualPosition(); virtual ~FlightTaskManualPosition() = default; - bool initializeSubscriptions(SubscriptionArray &subscription_array) override; bool activate(vehicle_local_position_setpoint_s last_setpoint) override; bool updateInitialize() override; diff --git a/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp b/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp index a67d47f714b8..bd1fbc0c7eff 100644 --- a/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp +++ b/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp @@ -39,24 +39,15 @@ using namespace matrix; -bool FlightTaskOffboard::initializeSubscriptions(SubscriptionArray &subscription_array) -{ - if (!FlightTask::initializeSubscriptions(subscription_array)) { - return false; - } - - if (!subscription_array.get(ORB_ID(position_setpoint_triplet), _sub_triplet_setpoint)) { - return false; - } - - return true; -} - bool FlightTaskOffboard::updateInitialize() { bool ret = FlightTask::updateInitialize(); + + _sub_triplet_setpoint.update(); + // require a valid triplet - ret = ret && _sub_triplet_setpoint->get().current.valid; + ret = ret && _sub_triplet_setpoint.get().current.valid; + // require valid position / velocity in xy return ret && PX4_ISFINITE(_position(0)) && PX4_ISFINITE(_position(1)) @@ -78,32 +69,32 @@ bool FlightTaskOffboard::update() // reset setpoint for every loop _resetSetpoints(); - if (!_sub_triplet_setpoint->get().current.valid) { + if (!_sub_triplet_setpoint.get().current.valid) { _setDefaultConstraints(); _position_setpoint = _position; return false; } // Yaw / Yaw-speed - if (_sub_triplet_setpoint->get().current.yaw_valid) { + if (_sub_triplet_setpoint.get().current.yaw_valid) { // yaw control required - _yaw_setpoint = _sub_triplet_setpoint->get().current.yaw; + _yaw_setpoint = _sub_triplet_setpoint.get().current.yaw; - if (_sub_triplet_setpoint->get().current.yawspeed_valid) { + if (_sub_triplet_setpoint.get().current.yawspeed_valid) { // yawspeed is used as feedforward - _yawspeed_setpoint = _sub_triplet_setpoint->get().current.yawspeed; + _yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed; } - } else if (_sub_triplet_setpoint->get().current.yawspeed_valid) { + } else if (_sub_triplet_setpoint.get().current.yawspeed_valid) { // only yawspeed required - _yawspeed_setpoint = _sub_triplet_setpoint->get().current.yawspeed; + _yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed; // set yaw setpoint to NAN since not used _yaw_setpoint = NAN; } // Loiter - if (_sub_triplet_setpoint->get().current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { + if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { // loiter just means that the vehicle should keep position if (!PX4_ISFINITE(_position_lock(0))) { _position_setpoint = _position_lock = _position; @@ -120,7 +111,7 @@ bool FlightTaskOffboard::update() } // Takeoff - if (_sub_triplet_setpoint->get().current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { // just do takeoff to default altitude if (!PX4_ISFINITE(_position_lock(0))) { _position_setpoint = _position_lock = _position; @@ -138,7 +129,7 @@ bool FlightTaskOffboard::update() } // Land - if (_sub_triplet_setpoint->get().current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { // land with landing speed, but keep position in xy if (!PX4_ISFINITE(_position_lock(0))) { _position_setpoint = _position_lock = _position; @@ -158,7 +149,7 @@ bool FlightTaskOffboard::update() } // IDLE - if (_sub_triplet_setpoint->get().current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _thrust_setpoint.zero(); return true; } @@ -168,17 +159,17 @@ bool FlightTaskOffboard::update() // 2. position setpoint + velocity setpoint (velocity used as feedforward) // 3. velocity setpoint // 4. acceleration setpoint -> this will be mapped to normalized thrust setpoint because acceleration is not supported - const bool position_ctrl_xy = _sub_triplet_setpoint->get().current.position_valid - && _sub_vehicle_local_position->get().xy_valid; - const bool position_ctrl_z = _sub_triplet_setpoint->get().current.alt_valid - && _sub_vehicle_local_position->get().z_valid; - const bool velocity_ctrl_xy = _sub_triplet_setpoint->get().current.velocity_valid - && _sub_vehicle_local_position->get().v_xy_valid; - const bool velocity_ctrl_z = _sub_triplet_setpoint->get().current.velocity_valid - && _sub_vehicle_local_position->get().v_z_valid; + const bool position_ctrl_xy = _sub_triplet_setpoint.get().current.position_valid + && _sub_vehicle_local_position.get().xy_valid; + const bool position_ctrl_z = _sub_triplet_setpoint.get().current.alt_valid + && _sub_vehicle_local_position.get().z_valid; + const bool velocity_ctrl_xy = _sub_triplet_setpoint.get().current.velocity_valid + && _sub_vehicle_local_position.get().v_xy_valid; + const bool velocity_ctrl_z = _sub_triplet_setpoint.get().current.velocity_valid + && _sub_vehicle_local_position.get().v_z_valid; const bool feedforward_ctrl_xy = position_ctrl_xy && velocity_ctrl_xy; const bool feedforward_ctrl_z = position_ctrl_z && velocity_ctrl_z; - const bool acceleration_ctrl = _sub_triplet_setpoint->get().current.acceleration_valid; + const bool acceleration_ctrl = _sub_triplet_setpoint.get().current.acceleration_valid; // if nothing is valid in xy, then exit offboard if (!(position_ctrl_xy || velocity_ctrl_xy || acceleration_ctrl)) { @@ -192,29 +183,29 @@ bool FlightTaskOffboard::update() // XY-direction if (feedforward_ctrl_xy) { - _position_setpoint(0) = _sub_triplet_setpoint->get().current.x; - _position_setpoint(1) = _sub_triplet_setpoint->get().current.y; - _velocity_setpoint(0) = _sub_triplet_setpoint->get().current.vx; - _velocity_setpoint(1) = _sub_triplet_setpoint->get().current.vy; + _position_setpoint(0) = _sub_triplet_setpoint.get().current.x; + _position_setpoint(1) = _sub_triplet_setpoint.get().current.y; + _velocity_setpoint(0) = _sub_triplet_setpoint.get().current.vx; + _velocity_setpoint(1) = _sub_triplet_setpoint.get().current.vy; } else if (position_ctrl_xy) { - _position_setpoint(0) = _sub_triplet_setpoint->get().current.x; - _position_setpoint(1) = _sub_triplet_setpoint->get().current.y; + _position_setpoint(0) = _sub_triplet_setpoint.get().current.x; + _position_setpoint(1) = _sub_triplet_setpoint.get().current.y; } else if (velocity_ctrl_xy) { - if (_sub_triplet_setpoint->get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_LOCAL_NED) { + if (_sub_triplet_setpoint.get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_LOCAL_NED) { // in local frame: don't require any transformation - _velocity_setpoint(0) = _sub_triplet_setpoint->get().current.vx; - _velocity_setpoint(1) = _sub_triplet_setpoint->get().current.vy; + _velocity_setpoint(0) = _sub_triplet_setpoint.get().current.vx; + _velocity_setpoint(1) = _sub_triplet_setpoint.get().current.vy; - } else if (_sub_triplet_setpoint->get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_BODY_NED) { + } else if (_sub_triplet_setpoint.get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_BODY_NED) { // in body frame: need to transorm first // Note, this transformation is wrong because body-xy is not neccessarily on the same plane as locale-xy - _velocity_setpoint(0) = cosf(_yaw) * _sub_triplet_setpoint->get().current.vx - sinf( - _yaw) * _sub_triplet_setpoint->get().current.vy; - _velocity_setpoint(1) = sinf(_yaw) * _sub_triplet_setpoint->get().current.vx + cosf( - _yaw) * _sub_triplet_setpoint->get().current.vy; + _velocity_setpoint(0) = cosf(_yaw) * _sub_triplet_setpoint.get().current.vx - sinf( + _yaw) * _sub_triplet_setpoint.get().current.vy; + _velocity_setpoint(1) = sinf(_yaw) * _sub_triplet_setpoint.get().current.vx + cosf( + _yaw) * _sub_triplet_setpoint.get().current.vy; } else { // no valid frame @@ -224,22 +215,22 @@ bool FlightTaskOffboard::update() // Z-direction if (feedforward_ctrl_z) { - _position_setpoint(2) = _sub_triplet_setpoint->get().current.z; - _velocity_setpoint(2) = _sub_triplet_setpoint->get().current.vz; + _position_setpoint(2) = _sub_triplet_setpoint.get().current.z; + _velocity_setpoint(2) = _sub_triplet_setpoint.get().current.vz; } else if (position_ctrl_z) { - _position_setpoint(2) = _sub_triplet_setpoint->get().current.z; + _position_setpoint(2) = _sub_triplet_setpoint.get().current.z; } else if (velocity_ctrl_z) { - _velocity_setpoint(2) = _sub_triplet_setpoint->get().current.vz; + _velocity_setpoint(2) = _sub_triplet_setpoint.get().current.vz; } // Acceleration // Note: this is not supported yet and will be mapped to normalized thrust directly. - if (_sub_triplet_setpoint->get().current.acceleration_valid) { - _thrust_setpoint(0) = _sub_triplet_setpoint->get().current.a_x; - _thrust_setpoint(1) = _sub_triplet_setpoint->get().current.a_y; - _thrust_setpoint(2) = _sub_triplet_setpoint->get().current.a_z; + if (_sub_triplet_setpoint.get().current.acceleration_valid) { + _thrust_setpoint(0) = _sub_triplet_setpoint.get().current.a_x; + _thrust_setpoint(1) = _sub_triplet_setpoint.get().current.a_y; + _thrust_setpoint(2) = _sub_triplet_setpoint.get().current.a_z; } // use default conditions of upwards position or velocity to take off diff --git a/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.hpp b/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.hpp index caf0d59e219d..605be2a31358 100644 --- a/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.hpp +++ b/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.hpp @@ -47,13 +47,13 @@ class FlightTaskOffboard : public FlightTask FlightTaskOffboard() = default; virtual ~FlightTaskOffboard() = default; - bool initializeSubscriptions(SubscriptionArray &subscription_array) override; bool update() override; bool activate(vehicle_local_position_setpoint_s last_setpoint) override; bool updateInitialize() override; protected: - uORB::SubscriptionPollable *_sub_triplet_setpoint{nullptr}; + uORB::SubscriptionData _sub_triplet_setpoint{ORB_ID(position_setpoint_triplet)}; + private: matrix::Vector3f _position_lock{}; diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp index eafcd6f6ed9b..3d34199e840a 100644 --- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp +++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp @@ -56,32 +56,21 @@ ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) : } -bool ObstacleAvoidance::initializeSubscriptions(SubscriptionArray &subscription_array) -{ - if (!subscription_array.get(ORB_ID(vehicle_trajectory_waypoint), _sub_vehicle_trajectory_waypoint)) { - return false; - } - - if (!subscription_array.get(ORB_ID(vehicle_status), _sub_vehicle_status)) { - return false; - } - - return true; -} - void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, float &yaw_sp, float &yaw_speed_sp) { + _sub_vehicle_status.update(); + _sub_vehicle_trajectory_waypoint.update(); - if (_sub_vehicle_status->get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) { + if (_sub_vehicle_status.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) { // if in failsafe nav_state LOITER, don't inject setpoints from avoidance system return; } - const bool avoidance_data_timeout = hrt_elapsed_time((hrt_abstime *)&_sub_vehicle_trajectory_waypoint->get().timestamp) + const bool avoidance_data_timeout = hrt_elapsed_time((hrt_abstime *)&_sub_vehicle_trajectory_waypoint.get().timestamp) > TRAJECTORY_STREAM_TIMEOUT_US; const bool avoidance_point_valid = - _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid == true; + _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid == true; _avoidance_point_not_valid_hysteresis.set_state_and_update(!avoidance_point_valid, hrt_absolute_time()); @@ -106,13 +95,13 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel } if (avoidance_point_valid) { - pos_sp = _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position; - vel_sp = _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity; + pos_sp = _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position; + vel_sp = _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity; if (!_ext_yaw_active) { // inject yaw setpoints only if weathervane isn't active - yaw_sp = _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw; - yaw_speed_sp = _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed; + yaw_sp = _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw; + yaw_speed_sp = _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed; } } diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp index 241575750bd2..a1b224c16856 100644 --- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp +++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp @@ -48,6 +48,7 @@ #include #include +#include #include #include #include @@ -58,8 +59,6 @@ #include -#include - const vehicle_trajectory_waypoint_s empty_trajectory_waypoint = {0, 0, {0, 0, 0, 0, 0, 0, 0}, { {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, @@ -75,8 +74,6 @@ class ObstacleAvoidance : public ModuleParams ObstacleAvoidance(ModuleParams *parent); ~ObstacleAvoidance() = default; - bool initializeSubscriptions(SubscriptionArray &subscription_array); - /** * Inject setpoints from obstacle avoidance system into FlightTasks. * @param pos_sp, position setpoint @@ -119,8 +116,8 @@ class ObstacleAvoidance : public ModuleParams protected: - uORB::SubscriptionPollable *_sub_vehicle_trajectory_waypoint{nullptr}; /**< vehicle trajectory waypoint subscription */ - uORB::SubscriptionPollable *_sub_vehicle_status{nullptr}; /**< vehicle status subscription */ + uORB::SubscriptionData _sub_vehicle_trajectory_waypoint{ORB_ID(vehicle_trajectory_waypoint)}; /**< vehicle trajectory waypoint subscription */ + uORB::SubscriptionData _sub_vehicle_status{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ vehicle_trajectory_waypoint_s _desired_waypoint{}; /**< desired vehicle trajectory waypoint to be sent to OA */ diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidanceTest.cpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidanceTest.cpp index 9f25375ec9ae..2389ac7233fb 100644 --- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidanceTest.cpp +++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidanceTest.cpp @@ -42,7 +42,6 @@ using namespace matrix; class ObstacleAvoidanceTest : public ::testing::Test { public: - SubscriptionArray subscription_array; Vector3f pos_sp; Vector3f vel_sp; float yaw_sp = 0.123f; @@ -71,7 +70,6 @@ TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy) // GIVEN: the flight controller setpoints from FlightTaskAutoMapper and a vehicle_trajectory_waypoint message coming // from offboard TestObstacleAvoidance oa; - oa.initializeSubscriptions(subscription_array); vehicle_trajectory_waypoint_s message = empty_trajectory_waypoint; message.timestamp = hrt_absolute_time(); @@ -91,8 +89,6 @@ TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy) orb_advert_t vehicle_status_pub = orb_advertise(ORB_ID(vehicle_status), &vehicle_status); orb_publish(ORB_ID(vehicle_status), vehicle_status_pub, &vehicle_status); - subscription_array.update(); - // WHEN: we inject the vehicle_trajectory_waypoint in the interface oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp); @@ -109,7 +105,6 @@ TEST_F(ObstacleAvoidanceTest, oa_enabled_not_healthy) { // GIVEN: the flight controller setpoints from FlightTaskAutoMapper and a vehicle_trajectory_waypoint message TestObstacleAvoidance oa; - oa.initializeSubscriptions(subscription_array); vehicle_trajectory_waypoint_s message = empty_trajectory_waypoint; Vector3f pos(3.1f, 4.7f, 5.2f); @@ -124,8 +119,6 @@ TEST_F(ObstacleAvoidanceTest, oa_enabled_not_healthy) orb_advert_t vehicle_status_pub = orb_advertise(ORB_ID(vehicle_status), &vehicle_status); orb_publish(ORB_ID(vehicle_status), vehicle_status_pub, &vehicle_status); - subscription_array.update(); - // WHEN: we inject the vehicle_trajectory_waypoint in the interface oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp); @@ -142,7 +135,6 @@ TEST_F(ObstacleAvoidanceTest, oa_desired) { // GIVEN: the flight controller setpoints from FlightTaskAutoMapper and the waypoints from FLightTaskAuto TestObstacleAvoidance oa; - oa.initializeSubscriptions(subscription_array); pos_sp = Vector3f(1.f, 1.2f, NAN); vel_sp = Vector3f(NAN, NAN, 0.7f); From 757ed6de8d2da98abc3ad07207b0bd5c0a990dfe Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 30 Sep 2019 09:20:00 -0400 Subject: [PATCH 2/2] fix FlightTaskError numbering --- src/lib/FlightTasks/FlightTasks.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index 64eedaa6b947..4d7948db3df6 100644 --- a/src/lib/FlightTasks/FlightTasks.hpp +++ b/src/lib/FlightTasks/FlightTasks.hpp @@ -54,7 +54,7 @@ enum class FlightTaskError : int { NoError = 0, InvalidTask = -1, - ActivationFailed = -3 + ActivationFailed = -2 }; class FlightTasks