#include <RoverPositionControl.hpp>

Inheritance diagram for RoverPositionControl:

Collaboration diagram for RoverPositionControl:

Public Member Functions
	RoverPositionControl ()

	~RoverPositionControl ()

	RoverPositionControl (const RoverPositionControl &)=delete

RoverPositionControl	operator= (const RoverPositionControl &other)=delete

void	run () override

Static Public Member Functions
static int	task_spawn (int argc, char *argv[])

static RoverPositionControl *	instantiate (int argc, char *argv[])

static int	custom_command (int argc, char *argv[])

static int	print_usage (const char *reason=nullptr)

Private Types
enum	UGV_POSCTRL_MODE { UGV_POSCTRL_MODE_AUTO, UGV_POSCTRL_MODE_OTHER }

Private Member Functions
	DEFINE_PARAMETERS ((ParamFloat< px4::params::GND_L1_PERIOD >) _param_l1_period,(ParamFloat< px4::params::GND_L1_DAMPING >) _param_l1_damping,(ParamFloat< px4::params::GND_L1_DIST >) _param_l1_distance,(ParamFloat< px4::params::GND_SPEED_TRIM >) _param_gndspeed_trim,(ParamFloat< px4::params::GND_SPEED_MAX >) _param_gndspeed_max,(ParamInt< px4::params::GND_SP_CTRL_MODE >) _param_speed_control_mode,(ParamFloat< px4::params::GND_SPEED_P >) _param_speed_p,(ParamFloat< px4::params::GND_SPEED_I >) _param_speed_i,(ParamFloat< px4::params::GND_SPEED_D >) _param_speed_d,(ParamFloat< px4::params::GND_SPEED_IMAX >) _param_speed_imax,(ParamFloat< px4::params::GND_SPEED_THR_SC >) _param_throttle_speed_scaler,(ParamFloat< px4::params::GND_THR_MIN >) _param_throttle_min,(ParamFloat< px4::params::GND_THR_MAX >) _param_throttle_max,(ParamFloat< px4::params::GND_THR_CRUISE >) _param_throttle_cruise,(ParamFloat< px4::params::GND_WHEEL_BASE >) _param_wheel_base,(ParamFloat< px4::params::GND_MAX_ANG >) _param_max_turn_angle) void parameters_update(bool force
	Update our local parameter cache. More...

void	manual_control_setpoint_poll ()

void	position_setpoint_triplet_poll ()

void	attitude_setpoint_poll ()

void	vehicle_control_mode_poll ()

void	vehicle_attitude_poll ()

bool	control_position (const matrix::Vector2f &global_pos, const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &_pos_sp_triplet)
	Control position. More...

void	control_velocity (const matrix::Vector3f &current_velocity, const position_setpoint_triplet_s &pos_sp_triplet)

void	control_attitude (const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp)

Private Attributes
uORB::Publication< position_controller_status_s >	_pos_ctrl_status_pub {ORB_ID(position_controller_status)}

uORB::Publication< actuator_controls_s >	_actuator_controls_pub {ORB_ID(actuator_controls_0)}

int	_control_mode_sub {-1}
	control mode subscription More...

int	_global_pos_sub {-1}

int	_local_pos_sub {-1}

int	_manual_control_sub {-1}
	notification of manual control updates More...

int	_pos_sp_triplet_sub {-1}

int	_att_sp_sub {-1}

int	_vehicle_attitude_sub {-1}

int	_sensor_combined_sub {-1}

uORB::Subscription	_parameter_update_sub {ORB_ID(parameter_update)}

manual_control_setpoint_s	_manual {}
	r/c channel data More...

position_setpoint_triplet_s	_pos_sp_triplet {}
	triplet of mission items More...

vehicle_attitude_setpoint_s	_att_sp {}
	attitude setpoint > More...

vehicle_control_mode_s	_control_mode {}
	control mode More...

vehicle_global_position_s	_global_pos {}
	global vehicle position More...

vehicle_local_position_s	_local_pos {}
	global vehicle position More...

actuator_controls_s	_act_controls {}
	direct control of actuators More...

vehicle_attitude_s	_vehicle_att {}

sensor_combined_s	_sensor_combined {}

SubscriptionData< vehicle_acceleration_s >	_vehicle_acceleration_sub {ORB_ID(vehicle_acceleration)}

perf_counter_t	_loop_perf
	loop performance counter More...

hrt_abstime	_control_position_last_called {0}
	last call of control_position More...

PID_t	_speed_ctrl {}

uint8_t	_pos_reset_counter {0}

ECL_L1_Pos_Controller	_gnd_control

bool	_waypoint_reached {false}

enum RoverPositionControl::UGV_POSCTRL_MODE	UGV_POSCTRL_MODE_OTHER
	used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode. More...

Detailed Description

Definition at line 79 of file RoverPositionControl.hpp.

Member Enumeration Documentation

◆ UGV_POSCTRL_MODE

enum RoverPositionControl::UGV_POSCTRL_MODE

private

Enumerator
UGV_POSCTRL_MODE_AUTO
UGV_POSCTRL_MODE_OTHER

Definition at line 143 of file RoverPositionControl.hpp.

Constructor & Destructor Documentation

◆ RoverPositionControl() [1/2]

RoverPositionControl::RoverPositionControl ( )

Definition at line 61 of file RoverPositionControl.cpp.

Referenced by instantiate().

Here is the caller graph for this function:

◆ ~RoverPositionControl()

RoverPositionControl::~RoverPositionControl ( )

Definition at line 68 of file RoverPositionControl.cpp.

References _gnd_control, _loop_perf, _parameter_update_sub, _speed_ctrl, uORB::Subscription::copy(), f(), mpu9x50::parameters_update(), perf_free(), pid_init(), PID_MODE_DERIVATIV_CALC, pid_set_parameters(), math::radians(), ECL_L1_Pos_Controller::set_l1_damping(), ECL_L1_Pos_Controller::set_l1_period(), ECL_L1_Pos_Controller::set_l1_roll_limit(), and uORB::Subscription::updated().

Here is the call graph for this function:

◆ RoverPositionControl() [2/2]

RoverPositionControl::RoverPositionControl ( const RoverPositionControl & )

delete

Member Function Documentation

◆ attitude_setpoint_poll()

void RoverPositionControl::attitude_setpoint_poll ( )

private

Definition at line 132 of file RoverPositionControl.cpp.

References _att_sp, _att_sp_sub, orb_check(), orb_copy(), and ORB_ID.

Referenced by run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ control_attitude()

void RoverPositionControl::control_attitude	(	const vehicle_attitude_s &	att,
		const vehicle_attitude_setpoint_s &	att_sp
	)

private

Definition at line 329 of file RoverPositionControl.cpp.

References _act_controls, math::constrain(), actuator_controls_s::control, f(), vehicle_attitude_s::q, vehicle_attitude_setpoint_s::q_d, and vehicle_attitude_setpoint_s::thrust_body.

Referenced by run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ control_position()

bool RoverPositionControl::control_position	(	const matrix::Vector2f &	global_pos,
		const matrix::Vector3f &	ground_speed,
		const position_setpoint_triplet_s &	_pos_sp_triplet
	)

private

Control position.

Definition at line 154 of file RoverPositionControl.cpp.

References _act_controls, _control_mode, _control_position_last_called, _global_pos, _gnd_control, _pos_sp_triplet, _speed_ctrl, _vehicle_acceleration_sub, _vehicle_att, _waypoint_reached, math::abs_t(), math::constrain(), actuator_controls_s::control, position_setpoint_s::cruising_speed, position_setpoint_s::cruising_throttle, position_setpoint_triplet_s::current, dt, f(), vehicle_control_mode_s::flag_control_auto_enabled, vehicle_control_mode_s::flag_control_offboard_enabled, uORB::SubscriptionData< T >::get(), get_distance_to_next_waypoint(), hrt_absolute_time(), hrt_elapsed_time(), vehicle_global_position_s::lat, position_setpoint_s::lat, position_setpoint_s::loiter_radius, vehicle_global_position_s::lon, position_setpoint_s::lon, M_PI_F, ECL_L1_Pos_Controller::nav_lateral_acceleration_demand(), ECL_L1_Pos_Controller::navigate_waypoints(), matrix::Vector< Type, M >::norm_squared(), pid_calculate(), position_setpoint_triplet_s::previous, vehicle_attitude_s::q, math::sign(), position_setpoint_s::type, UGV_POSCTRL_MODE_AUTO, UGV_POSCTRL_MODE_OTHER, position_setpoint_s::valid, and vehicle_acceleration_s::xyz.

Referenced by run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ control_velocity()

void RoverPositionControl::control_velocity	(	const matrix::Vector3f &	current_velocity,
		const position_setpoint_triplet_s &	pos_sp_triplet
	)

private

Definition at line 280 of file RoverPositionControl.cpp.

References _act_controls, _speed_ctrl, _vehicle_acceleration_sub, _vehicle_att, math::constrain(), actuator_controls_s::control, position_setpoint_triplet_s::current, dt, f(), uORB::SubscriptionData< T >::get(), pid_calculate(), vehicle_attitude_s::q, position_setpoint_s::velocity_frame, position_setpoint_s::vx, position_setpoint_s::vy, position_setpoint_s::vz, and vehicle_acceleration_s::xyz.

Referenced by run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ custom_command()

int RoverPositionControl::custom_command	(	int	argc,
		char *	argv[]
	)

static

Definition at line 573 of file RoverPositionControl.cpp.

References print_usage().

Here is the call graph for this function:

◆ DEFINE_PARAMETERS()

RoverPositionControl::DEFINE_PARAMETERS	(	(ParamFloat< px4::params::GND_L1_PERIOD >)	_param_l1_period,
		(ParamFloat< px4::params::GND_L1_DAMPING >)	_param_l1_damping,
		(ParamFloat< px4::params::GND_L1_DIST >)	_param_l1_distance,
		(ParamFloat< px4::params::GND_SPEED_TRIM >)	_param_gndspeed_trim,
		(ParamFloat< px4::params::GND_SPEED_MAX >)	_param_gndspeed_max,
		(ParamInt< px4::params::GND_SP_CTRL_MODE >)	_param_speed_control_mode,
		(ParamFloat< px4::params::GND_SPEED_P >)	_param_speed_p,
		(ParamFloat< px4::params::GND_SPEED_I >)	_param_speed_i,
		(ParamFloat< px4::params::GND_SPEED_D >)	_param_speed_d,
		(ParamFloat< px4::params::GND_SPEED_IMAX >)	_param_speed_imax,
		(ParamFloat< px4::params::GND_SPEED_THR_SC >)	_param_throttle_speed_scaler,
		(ParamFloat< px4::params::GND_THR_MIN >)	_param_throttle_min,
		(ParamFloat< px4::params::GND_THR_MAX >)	_param_throttle_max,
		(ParamFloat< px4::params::GND_THR_CRUISE >)	_param_throttle_cruise,
		(ParamFloat< px4::params::GND_WHEEL_BASE >)	_param_wheel_base,
		(ParamFloat< px4::params::GND_MAX_ANG >)	_param_max_turn_angle
	)

private

Update our local parameter cache.

◆ instantiate()

RoverPositionControl * RoverPositionControl::instantiate	(	int	argc,
		char *	argv[]
	)

static

See also: ModuleBase

Definition at line 556 of file RoverPositionControl.cpp.

References ll40ls::instance, and RoverPositionControl().

Here is the call graph for this function:

◆ manual_control_setpoint_poll()

void RoverPositionControl::manual_control_setpoint_poll ( )

private

Definition at line 110 of file RoverPositionControl.cpp.

References _manual, _manual_control_sub, orb_check(), orb_copy(), and ORB_ID.

Here is the call graph for this function:

◆ operator=()

RoverPositionControl RoverPositionControl::operator= ( const RoverPositionControl & other )

delete

◆ position_setpoint_triplet_poll()

void RoverPositionControl::position_setpoint_triplet_poll ( )

private

Definition at line 121 of file RoverPositionControl.cpp.

References _pos_sp_triplet, _pos_sp_triplet_sub, orb_check(), orb_copy(), and ORB_ID.

Referenced by run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ print_usage()

int RoverPositionControl::print_usage ( const char * reason = nullptr )

static

See also: ModuleBase

Definition at line 578 of file RoverPositionControl.cpp.

Referenced by custom_command().

Here is the caller graph for this function:

◆ run()

void RoverPositionControl::run ( )

override

See also: ModuleBase::run()

Definition at line 353 of file RoverPositionControl.cpp.

References _act_controls, _actuator_controls_pub, _att_sp, _att_sp_sub, _control_mode, _control_mode_sub, _global_pos, _global_pos_sub, _gnd_control, _local_pos, _local_pos_sub, _loop_perf, _manual, _manual_control_sub, _pos_ctrl_status_pub, _pos_reset_counter, _pos_sp_triplet, _pos_sp_triplet_sub, _sensor_combined, _sensor_combined_sub, _vehicle_acceleration_sub, _vehicle_att, _vehicle_attitude_sub, position_controller_status_s::acceptance_radius, position_setpoint_s::alt, attitude_setpoint_poll(), actuator_controls_s::control, control_attitude(), control_position(), control_velocity(), ECL_L1_Pos_Controller::crosstrack_error(), position_setpoint_triplet_s::current, vehicle_control_mode_s::flag_control_attitude_enabled, vehicle_control_mode_s::flag_control_manual_enabled, vehicle_control_mode_s::flag_control_offboard_enabled, vehicle_control_mode_s::flag_control_position_enabled, vehicle_control_mode_s::flag_control_velocity_enabled, get_distance_to_next_waypoint(), globallocalconverter_init(), globallocalconverter_initialized(), globallocalconverter_toglobal(), hrt_absolute_time(), vehicle_global_position_s::lat, position_setpoint_s::lat, vehicle_global_position_s::lat_lon_reset_counter, vehicle_global_position_s::lon, position_setpoint_s::lon, position_controller_status_s::nav_bearing, ECL_L1_Pos_Controller::nav_bearing(), position_controller_status_s::nav_pitch, position_controller_status_s::nav_roll, orb_copy(), ORB_ID, orb_set_interval(), orb_subscribe(), orb_unsubscribe(), mpu9x50::parameters_update(), perf_begin(), perf_end(), position_setpoint_triplet_poll(), uORB::Publication< T >::publish(), px4_poll(), manual_control_setpoint_s::r, vehicle_local_position_s::ref_alt, vehicle_local_position_s::ref_lat, vehicle_local_position_s::ref_lon, vehicle_local_position_s::ref_timestamp, position_controller_status_s::target_bearing, ECL_L1_Pos_Controller::target_bearing(), position_controller_status_s::timestamp, actuator_controls_s::timestamp, uORB::SubscriptionData< T >::update(), vehicle_attitude_poll(), vehicle_control_mode_poll(), vehicle_global_position_s::vel_d, vehicle_global_position_s::vel_e, vehicle_global_position_s::vel_n, vehicle_local_position_s::vx, vehicle_local_position_s::vy, vehicle_local_position_s::vz, warn, warnx, position_controller_status_s::wp_dist, position_setpoint_s::x, manual_control_setpoint_s::x, position_controller_status_s::xtrack_error, position_setpoint_s::y, manual_control_setpoint_s::y, position_controller_status_s::yaw_acceptance, position_setpoint_s::z, and manual_control_setpoint_s::z.

Here is the call graph for this function:

◆ task_spawn()

int RoverPositionControl::task_spawn	(	int	argc,
		char *	argv[]
	)

static

See also: ModuleBase

Definition at line 538 of file RoverPositionControl.cpp.

References OK, and warn.

◆ vehicle_attitude_poll()

void RoverPositionControl::vehicle_attitude_poll ( )

private

Definition at line 143 of file RoverPositionControl.cpp.

References _vehicle_att, _vehicle_attitude_sub, orb_check(), orb_copy(), and ORB_ID.

Referenced by run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ vehicle_control_mode_poll()

void RoverPositionControl::vehicle_control_mode_poll ( )

private

Definition at line 99 of file RoverPositionControl.cpp.

References _control_mode, _control_mode_sub, orb_check(), orb_copy(), and ORB_ID.

Referenced by run().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ _act_controls

actuator_controls_s RoverPositionControl::_act_controls {}

private

direct control of actuators

Definition at line 122 of file RoverPositionControl.hpp.

Referenced by control_attitude(), control_position(), control_velocity(), and run().

◆ _actuator_controls_pub

uORB::Publication<actuator_controls_s> RoverPositionControl::_actuator_controls_pub {ORB_ID(actuator_controls_0)}

private

Definition at line 103 of file RoverPositionControl.hpp.

Referenced by run().

◆ _att_sp

vehicle_attitude_setpoint_s RoverPositionControl::_att_sp {}

private

attitude setpoint >

Definition at line 118 of file RoverPositionControl.hpp.

Referenced by attitude_setpoint_poll(), and run().

◆ _att_sp_sub

int RoverPositionControl::_att_sp_sub {-1}

private

Definition at line 110 of file RoverPositionControl.hpp.

Referenced by attitude_setpoint_poll(), and run().

◆ _control_mode

vehicle_control_mode_s RoverPositionControl::_control_mode {}

private

control mode

Definition at line 119 of file RoverPositionControl.hpp.

Referenced by control_position(), run(), and vehicle_control_mode_poll().

◆ _control_mode_sub

int RoverPositionControl::_control_mode_sub {-1}

private

control mode subscription

Definition at line 105 of file RoverPositionControl.hpp.

Referenced by run(), and vehicle_control_mode_poll().

◆ _control_position_last_called

hrt_abstime RoverPositionControl::_control_position_last_called {0}

private

last call of control_position

Definition at line 130 of file RoverPositionControl.hpp.

Referenced by control_position().

◆ _global_pos

vehicle_global_position_s RoverPositionControl::_global_pos {}

private

global vehicle position

Definition at line 120 of file RoverPositionControl.hpp.

Referenced by control_position(), and run().

◆ _global_pos_sub

int RoverPositionControl::_global_pos_sub {-1}

private

Definition at line 106 of file RoverPositionControl.hpp.

Referenced by run().

◆ _gnd_control

ECL_L1_Pos_Controller RoverPositionControl::_gnd_control

private

Definition at line 139 of file RoverPositionControl.hpp.

Referenced by control_position(), run(), and ~RoverPositionControl().

◆ _local_pos

vehicle_local_position_s RoverPositionControl::_local_pos {}

private

global vehicle position

Definition at line 121 of file RoverPositionControl.hpp.

Referenced by run().

◆ _local_pos_sub

int RoverPositionControl::_local_pos_sub {-1}

private

Definition at line 107 of file RoverPositionControl.hpp.

Referenced by run().

◆ _loop_perf

perf_counter_t RoverPositionControl::_loop_perf

private

loop performance counter

Definition at line 128 of file RoverPositionControl.hpp.

Referenced by run(), and ~RoverPositionControl().

◆ _manual

manual_control_setpoint_s RoverPositionControl::_manual {}

private

r/c channel data

Definition at line 116 of file RoverPositionControl.hpp.

Referenced by manual_control_setpoint_poll(), and run().

◆ _manual_control_sub

int RoverPositionControl::_manual_control_sub {-1}

private

notification of manual control updates

Definition at line 108 of file RoverPositionControl.hpp.

Referenced by manual_control_setpoint_poll(), and run().

◆ _parameter_update_sub

uORB::Subscription RoverPositionControl::_parameter_update_sub {ORB_ID(parameter_update)}

private

Definition at line 114 of file RoverPositionControl.hpp.

Referenced by ~RoverPositionControl().

◆ _pos_ctrl_status_pub

uORB::Publication<position_controller_status_s> RoverPositionControl::_pos_ctrl_status_pub {ORB_ID(position_controller_status)}

private

Definition at line 102 of file RoverPositionControl.hpp.

Referenced by run().

◆ _pos_reset_counter

uint8_t RoverPositionControl::_pos_reset_counter {0}

private

Definition at line 137 of file RoverPositionControl.hpp.

Referenced by run().

◆ _pos_sp_triplet

position_setpoint_triplet_s RoverPositionControl::_pos_sp_triplet {}

private

triplet of mission items

Definition at line 117 of file RoverPositionControl.hpp.

Referenced by control_position(), position_setpoint_triplet_poll(), and run().

◆ _pos_sp_triplet_sub

int RoverPositionControl::_pos_sp_triplet_sub {-1}

private

Definition at line 109 of file RoverPositionControl.hpp.

Referenced by position_setpoint_triplet_poll(), and run().

◆ _sensor_combined

sensor_combined_s RoverPositionControl::_sensor_combined {}

private

Definition at line 124 of file RoverPositionControl.hpp.

Referenced by run().

◆ _sensor_combined_sub

int RoverPositionControl::_sensor_combined_sub {-1}

private

Definition at line 112 of file RoverPositionControl.hpp.

Referenced by run().

◆ _speed_ctrl

PID_t RoverPositionControl::_speed_ctrl {}

private

Definition at line 134 of file RoverPositionControl.hpp.

Referenced by control_position(), control_velocity(), and ~RoverPositionControl().

◆ _vehicle_acceleration_sub

SubscriptionData<vehicle_acceleration_s> RoverPositionControl::_vehicle_acceleration_sub {ORB_ID(vehicle_acceleration)}

private

Definition at line 126 of file RoverPositionControl.hpp.

Referenced by control_position(), control_velocity(), and run().

◆ _vehicle_att

vehicle_attitude_s RoverPositionControl::_vehicle_att {}

private

Definition at line 123 of file RoverPositionControl.hpp.

Referenced by control_position(), control_velocity(), run(), and vehicle_attitude_poll().

◆ _vehicle_attitude_sub

int RoverPositionControl::_vehicle_attitude_sub {-1}

private

Definition at line 111 of file RoverPositionControl.hpp.

Referenced by run(), and vehicle_attitude_poll().

◆ _waypoint_reached

bool RoverPositionControl::_waypoint_reached {false}

private

Definition at line 141 of file RoverPositionControl.hpp.

Referenced by control_position().

◆ UGV_POSCTRL_MODE_OTHER

enum RoverPositionControl::UGV_POSCTRL_MODE RoverPositionControl::UGV_POSCTRL_MODE_OTHER

private

used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.

Referenced by control_position().

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

src/modules/rover_pos_control/RoverPositionControl.hpp
src/modules/rover_pos_control/RoverPositionControl.cpp

Public Member Functions

Static Public Member Functions

Private Types

Private Member Functions

Private Attributes

Detailed Description

Member Enumeration Documentation

◆ UGV_POSCTRL_MODE

Constructor & Destructor Documentation

◆ RoverPositionControl() [1/2]

◆ ~RoverPositionControl()

◆ RoverPositionControl() [2/2]

Member Function Documentation

◆ attitude_setpoint_poll()

◆ control_attitude()

◆ control_position()

◆ control_velocity()

◆ custom_command()

◆ DEFINE_PARAMETERS()

◆ instantiate()

◆ manual_control_setpoint_poll()

◆ operator=()

◆ position_setpoint_triplet_poll()

◆ print_usage()

◆ run()

◆ task_spawn()

◆ vehicle_attitude_poll()

◆ vehicle_control_mode_poll()

Member Data Documentation

◆ _act_controls

◆ _actuator_controls_pub

◆ _att_sp

◆ _att_sp_sub

◆ _control_mode

◆ _control_mode_sub

◆ _control_position_last_called

◆ _global_pos

◆ _global_pos_sub

◆ _gnd_control

◆ _local_pos

◆ _local_pos_sub

◆ _loop_perf

◆ _manual

◆ _manual_control_sub

◆ _parameter_update_sub

◆ _pos_ctrl_status_pub

◆ _pos_reset_counter

◆ _pos_sp_triplet

◆ _pos_sp_triplet_sub

◆ _sensor_combined

◆ _sensor_combined_sub

◆ _speed_ctrl

◆ _vehicle_acceleration_sub

◆ _vehicle_att

◆ _vehicle_attitude_sub

◆ _waypoint_reached

◆ UGV_POSCTRL_MODE_OTHER