-
Notifications
You must be signed in to change notification settings - Fork 17k
/
mode_throw.cpp
325 lines (259 loc) · 12.7 KB
/
mode_throw.cpp
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
#include "Copter.h"
#if MODE_THROW_ENABLED == ENABLED
// throw_init - initialise throw controller
bool ModeThrow::init(bool ignore_checks)
{
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to use throw to start
return false;
#endif
// do not enter the mode when already armed or when flying
if (motors->armed()) {
return false;
}
// init state
stage = Throw_Disarmed;
nextmode_attempted = false;
// initialise pos controller speed and acceleration
pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), BRAKE_MODE_DECEL_RATE);
pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), BRAKE_MODE_DECEL_RATE);
// set vertical speed and acceleration limits
pos_control->set_max_speed_accel_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z, BRAKE_MODE_DECEL_RATE);
pos_control->set_correction_speed_accel_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z, BRAKE_MODE_DECEL_RATE);
return true;
}
// runs the throw to start controller
// should be called at 100hz or more
void ModeThrow::run()
{
/* Throw State Machine
Throw_Disarmed - motors are off
Throw_Detecting - motors are on and we are waiting for the throw
Throw_Uprighting - the throw has been detected and the copter is being uprighted
Throw_HgtStabilise - the copter is kept level and height is stabilised about the target height
Throw_PosHold - the copter is kept at a constant position and height
*/
if (!motors->armed()) {
// state machine entry is always from a disarmed state
stage = Throw_Disarmed;
} else if (stage == Throw_Disarmed && motors->armed()) {
gcs().send_text(MAV_SEVERITY_INFO,"waiting for throw");
stage = Throw_Detecting;
} else if (stage == Throw_Detecting && throw_detected()){
gcs().send_text(MAV_SEVERITY_INFO,"throw detected - spooling motors");
copter.set_land_complete(false);
stage = Throw_Wait_Throttle_Unlimited;
// Cancel the waiting for throw tone sequence
AP_Notify::flags.waiting_for_throw = false;
} else if (stage == Throw_Wait_Throttle_Unlimited &&
motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
gcs().send_text(MAV_SEVERITY_INFO,"throttle is unlimited - uprighting");
stage = Throw_Uprighting;
} else if (stage == Throw_Uprighting && throw_attitude_good()) {
gcs().send_text(MAV_SEVERITY_INFO,"uprighted - controlling height");
stage = Throw_HgtStabilise;
// initialise the z controller
pos_control->init_z_controller_no_descent();
// initialise the demanded height to 3m above the throw height
// we want to rapidly clear surrounding obstacles
if (g2.throw_type == ThrowType::Drop) {
pos_control->set_pos_target_z_cm(inertial_nav.get_position_z_up_cm() - 100);
} else {
pos_control->set_pos_target_z_cm(inertial_nav.get_position_z_up_cm() + 300);
}
// Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
copter.set_auto_armed(true);
} else if (stage == Throw_HgtStabilise && throw_height_good()) {
gcs().send_text(MAV_SEVERITY_INFO,"height achieved - controlling position");
stage = Throw_PosHold;
// initialise position controller
pos_control->init_xy_controller();
// Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
copter.set_auto_armed(true);
} else if (stage == Throw_PosHold && throw_position_good()) {
if (!nextmode_attempted) {
switch ((Mode::Number)g2.throw_nextmode.get()) {
case Mode::Number::AUTO:
case Mode::Number::GUIDED:
case Mode::Number::RTL:
case Mode::Number::LAND:
case Mode::Number::BRAKE:
case Mode::Number::LOITER:
set_mode((Mode::Number)g2.throw_nextmode.get(), ModeReason::THROW_COMPLETE);
break;
default:
// do nothing
break;
}
nextmode_attempted = true;
}
}
// Throw State Processing
switch (stage) {
case Throw_Disarmed:
// prevent motors from rotating before the throw is detected unless enabled by the user
if (g.throw_motor_start == PreThrowMotorState::RUNNING) {
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
} else {
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
}
// demand zero throttle (motors will be stopped anyway) and continually reset the attitude controller
attitude_control->reset_yaw_target_and_rate();
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_throttle_out(0,true,g.throttle_filt);
break;
case Throw_Detecting:
// prevent motors from rotating before the throw is detected unless enabled by the user
if (g.throw_motor_start == PreThrowMotorState::RUNNING) {
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
} else {
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
}
// Hold throttle at zero during the throw and continually reset the attitude controller
attitude_control->reset_yaw_target_and_rate();
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_throttle_out(0,true,g.throttle_filt);
// Play the waiting for throw tone sequence to alert the user
AP_Notify::flags.waiting_for_throw = true;
break;
case Throw_Wait_Throttle_Unlimited:
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
break;
case Throw_Uprighting:
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// demand a level roll/pitch attitude with zero yaw rate
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
// output 50% throttle and turn off angle boost to maximise righting moment
attitude_control->set_throttle_out(0.5f, false, g.throttle_filt);
break;
case Throw_HgtStabilise:
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
// call height controller
pos_control->set_pos_target_z_from_climb_rate_cm(0.0f);
pos_control->update_z_controller();
break;
case Throw_PosHold:
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// use position controller to stop
Vector2f vel;
Vector2f accel;
pos_control->input_vel_accel_xy(vel, accel);
pos_control->update_xy_controller();
// call attitude controller
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0f);
// call height controller
pos_control->set_pos_target_z_from_climb_rate_cm(0.0f);
pos_control->update_z_controller();
break;
}
#if HAL_LOGGING_ENABLED
// log at 10hz or if stage changes
uint32_t now = AP_HAL::millis();
if ((stage != prev_stage) || (now - last_log_ms) > 100) {
prev_stage = stage;
last_log_ms = now;
const float velocity = inertial_nav.get_velocity_neu_cms().length();
const float velocity_z = inertial_nav.get_velocity_z_up_cms();
const float accel = copter.ins.get_accel().length();
const float ef_accel_z = ahrs.get_accel_ef().z;
const bool throw_detect = (stage > Throw_Detecting) || throw_detected();
const bool attitude_ok = (stage > Throw_Uprighting) || throw_attitude_good();
const bool height_ok = (stage > Throw_HgtStabilise) || throw_height_good();
const bool pos_ok = (stage > Throw_PosHold) || throw_position_good();
// @LoggerMessage: THRO
// @Description: Throw Mode messages
// @URL: https://ardupilot.org/copter/docs/throw-mode.html
// @Field: TimeUS: Time since system startup
// @Field: Stage: Current stage of the Throw Mode
// @Field: Vel: Magnitude of the velocity vector
// @Field: VelZ: Vertical Velocity
// @Field: Acc: Magnitude of the vector of the current acceleration
// @Field: AccEfZ: Vertical earth frame accelerometer value
// @Field: Throw: True if a throw has been detected since entering this mode
// @Field: AttOk: True if the vehicle is upright
// @Field: HgtOk: True if the vehicle is within 50cm of the demanded height
// @Field: PosOk: True if the vehicle is within 50cm of the demanded horizontal position
AP::logger().WriteStreaming(
"THRO",
"TimeUS,Stage,Vel,VelZ,Acc,AccEfZ,Throw,AttOk,HgtOk,PosOk",
"s-nnoo----",
"F-0000----",
"QBffffbbbb",
AP_HAL::micros64(),
(uint8_t)stage,
(double)velocity,
(double)velocity_z,
(double)accel,
(double)ef_accel_z,
throw_detect,
attitude_ok,
height_ok,
pos_ok);
}
#endif // HAL_LOGGING_ENABLED
}
bool ModeThrow::throw_detected()
{
// Check that we have a valid navigation solution
nav_filter_status filt_status = inertial_nav.get_filter_status();
if (!filt_status.flags.attitude || !filt_status.flags.horiz_pos_abs || !filt_status.flags.vert_pos) {
return false;
}
// Check for high speed (>500 cm/s)
bool high_speed = inertial_nav.get_velocity_neu_cms().length_squared() > (THROW_HIGH_SPEED * THROW_HIGH_SPEED);
// check for upwards or downwards trajectory (airdrop) of 50cm/s
bool changing_height;
if (g2.throw_type == ThrowType::Drop) {
changing_height = inertial_nav.get_velocity_z_up_cms() < -THROW_VERTICAL_SPEED;
} else {
changing_height = inertial_nav.get_velocity_z_up_cms() > THROW_VERTICAL_SPEED;
}
// Check the vertical acceleraton is greater than 0.25g
bool free_falling = ahrs.get_accel_ef().z > -0.25 * GRAVITY_MSS;
// Check if the accel length is < 1.0g indicating that any throw action is complete and the copter has been released
bool no_throw_action = copter.ins.get_accel().length() < 1.0f * GRAVITY_MSS;
// fetch the altitude above home
float altitude_above_home; // Use altitude above home if it is set, otherwise relative to EKF origin
if (ahrs.home_is_set()) {
ahrs.get_relative_position_D_home(altitude_above_home);
altitude_above_home = -altitude_above_home; // altitude above home is returned as negative
} else {
altitude_above_home = inertial_nav.get_position_z_up_cm() * 0.01f; // centimeters to meters
}
// Check that the altitude is within user defined limits
const bool height_within_params = (g.throw_altitude_min == 0 || altitude_above_home > g.throw_altitude_min) && (g.throw_altitude_max == 0 || (altitude_above_home < g.throw_altitude_max));
// High velocity or free-fall combined with increasing height indicate a possible air-drop or throw release
bool possible_throw_detected = (free_falling || high_speed) && changing_height && no_throw_action && height_within_params;
// Record time and vertical velocity when we detect the possible throw
if (possible_throw_detected && ((AP_HAL::millis() - free_fall_start_ms) > 500)) {
free_fall_start_ms = AP_HAL::millis();
free_fall_start_velz = inertial_nav.get_velocity_z_up_cms();
}
// Once a possible throw condition has been detected, we check for 2.5 m/s of downwards velocity change in less than 0.5 seconds to confirm
bool throw_condition_confirmed = ((AP_HAL::millis() - free_fall_start_ms < 500) && ((inertial_nav.get_velocity_z_up_cms() - free_fall_start_velz) < -250.0f));
// start motors and enter the control mode if we are in continuous freefall
return throw_condition_confirmed;
}
bool ModeThrow::throw_attitude_good() const
{
// Check that we have uprighted the copter
const Matrix3f &rotMat = ahrs.get_rotation_body_to_ned();
return (rotMat.c.z > 0.866f); // is_upright
}
bool ModeThrow::throw_height_good() const
{
// Check that we are within 0.5m of the demanded height
return (pos_control->get_pos_error_z_cm() < 50.0f);
}
bool ModeThrow::throw_position_good() const
{
// check that our horizontal position error is within 50cm
return (pos_control->get_pos_error_xy_cm() < 50.0f);
}
#endif