Line data Source code
1 : #pragma once
2 :
3 : #include <AP_Arming/AP_Arming.h>
4 : #include <AP_WheelEncoder/AP_WheelRateControl.h>
5 : #include <SRV_Channel/SRV_Channel.h>
6 : #include <AP_BattMonitor/AP_BattMonitor_config.h>
7 :
8 : class AP_MotorsUGV {
9 : public:
10 : // Constructor
11 : AP_MotorsUGV(AP_WheelRateControl& rate_controller);
12 :
13 : // singleton support
14 566 : static AP_MotorsUGV *get_singleton(void) { return _singleton; }
15 :
16 : enum motor_test_order {
17 : MOTOR_TEST_THROTTLE = 1,
18 : MOTOR_TEST_STEERING = 2,
19 : MOTOR_TEST_THROTTLE_LEFT = 3,
20 : MOTOR_TEST_THROTTLE_RIGHT = 4,
21 : MOTOR_TEST_MAINSAIL = 5,
22 : MOTOR_TEST_LAST
23 : };
24 :
25 : // supported omni motor configurations
26 : enum frame_type {
27 : FRAME_TYPE_UNDEFINED = 0,
28 : FRAME_TYPE_OMNI3 = 1,
29 : FRAME_TYPE_OMNIX = 2,
30 : FRAME_TYPE_OMNIPLUS = 3,
31 : FRAME_TYPE_OMNI3MECANUM = 4,
32 : };
33 :
34 : // initialise motors
35 : void init(uint8_t ftype);
36 :
37 : // return true if motors are active
38 : bool active() const;
39 :
40 : // setup output in case of main CPU failure
41 : void setup_safety_output();
42 :
43 : // setup servo output ranges
44 : void setup_servo_output();
45 :
46 : // get or set steering as a value from -4500 to +4500
47 : // apply_scaling should be set to false for manual modes where
48 : // no scaling by speed or angle should e performed
49 119960 : float get_steering() const { return _steering; }
50 : void set_steering(float steering, bool apply_scaling = true);
51 :
52 : // get or set throttle as a value from -100 to 100
53 1299737 : float get_throttle() const { return _throttle; }
54 : void set_throttle(float throttle);
55 :
56 : // get or set roll as a value from -1 to 1
57 0 : float get_roll() const { return _roll; }
58 : void set_roll(float roll);
59 :
60 : // get or set pitch as a value from -1 to 1
61 0 : float get_pitch() const { return _pitch; }
62 : void set_pitch(float pitch);
63 :
64 : // get or set walking_height as a value from -1 to 1
65 0 : float get_walking_height() const { return _walking_height; }
66 : void set_walking_height(float walking_height);
67 :
68 : // get or set lateral input as a value from -100 to +100
69 0 : float get_lateral() const { return _lateral; }
70 : void set_lateral(float lateral);
71 :
72 : // set or get mainsail input as a value from 0 to 100
73 : void set_mainsail(float mainsail);
74 20419 : float get_mainsail() const { return _mainsail; }
75 :
76 : // set or get wingsail input as a value from -100 to 100
77 : void set_wingsail(float wingsail);
78 20419 : float get_wingsail() const { return _wingsail; }
79 :
80 : // set or get mast rotation input as a value from -100 to 100
81 : void set_mast_rotation(float mast_rotation);
82 20419 : float get_mast_rotation() const { return _mast_rotation; }
83 :
84 : // get slew limited throttle
85 : // used by manual mode to avoid bad steering behaviour during transitions from forward to reverse
86 : // same as private slew_limit_throttle method (see below) but does not update throttle state
87 : float get_slew_limited_throttle(float throttle, float dt) const;
88 :
89 : // true if vehicle is capable of skid steering
90 : bool have_skid_steering() const;
91 :
92 : // true if vehicle has vectored thrust (i.e. boat with motor on steering servo)
93 213545 : bool have_vectored_thrust() const { return is_positive(_vector_angle_max); }
94 :
95 : // output to motors and steering servos
96 : // ground_speed should be the vehicle's speed over the surface in m/s
97 : // dt should be expected time between calls to this function
98 : void output(bool armed, float ground_speed, float dt);
99 :
100 : // test steering or throttle output as a percentage of the total (range -100 to +100)
101 : // used in response to DO_MOTOR_TEST mavlink command
102 : bool output_test_pct(motor_test_order motor_seq, float pct);
103 :
104 : // test steering or throttle output using a pwm value
105 : bool output_test_pwm(motor_test_order motor_seq, float pwm);
106 :
107 : // returns true if checks pass, false if they fail. display_failure argument should be true to send text messages to GCS
108 : bool pre_arm_check(bool report) const;
109 :
110 : // return the motor mask
111 283 : uint32_t get_motor_mask() const { return _motor_mask; }
112 :
113 : // returns true if the configured PWM type is digital and should have fixed endpoints
114 : bool is_digital_pwm_type() const;
115 :
116 : // returns true if the vehicle is omni
117 1592620 : bool is_omni() const { return _frame_type != FRAME_TYPE_UNDEFINED && _motors_num > 0; }
118 :
119 : // Return the relay index that would be used for param conversion to relay functions
120 : bool get_legacy_relay_index(int8_t &index1, int8_t &index2, int8_t &index3, int8_t &index4) const;
121 :
122 : // structure for holding motor limit flags
123 : struct AP_MotorsUGV_limit {
124 : uint8_t steer_left : 1; // we have reached the steering controller's left most limit
125 : uint8_t steer_right : 1; // we have reached the steering controller's right most limit
126 : uint8_t throttle_lower : 1; // we have reached throttle's lower limit
127 : uint8_t throttle_upper : 1; // we have reached throttle's upper limit
128 : } limit;
129 :
130 : // var_info for holding Parameter information
131 : static const struct AP_Param::GroupInfo var_info[];
132 :
133 : private:
134 :
135 : enum PWMType {
136 : NORMAL = 0,
137 : ONESHOT = 1,
138 : ONESHOT125 = 2,
139 : BRUSHED_WITH_RELAY = 3,
140 : BRUSHED_BIPOLAR = 4,
141 : DSHOT150 = 5,
142 : DSHOT300 = 6,
143 : DSHOT600 = 7,
144 : DSHOT1200 = 8
145 : };
146 :
147 : // sanity check parameters
148 : void sanity_check_parameters();
149 :
150 : // setup pwm output type
151 : void setup_pwm_type();
152 :
153 : // setup for frames with omni motors
154 : void setup_omni();
155 :
156 : // add omni motor using separate throttle, steering and lateral factors
157 : void add_omni_motor(int8_t motor_num, float throttle_factor, float steering_factor, float lateral_factor);
158 :
159 : // add a motor and set up output function
160 : void add_omni_motor_num(int8_t motor_num);
161 :
162 : // disable omni motor and remove all throttle, steering and lateral factor for this motor
163 : void clear_omni_motors(int8_t motor_num);
164 :
165 : // output to regular steering and throttle channels
166 : void output_regular(bool armed, float ground_speed, float steering, float throttle);
167 :
168 : // output to skid steering channels
169 : void output_skid_steering(bool armed, float steering, float throttle, float dt);
170 :
171 : // output for omni motors
172 : void output_omni(bool armed, float steering, float throttle, float lateral);
173 :
174 : // output throttle (-100 ~ +100) to a throttle channel. Sets relays if required
175 : // dt is the main loop time interval and is required when rate control is required
176 : void output_throttle(SRV_Channel::Function function, float throttle, float dt = 0.0f);
177 :
178 : // output for sailboat's mainsail in the range of 0 to 100 and wing sail in the range +- 100
179 : void output_sail();
180 :
181 : // true if the vehicle has a mainsail or wing sail
182 : bool has_sail() const;
183 :
184 : // slew limit throttle for one iteration
185 : void slew_limit_throttle(float dt);
186 :
187 : // apply power limiting to throttle for one iteration
188 : #if AP_BATTERY_WATT_MAX_ENABLED
189 : void power_limit_throttle(float dt);
190 : #endif
191 :
192 : // set limits based on steering and throttle input
193 : void set_limits_from_input(bool armed, float steering, float throttle);
194 :
195 : // scale a throttle using the _thrust_curve_expo parameter. throttle should be in the range -100 to +100
196 : float get_scaled_throttle(float throttle) const;
197 :
198 : // use rate controller to achieve desired throttle
199 : float get_rate_controlled_throttle(SRV_Channel::Function function, float throttle, float dt);
200 :
201 : // return power_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max
202 : float get_power_limit_max_throttle(float dt);
203 :
204 : // external references
205 : AP_WheelRateControl &_rate_controller;
206 :
207 : static const int8_t AP_MOTORS_NUM_MOTORS_MAX = 4;
208 :
209 : // parameters
210 : AP_Int8 _pwm_type; // PWM output type
211 : AP_Int8 _pwm_freq; // PWM output freq for brushed motors
212 : AP_Int8 _disarm_disable_pwm; // disable PWM output while disarmed
213 : AP_Int16 _slew_rate; // slew rate expressed as a percentage / second
214 : AP_Int8 _throttle_min; // throttle minimum percentage
215 : AP_Int8 _throttle_max; // throttle maximum percentage
216 : AP_Float _thrust_curve_expo; // thrust curve exponent from -1 to +1 with 0 being linear
217 : AP_Float _thrust_asymmetry; // asymmetry factor, how much better your skid-steering motors are at going forward than backwards (forward/backward thrust ratio)
218 : AP_Float _vector_angle_max; // angle between steering's middle position and maximum position when using vectored thrust. zero to disable vectored thrust
219 : AP_Float _speed_scale_base; // speed above which steering is scaled down when using regular steering/throttle vehicles. zero to disable speed scaling
220 : AP_Float _steering_throttle_mix; // Steering vs Throttle priorisation. Higher numbers prioritise steering, lower numbers prioritise throttle. Only valid for Skid Steering vehicles
221 : AP_Float _reverse_delay; // delay in seconds when reversing motor
222 : AP_Float _batt_power_time_constant; // Time constant used to limit the battery power
223 :
224 : // internal variables
225 : float _steering; // requested steering as a value from -4500 to +4500
226 : float _throttle; // requested throttle as a value from -100 to 100
227 : float _throttle_prev; // throttle input from previous iteration
228 : float _throttle_limit = 1.0f; // used for current limiting
229 : bool _scale_steering = true; // true if we should scale steering by speed or angle
230 : float _lateral; // requested lateral input as a value from -100 to +100
231 : float _roll; // requested roll as a value from -1 to +1
232 : float _pitch; // requested pitch as a value from -1 to +1
233 : float _walking_height; // requested height as a value from -1 to +1
234 : float _mainsail; // requested mainsail input as a value from 0 to 100
235 : float _wingsail; // requested wing sail input as a value in the range +- 100
236 : float _mast_rotation; // requested mast rotation input as a value in the range +- 100
237 : uint32_t _motor_mask; // mask of motors configured with pwm_type
238 : frame_type _frame_type; // frame type requested at initialisation
239 :
240 : // omni variables
241 : float _throttle_factor[AP_MOTORS_NUM_MOTORS_MAX];
242 : float _steering_factor[AP_MOTORS_NUM_MOTORS_MAX];
243 : float _lateral_factor[AP_MOTORS_NUM_MOTORS_MAX];
244 : uint8_t _motors_num;
245 :
246 : /*
247 : 3 reversal handling structures, for k_throttle, k_throttleLeft and k_throttleRight
248 : */
249 : struct ReverseThrottle {
250 : float last_throttle;
251 : uint32_t last_output_ms;
252 :
253 : // output with delay for reversal
254 : void output(SRV_Channel::Function function, float throttle, float delay);
255 : } rev_delay_throttle, rev_delay_throttleLeft, rev_delay_throttleRight;
256 :
257 : static AP_MotorsUGV *_singleton;
258 : };
259 :
260 : namespace AP {
261 : AP_MotorsUGV *motors_ugv();
262 : };
|
