PX4 Firmware
PX4 Autopilot Software http://px4.io
|
#include <FlightTaskManualAltitude.hpp>
Public Member Functions | |
FlightTaskManualAltitude ()=default | |
virtual | ~FlightTaskManualAltitude ()=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 | update () override |
To be called regularly in the control loop cycle to execute the task. More... | |
Public Member Functions inherited from FlightTaskManual | |
FlightTaskManual ()=default | |
virtual | ~FlightTaskManual ()=default |
bool | applyCommandParameters (const vehicle_command_s &command) override |
To be called to adopt parameters from an arrived vehicle command. 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 | 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) |
Protected Member Functions | |
void | _updateHeadingSetpoints () |
sets yaw or yaw speed More... | |
void | _ekfResetHandlerHeading (float delta_psi) override |
adjust heading setpoint in case of EKF reset event More... | |
virtual void | _updateSetpoints () |
updates all setpoints More... | |
virtual void | _scaleSticks () |
scales sticks to velocity in z More... | |
bool | _checkTakeoff () override |
Determine when to trigger a takeoff (ignored in flight) More... | |
void | _rotateIntoHeadingFrame (matrix::Vector2f &vec) |
rotates vector into local frame More... | |
void | _updateAltitudeLock () |
Check and sets for position lock. More... | |
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManual,(ParamFloat< px4::params::MPC_HOLD_MAX_Z >) _param_mpc_hold_max_z,(ParamInt< px4::params::MPC_ALT_MODE >) _param_mpc_alt_mode,(ParamFloat< px4::params::MPC_HOLD_MAX_XY >) _param_mpc_hold_max_xy,(ParamFloat< px4::params::MPC_Z_P >) _param_mpc_z_p,(ParamFloat< px4::params::MPC_MAN_Y_MAX >) _param_mpc_man_y_max,(ParamFloat< px4::params::MPC_MAN_Y_TAU >) _param_mpc_man_y_tau,(ParamFloat< px4::params::MPC_MAN_TILT_MAX >) _param_mpc_man_tilt_max,(ParamFloat< px4::params::MPC_LAND_ALT1 >) _param_mpc_land_alt1,(ParamFloat< px4::params::MPC_LAND_ALT2 >) _param_mpc_land_alt2,(ParamFloat< px4::params::MPC_LAND_SPEED >) _param_mpc_land_speed,(ParamFloat< px4::params::MPC_TKO_SPEED >) _param_mpc_tko_speed) private void | _unlockYaw () |
void | _lockYaw () |
float | _applyYawspeedFilter (float yawspeed_target) |
Filter between stick input and yaw setpoint. More... | |
void | _terrainFollowing (bool apply_brake, bool stopped) |
Terrain following. More... | |
void | _respectMinAltitude () |
Minimum Altitude during range sensor operation. More... | |
void | _respectMaxAltitude () |
void | _respectGroundSlowdown () |
Sets downwards velocity constraint based on the distance to ground. More... | |
Protected Member Functions inherited from FlightTaskManual | |
float | stickDeadzone () const |
Protected Member Functions inherited from FlightTask | |
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... | |
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 () |
Protected Attributes | |
uORB::SubscriptionData< home_position_s > | _sub_home_position {ORB_ID(home_position)} |
float | _yawspeed_filter_state {} |
state of low-pass filter in rad/s More... | |
uint8_t | _reset_counter = 0 |
counter for estimator resets in z-direction More... | |
float | _max_speed_up = 10.0f |
float | _min_speed_down = 1.0f |
bool | _terrain_follow {false} |
true when the vehicle is following the terrain height More... | |
bool | _terrain_hold {false} |
true when vehicle is controlling height above a static ground position More... | |
float | _dist_to_ground_lock = NAN |
Distance to ground during terrain following. More... | |
Protected Attributes inherited from FlightTaskManual | |
bool | _sticks_data_required = true |
let inherited task-class define if it depends on stick data More... | |
matrix::Vector< float, 4 > | _sticks |
unmodified manual stick inputs More... | |
matrix::Vector< float, 4 > | _sticks_expo |
modified manual sticks using expo function More... | |
int | _gear_switch_old = manual_control_setpoint_s::SWITCH_POS_NONE |
old switch state 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 45 of file FlightTaskManualAltitude.hpp.
|
default |
|
virtualdefault |
|
protected |
Filter between stick input and yaw setpoint.
yawspeed_target | yaw setpoint desired by the stick |
Definition at line 97 of file FlightTaskManualAltitude.cpp.
References math::max().
|
overrideprotectedvirtual |
Determine when to trigger a takeoff (ignored in flight)
Reimplemented from FlightTask.
Definition at line 366 of file FlightTaskManualAltitude.cpp.
|
overrideprotectedvirtual |
adjust heading setpoint in case of EKF reset event
Reimplemented from FlightTask.
Definition at line 334 of file FlightTaskManualAltitude.cpp.
|
protected |
Definition at line 326 of file FlightTaskManualAltitude.cpp.
|
protected |
Sets downwards velocity constraint based on the distance to ground.
To ensure a slowdown to land speed before hitting the ground.
Definition at line 268 of file FlightTaskManualAltitude.cpp.
References math::constrain(), and math::gradual().
|
protected |
Definition at line 238 of file FlightTaskManualAltitude.cpp.
References math::constrain(), f(), and math::min().
|
protected |
Minimum Altitude during range sensor operation.
If a range sensor is used for altitude estimates, for best operation a minimum altitude is required. The minimum altitude is only enforced during altitude lock.
Definition at line 199 of file FlightTaskManualAltitude.cpp.
|
protected |
rotates vector into local frame
Definition at line 292 of file FlightTaskManualAltitude.cpp.
References f().
Referenced by FlightTaskManualPosition::_scaleSticks().
|
protectedvirtual |
scales sticks to velocity in z
Reimplemented in FlightTaskManualPosition.
Definition at line 86 of file FlightTaskManualAltitude.cpp.
References math::radians().
Referenced by FlightTaskManualPosition::_scaleSticks().
|
protected |
Terrain following.
During terrain following, the position setpoint is adjusted such that the distance to ground is kept constant.
apply_brake | is true if user wants to break |
stopped | is true if vehicle has stopped moving in D-direction |
Definition at line 212 of file FlightTaskManualAltitude.cpp.
|
protected |
Definition at line 320 of file FlightTaskManualAltitude.cpp.
|
protected |
Check and sets for position lock.
If sticks are at center position, the vehicle will exit velocity control and enter position control.
Definition at line 105 of file FlightTaskManualAltitude.cpp.
References FLT_EPSILON.
Referenced by FlightTaskManualAltitudeSmooth::_updateSetpoints(), and FlightTaskManualPositionSmooth::_updateSetpoints().
|
protected |
sets yaw or yaw speed
Definition at line 300 of file FlightTaskManualAltitude.cpp.
|
protectedvirtual |
updates all setpoints
Reimplemented in FlightTaskManualPosition, FlightTaskManualPositionSmoothVel, FlightTaskManualAltitudeSmoothVel, FlightTaskManualPositionSmooth, and FlightTaskManualAltitudeSmooth.
Definition at line 342 of file FlightTaskManualAltitude.cpp.
References matrix::Vector< Type, M >::length(), and matrix::Vector< Type, M >::normalize().
Referenced by FlightTaskManualAltitudeSmooth::_updateSetpoints(), FlightTaskManualAltitudeSmoothVel::_updateSetpoints(), and FlightTaskManualPosition::_updateSetpoints().
|
overridevirtual |
Call once on the event where you switch to the task.
state | of the previous task |
Reimplemented from FlightTask.
Reimplemented in FlightTaskManualPositionSmoothVel, FlightTaskOrbit, FlightTaskSport, FlightTaskManualPosition, and FlightTaskManualAltitudeSmoothVel.
Definition at line 54 of file FlightTaskManualAltitude.cpp.
References FlightTask::activate(), f(), and math::radians().
Referenced by FlightTaskManualAltitudeSmoothVel::activate(), FlightTaskManualPosition::activate(), and FlightTaskOrbit::activate().
|
overridevirtual |
To be called regularly in the control loop cycle to execute the task.
Implements FlightTask.
Reimplemented in FlightTaskOrbit.
Definition at line 372 of file FlightTaskManualAltitude.cpp.
Referenced by FlightTaskOrbit::update().
|
overridevirtual |
Call before activate() or update() to initialize time and input data.
Reimplemented from FlightTaskManual.
Reimplemented in FlightTaskManualPosition.
Definition at line 44 of file FlightTaskManualAltitude.cpp.
References FlightTaskManual::updateInitialize().
Referenced by FlightTaskManualPosition::updateInitialize().
|
protected |
Distance to ground during terrain following.
If user does not demand velocity change in D-direction and the vehcile is in terrain-following mode, then height to ground will be locked to _dist_to_ground_lock.
Definition at line 141 of file FlightTaskManualAltitude.hpp.
|
protected |
Definition at line 130 of file FlightTaskManualAltitude.hpp.
|
protected |
Definition at line 131 of file FlightTaskManualAltitude.hpp.
|
protected |
counter for estimator resets in z-direction
Definition at line 129 of file FlightTaskManualAltitude.hpp.
|
protected |
Definition at line 126 of file FlightTaskManualAltitude.hpp.
|
protected |
true when the vehicle is following the terrain height
Definition at line 132 of file FlightTaskManualAltitude.hpp.
|
protected |
true when vehicle is controlling height above a static ground position
Definition at line 133 of file FlightTaskManualAltitude.hpp.
|
protected |
state of low-pass filter in rad/s
Definition at line 128 of file FlightTaskManualAltitude.hpp.