/
mode.h
346 lines (242 loc) · 11.6 KB
/
mode.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
#pragma once
#include <stdint.h>
#include <GCS_MAVLink/GCS_MAVLink.h> // for MAV_SEVERITY
#include "defines.h"
#define MODE_NEXT_HEADING_UNKNOWN 99999.0f // used to indicate to set_desired_location method that next leg's heading is unknown
// pre-define ModeRTL so Auto can appear higher in this file
class ModeRTL;
class Mode
{
public:
// Constructor
Mode();
// enter this mode, returns false if we failed to enter
bool enter();
// perform any cleanups required:
void exit();
// returns a unique number specific to this mode
virtual uint32_t mode_number() const = 0;
//
// methods that sub classes should override to affect movement of the vehicle in this mode
//
// convert user input to targets, implement high level control for this mode
virtual void update() = 0;
//
// attributes of the mode
//
// return if in non-manual mode : AUTO, GUIDED, RTL
virtual bool is_autopilot_mode() const { return false; }
// returns true if steering is directly controlled by RC
virtual bool manual_steering() const { return false; }
// returns true if the throttle is controlled automatically
virtual bool auto_throttle() { return is_autopilot_mode(); }
// return true if throttle should be supressed in event of a
// FAILSAFE_EVENT_THROTTLE
virtual bool failsafe_throttle_suppress() const { return true; }
//
// attributes for mavlink system status reporting
//
// returns true if any RC input is used
virtual bool has_manual_input() const { return false; }
// true if heading is controlled
virtual bool attitude_stabilized() const { return true; }
//
// navigation methods
//
// return distance (in meters) to destination
virtual float get_distance_to_destination() const { return 0.0f; }
// set desired location and speed (used in RTL, Guided, Auto)
// next_leg_bearing_cd should be heading to the following waypoint (used to slow the vehicle in order to make the turn)
virtual void set_desired_location(const struct Location& destination, float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN);
// true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck
virtual bool reached_destination() { return true; }
// set desired heading and speed - supported in Auto and Guided modes
virtual void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed);
// get speed error in m/s, returns zero for modes that do not control speed
float speed_error() { return _speed_error; }
// Navigation control variables
// The instantaneous desired lateral acceleration in m/s/s
float lateral_acceleration;
protected:
// subclasses override this to perform checks before entering the mode
virtual bool _enter() { return true; }
// subclasses override this to perform any required cleanup when exiting the mode
virtual void _exit() { return; }
// decode pilot steering held in channel_steer, channel_throttle and return in steer_out and throttle_out arguments
// steering_out is in the range -4500 ~ +4500, throttle_out is in the range -100 ~ +100
void get_pilot_desired_steering_and_throttle(float &steering_out, float &throttle_out);
// calculate steering angle given a desired lateral acceleration
void calc_steering_from_lateral_acceleration(bool reversed = false);
// calculate steering output to drive along line from origin to destination waypoint
void calc_steering_to_waypoint(const struct Location &origin, const struct Location &destination, bool reversed = false);
// calculates the amount of throttle that should be output based
// on things like proximity to corners and current speed
virtual void calc_throttle(float target_speed, bool nudge_allowed = true);
// performs a controlled stop. returns true once vehicle has stopped
bool stop_vehicle();
// estimate maximum vehicle speed (in m/s)
float calc_speed_max(float cruise_speed, float cruise_throttle);
// calculate pilot input to nudge speed up or down
// target_speed should be in meters/sec
// cruise_speed is vehicle's cruising speed, cruise_throttle is the throttle (from -1 to +1) that achieves the cruising speed
// return value is a new speed (in m/s) which up to the projected maximum speed based on the cruise speed and cruise throttle
float calc_speed_nudge(float target_speed, float cruise_speed, float cruise_throttle);
// calculated a reduced speed(in m/s) based on yaw error and lateral acceleration and/or distance to a waypoint
// should be called after calc_steering_to_waypoint and before calc_throttle
// relies on these internal members being updated: lateral_acceleration, _yaw_error_cd, _distance_to_destination
float calc_reduced_speed_for_turn_or_distance(float desired_speed);
// references to avoid code churn:
class AP_AHRS &ahrs;
class Parameters &g;
class ParametersG2 &g2;
class RC_Channel *&channel_steer;
class RC_Channel *&channel_throttle;
class AP_Mission &mission;
class AR_AttitudeControl &attitude_control;
// private members for waypoint navigation
Location _origin; // origin Location (vehicle will travel from the origin to the destination)
Location _destination; // destination Location when in Guided_WP
float _distance_to_destination; // distance from vehicle to final destination in meters
bool _reached_destination; // true once the vehicle has reached the destination
float _desired_yaw_cd; // desired yaw in centi-degrees
float _yaw_error_cd; // error between desired yaw and actual yaw in centi-degrees
float _desired_speed; // desired speed in m/s
float _desired_speed_final; // desired speed in m/s when we reach the destination
float _speed_error; // ground speed error in m/s
};
class ModeAcro : public Mode
{
public:
uint32_t mode_number() const override { return ACRO; }
// methods that affect movement of the vehicle in this mode
void update() override;
// attributes for mavlink system status reporting
bool has_manual_input() const override { return true; }
};
class ModeAuto : public Mode
{
public:
// constructor
ModeAuto(ModeRTL& mode_rtl);
uint32_t mode_number() const override { return AUTO; }
// methods that affect movement of the vehicle in this mode
void update() override;
void calc_throttle(float target_speed, bool nudge_allowed = true);
// attributes of the mode
bool is_autopilot_mode() const override { return true; }
bool failsafe_throttle_suppress() const override { return false; }
// return distance (in meters) to destination
float get_distance_to_destination() const override { return _distance_to_destination; }
// set desired location, heading and speed
// set stay_active_at_dest if the vehicle should attempt to maintain it's position at the destination (mostly for boats)
void set_desired_location(const struct Location& destination, float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN, bool stay_active_at_dest = false);
bool reached_destination() override;
// heading and speed control
void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed) override;
bool reached_heading();
// start RTL (within auto)
void start_RTL();
// execute the mission in reverse (i.e. backing up)
void set_reversed(bool value);
protected:
bool _enter() override;
void _exit() override;
enum AutoSubMode {
Auto_WP, // drive to a given location
Auto_HeadingAndSpeed, // turn to a given heading
Auto_RTL // perform RTL within auto mode
} _submode;
private:
bool check_trigger(void);
// references
ModeRTL& _mode_rtl;
// this is set to true when auto has been triggered to start
bool auto_triggered;
bool _reached_heading; // true when vehicle has reached desired heading in TurnToHeading sub mode
bool _stay_active_at_dest; // true when we should actively maintain position even after reaching the destination
bool _reversed; // execute the mission by backing up
};
class ModeGuided : public Mode
{
public:
uint32_t mode_number() const override { return GUIDED; }
// methods that affect movement of the vehicle in this mode
void update() override;
// attributes of the mode
bool is_autopilot_mode() const override { return true; }
bool failsafe_throttle_suppress() const override { return false; }
// return distance (in meters) to destination
float get_distance_to_destination() const override;
// set desired location, heading and speed
void set_desired_location(const struct Location& destination);
void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed) override;
// set desired heading-delta, turn-rate and speed
void set_desired_heading_delta_and_speed(float yaw_delta_cd, float target_speed);
void set_desired_turn_rate_and_speed(float turn_rate_cds, float target_speed);
protected:
enum GuidedMode {
Guided_WP,
Guided_HeadingAndSpeed,
Guided_TurnRateAndSpeed
};
bool _enter() override;
GuidedMode _guided_mode; // stores which GUIDED mode the vehicle is in
// attitude control
bool have_attitude_target; // true if we have a valid attitude target
uint32_t _des_att_time_ms; // system time last call to set_desired_attitude was made (used for timeout)
float _desired_yaw_rate_cds;// target turn rate centi-degrees per second
};
class ModeHold : public Mode
{
public:
uint32_t mode_number() const override { return HOLD; }
// methods that affect movement of the vehicle in this mode
void update() override;
// attributes for mavlink system status reporting
bool attitude_stabilized() const override { return false; }
};
class ModeManual : public Mode
{
public:
uint32_t mode_number() const override { return MANUAL; }
// methods that affect movement of the vehicle in this mode
void update() override;
// attributes of the mode
bool manual_steering() const override { return true; }
// attributes for mavlink system status reporting
bool has_manual_input() const override { return true; }
bool attitude_stabilized() const override { return false; }
};
class ModeRTL : public Mode
{
public:
uint32_t mode_number() const override { return RTL; }
// methods that affect movement of the vehicle in this mode
void update() override;
// attributes of the mode
bool is_autopilot_mode() const override { return true; }
bool failsafe_throttle_suppress() const override { return false; }
float get_distance_to_destination() const override { return _distance_to_destination; }
bool reached_destination() override { return _reached_destination; }
protected:
bool _enter() override;
};
class ModeSteering : public Mode
{
public:
uint32_t mode_number() const override { return STEERING; }
// methods that affect movement of the vehicle in this mode
void update() override;
// attributes for mavlink system status reporting
bool has_manual_input() const override { return true; }
};
class ModeInitializing : public Mode
{
public:
uint32_t mode_number() const override { return INITIALISING; }
// methods that affect movement of the vehicle in this mode
void update() override { }
// attributes for mavlink system status reporting
bool has_manual_input() const override { return true; }
bool attitude_stabilized() const override { return false; }
};