Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[WIP]: FlightTasks move to new uORB::Subscription and delete SubscriptionArray #12212

Closed
wants to merge 1 commit into from
Closed

[WIP]: FlightTasks move to new uORB::Subscription and delete SubscriptionArray #12212

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 0 additions & 11 deletions src/lib/FlightTasks/FlightTasks.cpp
View file
Open in desktop
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 @@ -74,16 +73,6 @@ int FlightTasks::switchTask(FlightTaskIndex new_task_index)
return 0;
}

// subscription failed
if (!_current_task.task->initializeSubscriptions(_subscription_array)) {
_current_task.task->~FlightTask();
_current_task.task = nullptr;
_current_task.index = FlightTaskIndex::None;
return -2;
}

_subscription_array.forcedUpdate(); // make sure data is available for all new subscriptions

// activation failed
if (!_current_task.task->updateInitialize() || !_current_task.task->activate()) {
_current_task.task->~FlightTask();
Expand Down
3 changes: 0 additions & 3 deletions src/lib/FlightTasks/FlightTasks.hpp
View file
Open in desktop
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>

Expand Down Expand Up @@ -158,8 +157,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
127 changes: 57 additions & 70 deletions src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,31 +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 (!_obstacle_avoidance.initializeSubscriptions(subscription_array)) {
return false;
}

return true;
}

bool FlightTaskAuto::activate()
{
bool ret = FlightTask::activate();
Expand Down Expand Up @@ -144,18 +119,27 @@ bool FlightTaskAuto::_evaluateTriplets()
// takeoff/land was initiated. Until then we do this kind of logic here.

// Check if triplet is valid. There must be at least a valid altitude.
position_setpoint_triplet_s triplet_setpoint;

if (!_sub_triplet_setpoint->get().current.valid || !PX4_ISFINITE(_sub_triplet_setpoint->get().current.alt)) {
if (!_sub_triplet_setpoint.update(&triplet_setpoint)) {
return false;
}

_current_velocity_valid = triplet_setpoint.current.velocity_valid;
_current_vx = triplet_setpoint.current.vx;
_current_vy = triplet_setpoint.current.vy;

if (!triplet_setpoint.current.valid || !PX4_ISFINITE(triplet_setpoint.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)triplet_setpoint.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 = triplet_setpoint.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 @@ -168,8 +152,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(triplet_setpoint.current.lat) || !PX4_ISFINITE(triplet_setpoint.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 @@ -186,10 +170,11 @@ 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));
triplet_setpoint.current.lat, triplet_setpoint.current.lon,
&tmp_target(0), &tmp_target(1));
}

tmp_target(2) = -(_sub_triplet_setpoint->get().current.alt - _reference_altitude);
tmp_target(2) = -(triplet_setpoint.current.alt - _reference_altitude);

// Check if anything has changed. We do that by comparing the temporary target
// to the internal _triplet_target.
Expand All @@ -208,7 +193,7 @@ bool FlightTaskAuto::_evaluateTriplets()

} else {
_triplet_target = tmp_target;
_target_acceptance_radius = _sub_triplet_setpoint->get().current.acceptance_radius;
_target_acceptance_radius = triplet_setpoint.current.acceptance_radius;

if (!PX4_ISFINITE(_triplet_target(0)) || !PX4_ISFINITE(_triplet_target(1))) {
// Horizontal target is not finite.
Expand All @@ -223,10 +208,11 @@ 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(triplet_setpoint.previous) && triplet_setpoint.previous.valid) {
map_projection_project(&_reference_position,
triplet_setpoint.previous.lat, triplet_setpoint.previous.lon,
&_triplet_prev_wp(0), &_triplet_prev_wp(1));
_triplet_prev_wp(2) = -(triplet_setpoint.previous.alt - _reference_altitude);

} else {
_triplet_prev_wp = _position;
Expand All @@ -235,10 +221,11 @@ 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(triplet_setpoint.next) && triplet_setpoint.next.valid) {
map_projection_project(&_reference_position,
triplet_setpoint.next.lat, triplet_setpoint.next.lon,
&_triplet_next_wp(0), &_triplet_next_wp(1));
_triplet_next_wp(2) = -(triplet_setpoint.next.alt - _reference_altitude);

} else {
_triplet_next_wp = _triplet_target;
Expand All @@ -250,13 +237,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 && triplet_setpoint.current.yawspeed_valid) {
_yawspeed_setpoint = triplet_setpoint.current.yawspeed;
_yaw_setpoint = NAN;

} else {
if (_sub_triplet_setpoint->get().current.yaw_valid) {
_yaw_setpoint = _sub_triplet_setpoint->get().current.yaw;
if (triplet_setpoint.current.yaw_valid) {
_yaw_setpoint = triplet_setpoint.current.yaw;

} else {
_set_heading_from_mode();
Expand All @@ -271,25 +258,25 @@ bool FlightTaskAuto::_evaluateTriplets()

if (triplet_update || (_current_state != previous_state)) {
_updateInternalWaypoints();
_mission_gear = _sub_triplet_setpoint->get().current.landing_gear;
_mission_gear = triplet_setpoint.current.landing_gear;
}

if (_param_com_obs_avoid.get() && _sub_vehicle_status->get().is_rotary_wing) {
if (_param_com_obs_avoid.get() && _sub_vehicle_status.get().is_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,
triplet_setpoint.next.yaw,
triplet_setpoint.next.yawspeed_valid ? triplet_setpoint.next.yawspeed : NAN,
_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active());
_obstacle_avoidance.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt,
_sub_triplet_setpoint->get().current.type);
triplet_setpoint.current.type);
}

return true;
}

void FlightTaskAuto::_set_heading_from_mode()
{

_sub_home_position.update();
Vector2f v; // Vector that points towards desired location

switch (_param_mpc_yaw_mode.get()) {
Expand All @@ -299,15 +286,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 @@ -351,37 +338,38 @@ 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)) {
_sub_vehicle_local_position.update();

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;
_sub_vehicle_local_position.update();
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;
ref_lon = 0.0;
}

// init projection
map_projection_init(&_reference_position,
ref_lat,
ref_lon);
map_projection_init(&_reference_position, ref_lat, ref_lon);

// 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 @@ -403,10 +391,9 @@ 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;
bool velocity_valid = PX4_ISFINITE(vx) && PX4_ISFINITE(vy) &&
_sub_triplet_setpoint->get().current.velocity_valid;
const float vx = _current_vx;
const float vy = _current_vy;
bool velocity_valid = PX4_ISFINITE(vx) && PX4_ISFINITE(vy) && _current_velocity_valid;

if (velocity_valid) {
return Vector2f(vx, vy);
Expand Down
11 changes: 7 additions & 4 deletions src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,6 @@ class FlightTaskAuto : public FlightTask
FlightTaskAuto();

virtual ~FlightTaskAuto() = default;
bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
bool activate() override;
bool updateInitialize() override;
bool updateFinalize() override;
Expand All @@ -98,14 +97,18 @@ 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::SubscriptionData<home_position_s> _sub_home_position{ORB_ID(home_position)};

State _current_state{State::none};
float _target_acceptance_radius = 0.0f; /**< Acceptances radius of the target */
int _mission_gear = landing_gear_s::GEAR_KEEP;

float _yaw_sp_prev = NAN;

bool _current_velocity_valid{false};
float _current_vx{0.0f};
float _current_vy{0.0f};

ObstacleAvoidance _obstacle_avoidance; /**< class adjusting setpoints according to external avoidance module's input */

DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
Expand All @@ -121,8 +124,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::SubscriptionPollable<vehicle_status_s> *_sub_vehicle_status{nullptr};
uORB::Subscription _sub_triplet_setpoint{ORB_ID(position_setpoint_triplet)};
uORB::SubscriptionData<vehicle_status_s> _sub_vehicle_status{ORB_ID(vehicle_status)};

matrix::Vector3f
_triplet_target; /**< current triplet from navigator which may differ from the intenal one (_target) depending on the vehicle state. */
Expand Down
22 changes: 14 additions & 8 deletions src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -113,26 +113,32 @@ inline float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float con
void FlightTaskAutoLineSmoothVel::_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.update()) {
return;
}

const vehicle_local_position_s &lpos = _sub_vehicle_local_position.get();

if (lpos.xy_reset_counter != _reset_counters.xy) {
_trajectory[0].setCurrentPosition(_position(0));
_trajectory[1].setCurrentPosition(_position(1));
_reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter;
_reset_counters.xy = lpos.xy_reset_counter;
}

if (_sub_vehicle_local_position->get().vxy_reset_counter != _reset_counters.vxy) {
if (lpos.vxy_reset_counter != _reset_counters.vxy) {
_trajectory[0].setCurrentVelocity(_velocity(0));
_trajectory[1].setCurrentVelocity(_velocity(1));
_reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter;
_reset_counters.vxy = lpos.vxy_reset_counter;
}

if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counters.z) {
if (lpos.z_reset_counter != _reset_counters.z) {
_trajectory[2].setCurrentPosition(_position(2));
_reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter;
_reset_counters.z = lpos.z_reset_counter;
}

if (_sub_vehicle_local_position->get().vz_reset_counter != _reset_counters.vz) {
if (lpos.vz_reset_counter != _reset_counters.vz) {
_trajectory[2].setCurrentVelocity(_velocity(2));
_reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter;
_reset_counters.vz = lpos.vz_reset_counter;
}
}

Expand Down
Loading