PX4 Firmware
PX4 Autopilot Software http://px4.io
RoverPositionControl.hpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2017 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  *
36  * This module is a modification of the fixed wing module and it is designed for ground rovers.
37  * It has been developed starting from the fw module, simplified and improved with dedicated items.
38  *
39  * All the ackowledgments and credits for the fw wing app are reported in those files.
40  *
41  * @author Marco Zorzi <mzorzi@student.ethz.ch>
42  */
43 
44 #include <float.h>
45 
46 #include <drivers/drv_hrt.h>
47 #include <lib/ecl/geo/geo.h>
49 #include <lib/mathlib/mathlib.h>
50 #include <lib/perf/perf_counter.h>
51 #include <lib/pid/pid.h>
52 #include <px4_platform_common/px4_config.h>
53 #include <px4_platform_common/defines.h>
54 #include <px4_platform_common/posix.h>
55 #include <px4_platform_common/tasks.h>
56 #include <px4_platform_common/module.h>
57 #include <px4_platform_common/module_params.h>
58 #include <uORB/Subscription.hpp>
59 #include <uORB/Publication.hpp>
73 #include <uORB/uORB.h>
74 
75 using matrix::Dcmf;
76 
78 
79 class RoverPositionControl : public ModuleBase<RoverPositionControl>, public ModuleParams
80 {
81 public:
86 
87  /** @see ModuleBase */
88  static int task_spawn(int argc, char *argv[]);
89 
90  /** @see ModuleBase */
91  static RoverPositionControl *instantiate(int argc, char *argv[]);
92 
93  static int custom_command(int argc, char *argv[]);
94 
95  /** @see ModuleBase */
96  static int print_usage(const char *reason = nullptr);
97 
98  /** @see ModuleBase::run() */
99  void run() override;
100 
101 private:
104 
105  int _control_mode_sub{-1}; /**< control mode subscription */
107  int _local_pos_sub{-1};
108  int _manual_control_sub{-1}; /**< notification of manual control updates */
110  int _att_sp_sub{-1};
113 
115 
116  manual_control_setpoint_s _manual{}; /**< r/c channel data */
117  position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of mission items */
118  vehicle_attitude_setpoint_s _att_sp{}; /**< attitude setpoint > */
119  vehicle_control_mode_s _control_mode{}; /**< control mode */
120  vehicle_global_position_s _global_pos{}; /**< global vehicle position */
121  vehicle_local_position_s _local_pos{}; /**< global vehicle position */
122  actuator_controls_s _act_controls{}; /**< direct control of actuators */
125 
127 
128  perf_counter_t _loop_perf; /**< loop performance counter */
129 
130  hrt_abstime _control_position_last_called{0}; /**<last call of control_position */
131 
132  /* Pid controller for the speed. Here we assume we can control airspeed but the control variable is actually on
133  the throttle. For now just assuming a proportional scaler between controlled airspeed and throttle output.*/
135 
136  // estimator reset counters
137  uint8_t _pos_reset_counter{0}; // captures the number of times the estimator has reset the horizontal position
138 
140 
141  bool _waypoint_reached{false};
142 
146  } _control_mode_current{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.
147 
149  (ParamFloat<px4::params::GND_L1_PERIOD>) _param_l1_period,
150  (ParamFloat<px4::params::GND_L1_DAMPING>) _param_l1_damping,
151  (ParamFloat<px4::params::GND_L1_DIST>) _param_l1_distance,
152 
153  (ParamFloat<px4::params::GND_SPEED_TRIM>) _param_gndspeed_trim,
154  (ParamFloat<px4::params::GND_SPEED_MAX>) _param_gndspeed_max,
155 
156  (ParamInt<px4::params::GND_SP_CTRL_MODE>) _param_speed_control_mode,
157  (ParamFloat<px4::params::GND_SPEED_P>) _param_speed_p,
158  (ParamFloat<px4::params::GND_SPEED_I>) _param_speed_i,
159  (ParamFloat<px4::params::GND_SPEED_D>) _param_speed_d,
160  (ParamFloat<px4::params::GND_SPEED_IMAX>) _param_speed_imax,
161  (ParamFloat<px4::params::GND_SPEED_THR_SC>) _param_throttle_speed_scaler,
162 
163  (ParamFloat<px4::params::GND_THR_MIN>) _param_throttle_min,
164  (ParamFloat<px4::params::GND_THR_MAX>) _param_throttle_max,
165  (ParamFloat<px4::params::GND_THR_CRUISE>) _param_throttle_cruise,
166 
167  (ParamFloat<px4::params::GND_WHEEL_BASE>) _param_wheel_base,
168  (ParamFloat<px4::params::GND_MAX_ANG>) _param_max_turn_angle
169  )
170 
171  /**
172  * Update our local parameter cache.
173  */
174  void parameters_update(bool force = false);
175 
178  void attitude_setpoint_poll();
180  void vehicle_attitude_poll();
181 
182  /**
183  * Control position.
184  */
185  bool control_position(const matrix::Vector2f &global_pos, const matrix::Vector3f &ground_speed,
187  void control_velocity(const matrix::Vector3f &current_velocity, const position_setpoint_triplet_s &pos_sp_triplet);
188  void control_attitude(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp);
189 
190 };
void control_attitude(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp)
void control_velocity(const matrix::Vector3f &current_velocity, const position_setpoint_triplet_s &pos_sp_triplet)
RoverPositionControl operator=(const RoverPositionControl &other)=delete
API for the uORB lightweight object broker.
Definition of geo / math functions to perform geodesic calculations.
L1 Nonlinear Guidance Logic.
vehicle_local_position_s _local_pos
global vehicle position
Dcm< float > Dcmf
Definition: Dcm.hpp:185
sensor_combined_s _sensor_combined
uORB::Subscription _parameter_update_sub
ECL_L1_Pos_Controller _gnd_control
vehicle_global_position_s _global_pos
global vehicle position
bool control_position(const matrix::Vector2f &global_pos, const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &_pos_sp_triplet)
Control position.
manual_control_setpoint_s _manual
r/c channel data
High-resolution timer with callouts and timekeeping.
actuator_controls_s _act_controls
direct control of actuators
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 ...
Implementation of L1 position control.
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
vehicle_attitude_s _vehicle_att
Header common to all counters.
static void parameters_update()
update all parameters
vehicle_control_mode_s _control_mode
control mode
hrt_abstime _control_position_last_called
last call of control_position
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.
uORB::Publication< position_controller_status_s > _pos_ctrl_status_pub
perf_counter_t _loop_perf
loop performance counter
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
position_setpoint_triplet_s _pos_sp_triplet
triplet of mission items
Definition: pid.h:71
SubscriptionData< vehicle_acceleration_s > _vehicle_acceleration_sub
int _control_mode_sub
control mode subscription
static int print_usage(const char *reason=nullptr)
static int task_spawn(int argc, char *argv[])
static RoverPositionControl * instantiate(int argc, char *argv[])
uORB::Publication< actuator_controls_s > _actuator_controls_pub
int _manual_control_sub
notification of manual control updates
static int custom_command(int argc, char *argv[])
Definition of generic PID controller.
vehicle_attitude_setpoint_s _att_sp
attitude setpoint >
Performance measuring tools.