PX4 Firmware
PX4 Autopilot Software http://px4.io
FlightTaskAutoLineSmoothVel Class Reference

#include <FlightTaskAutoLineSmoothVel.hpp>

Inheritance diagram for FlightTaskAutoLineSmoothVel:
Collaboration diagram for FlightTaskAutoLineSmoothVel:

Public Member Functions

 FlightTaskAutoLineSmoothVel ()=default
 
virtual ~FlightTaskAutoLineSmoothVel ()=default
 
bool activate (vehicle_local_position_setpoint_s last_setpoint) override
 Call once on the event where you switch to the task. More...
 
void reActivate () override
 Call this to reset an active Flight Task. More...
 
- Public Member Functions inherited from FlightTaskAutoMapper2
 FlightTaskAutoMapper2 ()=default
 
virtual ~FlightTaskAutoMapper2 ()=default
 
bool activate (vehicle_local_position_setpoint_s last_setpoint) override
 Call once on the event where you switch to the task. More...
 
bool update () override
 To be called regularly in the control loop cycle to execute the task. More...
 
- Public Member Functions inherited from FlightTaskAuto
 FlightTaskAuto ()
 
virtual ~FlightTaskAuto ()=default
 
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 bool applyCommandParameters (const vehicle_command_s &command)
 To be called to adopt parameters from an arrived vehicle command. More...
 
const vehicle_local_position_setpoint_s getPositionSetpoint ()
 Get the output data. More...
 
const vehicle_constraints_sgetConstraints ()
 Get vehicle constraints. More...
 
const landing_gear_sgetGear ()
 Get landing gear position. More...
 
const vehicle_trajectory_waypoint_sgetAvoidanceWaypoint ()
 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 checkSetpoints (vehicle_local_position_setpoint_s &setpoints)
 
void _ekfResetHandlerPositionXY () override
 Reset position or velocity setpoints in case of EKF reset event. More...
 
void _ekfResetHandlerVelocityXY () override
 
void _ekfResetHandlerPositionZ () override
 
void _ekfResetHandlerVelocityZ () override
 
void _ekfResetHandlerHeading (float delta_psi) override
 
void _generateSetpoints () override
 Generate setpoints along line. More...
 
void _generateHeading ()
 
bool _generateHeadingAlongTraj ()
 Generates heading along trajectory. More...
 
float _getSpeedAtTarget (float next_target_speed) const
 Constrain the value -max <= val <= max. More...
 
float _getMaxSpeedFromDistance (float braking_distance, float final_speed) const
 
void _prepareSetpoints ()
 Generate velocity target points for the trajectory generator. More...
 
void _updateTrajConstraints ()
 
void _generateTrajectory ()
 
bool _checkTakeoff () override
 determines when to trigger a takeoff (ignored in flight) More...
 
 DEFINE_PARAMETERS_CUSTOM_PARENT (FlightTaskAutoMapper2,(ParamFloat< px4::params::MIS_YAW_ERR >) _param_mis_yaw_err,(ParamFloat< px4::params::MPC_ACC_HOR >) _param_mpc_acc_hor,(ParamFloat< px4::params::MPC_ACC_UP_MAX >) _param_mpc_acc_up_max,(ParamFloat< px4::params::MPC_ACC_DOWN_MAX >) _param_mpc_acc_down_max,(ParamFloat< px4::params::MPC_JERK_AUTO >) _param_mpc_jerk_auto,(ParamFloat< px4::params::MPC_XY_TRAJ_P >) _param_mpc_xy_traj_p,(ParamFloat< px4::params::MPC_Z_TRAJ_P >) _param_mpc_z_traj_p)
 
- Protected Member Functions inherited from FlightTaskAutoMapper2
void _prepareIdleSetpoints ()
 
void _prepareLandSetpoints ()
 
void _prepareVelocitySetpoints ()
 
void _prepareTakeoffSetpoints ()
 
void _preparePositionSetpoints ()
 
void _updateAltitudeAboveGround ()
 Computes altitude above ground based on sensors available. More...
 
void updateParams () override
 See ModuleParam class. More...
 
 DEFINE_PARAMETERS_CUSTOM_PARENT (FlightTaskAuto,(ParamFloat< px4::params::MPC_LAND_SPEED >) _param_mpc_land_speed,(ParamFloat< px4::params::MPC_TILTMAX_LND >) _param_mpc_tiltmax_lnd,(ParamInt< px4::params::MPC_LAND_RC_HELP >) _param_mpc_land_rc_help,(ParamFloat< px4::params::MPC_LAND_ALT1 >) _param_mpc_land_alt1,(ParamFloat< px4::params::MPC_LAND_ALT2 >) _param_mpc_land_alt2,(ParamFloat< px4::params::MPC_TKO_SPEED >) _param_mpc_tko_speed)
 
- Protected Member Functions inherited from FlightTaskAuto
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...
 
void _initEkfResetCounters ()
 Monitor the EKF reset counters and call the appropriate handling functions in case of a reset event. More...
 
void _checkEkfResetCounters ()
 

Static Protected Member Functions

static float _constrainOneSide (float val, float constraint)
 Constrain val between INF and constraint. More...
 
static float _constrainAbs (float val, float max)
 

Protected Attributes

bool _want_takeoff {false}
 
VelocitySmoothing _trajectory [3]
 Trajectories in x, y and z directions. More...
 
- Protected Attributes inherited from FlightTaskAutoMapper2
float _alt_above_ground {0.0f}
 If home provided, then it is altitude above home, otherwise it is altitude above local position reference. More...
 
- Protected Attributes inherited from FlightTaskAuto
matrix::Vector3f _prev_prev_wp {}
 Pre-previous waypoint (local frame). More...
 
matrix::Vector3f _prev_wp {}
 Previous waypoint (local frame). More...
 
matrix::Vector3f _target {}
 Target waypoint (local frame). More...
 
matrix::Vector3f _next_wp {}
 The next waypoint after target (local frame). More...
 
float _mc_cruise_speed {0.0f}
 Requested cruise speed. More...
 
WaypointType _type {WaypointType::idle}
 Type of current target triplet. More...
 
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. More...
 
int _mission_gear {landing_gear_s::GEAR_KEEP}
 
float _yaw_sp_prev {NAN}
 
bool _yaw_sp_aligned {false}
 
ObstacleAvoidance _obstacle_avoidance
 class adjusting setpoints according to external avoidance module's input More...
 
- Protected Attributes inherited from FlightTask
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)}
 
float _time = 0
 passed time in seconds since the task was activated More...
 
float _deltatime = 0
 passed time in seconds since the task was last updated More...
 
hrt_abstime _time_stamp_activate = 0
 time stamp when task was activated More...
 
hrt_abstime _time_stamp_current = 0
 time stamp at the beginning of the current task update More...
 
hrt_abstime _time_stamp_last = 0
 time stamp when task was last updated More...
 
matrix::Vector3f _position
 current vehicle position More...
 
matrix::Vector3f _velocity
 current vehicle velocity More...
 
float _yaw = 0.f
 current vehicle yaw heading More...
 
float _dist_to_bottom = 0.0f
 current height above ground level More...
 
matrix::Vector3f _position_setpoint
 Setpoints which the position controller has to execute. More...
 
matrix::Vector3f _velocity_setpoint
 
matrix::Vector3f _acceleration_setpoint
 
matrix::Vector3f _jerk_setpoint
 
matrix::Vector3f _thrust_setpoint
 
float _yaw_setpoint
 
float _yawspeed_setpoint
 
matrix::Vector3f _velocity_setpoint_feedback
 
matrix::Vector3f _thrust_setpoint_feedback
 
struct {
   uint8_t   xy = 0
 
   uint8_t   vxy = 0
 
   uint8_t   z = 0
 
   uint8_t   vz = 0
 
   uint8_t   quat = 0
 
_reset_counters
 
vehicle_constraints_s _constraints {}
 Vehicle constraints. More...
 
landing_gear_s _gear {}
 
vehicle_trajectory_waypoint_s _desired_waypoint {}
 Desired waypoints. 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...
 

Detailed Description

Definition at line 46 of file FlightTaskAutoLineSmoothVel.hpp.

Constructor & Destructor Documentation

◆ FlightTaskAutoLineSmoothVel()

FlightTaskAutoLineSmoothVel::FlightTaskAutoLineSmoothVel ( )
default

◆ ~FlightTaskAutoLineSmoothVel()

virtual FlightTaskAutoLineSmoothVel::~FlightTaskAutoLineSmoothVel ( )
virtualdefault

Member Function Documentation

◆ _checkTakeoff()

bool FlightTaskAutoLineSmoothVel::_checkTakeoff ( )
inlineoverrideprotectedvirtual

determines when to trigger a takeoff (ignored in flight)

Reimplemented from FlightTask.

Definition at line 83 of file FlightTaskAutoLineSmoothVel.hpp.

References _want_takeoff.

◆ _constrainAbs()

float FlightTaskAutoLineSmoothVel::_constrainAbs ( float  val,
float  max 
)
staticprotected

Definition at line 176 of file FlightTaskAutoLineSmoothVel.cpp.

References math::min(), and math::sign().

Here is the call graph for this function:

◆ _constrainOneSide()

float FlightTaskAutoLineSmoothVel::_constrainOneSide ( float  val,
float  constraint 
)
staticprotected

Constrain val between INF and constraint.

Definition at line 168 of file FlightTaskAutoLineSmoothVel.cpp.

References math::constrain(), f(), FLT_EPSILON, matrix::max(), and matrix::min().

Here is the call graph for this function:

◆ _ekfResetHandlerHeading()

void FlightTaskAutoLineSmoothVel::_ekfResetHandlerHeading ( float  delta_psi)
overrideprotectedvirtual

Reimplemented from FlightTask.

Definition at line 123 of file FlightTaskAutoLineSmoothVel.cpp.

◆ _ekfResetHandlerPositionXY()

void FlightTaskAutoLineSmoothVel::_ekfResetHandlerPositionXY ( )
overrideprotectedvirtual

Reset position or velocity setpoints in case of EKF reset event.

EKF reset handling functions Those functions are called by the base FlightTask in case of an EKF reset event.

Reimplemented from FlightTask.

Definition at line 101 of file FlightTaskAutoLineSmoothVel.cpp.

◆ _ekfResetHandlerPositionZ()

void FlightTaskAutoLineSmoothVel::_ekfResetHandlerPositionZ ( )
overrideprotectedvirtual

Reimplemented from FlightTask.

Definition at line 113 of file FlightTaskAutoLineSmoothVel.cpp.

◆ _ekfResetHandlerVelocityXY()

void FlightTaskAutoLineSmoothVel::_ekfResetHandlerVelocityXY ( )
overrideprotectedvirtual

Reimplemented from FlightTask.

Definition at line 107 of file FlightTaskAutoLineSmoothVel.cpp.

◆ _ekfResetHandlerVelocityZ()

void FlightTaskAutoLineSmoothVel::_ekfResetHandlerVelocityZ ( )
overrideprotectedvirtual

Reimplemented from FlightTask.

Definition at line 118 of file FlightTaskAutoLineSmoothVel.cpp.

◆ _generateHeading()

void FlightTaskAutoLineSmoothVel::_generateHeading ( )
protected

Definition at line 139 of file FlightTaskAutoLineSmoothVel.cpp.

◆ _generateHeadingAlongTraj()

bool FlightTaskAutoLineSmoothVel::_generateHeadingAlongTraj ( )
protected

Generates heading along trajectory.

Definition at line 147 of file FlightTaskAutoLineSmoothVel.cpp.

References matrix::Vector< Type, M >::length().

Here is the call graph for this function:

◆ _generateSetpoints()

void FlightTaskAutoLineSmoothVel::_generateSetpoints ( )
overrideprotectedvirtual

Generate setpoints along line.

Implements FlightTaskAutoMapper2.

Definition at line 128 of file FlightTaskAutoLineSmoothVel.cpp.

◆ _generateTrajectory()

void FlightTaskAutoLineSmoothVel::_generateTrajectory ( )
protected

Definition at line 322 of file FlightTaskAutoLineSmoothVel.cpp.

References math::constrain(), matrix::Vector< Type, M >::dot(), f(), matrix::Vector< Type, M >::length(), and VelocitySmoothing::timeSynchronization().

Here is the call graph for this function:

◆ _getMaxSpeedFromDistance()

float FlightTaskAutoLineSmoothVel::_getMaxSpeedFromDistance ( float  braking_distance,
float  final_speed 
) const
protected

Definition at line 222 of file FlightTaskAutoLineSmoothVel.cpp.

References math::trajectory::computeMaxSpeedFromDistance().

Here is the call graph for this function:

◆ _getSpeedAtTarget()

float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget ( float  next_target_speed) const
protected

Constrain the value -max <= val <= max.

Give 0 if next is the last target

Definition at line 181 of file FlightTaskAutoLineSmoothVel.cpp.

References math::trajectory::computeMaxSpeedInWaypoint(), f(), and math::min().

Here is the call graph for this function:

◆ _prepareSetpoints()

void FlightTaskAutoLineSmoothVel::_prepareSetpoints ( )
protected

Generate velocity target points for the trajectory generator.

Definition at line 232 of file FlightTaskAutoLineSmoothVel.cpp.

References f(), and matrix::Vector< Type, M >::unit_or_zero().

Here is the call graph for this function:

◆ _updateTrajConstraints()

void FlightTaskAutoLineSmoothVel::_updateTrajConstraints ( )
protected

Definition at line 301 of file FlightTaskAutoLineSmoothVel.cpp.

◆ activate()

bool FlightTaskAutoLineSmoothVel::activate ( vehicle_local_position_setpoint_s  last_setpoint)
overridevirtual

Call once on the event where you switch to the task.

Parameters
stateof the previous task
Returns
true on success, false on error

Reimplemented from FlightTaskAuto.

Definition at line 44 of file FlightTaskAutoLineSmoothVel.cpp.

References vehicle_local_position_setpoint_s::acceleration, FlightTaskAutoMapper2::activate(), 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, and vehicle_local_position_setpoint_s::z.

Here is the call graph for this function:

◆ checkSetpoints()

◆ DEFINE_PARAMETERS_CUSTOM_PARENT()

FlightTaskAutoLineSmoothVel::DEFINE_PARAMETERS_CUSTOM_PARENT ( FlightTaskAutoMapper2  ,
(ParamFloat< px4::params::MIS_YAW_ERR >)  _param_mis_yaw_err,
(ParamFloat< px4::params::MPC_ACC_HOR >)  _param_mpc_acc_hor,
(ParamFloat< px4::params::MPC_ACC_UP_MAX >)  _param_mpc_acc_up_max,
(ParamFloat< px4::params::MPC_ACC_DOWN_MAX >)  _param_mpc_acc_down_max,
(ParamFloat< px4::params::MPC_JERK_AUTO >)  _param_mpc_jerk_auto,
(ParamFloat< px4::params::MPC_XY_TRAJ_P >)  _param_mpc_xy_traj_p,
(ParamFloat< px4::params::MPC_Z_TRAJ_P >)  _param_mpc_z_traj_p 
)
protected

◆ reActivate()

void FlightTaskAutoLineSmoothVel::reActivate ( )
overridevirtual

Call this to reset an active Flight Task.

Reimplemented from FlightTask.

Definition at line 62 of file FlightTaskAutoLineSmoothVel.cpp.

References f().

Here is the call graph for this function:

Member Data Documentation

◆ _trajectory

VelocitySmoothing FlightTaskAutoLineSmoothVel::_trajectory[3]
protected

Trajectories in x, y and z directions.

Definition at line 86 of file FlightTaskAutoLineSmoothVel.hpp.

◆ _want_takeoff

bool FlightTaskAutoLineSmoothVel::_want_takeoff {false}
protected

Definition at line 84 of file FlightTaskAutoLineSmoothVel.hpp.

Referenced by _checkTakeoff().


The documentation for this class was generated from the following files: