#include <FlightTaskOrbit.hpp>

Inheritance diagram for FlightTaskOrbit:

Collaboration diagram for FlightTaskOrbit:

Public Member Functions
	FlightTaskOrbit ()

virtual	~FlightTaskOrbit ()=default

bool	applyCommandParameters (const vehicle_command_s &command) override
	To be called to adopt parameters from an arrived vehicle command. More...

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...

bool	checkAcceleration (float r, float v, float a)
	Check the feasibility of orbit parameters with respect to centripetal acceleration a = v^2 / r. More...

Public Member Functions inherited from FlightTaskManualAltitudeSmooth
	FlightTaskManualAltitudeSmooth ()

virtual	~FlightTaskManualAltitudeSmooth ()=default

Public Member Functions inherited from FlightTaskManualAltitude
	FlightTaskManualAltitude ()=default

virtual	~FlightTaskManualAltitude ()=default

bool	updateInitialize () override
	Call before activate() or update() to initialize time and input data. More...

Public Member Functions inherited from FlightTaskManual
	FlightTaskManual ()=default

virtual	~FlightTaskManual ()=default

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
bool	sendTelemetry ()
	Send out telemetry information for the log and MAVLink. More...

bool	setRadius (const float r)
	Change the radius of the circle. More...

bool	setVelocity (const float v)
	Change the velocity of the vehicle on the circle. More...

Protected Member Functions inherited from FlightTaskManualAltitudeSmooth
virtual void	_updateSetpoints () override
	updates all setpoints More...

Protected Member Functions inherited from FlightTaskManualAltitude
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	_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 ()

Private Member Functions
void	generate_circle_approach_setpoints ()
	generates setpoints to smoothly reach the closest point on the circle when starting from far away More...

void	generate_circle_setpoints (matrix::Vector2f center_to_position)
	generates xy setpoints to orbit the vehicle More...

Private Attributes
float	_r = 0.f
	radius with which to orbit the target More...

float	_v = 0.f
	clockwise tangential velocity for orbiting in m/s More...

matrix::Vector2f	_center
	local frame coordinates of the center point More...

bool	_in_circle_approach = false

StraightLine	_circle_approach_line

const float	_radius_min = 1.f

const float	_radius_max = 100.f

const float	_velocity_max = 10.f

const float	_acceleration_max = 2.f

uORB::Publication< orbit_status_s >	_orbit_status_pub {ORB_ID(orbit_status)}

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...

Protected Attributes inherited from FlightTaskManualAltitude
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...

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 49 of file FlightTaskOrbit.hpp.

Constructor & Destructor Documentation

◆ FlightTaskOrbit()

FlightTaskOrbit::FlightTaskOrbit ( )

Definition at line 45 of file FlightTaskOrbit.cpp.

References FlightTaskManual::_sticks_data_required.

◆ ~FlightTaskOrbit()

virtual FlightTaskOrbit::~FlightTaskOrbit ( )

virtualdefault

Member Function Documentation

◆ activate()

bool FlightTaskOrbit::activate ( vehicle_local_position_setpoint_s last_setpoint )

overridevirtual

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

Parameters

state of the previous task

Returns: true on success, false on error

Reimplemented from FlightTaskManualAltitude.

Definition at line 143 of file FlightTaskOrbit.cpp.

References _center, FlightTask::_position, _r, _radius_min, _v, FlightTask::_velocity, and FlightTaskManualAltitude::activate().

Here is the call graph for this function:

◆ applyCommandParameters()

bool FlightTaskOrbit::applyCommandParameters ( const vehicle_command_s & command )

overridevirtual

To be called to adopt parameters from an arrived vehicle command.

Parameters

command received command message containing the parameters

Returns: true if accepted, false if declined

Reimplemented from FlightTaskManual.

Definition at line 50 of file FlightTaskOrbit.cpp.

References _center, _in_circle_approach, FlightTask::_position_setpoint, _v, f(), globallocalconverter_tolocal(), vehicle_command_s::param1, vehicle_command_s::param2, vehicle_command_s::param5, vehicle_command_s::param6, vehicle_command_s::param7, setRadius(), and setVelocity().

Here is the call graph for this function:

◆ checkAcceleration()

bool FlightTaskOrbit::checkAcceleration	(	float	r,
		float	v,
		float	a
	)

Check the feasibility of orbit parameters with respect to centripetal acceleration a = v^2 / r.

Parameters

r	desired radius
v	desired velocity
a	maximal allowed acceleration

Returns: true on success, false if value not accepted

Definition at line 138 of file FlightTaskOrbit.cpp.

Referenced by setRadius(), and setVelocity().

Here is the caller graph for this function:

◆ generate_circle_approach_setpoints()

void FlightTaskOrbit::generate_circle_approach_setpoints ( )

private

generates setpoints to smoothly reach the closest point on the circle when starting from far away

Definition at line 192 of file FlightTaskOrbit.cpp.

References _center, _circle_approach_line, _in_circle_approach, FlightTask::_position, FlightTask::_position_setpoint, _r, FlightTask::_velocity_setpoint, FlightTask::_yawspeed_setpoint, StraightLine::generateSetpoints(), StraightLine::isEndReached(), matrix::Vector< Type, M >::norm(), StraightLine::setLineFromTo(), StraightLine::setSpeed(), and matrix::Vector< Type, M >::unit_or_zero().

Referenced by update().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ generate_circle_setpoints()

void FlightTaskOrbit::generate_circle_setpoints ( matrix::Vector2f center_to_position )

private

generates xy setpoints to orbit the vehicle

Definition at line 212 of file FlightTaskOrbit.cpp.

References FlightTask::_position_setpoint, _r, _v, FlightTask::_velocity_setpoint, FlightTask::_yawspeed_setpoint, matrix::Vector< Type, M >::norm(), and matrix::Vector< Type, M >::unit_or_zero().

Referenced by update().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ sendTelemetry()

bool FlightTaskOrbit::sendTelemetry ( )

protected

Send out telemetry information for the log and MAVLink.

Returns: true on success, false on error

Definition at line 96 of file FlightTaskOrbit.cpp.

References _center, _orbit_status_pub, FlightTask::_position_setpoint, _r, _v, globallocalconverter_toglobal(), hrt_absolute_time(), uORB::Publication< T >::publish(), math::signNoZero(), and orbit_status_s::timestamp.

Referenced by update().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setRadius()

bool FlightTaskOrbit::setRadius ( const float r )

protected

Change the radius of the circle.

Parameters

r	desired new radius

Returns: true on success, false if value not accepted

Definition at line 113 of file FlightTaskOrbit.cpp.

References _acceleration_max, _r, _radius_max, _radius_min, _v, checkAcceleration(), math::constrain(), and math::sign().

Referenced by applyCommandParameters(), and update().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setVelocity()

bool FlightTaskOrbit::setVelocity ( const float v )

protected

Change the velocity of the vehicle on the circle.

Parameters

v	desired new velocity

Returns: true on success, false if value not accepted

Definition at line 127 of file FlightTaskOrbit.cpp.

References _acceleration_max, _r, _v, _velocity_max, and checkAcceleration().

Referenced by applyCommandParameters(), and update().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ update()

bool FlightTaskOrbit::update ( )

overridevirtual

To be called regularly in the control loop cycle to execute the task.

Returns: true on success, false on error

Reimplemented from FlightTaskManualAltitude.

Definition at line 162 of file FlightTaskOrbit.cpp.

References _center, FlightTask::_deltatime, _in_circle_approach, FlightTask::_position, _r, _radius_max, FlightTaskManual::_sticks_expo, _v, _velocity_max, FlightTask::_yaw_setpoint, generate_circle_approach_setpoints(), generate_circle_setpoints(), M_PI_F, sendTelemetry(), setRadius(), setVelocity(), and FlightTaskManualAltitude::update().

Here is the call graph for this function:

Member Data Documentation

◆ _acceleration_max

const float FlightTaskOrbit::_acceleration_max = 2.f

private

Definition at line 105 of file FlightTaskOrbit.hpp.

Referenced by setRadius(), and setVelocity().

◆ _center

matrix::Vector2f FlightTaskOrbit::_center

private

local frame coordinates of the center point

Definition at line 96 of file FlightTaskOrbit.hpp.

Referenced by activate(), applyCommandParameters(), generate_circle_approach_setpoints(), sendTelemetry(), and update().

◆ _circle_approach_line

StraightLine FlightTaskOrbit::_circle_approach_line

private

Definition at line 99 of file FlightTaskOrbit.hpp.

Referenced by generate_circle_approach_setpoints().

◆ _in_circle_approach

bool FlightTaskOrbit::_in_circle_approach = false

private

Definition at line 98 of file FlightTaskOrbit.hpp.

Referenced by applyCommandParameters(), generate_circle_approach_setpoints(), and update().

◆ _orbit_status_pub

uORB::Publication<orbit_status_s> FlightTaskOrbit::_orbit_status_pub {ORB_ID(orbit_status)}

private

Definition at line 107 of file FlightTaskOrbit.hpp.

Referenced by sendTelemetry().

◆ _r

float FlightTaskOrbit::_r = 0.f

private

radius with which to orbit the target

Definition at line 94 of file FlightTaskOrbit.hpp.

Referenced by activate(), generate_circle_approach_setpoints(), generate_circle_setpoints(), sendTelemetry(), setRadius(), setVelocity(), and update().

◆ _radius_max

const float FlightTaskOrbit::_radius_max = 100.f

private

Definition at line 103 of file FlightTaskOrbit.hpp.

Referenced by setRadius(), and update().

◆ _radius_min

const float FlightTaskOrbit::_radius_min = 1.f

private

Definition at line 102 of file FlightTaskOrbit.hpp.

Referenced by activate(), and setRadius().

◆ _v

float FlightTaskOrbit::_v = 0.f

private

clockwise tangential velocity for orbiting in m/s

Definition at line 95 of file FlightTaskOrbit.hpp.

Referenced by activate(), applyCommandParameters(), generate_circle_setpoints(), sendTelemetry(), setRadius(), setVelocity(), and update().

◆ _velocity_max

const float FlightTaskOrbit::_velocity_max = 10.f

private

Definition at line 104 of file FlightTaskOrbit.hpp.

Referenced by setVelocity(), and update().

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

src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp
src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp

Public Member Functions

Protected Member Functions

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ FlightTaskOrbit()

◆ ~FlightTaskOrbit()

Member Function Documentation

◆ activate()

◆ applyCommandParameters()

◆ checkAcceleration()

◆ generate_circle_approach_setpoints()

◆ generate_circle_setpoints()

◆ sendTelemetry()

◆ setRadius()

◆ setVelocity()

◆ update()

Member Data Documentation

◆ _acceleration_max

◆ _center

◆ _circle_approach_line

◆ _in_circle_approach

◆ _orbit_status_pub

◆ _r

◆ _radius_max

◆ _radius_min

◆ _v

◆ _velocity_max