PX4 Firmware
PX4 Autopilot Software http://px4.io
|
#include <FlightTaskAutoLineSmoothVel.hpp>
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_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 | 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... | |
Definition at line 46 of file FlightTaskAutoLineSmoothVel.hpp.
|
default |
|
virtualdefault |
|
inlineoverrideprotectedvirtual |
determines when to trigger a takeoff (ignored in flight)
Reimplemented from FlightTask.
Definition at line 83 of file FlightTaskAutoLineSmoothVel.hpp.
References _want_takeoff.
|
staticprotected |
Definition at line 176 of file FlightTaskAutoLineSmoothVel.cpp.
References math::min(), and math::sign().
|
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().
|
overrideprotectedvirtual |
Reimplemented from FlightTask.
Definition at line 123 of file FlightTaskAutoLineSmoothVel.cpp.
|
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.
|
overrideprotectedvirtual |
Reimplemented from FlightTask.
Definition at line 113 of file FlightTaskAutoLineSmoothVel.cpp.
|
overrideprotectedvirtual |
Reimplemented from FlightTask.
Definition at line 107 of file FlightTaskAutoLineSmoothVel.cpp.
|
overrideprotectedvirtual |
Reimplemented from FlightTask.
Definition at line 118 of file FlightTaskAutoLineSmoothVel.cpp.
|
protected |
Definition at line 139 of file FlightTaskAutoLineSmoothVel.cpp.
|
protected |
Generates heading along trajectory.
Definition at line 147 of file FlightTaskAutoLineSmoothVel.cpp.
References matrix::Vector< Type, M >::length().
|
overrideprotectedvirtual |
Generate setpoints along line.
Implements FlightTaskAutoMapper2.
Definition at line 128 of file FlightTaskAutoLineSmoothVel.cpp.
|
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().
|
protected |
Definition at line 222 of file FlightTaskAutoLineSmoothVel.cpp.
References math::trajectory::computeMaxSpeedFromDistance().
|
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().
|
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().
|
protected |
Definition at line 301 of file FlightTaskAutoLineSmoothVel.cpp.
|
overridevirtual |
Call once on the event where you switch to the task.
state | of the previous task |
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.
|
protected |
Definition at line 72 of file FlightTaskAutoLineSmoothVel.cpp.
References vehicle_local_position_setpoint_s::acceleration, 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.
|
protected |
|
overridevirtual |
Call this to reset an active Flight Task.
Reimplemented from FlightTask.
Definition at line 62 of file FlightTaskAutoLineSmoothVel.cpp.
References f().
|
protected |
Trajectories in x, y and z directions.
Definition at line 86 of file FlightTaskAutoLineSmoothVel.hpp.
|
protected |
Definition at line 84 of file FlightTaskAutoLineSmoothVel.hpp.
Referenced by _checkTakeoff().