PX4 Firmware
PX4 Autopilot Software http://px4.io
|
#include <FlightTaskAuto.hpp>
Public Member Functions | |
FlightTaskAuto () | |
virtual | ~FlightTaskAuto ()=default |
bool | activate (vehicle_local_position_setpoint_s last_setpoint) override |
Call once on the event where you switch to the task. More... | |
bool | updateInitialize () override |
Call before activate() or update() to initialize time and input data. More... | |
bool | updateFinalize () override |
Call after update() to constrain the generated setpoints in order to comply with the constraints of the current mode. More... | |
void | setYawHandler (WeatherVane *ext_yaw_handler) override |
Sets an external yaw handler which can be used to implement a different yaw control strategy. More... | |
Public Member Functions inherited from FlightTask | |
FlightTask () | |
virtual | ~FlightTask ()=default |
virtual void | reActivate () |
Call this to reset an active Flight Task. More... | |
virtual bool | applyCommandParameters (const vehicle_command_s &command) |
To be called to adopt parameters from an arrived vehicle command. More... | |
virtual bool | update ()=0 |
To be called regularly in the control loop cycle to execute the task. More... | |
const vehicle_local_position_setpoint_s | getPositionSetpoint () |
Get the output data. More... | |
const vehicle_constraints_s & | getConstraints () |
Get vehicle constraints. More... | |
const landing_gear_s & | getGear () |
Get landing gear position. More... | |
const vehicle_trajectory_waypoint_s & | getAvoidanceWaypoint () |
Get avoidance desired waypoint. More... | |
void | handleParameterUpdate () |
Call this whenever a parameter update notification is received (parameter_update uORB message) More... | |
void | updateVelocityControllerIO (const matrix::Vector3f &vel_sp, const matrix::Vector3f &thrust_sp) |
Protected Member Functions | |
void | _setDefaultConstraints () override |
Set constraints to default values. More... | |
matrix::Vector2f | _getTargetVelocityXY () |
only used for follow-me and only here because of legacy reason. More... | |
void | _updateInternalWaypoints () |
Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). More... | |
bool | _compute_heading_from_2D_vector (float &heading, matrix::Vector2f v) |
Computes and sets heading a 2D vector. More... | |
DEFINE_PARAMETERS_CUSTOM_PARENT (FlightTask,(ParamFloat< px4::params::MPC_XY_CRUISE >) _param_mpc_xy_cruise,(ParamFloat< px4::params::MPC_CRUISE_90 >) _param_mpc_cruise_90,(ParamFloat< px4::params::NAV_MC_ALT_RAD >) _param_nav_mc_alt_rad,(ParamInt< px4::params::MPC_YAW_MODE >) _param_mpc_yaw_mode,(ParamInt< px4::params::COM_OBS_AVOID >) _param_com_obs_avoid,(ParamFloat< px4::params::MPC_YAWRAUTO_MAX >) _param_mpc_yawrauto_max,(ParamFloat< px4::params::MIS_YAW_ERR >) _param_mis_yaw_err,(ParamBool< px4::params::WV_EN >) _param_wv_en) | |
Protected Member Functions inherited from FlightTask | |
void | _resetSetpoints () |
Reset all setpoints to NAN. More... | |
void | _evaluateVehicleLocalPosition () |
Check and update local position. More... | |
virtual bool | _checkTakeoff () |
Determine when to trigger a takeoff (ignored in flight) More... | |
void | _initEkfResetCounters () |
Monitor the EKF reset counters and call the appropriate handling functions in case of a reset event. More... | |
void | _checkEkfResetCounters () |
virtual void | _ekfResetHandlerPositionXY () |
virtual void | _ekfResetHandlerVelocityXY () |
virtual void | _ekfResetHandlerPositionZ () |
virtual void | _ekfResetHandlerVelocityZ () |
virtual void | _ekfResetHandlerHeading (float delta_psi) |
Private Member Functions | |
void | _limitYawRate () |
Limits the rate of change of the yaw setpoint. More... | |
bool | _evaluateTriplets () |
Checks and sets triplets. More... | |
bool | _isFinite (const position_setpoint_s &sp) |
Checks if all waypoint triplets are finite. More... | |
bool | _evaluateGlobalReference () |
Check is global reference is available. More... | |
State | _getCurrentState () |
Computes the current vehicle state based on the vehicle position and navigator triplets. More... | |
void | _set_heading_from_mode () |
Private Attributes | |
matrix::Vector2f | _lock_position_xy {NAN, NAN} |
if no valid triplet is received, lock positition to current position More... | |
bool | _yaw_lock {false} |
if within acceptance radius, lock yaw to current yaw More... | |
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. More... | |
matrix::Vector3f | _triplet_prev_wp |
previous triplet from navigator which may differ from the intenal one (_prev_wp) depending on the vehicle state. More... | |
matrix::Vector3f | _triplet_next_wp |
next triplet from navigator which may differ from the intenal one (_next_wp) depending on the vehicle state. More... | |
matrix::Vector2f | _closest_pt |
closest point to the vehicle position on the line previous - target More... | |
map_projection_reference_s | _reference_position {} |
Structure used to project lat/lon setpoint into local frame. More... | |
float | _reference_altitude {NAN} |
Altitude relative to ground. More... | |
hrt_abstime | _time_stamp_reference {0} |
time stamp when last reference update occured. More... | |
WeatherVane * | _ext_yaw_handler {nullptr} |
external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind More... | |
Additional Inherited Members | |
Static Public Attributes inherited from FlightTask | |
static const vehicle_local_position_setpoint_s | empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {}} |
Empty setpoint. More... | |
static const vehicle_constraints_s | empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}} |
Empty constraints. More... | |
static const landing_gear_s | empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}} |
default landing gear state More... | |
Static Protected Attributes inherited from FlightTask | |
static constexpr uint64_t | _timeout = 500000 |
maximal time in us before a loop or data times out More... | |
Definition at line 74 of file FlightTaskAuto.hpp.
FlightTaskAuto::FlightTaskAuto | ( | ) |
Definition at line 45 of file FlightTaskAuto.cpp.
|
virtualdefault |
|
protected |
Computes and sets heading a 2D vector.
Definition at line 474 of file FlightTaskAuto.cpp.
References matrix::Vector< Type, M >::length(), matrix::Vector< Type, M >::normalize(), SIGMA_NORM, math::sign(), and matrix::wrap_pi().
Referenced by _set_heading_from_mode().
|
private |
Check is global reference is available.
Definition at line 346 of file FlightTaskAuto.cpp.
References _reference_altitude, _reference_position, FlightTask::_sub_vehicle_local_position, _time_stamp_reference, uORB::SubscriptionData< T >::get(), map_projection_init(), vehicle_local_position_s::ref_alt, vehicle_local_position_s::ref_lat, vehicle_local_position_s::ref_lon, vehicle_local_position_s::ref_timestamp, vehicle_local_position_s::xy_global, and vehicle_local_position_s::z_global.
Referenced by updateInitialize().
|
private |
Checks and sets triplets.
Definition at line 123 of file FlightTaskAuto.cpp.
References _closest_pt, FlightTask::_constraints, _current_state, _ext_yaw_handler, _getCurrentState(), _isFinite(), _lock_position_xy, _mc_cruise_speed, _mission_gear, _obstacle_avoidance, FlightTask::_position, _prev_prev_wp, _reference_altitude, _reference_position, _set_heading_from_mode(), _sub_triplet_setpoint, _sub_vehicle_status, _target_acceptance_radius, _triplet_next_wp, _triplet_prev_wp, _triplet_target, _type, _updateInternalWaypoints(), FlightTask::_yaw, FlightTask::_yaw_setpoint, FlightTask::_yawspeed_setpoint, position_setpoint_s::acceptance_radius, WeatherVane::activate(), position_setpoint_s::allow_weather_vane, position_setpoint_s::alt, ObstacleAvoidance::checkAvoidanceProgress(), position_setpoint_s::cruising_speed, position_setpoint_triplet_s::current, WeatherVane::deactivate(), f(), follow_target, uORB::SubscriptionData< T >::get(), WeatherVane::get_weathervane_yawrate(), WeatherVane::is_active(), position_setpoint_s::landing_gear, position_setpoint_s::lat, loiter, position_setpoint_s::lon, map_projection_project(), math::min(), position_setpoint_triplet_s::next, position, position_setpoint_triplet_s::previous, matrix::Matrix< Type, M, N >::setAll(), vehicle_constraints_s::speed_xy, position_setpoint_s::type, ObstacleAvoidance::updateAvoidanceDesiredWaypoints(), position_setpoint_s::valid, vehicle_status_s::vehicle_type, position_setpoint_s::yaw, position_setpoint_s::yaw_valid, position_setpoint_s::yawspeed, and position_setpoint_s::yawspeed_valid.
Referenced by updateInitialize().
|
private |
Computes the current vehicle state based on the vehicle position and navigator triplets.
Definition at line 408 of file FlightTaskAuto.cpp.
References _closest_pt, _mc_cruise_speed, FlightTask::_position, _triplet_prev_wp, _triplet_target, f(), matrix::Vector< Type, M >::length(), none, offtrack, previous_infront, and target_behind.
Referenced by _evaluateTriplets().
|
protected |
only used for follow-me and only here because of legacy reason.
Definition at line 391 of file FlightTaskAuto.cpp.
References _sub_triplet_setpoint, position_setpoint_triplet_s::current, uORB::SubscriptionData< T >::get(), position_setpoint_s::velocity_valid, position_setpoint_s::vx, and position_setpoint_s::vy.
|
private |
Checks if all waypoint triplets are finite.
Definition at line 341 of file FlightTaskAuto.cpp.
References position_setpoint_s::alt, position_setpoint_s::lat, and position_setpoint_s::lon.
Referenced by _evaluateTriplets().
|
private |
Limits the rate of change of the yaw setpoint.
Definition at line 94 of file FlightTaskAuto.cpp.
References FlightTask::_deltatime, FlightTask::_yaw_setpoint, _yaw_sp_aligned, _yaw_sp_prev, FlightTask::_yawspeed_setpoint, math::constrain(), math::radians(), and matrix::wrap_pi().
Referenced by updateFinalize().
|
private |
Definition at line 287 of file FlightTaskAuto.cpp.
References _compute_heading_from_2D_vector(), FlightTask::_position, _sub_home_position, _target, _target_acceptance_radius, FlightTask::_yaw, _yaw_lock, FlightTask::_yaw_setpoint, uORB::SubscriptionData< T >::get(), matrix::Vector< Type, M >::length(), matrix::Matrix< Type, M, N >::setAll(), home_position_s::valid_hpos, and home_position_s::x.
Referenced by _evaluateTriplets().
|
overrideprotectedvirtual |
Set constraints to default values.
Reimplemented from FlightTask.
Definition at line 381 of file FlightTaskAuto.cpp.
References FlightTask::_constraints, FlightTask::_setDefaultConstraints(), and vehicle_constraints_s::speed_xy.
Referenced by activate().
|
protected |
Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack).
Definition at line 436 of file FlightTaskAuto.cpp.
References _closest_pt, _current_state, _next_wp, FlightTask::_position, _prev_wp, _target, _triplet_next_wp, _triplet_prev_wp, _triplet_target, none, offtrack, previous_infront, and target_behind.
Referenced by _evaluateTriplets().
|
overridevirtual |
Call once on the event where you switch to the task.
state | of the previous task |
Reimplemented from FlightTask.
Reimplemented in FlightTaskAutoLineSmoothVel, FlightTaskAutoMapper, and FlightTaskAutoMapper2.
Definition at line 51 of file FlightTaskAuto.cpp.
References FlightTask::_position, FlightTask::_position_setpoint, _setDefaultConstraints(), FlightTask::_velocity, FlightTask::_velocity_setpoint, FlightTask::_yaw, FlightTask::_yaw_setpoint, _yaw_sp_prev, FlightTask::_yawspeed_setpoint, and FlightTask::activate().
Referenced by FlightTaskAutoMapper::activate(), and FlightTaskAutoMapper2::activate().
|
protected |
|
inlineoverridevirtual |
Sets an external yaw handler which can be used to implement a different yaw control strategy.
Reimplemented from FlightTask.
Definition at line 87 of file FlightTaskAuto.hpp.
|
overridevirtual |
Call after update() to constrain the generated setpoints in order to comply with the constraints of the current mode.
Reimplemented from FlightTask.
Definition at line 84 of file FlightTaskAuto.cpp.
References FlightTask::_checkTakeoff(), FlightTask::_constraints, _limitYawRate(), and vehicle_constraints_s::want_takeoff.
|
overridevirtual |
Call before activate() or update() to initialize time and input data.
Reimplemented from FlightTask.
Definition at line 62 of file FlightTaskAuto.cpp.
References _evaluateGlobalReference(), _evaluateTriplets(), FlightTask::_position, _sub_home_position, _sub_manual_control_setpoint, _sub_triplet_setpoint, _sub_vehicle_status, FlightTask::_velocity, uORB::SubscriptionData< T >::update(), and FlightTask::updateInitialize().
|
private |
closest point to the vehicle position on the line previous - target
Definition at line 139 of file FlightTaskAuto.hpp.
Referenced by _evaluateTriplets(), _getCurrentState(), and _updateInternalWaypoints().
|
protected |
Definition at line 106 of file FlightTaskAuto.hpp.
Referenced by _evaluateTriplets(), and _updateInternalWaypoints().
|
private |
external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind
Definition at line 145 of file FlightTaskAuto.hpp.
Referenced by _evaluateTriplets().
|
private |
if no valid triplet is received, lock positition to current position
Definition at line 128 of file FlightTaskAuto.hpp.
Referenced by _evaluateTriplets().
|
protected |
Requested cruise speed.
If not valid, default cruise speed is used.
Definition at line 99 of file FlightTaskAuto.hpp.
Referenced by _evaluateTriplets(), and _getCurrentState().
|
protected |
Definition at line 108 of file FlightTaskAuto.hpp.
Referenced by _evaluateTriplets().
|
protected |
The next waypoint after target (local frame).
If no next setpoint is available, next is set to target.
Definition at line 98 of file FlightTaskAuto.hpp.
Referenced by _updateInternalWaypoints().
|
protected |
class adjusting setpoints according to external avoidance module's input
Definition at line 113 of file FlightTaskAuto.hpp.
Referenced by _evaluateTriplets().
|
protected |
Pre-previous waypoint (local frame).
This will be used for smoothing trajectories -> not used yet.
Definition at line 95 of file FlightTaskAuto.hpp.
Referenced by _evaluateTriplets().
|
protected |
Previous waypoint (local frame).
If no previous triplet is available, the prev_wp is set to current position.
Definition at line 96 of file FlightTaskAuto.hpp.
Referenced by _updateInternalWaypoints().
|
private |
Altitude relative to ground.
Definition at line 142 of file FlightTaskAuto.hpp.
Referenced by _evaluateGlobalReference(), and _evaluateTriplets().
|
private |
Structure used to project lat/lon setpoint into local frame.
Definition at line 141 of file FlightTaskAuto.hpp.
Referenced by _evaluateGlobalReference(), and _evaluateTriplets().
|
protected |
Definition at line 102 of file FlightTaskAuto.hpp.
Referenced by _set_heading_from_mode(), and updateInitialize().
|
protected |
Definition at line 103 of file FlightTaskAuto.hpp.
Referenced by updateInitialize().
|
private |
Definition at line 131 of file FlightTaskAuto.hpp.
Referenced by _evaluateTriplets(), _getTargetVelocityXY(), and updateInitialize().
|
protected |
Definition at line 104 of file FlightTaskAuto.hpp.
Referenced by _evaluateTriplets(), and updateInitialize().
|
protected |
Target waypoint (local frame).
Definition at line 97 of file FlightTaskAuto.hpp.
Referenced by _set_heading_from_mode(), and _updateInternalWaypoints().
|
protected |
Acceptances radius of the target.
Definition at line 107 of file FlightTaskAuto.hpp.
Referenced by _evaluateTriplets(), and _set_heading_from_mode().
|
private |
time stamp when last reference update occured.
Definition at line 143 of file FlightTaskAuto.hpp.
Referenced by _evaluateGlobalReference().
|
private |
next triplet from navigator which may differ from the intenal one (_next_wp) depending on the vehicle state.
Definition at line 138 of file FlightTaskAuto.hpp.
Referenced by _evaluateTriplets(), and _updateInternalWaypoints().
|
private |
previous triplet from navigator which may differ from the intenal one (_prev_wp) depending on the vehicle state.
Definition at line 136 of file FlightTaskAuto.hpp.
Referenced by _evaluateTriplets(), _getCurrentState(), and _updateInternalWaypoints().
|
private |
current triplet from navigator which may differ from the intenal one (_target) depending on the vehicle state.
Definition at line 134 of file FlightTaskAuto.hpp.
Referenced by _evaluateTriplets(), _getCurrentState(), and _updateInternalWaypoints().
|
protected |
Type of current target triplet.
Definition at line 100 of file FlightTaskAuto.hpp.
Referenced by _evaluateTriplets().
|
private |
if within acceptance radius, lock yaw to current yaw
Definition at line 129 of file FlightTaskAuto.hpp.
Referenced by _set_heading_from_mode().
|
protected |
Definition at line 111 of file FlightTaskAuto.hpp.
Referenced by _limitYawRate().
|
protected |
Definition at line 110 of file FlightTaskAuto.hpp.
Referenced by _limitYawRate(), and activate().