PX4 Firmware
PX4 Autopilot Software http://px4.io
|
#include <FlightTask.hpp>
Public Member Functions | |
FlightTask () | |
virtual | ~FlightTask ()=default |
virtual bool | activate (vehicle_local_position_setpoint_s last_setpoint) |
Call once on the event where you switch to the task. More... | |
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 | updateInitialize () |
Call before activate() or update() to initialize time and input data. More... | |
virtual bool | update ()=0 |
To be called regularly in the control loop cycle to execute the task. More... | |
virtual bool | updateFinalize () |
Call after update() to constrain the generated setpoints in order to comply with the constraints of the current mode. 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... | |
virtual void | setYawHandler (WeatherVane *ext_yaw_handler) |
Sets an external yaw handler which can be used by any flight task to implement a different yaw control strategy. More... | |
void | updateVelocityControllerIO (const matrix::Vector3f &vel_sp, const matrix::Vector3f &thrust_sp) |
Static Public Attributes | |
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... | |
Protected Member Functions | |
void | _resetSetpoints () |
Reset all setpoints to NAN. More... | |
void | _evaluateVehicleLocalPosition () |
Check and update local position. More... | |
virtual void | _setDefaultConstraints () |
Set constraints to default values. 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) |
Static Protected Attributes | |
static constexpr uint64_t | _timeout = 500000 |
maximal time in us before a loop or data times out More... | |
Definition at line 57 of file FlightTask.hpp.
|
inline |
Definition at line 60 of file FlightTask.hpp.
References _constraints, _resetSetpoints(), activate(), empty_constraints, reActivate(), and ~FlightTask().
|
virtualdefault |
Referenced by FlightTask(), and FlightTasks::switchTask().
|
protected |
Definition at line 51 of file FlightTask.cpp.
References _ekfResetHandlerHeading(), _ekfResetHandlerPositionXY(), _ekfResetHandlerPositionZ(), _ekfResetHandlerVelocityXY(), _ekfResetHandlerVelocityZ(), _reset_counters, _sub_attitude, _sub_vehicle_local_position, vehicle_attitude_s::delta_q_reset, uORB::SubscriptionData< T >::get(), vehicle_attitude_s::quat_reset_counter, vehicle_local_position_s::vxy_reset_counter, vehicle_local_position_s::vz_reset_counter, vehicle_local_position_s::xy_reset_counter, and vehicle_local_position_s::z_reset_counter.
Referenced by updateInitialize().
|
protectedvirtual |
Determine when to trigger a takeoff (ignored in flight)
Reimplemented in FlightTaskAutoLineSmoothVel, and FlightTaskManualAltitude.
Definition at line 174 of file FlightTask.cpp.
References _position, _position_setpoint, _sub_vehicle_local_position, _velocity_setpoint, uORB::SubscriptionData< T >::get(), and vehicle_local_position_s::hagl_min.
Referenced by FlightTaskAuto::updateFinalize().
|
inlineprotectedvirtual |
Reimplemented in FlightTaskAutoLineSmoothVel, and FlightTaskManualAltitude.
Definition at line 196 of file FlightTask.hpp.
Referenced by _checkEkfResetCounters().
|
inlineprotectedvirtual |
Reimplemented in FlightTaskManualPositionSmoothVel, and FlightTaskAutoLineSmoothVel.
Definition at line 192 of file FlightTask.hpp.
Referenced by _checkEkfResetCounters().
|
inlineprotectedvirtual |
Reimplemented in FlightTaskManualPositionSmoothVel, FlightTaskAutoLineSmoothVel, and FlightTaskManualAltitudeSmoothVel.
Definition at line 194 of file FlightTask.hpp.
Referenced by _checkEkfResetCounters().
|
inlineprotectedvirtual |
Reimplemented in FlightTaskManualPositionSmoothVel, and FlightTaskAutoLineSmoothVel.
Definition at line 193 of file FlightTask.hpp.
Referenced by _checkEkfResetCounters().
|
inlineprotectedvirtual |
Reimplemented in FlightTaskManualPositionSmoothVel, FlightTaskAutoLineSmoothVel, and FlightTaskManualAltitudeSmoothVel.
Definition at line 195 of file FlightTask.hpp.
Referenced by _checkEkfResetCounters().
|
protected |
Check and update local position.
Definition at line 114 of file FlightTask.cpp.
References _dist_to_bottom, _position, _sub_attitude, _sub_vehicle_local_position, _time_stamp_current, _timeout, _velocity, _yaw, vehicle_local_position_s::dist_bottom, vehicle_local_position_s::dist_bottom_valid, uORB::SubscriptionData< T >::get(), globallocalconverter_init(), vehicle_attitude_s::q, vehicle_local_position_s::ref_alt, vehicle_local_position_s::ref_lat, vehicle_local_position_s::ref_lon, vehicle_local_position_s::ref_timestamp, matrix::Matrix< Type, M, N >::setAll(), vehicle_attitude_s::timestamp, vehicle_local_position_s::timestamp, vehicle_local_position_s::v_xy_valid, vehicle_local_position_s::v_z_valid, vehicle_local_position_s::vx, vehicle_local_position_s::vy, vehicle_local_position_s::vz, vehicle_local_position_s::x, vehicle_local_position_s::xy_global, vehicle_local_position_s::xy_valid, vehicle_local_position_s::y, vehicle_local_position_s::z, vehicle_local_position_s::z_global, and vehicle_local_position_s::z_valid.
Referenced by updateInitialize().
|
protected |
Monitor the EKF reset counters and call the appropriate handling functions in case of a reset event.
Definition at line 42 of file FlightTask.cpp.
References _reset_counters, _sub_attitude, _sub_vehicle_local_position, uORB::SubscriptionData< T >::get(), vehicle_attitude_s::quat_reset_counter, vehicle_local_position_s::vxy_reset_counter, vehicle_local_position_s::vz_reset_counter, vehicle_local_position_s::xy_reset_counter, and vehicle_local_position_s::z_reset_counter.
Referenced by activate().
|
protected |
Reset all setpoints to NAN.
Definition at line 104 of file FlightTask.cpp.
References _acceleration_setpoint, _jerk_setpoint, _position_setpoint, _thrust_setpoint, _velocity_setpoint, _yaw_setpoint, _yawspeed_setpoint, and matrix::Matrix< Type, M, N >::setAll().
Referenced by activate(), and FlightTask().
|
protectedvirtual |
Set constraints to default values.
Reimplemented in FlightTaskAuto.
Definition at line 163 of file FlightTask.cpp.
References _constraints, vehicle_constraints_s::max_distance_to_ground, vehicle_constraints_s::min_distance_to_ground, math::radians(), vehicle_constraints_s::speed_down, vehicle_constraints_s::speed_up, vehicle_constraints_s::speed_xy, vehicle_constraints_s::tilt, and vehicle_constraints_s::want_takeoff.
Referenced by FlightTaskAuto::_setDefaultConstraints(), and activate().
|
virtual |
Call once on the event where you switch to the task.
state | of the previous task |
Reimplemented in FlightTaskAuto, FlightTaskManualPositionSmoothVel, FlightTaskOrbit, FlightTaskSport, FlightTaskAutoLineSmoothVel, FlightTaskManualPosition, FlightTaskManualAltitudeSmoothVel, FlightTaskOffboard, FlightTaskAutoMapper, FlightTaskAutoMapper2, FlightTaskFailsafe, FlightTaskManualAltitude, FlightTaskTransition, and FlightTaskDescend.
Definition at line 12 of file FlightTask.cpp.
References _gear, _initEkfResetCounters(), _resetSetpoints(), _setDefaultConstraints(), _time_stamp_activate, empty_landing_gear_default_keep, and hrt_absolute_time().
Referenced by FlightTaskDescend::activate(), FlightTaskFailsafe::activate(), FlightTaskTransition::activate(), FlightTaskManualAltitude::activate(), FlightTaskOffboard::activate(), FlightTaskAuto::activate(), FlightTask(), reActivate(), and FlightTasks::switchTask().
|
inlinevirtual |
To be called to adopt parameters from an arrived vehicle command.
command | received command message containing the parameters |
Reimplemented in FlightTaskOrbit, and FlightTaskManual.
Definition at line 86 of file FlightTask.hpp.
References update(), and updateInitialize().
Referenced by FlightTasks::_updateCommand(), and FlightTaskManual::applyCommandParameters().
|
inline |
Get avoidance desired waypoint.
Definition at line 133 of file FlightTask.hpp.
References _desired_waypoint.
|
inline |
Get vehicle constraints.
The constraints can vary with task.
Definition at line 120 of file FlightTask.hpp.
References _constraints.
Referenced by FlightTasks::getConstraints().
|
inline |
Get landing gear position.
The constraints can vary with task.
Definition at line 127 of file FlightTask.hpp.
References _gear.
Referenced by FlightTasks::getGear().
const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint | ( | ) |
Get the output data.
Definition at line 81 of file FlightTask.cpp.
References _acceleration_setpoint, _jerk_setpoint, _position_setpoint, _thrust_setpoint, _velocity_setpoint, _yaw_setpoint, _yawspeed_setpoint, vehicle_local_position_setpoint_s::acceleration, matrix::Matrix< Type, M, N >::copyTo(), hrt_absolute_time(), vehicle_local_position_setpoint_s::jerk, vehicle_local_position_setpoint_s::thrust, vehicle_local_position_setpoint_s::timestamp, vehicle_local_position_setpoint_s::vx, vehicle_local_position_setpoint_s::vy, vehicle_local_position_setpoint_s::vz, vehicle_local_position_setpoint_s::x, vehicle_local_position_setpoint_s::y, vehicle_local_position_setpoint_s::yaw, vehicle_local_position_setpoint_s::yawspeed, and vehicle_local_position_setpoint_s::z.
Referenced by FlightTasks::getPositionSetpoint(), reActivate(), and updateFinalize().
|
inline |
Call this whenever a parameter update notification is received (parameter_update uORB message)
Definition at line 155 of file FlightTask.hpp.
Referenced by FlightTasks::handleParameterUpdate().
|
virtual |
Call this to reset an active Flight Task.
Reimplemented in FlightTaskManualPositionSmoothVel, FlightTaskAutoLineSmoothVel, and FlightTaskManualAltitudeSmoothVel.
Definition at line 22 of file FlightTask.cpp.
References activate(), and getPositionSetpoint().
Referenced by FlightTask(), and FlightTasks::reActivate().
|
inlinevirtual |
Sets an external yaw handler which can be used by any flight task to implement a different yaw control strategy.
This method does nothing, each flighttask which wants to use the yaw handler needs to override this method.
Reimplemented in FlightTaskAuto, and FlightTaskManualPosition.
Definition at line 164 of file FlightTask.hpp.
|
pure virtual |
To be called regularly in the control loop cycle to execute the task.
Implemented in FlightTaskOrbit, FlightTaskManualAltitude, FlightTaskTransition, FlightTaskAutoMapper, FlightTaskAutoMapper2, FlightTaskOffboard, FlightTaskAutoFollowMe, FlightTaskFailsafe, and FlightTaskDescend.
Referenced by applyCommandParameters(), and FlightTasks::update().
|
inlinevirtual |
Call after update() to constrain the generated setpoints in order to comply with the constraints of the current mode.
Reimplemented in FlightTaskAuto.
Definition at line 107 of file FlightTask.hpp.
References getPositionSetpoint().
Referenced by FlightTasks::update().
|
virtual |
Call before activate() or update() to initialize time and input data.
Reimplemented in FlightTaskAuto, FlightTaskManual, FlightTaskManualPosition, FlightTaskOffboard, FlightTaskManualAltitude, and FlightTaskTransition.
Definition at line 27 of file FlightTask.cpp.
References _checkEkfResetCounters(), _deltatime, _evaluateVehicleLocalPosition(), _sub_attitude, _sub_vehicle_local_position, _time, _time_stamp_activate, _time_stamp_current, _time_stamp_last, _timeout, hrt_absolute_time(), math::min(), and uORB::SubscriptionData< T >::update().
Referenced by applyCommandParameters(), FlightTasks::switchTask(), FlightTasks::update(), FlightTaskTransition::updateInitialize(), FlightTaskOffboard::updateInitialize(), FlightTaskManual::updateInitialize(), and FlightTaskAuto::updateInitialize().
|
inline |
Definition at line 166 of file FlightTask.hpp.
References _thrust_setpoint_feedback, and _velocity_setpoint_feedback.
|
protected |
Definition at line 222 of file FlightTask.hpp.
Referenced by _resetSetpoints(), FlightTaskTransition::activate(), getPositionSetpoint(), and FlightTaskTransition::updateAccelerationEstimate().
|
protected |
Vehicle constraints.
The constraints can vary with tasks.
Definition at line 244 of file FlightTask.hpp.
Referenced by FlightTaskAuto::_evaluateTriplets(), FlightTaskManualPosition::_scaleSticks(), FlightTaskAuto::_setDefaultConstraints(), _setDefaultConstraints(), FlightTaskManualPositionSmooth::_updateSetpoints(), FlightTaskManualPosition::activate(), FlightTask(), getConstraints(), and FlightTaskAuto::updateFinalize().
|
protected |
passed time in seconds since the task was last updated
Definition at line 201 of file FlightTask.hpp.
Referenced by FlightTaskAuto::_limitYawRate(), FlightTaskManualPosition::_scaleSticks(), FlightTaskManualAltitudeSmooth::_updateSetpoints(), FlightTaskManualPositionSmooth::_updateSetpoints(), FlightTaskOrbit::update(), FlightTaskTransition::updateAccelerationEstimate(), and updateInitialize().
|
protected |
Desired waypoints.
Goals set by the FCU to be sent to the obstacle avoidance system.
Definition at line 252 of file FlightTask.hpp.
Referenced by getAvoidanceWaypoint().
|
protected |
current height above ground level
Definition at line 210 of file FlightTask.hpp.
Referenced by _evaluateVehicleLocalPosition().
|
protected |
Definition at line 246 of file FlightTask.hpp.
Referenced by activate(), and getGear().
|
protected |
Definition at line 223 of file FlightTask.hpp.
Referenced by _resetSetpoints(), and getPositionSetpoint().
|
protected |
current vehicle position
Definition at line 207 of file FlightTask.hpp.
Referenced by _checkTakeoff(), FlightTaskAuto::_evaluateTriplets(), _evaluateVehicleLocalPosition(), FlightTaskAuto::_getCurrentState(), FlightTaskManualPosition::_scaleSticks(), FlightTaskAuto::_set_heading_from_mode(), FlightTaskAuto::_updateInternalWaypoints(), FlightTaskManualPosition::_updateXYlock(), FlightTaskFailsafe::activate(), FlightTaskManualPosition::activate(), FlightTaskOrbit::activate(), FlightTaskAuto::activate(), FlightTaskTransition::checkSetpoints(), FlightTaskOrbit::generate_circle_approach_setpoints(), FlightTaskFailsafe::update(), FlightTaskOrbit::update(), FlightTaskManualPosition::updateInitialize(), and FlightTaskAuto::updateInitialize().
|
protected |
Setpoints which the position controller has to execute.
Setpoints that are set to NAN are not controlled. Not all setpoints can be set at the same time. If more than one type of setpoint is set, then order of control is a as follow: position, velocity, acceleration, thrust. The exception is _position_setpoint together with _velocity_setpoint, where the _velocity_setpoint is used as feedforward. _acceleration_setpoint and _jerk_setpoint are currently not supported.
Definition at line 220 of file FlightTask.hpp.
Referenced by _checkTakeoff(), _resetSetpoints(), FlightTaskManualPosition::_updateXYlock(), FlightTaskFailsafe::activate(), FlightTaskManualPosition::activate(), FlightTaskAuto::activate(), FlightTaskOrbit::applyCommandParameters(), FlightTaskOrbit::generate_circle_approach_setpoints(), FlightTaskOrbit::generate_circle_setpoints(), getPositionSetpoint(), FlightTaskOrbit::sendTelemetry(), FlightTaskFailsafe::update(), and FlightTaskTransition::update().
struct { ... } FlightTask::_reset_counters |
Referenced by _checkEkfResetCounters(), and _initEkfResetCounters().
|
protected |
Definition at line 172 of file FlightTask.hpp.
Referenced by _checkEkfResetCounters(), _evaluateVehicleLocalPosition(), _initEkfResetCounters(), and updateInitialize().
|
protected |
Definition at line 171 of file FlightTask.hpp.
Referenced by _checkEkfResetCounters(), _checkTakeoff(), FlightTaskAuto::_evaluateGlobalReference(), _evaluateVehicleLocalPosition(), _initEkfResetCounters(), FlightTaskManualPosition::_scaleSticks(), FlightTaskManualPosition::_updateXYlock(), and updateInitialize().
|
protected |
Definition at line 224 of file FlightTask.hpp.
Referenced by _resetSetpoints(), FlightTaskManualPosition::_updateSetpoints(), FlightTaskDescend::activate(), FlightTaskFailsafe::activate(), getPositionSetpoint(), FlightTaskDescend::update(), FlightTaskFailsafe::update(), and FlightTaskTransition::update().
|
protected |
Definition at line 229 of file FlightTask.hpp.
Referenced by updateVelocityControllerIO().
|
protected |
passed time in seconds since the task was activated
Definition at line 200 of file FlightTask.hpp.
Referenced by updateInitialize().
|
protected |
time stamp when task was activated
Definition at line 202 of file FlightTask.hpp.
Referenced by activate(), and updateInitialize().
|
protected |
time stamp at the beginning of the current task update
Definition at line 203 of file FlightTask.hpp.
Referenced by _evaluateVehicleLocalPosition(), and updateInitialize().
|
protected |
time stamp when task was last updated
Definition at line 204 of file FlightTask.hpp.
Referenced by updateInitialize().
|
staticprotected |
maximal time in us before a loop or data times out
Definition at line 199 of file FlightTask.hpp.
Referenced by _evaluateVehicleLocalPosition(), and updateInitialize().
|
protected |
current vehicle velocity
Definition at line 208 of file FlightTask.hpp.
Referenced by _evaluateVehicleLocalPosition(), FlightTaskManualPosition::_scaleSticks(), FlightTaskManualPositionSmooth::_updateSetpoints(), FlightTaskManualPosition::_updateXYlock(), FlightTaskTransition::activate(), FlightTaskOrbit::activate(), FlightTaskAuto::activate(), FlightTaskDescend::update(), FlightTaskFailsafe::update(), FlightTaskTransition::updateAccelerationEstimate(), FlightTaskManualPosition::updateInitialize(), and FlightTaskAuto::updateInitialize().
|
protected |
Definition at line 221 of file FlightTask.hpp.
Referenced by _checkTakeoff(), _resetSetpoints(), FlightTaskManualPosition::_scaleSticks(), FlightTaskManualAltitudeSmooth::_updateSetpoints(), FlightTaskManualPositionSmooth::_updateSetpoints(), FlightTaskManualPosition::_updateXYlock(), FlightTaskFailsafe::activate(), FlightTaskManualPosition::activate(), FlightTaskAuto::activate(), FlightTaskOrbit::generate_circle_approach_setpoints(), FlightTaskOrbit::generate_circle_setpoints(), getPositionSetpoint(), FlightTaskDescend::update(), FlightTaskFailsafe::update(), and FlightTaskTransition::update().
|
protected |
Definition at line 228 of file FlightTask.hpp.
Referenced by updateVelocityControllerIO().
|
protected |
current vehicle yaw heading
Definition at line 209 of file FlightTask.hpp.
Referenced by FlightTaskAuto::_evaluateTriplets(), _evaluateVehicleLocalPosition(), FlightTaskAuto::_set_heading_from_mode(), FlightTaskManualPositionSmooth::_updateSetpoints(), FlightTaskDescend::activate(), FlightTaskFailsafe::activate(), FlightTaskAuto::activate(), and FlightTaskTransition::checkSetpoints().
|
protected |
Definition at line 225 of file FlightTask.hpp.
Referenced by FlightTaskAuto::_evaluateTriplets(), FlightTaskAuto::_limitYawRate(), _resetSetpoints(), FlightTaskAuto::_set_heading_from_mode(), FlightTaskManualPosition::_updateSetpoints(), FlightTaskDescend::activate(), FlightTaskFailsafe::activate(), FlightTaskAuto::activate(), getPositionSetpoint(), FlightTaskTransition::update(), and FlightTaskOrbit::update().
|
protected |
Definition at line 226 of file FlightTask.hpp.
Referenced by FlightTaskAuto::_evaluateTriplets(), FlightTaskAuto::_limitYawRate(), _resetSetpoints(), FlightTaskManualPositionSmooth::_updateSetpoints(), FlightTaskManualPosition::_updateSetpoints(), FlightTaskFailsafe::activate(), FlightTaskAuto::activate(), FlightTaskOrbit::generate_circle_approach_setpoints(), FlightTaskOrbit::generate_circle_setpoints(), and getPositionSetpoint().
|
static |
Empty constraints.
All constraints are set to NAN.
Definition at line 145 of file FlightTask.hpp.
Referenced by FlightTask(), FlightTasks::getConstraints(), and MulticopterPositionControl::Run().
|
static |
default landing gear state
Definition at line 150 of file FlightTask.hpp.
Referenced by activate(), and FlightTasks::getGear().
|
static |
Empty setpoint.
All setpoints are set to NAN.
Definition at line 139 of file FlightTask.hpp.
Referenced by FlightTasks::getPositionSetpoint(), and MulticopterPositionControl::Run().
uint8_t FlightTask::quat = 0 |
Definition at line 237 of file FlightTask.hpp.
uint8_t FlightTask::vxy = 0 |
Definition at line 234 of file FlightTask.hpp.
uint8_t FlightTask::vz = 0 |
Definition at line 236 of file FlightTask.hpp.
uint8_t FlightTask::xy = 0 |
Definition at line 233 of file FlightTask.hpp.
uint8_t FlightTask::z = 0 |
Definition at line 235 of file FlightTask.hpp.