Skip to content

Commit

Permalink
FlightTasks: switch to uORB::Subscription
Browse files Browse the repository at this point in the history
  • Loading branch information
@dagar
dagar committed Sep 28, 2019
1 parent fd67bd0 commit 7f563ec
Show file tree
Hide file tree
Showing 22 changed files with 189 additions and 503 deletions.
11 changes: 0 additions & 11 deletions src/lib/FlightTasks/FlightTasks.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}

Expand Down Expand Up @@ -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();
Expand Down
3 changes: 0 additions & 3 deletions src/lib/FlightTasks/FlightTasks.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,6 @@
#pragma once

#include "FlightTask.hpp"
#include "SubscriptionArray.hpp"
#include "FlightTasks_generated.hpp"
#include <lib/WeatherVane/WeatherVane.hpp>
#include <uORB/PublicationQueued.hpp>
Expand Down Expand Up @@ -169,8 +168,6 @@ class FlightTasks
};
flight_task_t _current_task = {nullptr, FlightTaskIndex::None};

SubscriptionArray _subscription_array;

struct task_error_t {
int error;
const char *msg;
Expand Down
117 changes: 47 additions & 70 deletions src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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);
Expand All @@ -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.
Expand All @@ -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.
Expand All @@ -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;
Expand All @@ -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;
Expand All @@ -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();
}

Expand All @@ -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();
Expand All @@ -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);
}

Expand All @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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 {
Expand All @@ -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);
Expand Down
11 changes: 6 additions & 5 deletions src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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<home_position_s> *_sub_home_position{nullptr};
uORB::SubscriptionPollable<manual_control_setpoint_s> *_sub_manual_control_setpoint{nullptr};
uORB::SubscriptionPollable<vehicle_status_s> *_sub_vehicle_status{nullptr};

uORB::SubscriptionData<home_position_s> _sub_home_position{ORB_ID(home_position)};
uORB::SubscriptionData<manual_control_setpoint_s> _sub_manual_control_setpoint{ORB_ID(manual_control_setpoint)};
uORB::SubscriptionData<vehicle_status_s> _sub_vehicle_status{ORB_ID(vehicle_status)};

State _current_state{State::none};
float _target_acceptance_radius{0.0f}; /**< Acceptances radius of the target */
Expand All @@ -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<position_setpoint_triplet_s> *_sub_triplet_setpoint{nullptr};

uORB::SubscriptionData<position_setpoint_triplet_s> _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. */
Expand Down
4 changes: 2 additions & 2 deletions src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
}

Expand Down
8 changes: 4 additions & 4 deletions src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
}

Expand All @@ -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;
Expand Down
1 change: 0 additions & 1 deletion src/lib/FlightTasks/tasks/FlightTask/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,6 @@

px4_add_library(FlightTask
FlightTask.cpp
SubscriptionArray.cpp
)

target_include_directories(FlightTask PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
Loading

0 comments on commit 7f563ec

Please sign in to comment.