mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
FlightTasks: switch to uORB::Subscription
- delete SubscriptionArray - all FlightTasks are now responsible for updating their own subscriptions (typically in updateInitialize()).
This commit is contained in:
Daniel Agar
GitHub
parent
ad728cc0c7
commit
045f6233d4
@ -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();
|
||||
|
||||
@ -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>
|
||||
@ -55,8 +54,7 @@
|
||||
enum class FlightTaskError : int {
|
||||
NoError = 0,
|
||||
InvalidTask = -1,
|
||||
SubscriptionFailed = -2,
|
||||
ActivationFailed = -3
|
||||
ActivationFailed = -2
|
||||
};
|
||||
|
||||
class FlightTasks
|
||||
@ -169,23 +167,21 @@ private:
|
||||
};
|
||||
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<int>(FlightTaskError::NoError), "No Error"},
|
||||
{static_cast<int>(FlightTaskError::InvalidTask), "Invalid Task "},
|
||||
{static_cast<int>(FlightTaskError::SubscriptionFailed), "Subscription Failed"},
|
||||
{static_cast<int>(FlightTaskError::ActivationFailed), "Activation Failed"}
|
||||
};
|
||||
|
||||
/**
|
||||
* Check for vehicle commands (received via MAVLink), evaluate and acknowledge them
|
||||
*/
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -77,7 +77,6 @@ public:
|
||||
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 @@ protected:
|
||||
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 */
|
||||
@ -127,7 +127,8 @@ protected:
|
||||
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. */
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -33,7 +33,6 @@
|
||||
|
||||
px4_add_library(FlightTask
|
||||
FlightTask.cpp
|
||||
SubscriptionArray.cpp
|
||||
)
|
||||
|
||||
target_include_directories(FlightTask PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -44,7 +44,7 @@
|
||||
#include <px4_module_params.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <matrix/matrix/math.hpp>
|
||||
#include <uORB/SubscriptionPollable.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/landing_gear.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_local_position_setpoint.h>
|
||||
@ -53,7 +53,6 @@
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_trajectory_waypoint.h>
|
||||
#include <lib/WeatherVane/WeatherVane.hpp>
|
||||
#include "SubscriptionArray.hpp"
|
||||
|
||||
class FlightTask : public ModuleParams
|
||||
{
|
||||
@ -67,13 +66,6 @@ public:
|
||||
|
||||
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 @@ public:
|
||||
|
||||
protected:
|
||||
|
||||
uORB::SubscriptionPollable<vehicle_local_position_s> *_sub_vehicle_local_position{nullptr};
|
||||
uORB::SubscriptionPollable<vehicle_attitude_s> *_sub_attitude{nullptr};
|
||||
uORB::SubscriptionData<vehicle_local_position_s> _sub_vehicle_local_position{ORB_ID(vehicle_local_position)};
|
||||
uORB::SubscriptionData<vehicle_attitude_s> _sub_attitude{ORB_ID(vehicle_attitude)};
|
||||
|
||||
/** Reset all setpoints to NAN */
|
||||
void _resetSetpoints();
|
||||
|
||||
@ -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 <string.h>
|
||||
|
||||
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();
|
||||
}
|
||||
}
|
||||
@ -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<T> instances
|
||||
*
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <uORB/SubscriptionPollable.hpp>
|
||||
|
||||
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<class T>
|
||||
bool get(const struct orb_metadata *meta, uORB::SubscriptionPollable<T> *&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<class T>
|
||||
bool SubscriptionArray::get(const struct orb_metadata *meta, uORB::SubscriptionPollable<T> *&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<uORB::SubscriptionPollable<T>*>(_subscriptions[i]);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
// resize if needed
|
||||
if (_subscriptions_count >= _subscriptions_size) {
|
||||
if (!resizeSubscriptions()) {
|
||||
subscription = nullptr;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
subscription = new uORB::SubscriptionPollable<T>(meta, 0, instance);
|
||||
|
||||
if (!subscription) {
|
||||
return false;
|
||||
}
|
||||
|
||||
_subscriptions[_subscriptions_count++] = subscription;
|
||||
return true;
|
||||
}
|
||||
@ -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);
|
||||
|
||||
@ -50,7 +50,6 @@ public:
|
||||
|
||||
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 @@ private:
|
||||
bool _evaluateSticks(); /**< checks and sets stick inputs */
|
||||
void _applyGearSwitch(uint8_t gswitch); /**< Sets gears according to switch */
|
||||
|
||||
uORB::SubscriptionPollable<manual_control_setpoint_s> *_sub_manual_control_setpoint{nullptr};
|
||||
uORB::SubscriptionData<manual_control_setpoint_s> _sub_manual_control_setpoint{ORB_ID(manual_control_setpoint)};
|
||||
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
|
||||
(ParamFloat<px4::params::MPC_HOLD_DZ>) _param_mpc_hold_dz, /**< 0-deadzone around the center for the sticks */
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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 @@ private:
|
||||
*/
|
||||
void _respectGroundSlowdown();
|
||||
|
||||
uORB::SubscriptionPollable<home_position_s> *_sub_home_position{nullptr};
|
||||
uORB::SubscriptionData<home_position_s> _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;
|
||||
|
||||
@ -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 {
|
||||
|
||||
@ -49,7 +49,6 @@ public:
|
||||
FlightTaskManualPosition();
|
||||
|
||||
virtual ~FlightTaskManualPosition() = default;
|
||||
bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
|
||||
bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
|
||||
bool updateInitialize() override;
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -47,13 +47,13 @@ public:
|
||||
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<position_setpoint_triplet_s> *_sub_triplet_setpoint{nullptr};
|
||||
uORB::SubscriptionData<position_setpoint_triplet_s> _sub_triplet_setpoint{ORB_ID(position_setpoint_triplet)};
|
||||
|
||||
private:
|
||||
matrix::Vector3f _position_lock{};
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -48,6 +48,7 @@
|
||||
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/PublicationQueued.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/position_controller_status.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
@ -58,8 +59,6 @@
|
||||
|
||||
#include <matrix/matrix/math.hpp>
|
||||
|
||||
#include <SubscriptionArray.hpp>
|
||||
|
||||
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 @@ public:
|
||||
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 @@ public:
|
||||
|
||||
protected:
|
||||
|
||||
uORB::SubscriptionPollable<vehicle_trajectory_waypoint_s> *_sub_vehicle_trajectory_waypoint{nullptr}; /**< vehicle trajectory waypoint subscription */
|
||||
uORB::SubscriptionPollable<vehicle_status_s> *_sub_vehicle_status{nullptr}; /**< vehicle status subscription */
|
||||
uORB::SubscriptionData<vehicle_trajectory_waypoint_s> _sub_vehicle_trajectory_waypoint{ORB_ID(vehicle_trajectory_waypoint)}; /**< vehicle trajectory waypoint subscription */
|
||||
uORB::SubscriptionData<vehicle_status_s> _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 */
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user