CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/Tools/autotest/arducopter.py
Views: 1798
1
'''
2
Fly Copter in SITL
3
4
AP_FLAKE8_CLEAN
5
'''
6
7
from __future__ import print_function
8
import copy
9
import math
10
import os
11
import shutil
12
import time
13
import numpy
14
15
from pymavlink import quaternion
16
from pymavlink import mavutil
17
from pymavlink import mavextra
18
from pymavlink import rotmat
19
20
from pysim import util
21
from pysim import vehicleinfo
22
23
import vehicle_test_suite
24
25
from vehicle_test_suite import NotAchievedException, AutoTestTimeoutException, PreconditionFailedException
26
from vehicle_test_suite import Test
27
from vehicle_test_suite import MAV_POS_TARGET_TYPE_MASK
28
from vehicle_test_suite import WaitAndMaintainArmed
29
from vehicle_test_suite import WaitModeTimeout
30
31
from pymavlink.rotmat import Vector3
32
33
# get location of scripts
34
testdir = os.path.dirname(os.path.realpath(__file__))
35
SITL_START_LOCATION = mavutil.location(-35.362938, 149.165085, 584, 270)
36
37
# Flight mode switch positions are set-up in arducopter.param to be
38
# switch 1 = Circle
39
# switch 2 = Land
40
# switch 3 = RTL
41
# switch 4 = Auto
42
# switch 5 = Loiter
43
# switch 6 = Stabilize
44
45
46
class AutoTestCopter(vehicle_test_suite.TestSuite):
47
@staticmethod
48
def get_not_armable_mode_list():
49
return ["AUTO", "AUTOTUNE", "BRAKE", "CIRCLE", "FLIP", "LAND", "RTL", "SMART_RTL", "AVOID_ADSB", "FOLLOW"]
50
51
@staticmethod
52
def get_not_disarmed_settable_modes_list():
53
return ["FLIP", "AUTOTUNE"]
54
55
@staticmethod
56
def get_no_position_not_settable_modes_list():
57
return []
58
59
@staticmethod
60
def get_position_armable_modes_list():
61
return ["DRIFT", "GUIDED", "LOITER", "POSHOLD", "THROW"]
62
63
@staticmethod
64
def get_normal_armable_modes_list():
65
return ["ACRO", "ALT_HOLD", "STABILIZE", "GUIDED_NOGPS"]
66
67
def log_name(self):
68
return "ArduCopter"
69
70
def test_filepath(self):
71
return os.path.realpath(__file__)
72
73
def default_speedup(self):
74
return 100
75
76
def set_current_test_name(self, name):
77
self.current_test_name_directory = "ArduCopter_Tests/" + name + "/"
78
79
def sitl_start_location(self):
80
return SITL_START_LOCATION
81
82
def mavproxy_options(self):
83
ret = super(AutoTestCopter, self).mavproxy_options()
84
if self.frame != 'heli':
85
ret.append('--quadcopter')
86
return ret
87
88
def sitl_streamrate(self):
89
return 5
90
91
def vehicleinfo_key(self):
92
return 'ArduCopter'
93
94
def default_frame(self):
95
return "+"
96
97
def apply_defaultfile_parameters(self):
98
# Copter passes in a defaults_filepath in place of applying
99
# parameters afterwards.
100
pass
101
102
def defaults_filepath(self):
103
return self.model_defaults_filepath(self.frame)
104
105
def wait_disarmed_default_wait_time(self):
106
return 120
107
108
def close(self):
109
super(AutoTestCopter, self).close()
110
111
# [2014/05/07] FC Because I'm doing a cross machine build
112
# (source is on host, build is on guest VM) I cannot hard link
113
# This flag tells me that I need to copy the data out
114
if self.copy_tlog:
115
shutil.copy(self.logfile, self.buildlog)
116
117
def is_copter(self):
118
return True
119
120
def get_stick_arming_channel(self):
121
return int(self.get_parameter("RCMAP_YAW"))
122
123
def get_disarm_delay(self):
124
return int(self.get_parameter("DISARM_DELAY"))
125
126
def set_autodisarm_delay(self, delay):
127
self.set_parameter("DISARM_DELAY", delay)
128
129
def takeoff(self,
130
alt_min=30,
131
takeoff_throttle=1700,
132
require_absolute=True,
133
mode="STABILIZE",
134
timeout=120,
135
max_err=5):
136
"""Takeoff get to 30m altitude."""
137
self.progress("TAKEOFF")
138
self.change_mode(mode)
139
if not self.armed():
140
self.wait_ready_to_arm(require_absolute=require_absolute, timeout=timeout)
141
self.zero_throttle()
142
self.arm_vehicle()
143
if mode == 'GUIDED':
144
self.user_takeoff(alt_min=alt_min, timeout=timeout, max_err=max_err)
145
else:
146
self.set_rc(3, takeoff_throttle)
147
self.wait_altitude(alt_min-1, alt_min+max_err, relative=True, timeout=timeout)
148
self.hover()
149
self.progress("TAKEOFF COMPLETE")
150
151
def land_and_disarm(self, timeout=60):
152
"""Land the quad."""
153
self.progress("STARTING LANDING")
154
self.change_mode("LAND")
155
self.wait_landed_and_disarmed(timeout=timeout)
156
157
def wait_landed_and_disarmed(self, min_alt=6, timeout=60):
158
"""Wait to be landed and disarmed"""
159
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
160
alt = m.relative_alt / 1000.0 # mm -> m
161
if alt > min_alt:
162
self.wait_altitude(min_alt-1, min_alt+5, relative=True, timeout=timeout)
163
# self.wait_statustext("SIM Hit ground", timeout=timeout)
164
self.wait_disarmed()
165
166
def hover(self, hover_throttle=1500):
167
self.set_rc(3, hover_throttle)
168
169
# Climb/descend to a given altitude
170
def setAlt(self, desiredAlt=50):
171
pos = self.mav.location(relative_alt=True)
172
if pos.alt > desiredAlt:
173
self.set_rc(3, 1300)
174
self.wait_altitude((desiredAlt-5), desiredAlt, relative=True)
175
if pos.alt < (desiredAlt-5):
176
self.set_rc(3, 1800)
177
self.wait_altitude((desiredAlt-5), desiredAlt, relative=True)
178
self.hover()
179
180
# Takeoff, climb to given altitude, and fly east for 10 seconds
181
def takeoffAndMoveAway(self, dAlt=50, dDist=50):
182
self.progress("Centering sticks")
183
self.set_rc_from_map({
184
1: 1500,
185
2: 1500,
186
3: 1000,
187
4: 1500,
188
})
189
self.takeoff(alt_min=dAlt, mode='GUIDED')
190
self.change_mode("ALT_HOLD")
191
192
self.progress("Yaw to east")
193
self.set_rc(4, 1580)
194
self.wait_heading(90)
195
self.set_rc(4, 1500)
196
197
self.progress("Fly eastbound away from home")
198
self.set_rc(2, 1800)
199
self.delay_sim_time(10)
200
self.set_rc(2, 1500)
201
self.hover()
202
self.progress("Copter staging 50 meters east of home at 50 meters altitude In mode Alt Hold")
203
204
# loiter - fly south west, then loiter within 5m position and altitude
205
def ModeLoiter(self, holdtime=10, maxaltchange=5, maxdistchange=5):
206
"""Hold loiter position."""
207
self.takeoff(10, mode="LOITER")
208
209
# first aim south east
210
self.progress("turn south east")
211
self.set_rc(4, 1580)
212
self.wait_heading(170)
213
self.set_rc(4, 1500)
214
215
# fly south east 50m
216
self.set_rc(2, 1100)
217
self.wait_distance(50)
218
self.set_rc(2, 1500)
219
220
# wait for copter to slow moving
221
self.wait_groundspeed(0, 2)
222
223
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
224
start_altitude = m.alt
225
start = self.mav.location()
226
tstart = self.get_sim_time()
227
self.progress("Holding loiter at %u meters for %u seconds" %
228
(start_altitude, holdtime))
229
while self.get_sim_time_cached() < tstart + holdtime:
230
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
231
pos = self.mav.location()
232
delta = self.get_distance(start, pos)
233
alt_delta = math.fabs(m.alt - start_altitude)
234
self.progress("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt))
235
if alt_delta > maxaltchange:
236
raise NotAchievedException(
237
"Loiter alt shifted %u meters (> limit %u)" %
238
(alt_delta, maxaltchange))
239
if delta > maxdistchange:
240
raise NotAchievedException(
241
"Loiter shifted %u meters (> limit of %u)" %
242
(delta, maxdistchange))
243
self.progress("Loiter OK for %u seconds" % holdtime)
244
245
self.progress("Climb to 30m")
246
self.change_alt(30)
247
248
self.progress("Descend to 20m")
249
self.change_alt(20)
250
251
self.do_RTL()
252
253
def ModeAltHold(self):
254
'''Test AltHold Mode'''
255
self.takeoff(10, mode="ALT_HOLD")
256
self.watch_altitude_maintained(altitude_min=9, altitude_max=11)
257
# feed in full elevator and aileron input and make sure we
258
# retain altitude:
259
self.set_rc_from_map({
260
1: 1000,
261
2: 1000,
262
})
263
self.watch_altitude_maintained(altitude_min=9, altitude_max=11)
264
self.set_rc_from_map({
265
1: 1500,
266
2: 1500,
267
})
268
self.do_RTL()
269
270
def fly_to_origin(self, final_alt=10):
271
origin = self.poll_message("GPS_GLOBAL_ORIGIN")
272
self.change_mode("GUIDED")
273
self.guided_move_global_relative_alt(origin.latitude,
274
origin.longitude,
275
final_alt)
276
277
def change_alt(self, alt_min, climb_throttle=1920, descend_throttle=1080):
278
"""Change altitude."""
279
def adjust_altitude(current_alt, target_alt, accuracy):
280
if math.fabs(current_alt - target_alt) <= accuracy:
281
self.hover()
282
elif current_alt < target_alt:
283
self.set_rc(3, climb_throttle)
284
else:
285
self.set_rc(3, descend_throttle)
286
self.wait_altitude(
287
(alt_min - 5),
288
alt_min,
289
relative=True,
290
called_function=lambda current_alt, target_alt: adjust_altitude(current_alt, target_alt, 1)
291
)
292
self.hover()
293
294
def RecordThenPlayMission(self, side=50, timeout=300):
295
'''Use switches to toggle in mission, then fly it'''
296
self.takeoff(20, mode="ALT_HOLD")
297
298
"""Fly a square, flying N then E ."""
299
tstart = self.get_sim_time()
300
301
# ensure all sticks in the middle
302
self.set_rc_from_map({
303
1: 1500,
304
2: 1500,
305
3: 1500,
306
4: 1500,
307
})
308
309
# switch to loiter mode temporarily to stop us from rising
310
self.change_mode('LOITER')
311
312
# first aim north
313
self.progress("turn right towards north")
314
self.set_rc(4, 1580)
315
self.wait_heading(10)
316
self.set_rc(4, 1500)
317
318
# save bottom left corner of box as waypoint
319
self.progress("Save WP 1 & 2")
320
self.save_wp()
321
322
# switch back to ALT_HOLD mode
323
self.change_mode('ALT_HOLD')
324
325
# pitch forward to fly north
326
self.progress("Going north %u meters" % side)
327
self.set_rc(2, 1300)
328
self.wait_distance(side)
329
self.set_rc(2, 1500)
330
331
# save top left corner of square as waypoint
332
self.progress("Save WP 3")
333
self.save_wp()
334
335
# roll right to fly east
336
self.progress("Going east %u meters" % side)
337
self.set_rc(1, 1700)
338
self.wait_distance(side)
339
self.set_rc(1, 1500)
340
341
# save top right corner of square as waypoint
342
self.progress("Save WP 4")
343
self.save_wp()
344
345
# pitch back to fly south
346
self.progress("Going south %u meters" % side)
347
self.set_rc(2, 1700)
348
self.wait_distance(side)
349
self.set_rc(2, 1500)
350
351
# save bottom right corner of square as waypoint
352
self.progress("Save WP 5")
353
self.save_wp()
354
355
# roll left to fly west
356
self.progress("Going west %u meters" % side)
357
self.set_rc(1, 1300)
358
self.wait_distance(side)
359
self.set_rc(1, 1500)
360
361
# save bottom left corner of square (should be near home) as waypoint
362
self.progress("Save WP 6")
363
self.save_wp()
364
365
# reduce throttle again
366
self.set_rc(3, 1500)
367
368
# descend to 10m
369
self.progress("Descend to 10m in Loiter")
370
self.change_mode('LOITER')
371
self.set_rc(3, 1200)
372
time_left = timeout - (self.get_sim_time() - tstart)
373
self.progress("timeleft = %u" % time_left)
374
if time_left < 20:
375
time_left = 20
376
self.wait_altitude(-10, 10, timeout=time_left, relative=True)
377
self.set_rc(3, 1500)
378
self.save_wp()
379
380
# save the stored mission to file
381
mavproxy = self.start_mavproxy()
382
num_wp = self.save_mission_to_file_using_mavproxy(
383
mavproxy,
384
os.path.join(testdir, "ch7_mission.txt"))
385
self.stop_mavproxy(mavproxy)
386
if not num_wp:
387
raise NotAchievedException("save_mission_to_file failed")
388
389
self.progress("test: Fly a mission from 1 to %u" % num_wp)
390
self.change_mode('AUTO')
391
self.set_current_waypoint(1)
392
self.wait_waypoint(0, num_wp-1, timeout=500)
393
self.progress("test: MISSION COMPLETE: passed!")
394
self.land_and_disarm()
395
396
# enter RTL mode and wait for the vehicle to disarm
397
def do_RTL(self, distance_min=None, check_alt=True, distance_max=10, timeout=250, quiet=False):
398
"""Enter RTL mode and wait for the vehicle to disarm at Home."""
399
self.change_mode("RTL")
400
self.hover()
401
self.wait_rtl_complete(check_alt=check_alt, distance_max=distance_max, timeout=timeout, quiet=True)
402
403
def wait_rtl_complete(self, check_alt=True, distance_max=10, timeout=250, quiet=False):
404
"""Wait for RTL to reach home and disarm"""
405
self.progress("Waiting RTL to reach Home and disarm")
406
tstart = self.get_sim_time()
407
while self.get_sim_time_cached() < tstart + timeout:
408
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
409
alt = m.relative_alt / 1000.0 # mm -> m
410
home_distance = self.distance_to_home(use_cached_home=True)
411
home = ""
412
alt_valid = alt <= 1
413
distance_valid = home_distance < distance_max
414
if check_alt:
415
if alt_valid and distance_valid:
416
home = "HOME"
417
else:
418
if distance_valid:
419
home = "HOME"
420
if not quiet:
421
self.progress("Alt: %.02f HomeDist: %.02f %s" %
422
(alt, home_distance, home))
423
424
# our post-condition is that we are disarmed:
425
if not self.armed():
426
if home == "":
427
raise NotAchievedException("Did not get home")
428
# success!
429
return
430
431
raise AutoTestTimeoutException("Did not get home and disarm")
432
433
def LoiterToAlt(self):
434
"""Loiter-To-Alt"""
435
436
self.context_push()
437
438
self.set_parameters({
439
"PLND_ENABLED": 1,
440
"PLND_TYPE": 4,
441
})
442
443
self.set_analog_rangefinder_parameters()
444
445
self.reboot_sitl()
446
447
num_wp = self.load_mission("copter_loiter_to_alt.txt")
448
449
self.change_mode('LOITER')
450
451
self.install_terrain_handlers_context()
452
self.wait_ready_to_arm()
453
454
self.arm_vehicle()
455
456
self.change_mode('AUTO')
457
458
self.set_rc(3, 1550)
459
460
self.wait_current_waypoint(2)
461
462
self.set_rc(3, 1500)
463
464
self.wait_waypoint(0, num_wp-1, timeout=500)
465
466
self.wait_disarmed()
467
468
self.context_pop()
469
self.reboot_sitl()
470
471
# Tests all actions and logic behind the radio failsafe
472
def ThrottleFailsafe(self, side=60, timeout=360):
473
'''Test Throttle Failsafe'''
474
self.start_subtest("If you haven't taken off yet RC failure should be instant disarm")
475
self.change_mode("STABILIZE")
476
self.set_parameter("DISARM_DELAY", 0)
477
self.arm_vehicle()
478
self.set_parameter("SIM_RC_FAIL", 1)
479
self.disarm_wait(timeout=1)
480
self.set_parameter("SIM_RC_FAIL", 0)
481
self.set_parameter("DISARM_DELAY", 10)
482
483
# Trigger an RC failure with the failsafe disabled. Verify no action taken.
484
self.start_subtest("Radio failsafe disabled test: FS_THR_ENABLE=0 should take no failsafe action")
485
self.set_parameter('FS_THR_ENABLE', 0)
486
self.set_parameter('FS_OPTIONS', 0)
487
self.takeoffAndMoveAway()
488
self.set_parameter("SIM_RC_FAIL", 1)
489
self.delay_sim_time(5)
490
self.wait_mode("ALT_HOLD")
491
self.set_parameter("SIM_RC_FAIL", 0)
492
self.delay_sim_time(5)
493
self.wait_mode("ALT_HOLD")
494
self.end_subtest("Completed Radio failsafe disabled test")
495
496
# Trigger an RC failure, verify radio failsafe triggers,
497
# restore radio, verify RC function by changing modes to cicle
498
# and stabilize.
499
self.start_subtest("Radio failsafe recovery test")
500
self.set_parameter('FS_THR_ENABLE', 1)
501
self.set_parameter("SIM_RC_FAIL", 1)
502
self.wait_mode("RTL")
503
self.delay_sim_time(5)
504
self.set_parameter("SIM_RC_FAIL", 0)
505
self.delay_sim_time(5)
506
self.set_rc(5, 1050)
507
self.wait_mode("CIRCLE")
508
self.set_rc(5, 1950)
509
self.wait_mode("STABILIZE")
510
self.end_subtest("Completed Radio failsafe recovery test")
511
512
# Trigger and RC failure, verify failsafe triggers and RTL completes
513
self.start_subtest("Radio failsafe RTL with no options test: FS_THR_ENABLE=1 & FS_OPTIONS=0")
514
self.set_parameter("SIM_RC_FAIL", 1)
515
self.wait_mode("RTL")
516
self.wait_rtl_complete()
517
self.set_parameter("SIM_RC_FAIL", 0)
518
self.end_subtest("Completed Radio failsafe RTL with no options test")
519
520
# Trigger and RC failure, verify failsafe triggers and land completes
521
self.start_subtest("Radio failsafe LAND with no options test: FS_THR_ENABLE=3 & FS_OPTIONS=0")
522
self.set_parameter('FS_THR_ENABLE', 3)
523
self.takeoffAndMoveAway()
524
self.set_parameter("SIM_RC_FAIL", 1)
525
self.wait_mode("LAND")
526
self.wait_landed_and_disarmed()
527
self.set_parameter("SIM_RC_FAIL", 0)
528
self.end_subtest("Completed Radio failsafe LAND with no options test")
529
530
# Trigger and RC failure, verify failsafe triggers and SmartRTL completes
531
self.start_subtest("Radio failsafe SmartRTL->RTL with no options test: FS_THR_ENABLE=4 & FS_OPTIONS=0")
532
self.set_parameter('FS_THR_ENABLE', 4)
533
self.takeoffAndMoveAway()
534
self.set_parameter("SIM_RC_FAIL", 1)
535
self.wait_mode("SMART_RTL")
536
self.wait_disarmed()
537
self.set_parameter("SIM_RC_FAIL", 0)
538
self.end_subtest("Completed Radio failsafe SmartRTL->RTL with no options test")
539
540
# Trigger and RC failure, verify failsafe triggers and SmartRTL completes
541
self.start_subtest("Radio failsafe SmartRTL->Land with no options test: FS_THR_ENABLE=5 & FS_OPTIONS=0")
542
self.set_parameter('FS_THR_ENABLE', 5)
543
self.takeoffAndMoveAway()
544
self.set_parameter("SIM_RC_FAIL", 1)
545
self.wait_mode("SMART_RTL")
546
self.wait_disarmed()
547
self.set_parameter("SIM_RC_FAIL", 0)
548
self.end_subtest("Completed Radio failsafe SmartRTL_Land with no options test")
549
550
# Trigger a GPS failure and RC failure, verify RTL fails into
551
# land mode and completes
552
self.start_subtest("Radio failsafe RTL fails into land mode due to bad position.")
553
self.set_parameter('FS_THR_ENABLE', 1)
554
self.takeoffAndMoveAway()
555
self.set_parameter('SIM_GPS1_ENABLE', 0)
556
self.delay_sim_time(5)
557
self.set_parameter("SIM_RC_FAIL", 1)
558
self.wait_mode("LAND")
559
self.wait_landed_and_disarmed()
560
self.set_parameter("SIM_RC_FAIL", 0)
561
self.set_parameter('SIM_GPS1_ENABLE', 1)
562
self.wait_ekf_happy()
563
self.end_subtest("Completed Radio failsafe RTL fails into land mode due to bad position.")
564
565
# Trigger a GPS failure and RC failure, verify SmartRTL fails
566
# into land mode and completes
567
self.start_subtest("Radio failsafe SmartRTL->RTL fails into land mode due to bad position.")
568
self.set_parameter('FS_THR_ENABLE', 4)
569
self.takeoffAndMoveAway()
570
self.set_parameter('SIM_GPS1_ENABLE', 0)
571
self.delay_sim_time(5)
572
self.set_parameter("SIM_RC_FAIL", 1)
573
self.wait_mode("LAND")
574
self.wait_landed_and_disarmed()
575
self.set_parameter("SIM_RC_FAIL", 0)
576
self.set_parameter('SIM_GPS1_ENABLE', 1)
577
self.wait_ekf_happy()
578
self.end_subtest("Completed Radio failsafe SmartRTL->RTL fails into land mode due to bad position.")
579
580
# Trigger a GPS failure and RC failure, verify SmartRTL fails
581
# into land mode and completes
582
self.start_subtest("Radio failsafe SmartRTL->LAND fails into land mode due to bad position.")
583
self.set_parameter('FS_THR_ENABLE', 5)
584
self.takeoffAndMoveAway()
585
self.set_parameter('SIM_GPS1_ENABLE', 0)
586
self.delay_sim_time(5)
587
self.set_parameter("SIM_RC_FAIL", 1)
588
self.wait_mode("LAND")
589
self.wait_landed_and_disarmed()
590
self.set_parameter("SIM_RC_FAIL", 0)
591
self.set_parameter('SIM_GPS1_ENABLE', 1)
592
self.wait_ekf_happy()
593
self.end_subtest("Completed Radio failsafe SmartRTL->LAND fails into land mode due to bad position.")
594
595
# Trigger a GPS failure, then restore the GPS. Trigger an RC
596
# failure, verify SmartRTL fails into RTL and completes
597
self.start_subtest("Radio failsafe SmartRTL->RTL fails into RTL mode due to no path.")
598
self.set_parameter('FS_THR_ENABLE', 4)
599
self.takeoffAndMoveAway()
600
self.set_parameter('SIM_GPS1_ENABLE', 0)
601
self.wait_statustext("SmartRTL deactivated: bad position", timeout=60)
602
self.set_parameter('SIM_GPS1_ENABLE', 1)
603
self.wait_ekf_happy()
604
self.delay_sim_time(5)
605
self.set_parameter("SIM_RC_FAIL", 1)
606
self.wait_mode("RTL")
607
self.wait_rtl_complete()
608
self.set_parameter("SIM_RC_FAIL", 0)
609
self.end_subtest("Completed Radio failsafe SmartRTL->RTL fails into RTL mode due to no path.")
610
611
# Trigger a GPS failure, then restore the GPS. Trigger an RC
612
# failure, verify SmartRTL fails into Land and completes
613
self.start_subtest("Radio failsafe SmartRTL->LAND fails into land mode due to no path.")
614
self.set_parameter('FS_THR_ENABLE', 5)
615
self.takeoffAndMoveAway()
616
self.set_parameter('SIM_GPS1_ENABLE', 0)
617
self.wait_statustext("SmartRTL deactivated: bad position", timeout=60)
618
self.set_parameter('SIM_GPS1_ENABLE', 1)
619
self.wait_ekf_happy()
620
self.delay_sim_time(5)
621
self.set_parameter("SIM_RC_FAIL", 1)
622
self.wait_mode("LAND")
623
self.wait_landed_and_disarmed()
624
self.set_parameter("SIM_RC_FAIL", 0)
625
self.end_subtest("Completed Radio failsafe SmartRTL->LAND fails into land mode due to no path.")
626
627
# Trigger an RC failure in guided mode with the option enabled
628
# to continue in guided. Verify no failsafe action takes place
629
self.start_subtest("Radio failsafe with option to continue in guided mode: FS_THR_ENABLE=1 & FS_OPTIONS=4")
630
self.set_parameter("SYSID_MYGCS", self.mav.source_system)
631
self.setGCSfailsafe(1)
632
self.set_parameter('FS_THR_ENABLE', 1)
633
self.set_parameter('FS_OPTIONS', 4)
634
self.takeoffAndMoveAway()
635
self.change_mode("GUIDED")
636
self.set_parameter("SIM_RC_FAIL", 1)
637
self.delay_sim_time(5)
638
self.wait_mode("GUIDED")
639
self.set_parameter("SIM_RC_FAIL", 0)
640
self.delay_sim_time(5)
641
self.change_mode("ALT_HOLD")
642
self.setGCSfailsafe(0)
643
# self.change_mode("RTL")
644
# self.wait_disarmed()
645
self.end_subtest("Completed Radio failsafe with option to continue in guided mode")
646
647
# Trigger an RC failure in AUTO mode with the option enabled
648
# to continue the mission. Verify no failsafe action takes
649
# place
650
self.start_subtest("Radio failsafe RTL with option to continue mission: FS_THR_ENABLE=1 & FS_OPTIONS=1")
651
self.set_parameter('FS_OPTIONS', 1)
652
self.progress("# Load copter_mission")
653
num_wp = self.load_mission("copter_mission.txt", strict=False)
654
if not num_wp:
655
raise NotAchievedException("load copter_mission failed")
656
# self.takeoffAndMoveAway()
657
self.change_mode("AUTO")
658
self.set_parameter("SIM_RC_FAIL", 1)
659
self.delay_sim_time(5)
660
self.wait_mode("AUTO")
661
self.set_parameter("SIM_RC_FAIL", 0)
662
self.delay_sim_time(5)
663
self.wait_mode("AUTO")
664
# self.change_mode("RTL")
665
# self.wait_disarmed()
666
self.end_subtest("Completed Radio failsafe RTL with option to continue mission")
667
668
# Trigger an RC failure in AUTO mode without the option
669
# enabled to continue. Verify failsafe triggers and RTL
670
# completes
671
self.start_subtest("Radio failsafe RTL in mission without "
672
"option to continue should RTL: FS_THR_ENABLE=1 & FS_OPTIONS=0")
673
self.set_parameter('FS_OPTIONS', 0)
674
self.set_parameter("SIM_RC_FAIL", 1)
675
self.wait_mode("RTL")
676
self.wait_rtl_complete()
677
self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
678
self.set_parameter("SIM_RC_FAIL", 0)
679
self.end_subtest("Completed Radio failsafe RTL in mission without option to continue")
680
681
self.progress("All radio failsafe tests complete")
682
self.set_parameter('FS_THR_ENABLE', 0)
683
self.reboot_sitl()
684
685
def ThrottleFailsafePassthrough(self):
686
'''check servo passthrough on RC failsafe. Make sure it doesn't glitch to the bad RC input value'''
687
channel = 7
688
trim_value = 1450
689
self.set_parameters({
690
'RC%u_MIN' % channel: 1000,
691
'RC%u_MAX' % channel: 2000,
692
'SERVO%u_MIN' % channel: 1000,
693
'SERVO%u_MAX' % channel: 2000,
694
'SERVO%u_TRIM' % channel: trim_value,
695
'SERVO%u_FUNCTION' % channel: 146, # scaled passthrough for channel 7
696
'FS_THR_ENABLE': 1,
697
'RC_FS_TIMEOUT': 10,
698
'SERVO_RC_FS_MSK': 1 << (channel-1),
699
})
700
701
self.reboot_sitl()
702
703
self.context_set_message_rate_hz('SERVO_OUTPUT_RAW', 200)
704
705
self.set_rc(channel, 1799)
706
expected_servo_output_value = 1778 # 1778 because of weird trim
707
self.wait_servo_channel_value(channel, expected_servo_output_value)
708
# receiver goes into failsafe with wild override values:
709
710
def ensure_SERVO_values_never_input(mav, m):
711
if m.get_type() != "SERVO_OUTPUT_RAW":
712
return
713
value = getattr(m, "servo%u_raw" % channel)
714
if value != expected_servo_output_value and value != trim_value:
715
raise NotAchievedException("Bad servo value %u received" % value)
716
717
self.install_message_hook_context(ensure_SERVO_values_never_input)
718
self.progress("Forcing receiver into failsafe")
719
self.set_rc_from_map({
720
3: 800,
721
channel: 1300,
722
})
723
self.wait_servo_channel_value(channel, trim_value)
724
self.delay_sim_time(10)
725
726
# Tests all actions and logic behind the GCS failsafe
727
def GCSFailsafe(self, side=60, timeout=360):
728
'''Test GCS Failsafe'''
729
try:
730
self.test_gcs_failsafe(side=side, timeout=timeout)
731
except Exception as ex:
732
self.setGCSfailsafe(0)
733
self.set_parameter('FS_OPTIONS', 0)
734
self.disarm_vehicle(force=True)
735
self.reboot_sitl()
736
raise ex
737
738
def test_gcs_failsafe(self, side=60, timeout=360):
739
# Test double-SmartRTL; ensure we do SmarRTL twice rather than
740
# landing (tests fix for actual bug)
741
self.set_parameter("SYSID_MYGCS", self.mav.source_system)
742
self.context_push()
743
self.start_subtest("GCS failsafe SmartRTL twice")
744
self.setGCSfailsafe(3)
745
self.set_parameter('FS_OPTIONS', 8)
746
self.takeoffAndMoveAway()
747
self.set_heartbeat_rate(0)
748
self.wait_mode("SMART_RTL")
749
self.wait_disarmed()
750
self.set_heartbeat_rate(self.speedup)
751
self.wait_statustext("GCS Failsafe Cleared", timeout=60)
752
753
self.takeoffAndMoveAway()
754
self.set_heartbeat_rate(0)
755
self.wait_statustext("GCS Failsafe")
756
757
def ensure_smartrtl(mav, m):
758
if m.get_type() != "HEARTBEAT":
759
return
760
# can't use mode_is here because we're in the message hook
761
print("Mode: %s" % self.mav.flightmode)
762
if self.mav.flightmode != "SMART_RTL":
763
raise NotAchievedException("Not in SMART_RTL")
764
self.install_message_hook_context(ensure_smartrtl)
765
766
self.set_heartbeat_rate(self.speedup)
767
self.wait_statustext("GCS Failsafe Cleared", timeout=60)
768
self.set_heartbeat_rate(0)
769
self.wait_statustext("GCS Failsafe")
770
771
self.wait_disarmed()
772
773
self.end_subtest("GCS failsafe SmartRTL twice")
774
self.set_heartbeat_rate(self.speedup)
775
self.wait_statustext("GCS Failsafe Cleared", timeout=60)
776
self.context_pop()
777
778
# Trigger telemetry loss with failsafe disabled. Verify no action taken.
779
self.start_subtest("GCS failsafe disabled test: FS_GCS_ENABLE=0 should take no failsafe action")
780
self.setGCSfailsafe(0)
781
self.takeoffAndMoveAway()
782
self.set_heartbeat_rate(0)
783
self.delay_sim_time(5)
784
self.wait_mode("ALT_HOLD")
785
self.set_heartbeat_rate(self.speedup)
786
self.delay_sim_time(5)
787
self.wait_mode("ALT_HOLD")
788
self.end_subtest("Completed GCS failsafe disabled test")
789
790
# Trigger telemetry loss with failsafe enabled. Verify
791
# failsafe triggers to RTL. Restore telemetry, verify failsafe
792
# clears, and change modes.
793
self.start_subtest("GCS failsafe recovery test: FS_GCS_ENABLE=1 & FS_OPTIONS=0")
794
self.setGCSfailsafe(1)
795
self.set_parameter('FS_OPTIONS', 0)
796
self.set_heartbeat_rate(0)
797
self.wait_mode("RTL")
798
self.set_heartbeat_rate(self.speedup)
799
self.wait_statustext("GCS Failsafe Cleared", timeout=60)
800
self.change_mode("LOITER")
801
self.end_subtest("Completed GCS failsafe recovery test")
802
803
# Trigger telemetry loss with failsafe enabled. Verify
804
# failsafe triggers to RTL. Restore telemetry, verify failsafe
805
# clears, and change modes.
806
self.start_subtest("GCS failsafe recovery test: FS_GCS_ENABLE=1 & FS_OPTIONS=0 & FS_GCS_TIMEOUT=10")
807
self.setGCSfailsafe(1)
808
self.set_parameter('FS_OPTIONS', 0)
809
old_gcs_timeout = self.get_parameter("FS_GCS_TIMEOUT")
810
new_gcs_timeout = old_gcs_timeout * 2
811
self.set_parameter("FS_GCS_TIMEOUT", new_gcs_timeout)
812
self.set_heartbeat_rate(0)
813
self.delay_sim_time(old_gcs_timeout + (new_gcs_timeout - old_gcs_timeout) / 2)
814
self.assert_mode("LOITER")
815
self.wait_mode("RTL")
816
self.set_heartbeat_rate(self.speedup)
817
self.wait_statustext("GCS Failsafe Cleared", timeout=60)
818
self.change_mode("LOITER")
819
self.set_parameter('FS_GCS_TIMEOUT', old_gcs_timeout)
820
self.end_subtest("Completed GCS failsafe recovery test")
821
822
# Trigger telemetry loss with failsafe enabled. Verify failsafe triggers and RTL completes
823
self.start_subtest("GCS failsafe RTL with no options test: FS_GCS_ENABLE=1 & FS_OPTIONS=0")
824
self.setGCSfailsafe(1)
825
self.set_parameter('FS_OPTIONS', 0)
826
self.set_heartbeat_rate(0)
827
self.wait_mode("RTL")
828
self.wait_rtl_complete()
829
self.set_heartbeat_rate(self.speedup)
830
self.wait_statustext("GCS Failsafe Cleared", timeout=60)
831
self.end_subtest("Completed GCS failsafe RTL with no options test")
832
833
# Trigger telemetry loss with failsafe enabled. Verify failsafe triggers and land completes
834
self.start_subtest("GCS failsafe LAND with no options test: FS_GCS_ENABLE=5 & FS_OPTIONS=0")
835
self.setGCSfailsafe(5)
836
self.takeoffAndMoveAway()
837
self.set_heartbeat_rate(0)
838
self.wait_mode("LAND")
839
self.wait_landed_and_disarmed()
840
self.set_heartbeat_rate(self.speedup)
841
self.wait_statustext("GCS Failsafe Cleared", timeout=60)
842
self.end_subtest("Completed GCS failsafe land with no options test")
843
844
# Trigger telemetry loss with failsafe enabled. Verify failsafe triggers and SmartRTL completes
845
self.start_subtest("GCS failsafe SmartRTL->RTL with no options test: FS_GCS_ENABLE=3 & FS_OPTIONS=0")
846
self.setGCSfailsafe(3)
847
self.takeoffAndMoveAway()
848
self.set_heartbeat_rate(0)
849
self.wait_mode("SMART_RTL")
850
self.wait_disarmed()
851
self.set_heartbeat_rate(self.speedup)
852
self.wait_statustext("GCS Failsafe Cleared", timeout=60)
853
self.end_subtest("Completed GCS failsafe SmartRTL->RTL with no options test")
854
855
# Trigger telemetry loss with failsafe enabled. Verify failsafe triggers and SmartRTL completes
856
self.start_subtest("GCS failsafe SmartRTL->Land with no options test: FS_GCS_ENABLE=4 & FS_OPTIONS=0")
857
self.setGCSfailsafe(4)
858
self.takeoffAndMoveAway()
859
self.set_heartbeat_rate(0)
860
self.wait_mode("SMART_RTL")
861
self.wait_disarmed()
862
self.set_heartbeat_rate(self.speedup)
863
self.wait_statustext("GCS Failsafe Cleared", timeout=60)
864
self.end_subtest("Completed GCS failsafe SmartRTL->Land with no options test")
865
866
# Trigger telemetry loss with an invalid failsafe value. Verify failsafe triggers and RTL completes
867
self.start_subtest("GCS failsafe invalid value with no options test: FS_GCS_ENABLE=99 & FS_OPTIONS=0")
868
self.setGCSfailsafe(99)
869
self.takeoffAndMoveAway()
870
self.set_heartbeat_rate(0)
871
self.wait_mode("RTL")
872
self.wait_rtl_complete()
873
self.set_heartbeat_rate(self.speedup)
874
self.wait_statustext("GCS Failsafe Cleared", timeout=60)
875
self.end_subtest("Completed GCS failsafe invalid value with no options test")
876
877
# Trigger telemetry loss with failsafe enabled to test FS_OPTIONS settings
878
self.start_subtest("GCS failsafe with option bit tests: FS_GCS_ENABLE=1 & FS_OPTIONS=64/2/16")
879
num_wp = self.load_mission("copter_mission.txt", strict=False)
880
if not num_wp:
881
raise NotAchievedException("load copter_mission failed")
882
self.setGCSfailsafe(1)
883
self.set_parameter('FS_OPTIONS', 16)
884
self.takeoffAndMoveAway()
885
self.progress("Testing continue in pilot controlled modes")
886
self.set_heartbeat_rate(0)
887
self.wait_statustext("GCS Failsafe - Continuing Pilot Control", timeout=60)
888
self.delay_sim_time(5)
889
self.wait_mode("ALT_HOLD")
890
self.set_heartbeat_rate(self.speedup)
891
self.wait_statustext("GCS Failsafe Cleared", timeout=60)
892
893
self.progress("Testing continue in auto mission")
894
self.set_parameter('FS_OPTIONS', 2)
895
self.change_mode("AUTO")
896
self.delay_sim_time(5)
897
self.set_heartbeat_rate(0)
898
self.wait_statustext("GCS Failsafe - Continuing Auto Mode", timeout=60)
899
self.delay_sim_time(5)
900
self.wait_mode("AUTO")
901
self.set_heartbeat_rate(self.speedup)
902
self.wait_statustext("GCS Failsafe Cleared", timeout=60)
903
904
self.progress("Testing continue landing in land mode")
905
self.set_parameter('FS_OPTIONS', 8)
906
self.change_mode("LAND")
907
self.delay_sim_time(5)
908
self.set_heartbeat_rate(0)
909
self.wait_statustext("GCS Failsafe - Continuing Landing", timeout=60)
910
self.delay_sim_time(5)
911
self.wait_mode("LAND")
912
self.wait_landed_and_disarmed()
913
self.set_heartbeat_rate(self.speedup)
914
self.wait_statustext("GCS Failsafe Cleared", timeout=60)
915
self.end_subtest("Completed GCS failsafe with option bits")
916
917
self.setGCSfailsafe(0)
918
self.set_parameter('FS_OPTIONS', 0)
919
self.progress("All GCS failsafe tests complete")
920
921
def CustomController(self, timeout=300):
922
'''Test Custom Controller'''
923
self.progress("Configure custom controller parameters")
924
self.set_parameters({
925
'CC_TYPE': 2,
926
'CC_AXIS_MASK': 7,
927
'RC6_OPTION': 109,
928
})
929
self.set_rc(6, 1000)
930
self.reboot_sitl()
931
932
if self.get_parameter("CC_TYPE") != 2 :
933
raise NotAchievedException("Custom controller is not switched to PID backend.")
934
935
# check if we can retrive any param inside PID backend
936
self.get_parameter("CC2_RAT_YAW_P")
937
938
# takeoff in GPS mode and switch to CIRCLE
939
self.takeoff(10, mode="LOITER", takeoff_throttle=2000)
940
self.change_mode("CIRCLE")
941
942
self.context_push()
943
self.context_collect('STATUSTEXT')
944
945
# switch custom controller on
946
self.set_rc(6, 2000)
947
self.wait_statustext("Custom controller is ON", check_context=True)
948
949
# wait 20 second to see if the custom controller destabilize the aircraft
950
if self.wait_altitude(7, 13, relative=True, minimum_duration=20) :
951
raise NotAchievedException("Custom controller is not stable.")
952
953
# switch custom controller off
954
self.set_rc(6, 1000)
955
self.wait_statustext("Custom controller is OFF", check_context=True)
956
957
self.context_pop()
958
self.do_RTL()
959
self.progress("Custom controller test complete")
960
961
# Tests all actions and logic behind the battery failsafe
962
def BatteryFailsafe(self, timeout=300):
963
'''Fly Battery Failsafe'''
964
self.progress("Configure battery failsafe parameters")
965
self.set_parameters({
966
'SIM_SPEEDUP': 4,
967
'BATT_LOW_VOLT': 11.5,
968
'BATT_CRT_VOLT': 10.1,
969
'BATT_FS_LOW_ACT': 0,
970
'BATT_FS_CRT_ACT': 0,
971
'FS_OPTIONS': 0,
972
'SIM_BATT_VOLTAGE': 12.5,
973
})
974
975
# Trigger low battery condition with failsafe disabled. Verify
976
# no action taken.
977
self.start_subtest("Batt failsafe disabled test")
978
self.takeoffAndMoveAway()
979
m = self.mav.recv_match(type='BATTERY_STATUS', blocking=True, timeout=1)
980
if m.charge_state != mavutil.mavlink.MAV_BATTERY_CHARGE_STATE_OK:
981
raise NotAchievedException("Expected state ok")
982
self.set_parameter('SIM_BATT_VOLTAGE', 11.4)
983
self.wait_statustext("Battery 1 is low", timeout=60)
984
m = self.mav.recv_match(type='BATTERY_STATUS', blocking=True, timeout=1)
985
if m.charge_state != mavutil.mavlink.MAV_BATTERY_CHARGE_STATE_LOW:
986
raise NotAchievedException("Expected state low")
987
self.delay_sim_time(5)
988
self.wait_mode("ALT_HOLD")
989
self.set_parameter('SIM_BATT_VOLTAGE', 10.0)
990
self.wait_statustext("Battery 1 is critical", timeout=60)
991
m = self.mav.recv_match(type='BATTERY_STATUS', blocking=True, timeout=1)
992
if m.charge_state != mavutil.mavlink.MAV_BATTERY_CHARGE_STATE_CRITICAL:
993
raise NotAchievedException("Expected state critical")
994
self.delay_sim_time(5)
995
self.wait_mode("ALT_HOLD")
996
self.change_mode("RTL")
997
self.wait_rtl_complete()
998
self.set_parameter('SIM_BATT_VOLTAGE', 12.5)
999
self.reboot_sitl()
1000
self.end_subtest("Completed Batt failsafe disabled test")
1001
1002
# TWO STAGE BATTERY FAILSAFE: Trigger low battery condition,
1003
# then critical battery condition. Verify RTL and Land actions
1004
# complete.
1005
self.start_subtest("Two stage battery failsafe test with RTL and Land")
1006
self.takeoffAndMoveAway()
1007
self.delay_sim_time(3)
1008
self.set_parameters({
1009
'BATT_FS_LOW_ACT': 2,
1010
'BATT_FS_CRT_ACT': 1,
1011
'SIM_BATT_VOLTAGE': 11.4,
1012
})
1013
self.wait_statustext("Battery 1 is low", timeout=60)
1014
self.delay_sim_time(5)
1015
self.wait_mode("RTL")
1016
self.delay_sim_time(10)
1017
self.set_parameter('SIM_BATT_VOLTAGE', 10.0)
1018
self.wait_statustext("Battery 1 is critical", timeout=60)
1019
self.delay_sim_time(5)
1020
self.wait_mode("LAND")
1021
self.wait_landed_and_disarmed()
1022
self.set_parameter('SIM_BATT_VOLTAGE', 12.5)
1023
self.reboot_sitl()
1024
self.end_subtest("Completed two stage battery failsafe test with RTL and Land")
1025
1026
# TWO STAGE BATTERY FAILSAFE: Trigger low battery condition,
1027
# then critical battery condition. Verify both SmartRTL
1028
# actions complete
1029
self.start_subtest("Two stage battery failsafe test with SmartRTL")
1030
self.takeoffAndMoveAway()
1031
self.set_parameter('BATT_FS_LOW_ACT', 3)
1032
self.set_parameter('BATT_FS_CRT_ACT', 4)
1033
self.delay_sim_time(10)
1034
self.set_parameter('SIM_BATT_VOLTAGE', 11.4)
1035
self.wait_statustext("Battery 1 is low", timeout=60)
1036
self.delay_sim_time(5)
1037
self.wait_mode("SMART_RTL")
1038
self.change_mode("LOITER")
1039
self.delay_sim_time(10)
1040
self.set_parameter('SIM_BATT_VOLTAGE', 10.0)
1041
self.wait_statustext("Battery 1 is critical", timeout=60)
1042
self.delay_sim_time(5)
1043
self.wait_mode("SMART_RTL")
1044
self.wait_disarmed()
1045
self.set_parameter('SIM_BATT_VOLTAGE', 12.5)
1046
self.reboot_sitl()
1047
self.end_subtest("Completed two stage battery failsafe test with SmartRTL")
1048
1049
# Trigger low battery condition in land mode with FS_OPTIONS
1050
# set to allow land mode to continue. Verify landing completes
1051
# uninterrupted.
1052
self.start_subtest("Battery failsafe with FS_OPTIONS set to continue landing")
1053
self.takeoffAndMoveAway()
1054
self.set_parameter('FS_OPTIONS', 8)
1055
self.change_mode("LAND")
1056
self.delay_sim_time(5)
1057
self.set_parameter('SIM_BATT_VOLTAGE', 11.4)
1058
self.wait_statustext("Battery 1 is low", timeout=60)
1059
self.delay_sim_time(5)
1060
self.wait_mode("LAND")
1061
self.wait_landed_and_disarmed()
1062
self.set_parameter('SIM_BATT_VOLTAGE', 12.5)
1063
self.reboot_sitl()
1064
self.end_subtest("Completed battery failsafe with FS_OPTIONS set to continue landing")
1065
1066
# Trigger a critical battery condition, which triggers a land
1067
# mode failsafe. Trigger an RC failure. Verify the RC failsafe
1068
# is prevented from stopping the low battery landing.
1069
self.start_subtest("Battery failsafe critical landing")
1070
self.takeoffAndMoveAway(100, 50)
1071
self.set_parameters({
1072
'FS_OPTIONS': 0,
1073
'BATT_FS_LOW_ACT': 1,
1074
'BATT_FS_CRT_ACT': 1,
1075
'FS_THR_ENABLE': 1,
1076
})
1077
self.delay_sim_time(5)
1078
self.set_parameter('SIM_BATT_VOLTAGE', 10.0)
1079
self.wait_statustext("Battery 1 is critical", timeout=60)
1080
self.wait_mode("LAND")
1081
self.delay_sim_time(10)
1082
self.set_parameter("SIM_RC_FAIL", 1)
1083
self.delay_sim_time(10)
1084
self.wait_mode("LAND")
1085
self.wait_landed_and_disarmed()
1086
self.set_parameter('SIM_BATT_VOLTAGE', 12.5)
1087
self.set_parameter("SIM_RC_FAIL", 0)
1088
self.reboot_sitl()
1089
self.end_subtest("Completed battery failsafe critical landing")
1090
1091
# Trigger low battery condition with failsafe set to terminate. Copter will disarm and crash.
1092
self.start_subtest("Battery failsafe terminate")
1093
self.takeoffAndMoveAway()
1094
self.set_parameter('BATT_FS_LOW_ACT', 5)
1095
self.delay_sim_time(10)
1096
self.set_parameter('SIM_BATT_VOLTAGE', 11.4)
1097
self.wait_statustext("Battery 1 is low", timeout=60)
1098
self.wait_disarmed()
1099
self.end_subtest("Completed terminate failsafe test")
1100
1101
self.progress("All Battery failsafe tests complete")
1102
1103
def BatteryMissing(self):
1104
''' Test battery health pre-arm and missing failsafe'''
1105
self.context_push()
1106
1107
# Should be good to arm with no changes
1108
self.wait_ready_to_arm()
1109
1110
# Make monitor unhealthy, this should result in unhealthy prearm
1111
self.set_parameters({
1112
'BATT_VOLT_PIN': -1,
1113
})
1114
1115
self.drain_mav()
1116
1117
# Battery should go unhealthy immediately
1118
self.assert_prearm_failure("Battery 1 unhealthy", other_prearm_failures_fatal=False)
1119
1120
# Return monitor to health
1121
self.context_pop()
1122
self.context_push()
1123
1124
self.wait_ready_to_arm()
1125
1126
# take off and then trigger in flight
1127
self.takeoff(10, mode="LOITER")
1128
self.set_parameters({
1129
'BATT_VOLT_PIN': -1,
1130
})
1131
1132
# Should trigger missing failsafe
1133
self.wait_statustext("Battery 1 is missing")
1134
1135
# Done, reset params and reboot to clear failsafe
1136
self.land_and_disarm()
1137
self.context_pop()
1138
self.reboot_sitl()
1139
1140
def VibrationFailsafe(self):
1141
'''Test Vibration Failsafe'''
1142
self.context_push()
1143
1144
# takeoff in Loiter to 20m
1145
self.takeoff(20, mode="LOITER")
1146
1147
# simulate accel bias caused by high vibration
1148
self.set_parameters({
1149
'SIM_ACC1_BIAS_Z': 2,
1150
'SIM_ACC2_BIAS_Z': 2,
1151
'SIM_ACC3_BIAS_Z': 2,
1152
})
1153
1154
# wait for Vibration compensation warning and change to LAND mode
1155
self.wait_statustext("Vibration compensation ON", timeout=30)
1156
self.change_mode("LAND")
1157
1158
# check vehicle descends to 2m or less within 40 seconds
1159
self.wait_altitude(-5, 2, timeout=50, relative=True)
1160
1161
# force disarm of vehicle (it will likely not automatically disarm)
1162
self.disarm_vehicle(force=True)
1163
1164
# revert simulated accel bias and reboot to restore EKF health
1165
self.context_pop()
1166
self.reboot_sitl()
1167
1168
def test_takeoff_check_mode(self, mode, user_takeoff=False):
1169
# stabilize check
1170
self.progress("Motor takeoff check in %s" % mode)
1171
self.change_mode(mode)
1172
self.zero_throttle()
1173
self.wait_ready_to_arm()
1174
self.context_push()
1175
self.context_collect('STATUSTEXT')
1176
self.arm_vehicle()
1177
if user_takeoff:
1178
self.run_cmd(
1179
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
1180
p7=10,
1181
)
1182
else:
1183
self.set_rc(3, 1700)
1184
# we may never see ourselves as armed in a heartbeat
1185
self.wait_statustext("Takeoff blocked: ESC RPM out of range", check_context=True)
1186
self.context_pop()
1187
self.zero_throttle()
1188
self.disarm_vehicle()
1189
self.wait_disarmed()
1190
1191
# Tests the motor failsafe
1192
def TakeoffCheck(self):
1193
'''Test takeoff check'''
1194
self.set_parameters({
1195
"AHRS_EKF_TYPE": 10,
1196
'SIM_ESC_TELEM': 1,
1197
'SIM_ESC_ARM_RPM': 500,
1198
'TKOFF_RPM_MIN': 1000,
1199
})
1200
1201
self.test_takeoff_check_mode("STABILIZE")
1202
self.test_takeoff_check_mode("ACRO")
1203
self.test_takeoff_check_mode("LOITER")
1204
self.test_takeoff_check_mode("ALT_HOLD")
1205
# self.test_takeoff_check_mode("FLOWHOLD")
1206
self.test_takeoff_check_mode("GUIDED", True)
1207
self.test_takeoff_check_mode("POSHOLD")
1208
# self.test_takeoff_check_mode("SPORT")
1209
1210
self.set_parameters({
1211
"AHRS_EKF_TYPE": 10,
1212
'SIM_ESC_TELEM': 1,
1213
'TKOFF_RPM_MIN': 1,
1214
'TKOFF_RPM_MAX': 3,
1215
})
1216
self.test_takeoff_check_mode("STABILIZE")
1217
self.test_takeoff_check_mode("ACRO")
1218
self.test_takeoff_check_mode("LOITER")
1219
self.test_takeoff_check_mode("ALT_HOLD")
1220
# self.test_takeoff_check_mode("FLOWHOLD")
1221
self.test_takeoff_check_mode("GUIDED", True)
1222
self.test_takeoff_check_mode("POSHOLD")
1223
# self.test_takeoff_check_mode("SPORT")
1224
1225
def assert_dataflash_message_field_level_at(self,
1226
mtype,
1227
field,
1228
level,
1229
maintain=1,
1230
tolerance=0.05,
1231
timeout=30,
1232
condition=None,
1233
dfreader_start_timestamp=None,
1234
verbose=False):
1235
'''wait for EKF's accel bias to reach a level and maintain it'''
1236
1237
if verbose:
1238
self.progress("current onboard log filepath: %s" % self.current_onboard_log_filepath())
1239
dfreader = self.dfreader_for_current_onboard_log()
1240
1241
achieve_start = None
1242
current_value = None
1243
while True:
1244
m = dfreader.recv_match(type=mtype, condition=condition)
1245
if m is None:
1246
raise NotAchievedException("%s.%s did not maintain %f" %
1247
(mtype, field, level))
1248
if dfreader_start_timestamp is not None:
1249
if m.TimeUS < dfreader_start_timestamp:
1250
continue
1251
if verbose:
1252
print("m=%s" % str(m))
1253
current_value = getattr(m, field)
1254
1255
if abs(current_value - level) > tolerance:
1256
if achieve_start is not None:
1257
self.progress("Achieve stop at %u" % m.TimeUS)
1258
achieve_start = None
1259
continue
1260
1261
dfreader_now = m.TimeUS
1262
if achieve_start is None:
1263
self.progress("Achieve start at %u (got=%f want=%f)" %
1264
(dfreader_now, current_value, level))
1265
if maintain is None:
1266
return
1267
achieve_start = m.TimeUS
1268
continue
1269
1270
# we're achieving....
1271
if dfreader_now - achieve_start > maintain*1e6:
1272
return dfreader_now
1273
1274
# Tests any EK3 accel bias is subtracted from the correct IMU data
1275
def EK3AccelBias(self):
1276
'''Test EK3 Accel Bias data'''
1277
self.context_push()
1278
1279
self.start_test("Test zero bias")
1280
dfreader_tstart = self.assert_dataflash_message_field_level_at(
1281
"XKF2",
1282
"AZ",
1283
0.0,
1284
condition="XKF2.C==1",
1285
)
1286
1287
# Add 2m/s/s bias to the second IMU
1288
self.set_parameters({
1289
'SIM_ACC2_BIAS_Z': 0.7,
1290
})
1291
1292
self.start_subtest("Ensuring second core has bias")
1293
self.delay_sim_time(30)
1294
dfreader_tstart = self.assert_dataflash_message_field_level_at(
1295
"XKF2", "AZ", 0.7,
1296
condition="XKF2.C==1",
1297
)
1298
1299
self.start_subtest("Ensuring earth frame is compensated")
1300
self.assert_dataflash_message_field_level_at(
1301
"RATE", "A", 0,
1302
maintain=1,
1303
tolerance=2, # RATE.A is in cm/s/s
1304
dfreader_start_timestamp=dfreader_tstart,
1305
)
1306
1307
# now switch the EKF to only using the second core:
1308
self.set_parameters({
1309
'SIM_ACC2_BIAS_Z': 0.0,
1310
"EK3_IMU_MASK": 0b10,
1311
})
1312
self.reboot_sitl()
1313
1314
self.delay_sim_time(30)
1315
dfreader_tstart = self.assert_dataflash_message_field_level_at(
1316
"XKF2", "AZ", 0.0,
1317
condition="XKF2.C==0",
1318
)
1319
1320
# Add 2m/s/s bias to the second IMU
1321
self.set_parameters({
1322
'SIM_ACC2_BIAS_Z': 0.7,
1323
})
1324
1325
self.start_subtest("Ensuring first core now has bias")
1326
self.delay_sim_time(30)
1327
dfreader_tstart = self.assert_dataflash_message_field_level_at(
1328
"XKF2", "AZ", 0.7,
1329
condition="XKF2.C==0",
1330
)
1331
1332
self.start_subtest("Ensuring earth frame is compensated")
1333
self.assert_dataflash_message_field_level_at(
1334
"RATE", "A", 0,
1335
maintain=1,
1336
tolerance=2, # RATE.A is in cm/s/s
1337
dfreader_start_timestamp=dfreader_tstart,
1338
verbose=True,
1339
)
1340
1341
# revert simulated accel bias and reboot to restore EKF health
1342
self.context_pop()
1343
self.reboot_sitl()
1344
1345
# StabilityPatch - fly south, then hold loiter within 5m
1346
# position and altitude and reduce 1 motor to 60% efficiency
1347
def StabilityPatch(self,
1348
holdtime=30,
1349
maxaltchange=5,
1350
maxdistchange=10):
1351
'''Fly stability patch'''
1352
self.takeoff(10, mode="LOITER")
1353
1354
# first south
1355
self.progress("turn south")
1356
self.set_rc(4, 1580)
1357
self.wait_heading(180)
1358
self.set_rc(4, 1500)
1359
1360
# fly west 80m
1361
self.set_rc(2, 1100)
1362
self.wait_distance(80)
1363
self.set_rc(2, 1500)
1364
1365
# wait for copter to slow moving
1366
self.wait_groundspeed(0, 2)
1367
1368
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
1369
start_altitude = m.alt
1370
start = self.mav.location()
1371
tstart = self.get_sim_time()
1372
self.progress("Holding loiter at %u meters for %u seconds" %
1373
(start_altitude, holdtime))
1374
1375
# cut motor 1's to efficiency
1376
self.progress("Cutting motor 1 to 65% efficiency")
1377
self.set_parameters({
1378
"SIM_ENGINE_MUL": 0.65,
1379
"SIM_ENGINE_FAIL": 1 << 0, # motor 1
1380
})
1381
1382
while self.get_sim_time_cached() < tstart + holdtime:
1383
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
1384
pos = self.mav.location()
1385
delta = self.get_distance(start, pos)
1386
alt_delta = math.fabs(m.alt - start_altitude)
1387
self.progress("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt))
1388
if alt_delta > maxaltchange:
1389
raise NotAchievedException(
1390
"Loiter alt shifted %u meters (> limit %u)" %
1391
(alt_delta, maxaltchange))
1392
if delta > maxdistchange:
1393
raise NotAchievedException(
1394
("Loiter shifted %u meters (> limit of %u)" %
1395
(delta, maxdistchange)))
1396
1397
# restore motor 1 to 100% efficiency
1398
self.set_parameter("SIM_ENGINE_MUL", 1.0)
1399
1400
self.progress("Stability patch and Loiter OK for %us" % holdtime)
1401
1402
self.progress("RTL after stab patch")
1403
self.do_RTL()
1404
1405
def debug_arming_issue(self):
1406
while True:
1407
self.send_mavlink_arm_command()
1408
m = self.mav.recv_match(blocking=True, timeout=1)
1409
if m is None:
1410
continue
1411
if m.get_type() in ["STATUSTEXT", "COMMAND_ACK"]:
1412
print("Got: %s" % str(m))
1413
if self.mav.motors_armed():
1414
self.progress("Armed")
1415
return
1416
1417
# fly_fence_test - fly east until you hit the horizontal circular fence
1418
avoid_behave_slide = 0
1419
1420
def fly_fence_avoid_test_radius_check(self, timeout=180, avoid_behave=avoid_behave_slide):
1421
using_mode = "LOITER" # must be something which adjusts velocity!
1422
self.change_mode(using_mode)
1423
fence_radius = 15
1424
fence_margin = 3
1425
self.set_parameters({
1426
"FENCE_ENABLE": 1, # fence
1427
"FENCE_TYPE": 2, # circle
1428
"FENCE_RADIUS": fence_radius,
1429
"FENCE_MARGIN": fence_margin,
1430
"AVOID_ENABLE": 1,
1431
"AVOID_BEHAVE": avoid_behave,
1432
"RC10_OPTION": 40, # avoid-enable
1433
})
1434
self.wait_ready_to_arm()
1435
self.set_rc(10, 2000)
1436
home_distance = self.distance_to_home(use_cached_home=True)
1437
if home_distance > 5:
1438
raise PreconditionFailedException("Expected to be within 5m of home")
1439
self.zero_throttle()
1440
self.arm_vehicle()
1441
self.set_rc(3, 1700)
1442
self.wait_altitude(10, 100, relative=True)
1443
self.set_rc(3, 1500)
1444
self.set_rc(2, 1400)
1445
self.wait_distance_to_home(12, 20, timeout=30)
1446
tstart = self.get_sim_time()
1447
push_time = 70 # push against barrier for 60 seconds
1448
failed_max = False
1449
failed_min = False
1450
while True:
1451
if self.get_sim_time() - tstart > push_time:
1452
self.progress("Push time up")
1453
break
1454
# make sure we don't RTL:
1455
if not self.mode_is(using_mode):
1456
raise NotAchievedException("Changed mode away from %s" % using_mode)
1457
distance = self.distance_to_home(use_cached_home=True)
1458
inner_radius = fence_radius - fence_margin
1459
want_min = inner_radius - 1 # allow 1m either way
1460
want_max = inner_radius + 1 # allow 1m either way
1461
self.progress("Push: distance=%f %f<want<%f" %
1462
(distance, want_min, want_max))
1463
if distance < want_min:
1464
if failed_min is False:
1465
self.progress("Failed min")
1466
failed_min = True
1467
if distance > want_max:
1468
if failed_max is False:
1469
self.progress("Failed max")
1470
failed_max = True
1471
if failed_min and failed_max:
1472
raise NotAchievedException("Failed both min and max checks. Clever")
1473
if failed_min:
1474
raise NotAchievedException("Failed min")
1475
if failed_max:
1476
raise NotAchievedException("Failed max")
1477
self.set_rc(2, 1500)
1478
self.do_RTL()
1479
1480
def HorizontalAvoidFence(self, timeout=180):
1481
'''Test horizontal Avoidance fence'''
1482
self.fly_fence_avoid_test_radius_check(avoid_behave=1, timeout=timeout)
1483
self.fly_fence_avoid_test_radius_check(avoid_behave=0, timeout=timeout)
1484
1485
# fly_fence_test - fly east until you hit the horizontal circular fence
1486
def HorizontalFence(self, timeout=180):
1487
'''Test horizontal fence'''
1488
# enable fence, disable avoidance
1489
self.set_parameters({
1490
"FENCE_ENABLE": 1,
1491
"AVOID_ENABLE": 0,
1492
})
1493
1494
self.change_mode("LOITER")
1495
self.wait_ready_to_arm()
1496
1497
# fence requires home to be set:
1498
m = self.poll_home_position(quiet=False)
1499
1500
self.start_subtest("ensure we can't arm if outside fence")
1501
self.load_fence("fence-in-middle-of-nowhere.txt")
1502
1503
self.delay_sim_time(5) # let fence check run so it loads-from-eeprom
1504
self.assert_prearm_failure("Vehicle breaching Polygon fence")
1505
self.progress("Failed to arm outside fence (good!)")
1506
self.clear_fence()
1507
self.delay_sim_time(5) # let fence breach clear
1508
self.drain_mav()
1509
self.end_subtest("ensure we can't arm if outside fence")
1510
1511
self.start_subtest("ensure we can't arm with bad radius")
1512
self.context_push()
1513
self.set_parameter("FENCE_RADIUS", -1)
1514
self.assert_prearm_failure("Invalid Circle FENCE_RADIUS value")
1515
self.context_pop()
1516
self.progress("Failed to arm with bad radius")
1517
self.drain_mav()
1518
self.end_subtest("ensure we can't arm with bad radius")
1519
1520
self.start_subtest("ensure we can't arm with bad alt")
1521
self.context_push()
1522
self.set_parameter("FENCE_ALT_MAX", -1)
1523
self.assert_prearm_failure("Invalid FENCE_ALT_MAX value")
1524
self.context_pop()
1525
self.progress("Failed to arm with bad altitude")
1526
self.end_subtest("ensure we can't arm with bad radius")
1527
1528
self.start_subtest("Check breach-fence behaviour")
1529
self.set_parameter("FENCE_TYPE", 2)
1530
self.takeoff(10, mode="LOITER")
1531
1532
# first east
1533
self.progress("turn east")
1534
self.set_rc(4, 1580)
1535
self.wait_heading(160, timeout=60)
1536
self.set_rc(4, 1500)
1537
1538
fence_radius = self.get_parameter("FENCE_RADIUS")
1539
1540
self.progress("flying forward (east) until we hit fence")
1541
pitching_forward = True
1542
self.set_rc(2, 1100)
1543
1544
self.progress("Waiting for fence breach")
1545
tstart = self.get_sim_time()
1546
while not self.mode_is("RTL"):
1547
if self.get_sim_time_cached() - tstart > 30:
1548
raise NotAchievedException("Did not breach fence")
1549
1550
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
1551
alt = m.relative_alt / 1000.0 # mm -> m
1552
home_distance = self.distance_to_home(use_cached_home=True)
1553
self.progress("Alt: %.02f HomeDistance: %.02f (fence radius=%f)" %
1554
(alt, home_distance, fence_radius))
1555
1556
self.progress("Waiting until we get home and disarm")
1557
tstart = self.get_sim_time()
1558
while self.get_sim_time_cached() < tstart + timeout:
1559
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
1560
alt = m.relative_alt / 1000.0 # mm -> m
1561
home_distance = self.distance_to_home(use_cached_home=True)
1562
self.progress("Alt: %.02f HomeDistance: %.02f" %
1563
(alt, home_distance))
1564
# recenter pitch sticks once we're home so we don't fly off again
1565
if pitching_forward and home_distance < 50:
1566
pitching_forward = False
1567
self.set_rc(2, 1475)
1568
# disable fence
1569
self.set_parameter("FENCE_ENABLE", 0)
1570
if (alt <= 1 and home_distance < 10) or (not self.armed() and home_distance < 10):
1571
# reduce throttle
1572
self.zero_throttle()
1573
self.change_mode("LAND")
1574
self.wait_landed_and_disarmed()
1575
self.progress("Reached home OK")
1576
self.zero_throttle()
1577
return
1578
1579
# give we're testing RTL, doing one here probably doesn't make sense
1580
home_distance = self.distance_to_home(use_cached_home=True)
1581
raise AutoTestTimeoutException(
1582
"Fence test failed to reach home (%fm distance) - "
1583
"timed out after %u seconds" % (home_distance, timeout,))
1584
1585
# MaxAltFence - fly up until you hit the fence ceiling
1586
def MaxAltFence(self):
1587
'''Test Max Alt Fence'''
1588
self.takeoff(10, mode="LOITER")
1589
"""Hold loiter position."""
1590
1591
# enable fence, disable avoidance
1592
self.set_parameters({
1593
"FENCE_ENABLE": 1,
1594
"AVOID_ENABLE": 0,
1595
"FENCE_TYPE": 1,
1596
"FENCE_ENABLE" : 1,
1597
})
1598
1599
self.change_alt(10)
1600
1601
# first east
1602
self.progress("turning east")
1603
self.set_rc(4, 1580)
1604
self.wait_heading(160, timeout=60)
1605
self.set_rc(4, 1500)
1606
1607
self.progress("flying east 20m")
1608
self.set_rc(2, 1100)
1609
self.wait_distance(20)
1610
1611
self.progress("flying up")
1612
self.set_rc_from_map({
1613
2: 1500,
1614
3: 1800,
1615
})
1616
1617
# wait for fence to trigger
1618
self.wait_mode('RTL', timeout=120)
1619
1620
self.wait_rtl_complete()
1621
1622
self.zero_throttle()
1623
1624
# MaxAltFence - fly up and make sure fence action does not trigger
1625
# Also check that the vehicle will not try and descend too fast when trying to backup from a max alt fence due to avoidance
1626
def MaxAltFenceAvoid(self):
1627
'''Test Max Alt Fence Avoidance'''
1628
self.takeoff(10, mode="LOITER")
1629
"""Hold loiter position."""
1630
1631
# enable fence, only max altitude, defualt is 100m
1632
# No action, rely on avoidance to prevent the breach
1633
self.set_parameters({
1634
"FENCE_ENABLE": 1,
1635
"FENCE_TYPE": 1,
1636
"FENCE_ACTION": 0,
1637
})
1638
1639
# Try and fly past the fence
1640
self.set_rc(3, 1920)
1641
1642
# Avoid should prevent the vehicle flying past the fence, so the altitude wait should timeouts
1643
try:
1644
self.wait_altitude(140, 150, timeout=90, relative=True)
1645
raise NotAchievedException("Avoid should prevent reaching altitude")
1646
except AutoTestTimeoutException:
1647
pass
1648
except Exception as e:
1649
raise e
1650
1651
# Check descent is not too fast, allow 10% above the configured backup speed
1652
max_descent_rate = -self.get_parameter("AVOID_BACKUP_SPD") * 1.1
1653
1654
def get_climb_rate(mav, m):
1655
m_type = m.get_type()
1656
if m_type != 'VFR_HUD':
1657
return
1658
if m.climb < max_descent_rate:
1659
raise NotAchievedException("Decending too fast want %f got %f" % (max_descent_rate, m.climb))
1660
1661
self.context_push()
1662
self.install_message_hook_context(get_climb_rate)
1663
1664
# Reduce fence alt, this will result in a fence breach, but there is no action.
1665
# Avoid should then backup the vehicle to be under the new fence alt.
1666
self.set_parameters({
1667
"FENCE_ALT_MAX": 50,
1668
})
1669
self.wait_altitude(40, 50, timeout=90, relative=True)
1670
1671
self.context_pop()
1672
1673
self.set_rc(3, 1500)
1674
self.do_RTL()
1675
1676
# fly_alt_min_fence_test - fly down until you hit the fence floor
1677
def MinAltFence(self):
1678
'''Test Min Alt Fence'''
1679
self.takeoff(30, mode="LOITER", timeout=60)
1680
1681
# enable fence, disable avoidance
1682
self.set_parameters({
1683
"AVOID_ENABLE": 0,
1684
"FENCE_ENABLE" : 1,
1685
"FENCE_TYPE": 8,
1686
"FENCE_ALT_MIN": 20,
1687
})
1688
1689
self.change_alt(30)
1690
1691
# Activate the floor fence
1692
# TODO this test should run without requiring this
1693
self.do_fence_enable()
1694
1695
# first east
1696
self.progress("turn east")
1697
self.set_rc(4, 1580)
1698
self.wait_heading(160, timeout=60)
1699
self.set_rc(4, 1500)
1700
1701
# fly forward (east) at least 20m
1702
self.set_rc(2, 1100)
1703
self.wait_distance(20)
1704
1705
# stop flying forward and start flying down:
1706
self.set_rc_from_map({
1707
2: 1500,
1708
3: 1200,
1709
})
1710
1711
# wait for fence to trigger
1712
self.wait_mode('RTL', timeout=120)
1713
1714
self.wait_rtl_complete()
1715
1716
# Disable the fence using mavlink command to ensure cleaned up SITL state
1717
self.do_fence_disable()
1718
1719
self.zero_throttle()
1720
1721
# MinAltFenceAvoid - fly down and make sure fence action does not trigger
1722
# Also check that the vehicle will not try and ascend too fast when trying to backup from a min alt fence due to avoidance
1723
def MinAltFenceAvoid(self):
1724
'''Test Min Alt Fence Avoidance'''
1725
1726
# enable fence, only min altitude
1727
# No action, rely on avoidance to prevent the breach
1728
self.set_parameters({
1729
"FENCE_ENABLE": 1,
1730
"FENCE_TYPE": 8,
1731
"FENCE_ALT_MIN": 20,
1732
"FENCE_ACTION": 0,
1733
})
1734
self.reboot_sitl()
1735
1736
self.takeoff(30, mode="LOITER")
1737
"""Hold loiter position."""
1738
1739
# Try and fly past the fence
1740
self.set_rc(3, 1120)
1741
1742
# Avoid should prevent the vehicle flying past the fence, so the altitude wait should timeouts
1743
try:
1744
self.wait_altitude(10, 15, timeout=90, relative=True)
1745
raise NotAchievedException("Avoid should prevent reaching altitude")
1746
except AutoTestTimeoutException:
1747
pass
1748
except Exception as e:
1749
raise e
1750
1751
# Check ascent is not too fast, allow 10% above the configured backup speed
1752
max_ascent_rate = self.get_parameter("AVOID_BACKUP_SPD") * 1.1
1753
1754
def get_climb_rate(mav, m):
1755
m_type = m.get_type()
1756
if m_type != 'VFR_HUD':
1757
return
1758
if m.climb > max_ascent_rate:
1759
raise NotAchievedException("Ascending too fast want %f got %f" % (max_ascent_rate, m.climb))
1760
1761
self.context_push()
1762
self.install_message_hook_context(get_climb_rate)
1763
1764
# Reduce fence alt, this will result in a fence breach, but there is no action.
1765
# Avoid should then backup the vehicle to be over the new fence alt.
1766
self.set_parameters({
1767
"FENCE_ALT_MIN": 30,
1768
})
1769
self.wait_altitude(30, 40, timeout=90, relative=True)
1770
1771
self.context_pop()
1772
1773
self.set_rc(3, 1500)
1774
self.do_RTL()
1775
1776
def FenceFloorEnabledLanding(self):
1777
"""Ensures we can initiate and complete an RTL while the fence is
1778
enabled.
1779
"""
1780
fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
1781
1782
self.progress("Test Landing while fence floor enabled")
1783
self.set_parameters({
1784
"AVOID_ENABLE": 0,
1785
"FENCE_ENABLE" : 1,
1786
"FENCE_TYPE": 15,
1787
"FENCE_ALT_MIN": 20,
1788
"FENCE_ALT_MAX": 30,
1789
})
1790
1791
self.change_mode("GUIDED")
1792
self.wait_ready_to_arm()
1793
self.arm_vehicle()
1794
self.user_takeoff(alt_min=25)
1795
1796
# Check fence is enabled
1797
self.assert_fence_enabled()
1798
1799
# Change to RC controlled mode
1800
self.change_mode('LOITER')
1801
1802
self.set_rc(3, 1800)
1803
1804
self.wait_mode('RTL', timeout=120)
1805
# center throttle
1806
self.set_rc(3, 1500)
1807
1808
# wait until we are below the fence floor and re-enter loiter
1809
self.wait_altitude(5, 15, relative=True)
1810
self.change_mode('LOITER')
1811
# wait for manual recovery to expire
1812
self.delay_sim_time(15)
1813
1814
# lower throttle and try and land
1815
self.set_rc(3, 1300)
1816
self.wait_altitude(0, 2, relative=True)
1817
self.zero_throttle()
1818
self.wait_landed_and_disarmed()
1819
self.assert_fence_enabled()
1820
# must not be in RTL
1821
self.assert_mode("LOITER")
1822
1823
# Assert fence is healthy since it was enabled automatically
1824
self.assert_sensor_state(fence_bit, healthy=True)
1825
1826
# Disable the fence using mavlink command to ensure cleaned up SITL state
1827
self.do_fence_disable()
1828
self.assert_fence_disabled()
1829
1830
def FenceFloorAutoDisableLanding(self):
1831
"""Ensures we can initiate and complete an RTL while the fence is enabled"""
1832
1833
fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
1834
1835
self.progress("Test Landing while fence floor enabled")
1836
self.set_parameters({
1837
"AVOID_ENABLE": 0,
1838
"FENCE_TYPE": 11,
1839
"FENCE_ALT_MIN": 10,
1840
"FENCE_ALT_MAX": 20,
1841
"FENCE_AUTOENABLE" : 1,
1842
})
1843
1844
self.change_mode("GUIDED")
1845
self.wait_ready_to_arm()
1846
self.arm_vehicle()
1847
self.takeoff(alt_min=15, mode="GUIDED")
1848
1849
# Check fence is enabled
1850
self.assert_fence_enabled()
1851
1852
# Change to RC controlled mode
1853
self.change_mode('LOITER')
1854
1855
self.set_rc(3, 1800)
1856
1857
self.wait_mode('RTL', timeout=120)
1858
1859
self.wait_landed_and_disarmed(0)
1860
# the breach should have cleared since we auto-disable the
1861
# fence on landing
1862
self.assert_fence_disabled()
1863
1864
# Assert fences have gone now that we have landed and disarmed
1865
self.assert_sensor_state(fence_bit, present=True, enabled=False)
1866
1867
def FenceFloorAutoEnableOnArming(self):
1868
"""Ensures we can auto-enable fences on arming and still takeoff and land"""
1869
1870
fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
1871
1872
self.set_parameters({
1873
"AVOID_ENABLE": 0,
1874
"FENCE_TYPE": 11,
1875
"FENCE_ALT_MIN": 10,
1876
"FENCE_ALT_MAX": 20,
1877
"FENCE_AUTOENABLE" : 3,
1878
})
1879
1880
self.change_mode("GUIDED")
1881
# Check fence is not enabled
1882
self.assert_fence_disabled()
1883
1884
self.wait_ready_to_arm()
1885
self.arm_vehicle()
1886
self.takeoff(alt_min=15, mode="GUIDED")
1887
1888
# Check fence is enabled
1889
self.assert_fence_enabled()
1890
1891
# Change to RC controlled mode
1892
self.change_mode('LOITER')
1893
1894
self.set_rc(3, 1800)
1895
1896
self.wait_mode('RTL', timeout=120)
1897
# Assert fence is not healthy now that we are in RTL
1898
self.assert_sensor_state(fence_bit, healthy=False)
1899
1900
self.wait_landed_and_disarmed(0)
1901
# the breach should have cleared since we auto-disable the
1902
# fence on landing
1903
self.assert_fence_disabled()
1904
1905
# Assert fences have gone now that we have landed and disarmed
1906
self.assert_sensor_state(fence_bit, present=True, enabled=False)
1907
1908
# Disable the fence using mavlink command to ensure cleaned up SITL state
1909
self.assert_fence_disabled()
1910
1911
def GPSGlitchLoiter(self, timeout=30, max_distance=20):
1912
"""fly_gps_glitch_loiter_test. Fly south east in loiter and test
1913
reaction to gps glitch."""
1914
self.takeoff(10, mode="LOITER")
1915
1916
# turn on simulator display of gps and actual position
1917
if self.use_map:
1918
self.show_gps_and_sim_positions(True)
1919
1920
# set-up gps glitch array
1921
glitch_lat = [0.0002996,
1922
0.0006958,
1923
0.0009431,
1924
0.0009991,
1925
0.0009444,
1926
0.0007716,
1927
0.0006221]
1928
glitch_lon = [0.0000717,
1929
0.0000912,
1930
0.0002761,
1931
0.0002626,
1932
0.0002807,
1933
0.0002049,
1934
0.0001304]
1935
glitch_num = len(glitch_lat)
1936
self.progress("GPS Glitches:")
1937
for i in range(1, glitch_num):
1938
self.progress("glitch %d %.7f %.7f" %
1939
(i, glitch_lat[i], glitch_lon[i]))
1940
1941
# turn south east
1942
self.progress("turn south east")
1943
self.set_rc(4, 1580)
1944
try:
1945
self.wait_heading(150)
1946
self.set_rc(4, 1500)
1947
# fly forward (south east) at least 60m
1948
self.set_rc(2, 1100)
1949
self.wait_distance(60)
1950
self.set_rc(2, 1500)
1951
# wait for copter to slow down
1952
except Exception as e:
1953
if self.use_map:
1954
self.show_gps_and_sim_positions(False)
1955
raise e
1956
1957
# record time and position
1958
tstart = self.get_sim_time()
1959
tnow = tstart
1960
start_pos = self.sim_location()
1961
1962
# initialise current glitch
1963
glitch_current = 0
1964
self.progress("Apply first glitch")
1965
self.set_parameters({
1966
"SIM_GPS1_GLTCH_X": glitch_lat[glitch_current],
1967
"SIM_GPS1_GLTCH_Y": glitch_lon[glitch_current],
1968
})
1969
1970
# record position for 30 seconds
1971
while tnow < tstart + timeout:
1972
tnow = self.get_sim_time_cached()
1973
desired_glitch_num = int((tnow - tstart) * 2.2)
1974
if desired_glitch_num > glitch_current and glitch_current != -1:
1975
glitch_current = desired_glitch_num
1976
# turn off glitching if we've reached the end of glitch list
1977
if glitch_current >= glitch_num:
1978
glitch_current = -1
1979
self.progress("Completed Glitches")
1980
self.set_parameters({
1981
"SIM_GPS1_GLTCH_X": 0,
1982
"SIM_GPS1_GLTCH_Y": 0,
1983
})
1984
else:
1985
self.progress("Applying glitch %u" % glitch_current)
1986
# move onto the next glitch
1987
self.set_parameters({
1988
"SIM_GPS1_GLTCH_X": glitch_lat[glitch_current],
1989
"SIM_GPS1_GLTCH_Y": glitch_lon[glitch_current],
1990
})
1991
1992
# start displaying distance moved after all glitches applied
1993
if glitch_current == -1:
1994
m = self.mav.recv_match(type='GLOBAL_POSITION_INT',
1995
blocking=True)
1996
alt = m.alt/1000.0 # mm -> m
1997
curr_pos = self.sim_location()
1998
moved_distance = self.get_distance(curr_pos, start_pos)
1999
self.progress("Alt: %.02f Moved: %.0f" %
2000
(alt, moved_distance))
2001
if moved_distance > max_distance:
2002
raise NotAchievedException(
2003
"Moved over %u meters, Failed!" % max_distance)
2004
else:
2005
self.drain_mav()
2006
2007
# disable gps glitch
2008
if glitch_current != -1:
2009
self.set_parameters({
2010
"SIM_GPS1_GLTCH_X": 0,
2011
"SIM_GPS1_GLTCH_Y": 0,
2012
})
2013
if self.use_map:
2014
self.show_gps_and_sim_positions(False)
2015
2016
self.progress("GPS glitch test passed!"
2017
" stayed within %u meters for %u seconds" %
2018
(max_distance, timeout))
2019
self.do_RTL()
2020
# re-arming is problematic because the GPS is glitching!
2021
self.reboot_sitl()
2022
2023
def GPSGlitchLoiter2(self):
2024
"""test vehicle handles GPS glitch (aka EKF Reset) without twitching"""
2025
self.context_push()
2026
self.takeoff(10, mode="LOITER")
2027
2028
# wait for vehicle to level
2029
self.wait_attitude(desroll=0, despitch=0, timeout=10, tolerance=1)
2030
2031
# apply glitch
2032
self.set_parameter("SIM_GPS1_GLTCH_X", 0.001)
2033
2034
# check lean angles remain stable for 20 seconds
2035
tstart = self.get_sim_time()
2036
while self.get_sim_time_cached() - tstart < 20:
2037
m = self.mav.recv_match(type='ATTITUDE', blocking=True)
2038
roll_deg = math.degrees(m.roll)
2039
pitch_deg = math.degrees(m.pitch)
2040
self.progress("checking att: roll=%f pitch=%f " % (roll_deg, pitch_deg))
2041
if abs(roll_deg) > 2 or abs(pitch_deg) > 2:
2042
raise NotAchievedException("fly_gps_glitch_loiter_test2 failed, roll or pitch moved during GPS glitch")
2043
2044
# RTL, remove glitch and reboot sitl
2045
self.do_RTL()
2046
self.context_pop()
2047
self.reboot_sitl()
2048
2049
def GPSGlitchAuto(self, timeout=180):
2050
'''fly mission and test reaction to gps glitch'''
2051
# set-up gps glitch array
2052
glitch_lat = [0.0002996,
2053
0.0006958,
2054
0.0009431,
2055
0.0009991,
2056
0.0009444,
2057
0.0007716,
2058
0.0006221]
2059
glitch_lon = [0.0000717,
2060
0.0000912,
2061
0.0002761,
2062
0.0002626,
2063
0.0002807,
2064
0.0002049,
2065
0.0001304]
2066
glitch_num = len(glitch_lat)
2067
self.progress("GPS Glitches:")
2068
for i in range(1, glitch_num):
2069
self.progress("glitch %d %.7f %.7f" %
2070
(i, glitch_lat[i], glitch_lon[i]))
2071
2072
# Fly mission #1
2073
self.progress("# Load copter_glitch_mission")
2074
# load the waypoint count
2075
num_wp = self.load_mission("copter_glitch_mission.txt", strict=False)
2076
if not num_wp:
2077
raise NotAchievedException("load copter_glitch_mission failed")
2078
2079
# turn on simulator display of gps and actual position
2080
if self.use_map:
2081
self.show_gps_and_sim_positions(True)
2082
2083
self.progress("test: Fly a mission from 1 to %u" % num_wp)
2084
self.set_current_waypoint(1)
2085
2086
self.change_mode("STABILIZE")
2087
self.wait_ready_to_arm()
2088
self.zero_throttle()
2089
self.arm_vehicle()
2090
2091
# switch into AUTO mode and raise throttle
2092
self.change_mode('AUTO')
2093
self.set_rc(3, 1500)
2094
2095
# wait until 100m from home
2096
try:
2097
self.wait_distance(100, 5, 90)
2098
except Exception as e:
2099
if self.use_map:
2100
self.show_gps_and_sim_positions(False)
2101
raise e
2102
2103
# stop and test loss of GPS for a short time - it should resume GPS use without falling back into a non aiding mode
2104
self.change_mode("LOITER")
2105
self.set_parameters({
2106
"SIM_GPS1_ENABLE": 0,
2107
})
2108
self.delay_sim_time(2)
2109
self.set_parameters({
2110
"SIM_GPS1_ENABLE": 1,
2111
})
2112
# regaining GPS should not result in it falling back to a non-navigation mode
2113
self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=1)
2114
# It should still be navigating after enougnh time has passed for any pending timeouts to activate.
2115
self.delay_sim_time(10)
2116
self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=1)
2117
self.change_mode("AUTO")
2118
2119
# record time and position
2120
tstart = self.get_sim_time()
2121
2122
# initialise current glitch
2123
glitch_current = 0
2124
self.progress("Apply first glitch")
2125
self.set_parameters({
2126
"SIM_GPS1_GLTCH_X": glitch_lat[glitch_current],
2127
"SIM_GPS1_GLTCH_Y": glitch_lon[glitch_current],
2128
})
2129
2130
# record position for 30 seconds
2131
while glitch_current < glitch_num:
2132
tnow = self.get_sim_time()
2133
desired_glitch_num = int((tnow - tstart) * 2.2)
2134
if desired_glitch_num > glitch_current and glitch_current != -1:
2135
glitch_current = desired_glitch_num
2136
# apply next glitch
2137
if glitch_current < glitch_num:
2138
self.progress("Applying glitch %u" % glitch_current)
2139
self.set_parameters({
2140
"SIM_GPS1_GLTCH_X": glitch_lat[glitch_current],
2141
"SIM_GPS1_GLTCH_Y": glitch_lon[glitch_current],
2142
})
2143
2144
# turn off glitching
2145
self.progress("Completed Glitches")
2146
self.set_parameters({
2147
"SIM_GPS1_GLTCH_X": 0,
2148
"SIM_GPS1_GLTCH_Y": 0,
2149
})
2150
2151
# continue with the mission
2152
self.wait_waypoint(0, num_wp-1, timeout=500)
2153
2154
# wait for arrival back home
2155
self.wait_distance_to_home(0, 10, timeout=timeout)
2156
2157
# turn off simulator display of gps and actual position
2158
if self.use_map:
2159
self.show_gps_and_sim_positions(False)
2160
2161
self.progress("GPS Glitch test Auto completed: passed!")
2162
self.wait_disarmed()
2163
# re-arming is problematic because the GPS is glitching!
2164
self.reboot_sitl()
2165
2166
# fly_simple - assumes the simple bearing is initialised to be
2167
# directly north flies a box with 100m west, 15 seconds north,
2168
# 50 seconds east, 15 seconds south
2169
def SimpleMode(self, side=50):
2170
'''Fly in SIMPLE mode'''
2171
self.takeoff(10, mode="LOITER")
2172
2173
# set SIMPLE mode for all flight modes
2174
self.set_parameter("SIMPLE", 63)
2175
2176
# switch to stabilize mode
2177
self.change_mode('STABILIZE')
2178
self.set_rc(3, 1545)
2179
2180
# fly south 50m
2181
self.progress("# Flying south %u meters" % side)
2182
self.set_rc(1, 1300)
2183
self.wait_distance(side, 5, 60)
2184
self.set_rc(1, 1500)
2185
2186
# fly west 8 seconds
2187
self.progress("# Flying west for 8 seconds")
2188
self.set_rc(2, 1300)
2189
tstart = self.get_sim_time()
2190
while self.get_sim_time_cached() < (tstart + 8):
2191
self.mav.recv_match(type='VFR_HUD', blocking=True)
2192
self.set_rc(2, 1500)
2193
2194
# fly north 25 meters
2195
self.progress("# Flying north %u meters" % (side/2.0))
2196
self.set_rc(1, 1700)
2197
self.wait_distance(side/2, 5, 60)
2198
self.set_rc(1, 1500)
2199
2200
# fly east 8 seconds
2201
self.progress("# Flying east for 8 seconds")
2202
self.set_rc(2, 1700)
2203
tstart = self.get_sim_time()
2204
while self.get_sim_time_cached() < (tstart + 8):
2205
self.mav.recv_match(type='VFR_HUD', blocking=True)
2206
self.set_rc(2, 1500)
2207
2208
# hover in place
2209
self.hover()
2210
2211
self.do_RTL(timeout=500)
2212
2213
# fly_super_simple - flies a circle around home for 45 seconds
2214
def SuperSimpleCircle(self, timeout=45):
2215
'''Fly a circle in SUPER SIMPLE mode'''
2216
self.takeoff(10, mode="LOITER")
2217
2218
# fly forward 20m
2219
self.progress("# Flying forward 20 meters")
2220
self.set_rc(2, 1300)
2221
self.wait_distance(20, 5, 60)
2222
self.set_rc(2, 1500)
2223
2224
# set SUPER SIMPLE mode for all flight modes
2225
self.set_parameter("SUPER_SIMPLE", 63)
2226
2227
# switch to stabilize mode
2228
self.change_mode("ALT_HOLD")
2229
self.set_rc(3, 1500)
2230
2231
# start copter yawing slowly
2232
self.set_rc(4, 1550)
2233
2234
# roll left for timeout seconds
2235
self.progress("# rolling left from pilot's POV for %u seconds"
2236
% timeout)
2237
self.set_rc(1, 1300)
2238
tstart = self.get_sim_time()
2239
while self.get_sim_time_cached() < (tstart + timeout):
2240
self.mav.recv_match(type='VFR_HUD', blocking=True)
2241
2242
# stop rolling and yawing
2243
self.set_rc(1, 1500)
2244
self.set_rc(4, 1500)
2245
2246
# restore simple mode parameters to default
2247
self.set_parameter("SUPER_SIMPLE", 0)
2248
2249
# hover in place
2250
self.hover()
2251
2252
self.do_RTL()
2253
2254
# fly_circle - flies a circle with 20m radius
2255
def ModeCircle(self, holdtime=36):
2256
'''Fly CIRCLE mode'''
2257
# the following should not be required. But there appears to
2258
# be a physics failure in the simulation which is causing CI
2259
# to fall over a lot. -pb 202007021209
2260
self.reboot_sitl()
2261
2262
self.takeoff(10, mode="LOITER")
2263
2264
# face west
2265
self.progress("turn west")
2266
self.set_rc(4, 1580)
2267
self.wait_heading(270)
2268
self.set_rc(4, 1500)
2269
2270
# set CIRCLE radius
2271
self.set_parameter("CIRCLE_RADIUS", 3000)
2272
2273
# fly forward (east) at least 100m
2274
self.set_rc(2, 1100)
2275
self.wait_distance(100)
2276
# return pitch stick back to middle
2277
self.set_rc(2, 1500)
2278
2279
# set CIRCLE mode
2280
self.change_mode('CIRCLE')
2281
2282
# wait
2283
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
2284
start_altitude = m.alt
2285
tstart = self.get_sim_time()
2286
self.progress("Circle at %u meters for %u seconds" %
2287
(start_altitude, holdtime))
2288
while self.get_sim_time_cached() < tstart + holdtime:
2289
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
2290
self.progress("heading %d" % m.heading)
2291
2292
self.progress("CIRCLE OK for %u seconds" % holdtime)
2293
2294
self.do_RTL()
2295
2296
def CompassMot(self):
2297
'''test code that adjust mag field for motor interference'''
2298
self.run_cmd(
2299
mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION,
2300
0, # p1
2301
0, # p2
2302
0, # p3
2303
0, # p4
2304
0, # p5
2305
1, # p6
2306
0 # p7
2307
)
2308
self.context_collect("STATUSTEXT")
2309
self.wait_statustext("Starting calibration", check_context=True)
2310
self.wait_statustext("Current", check_context=True)
2311
rc3_min = self.get_parameter('RC3_MIN')
2312
rc3_max = self.get_parameter('RC3_MAX')
2313
rc3_dz = self.get_parameter('RC3_DZ')
2314
2315
def set_rc3_for_throttle_pct(thr_pct):
2316
value = int((rc3_min+rc3_dz) + (thr_pct/100.0) * (rc3_max-(rc3_min+rc3_dz)))
2317
self.progress("Setting rc3 to %u" % value)
2318
self.set_rc(3, value)
2319
2320
throttle_in_pct = 0
2321
set_rc3_for_throttle_pct(throttle_in_pct)
2322
self.assert_received_message_field_values("COMPASSMOT_STATUS", {
2323
"interference": 0,
2324
"throttle": throttle_in_pct
2325
}, verbose=True, very_verbose=True)
2326
tstart = self.get_sim_time()
2327
delta = 5
2328
while True:
2329
if self.get_sim_time_cached() - tstart > 60:
2330
raise NotAchievedException("did not run through entire range")
2331
throttle_in_pct += delta
2332
self.progress("Using throttle %f%%" % throttle_in_pct)
2333
set_rc3_for_throttle_pct(throttle_in_pct)
2334
self.wait_message_field_values("COMPASSMOT_STATUS", {
2335
"throttle": throttle_in_pct * 10.0,
2336
}, verbose=True, very_verbose=True, epsilon=1)
2337
if throttle_in_pct == 0:
2338
# finished counting down
2339
break
2340
if throttle_in_pct == 100:
2341
# start counting down
2342
delta = -delta
2343
2344
m = self.wait_message_field_values("COMPASSMOT_STATUS", {
2345
"throttle": 0,
2346
}, verbose=True)
2347
for axis in "X", "Y", "Z":
2348
fieldname = "Compensation" + axis
2349
if getattr(m, fieldname) <= 0:
2350
raise NotAchievedException("Expected non-zero %s" % fieldname)
2351
2352
# it's kind of crap - but any command-ack will stop the
2353
# calibration
2354
self.mav.mav.command_ack_send(0, 1)
2355
self.wait_statustext("Calibration successful")
2356
2357
def MagFail(self):
2358
'''test failover of compass in EKF'''
2359
# we want both EK2 and EK3
2360
self.set_parameters({
2361
"EK2_ENABLE": 1,
2362
"EK3_ENABLE": 1,
2363
})
2364
2365
self.takeoff(10, mode="LOITER")
2366
2367
self.change_mode('CIRCLE')
2368
2369
self.delay_sim_time(20)
2370
2371
self.context_collect("STATUSTEXT")
2372
2373
self.progress("Failing first compass")
2374
self.set_parameter("SIM_MAG1_FAIL", 1)
2375
2376
# we want for the message twice, one for EK2 and again for EK3
2377
self.wait_statustext("EKF2 IMU0 switching to compass 1", check_context=True)
2378
self.wait_statustext("EKF3 IMU0 switching to compass 1", check_context=True)
2379
self.progress("compass switch 1 OK")
2380
2381
self.delay_sim_time(2)
2382
2383
self.context_clear_collection("STATUSTEXT")
2384
2385
self.progress("Failing 2nd compass")
2386
self.set_parameter("SIM_MAG2_FAIL", 1)
2387
2388
self.wait_statustext("EKF2 IMU0 switching to compass 2", check_context=True)
2389
self.wait_statustext("EKF3 IMU0 switching to compass 2", check_context=True)
2390
2391
self.progress("compass switch 2 OK")
2392
2393
self.delay_sim_time(2)
2394
2395
self.context_clear_collection("STATUSTEXT")
2396
2397
self.progress("Failing 3rd compass")
2398
self.set_parameter("SIM_MAG3_FAIL", 1)
2399
self.delay_sim_time(2)
2400
self.set_parameter("SIM_MAG1_FAIL", 0)
2401
2402
self.wait_statustext("EKF2 IMU0 switching to compass 0", check_context=True)
2403
self.wait_statustext("EKF3 IMU0 switching to compass 0", check_context=True)
2404
self.progress("compass switch 0 OK")
2405
2406
self.do_RTL()
2407
2408
def ModeFlip(self):
2409
'''Fly Flip Mode'''
2410
self.context_set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_ATTITUDE, 100)
2411
2412
self.takeoff(20)
2413
2414
self.progress("Flipping in roll")
2415
self.set_rc(1, 1700)
2416
self.send_cmd_do_set_mode('FLIP') # don't wait for success
2417
self.wait_attitude(despitch=0, desroll=45, tolerance=30)
2418
self.wait_attitude(despitch=0, desroll=90, tolerance=30)
2419
self.wait_attitude(despitch=0, desroll=-45, tolerance=30)
2420
self.progress("Waiting for level")
2421
self.set_rc(1, 1500) # can't change quickly enough!
2422
self.wait_attitude(despitch=0, desroll=0, tolerance=5)
2423
2424
self.progress("Regaining altitude")
2425
self.change_mode('ALT_HOLD')
2426
self.wait_altitude(19, 60, relative=True)
2427
2428
self.progress("Flipping in pitch")
2429
self.set_rc(2, 1700)
2430
self.send_cmd_do_set_mode('FLIP') # don't wait for success
2431
self.wait_attitude(despitch=45, desroll=0, tolerance=30)
2432
# can't check roll here as it flips from 0 to -180..
2433
self.wait_attitude(despitch=90, tolerance=30)
2434
self.wait_attitude(despitch=-45, tolerance=30)
2435
self.progress("Waiting for level")
2436
self.set_rc(2, 1500) # can't change quickly enough!
2437
self.wait_attitude(despitch=0, desroll=0, tolerance=5)
2438
2439
self.do_RTL()
2440
2441
def configure_EKFs_to_use_optical_flow_instead_of_GPS(self):
2442
'''configure EKF to use optical flow instead of GPS'''
2443
ahrs_ekf_type = self.get_parameter("AHRS_EKF_TYPE")
2444
if ahrs_ekf_type == 2:
2445
self.set_parameter("EK2_GPS_TYPE", 3)
2446
if ahrs_ekf_type == 3:
2447
self.set_parameters({
2448
"EK3_SRC1_POSXY": 0,
2449
"EK3_SRC1_VELXY": 5,
2450
"EK3_SRC1_VELZ": 0,
2451
})
2452
2453
def OpticalFlowLocation(self):
2454
'''test optical flow doesn't supply location'''
2455
2456
self.context_push()
2457
2458
self.assert_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW, False, False, False, verbose=True)
2459
2460
self.start_subtest("Make sure no crash if no rangefinder")
2461
self.set_parameter("SIM_FLOW_ENABLE", 1)
2462
self.set_parameter("FLOW_TYPE", 10)
2463
2464
self.configure_EKFs_to_use_optical_flow_instead_of_GPS()
2465
2466
self.reboot_sitl()
2467
2468
self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW, True, True, True, verbose=True)
2469
2470
self.change_mode('LOITER')
2471
self.delay_sim_time(5)
2472
self.wait_statustext("Need Position Estimate", timeout=300)
2473
2474
self.context_pop()
2475
2476
self.reboot_sitl()
2477
2478
def OpticalFlow(self):
2479
'''test OpticalFlow in flight'''
2480
self.start_subtest("Make sure no crash if no rangefinder")
2481
2482
self.set_parameters({
2483
"SIM_FLOW_ENABLE": 1,
2484
"FLOW_TYPE": 10,
2485
})
2486
2487
self.set_analog_rangefinder_parameters()
2488
2489
self.reboot_sitl()
2490
2491
self.change_mode('LOITER')
2492
2493
# ensure OPTICAL_FLOW message is reasonable:
2494
global flow_rate_rads
2495
global rangefinder_distance
2496
global gps_speed
2497
global last_debug_time
2498
flow_rate_rads = 0
2499
rangefinder_distance = 0
2500
gps_speed = 0
2501
last_debug_time = 0
2502
2503
def check_optical_flow(mav, m):
2504
global flow_rate_rads
2505
global rangefinder_distance
2506
global gps_speed
2507
global last_debug_time
2508
m_type = m.get_type()
2509
if m_type == "OPTICAL_FLOW":
2510
flow_rate_rads = math.sqrt(m.flow_comp_m_x**2+m.flow_comp_m_y**2)
2511
elif m_type == "RANGEFINDER":
2512
rangefinder_distance = m.distance
2513
elif m_type == "GPS_RAW_INT":
2514
gps_speed = m.vel/100.0 # cm/s -> m/s
2515
of_speed = flow_rate_rads * rangefinder_distance
2516
if abs(of_speed - gps_speed) > 3:
2517
raise NotAchievedException("gps=%f vs of=%f mismatch" %
2518
(gps_speed, of_speed))
2519
2520
now = self.get_sim_time_cached()
2521
if now - last_debug_time > 5:
2522
last_debug_time = now
2523
self.progress("gps=%f of=%f" % (gps_speed, of_speed))
2524
2525
self.install_message_hook_context(check_optical_flow)
2526
2527
self.fly_generic_mission("CMAC-copter-navtest.txt")
2528
2529
def OpticalFlowLimits(self):
2530
'''test EKF navigation limiting'''
2531
self.set_parameters({
2532
"SIM_FLOW_ENABLE": 1,
2533
"FLOW_TYPE": 10,
2534
"SIM_GPS1_ENABLE": 0,
2535
"SIM_TERRAIN": 0,
2536
})
2537
2538
self.configure_EKFs_to_use_optical_flow_instead_of_GPS()
2539
2540
self.set_analog_rangefinder_parameters()
2541
2542
self.reboot_sitl()
2543
2544
# we can't takeoff in loiter as we need flow healthy
2545
self.takeoff(alt_min=5, mode='ALT_HOLD', require_absolute=False, takeoff_throttle=1800)
2546
self.change_mode('LOITER')
2547
2548
# speed should be limited to <10m/s
2549
self.set_rc(2, 1000)
2550
2551
tstart = self.get_sim_time()
2552
timeout = 60
2553
started_climb = False
2554
while self.get_sim_time_cached() - tstart < timeout:
2555
m = self.assert_receive_message('GLOBAL_POSITION_INT')
2556
spd = math.sqrt(m.vx**2 + m.vy**2) * 0.01
2557
alt = m.relative_alt*0.001
2558
2559
# calculate max speed from altitude above the ground
2560
margin = 2.0
2561
max_speed = alt * 1.5 + margin
2562
self.progress("%0.1f: Low Speed: %f (want <= %u) alt=%.1f" %
2563
(self.get_sim_time_cached() - tstart,
2564
spd,
2565
max_speed, alt))
2566
if spd > max_speed:
2567
raise NotAchievedException(("Speed should be limited by"
2568
"EKF optical flow limits"))
2569
2570
# after 30 seconds start climbing
2571
if not started_climb and self.get_sim_time_cached() - tstart > 30:
2572
started_climb = True
2573
self.set_rc(3, 1900)
2574
self.progress("Moving higher")
2575
2576
# check altitude is not climbing above 35m
2577
if alt > 35:
2578
raise NotAchievedException("Alt should be limited by EKF optical flow limits")
2579
self.reboot_sitl(force=True)
2580
2581
def OpticalFlowCalibration(self):
2582
'''test optical flow calibration'''
2583
ex = None
2584
self.context_push()
2585
try:
2586
2587
self.set_parameter("SIM_FLOW_ENABLE", 1)
2588
self.set_parameter("FLOW_TYPE", 10)
2589
self.set_analog_rangefinder_parameters()
2590
2591
# RC9 starts/stops calibration
2592
self.set_parameter("RC9_OPTION", 158)
2593
2594
# initialise flow scaling parameters to incorrect values
2595
self.set_parameter("FLOW_FXSCALER", -200)
2596
self.set_parameter("FLOW_FYSCALER", 200)
2597
2598
self.reboot_sitl()
2599
2600
# ensure calibration is off
2601
self.set_rc(9, 1000)
2602
2603
# takeoff to 10m in loiter
2604
self.takeoff(10, mode="LOITER", require_absolute=True, timeout=720)
2605
2606
# start calibration
2607
self.set_rc(9, 2000)
2608
2609
tstart = self.get_sim_time()
2610
timeout = 90
2611
veh_dir_tstart = self.get_sim_time()
2612
veh_dir = 0
2613
while self.get_sim_time_cached() - tstart < timeout:
2614
# roll and pitch vehicle until samples collected
2615
# change direction of movement every 2 seconds
2616
if self.get_sim_time_cached() - veh_dir_tstart > 2:
2617
veh_dir_tstart = self.get_sim_time()
2618
veh_dir = veh_dir + 1
2619
if veh_dir > 3:
2620
veh_dir = 0
2621
if veh_dir == 0:
2622
# move right
2623
self.set_rc(1, 1800)
2624
self.set_rc(2, 1500)
2625
if veh_dir == 1:
2626
# move left
2627
self.set_rc(1, 1200)
2628
self.set_rc(2, 1500)
2629
if veh_dir == 2:
2630
# move forward
2631
self.set_rc(1, 1500)
2632
self.set_rc(2, 1200)
2633
if veh_dir == 3:
2634
# move back
2635
self.set_rc(1, 1500)
2636
self.set_rc(2, 1800)
2637
2638
# return sticks to center
2639
self.set_rc(1, 1500)
2640
self.set_rc(2, 1500)
2641
2642
# stop calibration (not actually necessary)
2643
self.set_rc(9, 1000)
2644
2645
# check scaling parameters have been restored to values near zero
2646
flow_scalar_x = self.get_parameter("FLOW_FXSCALER")
2647
flow_scalar_y = self.get_parameter("FLOW_FYSCALER")
2648
if ((flow_scalar_x > 30) or (flow_scalar_x < -30)):
2649
raise NotAchievedException("FlowCal failed to set FLOW_FXSCALER correctly")
2650
if ((flow_scalar_y > 30) or (flow_scalar_y < -30)):
2651
raise NotAchievedException("FlowCal failed to set FLOW_FYSCALER correctly")
2652
2653
except Exception as e:
2654
self.print_exception_caught(e)
2655
ex = e
2656
2657
self.disarm_vehicle(force=True)
2658
self.context_pop()
2659
self.reboot_sitl()
2660
2661
if ex is not None:
2662
raise ex
2663
2664
def AutoTune(self):
2665
"""Test autotune mode"""
2666
2667
rlld = self.get_parameter("ATC_RAT_RLL_D")
2668
rlli = self.get_parameter("ATC_RAT_RLL_I")
2669
rllp = self.get_parameter("ATC_RAT_RLL_P")
2670
self.set_parameter("ATC_RAT_RLL_SMAX", 1)
2671
self.takeoff(10)
2672
2673
# hold position in loiter
2674
self.change_mode('AUTOTUNE')
2675
2676
tstart = self.get_sim_time()
2677
sim_time_expected = 5000
2678
deadline = tstart + sim_time_expected
2679
while self.get_sim_time_cached() < deadline:
2680
now = self.get_sim_time_cached()
2681
m = self.mav.recv_match(type='STATUSTEXT',
2682
blocking=True,
2683
timeout=1)
2684
if m is None:
2685
continue
2686
self.progress("STATUSTEXT (%u<%u): %s" % (now, deadline, m.text))
2687
if "AutoTune: Success" in m.text:
2688
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
2689
# near enough for now:
2690
self.change_mode('LAND')
2691
self.wait_landed_and_disarmed()
2692
# check the original gains have been re-instated
2693
if (rlld != self.get_parameter("ATC_RAT_RLL_D") or
2694
rlli != self.get_parameter("ATC_RAT_RLL_I") or
2695
rllp != self.get_parameter("ATC_RAT_RLL_P")):
2696
raise NotAchievedException("AUTOTUNE gains still present")
2697
return
2698
2699
raise NotAchievedException("AUTOTUNE failed (%u seconds)" %
2700
(self.get_sim_time() - tstart))
2701
2702
def AutoTuneYawD(self):
2703
"""Test autotune mode"""
2704
2705
rlld = self.get_parameter("ATC_RAT_RLL_D")
2706
rlli = self.get_parameter("ATC_RAT_RLL_I")
2707
rllp = self.get_parameter("ATC_RAT_RLL_P")
2708
self.set_parameter("ATC_RAT_RLL_SMAX", 1)
2709
self.set_parameter("AUTOTUNE_AXES", 15)
2710
self.takeoff(10)
2711
2712
# hold position in loiter
2713
self.change_mode('AUTOTUNE')
2714
2715
tstart = self.get_sim_time()
2716
sim_time_expected = 5000
2717
deadline = tstart + sim_time_expected
2718
while self.get_sim_time_cached() < deadline:
2719
now = self.get_sim_time_cached()
2720
m = self.mav.recv_match(type='STATUSTEXT',
2721
blocking=True,
2722
timeout=1)
2723
if m is None:
2724
continue
2725
self.progress("STATUSTEXT (%u<%u): %s" % (now, deadline, m.text))
2726
if "AutoTune: Success" in m.text:
2727
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
2728
# near enough for now:
2729
self.change_mode('LAND')
2730
self.wait_landed_and_disarmed()
2731
# check the original gains have been re-instated
2732
if (rlld != self.get_parameter("ATC_RAT_RLL_D") or
2733
rlli != self.get_parameter("ATC_RAT_RLL_I") or
2734
rllp != self.get_parameter("ATC_RAT_RLL_P")):
2735
raise NotAchievedException("AUTOTUNE gains still present")
2736
return
2737
2738
raise NotAchievedException("AUTOTUNE failed (%u seconds)" %
2739
(self.get_sim_time() - tstart))
2740
2741
def AutoTuneSwitch(self):
2742
"""Test autotune on a switch with gains being saved"""
2743
2744
# autotune changes a set of parameters on the vehicle which
2745
# are not in our context. That changes the flight
2746
# characterstics, which we can't afford between runs. So
2747
# completely reset the simulated vehicle after the run is
2748
# complete by "customising" the commandline here:
2749
self.customise_SITL_commandline([])
2750
2751
self.set_parameters({
2752
"RC8_OPTION": 17,
2753
"ATC_RAT_RLL_FLTT": 20,
2754
})
2755
2756
self.takeoff(10, mode='LOITER')
2757
2758
def print_gains(name, gains):
2759
self.progress(f"AUTOTUNE {name} gains are P:%f I:%f D:%f" % (
2760
gains["ATC_RAT_RLL_P"],
2761
gains["ATC_RAT_RLL_I"],
2762
gains["ATC_RAT_RLL_D"]
2763
))
2764
2765
def get_roll_gains(name):
2766
ret = self.get_parameters([
2767
"ATC_RAT_RLL_D",
2768
"ATC_RAT_RLL_I",
2769
"ATC_RAT_RLL_P",
2770
], verbose=False)
2771
print_gains(name, ret)
2772
return ret
2773
2774
def gains_same(gains1, gains2):
2775
for c in 'P', 'I', 'D':
2776
p_name = f"ATC_RAT_RLL_{c}"
2777
if abs(gains1[p_name] - gains2[p_name]) > 0.00001:
2778
return False
2779
return True
2780
2781
self.progress("Take a copy of original gains")
2782
original_gains = get_roll_gains("pre-tuning")
2783
scaled_original_gains = copy.copy(original_gains)
2784
scaled_original_gains["ATC_RAT_RLL_I"] *= 0.1
2785
2786
pre_rllt = self.get_parameter("ATC_RAT_RLL_FLTT")
2787
2788
# hold position in loiter and run autotune
2789
self.set_rc(8, 1850)
2790
self.wait_mode('AUTOTUNE')
2791
2792
tstart = self.get_sim_time()
2793
sim_time_expected = 5000
2794
deadline = tstart + sim_time_expected
2795
while self.get_sim_time_cached() < deadline:
2796
now = self.get_sim_time_cached()
2797
m = self.mav.recv_match(type='STATUSTEXT',
2798
blocking=True,
2799
timeout=1)
2800
if m is None:
2801
continue
2802
self.progress("STATUSTEXT (%u<%u): %s" % (now, deadline, m.text))
2803
if "Determination Failed" in m.text:
2804
break
2805
if "AutoTune: Success" in m.text:
2806
self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
2807
post_gains = get_roll_gains("post")
2808
if gains_same(original_gains, post_gains):
2809
raise NotAchievedException("AUTOTUNE gains not changed")
2810
2811
# because of the way AutoTune works, once autotune is
2812
# complete we return the original parameters via
2813
# parameter-fetching, but fly on the tuned parameters
2814
# (both sets with the I term scaled down). This test
2815
# makes sure that's still the case. It would be nice
2816
# if the PIDs parameters were `set` on success, but
2817
# they aren't... Note that if we use the switch to
2818
# restore the original gains and then start testing
2819
# again (with the switch) then we see the new gains!
2820
2821
# gains are scaled during the testing phase:
2822
if not gains_same(scaled_original_gains, post_gains):
2823
raise NotAchievedException("AUTOTUNE gains were reported as just original gains in test-mode. If you're fixing this, good!") # noqa
2824
2825
self.progress("Check original gains are re-instated by switch")
2826
self.set_rc(8, 1100)
2827
self.delay_sim_time(1)
2828
current_gains = get_roll_gains("set-original")
2829
if not gains_same(original_gains, current_gains):
2830
raise NotAchievedException("AUTOTUNE original gains not restored")
2831
2832
self.progress("Use autotuned gains")
2833
self.set_rc(8, 1850)
2834
self.delay_sim_time(1)
2835
tuned_gains = get_roll_gains("tuned")
2836
if gains_same(tuned_gains, original_gains):
2837
raise NotAchievedException("AUTOTUNE tuned gains same as pre gains")
2838
if gains_same(tuned_gains, scaled_original_gains):
2839
raise NotAchievedException("AUTOTUNE tuned gains same as scaled pre gains")
2840
2841
self.progress("land without changing mode")
2842
self.set_rc(3, 1000)
2843
self.wait_altitude(-1, 5, relative=True)
2844
self.wait_disarmed()
2845
self.progress("Check gains are still there after disarm")
2846
disarmed_gains = get_roll_gains("post-disarm")
2847
if not gains_same(tuned_gains, disarmed_gains):
2848
raise NotAchievedException("AUTOTUNE gains not present on disarm")
2849
2850
self.reboot_sitl()
2851
self.progress("Check gains are still there after reboot")
2852
reboot_gains = get_roll_gains("post-reboot")
2853
if not gains_same(tuned_gains, reboot_gains):
2854
raise NotAchievedException("AUTOTUNE gains not present on reboot")
2855
self.progress("Check FLTT is unchanged")
2856
if pre_rllt != self.get_parameter("ATC_RAT_RLL_FLTT"):
2857
raise NotAchievedException("AUTOTUNE FLTT was modified")
2858
return
2859
2860
raise NotAchievedException("AUTOTUNE failed (%u seconds)" %
2861
(self.get_sim_time() - tstart))
2862
2863
def EK3_RNG_USE_HGT(self):
2864
'''basic tests for using rangefinder when speed and height below thresholds'''
2865
# this takes advantage of some code in send_status_report
2866
# which only reports terrain variance when using switch-height
2867
# and using the rangefinder
2868
self.context_push()
2869
2870
self.set_analog_rangefinder_parameters()
2871
# set use-height to 20m (the parameter is a percentage of max range)
2872
self.set_parameters({
2873
'EK3_RNG_USE_HGT': 200000 / self.get_parameter('RNGFND1_MAX_CM'),
2874
})
2875
self.reboot_sitl()
2876
2877
# add a listener that verifies rangefinder innovations look good
2878
global alt
2879
alt = None
2880
2881
def verify_innov(mav, m):
2882
global alt
2883
if m.get_type() == 'GLOBAL_POSITION_INT':
2884
alt = m.relative_alt * 0.001 # mm -> m
2885
return
2886
if m.get_type() != 'EKF_STATUS_REPORT':
2887
return
2888
if alt is None:
2889
return
2890
if alt > 1 and alt < 8: # 8 is very low, but it takes a long time to start to use the rangefinder again
2891
zero_variance_wanted = False
2892
elif alt > 20:
2893
zero_variance_wanted = True
2894
else:
2895
return
2896
variance = m.terrain_alt_variance
2897
if zero_variance_wanted and variance > 0.00001:
2898
raise NotAchievedException("Wanted zero variance at height %f, got %f" % (alt, variance))
2899
elif not zero_variance_wanted and variance == 0:
2900
raise NotAchievedException("Wanted non-zero variance at alt=%f, got zero" % alt)
2901
2902
self.install_message_hook_context(verify_innov)
2903
2904
self.takeoff(50, mode='GUIDED')
2905
current_alt = self.mav.location().alt
2906
target_position = mavutil.location(
2907
-35.362938,
2908
149.165185,
2909
current_alt,
2910
0
2911
)
2912
2913
self.fly_guided_move_to(target_position, timeout=300)
2914
2915
self.change_mode('LAND')
2916
self.wait_disarmed()
2917
2918
self.context_pop()
2919
2920
self.reboot_sitl()
2921
2922
def TerrainDBPreArm(self):
2923
'''test that pre-arm checks are working corrctly for terrain database'''
2924
self.context_push()
2925
2926
self.progress("# Load msission with terrain alt")
2927
# load the waypoint
2928
num_wp = self.load_mission("terrain_wp.txt", strict=False)
2929
if not num_wp:
2930
raise NotAchievedException("load terrain_wp failed")
2931
2932
self.set_analog_rangefinder_parameters()
2933
self.set_parameters({
2934
"WPNAV_RFND_USE": 1,
2935
"TERRAIN_ENABLE": 1,
2936
})
2937
self.reboot_sitl()
2938
self.wait_ready_to_arm()
2939
2940
# make sure we can still arm with valid rangefinder and terrain db disabled
2941
self.set_parameter("TERRAIN_ENABLE", 0)
2942
self.wait_ready_to_arm()
2943
self.progress("# Vehicle armed with terrain db disabled")
2944
2945
# make sure we can't arm with terrain db enabled and no rangefinder in us
2946
self.set_parameter("WPNAV_RFND_USE", 0)
2947
self.assert_prearm_failure("terrain disabled")
2948
2949
self.context_pop()
2950
2951
self.reboot_sitl()
2952
2953
def CopterMission(self):
2954
'''fly mission which tests a significant number of commands'''
2955
# Fly mission #1
2956
self.progress("# Load copter_mission")
2957
# load the waypoint count
2958
num_wp = self.load_mission("copter_mission.txt", strict=False)
2959
if not num_wp:
2960
raise NotAchievedException("load copter_mission failed")
2961
2962
self.fly_loaded_mission(num_wp)
2963
2964
self.progress("Auto mission completed: passed!")
2965
2966
def set_origin(self, loc, timeout=60):
2967
'''set the GPS global origin to loc'''
2968
tstart = self.get_sim_time()
2969
while True:
2970
if self.get_sim_time_cached() - tstart > timeout:
2971
raise AutoTestTimeoutException("Did not get non-zero lat")
2972
target_system = 1
2973
self.mav.mav.set_gps_global_origin_send(
2974
target_system,
2975
int(loc.lat * 1e7),
2976
int(loc.lng * 1e7),
2977
int(loc.alt * 1e3)
2978
)
2979
gpi = self.assert_receive_message('GLOBAL_POSITION_INT')
2980
self.progress("gpi=%s" % str(gpi))
2981
if gpi.lat != 0:
2982
break
2983
2984
def FarOrigin(self):
2985
'''fly a mission far from the vehicle origin'''
2986
# Fly mission #1
2987
self.set_parameters({
2988
"SIM_GPS1_ENABLE": 0,
2989
})
2990
self.reboot_sitl()
2991
nz = mavutil.location(-43.730171, 169.983118, 1466.3, 270)
2992
self.set_origin(nz)
2993
self.set_parameters({
2994
"SIM_GPS1_ENABLE": 1,
2995
})
2996
self.progress("# Load copter_mission")
2997
# load the waypoint count
2998
num_wp = self.load_mission("copter_mission.txt", strict=False)
2999
if not num_wp:
3000
raise NotAchievedException("load copter_mission failed")
3001
3002
self.fly_loaded_mission(num_wp)
3003
3004
self.progress("Auto mission completed: passed!")
3005
3006
def fly_loaded_mission(self, num_wp):
3007
'''fly mission loaded on vehicle. FIXME: get num_wp from vehicle'''
3008
self.progress("test: Fly a mission from 1 to %u" % num_wp)
3009
self.set_current_waypoint(1)
3010
3011
self.change_mode("LOITER")
3012
self.wait_ready_to_arm()
3013
self.arm_vehicle()
3014
3015
# switch into AUTO mode and raise throttle
3016
self.change_mode("AUTO")
3017
self.set_rc(3, 1500)
3018
3019
# fly the mission
3020
self.wait_waypoint(0, num_wp-1, timeout=500)
3021
3022
# set throttle to minimum
3023
self.zero_throttle()
3024
3025
# wait for disarm
3026
self.wait_disarmed()
3027
self.progress("MOTORS DISARMED OK")
3028
3029
def CANGPSCopterMission(self):
3030
'''fly mission which tests normal operation alongside CAN GPS'''
3031
self.set_parameters({
3032
"CAN_P1_DRIVER": 1,
3033
"GPS1_TYPE": 9,
3034
"GPS2_TYPE": 9,
3035
# disable simulated GPS, so only via DroneCAN
3036
"SIM_GPS1_ENABLE": 0,
3037
"SIM_GPS2_ENABLE": 0,
3038
# this ensures we use DroneCAN baro and compass
3039
"SIM_BARO_COUNT" : 0,
3040
"SIM_MAG1_DEVID" : 0,
3041
"SIM_MAG2_DEVID" : 0,
3042
"SIM_MAG3_DEVID" : 0,
3043
"COMPASS_USE2" : 0,
3044
"COMPASS_USE3" : 0,
3045
# use DroneCAN rangefinder
3046
"RNGFND1_TYPE" : 24,
3047
"RNGFND1_MAX_CM" : 11000,
3048
# use DroneCAN battery monitoring, and enforce with a arming voltage
3049
"BATT_MONITOR" : 8,
3050
"BATT_ARM_VOLT" : 12.0,
3051
"SIM_SPEEDUP": 2,
3052
})
3053
3054
self.context_push()
3055
self.set_parameter("ARMING_CHECK", 1 << 3)
3056
self.context_collect('STATUSTEXT')
3057
3058
self.reboot_sitl()
3059
# Test UAVCAN GPS ordering working
3060
gps1_det_text = self.wait_text("GPS 1: specified as DroneCAN.*", regex=True, check_context=True)
3061
gps2_det_text = self.wait_text("GPS 2: specified as DroneCAN.*", regex=True, check_context=True)
3062
gps1_nodeid = int(gps1_det_text.split('-')[1])
3063
gps2_nodeid = int(gps2_det_text.split('-')[1])
3064
if gps1_nodeid is None or gps2_nodeid is None:
3065
raise NotAchievedException("GPS not ordered per the order of Node IDs")
3066
3067
self.context_stop_collecting('STATUSTEXT')
3068
3069
GPS_Order_Tests = [[gps2_nodeid, gps2_nodeid, gps2_nodeid, 0,
3070
"PreArm: Same Node Id {} set for multiple GPS".format(gps2_nodeid)],
3071
[gps1_nodeid, int(gps2_nodeid/2), gps1_nodeid, 0,
3072
"Selected GPS Node {} not set as instance {}".format(int(gps2_nodeid/2), 2)],
3073
[int(gps1_nodeid/2), gps2_nodeid, 0, gps2_nodeid,
3074
"Selected GPS Node {} not set as instance {}".format(int(gps1_nodeid/2), 1)],
3075
[gps1_nodeid, gps2_nodeid, gps1_nodeid, gps2_nodeid, ""],
3076
[gps2_nodeid, gps1_nodeid, gps2_nodeid, gps1_nodeid, ""],
3077
[gps1_nodeid, 0, gps1_nodeid, gps2_nodeid, ""],
3078
[0, gps2_nodeid, gps1_nodeid, gps2_nodeid, ""]]
3079
for case in GPS_Order_Tests:
3080
self.progress("############################### Trying Case: " + str(case))
3081
self.set_parameters({
3082
"GPS1_CAN_OVRIDE": case[0],
3083
"GPS2_CAN_OVRIDE": case[1],
3084
})
3085
self.drain_mav()
3086
self.context_collect('STATUSTEXT')
3087
self.reboot_sitl()
3088
gps1_det_text = None
3089
gps2_det_text = None
3090
try:
3091
gps1_det_text = self.wait_text("GPS 1: specified as DroneCAN.*", regex=True, check_context=True)
3092
except AutoTestTimeoutException:
3093
pass
3094
try:
3095
gps2_det_text = self.wait_text("GPS 2: specified as DroneCAN.*", regex=True, check_context=True)
3096
except AutoTestTimeoutException:
3097
pass
3098
3099
self.context_stop_collecting('STATUSTEXT')
3100
self.change_mode('LOITER')
3101
if case[2] == 0 and case[3] == 0:
3102
if gps1_det_text or gps2_det_text:
3103
raise NotAchievedException("Failed ordering for requested CASE:", case)
3104
3105
if case[2] == 0 or case[3] == 0:
3106
if bool(gps1_det_text is not None) == bool(gps2_det_text is not None):
3107
print(gps1_det_text)
3108
print(gps2_det_text)
3109
raise NotAchievedException("Failed ordering for requested CASE:", case)
3110
3111
if gps1_det_text:
3112
if case[2] != int(gps1_det_text.split('-')[1]):
3113
raise NotAchievedException("Failed ordering for requested CASE:", case)
3114
if gps2_det_text:
3115
if case[3] != int(gps2_det_text.split('-')[1]):
3116
raise NotAchievedException("Failed ordering for requested CASE:", case)
3117
if len(case[4]):
3118
self.context_collect('STATUSTEXT')
3119
self.run_cmd(
3120
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
3121
p1=1, # ARM
3122
timeout=10,
3123
want_result=mavutil.mavlink.MAV_RESULT_FAILED,
3124
)
3125
self.wait_statustext(case[4], check_context=True)
3126
self.context_stop_collecting('STATUSTEXT')
3127
self.progress("############################### All GPS Order Cases Tests Passed")
3128
self.progress("############################### Test Healthy Prearm check")
3129
self.set_parameter("ARMING_CHECK", 1)
3130
self.stop_sup_program(instance=0)
3131
self.start_sup_program(instance=0, args="-M")
3132
self.stop_sup_program(instance=1)
3133
self.start_sup_program(instance=1, args="-M")
3134
self.delay_sim_time(2)
3135
self.context_collect('STATUSTEXT')
3136
self.run_cmd(
3137
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
3138
p1=1, # ARM
3139
timeout=10,
3140
want_result=mavutil.mavlink.MAV_RESULT_FAILED,
3141
)
3142
self.wait_statustext(".*Node .* unhealthy", check_context=True, regex=True)
3143
self.stop_sup_program(instance=0)
3144
self.start_sup_program(instance=0)
3145
self.stop_sup_program(instance=1)
3146
self.start_sup_program(instance=1)
3147
self.context_stop_collecting('STATUSTEXT')
3148
self.context_pop()
3149
3150
self.set_parameters({
3151
# use DroneCAN ESCs for flight
3152
"CAN_D1_UC_ESC_BM" : 0x0f,
3153
# this stops us using local servo output, guaranteeing we are
3154
# flying on DroneCAN ESCs
3155
"SIM_CAN_SRV_MSK" : 0xFF,
3156
# we can do the flight faster
3157
"SIM_SPEEDUP" : 5,
3158
})
3159
3160
self.CopterMission()
3161
3162
def TakeoffAlt(self):
3163
'''Test Takeoff command altitude'''
3164
# Test case #1 (set target altitude to relative -10m from the ground, -10m is invalid, so it is set to 1m)
3165
self.progress("Testing relative alt from the ground")
3166
self.do_takeoff_alt("copter_takeoff.txt", 1, False)
3167
# Test case #2 (set target altitude to relative -10m during flight, -10m is invalid, so keeps current altitude)
3168
self.progress("Testing relative alt during flight")
3169
self.do_takeoff_alt("copter_takeoff.txt", 10, True)
3170
3171
self.progress("Takeoff mission completed: passed!")
3172
3173
def do_takeoff_alt(self, mission_file, target_alt, during_flight=False):
3174
self.progress("# Load %s" % mission_file)
3175
# load the waypoint count
3176
num_wp = self.load_mission(mission_file, strict=False)
3177
if not num_wp:
3178
raise NotAchievedException("load %s failed" % mission_file)
3179
3180
self.set_current_waypoint(1)
3181
3182
self.change_mode("GUIDED")
3183
self.wait_ready_to_arm()
3184
self.arm_vehicle()
3185
3186
if during_flight:
3187
self.user_takeoff(alt_min=target_alt)
3188
3189
# switch into AUTO mode and raise throttle
3190
self.change_mode("AUTO")
3191
self.set_rc(3, 1500)
3192
3193
# fly the mission
3194
self.wait_waypoint(0, num_wp-1, timeout=500)
3195
3196
# altitude check
3197
self.wait_altitude(target_alt - 1 , target_alt + 1, relative=True)
3198
3199
self.change_mode('LAND')
3200
3201
# set throttle to minimum
3202
self.zero_throttle()
3203
3204
# wait for disarm
3205
self.wait_disarmed()
3206
self.progress("MOTORS DISARMED OK")
3207
3208
def GuidedEKFLaneChange(self):
3209
'''test lane change with GPS diff on startup'''
3210
self.set_parameters({
3211
"EK3_SRC1_POSZ": 3,
3212
"EK3_AFFINITY" : 1,
3213
"GPS2_TYPE" : 1,
3214
"SIM_GPS2_ENABLE" : 1,
3215
"SIM_GPS2_GLTCH_Z" : -30
3216
})
3217
self.reboot_sitl()
3218
3219
self.change_mode("GUIDED")
3220
self.wait_ready_to_arm()
3221
3222
self.delay_sim_time(10, reason='"both EKF lanes to init"')
3223
3224
self.set_parameters({
3225
"SIM_GPS2_GLTCH_Z" : 0
3226
})
3227
3228
self.delay_sim_time(20, reason="EKF to do a position Z reset")
3229
3230
self.arm_vehicle()
3231
self.user_takeoff(alt_min=20)
3232
gps_alt = self.get_altitude(altitude_source='GPS_RAW_INT.alt')
3233
self.progress("Initial guided alt=%.1fm" % gps_alt)
3234
3235
self.context_collect('STATUSTEXT')
3236
self.progress("force a lane change")
3237
self.set_parameters({
3238
"INS_ACCOFFS_X" : 5
3239
})
3240
self.wait_statustext("EKF3 lane switch 1", timeout=10, check_context=True)
3241
3242
self.watch_altitude_maintained(
3243
altitude_min=gps_alt-2,
3244
altitude_max=gps_alt+2,
3245
altitude_source='GPS_RAW_INT.alt',
3246
minimum_duration=10,
3247
)
3248
3249
self.disarm_vehicle(force=True)
3250
self.reboot_sitl()
3251
3252
def MotorFail(self, ):
3253
"""Test flight with reduced motor efficiency"""
3254
# we only expect an octocopter to survive ATM:
3255
self.MotorFail_test_frame('octa', 8, frame_class=3)
3256
# self.MotorFail_test_frame('hexa', 6, frame_class=2)
3257
# self.MotorFail_test_frame('y6', 6, frame_class=5)
3258
3259
def MotorFail_test_frame(self, model, servo_count, frame_class, fail_servo=0, fail_mul=0.0, holdtime=30):
3260
self.set_parameters({
3261
'FRAME_CLASS': frame_class,
3262
})
3263
self.customise_SITL_commandline([], model=model)
3264
3265
self.takeoff(25, mode="LOITER")
3266
3267
# Get initial values
3268
start_hud = self.assert_receive_message('VFR_HUD')
3269
start_attitude = self.assert_receive_message('ATTITUDE')
3270
3271
hover_time = 5
3272
tstart = self.get_sim_time()
3273
int_error_alt = 0
3274
int_error_yaw_rate = 0
3275
int_error_yaw = 0
3276
self.progress("Hovering for %u seconds" % hover_time)
3277
failed = False
3278
while True:
3279
now = self.get_sim_time_cached()
3280
if now - tstart > holdtime + hover_time:
3281
break
3282
3283
servo = self.assert_receive_message('SERVO_OUTPUT_RAW')
3284
hud = self.assert_receive_message('VFR_HUD')
3285
attitude = self.assert_receive_message('ATTITUDE')
3286
3287
if not failed and now - tstart > hover_time:
3288
self.progress("Killing motor %u (%u%%)" %
3289
(fail_servo+1, fail_mul))
3290
self.set_parameters({
3291
"SIM_ENGINE_MUL": fail_mul,
3292
"SIM_ENGINE_FAIL": 1 << fail_servo,
3293
})
3294
failed = True
3295
3296
if failed:
3297
self.progress("Hold Time: %f/%f" % (now-tstart, holdtime))
3298
3299
servo_pwm = [
3300
servo.servo1_raw,
3301
servo.servo2_raw,
3302
servo.servo3_raw,
3303
servo.servo4_raw,
3304
servo.servo5_raw,
3305
servo.servo6_raw,
3306
servo.servo7_raw,
3307
servo.servo8_raw,
3308
]
3309
3310
self.progress("PWM output per motor")
3311
for i, pwm in enumerate(servo_pwm[0:servo_count]):
3312
if pwm > 1900:
3313
state = "oversaturated"
3314
elif pwm < 1200:
3315
state = "undersaturated"
3316
else:
3317
state = "OK"
3318
3319
if failed and i == fail_servo:
3320
state += " (failed)"
3321
3322
self.progress("servo %u [pwm=%u] [%s]" % (i+1, pwm, state))
3323
3324
alt_delta = hud.alt - start_hud.alt
3325
yawrate_delta = attitude.yawspeed - start_attitude.yawspeed
3326
yaw_delta = attitude.yaw - start_attitude.yaw
3327
3328
self.progress("Alt=%fm (delta=%fm)" % (hud.alt, alt_delta))
3329
self.progress("Yaw rate=%f (delta=%f) (rad/s)" %
3330
(attitude.yawspeed, yawrate_delta))
3331
self.progress("Yaw=%f (delta=%f) (deg)" %
3332
(attitude.yaw, yaw_delta))
3333
3334
dt = self.get_sim_time() - now
3335
int_error_alt += abs(alt_delta/dt)
3336
int_error_yaw_rate += abs(yawrate_delta/dt)
3337
int_error_yaw += abs(yaw_delta/dt)
3338
self.progress("## Error Integration ##")
3339
self.progress(" Altitude: %fm" % int_error_alt)
3340
self.progress(" Yaw rate: %f rad/s" % int_error_yaw_rate)
3341
self.progress(" Yaw: %f deg" % int_error_yaw)
3342
self.progress("----")
3343
3344
if int_error_yaw > 5:
3345
raise NotAchievedException("Vehicle is spinning")
3346
3347
if alt_delta < -20:
3348
raise NotAchievedException("Vehicle is descending")
3349
3350
self.progress("Fixing motors")
3351
self.set_parameter("SIM_ENGINE_FAIL", 0)
3352
3353
self.do_RTL()
3354
3355
def hover_for_interval(self, hover_time):
3356
'''hovers for an interval of hover_time seconds. Returns the bookend
3357
times for that interval (in time-since-boot frame), and the
3358
output throttle level at the end of the period.
3359
'''
3360
self.progress("Hovering for %u seconds" % hover_time)
3361
tstart = self.get_sim_time()
3362
self.delay_sim_time(hover_time, reason='data collection')
3363
vfr_hud = self.poll_message('VFR_HUD')
3364
tend = self.get_sim_time()
3365
return tstart, tend, vfr_hud.throttle
3366
3367
def MotorVibration(self):
3368
"""Test flight with motor vibration"""
3369
# magic tridge EKF type that dramatically speeds up the test
3370
self.set_parameters({
3371
"AHRS_EKF_TYPE": 10,
3372
"INS_LOG_BAT_MASK": 3,
3373
"INS_LOG_BAT_OPT": 0,
3374
"LOG_BITMASK": 958,
3375
"LOG_DISARMED": 0,
3376
"SIM_VIB_MOT_MAX": 350,
3377
# these are real values taken from a 180mm Quad:
3378
"SIM_GYR1_RND": 20,
3379
"SIM_ACC1_RND": 5,
3380
"SIM_ACC2_RND": 5,
3381
"SIM_INS_THR_MIN": 0.1,
3382
})
3383
self.reboot_sitl()
3384
3385
# do a simple up-and-down flight to gather data:
3386
self.takeoff(15, mode="ALT_HOLD")
3387
tstart, tend, hover_throttle = self.hover_for_interval(15)
3388
# if we don't reduce vibes here then the landing detector
3389
# may not trigger
3390
self.set_parameter("SIM_VIB_MOT_MAX", 0)
3391
self.do_RTL()
3392
3393
psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6)
3394
# ignore the first 20Hz and look for a peak at -15dB or more
3395
# it should be at about 190Hz, each bin is 1000/1024Hz wide
3396
ignore_bins = int(100 * 1.024) # start at 100Hz to be safe
3397
freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins]
3398
if numpy.amax(psd["X"][ignore_bins:]) < -15 or freq < 100 or freq > 300:
3399
raise NotAchievedException(
3400
"Did not detect a motor peak, found %f at %f dB" %
3401
(freq, numpy.amax(psd["X"][ignore_bins:])))
3402
else:
3403
self.progress("Detected motor peak at %fHz" % freq)
3404
3405
# now add a notch and check that post-filter the peak is squashed below 40dB
3406
self.set_parameters({
3407
"INS_LOG_BAT_OPT": 2,
3408
"INS_HNTC2_ENABLE": 1,
3409
"INS_HNTC2_FREQ": freq,
3410
"INS_HNTC2_ATT": 50,
3411
"INS_HNTC2_BW": freq/2,
3412
"INS_HNTC2_MODE": 0,
3413
"SIM_VIB_MOT_MAX": 350,
3414
})
3415
self.reboot_sitl()
3416
3417
# do a simple up-and-down flight to gather data:
3418
self.takeoff(15, mode="ALT_HOLD")
3419
tstart, tend, hover_throttle = self.hover_for_interval(15)
3420
self.set_parameter("SIM_VIB_MOT_MAX", 0)
3421
self.do_RTL()
3422
3423
psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6)
3424
freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins]
3425
peakdB = numpy.amax(psd["X"][ignore_bins:])
3426
if peakdB < -23:
3427
self.progress("Did not detect a motor peak, found %f at %f dB" % (freq, peakdB))
3428
else:
3429
raise NotAchievedException("Detected peak %.1f Hz %.2f dB" % (freq, peakdB))
3430
3431
def VisionPosition(self):
3432
"""Disable GPS navigation, enable Vicon input."""
3433
# scribble down a location we can set origin to:
3434
3435
self.customise_SITL_commandline(["--serial5=sim:vicon:"])
3436
self.progress("Waiting for location")
3437
self.change_mode('LOITER')
3438
self.wait_ready_to_arm()
3439
3440
old_pos = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
3441
print("old_pos=%s" % str(old_pos))
3442
3443
# configure EKF to use external nav instead of GPS
3444
ahrs_ekf_type = self.get_parameter("AHRS_EKF_TYPE")
3445
if ahrs_ekf_type == 2:
3446
self.set_parameter("EK2_GPS_TYPE", 3)
3447
if ahrs_ekf_type == 3:
3448
self.set_parameters({
3449
"EK3_SRC1_POSXY": 6,
3450
"EK3_SRC1_VELXY": 6,
3451
"EK3_SRC1_POSZ": 6,
3452
"EK3_SRC1_VELZ": 6,
3453
})
3454
self.set_parameters({
3455
"GPS1_TYPE": 0,
3456
"VISO_TYPE": 1,
3457
"SERIAL5_PROTOCOL": 1,
3458
})
3459
self.reboot_sitl()
3460
# without a GPS or some sort of external prompting, AP
3461
# doesn't send system_time messages. So prompt it:
3462
self.mav.mav.system_time_send(int(time.time() * 1000000), 0)
3463
self.progress("Waiting for non-zero-lat")
3464
tstart = self.get_sim_time()
3465
while True:
3466
if self.get_sim_time_cached() - tstart > 60:
3467
raise AutoTestTimeoutException("Did not get non-zero lat")
3468
self.mav.mav.set_gps_global_origin_send(1,
3469
old_pos.lat,
3470
old_pos.lon,
3471
old_pos.alt)
3472
gpi = self.assert_receive_message('GLOBAL_POSITION_INT')
3473
self.progress("gpi=%s" % str(gpi))
3474
if gpi.lat != 0:
3475
break
3476
3477
self.takeoff()
3478
self.set_rc(1, 1600)
3479
tstart = self.get_sim_time()
3480
while True:
3481
vicon_pos = self.assert_receive_message('VISION_POSITION_ESTIMATE')
3482
# print("vpe=%s" % str(vicon_pos))
3483
# gpi = self.assert_receive_message('GLOBAL_POSITION_INT')
3484
# self.progress("gpi=%s" % str(gpi))
3485
if vicon_pos.x > 40:
3486
break
3487
3488
if self.get_sim_time_cached() - tstart > 100:
3489
raise AutoTestTimeoutException("Vicon showed no movement")
3490
3491
# recenter controls:
3492
self.set_rc(1, 1500)
3493
self.progress("# Enter RTL")
3494
self.change_mode('RTL')
3495
self.set_rc(3, 1500)
3496
tstart = self.get_sim_time()
3497
# self.install_messageprinter_handlers_context(['SIMSTATE', 'GLOBAL_POSITION_INT'])
3498
self.wait_disarmed(timeout=200)
3499
3500
def BodyFrameOdom(self):
3501
"""Disable GPS navigation, enable input of VISION_POSITION_DELTA."""
3502
3503
if self.get_parameter("AHRS_EKF_TYPE") != 3:
3504
# only tested on this EKF
3505
return
3506
3507
self.customise_SITL_commandline(["--serial5=sim:vicon:"])
3508
3509
if self.current_onboard_log_contains_message("XKFD"):
3510
raise NotAchievedException("Found unexpected XKFD message")
3511
3512
# scribble down a location we can set origin to:
3513
self.progress("Waiting for location")
3514
self.change_mode('LOITER')
3515
self.wait_ready_to_arm()
3516
3517
old_pos = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
3518
print("old_pos=%s" % str(old_pos))
3519
3520
# configure EKF to use external nav instead of GPS
3521
self.set_parameters({
3522
"EK3_SRC1_POSXY": 6,
3523
"EK3_SRC1_VELXY": 6,
3524
"EK3_SRC1_POSZ": 6,
3525
"EK3_SRC1_VELZ": 6,
3526
"GPS1_TYPE": 0,
3527
"VISO_TYPE": 1,
3528
"SERIAL5_PROTOCOL": 1,
3529
"SIM_VICON_TMASK": 8, # send VISION_POSITION_DELTA
3530
})
3531
self.reboot_sitl()
3532
# without a GPS or some sort of external prompting, AP
3533
# doesn't send system_time messages. So prompt it:
3534
self.mav.mav.system_time_send(int(time.time() * 1000000), 0)
3535
self.progress("Waiting for non-zero-lat")
3536
tstart = self.get_sim_time()
3537
while True:
3538
self.mav.mav.set_gps_global_origin_send(1,
3539
old_pos.lat,
3540
old_pos.lon,
3541
old_pos.alt)
3542
gpi = self.mav.recv_match(type='GLOBAL_POSITION_INT',
3543
blocking=True)
3544
self.progress("gpi=%s" % str(gpi))
3545
if gpi.lat != 0:
3546
break
3547
3548
if self.get_sim_time_cached() - tstart > 60:
3549
raise AutoTestTimeoutException("Did not get non-zero lat")
3550
3551
self.takeoff(alt_min=5, mode='ALT_HOLD', require_absolute=False, takeoff_throttle=1800)
3552
self.change_mode('LAND')
3553
# TODO: something more elaborate here - EKF will only aid
3554
# relative position
3555
self.wait_disarmed()
3556
if not self.current_onboard_log_contains_message("XKFD"):
3557
raise NotAchievedException("Did not find expected XKFD message")
3558
3559
def FlyMissionTwice(self):
3560
'''fly a mission twice in a row without changing modes in between.
3561
Seeks to show bugs in mission state machine'''
3562
3563
self.upload_simple_relhome_mission([
3564
(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 20),
3565
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 20, 0, 20),
3566
(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),
3567
])
3568
3569
num_wp = self.get_mission_count()
3570
self.set_parameter("AUTO_OPTIONS", 3)
3571
self.change_mode('AUTO')
3572
self.wait_ready_to_arm()
3573
for i in 1, 2:
3574
self.progress("run %u" % i)
3575
self.arm_vehicle()
3576
self.wait_waypoint(num_wp-1, num_wp-1)
3577
self.wait_disarmed()
3578
self.delay_sim_time(20)
3579
3580
def FlyMissionTwiceWithReset(self):
3581
'''Fly a mission twice in a row without changing modes in between.
3582
Allow the mission to complete, then reset the mission state machine and restart the mission.'''
3583
3584
self.upload_simple_relhome_mission([
3585
(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 20),
3586
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 20, 0, 20),
3587
(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),
3588
])
3589
3590
num_wp = self.get_mission_count()
3591
self.set_parameter("AUTO_OPTIONS", 3)
3592
self.change_mode('AUTO')
3593
self.wait_ready_to_arm()
3594
3595
for i in 1, 2:
3596
self.progress("run %u" % i)
3597
# Use the "Reset Mission" param of DO_SET_MISSION_CURRENT to reset mission state machine
3598
self.set_current_waypoint_using_mav_cmd_do_set_mission_current(seq=0, reset=1)
3599
self.arm_vehicle()
3600
self.wait_waypoint(num_wp-1, num_wp-1)
3601
self.wait_disarmed()
3602
self.delay_sim_time(20)
3603
3604
def MissionIndexValidity(self):
3605
'''Confirm that attempting to select an invalid mission item is rejected.'''
3606
3607
self.upload_simple_relhome_mission([
3608
(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 20),
3609
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 20, 0, 20),
3610
(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),
3611
])
3612
3613
num_wp = self.get_mission_count()
3614
accepted_indices = [0, 1, num_wp-1]
3615
denied_indices = [-1, num_wp]
3616
3617
for seq in accepted_indices:
3618
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_MISSION_CURRENT,
3619
p1=seq,
3620
timeout=1,
3621
want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED)
3622
3623
for seq in denied_indices:
3624
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_MISSION_CURRENT,
3625
p1=seq,
3626
timeout=1,
3627
want_result=mavutil.mavlink.MAV_RESULT_DENIED)
3628
3629
def InvalidJumpTags(self):
3630
'''Verify the behaviour when selecting invalid jump tags.'''
3631
3632
MAX_TAG_NUM = 65535
3633
# Jump tag is not present, so expect FAILED
3634
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_JUMP_TAG,
3635
p1=MAX_TAG_NUM,
3636
timeout=1,
3637
want_result=mavutil.mavlink.MAV_RESULT_FAILED)
3638
3639
# Jump tag is too big, so expect DENIED
3640
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_JUMP_TAG,
3641
p1=MAX_TAG_NUM+1,
3642
timeout=1,
3643
want_result=mavutil.mavlink.MAV_RESULT_DENIED)
3644
3645
def GPSViconSwitching(self):
3646
"""Fly GPS and Vicon switching test"""
3647
"""Setup parameters including switching to EKF3"""
3648
self.set_parameters({
3649
"VISO_TYPE": 2, # enable vicon
3650
"SERIAL5_PROTOCOL": 2,
3651
"EK3_ENABLE": 1,
3652
"EK3_SRC2_POSXY": 6, # External Nav
3653
"EK3_SRC2_POSZ": 6, # External Nav
3654
"EK3_SRC2_VELXY": 6, # External Nav
3655
"EK3_SRC2_VELZ": 6, # External Nav
3656
"EK3_SRC2_YAW": 6, # External Nav
3657
"RC7_OPTION": 80, # RC aux switch 7 set to Viso Align
3658
"RC8_OPTION": 90, # RC aux switch 8 set to EKF source selector
3659
"EK2_ENABLE": 0,
3660
"AHRS_EKF_TYPE": 3,
3661
})
3662
self.customise_SITL_commandline(["--serial5=sim:vicon:"])
3663
3664
# switch to use GPS
3665
self.set_rc(8, 1000)
3666
3667
# ensure we can get a global position:
3668
self.poll_home_position(timeout=120)
3669
3670
# record starting position
3671
old_pos = self.get_global_position_int()
3672
print("old_pos=%s" % str(old_pos))
3673
3674
# align vicon yaw with ahrs heading
3675
self.set_rc(7, 2000)
3676
3677
# takeoff to 10m in Loiter
3678
self.progress("Moving to ensure location is tracked")
3679
self.takeoff(10, mode="LOITER", require_absolute=True, timeout=720)
3680
3681
# fly forward in Loiter
3682
self.set_rc(2, 1300)
3683
3684
# disable vicon
3685
self.set_parameter("SIM_VICON_FAIL", 1)
3686
3687
# ensure vehicle remain in Loiter for 15 seconds
3688
tstart = self.get_sim_time()
3689
while self.get_sim_time() - tstart < 15:
3690
if not self.mode_is('LOITER'):
3691
raise NotAchievedException("Expected to stay in loiter for >15 seconds")
3692
3693
# re-enable vicon
3694
self.set_parameter("SIM_VICON_FAIL", 0)
3695
3696
# switch to vicon, disable GPS and wait 10sec to ensure vehicle remains in Loiter
3697
self.set_rc(8, 1500)
3698
self.set_parameter("GPS1_TYPE", 0)
3699
3700
# ensure vehicle remain in Loiter for 15 seconds
3701
tstart = self.get_sim_time()
3702
while self.get_sim_time() - tstart < 15:
3703
if not self.mode_is('LOITER'):
3704
raise NotAchievedException("Expected to stay in loiter for >15 seconds")
3705
3706
# RTL and check vehicle arrives within 10m of home
3707
self.set_rc(2, 1500)
3708
self.do_RTL()
3709
3710
def RTLSpeed(self):
3711
"""Test RTL Speed parameters"""
3712
rtl_speed_ms = 7
3713
wpnav_speed_ms = 4
3714
wpnav_accel_mss = 3
3715
tolerance = 0.5
3716
self.load_mission("copter_rtl_speed.txt")
3717
self.set_parameters({
3718
'WPNAV_ACCEL': wpnav_accel_mss * 100,
3719
'RTL_SPEED': rtl_speed_ms * 100,
3720
'WPNAV_SPEED': wpnav_speed_ms * 100,
3721
})
3722
self.change_mode('LOITER')
3723
self.wait_ready_to_arm()
3724
self.arm_vehicle()
3725
self.change_mode('AUTO')
3726
self.set_rc(3, 1600)
3727
self.wait_altitude(19, 25, relative=True)
3728
self.wait_groundspeed(wpnav_speed_ms-tolerance, wpnav_speed_ms+tolerance)
3729
self.monitor_groundspeed(wpnav_speed_ms, timeout=20)
3730
self.change_mode('RTL')
3731
self.wait_groundspeed(rtl_speed_ms-tolerance, rtl_speed_ms+tolerance)
3732
self.monitor_groundspeed(rtl_speed_ms, timeout=5)
3733
self.change_mode('AUTO')
3734
self.wait_groundspeed(0-tolerance, 0+tolerance)
3735
self.wait_groundspeed(wpnav_speed_ms-tolerance, wpnav_speed_ms+tolerance)
3736
self.monitor_groundspeed(wpnav_speed_ms, tolerance=0.6, timeout=5)
3737
self.do_RTL()
3738
3739
def NavDelay(self):
3740
"""Fly a simple mission that has a delay in it."""
3741
3742
self.load_mission("copter_nav_delay.txt")
3743
3744
self.set_parameter("DISARM_DELAY", 0)
3745
3746
self.change_mode("LOITER")
3747
self.wait_ready_to_arm()
3748
3749
self.arm_vehicle()
3750
self.change_mode("AUTO")
3751
self.set_rc(3, 1600)
3752
count_start = -1
3753
count_stop = -1
3754
tstart = self.get_sim_time()
3755
last_mission_current_msg = 0
3756
last_seq = None
3757
while self.armed(): # we RTL at end of mission
3758
now = self.get_sim_time_cached()
3759
if now - tstart > 200:
3760
raise AutoTestTimeoutException("Did not disarm as expected")
3761
m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True)
3762
at_delay_item = ""
3763
if m.seq == 3:
3764
at_delay_item = "(At delay item)"
3765
if count_start == -1:
3766
count_start = now
3767
if ((now - last_mission_current_msg) > 1 or m.seq != last_seq):
3768
dist = None
3769
x = self.mav.messages.get("NAV_CONTROLLER_OUTPUT", None)
3770
if x is not None:
3771
dist = x.wp_dist
3772
self.progress("MISSION_CURRENT.seq=%u dist=%s %s" %
3773
(m.seq, dist, at_delay_item))
3774
last_mission_current_msg = self.get_sim_time_cached()
3775
last_seq = m.seq
3776
if m.seq > 3:
3777
if count_stop == -1:
3778
count_stop = now
3779
calculated_delay = count_stop - count_start
3780
want_delay = 59 # should reflect what's in the mission file
3781
self.progress("Stopped for %u seconds (want >=%u seconds)" %
3782
(calculated_delay, want_delay))
3783
if calculated_delay < want_delay:
3784
raise NotAchievedException("Did not delay for long enough")
3785
3786
def RangeFinder(self):
3787
'''Test RangeFinder Basic Functionality'''
3788
self.progress("Making sure we don't ordinarily get RANGEFINDER")
3789
m = self.mav.recv_match(type='RANGEFINDER',
3790
blocking=True,
3791
timeout=5)
3792
3793
if m is not None:
3794
raise NotAchievedException("Received unexpected RANGEFINDER msg")
3795
3796
# may need to force a rotation if some other test has used the
3797
# rangefinder...
3798
self.progress("Ensure no RFND messages in log")
3799
self.set_parameter("LOG_DISARMED", 1)
3800
if self.current_onboard_log_contains_message("RFND"):
3801
raise NotAchievedException("Found unexpected RFND message")
3802
3803
self.set_analog_rangefinder_parameters()
3804
self.set_parameter("RC9_OPTION", 10) # rangefinder
3805
self.set_rc(9, 2000)
3806
3807
self.reboot_sitl()
3808
3809
self.progress("Making sure we now get RANGEFINDER messages")
3810
m = self.assert_receive_message('RANGEFINDER', timeout=10)
3811
3812
self.progress("Checking RangeFinder is marked as enabled in mavlink")
3813
m = self.mav.recv_match(type='SYS_STATUS',
3814
blocking=True,
3815
timeout=10)
3816
flags = m.onboard_control_sensors_enabled
3817
if not flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION:
3818
raise NotAchievedException("Laser not enabled in SYS_STATUS")
3819
self.progress("Disabling laser using switch")
3820
self.set_rc(9, 1000)
3821
self.delay_sim_time(1)
3822
self.progress("Checking RangeFinder is marked as disabled in mavlink")
3823
m = self.mav.recv_match(type='SYS_STATUS',
3824
blocking=True,
3825
timeout=10)
3826
flags = m.onboard_control_sensors_enabled
3827
if flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION:
3828
raise NotAchievedException("Laser enabled in SYS_STATUS")
3829
3830
self.progress("Re-enabling rangefinder")
3831
self.set_rc(9, 2000)
3832
self.delay_sim_time(1)
3833
m = self.mav.recv_match(type='SYS_STATUS',
3834
blocking=True,
3835
timeout=10)
3836
flags = m.onboard_control_sensors_enabled
3837
if not flags & mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION:
3838
raise NotAchievedException("Laser not enabled in SYS_STATUS")
3839
3840
self.takeoff(10, mode="LOITER")
3841
3842
m_r = self.mav.recv_match(type='RANGEFINDER',
3843
blocking=True)
3844
m_p = self.mav.recv_match(type='GLOBAL_POSITION_INT',
3845
blocking=True)
3846
3847
if abs(m_r.distance - m_p.relative_alt/1000) > 1:
3848
raise NotAchievedException(
3849
"rangefinder/global position int mismatch %0.2f vs %0.2f" %
3850
(m_r.distance, m_p.relative_alt/1000))
3851
3852
self.land_and_disarm()
3853
3854
if not self.current_onboard_log_contains_message("RFND"):
3855
raise NotAchievedException("Did not see expected RFND message")
3856
3857
def SplineTerrain(self):
3858
'''Test Splines and Terrain'''
3859
self.set_parameter("TERRAIN_ENABLE", 0)
3860
self.fly_mission("wp.txt")
3861
3862
def WPNAV_SPEED(self):
3863
'''ensure resetting WPNAV_SPEED during a mission works'''
3864
3865
loc = self.poll_home_position()
3866
alt = 20
3867
loc.alt = alt
3868
items = []
3869
3870
# 100 waypoints in a line, 10m apart in a northerly direction
3871
# for i in range(1, 100):
3872
# items.append((mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, i*10, 0, alt))
3873
3874
# 1 waypoint a long way away
3875
items.append((mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2000, 0, alt),)
3876
3877
items.append((mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0))
3878
3879
self.upload_simple_relhome_mission(items)
3880
3881
start_speed_ms = self.get_parameter('WPNAV_SPEED') / 100.0
3882
3883
self.takeoff(20)
3884
self.change_mode('AUTO')
3885
self.wait_groundspeed(start_speed_ms-1, start_speed_ms+1, minimum_duration=10)
3886
3887
for speed_ms in 7, 8, 7, 8, 9, 10, 11, 7:
3888
self.set_parameter('WPNAV_SPEED', speed_ms*100)
3889
self.wait_groundspeed(speed_ms-1, speed_ms+1, minimum_duration=10)
3890
self.do_RTL()
3891
3892
def WPNAV_SPEED_UP(self):
3893
'''Change speed (up) during mission'''
3894
3895
items = []
3896
3897
# 1 waypoint a long way up
3898
items.append((mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 20000),)
3899
3900
items.append((mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0))
3901
3902
self.upload_simple_relhome_mission(items)
3903
3904
start_speed_ms = self.get_parameter('WPNAV_SPEED_UP') / 100.0
3905
3906
minimum_duration = 5
3907
3908
self.takeoff(20)
3909
self.change_mode('AUTO')
3910
self.wait_climbrate(start_speed_ms-1, start_speed_ms+1, minimum_duration=minimum_duration)
3911
3912
for speed_ms in 7, 8, 7, 8, 6, 2:
3913
self.set_parameter('WPNAV_SPEED_UP', speed_ms*100)
3914
self.wait_climbrate(speed_ms-1, speed_ms+1, minimum_duration=minimum_duration)
3915
self.do_RTL(timeout=240)
3916
3917
def WPNAV_SPEED_DN(self):
3918
'''Change speed (down) during mission'''
3919
3920
items = []
3921
3922
# 1 waypoint a long way back down
3923
items.append((mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 10),)
3924
3925
items.append((mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0))
3926
3927
self.upload_simple_relhome_mission(items)
3928
3929
minimum_duration = 5
3930
3931
self.takeoff(500, timeout=70)
3932
self.change_mode('AUTO')
3933
3934
start_speed_ms = self.get_parameter('WPNAV_SPEED_DN') / 100.0
3935
self.wait_climbrate(-start_speed_ms-1, -start_speed_ms+1, minimum_duration=minimum_duration)
3936
3937
for speed_ms in 7, 8, 7, 8, 6, 2:
3938
self.set_parameter('WPNAV_SPEED_DN', speed_ms*100)
3939
self.wait_climbrate(-speed_ms-1, -speed_ms+1, minimum_duration=minimum_duration)
3940
self.do_RTL()
3941
3942
def fly_mission(self, filename, strict=True):
3943
num_wp = self.load_mission(filename, strict=strict)
3944
self.set_parameter("AUTO_OPTIONS", 3)
3945
self.change_mode('AUTO')
3946
self.wait_ready_to_arm()
3947
self.arm_vehicle()
3948
self.wait_waypoint(num_wp-1, num_wp-1)
3949
self.wait_disarmed()
3950
3951
def fly_generic_mission(self, filename, strict=True):
3952
num_wp = self.load_generic_mission(filename, strict=strict)
3953
self.set_parameter("AUTO_OPTIONS", 3)
3954
self.change_mode('AUTO')
3955
self.wait_ready_to_arm()
3956
self.arm_vehicle()
3957
self.wait_waypoint(num_wp-1, num_wp-1)
3958
self.wait_disarmed()
3959
3960
def SurfaceTracking(self):
3961
'''Test Surface Tracking'''
3962
ex = None
3963
self.context_push()
3964
3965
self.install_terrain_handlers_context()
3966
3967
try:
3968
self.set_analog_rangefinder_parameters()
3969
self.set_parameter("RC9_OPTION", 10) # rangefinder
3970
self.set_rc(9, 2000)
3971
3972
self.reboot_sitl() # needed for both rangefinder and initial position
3973
self.assert_vehicle_location_is_at_startup_location()
3974
3975
self.takeoff(10, mode="LOITER")
3976
lower_surface_pos = mavutil.location(-35.362421, 149.164534, 584, 270)
3977
here = self.mav.location()
3978
bearing = self.get_bearing(here, lower_surface_pos)
3979
3980
self.change_mode("GUIDED")
3981
self.guided_achieve_heading(bearing)
3982
self.change_mode("LOITER")
3983
self.delay_sim_time(2)
3984
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
3985
orig_absolute_alt_mm = m.alt
3986
3987
self.progress("Original alt: absolute=%f" % orig_absolute_alt_mm)
3988
3989
self.progress("Flying somewhere which surface is known lower compared to takeoff point")
3990
self.set_rc(2, 1450)
3991
tstart = self.get_sim_time()
3992
while True:
3993
if self.get_sim_time() - tstart > 200:
3994
raise NotAchievedException("Did not reach lower point")
3995
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
3996
x = mavutil.location(m.lat/1e7, m.lon/1e7, m.alt/1e3, 0)
3997
dist = self.get_distance(x, lower_surface_pos)
3998
delta = (orig_absolute_alt_mm - m.alt)/1000.0
3999
4000
self.progress("Distance: %fm abs-alt-delta: %fm" %
4001
(dist, delta))
4002
if dist < 15:
4003
if delta < 0.8:
4004
raise NotAchievedException("Did not dip in altitude as expected")
4005
break
4006
4007
self.set_rc(2, 1500)
4008
self.do_RTL()
4009
4010
except Exception as e:
4011
self.print_exception_caught(e)
4012
self.disarm_vehicle(force=True)
4013
ex = e
4014
4015
self.context_pop()
4016
self.reboot_sitl()
4017
if ex is not None:
4018
raise ex
4019
4020
def test_rangefinder_switchover(self):
4021
"""test that the EKF correctly handles the switchover between baro and rangefinder"""
4022
ex = None
4023
self.context_push()
4024
4025
try:
4026
self.set_analog_rangefinder_parameters()
4027
4028
self.set_parameters({
4029
"RNGFND1_MAX_CM": 1500
4030
})
4031
4032
# configure EKF to use rangefinder for altitude at low altitudes
4033
ahrs_ekf_type = self.get_parameter("AHRS_EKF_TYPE")
4034
if ahrs_ekf_type == 2:
4035
self.set_parameter("EK2_RNG_USE_HGT", 70)
4036
if ahrs_ekf_type == 3:
4037
self.set_parameter("EK3_RNG_USE_HGT", 70)
4038
4039
self.reboot_sitl() # needed for both rangefinder and initial position
4040
self.assert_vehicle_location_is_at_startup_location()
4041
4042
self.change_mode("LOITER")
4043
self.wait_ready_to_arm()
4044
self.arm_vehicle()
4045
self.set_rc(3, 1800)
4046
self.set_rc(2, 1200)
4047
# wait till we get to 50m
4048
self.wait_altitude(50, 52, True, 60)
4049
4050
self.change_mode("RTL")
4051
# wait till we get to 25m
4052
self.wait_altitude(25, 27, True, 120)
4053
4054
# level up
4055
self.set_rc(2, 1500)
4056
self.wait_altitude(14, 15, relative=True)
4057
4058
self.wait_rtl_complete()
4059
4060
except Exception as e:
4061
self.print_exception_caught(e)
4062
self.disarm_vehicle(force=True)
4063
ex = e
4064
self.context_pop()
4065
self.reboot_sitl()
4066
if ex is not None:
4067
raise ex
4068
4069
def _Parachute(self, command):
4070
'''Test Parachute Functionality using specific mavlink command'''
4071
self.set_rc(9, 1000)
4072
self.set_parameters({
4073
"CHUTE_ENABLED": 1,
4074
"CHUTE_TYPE": 10,
4075
"SERVO9_FUNCTION": 27,
4076
"SIM_PARA_ENABLE": 1,
4077
"SIM_PARA_PIN": 9,
4078
})
4079
4080
self.progress("Test triggering parachute in mission")
4081
self.load_mission("copter_parachute_mission.txt")
4082
self.change_mode('LOITER')
4083
self.wait_ready_to_arm()
4084
self.arm_vehicle()
4085
self.change_mode('AUTO')
4086
self.set_rc(3, 1600)
4087
self.wait_statustext('BANG', timeout=60)
4088
self.disarm_vehicle(force=True)
4089
self.reboot_sitl()
4090
4091
self.progress("Test triggering with mavlink message")
4092
self.takeoff(20)
4093
command(
4094
mavutil.mavlink.MAV_CMD_DO_PARACHUTE,
4095
p1=2, # release
4096
)
4097
self.wait_statustext('BANG', timeout=60)
4098
self.disarm_vehicle(force=True)
4099
self.reboot_sitl()
4100
4101
self.progress("Testing three-position switch")
4102
self.set_parameter("RC9_OPTION", 23) # parachute 3pos
4103
4104
self.progress("Test manual triggering")
4105
self.takeoff(20)
4106
self.set_rc(9, 2000)
4107
self.wait_statustext('BANG', timeout=60)
4108
self.set_rc(9, 1000)
4109
self.disarm_vehicle(force=True)
4110
self.reboot_sitl()
4111
4112
self.progress("Test mavlink triggering")
4113
self.takeoff(20)
4114
command(
4115
mavutil.mavlink.MAV_CMD_DO_PARACHUTE,
4116
p1=mavutil.mavlink.PARACHUTE_DISABLE,
4117
)
4118
ok = False
4119
try:
4120
self.wait_statustext('BANG', timeout=2)
4121
except AutoTestTimeoutException:
4122
ok = True
4123
if not ok:
4124
raise NotAchievedException("Disabled parachute fired")
4125
command(
4126
mavutil.mavlink.MAV_CMD_DO_PARACHUTE,
4127
p1=mavutil.mavlink.PARACHUTE_ENABLE,
4128
)
4129
ok = False
4130
try:
4131
self.wait_statustext('BANG', timeout=2)
4132
except AutoTestTimeoutException:
4133
ok = True
4134
if not ok:
4135
raise NotAchievedException("Enabled parachute fired")
4136
4137
self.set_rc(9, 1000)
4138
self.disarm_vehicle(force=True)
4139
self.reboot_sitl()
4140
4141
# parachute should not fire if you go from disabled to release:
4142
self.takeoff(20)
4143
command(
4144
mavutil.mavlink.MAV_CMD_DO_PARACHUTE,
4145
p1=mavutil.mavlink.PARACHUTE_RELEASE,
4146
)
4147
ok = False
4148
try:
4149
self.wait_statustext('BANG', timeout=2)
4150
except AutoTestTimeoutException:
4151
ok = True
4152
if not ok:
4153
raise NotAchievedException("Parachute fired when going straight from disabled to release")
4154
4155
# now enable then release parachute:
4156
command(
4157
mavutil.mavlink.MAV_CMD_DO_PARACHUTE,
4158
p1=mavutil.mavlink.PARACHUTE_ENABLE,
4159
)
4160
command(
4161
mavutil.mavlink.MAV_CMD_DO_PARACHUTE,
4162
p1=mavutil.mavlink.PARACHUTE_RELEASE,
4163
)
4164
self.wait_statustext('BANG! Parachute deployed', timeout=2)
4165
self.disarm_vehicle(force=True)
4166
self.reboot_sitl()
4167
4168
self.context_push()
4169
self.progress("Crashing with 3pos switch in enable position")
4170
self.takeoff(40)
4171
self.set_rc(9, 1500)
4172
self.set_parameters({
4173
"SIM_ENGINE_FAIL": 1 << 1, # motor 2
4174
})
4175
self.wait_statustext('BANG! Parachute deployed', timeout=60)
4176
self.set_rc(9, 1000)
4177
self.disarm_vehicle(force=True)
4178
self.reboot_sitl()
4179
self.context_pop()
4180
4181
self.progress("Crashing with 3pos switch in disable position")
4182
loiter_alt = 10
4183
self.takeoff(loiter_alt, mode='LOITER')
4184
self.set_rc(9, 1100)
4185
self.set_parameters({
4186
"SIM_ENGINE_FAIL": 1 << 1, # motor 2
4187
})
4188
tstart = self.get_sim_time()
4189
while self.get_sim_time_cached() < tstart + 5:
4190
m = self.mav.recv_match(type='STATUSTEXT', blocking=True, timeout=1)
4191
if m is None:
4192
continue
4193
if "BANG" in m.text:
4194
self.set_rc(9, 1000)
4195
self.reboot_sitl()
4196
raise NotAchievedException("Parachute deployed when disabled")
4197
self.set_rc(9, 1000)
4198
self.disarm_vehicle(force=True)
4199
self.reboot_sitl()
4200
4201
def Parachute(self):
4202
'''Test Parachute Functionality'''
4203
self._Parachute(self.run_cmd)
4204
self._Parachute(self.run_cmd_int)
4205
4206
def PrecisionLanding(self):
4207
"""Use PrecLand backends precision messages to land aircraft."""
4208
4209
self.context_push()
4210
4211
for backend in [4, 2]: # SITL, SITL-IRLOCK
4212
ex = None
4213
try:
4214
self.set_parameters({
4215
"PLND_ENABLED": 1,
4216
"PLND_TYPE": backend,
4217
})
4218
4219
self.set_analog_rangefinder_parameters()
4220
self.set_parameter("SIM_SONAR_SCALE", 12)
4221
4222
start = self.mav.location()
4223
target = start
4224
(target.lat, target.lng) = mavextra.gps_offset(start.lat, start.lng, 4, -4)
4225
self.progress("Setting target to %f %f" % (target.lat, target.lng))
4226
4227
self.set_parameters({
4228
"SIM_PLD_ENABLE": 1,
4229
"SIM_PLD_LAT": target.lat,
4230
"SIM_PLD_LON": target.lng,
4231
"SIM_PLD_HEIGHT": 0,
4232
"SIM_PLD_ALT_LMT": 15,
4233
"SIM_PLD_DIST_LMT": 10,
4234
})
4235
4236
self.reboot_sitl()
4237
4238
self.progress("Waiting for location")
4239
self.zero_throttle()
4240
self.takeoff(10, 1800, mode="LOITER")
4241
self.change_mode("LAND")
4242
self.zero_throttle()
4243
self.wait_landed_and_disarmed()
4244
self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
4245
new_pos = self.mav.location()
4246
delta = self.get_distance(target, new_pos)
4247
self.progress("Landed %f metres from target position" % delta)
4248
max_delta = 1.5
4249
if delta > max_delta:
4250
raise NotAchievedException("Did not land close enough to target position (%fm > %fm" % (delta, max_delta))
4251
4252
if not self.current_onboard_log_contains_message("PL"):
4253
raise NotAchievedException("Did not see expected PL message")
4254
4255
except Exception as e:
4256
self.print_exception_caught(e)
4257
ex = e
4258
self.reboot_sitl()
4259
self.zero_throttle()
4260
self.context_pop()
4261
self.reboot_sitl()
4262
self.progress("All done")
4263
4264
if ex is not None:
4265
raise ex
4266
4267
def Landing(self):
4268
"""Test landing the aircraft."""
4269
4270
def check_landing_speeds(land_speed_high, land_speed_low, land_alt_low, land_speed_high_accuracy=0.1):
4271
self.progress("Checking landing speeds (speed_high=%f speed_low=%f alt_low=%f" %
4272
(land_speed_high, land_speed_low, land_alt_low))
4273
land_high_maintain = 5
4274
land_low_maintain = land_alt_low / land_speed_low / 2
4275
4276
takeoff_alt = (land_high_maintain * land_speed_high + land_alt_low) + 20
4277
# this is pretty rough, but takes *so much longer* in LOITER
4278
self.takeoff(takeoff_alt, mode='STABILIZE', timeout=200, takeoff_throttle=2000)
4279
# check default landing speeds:
4280
self.change_mode('LAND')
4281
# ensure higher-alt descent rate:
4282
self.wait_descent_rate(land_speed_high,
4283
minimum_duration=land_high_maintain,
4284
accuracy=land_speed_high_accuracy)
4285
self.wait_descent_rate(land_speed_low)
4286
# ensure we transition to low descent rate at correct height:
4287
self.assert_altitude(land_alt_low, relative=True)
4288
# now make sure we maintain that descent rate:
4289
self.wait_descent_rate(land_speed_low, minimum_duration=land_low_maintain)
4290
self.wait_disarmed()
4291
4292
# test the defaults. By default LAND_SPEED_HIGH is 0 so
4293
# WPNAV_SPEED_DN is used
4294
check_landing_speeds(
4295
self.get_parameter("WPNAV_SPEED_DN") / 100, # cm/s -> m/s
4296
self.get_parameter("LAND_SPEED") / 100, # cm/s -> m/s
4297
self.get_parameter("LAND_ALT_LOW") / 100 # cm -> m
4298
)
4299
4300
def test_landing_speeds(land_speed_high, land_speed_low, land_alt_low, **kwargs):
4301
self.set_parameters({
4302
"LAND_SPEED_HIGH": land_speed_high * 100, # m/s -> cm/s
4303
"LAND_SPEED": land_speed_low * 100, # m/s -> cm/s
4304
"LAND_ALT_LOW": land_alt_low * 100, # m -> cm
4305
})
4306
check_landing_speeds(land_speed_high, land_speed_low, land_alt_low, **kwargs)
4307
4308
test_landing_speeds(
4309
5, # descent speed high
4310
1, # descent speed low
4311
30, # transition altitude
4312
land_speed_high_accuracy=0.5
4313
)
4314
4315
def get_system_clock_utc(self, time_seconds):
4316
# this is a copy of ArduPilot's AP_RTC function!
4317
# separate time into ms, sec, min, hour and days but all expressed
4318
# in milliseconds
4319
time_ms = time_seconds * 1000
4320
ms = time_ms % 1000
4321
sec_ms = (time_ms % (60 * 1000)) - ms
4322
min_ms = (time_ms % (60 * 60 * 1000)) - sec_ms - ms
4323
hour_ms = (time_ms % (24 * 60 * 60 * 1000)) - min_ms - sec_ms - ms
4324
4325
# convert times as milliseconds into appropriate units
4326
secs = sec_ms / 1000
4327
mins = min_ms / (60 * 1000)
4328
hours = hour_ms / (60 * 60 * 1000)
4329
return (hours, mins, secs, 0)
4330
4331
def calc_delay(self, seconds, delay_for_seconds):
4332
# delay-for-seconds has to be long enough that we're at the
4333
# waypoint before that time. Otherwise we'll try to wait a
4334
# day....
4335
if delay_for_seconds >= 3600:
4336
raise ValueError("Won't handle large delays")
4337
(hours,
4338
mins,
4339
secs,
4340
ms) = self.get_system_clock_utc(seconds)
4341
self.progress("Now is %uh %um %us" % (hours, mins, secs))
4342
secs += delay_for_seconds # add seventeen seconds
4343
mins += int(secs/60)
4344
secs %= 60
4345
4346
hours += int(mins / 60)
4347
mins %= 60
4348
4349
if hours > 24:
4350
raise ValueError("Way too big a delay")
4351
self.progress("Delay until %uh %um %us" %
4352
(hours, mins, secs))
4353
return (hours, mins, secs, 0)
4354
4355
def reset_delay_item(self, seq, seconds_in_future):
4356
frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
4357
command = mavutil.mavlink.MAV_CMD_NAV_DELAY
4358
# retrieve mission item and check it:
4359
tried_set = False
4360
hours = None
4361
mins = None
4362
secs = None
4363
while True:
4364
self.progress("Requesting item")
4365
self.mav.mav.mission_request_send(1,
4366
1,
4367
seq)
4368
st = self.mav.recv_match(type='MISSION_ITEM',
4369
blocking=True,
4370
timeout=1)
4371
if st is None:
4372
continue
4373
4374
print("Item: %s" % str(st))
4375
have_match = (tried_set and
4376
st.seq == seq and
4377
st.command == command and
4378
st.param2 == hours and
4379
st.param3 == mins and
4380
st.param4 == secs)
4381
if have_match:
4382
return
4383
4384
self.progress("Mission mismatch")
4385
4386
m = None
4387
tstart = self.get_sim_time()
4388
while True:
4389
if self.get_sim_time_cached() - tstart > 3:
4390
raise NotAchievedException(
4391
"Did not receive MISSION_REQUEST")
4392
self.mav.mav.mission_write_partial_list_send(1,
4393
1,
4394
seq,
4395
seq)
4396
m = self.mav.recv_match(type='MISSION_REQUEST',
4397
blocking=True,
4398
timeout=1)
4399
if m is None:
4400
continue
4401
if m.seq != st.seq:
4402
continue
4403
break
4404
4405
self.progress("Sending absolute-time mission item")
4406
4407
# we have to change out the delay time...
4408
now = self.mav.messages["SYSTEM_TIME"]
4409
if now is None:
4410
raise PreconditionFailedException("Never got SYSTEM_TIME")
4411
if now.time_unix_usec == 0:
4412
raise PreconditionFailedException("system time is zero")
4413
(hours, mins, secs, ms) = self.calc_delay(now.time_unix_usec/1000000, seconds_in_future)
4414
4415
self.mav.mav.mission_item_send(
4416
1, # target system
4417
1, # target component
4418
seq, # seq
4419
frame, # frame
4420
command, # command
4421
0, # current
4422
1, # autocontinue
4423
0, # p1 (relative seconds)
4424
hours, # p2
4425
mins, # p3
4426
secs, # p4
4427
0, # p5
4428
0, # p6
4429
0) # p7
4430
tried_set = True
4431
ack = self.mav.recv_match(type='MISSION_ACK',
4432
blocking=True,
4433
timeout=1)
4434
self.progress("Received ack: %s" % str(ack))
4435
4436
def NavDelayAbsTime(self):
4437
"""fly a simple mission that has a delay in it"""
4438
self.fly_nav_delay_abstime_x(87)
4439
4440
def fly_nav_delay_abstime_x(self, delay_for, expected_delay=None):
4441
"""fly a simple mission that has a delay in it, expect a delay"""
4442
4443
if expected_delay is None:
4444
expected_delay = delay_for
4445
4446
self.load_mission("copter_nav_delay.txt")
4447
4448
self.change_mode("LOITER")
4449
4450
self.wait_ready_to_arm()
4451
4452
delay_item_seq = 3
4453
self.reset_delay_item(delay_item_seq, delay_for)
4454
delay_for_seconds = delay_for
4455
reset_at_m = self.mav.recv_match(type='SYSTEM_TIME', blocking=True)
4456
reset_at = reset_at_m.time_unix_usec/1000000
4457
4458
self.arm_vehicle()
4459
self.change_mode("AUTO")
4460
self.set_rc(3, 1600)
4461
count_stop = -1
4462
tstart = self.get_sim_time()
4463
while self.armed(): # we RTL at end of mission
4464
now = self.get_sim_time_cached()
4465
if now - tstart > 240:
4466
raise AutoTestTimeoutException("Did not disarm as expected")
4467
m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True)
4468
at_delay_item = ""
4469
if m.seq == delay_item_seq:
4470
at_delay_item = "(delay item)"
4471
self.progress("MISSION_CURRENT.seq=%u %s" % (m.seq, at_delay_item))
4472
if m.seq > delay_item_seq:
4473
if count_stop == -1:
4474
count_stop_m = self.mav.recv_match(type='SYSTEM_TIME',
4475
blocking=True)
4476
count_stop = count_stop_m.time_unix_usec/1000000
4477
calculated_delay = count_stop - reset_at
4478
error = abs(calculated_delay - expected_delay)
4479
self.progress("Stopped for %u seconds (want >=%u seconds)" %
4480
(calculated_delay, delay_for_seconds))
4481
if error > 2:
4482
raise NotAchievedException("delay outside expectations")
4483
4484
def NavDelayTakeoffAbsTime(self):
4485
"""make sure taking off at a specific time works"""
4486
self.load_mission("copter_nav_delay_takeoff.txt")
4487
4488
self.change_mode("LOITER")
4489
self.wait_ready_to_arm()
4490
4491
delay_item_seq = 2
4492
delay_for_seconds = 77
4493
self.reset_delay_item(delay_item_seq, delay_for_seconds)
4494
reset_at = self.get_sim_time_cached()
4495
4496
self.arm_vehicle()
4497
self.change_mode("AUTO")
4498
4499
self.set_rc(3, 1600)
4500
4501
# should not take off for about least 77 seconds
4502
tstart = self.get_sim_time()
4503
took_off = False
4504
while self.armed():
4505
now = self.get_sim_time_cached()
4506
if now - tstart > 200:
4507
# timeout
4508
break
4509
m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True)
4510
now = self.get_sim_time_cached()
4511
self.progress("%s" % str(m))
4512
if m.seq > delay_item_seq:
4513
if not took_off:
4514
took_off = True
4515
delta_time = now - reset_at
4516
if abs(delta_time - delay_for_seconds) > 2:
4517
raise NotAchievedException((
4518
"Did not take off on time "
4519
"measured=%f want=%f" %
4520
(delta_time, delay_for_seconds)))
4521
4522
if not took_off:
4523
raise NotAchievedException("Did not take off")
4524
4525
def ModeZigZag(self):
4526
'''test zigzag mode'''
4527
# set channel 8 for zigzag savewp and recentre it
4528
self.set_parameter("RC8_OPTION", 61)
4529
4530
self.takeoff(alt_min=5, mode='LOITER')
4531
4532
ZIGZAG = 24
4533
j = 0
4534
slowdown_speed = 0.3 # because Copter takes a long time to actually stop
4535
self.start_subtest("Conduct ZigZag test for all 4 directions")
4536
while j < 4:
4537
self.progress("## Align heading with the run-way (j=%d)##" % j)
4538
self.set_rc(8, 1500)
4539
self.set_rc(4, 1420)
4540
self.wait_heading(352-j*90)
4541
self.set_rc(4, 1500)
4542
self.change_mode(ZIGZAG)
4543
self.progress("## Record Point A ##")
4544
self.set_rc(8, 1100) # record point A
4545
self.set_rc(1, 1700) # fly side-way for 20m
4546
self.wait_distance(20)
4547
self.set_rc(1, 1500)
4548
self.wait_groundspeed(0, slowdown_speed) # wait until the copter slows down
4549
self.progress("## Record Point A ##")
4550
self.set_rc(8, 1500) # pilot always have to cross mid position when changing for low to high position
4551
self.set_rc(8, 1900) # record point B
4552
4553
i = 1
4554
while i < 2:
4555
self.start_subtest("Run zigzag A->B and B->A (i=%d)" % i)
4556
self.progress("## fly forward for 10 meter ##")
4557
self.set_rc(2, 1300)
4558
self.wait_distance(10)
4559
self.set_rc(2, 1500) # re-centre pitch rc control
4560
self.wait_groundspeed(0, slowdown_speed) # wait until the copter slows down
4561
self.set_rc(8, 1500) # switch to mid position
4562
self.progress("## auto execute vector BA ##")
4563
self.set_rc(8, 1100)
4564
self.wait_distance(17) # wait for it to finish
4565
self.wait_groundspeed(0, slowdown_speed) # wait until the copter slows down
4566
4567
self.progress("## fly forward for 10 meter ##")
4568
self.set_rc(2, 1300) # fly forward for 10 meter
4569
self.wait_distance(10)
4570
self.set_rc(2, 1500) # re-centre pitch rc control
4571
self.wait_groundspeed(0, slowdown_speed) # wait until the copter slows down
4572
self.set_rc(8, 1500) # switch to mid position
4573
self.progress("## auto execute vector AB ##")
4574
self.set_rc(8, 1900)
4575
self.wait_distance(17) # wait for it to finish
4576
self.wait_groundspeed(0, slowdown_speed) # wait until the copter slows down
4577
i = i + 1
4578
# test the case when pilot switch to manual control during the auto flight
4579
self.start_subtest("test the case when pilot switch to manual control during the auto flight")
4580
self.progress("## fly forward for 10 meter ##")
4581
self.set_rc(2, 1300) # fly forward for 10 meter
4582
self.wait_distance(10)
4583
self.set_rc(2, 1500) # re-centre pitch rc control
4584
self.wait_groundspeed(0, 0.3) # wait until the copter slows down
4585
self.set_rc(8, 1500) # switch to mid position
4586
self.progress("## auto execute vector BA ##")
4587
self.set_rc(8, 1100) # switch to low position, auto execute vector BA
4588
self.wait_distance(8) # purposely switch to manual halfway
4589
self.set_rc(8, 1500)
4590
self.wait_groundspeed(0, slowdown_speed) # copter should slow down here
4591
self.progress("## Manual control to fly forward ##")
4592
self.set_rc(2, 1300) # manual control to fly forward
4593
self.wait_distance(8)
4594
self.set_rc(2, 1500) # re-centre pitch rc control
4595
self.wait_groundspeed(0, slowdown_speed) # wait until the copter slows down
4596
self.progress("## continue vector BA ##")
4597
self.set_rc(8, 1100) # copter should continue mission here
4598
self.wait_distance(8) # wait for it to finish rest of BA
4599
self.wait_groundspeed(0, slowdown_speed) # wait until the copter slows down
4600
self.set_rc(8, 1500) # switch to mid position
4601
self.progress("## auto execute vector AB ##")
4602
self.set_rc(8, 1900) # switch to execute AB again
4603
self.wait_distance(17) # wait for it to finish
4604
self.wait_groundspeed(0, slowdown_speed) # wait until the copter slows down
4605
self.change_mode('LOITER')
4606
j = j + 1
4607
4608
self.do_RTL()
4609
4610
def SetModesViaModeSwitch(self):
4611
'''Set modes via modeswitch'''
4612
fltmode_ch = 5
4613
self.set_parameter("FLTMODE_CH", fltmode_ch)
4614
self.set_rc(fltmode_ch, 1000) # PWM for mode1
4615
testmodes = [("FLTMODE1", 4, "GUIDED", 1165),
4616
("FLTMODE2", 2, "ALT_HOLD", 1295),
4617
("FLTMODE3", 6, "RTL", 1425),
4618
("FLTMODE4", 7, "CIRCLE", 1555),
4619
("FLTMODE5", 1, "ACRO", 1685),
4620
("FLTMODE6", 17, "BRAKE", 1815),
4621
]
4622
for mode in testmodes:
4623
(parm, parm_value, name, pwm) = mode
4624
self.set_parameter(parm, parm_value)
4625
4626
for mode in reversed(testmodes):
4627
(parm, parm_value, name, pwm) = mode
4628
self.set_rc(fltmode_ch, pwm)
4629
self.wait_mode(name)
4630
4631
for mode in testmodes:
4632
(parm, parm_value, name, pwm) = mode
4633
self.set_rc(fltmode_ch, pwm)
4634
self.wait_mode(name)
4635
4636
for mode in reversed(testmodes):
4637
(parm, parm_value, name, pwm) = mode
4638
self.set_rc(fltmode_ch, pwm)
4639
self.wait_mode(name)
4640
4641
def SetModesViaAuxSwitch(self):
4642
'''"Set modes via auxswitch"'''
4643
fltmode_ch = int(self.get_parameter("FLTMODE_CH"))
4644
self.set_rc(fltmode_ch, 1000)
4645
self.wait_mode("CIRCLE")
4646
self.set_rc(9, 1000)
4647
self.set_rc(10, 1000)
4648
self.set_parameters({
4649
"RC9_OPTION": 18, # land
4650
"RC10_OPTION": 55, # guided
4651
})
4652
self.set_rc(9, 1900)
4653
self.wait_mode("LAND")
4654
self.set_rc(10, 1900)
4655
self.wait_mode("GUIDED")
4656
self.set_rc(10, 1000) # this re-polls the mode switch
4657
self.wait_mode("CIRCLE")
4658
4659
def fly_guided_stop(self,
4660
timeout=20,
4661
groundspeed_tolerance=0.05,
4662
climb_tolerance=0.01):
4663
"""stop the vehicle moving in guided mode"""
4664
self.progress("Stopping vehicle")
4665
tstart = self.get_sim_time()
4666
# send a position-control command
4667
self.mav.mav.set_position_target_local_ned_send(
4668
0, # timestamp
4669
1, # target system_id
4670
1, # target component id
4671
mavutil.mavlink.MAV_FRAME_BODY_NED,
4672
MAV_POS_TARGET_TYPE_MASK.POS_ONLY | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE, # mask specifying use-only-x-y-z
4673
0, # x
4674
0, # y
4675
0, # z
4676
0, # vx
4677
0, # vy
4678
0, # vz
4679
0, # afx
4680
0, # afy
4681
0, # afz
4682
0, # yaw
4683
0, # yawrate
4684
)
4685
while True:
4686
if self.get_sim_time_cached() - tstart > timeout:
4687
raise NotAchievedException("Vehicle did not stop")
4688
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
4689
print("%s" % str(m))
4690
if (m.groundspeed < groundspeed_tolerance and
4691
m.climb < climb_tolerance):
4692
break
4693
4694
def send_set_position_target_global_int(self, lat, lon, alt):
4695
self.mav.mav.set_position_target_global_int_send(
4696
0, # timestamp
4697
1, # target system_id
4698
1, # target component id
4699
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
4700
MAV_POS_TARGET_TYPE_MASK.POS_ONLY, # mask specifying use-only-lat-lon-alt
4701
lat, # lat
4702
lon, # lon
4703
alt, # alt
4704
0, # vx
4705
0, # vy
4706
0, # vz
4707
0, # afx
4708
0, # afy
4709
0, # afz
4710
0, # yaw
4711
0, # yawrate
4712
)
4713
4714
def fly_guided_move_global_relative_alt(self, lat, lon, alt):
4715
startpos = self.mav.recv_match(type='GLOBAL_POSITION_INT',
4716
blocking=True)
4717
4718
self.send_set_position_target_global_int(lat, lon, alt)
4719
4720
tstart = self.get_sim_time()
4721
while True:
4722
if self.get_sim_time_cached() - tstart > 200:
4723
raise NotAchievedException("Did not move far enough")
4724
# send a position-control command
4725
pos = self.mav.recv_match(type='GLOBAL_POSITION_INT',
4726
blocking=True)
4727
delta = self.get_distance_int(startpos, pos)
4728
self.progress("delta=%f (want >10)" % delta)
4729
if delta > 10:
4730
break
4731
4732
def fly_guided_move_local(self, x, y, z_up, timeout=100):
4733
"""move the vehicle using MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED"""
4734
startpos = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True)
4735
self.progress("startpos=%s" % str(startpos))
4736
4737
tstart = self.get_sim_time()
4738
# send a position-control command
4739
self.mav.mav.set_position_target_local_ned_send(
4740
0, # timestamp
4741
1, # target system_id
4742
1, # target component id
4743
mavutil.mavlink.MAV_FRAME_LOCAL_NED,
4744
MAV_POS_TARGET_TYPE_MASK.POS_ONLY | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE, # mask specifying use-only-x-y-z
4745
x, # x
4746
y, # y
4747
-z_up, # z
4748
0, # vx
4749
0, # vy
4750
0, # vz
4751
0, # afx
4752
0, # afy
4753
0, # afz
4754
0, # yaw
4755
0, # yawrate
4756
)
4757
while True:
4758
if self.get_sim_time_cached() - tstart > timeout:
4759
raise NotAchievedException("Did not reach destination")
4760
if self.distance_to_local_position((x, y, -z_up)) < 1:
4761
break
4762
4763
def test_guided_local_position_target(self, x, y, z_up):
4764
""" Check target position being received by vehicle """
4765
# set POSITION_TARGET_LOCAL_NED message rate using SET_MESSAGE_INTERVAL
4766
self.progress("Setting local target in NED: (%f, %f, %f)" % (x, y, -z_up))
4767
self.progress("Setting rate to 1 Hz")
4768
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, 1)
4769
4770
# mask specifying use only xyz
4771
target_typemask = MAV_POS_TARGET_TYPE_MASK.POS_ONLY
4772
4773
# set position target
4774
self.mav.mav.set_position_target_local_ned_send(
4775
0, # timestamp
4776
1, # target system_id
4777
1, # target component id
4778
mavutil.mavlink.MAV_FRAME_LOCAL_NED,
4779
target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE,
4780
x, # x
4781
y, # y
4782
-z_up, # z
4783
0, # vx
4784
0, # vy
4785
0, # vz
4786
0, # afx
4787
0, # afy
4788
0, # afz
4789
0, # yaw
4790
0, # yawrate
4791
)
4792
m = self.mav.recv_match(type='POSITION_TARGET_LOCAL_NED', blocking=True, timeout=2)
4793
self.progress("Received local target: %s" % str(m))
4794
4795
if not (m.type_mask == (target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE) or m.type_mask == target_typemask):
4796
raise NotAchievedException("Did not receive proper mask: expected=%u or %u, got=%u" %
4797
((target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE), target_typemask, m.type_mask))
4798
4799
if x - m.x > 0.1:
4800
raise NotAchievedException("Did not receive proper target position x: wanted=%f got=%f" % (x, m.x))
4801
4802
if y - m.y > 0.1:
4803
raise NotAchievedException("Did not receive proper target position y: wanted=%f got=%f" % (y, m.y))
4804
4805
if z_up - (-m.z) > 0.1:
4806
raise NotAchievedException("Did not receive proper target position z: wanted=%f got=%f" % (z_up, -m.z))
4807
4808
def test_guided_local_velocity_target(self, vx, vy, vz_up, timeout=3):
4809
" Check local target velocity being received by vehicle "
4810
self.progress("Setting local NED velocity target: (%f, %f, %f)" % (vx, vy, -vz_up))
4811
self.progress("Setting POSITION_TARGET_LOCAL_NED message rate to 10Hz")
4812
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, 10)
4813
4814
# mask specifying use only vx,vy,vz & accel. Even though we don't test acceltargets below currently
4815
# a velocity only mask returns a velocity & accel mask
4816
target_typemask = (MAV_POS_TARGET_TYPE_MASK.POS_IGNORE |
4817
MAV_POS_TARGET_TYPE_MASK.YAW_IGNORE | MAV_POS_TARGET_TYPE_MASK.YAW_RATE_IGNORE)
4818
4819
# Drain old messages and ignore the ramp-up to the required target velocity
4820
tstart = self.get_sim_time()
4821
while self.get_sim_time_cached() - tstart < timeout:
4822
# send velocity-control command
4823
self.mav.mav.set_position_target_local_ned_send(
4824
0, # timestamp
4825
1, # target system_id
4826
1, # target component id
4827
mavutil.mavlink.MAV_FRAME_LOCAL_NED,
4828
target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE,
4829
0, # x
4830
0, # y
4831
0, # z
4832
vx, # vx
4833
vy, # vy
4834
-vz_up, # vz
4835
0, # afx
4836
0, # afy
4837
0, # afz
4838
0, # yaw
4839
0, # yawrate
4840
)
4841
m = self.assert_receive_message('POSITION_TARGET_LOCAL_NED')
4842
4843
self.progress("Received local target: %s" % str(m))
4844
4845
# Check the last received message
4846
if not (m.type_mask == (target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE) or m.type_mask == target_typemask):
4847
raise NotAchievedException("Did not receive proper mask: expected=%u or %u, got=%u" %
4848
((target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE), target_typemask, m.type_mask))
4849
4850
if vx - m.vx > 0.1:
4851
raise NotAchievedException("Did not receive proper target velocity vx: wanted=%f got=%f" % (vx, m.vx))
4852
4853
if vy - m.vy > 0.1:
4854
raise NotAchievedException("Did not receive proper target velocity vy: wanted=%f got=%f" % (vy, m.vy))
4855
4856
if vz_up - (-m.vz) > 0.1:
4857
raise NotAchievedException("Did not receive proper target velocity vz: wanted=%f got=%f" % (vz_up, -m.vz))
4858
4859
self.progress("Received proper target velocity commands")
4860
4861
def wait_for_local_velocity(self, vx, vy, vz_up, timeout=10):
4862
""" Wait for local target velocity"""
4863
4864
# debug messages
4865
self.progress("Waiting for local NED velocity target: (%f, %f, %f)" % (vx, vy, -vz_up))
4866
self.progress("Setting LOCAL_POSITION_NED message rate to 10Hz")
4867
4868
# set position local ned message stream rate
4869
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_LOCAL_POSITION_NED, 10)
4870
4871
# wait for position local ned message
4872
tstart = self.get_sim_time()
4873
while self.get_sim_time_cached() - tstart < timeout:
4874
4875
# get position target local ned message
4876
m = self.mav.recv_match(type="LOCAL_POSITION_NED", blocking=True, timeout=1)
4877
4878
# could not be able to get a valid target local ned message within given time
4879
if m is None:
4880
4881
# raise an error that did not receive a valid target local ned message within given time
4882
raise NotAchievedException("Did not receive any position local ned message for 1 second!")
4883
4884
# got a valid target local ned message within given time
4885
else:
4886
4887
# debug message
4888
self.progress("Received local position ned message: %s" % str(m))
4889
4890
# check if velocity values are in range
4891
if vx - m.vx <= 0.1 and vy - m.vy <= 0.1 and vz_up - (-m.vz) <= 0.1:
4892
4893
# get out of function
4894
self.progress("Vehicle successfully reached to target velocity!")
4895
return
4896
4897
# raise an exception
4898
error_message = "Did not receive target velocities vx, vy, vz_up, wanted=(%f, %f, %f) got=(%f, %f, %f)"
4899
error_message = error_message % (vx, vy, vz_up, m.vx, m.vy, -m.vz)
4900
raise NotAchievedException(error_message)
4901
4902
def test_position_target_message_mode(self):
4903
" Ensure that POSITION_TARGET_LOCAL_NED messages are sent in Guided Mode only "
4904
self.hover()
4905
self.change_mode('LOITER')
4906
self.progress("Setting POSITION_TARGET_LOCAL_NED message rate to 10Hz")
4907
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, 10)
4908
4909
tstart = self.get_sim_time()
4910
while self.get_sim_time_cached() < tstart + 5:
4911
m = self.mav.recv_match(type='POSITION_TARGET_LOCAL_NED', blocking=True, timeout=1)
4912
if m is None:
4913
continue
4914
4915
raise NotAchievedException("Received POSITION_TARGET message in LOITER mode: %s" % str(m))
4916
4917
self.progress("Did not receive any POSITION_TARGET_LOCAL_NED message in LOITER mode. Success")
4918
4919
def earth_to_body(self, vector):
4920
r = mavextra.rotation(self.mav.messages["ATTITUDE"]).invert()
4921
# print("r=%s" % str(r))
4922
return r * vector
4923
4924
def precision_loiter_to_pos(self, x, y, z, timeout=40):
4925
'''send landing_target messages at vehicle until it arrives at
4926
location to x, y, z from origin (in metres), z is *up*'''
4927
dest_ned = rotmat.Vector3(x, y, -z)
4928
tstart = self.get_sim_time()
4929
success_start = -1
4930
while True:
4931
now = self.get_sim_time_cached()
4932
if now - tstart > timeout:
4933
raise NotAchievedException("Did not loiter to position!")
4934
m_pos = self.mav.recv_match(type='LOCAL_POSITION_NED',
4935
blocking=True)
4936
pos_ned = rotmat.Vector3(m_pos.x, m_pos.y, m_pos.z)
4937
# print("dest_ned=%s" % str(dest_ned))
4938
# print("pos_ned=%s" % str(pos_ned))
4939
delta_ef = dest_ned - pos_ned
4940
# print("delta_ef=%s" % str(delta_ef))
4941
4942
# determine if we've successfully navigated to close to
4943
# where we should be:
4944
dist = math.sqrt(delta_ef.x * delta_ef.x + delta_ef.y * delta_ef.y)
4945
dist_max = 1
4946
self.progress("dist=%f want <%f" % (dist, dist_max))
4947
if dist < dist_max:
4948
# success! We've gotten within our target distance
4949
if success_start == -1:
4950
success_start = now
4951
elif now - success_start > 10:
4952
self.progress("Yay!")
4953
break
4954
else:
4955
success_start = -1
4956
4957
delta_bf = self.earth_to_body(delta_ef)
4958
# print("delta_bf=%s" % str(delta_bf))
4959
angle_x = math.atan2(delta_bf.y, delta_bf.z)
4960
angle_y = -math.atan2(delta_bf.x, delta_bf.z)
4961
distance = math.sqrt(delta_bf.x * delta_bf.x +
4962
delta_bf.y * delta_bf.y +
4963
delta_bf.z * delta_bf.z)
4964
# att = self.mav.messages["ATTITUDE"]
4965
# print("r=%f p=%f y=%f" % (math.degrees(att.roll), math.degrees(att.pitch), math.degrees(att.yaw)))
4966
# print("angle_x=%s angle_y=%s" % (str(math.degrees(angle_x)), str(math.degrees(angle_y))))
4967
# print("distance=%s" % str(distance))
4968
4969
self.mav.mav.landing_target_send(
4970
0, # time_usec
4971
1, # target_num
4972
mavutil.mavlink.MAV_FRAME_GLOBAL, # frame; AP ignores
4973
angle_x, # angle x (radians)
4974
angle_y, # angle y (radians)
4975
distance, # distance to target
4976
0.01, # size of target in radians, X-axis
4977
0.01 # size of target in radians, Y-axis
4978
)
4979
4980
def set_servo_gripper_parameters(self):
4981
self.set_parameters({
4982
"GRIP_ENABLE": 1,
4983
"GRIP_TYPE": 1,
4984
"SIM_GRPS_ENABLE": 1,
4985
"SIM_GRPS_PIN": 8,
4986
"SERVO8_FUNCTION": 28,
4987
})
4988
4989
def PayloadPlaceMission(self):
4990
"""Test payload placing in auto."""
4991
self.context_push()
4992
4993
self.set_analog_rangefinder_parameters()
4994
self.set_servo_gripper_parameters()
4995
self.reboot_sitl()
4996
4997
self.load_mission("copter_payload_place.txt")
4998
if self.mavproxy is not None:
4999
self.mavproxy.send('wp list\n')
5000
5001
self.set_parameter("AUTO_OPTIONS", 3)
5002
self.change_mode('AUTO')
5003
self.wait_ready_to_arm()
5004
5005
self.arm_vehicle()
5006
5007
self.wait_text("Gripper load releas", timeout=90)
5008
dist_limit = 1
5009
# this is a copy of the point in the mission file:
5010
target_loc = mavutil.location(-35.363106,
5011
149.165436,
5012
0,
5013
0)
5014
dist = self.get_distance(target_loc, self.mav.location())
5015
self.progress("dist=%f" % (dist,))
5016
if dist > dist_limit:
5017
raise NotAchievedException("Did not honour target lat/lng (dist=%f want <%f" %
5018
(dist, dist_limit))
5019
5020
self.wait_disarmed()
5021
5022
self.context_pop()
5023
self.reboot_sitl()
5024
self.progress("All done")
5025
5026
def Weathervane(self):
5027
'''Test copter weathervaning'''
5028
# We test nose into wind code paths and yaw direction here and test side into wind
5029
# yaw direction in QuadPlane tests to reduce repetition.
5030
self.set_parameters({
5031
"SIM_WIND_SPD": 10,
5032
"SIM_WIND_DIR": 100,
5033
"GUID_OPTIONS": 129, # allow weathervaning and arming from tx in guided
5034
"AUTO_OPTIONS": 131, # allow arming in auto, take off without raising the stick, and weathervaning
5035
"WVANE_ENABLE": 1,
5036
"WVANE_GAIN": 3,
5037
"WVANE_VELZ_MAX": 1,
5038
"WVANE_SPD_MAX": 2
5039
})
5040
5041
self.progress("Test weathervaning in auto")
5042
self.load_mission("weathervane_mission.txt", strict=False)
5043
5044
self.change_mode("AUTO")
5045
self.wait_ready_to_arm()
5046
self.arm_vehicle()
5047
5048
self.wait_statustext("Weathervane Active", timeout=60)
5049
self.do_RTL()
5050
self.wait_disarmed()
5051
self.change_mode("GUIDED")
5052
5053
# After take off command in guided we enter the velaccl sub mode
5054
self.progress("Test weathervaning in guided vel-accel")
5055
self.set_rc(3, 1000)
5056
self.wait_ready_to_arm()
5057
5058
self.arm_vehicle()
5059
self.user_takeoff(alt_min=15)
5060
# Wait for heading to match wind direction.
5061
self.wait_heading(100, accuracy=8, timeout=100)
5062
5063
self.progress("Test weathervaning in guided pos only")
5064
# Travel directly north to align heading north and build some airspeed.
5065
self.fly_guided_move_local(x=40, y=0, z_up=15)
5066
# Wait for heading to match wind direction.
5067
self.wait_heading(100, accuracy=8, timeout=100)
5068
self.do_RTL()
5069
5070
def _DO_WINCH(self, command):
5071
self.context_push()
5072
self.load_default_params_file("copter-winch.parm")
5073
self.reboot_sitl()
5074
self.wait_ready_to_arm()
5075
5076
self.start_subtest("starts relaxed")
5077
self.wait_servo_channel_value(9, 0)
5078
5079
self.start_subtest("rate control")
5080
command(
5081
mavutil.mavlink.MAV_CMD_DO_WINCH,
5082
p1=1, # instance number
5083
p2=mavutil.mavlink.WINCH_RATE_CONTROL, # command
5084
p3=0, # length to release
5085
p4=1, # rate in m/s
5086
)
5087
self.wait_servo_channel_value(9, 1900)
5088
5089
self.start_subtest("relax")
5090
command(
5091
mavutil.mavlink.MAV_CMD_DO_WINCH,
5092
p1=1, # instance number
5093
p2=mavutil.mavlink.WINCH_RELAXED, # command
5094
p3=0, # length to release
5095
p4=1, # rate in m/s
5096
)
5097
self.wait_servo_channel_value(9, 0)
5098
5099
self.start_subtest("hold but zero output")
5100
command(
5101
mavutil.mavlink.MAV_CMD_DO_WINCH,
5102
p1=1, # instance number
5103
p2=mavutil.mavlink.WINCH_RATE_CONTROL, # command
5104
p3=0, # length to release
5105
p4=0, # rate in m/s
5106
)
5107
self.wait_servo_channel_value(9, 1500)
5108
5109
self.start_subtest("relax")
5110
command(
5111
mavutil.mavlink.MAV_CMD_DO_WINCH,
5112
p1=1, # instance number
5113
p2=mavutil.mavlink.WINCH_RELAXED, # command
5114
p3=0, # length to release
5115
p4=1, # rate in m/s
5116
)
5117
self.wait_servo_channel_value(9, 0)
5118
5119
self.start_subtest("position")
5120
command(
5121
mavutil.mavlink.MAV_CMD_DO_WINCH,
5122
p1=1, # instance number
5123
p2=mavutil.mavlink.WINCH_RELATIVE_LENGTH_CONTROL, # command
5124
p3=2, # length to release
5125
p4=1, # rate in m/s
5126
)
5127
self.wait_servo_channel_value(9, 1900)
5128
self.wait_servo_channel_value(9, 1500, timeout=60)
5129
5130
self.context_pop()
5131
self.reboot_sitl()
5132
5133
def DO_WINCH(self):
5134
'''test mavlink DO_WINCH command'''
5135
self._DO_WINCH(self.run_cmd_int)
5136
self._DO_WINCH(self.run_cmd)
5137
5138
def GuidedSubModeChange(self):
5139
""""Ensure we can move around in guided after a takeoff command."""
5140
5141
'''start by disabling GCS failsafe, otherwise we immediately disarm
5142
due to (apparently) not receiving traffic from the GCS for
5143
too long. This is probably a function of --speedup'''
5144
self.set_parameters({
5145
"FS_GCS_ENABLE": 0,
5146
"DISARM_DELAY": 0, # until traffic problems are fixed
5147
})
5148
self.change_mode("GUIDED")
5149
self.wait_ready_to_arm()
5150
self.arm_vehicle()
5151
5152
self.user_takeoff(alt_min=10)
5153
5154
self.start_subtest("yaw through absolute angles using MAV_CMD_CONDITION_YAW")
5155
self.guided_achieve_heading(45)
5156
self.guided_achieve_heading(135)
5157
5158
self.start_subtest("move the vehicle using set_position_target_global_int")
5159
# the following numbers are 5-degree-latitude and 5-degrees
5160
# longitude - just so that we start to really move a lot.
5161
self.fly_guided_move_global_relative_alt(5, 5, 10)
5162
5163
self.start_subtest("move the vehicle using MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED")
5164
self.fly_guided_stop(groundspeed_tolerance=0.1)
5165
self.fly_guided_move_local(5, 5, 10)
5166
5167
self.start_subtest("Checking that WP_YAW_BEHAVIOUR 0 works")
5168
self.set_parameter('WP_YAW_BEHAVIOR', 0)
5169
self.delay_sim_time(2)
5170
orig_heading = self.get_heading()
5171
self.fly_guided_move_local(5, 0, 10)
5172
# ensure our heading hasn't changed:
5173
self.assert_heading(orig_heading)
5174
self.fly_guided_move_local(0, 5, 10)
5175
# ensure our heading hasn't changed:
5176
self.assert_heading(orig_heading)
5177
5178
self.start_subtest("Check target position received by vehicle using SET_MESSAGE_INTERVAL")
5179
self.test_guided_local_position_target(5, 5, 10)
5180
self.test_guided_local_velocity_target(2, 2, 1)
5181
self.test_position_target_message_mode()
5182
5183
self.do_RTL()
5184
5185
def TestGripperMission(self):
5186
'''Test Gripper mission items'''
5187
num_wp = self.load_mission("copter-gripper-mission.txt")
5188
self.change_mode('LOITER')
5189
self.wait_ready_to_arm()
5190
self.assert_vehicle_location_is_at_startup_location()
5191
self.arm_vehicle()
5192
self.change_mode('AUTO')
5193
self.set_rc(3, 1500)
5194
self.wait_statustext("Gripper Grabbed", timeout=60)
5195
self.wait_statustext("Gripper Released", timeout=60)
5196
self.wait_waypoint(num_wp-1, num_wp-1)
5197
self.wait_disarmed()
5198
5199
def SplineLastWaypoint(self):
5200
'''Test Spline as last waypoint'''
5201
self.load_mission("copter-spline-last-waypoint.txt")
5202
self.change_mode('LOITER')
5203
self.wait_ready_to_arm()
5204
self.arm_vehicle()
5205
self.change_mode('AUTO')
5206
self.set_rc(3, 1500)
5207
self.wait_altitude(10, 3000, relative=True)
5208
self.do_RTL()
5209
5210
def ManualThrottleModeChange(self):
5211
'''Check manual throttle mode changes denied on high throttle'''
5212
self.set_parameter("FS_GCS_ENABLE", 0) # avoid GUIDED instant disarm
5213
self.change_mode("STABILIZE")
5214
self.wait_ready_to_arm()
5215
self.arm_vehicle()
5216
self.change_mode("ACRO")
5217
self.change_mode("STABILIZE")
5218
self.change_mode("GUIDED")
5219
self.set_rc(3, 1700)
5220
self.watch_altitude_maintained(altitude_min=-1, altitude_max=0.2) # should not take off in guided
5221
self.run_cmd_do_set_mode(
5222
"ACRO",
5223
want_result=mavutil.mavlink.MAV_RESULT_FAILED)
5224
self.run_cmd_do_set_mode(
5225
"STABILIZE",
5226
want_result=mavutil.mavlink.MAV_RESULT_FAILED)
5227
self.run_cmd_do_set_mode(
5228
"DRIFT",
5229
want_result=mavutil.mavlink.MAV_RESULT_FAILED)
5230
self.progress("Check setting an invalid mode")
5231
self.run_cmd(
5232
mavutil.mavlink.MAV_CMD_DO_SET_MODE,
5233
p1=mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
5234
p2=126,
5235
want_result=mavutil.mavlink.MAV_RESULT_FAILED,
5236
timeout=1,
5237
)
5238
self.set_rc(3, 1000)
5239
self.run_cmd_do_set_mode("ACRO")
5240
self.wait_disarmed()
5241
5242
def constrained_mount_pitch(self, pitch_angle_deg, mount_instance=1):
5243
PITCH_MIN = self.get_parameter("MNT%u_PITCH_MIN" % mount_instance)
5244
PITCH_MAX = self.get_parameter("MNT%u_PITCH_MAX" % mount_instance)
5245
return min(max(pitch_angle_deg, PITCH_MIN), PITCH_MAX)
5246
5247
def test_mount_pitch(self, despitch, despitch_tolerance, mount_mode, timeout=10, hold=0, constrained=True):
5248
tstart = self.get_sim_time()
5249
success_start = 0
5250
5251
while True:
5252
now = self.get_sim_time_cached()
5253
if now - tstart > timeout:
5254
raise NotAchievedException("Mount pitch not achieved")
5255
5256
# We expect to achieve the desired pitch angle unless constrained by mount limits
5257
if constrained:
5258
despitch = self.constrained_mount_pitch(despitch)
5259
5260
'''retrieve latest angles from GIMBAL_DEVICE_ATTITUDE_STATUS'''
5261
mount_roll, mount_pitch, mount_yaw, mount_yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg()
5262
5263
# self.progress("despitch=%f roll=%f pitch=%f yaw=%f" % (despitch, mount_roll, mount_pitch, mount_yaw))
5264
if abs(despitch - mount_pitch) > despitch_tolerance:
5265
self.progress("Mount pitch incorrect: got=%f want=%f (+/- %f)" %
5266
(mount_pitch, despitch, despitch_tolerance))
5267
success_start = 0
5268
continue
5269
self.progress("Mount pitch correct: %f degrees == %f" %
5270
(mount_pitch, despitch))
5271
if success_start == 0:
5272
success_start = now
5273
if now - success_start >= hold:
5274
self.progress("Mount pitch achieved")
5275
return
5276
5277
def do_pitch(self, pitch):
5278
'''pitch aircraft in guided/angle mode'''
5279
self.mav.mav.set_attitude_target_send(
5280
0, # time_boot_ms
5281
1, # target sysid
5282
1, # target compid
5283
0, # bitmask of things to ignore
5284
mavextra.euler_to_quat([0, math.radians(pitch), 0]), # att
5285
0, # roll rate (rad/s)
5286
0, # pitch rate (rad/s)
5287
0, # yaw rate (rad/s)
5288
0.5) # thrust, 0 to 1, translated to a climb/descent rate
5289
5290
def do_yaw_rate(self, yaw_rate):
5291
'''yaw aircraft in guided/rate mode'''
5292
self.run_cmd(
5293
mavutil.mavlink.MAV_CMD_CONDITION_YAW,
5294
p1=60, # target angle
5295
p2=0, # degrees/second
5296
p3=1, # -1 is counter-clockwise, 1 clockwise
5297
p4=1, # 1 for relative, 0 for absolute
5298
quiet=True,
5299
)
5300
5301
def setup_servo_mount(self, roll_servo=5, pitch_servo=6, yaw_servo=7):
5302
'''configure a rpy servo mount; caller responsible for required rebooting'''
5303
self.progress("Setting up servo mount")
5304
self.set_parameters({
5305
"MNT1_TYPE": 1,
5306
"MNT1_PITCH_MIN": -45,
5307
"MNT1_PITCH_MAX": 45,
5308
"RC6_OPTION": 213, # MOUNT1_PITCH
5309
"SERVO%u_FUNCTION" % roll_servo: 8, # roll
5310
"SERVO%u_FUNCTION" % pitch_servo: 7, # pitch
5311
"SERVO%u_FUNCTION" % yaw_servo: 6, # yaw
5312
})
5313
5314
def get_mount_roll_pitch_yaw_deg(self):
5315
'''return mount (aka gimbal) roll, pitch and yaw angles in degrees'''
5316
# wait for gimbal attitude message
5317
m = self.assert_receive_message('GIMBAL_DEVICE_ATTITUDE_STATUS', timeout=5)
5318
5319
yaw_is_absolute = m.flags & mavutil.mavlink.GIMBAL_DEVICE_FLAGS_YAW_LOCK
5320
# convert quaternion to euler angles and return
5321
q = quaternion.Quaternion(m.q)
5322
euler = q.euler
5323
return math.degrees(euler[0]), math.degrees(euler[1]), math.degrees(euler[2]), yaw_is_absolute
5324
5325
def set_mount_mode(self, mount_mode):
5326
'''set mount mode'''
5327
self.run_cmd_int(
5328
mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE,
5329
p1=mount_mode,
5330
p2=0, # stabilize roll (unsupported)
5331
p3=0, # stabilize pitch (unsupported)
5332
)
5333
self.run_cmd(
5334
mavutil.mavlink.MAV_CMD_DO_MOUNT_CONFIGURE,
5335
p1=mount_mode,
5336
p2=0, # stabilize roll (unsupported)
5337
p3=0, # stabilize pitch (unsupported)
5338
)
5339
5340
def test_mount_rc_targetting(self, pitch_rc_neutral=1500, do_rate_tests=True):
5341
'''called in multipleplaces to make sure that mount RC targetting works'''
5342
if True:
5343
self.context_push()
5344
self.set_parameters({
5345
'RC6_OPTION': 0,
5346
'RC11_OPTION': 212, # MOUNT1_ROLL
5347
'RC12_OPTION': 213, # MOUNT1_PITCH
5348
'RC13_OPTION': 214, # MOUNT1_YAW
5349
'RC12_MIN': 1100,
5350
'RC12_MAX': 1900,
5351
'RC12_TRIM': 1500,
5352
'MNT1_PITCH_MIN': -45,
5353
'MNT1_PITCH_MAX': 45,
5354
})
5355
self.progress("Testing RC angular control")
5356
# default RC min=1100 max=1900
5357
self.set_rc_from_map({
5358
11: 1500,
5359
12: 1500,
5360
13: 1500,
5361
})
5362
self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
5363
self.progress("Testing RC input down 1/4 of its range in the output, should be down 1/4 range in output")
5364
rc12_in = 1400
5365
rc12_min = 1100 # default
5366
rc12_max = 1900 # default
5367
mpitch_min = -45.0
5368
mpitch_max = 45.0
5369
expected_pitch = (float(rc12_in-rc12_min)/float(rc12_max-rc12_min) * (mpitch_max-mpitch_min)) + mpitch_min
5370
self.progress("expected mount pitch: %f" % expected_pitch)
5371
if expected_pitch != -11.25:
5372
raise NotAchievedException("Calculation wrong - defaults changed?!")
5373
self.set_rc(12, rc12_in)
5374
self.test_mount_pitch(-11.25, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
5375
self.set_rc(12, 1800)
5376
self.test_mount_pitch(33.75, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
5377
self.set_rc_from_map({
5378
11: 1500,
5379
12: 1500,
5380
13: 1500,
5381
})
5382
5383
try:
5384
self.context_push()
5385
self.set_parameters({
5386
"RC12_MIN": 1000,
5387
"RC12_MAX": 2000,
5388
"MNT1_PITCH_MIN": -90,
5389
"MNT1_PITCH_MAX": 10,
5390
})
5391
self.set_rc(12, 1000)
5392
self.test_mount_pitch(-90.00, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
5393
self.set_rc(12, 2000)
5394
self.test_mount_pitch(10.00, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
5395
self.set_rc(12, 1500)
5396
self.test_mount_pitch(-40.00, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
5397
finally:
5398
self.context_pop()
5399
5400
self.set_rc(12, 1500)
5401
5402
if do_rate_tests:
5403
self.test_mount_rc_targetting_rate_control()
5404
5405
self.context_pop()
5406
5407
def test_mount_rc_targetting_rate_control(self, pitch_rc_neutral=1500):
5408
if True:
5409
self.progress("Testing RC rate control")
5410
self.set_parameter('MNT1_RC_RATE', 10)
5411
self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
5412
# Note that we don't constrain the desired angle in the following so that we don't
5413
# timeout due to fetching Mount pitch limit params.
5414
self.set_rc(12, 1300)
5415
self.test_mount_pitch(-5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)
5416
self.test_mount_pitch(-10, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)
5417
self.test_mount_pitch(-15, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)
5418
self.test_mount_pitch(-20, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)
5419
self.set_rc(12, 1700)
5420
self.test_mount_pitch(-15, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)
5421
self.test_mount_pitch(-10, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)
5422
self.test_mount_pitch(-5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)
5423
self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)
5424
self.test_mount_pitch(5, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING, constrained=False)
5425
5426
self.progress("Reverting to angle mode")
5427
self.set_parameter('MNT1_RC_RATE', 0)
5428
self.set_rc(12, 1500)
5429
self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
5430
5431
def mount_test_body(self, pitch_rc_neutral=1500, do_rate_tests=True, constrain_sysid_target=True):
5432
'''Test Camera/Antenna Mount - assumes a camera is set up and ready to go'''
5433
if True:
5434
# make sure we're getting gimbal device attitude status
5435
self.assert_receive_message('GIMBAL_DEVICE_ATTITUDE_STATUS', timeout=5, very_verbose=True)
5436
5437
# change mount to neutral mode (point forward, not stabilising)
5438
self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)
5439
5440
# test pitch is not neutral to start with
5441
mount_roll_deg, mount_pitch_deg, mount_yaw_deg, mount_yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg()
5442
if mount_roll_deg != 0 or mount_pitch_deg != 0 or mount_yaw_deg != 0:
5443
raise NotAchievedException("Mount not neutral")
5444
5445
self.takeoff(30, mode='GUIDED')
5446
5447
# pitch vehicle back and confirm gimbal is still not stabilising
5448
despitch = 10
5449
despitch_tolerance = 3
5450
5451
self.progress("Pitching vehicle")
5452
self.do_pitch(despitch) # will time out!
5453
5454
self.wait_pitch(despitch, despitch_tolerance)
5455
5456
# check gimbal is still not stabilising
5457
mount_roll_deg, mount_pitch_deg, mount_yaw_deg, mount_yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg()
5458
if mount_roll_deg != 0 or mount_pitch_deg != 0 or mount_yaw_deg != 0:
5459
raise NotAchievedException("Mount stabilising when not requested")
5460
5461
# center RC tilt control and change mount to RC_TARGETING mode
5462
self.progress("Gimbal to RC Targetting mode")
5463
self.set_rc(6, pitch_rc_neutral)
5464
self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
5465
5466
# pitch vehicle back and confirm gimbal is stabilising
5467
self.progress("Pitching vehicle")
5468
self.do_pitch(despitch)
5469
self.wait_pitch(despitch, despitch_tolerance)
5470
self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
5471
5472
# point gimbal at specified angle
5473
self.progress("Point gimbal using GIMBAL_MANAGER_PITCHYAW (ANGLE)")
5474
self.do_pitch(0) # level vehicle
5475
self.wait_pitch(0, despitch_tolerance)
5476
self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING)
5477
for (method, angle) in (self.run_cmd, -20), (self.run_cmd_int, -30):
5478
method(
5479
mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW,
5480
p1=angle, # pitch angle in degrees
5481
p2=0, # yaw angle in degrees
5482
p3=0, # pitch rate in degrees (NaN to ignore)
5483
p4=0, # yaw rate in degrees (NaN to ignore)
5484
p5=0, # flags (0=Body-frame, 16/GIMBAL_MANAGER_FLAGS_YAW_LOCK=Earth Frame)
5485
p6=0, # unused
5486
p7=0, # gimbal id
5487
)
5488
self.test_mount_pitch(angle, 1, mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING)
5489
5490
# this is a one-off; ArduCopter *will* time out this directive!
5491
self.progress("Levelling aircraft")
5492
self.mav.mav.set_attitude_target_send(
5493
0, # time_boot_ms
5494
1, # target sysid
5495
1, # target compid
5496
0, # bitmask of things to ignore
5497
mavextra.euler_to_quat([0, 0, 0]), # att
5498
0, # roll rate (rad/s)
5499
0, # pitch rate (rad/s)
5500
0, # yaw rate (rad/s)
5501
0.5) # thrust, 0 to 1, translated to a climb/descent rate
5502
5503
self.wait_groundspeed(0, 1)
5504
5505
# now test RC targetting
5506
self.progress("Testing mount RC targetting")
5507
5508
self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
5509
self.test_mount_rc_targetting(
5510
pitch_rc_neutral=pitch_rc_neutral,
5511
do_rate_tests=do_rate_tests,
5512
)
5513
5514
self.progress("Testing mount ROI behaviour")
5515
self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
5516
start = self.mav.location()
5517
self.progress("start=%s" % str(start))
5518
(roi_lat, roi_lon) = mavextra.gps_offset(start.lat,
5519
start.lng,
5520
10,
5521
20)
5522
roi_alt = 0
5523
self.progress("Using MAV_CMD_DO_SET_ROI_LOCATION")
5524
self.run_cmd(
5525
mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION,
5526
p5=roi_lat,
5527
p6=roi_lon,
5528
p7=roi_alt,
5529
)
5530
self.test_mount_pitch(-52, 5, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT)
5531
self.progress("Using MAV_CMD_DO_SET_ROI_LOCATION")
5532
# start by pointing the gimbal elsewhere with a
5533
# known-working command:
5534
self.run_cmd(
5535
mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION,
5536
p5=roi_lat + 1,
5537
p6=roi_lon + 1,
5538
p7=roi_alt,
5539
)
5540
# now point it with command_int:
5541
self.run_cmd_int(
5542
mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION,
5543
p5=int(roi_lat * 1e7),
5544
p6=int(roi_lon * 1e7),
5545
p7=roi_alt,
5546
frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
5547
)
5548
self.test_mount_pitch(-52, 5, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT)
5549
5550
self.progress("Using MAV_CMD_DO_SET_ROI_NONE")
5551
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI_NONE)
5552
self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_SET_ROI_NONE)
5553
self.test_mount_pitch(0, 1, mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING)
5554
5555
start = self.mav.location()
5556
(roi_lat, roi_lon) = mavextra.gps_offset(start.lat,
5557
start.lng,
5558
-100,
5559
-200)
5560
roi_alt = 0
5561
self.progress("Using MAV_CMD_DO_SET_ROI")
5562
self.run_cmd(
5563
mavutil.mavlink.MAV_CMD_DO_SET_ROI,
5564
p5=roi_lat,
5565
p6=roi_lon,
5566
p7=roi_alt,
5567
)
5568
self.test_mount_pitch(-7.5, 1, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT)
5569
5570
start = self.mav.location()
5571
(roi_lat, roi_lon) = mavextra.gps_offset(start.lat,
5572
start.lng,
5573
-100,
5574
-200)
5575
roi_alt = 0
5576
self.progress("Using MAV_CMD_DO_SET_ROI (COMMAND_INT)")
5577
self.run_cmd_int(
5578
mavutil.mavlink.MAV_CMD_DO_SET_ROI,
5579
0,
5580
0,
5581
0,
5582
0,
5583
int(roi_lat*1e7),
5584
int(roi_lon*1e7),
5585
roi_alt,
5586
frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
5587
)
5588
self.test_mount_pitch(-7.5, 1, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT)
5589
self.progress("Using MAV_CMD_DO_SET_ROI (COMMAND_INT), absolute-alt-frame")
5590
# this is pointing essentially straight down
5591
self.run_cmd_int(
5592
mavutil.mavlink.MAV_CMD_DO_SET_ROI,
5593
0,
5594
0,
5595
0,
5596
0,
5597
int(roi_lat*1e7),
5598
int(roi_lon*1e7),
5599
roi_alt,
5600
frame=mavutil.mavlink.MAV_FRAME_GLOBAL,
5601
)
5602
self.test_mount_pitch(-70, 1, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT, hold=2)
5603
5604
self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)
5605
self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)
5606
5607
self.progress("Testing mount roi-sysid behaviour")
5608
self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)
5609
start = self.mav.location()
5610
self.progress("start=%s" % str(start))
5611
(roi_lat, roi_lon) = mavextra.gps_offset(start.lat,
5612
start.lng,
5613
10,
5614
20)
5615
roi_alt = 0
5616
self.progress("Using MAV_CMD_DO_SET_ROI_SYSID")
5617
self.run_cmd(
5618
mavutil.mavlink.MAV_CMD_DO_SET_ROI_SYSID,
5619
p1=self.mav.source_system,
5620
)
5621
self.mav.mav.global_position_int_send(
5622
0, # time boot ms
5623
int(roi_lat * 1e7),
5624
int(roi_lon * 1e7),
5625
0 * 1000, # mm alt amsl
5626
0 * 1000, # relalt mm UP!
5627
0, # vx
5628
0, # vy
5629
0, # vz
5630
0 # heading
5631
)
5632
self.test_mount_pitch(-89, 5, mavutil.mavlink.MAV_MOUNT_MODE_SYSID_TARGET, hold=2)
5633
5634
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI_NONE)
5635
self.run_cmd_int(
5636
mavutil.mavlink.MAV_CMD_DO_SET_ROI_SYSID,
5637
p1=self.mav.source_system,
5638
)
5639
self.mav.mav.global_position_int_send(
5640
0, # time boot ms
5641
int(roi_lat * 1e7),
5642
int(roi_lon * 1e7),
5643
670 * 1000, # mm alt amsl
5644
100 * 1000, # mm UP!
5645
0, # vx
5646
0, # vy
5647
0, # vz
5648
0 # heading
5649
)
5650
self.test_mount_pitch(
5651
68,
5652
5,
5653
mavutil.mavlink.MAV_MOUNT_MODE_SYSID_TARGET,
5654
hold=2,
5655
constrained=constrain_sysid_target,
5656
)
5657
5658
self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)
5659
self.test_mount_pitch(0, 0.1, mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)
5660
5661
self.disarm_vehicle(force=True)
5662
5663
self.test_mount_body_yaw()
5664
5665
def test_mount_body_yaw(self):
5666
'''check reporting of yaw'''
5667
# change mount to neutral mode (point forward, not stabilising)
5668
self.takeoff(10, mode='GUIDED')
5669
5670
self.set_mount_mode(mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL)
5671
5672
for heading in 30, 45, 150:
5673
self.guided_achieve_heading(heading)
5674
5675
r, p , y, yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg()
5676
5677
if yaw_is_absolute:
5678
raise NotAchievedException("Expected a relative yaw")
5679
5680
if y > 1:
5681
raise NotAchievedException("Bad yaw (y=%f)")
5682
5683
self.do_RTL()
5684
5685
def Mount(self):
5686
'''test servo mount'''
5687
self.setup_servo_mount()
5688
self.reboot_sitl() # to handle MNT_TYPE changing
5689
self.mount_test_body()
5690
5691
def MountSolo(self):
5692
'''test type=2, a "Solo" mount'''
5693
self.set_parameters({
5694
"MNT1_TYPE": 2,
5695
"RC6_OPTION": 213, # MOUNT1_PITCH
5696
})
5697
self.customise_SITL_commandline([
5698
"--gimbal" # connects on port 5762
5699
])
5700
self.mount_test_body(
5701
pitch_rc_neutral=1818,
5702
do_rate_tests=False, # solo can't do rate control (yet?)
5703
constrain_sysid_target=False, # not everything constrains all angles
5704
)
5705
5706
def assert_mount_rpy(self, r, p, y, tolerance=1):
5707
'''assert mount atttiude in degrees'''
5708
got_r, got_p, got_y, yaw_is_absolute = self.get_mount_roll_pitch_yaw_deg()
5709
for (want, got, name) in (r, got_r, "roll"), (p, got_p, "pitch"), (y, got_y, "yaw"):
5710
if abs(want - got) > tolerance:
5711
raise NotAchievedException("%s incorrect; want=%f got=%f" %
5712
(name, want, got))
5713
5714
def neutralise_gimbal(self):
5715
'''put mount into neutralise mode, assert it is at zero angles'''
5716
self.run_cmd(
5717
mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,
5718
p7=mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL,
5719
)
5720
self.test_mount_pitch(0, 0, mavutil.mavlink.MAV_MOUNT_MODE_RETRACT)
5721
5722
def MAV_CMD_DO_MOUNT_CONTROL(self):
5723
'''test MAV_CMD_DO_MOUNT_CONTROL mavlink command'''
5724
5725
# setup mount parameters
5726
self.context_push()
5727
self.setup_servo_mount()
5728
self.reboot_sitl() # to handle MNT_TYPE changing
5729
5730
takeoff_loc = self.mav.location()
5731
5732
self.takeoff(20, mode='GUIDED')
5733
self.guided_achieve_heading(315)
5734
5735
self.run_cmd(
5736
mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,
5737
p7=mavutil.mavlink.MAV_MOUNT_MODE_RETRACT,
5738
)
5739
self.run_cmd_int(
5740
mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,
5741
p7=mavutil.mavlink.MAV_MOUNT_MODE_RETRACT,
5742
)
5743
5744
for method in self.run_cmd, self.run_cmd_int:
5745
self.start_subtest("MAV_MOUNT_MODE_GPS_POINT")
5746
5747
self.progress("start=%s" % str(takeoff_loc))
5748
t = self.offset_location_ne(takeoff_loc, 20, 0)
5749
self.progress("targetting=%s" % str(t))
5750
5751
# this command is *weird* as the lat/lng is *always* 1e7,
5752
# even when transported via COMMAND_LONG!
5753
x = int(t.lat * 1e7)
5754
y = int(t.lng * 1e7)
5755
method(
5756
mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,
5757
p4=0, # this is a relative altitude!
5758
p5=x,
5759
p6=y,
5760
p7=mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT,
5761
)
5762
self.test_mount_pitch(-45, 5, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT)
5763
self.neutralise_gimbal()
5764
5765
self.start_subtest("MAV_MOUNT_MODE_HOME_LOCATION")
5766
method(
5767
mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,
5768
p7=mavutil.mavlink.MAV_MOUNT_MODE_HOME_LOCATION,
5769
)
5770
self.test_mount_pitch(-90, 5, mavutil.mavlink.MAV_MOUNT_MODE_HOME_LOCATION)
5771
self.neutralise_gimbal()
5772
5773
# try an invalid mount mode. Note that this is asserting we
5774
# are receiving a result code which is actually incorrect;
5775
# this should be MAV_RESULT_DENIED
5776
self.start_subtest("Invalid mode")
5777
method(
5778
mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,
5779
p7=87,
5780
want_result=mavutil.mavlink.MAV_RESULT_FAILED,
5781
)
5782
5783
self.start_subtest("MAV_MOUNT_MODE_MAVLINK_TARGETING")
5784
r = 15
5785
p = 20
5786
y = 30
5787
method(
5788
mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,
5789
p1=p,
5790
p2=r,
5791
p3=y,
5792
p7=mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING,
5793
)
5794
self.delay_sim_time(2)
5795
self.assert_mount_rpy(r, p, y)
5796
self.neutralise_gimbal()
5797
5798
self.start_subtest("MAV_MOUNT_MODE_RC_TARGETING")
5799
method(
5800
mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,
5801
p7=mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING,
5802
)
5803
self.test_mount_rc_targetting()
5804
5805
self.start_subtest("MAV_MOUNT_MODE_RETRACT")
5806
self.context_push()
5807
retract_r = 13
5808
retract_p = 23
5809
retract_y = 33
5810
self.set_parameters({
5811
"MNT1_RETRACT_X": retract_r,
5812
"MNT1_RETRACT_Y": retract_p,
5813
"MNT1_RETRACT_Z": retract_y,
5814
})
5815
method(
5816
mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,
5817
p7=mavutil.mavlink.MAV_MOUNT_MODE_RETRACT,
5818
)
5819
self.delay_sim_time(3)
5820
self.assert_mount_rpy(retract_r, retract_p, retract_y)
5821
self.context_pop()
5822
5823
self.do_RTL()
5824
5825
self.context_pop()
5826
self.reboot_sitl()
5827
5828
def AutoYawDO_MOUNT_CONTROL(self):
5829
'''test AutoYaw behaviour when MAV_CMD_DO_MOUNT_CONTROL sent to Mount without Yaw control'''
5830
5831
# setup mount parameters
5832
self.context_push()
5833
5834
yaw_servo = 7
5835
self.setup_servo_mount(roll_servo=5, pitch_servo=6, yaw_servo=yaw_servo)
5836
# Disable Mount Yaw servo
5837
self.set_parameters({
5838
"SERVO%u_FUNCTION" % yaw_servo: 0,
5839
})
5840
self.reboot_sitl() # to handle MNT_TYPE changing
5841
5842
self.takeoff(20, mode='GUIDED')
5843
5844
for mount_yaw in [-45, 0, 45]:
5845
heading = 330
5846
self.guided_achieve_heading(heading)
5847
self.assert_heading(heading)
5848
5849
self.neutralise_gimbal()
5850
5851
r = 15
5852
p = 20
5853
self.run_cmd_int(
5854
mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,
5855
p1=p,
5856
p2=r,
5857
p3=mount_yaw,
5858
p7=mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING,
5859
)
5860
self.delay_sim_time(5)
5861
# We have disabled yaw servo, so expect mount yaw to be zero
5862
self.assert_mount_rpy(r, p, 0)
5863
# But we expect the copter to yaw instead
5864
self.assert_heading(heading + mount_yaw)
5865
5866
self.do_RTL()
5867
5868
self.context_pop()
5869
self.reboot_sitl()
5870
5871
def MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE(self):
5872
'''test MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE mavlink command'''
5873
# setup mount parameters
5874
self.context_push()
5875
self.setup_servo_mount()
5876
self.reboot_sitl() # to handle MNT_TYPE changing
5877
5878
self.context_set_message_rate_hz('GIMBAL_MANAGER_STATUS', 10)
5879
self.assert_received_message_field_values('GIMBAL_MANAGER_STATUS', {
5880
"gimbal_device_id": 1,
5881
"primary_control_sysid": 0,
5882
"primary_control_compid": 0,
5883
})
5884
5885
for method in self.run_cmd, self.run_cmd_int:
5886
self.start_subtest("set_sysid-compid")
5887
method(
5888
mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE,
5889
p1=37,
5890
p2=38,
5891
)
5892
self.assert_received_message_field_values('GIMBAL_MANAGER_STATUS', {
5893
"gimbal_device_id": 1,
5894
"primary_control_sysid": 37,
5895
"primary_control_compid": 38,
5896
})
5897
5898
self.start_subtest("leave unchanged")
5899
method(mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, p1=-1)
5900
self.assert_received_message_field_values('GIMBAL_MANAGER_STATUS', {
5901
"gimbal_device_id": 1,
5902
"primary_control_sysid": 37,
5903
"primary_control_compid": 38,
5904
})
5905
5906
# ardupilot currently handles this incorrectly:
5907
# self.start_subtest("self-controlled")
5908
# method(mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, p1=-2)
5909
# self.assert_received_message_field_values('GIMBAL_MANAGER_STATUS', {
5910
# "gimbal_device_id": 1,
5911
# "primary_control_sysid": 1,
5912
# "primary_control_compid": 1,
5913
# })
5914
5915
self.start_subtest("release control")
5916
method(
5917
mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE,
5918
p1=self.mav.source_system,
5919
p2=self.mav.source_component,
5920
)
5921
self.assert_received_message_field_values('GIMBAL_MANAGER_STATUS', {
5922
"gimbal_device_id": 1,
5923
"primary_control_sysid": self.mav.source_system,
5924
"primary_control_compid": self.mav.source_component,
5925
})
5926
method(mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, p1=-3)
5927
self.assert_received_message_field_values('GIMBAL_MANAGER_STATUS', {
5928
"gimbal_device_id": 1,
5929
"primary_control_sysid": 0,
5930
"primary_control_compid": 0,
5931
})
5932
5933
self.context_pop()
5934
self.reboot_sitl()
5935
5936
def MountYawVehicleForMountROI(self):
5937
'''Test Camera/Antenna Mount vehicle yawing for ROI'''
5938
self.context_push()
5939
5940
self.set_parameter("SYSID_MYGCS", self.mav.source_system)
5941
yaw_servo = 7
5942
self.setup_servo_mount(yaw_servo=yaw_servo)
5943
self.reboot_sitl() # to handle MNT1_TYPE changing
5944
5945
self.progress("checking ArduCopter yaw-aircraft-for-roi")
5946
ex = None
5947
try:
5948
self.takeoff(20, mode='GUIDED')
5949
5950
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
5951
self.progress("current heading %u" % m.heading)
5952
self.set_parameter("SERVO%u_FUNCTION" % yaw_servo, 0) # yaw
5953
self.progress("Waiting for check_servo_map to do its job")
5954
self.delay_sim_time(5)
5955
self.progress("Pointing North")
5956
self.guided_achieve_heading(0)
5957
self.delay_sim_time(5)
5958
start = self.mav.location()
5959
(roi_lat, roi_lon) = mavextra.gps_offset(start.lat,
5960
start.lng,
5961
-100,
5962
-100)
5963
roi_alt = 0
5964
self.progress("Using MAV_CMD_DO_SET_ROI")
5965
self.run_cmd(
5966
mavutil.mavlink.MAV_CMD_DO_SET_ROI,
5967
p5=roi_lat,
5968
p6=roi_lon,
5969
p7=roi_alt,
5970
)
5971
5972
self.progress("Waiting for vehicle to point towards ROI")
5973
self.wait_heading(225, timeout=600, minimum_duration=2)
5974
5975
# the following numbers are 1-degree-latitude and
5976
# 0-degrees longitude - just so that we start to
5977
# really move a lot.
5978
there = mavutil.location(1, 0, 0, 0)
5979
5980
self.progress("Starting to move")
5981
self.mav.mav.set_position_target_global_int_send(
5982
0, # timestamp
5983
1, # target system_id
5984
1, # target component id
5985
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
5986
MAV_POS_TARGET_TYPE_MASK.POS_ONLY | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE, # mask specifying use-only-lat-lon-alt
5987
there.lat, # lat
5988
there.lng, # lon
5989
there.alt, # alt
5990
0, # vx
5991
0, # vy
5992
0, # vz
5993
0, # afx
5994
0, # afy
5995
0, # afz
5996
0, # yaw
5997
0, # yawrate
5998
)
5999
6000
self.progress("Starting to move changes the target")
6001
bearing = self.bearing_to(there)
6002
self.wait_heading(bearing, timeout=600, minimum_duration=2)
6003
6004
self.run_cmd(
6005
mavutil.mavlink.MAV_CMD_DO_SET_ROI,
6006
p5=roi_lat,
6007
p6=roi_lon,
6008
p7=roi_alt,
6009
)
6010
6011
self.progress("Wait for vehicle to point sssse due to moving")
6012
self.wait_heading(170, timeout=600, minimum_duration=1)
6013
6014
self.do_RTL()
6015
6016
except Exception as e:
6017
self.print_exception_caught(e)
6018
ex = e
6019
6020
self.context_pop()
6021
6022
if ex is not None:
6023
raise ex
6024
6025
def ThrowMode(self):
6026
'''Fly Throw Mode'''
6027
# test boomerang mode:
6028
self.progress("Throwing vehicle away")
6029
self.set_parameters({
6030
"THROW_NEXTMODE": 6,
6031
"SIM_SHOVE_Z": -30,
6032
"SIM_SHOVE_X": -20,
6033
})
6034
self.change_mode('THROW')
6035
self.wait_ready_to_arm()
6036
self.arm_vehicle()
6037
try:
6038
self.set_parameter("SIM_SHOVE_TIME", 500)
6039
except ValueError:
6040
# the shove resets this to zero
6041
pass
6042
6043
tstart = self.get_sim_time()
6044
self.wait_mode('RTL')
6045
max_good_tdelta = 15
6046
tdelta = self.get_sim_time() - tstart
6047
self.progress("Vehicle in RTL")
6048
self.wait_rtl_complete()
6049
self.progress("Vehicle disarmed")
6050
if tdelta > max_good_tdelta:
6051
raise NotAchievedException("Took too long to enter RTL: %fs > %fs" %
6052
(tdelta, max_good_tdelta))
6053
self.progress("Vehicle returned")
6054
6055
def hover_and_check_matched_frequency_with_fft_and_psd(self, dblevel=-15, minhz=200, maxhz=300, peakhz=None,
6056
reverse=None, takeoff=True, instance=0):
6057
'''Takeoff and hover, checking the noise against the provided db level and returning psd'''
6058
# find a motor peak
6059
if takeoff:
6060
self.takeoff(10, mode="ALT_HOLD")
6061
6062
tstart, tend, hover_throttle = self.hover_for_interval(15)
6063
self.do_RTL()
6064
6065
psd = self.mavfft_fttd(1, instance, tstart * 1.0e6, tend * 1.0e6)
6066
6067
# batch sampler defaults give 1024 fft and sample rate of 1kz so roughly 1hz/bin
6068
freq = psd["F"][numpy.argmax(psd["X"][minhz:maxhz]) + minhz] * (1000. / 1024.)
6069
peakdb = numpy.amax(psd["X"][minhz:maxhz])
6070
if peakdb < dblevel or (peakhz is not None and abs(freq - peakhz) / peakhz > 0.05):
6071
if reverse is not None:
6072
self.progress("Did not detect a motor peak, found %fHz at %fdB" % (freq, peakdb))
6073
else:
6074
raise NotAchievedException("Did not detect a motor peak, found %fHz at %fdB" % (freq, peakdb))
6075
else:
6076
if reverse is not None:
6077
raise NotAchievedException(
6078
"Detected motor peak at %fHz, throttle %f%%, %fdB" %
6079
(freq, hover_throttle, peakdb))
6080
else:
6081
self.progress("Detected motor peak at %fHz, throttle %f%%, %fdB" %
6082
(freq, hover_throttle, peakdb))
6083
6084
return freq, hover_throttle, peakdb, psd
6085
6086
def hover_and_check_matched_frequency_with_fft(self, dblevel=-15, minhz=200, maxhz=300, peakhz=None,
6087
reverse=None, takeoff=True, instance=0):
6088
'''Takeoff and hover, checking the noise against the provided db level and returning peak db'''
6089
freq, hover_throttle, peakdb, psd = \
6090
self.hover_and_check_matched_frequency_with_fft_and_psd(dblevel, minhz,
6091
maxhz, peakhz, reverse, takeoff, instance)
6092
return freq, hover_throttle, peakdb
6093
6094
def get_average_esc_frequency(self):
6095
mlog = self.dfreader_for_current_onboard_log()
6096
rpm_total = 0
6097
rpm_count = 0
6098
tho = 0
6099
while True:
6100
m = mlog.recv_match()
6101
if m is None:
6102
break
6103
msg_type = m.get_type()
6104
if msg_type == "CTUN":
6105
tho = m.ThO
6106
elif msg_type == "ESC" and tho > 0.1:
6107
rpm_total += m.RPM
6108
rpm_count += 1
6109
6110
esc_hz = rpm_total / (rpm_count * 60)
6111
return esc_hz
6112
6113
def DynamicNotches(self):
6114
"""Use dynamic harmonic notch to control motor noise."""
6115
self.progress("Flying with dynamic notches")
6116
self.context_push()
6117
6118
ex = None
6119
try:
6120
self.set_parameters({
6121
"AHRS_EKF_TYPE": 10,
6122
"INS_LOG_BAT_MASK": 3,
6123
"INS_LOG_BAT_OPT": 0,
6124
"INS_GYRO_FILTER": 100, # set the gyro filter high so we can observe behaviour
6125
"LOG_BITMASK": 958,
6126
"LOG_DISARMED": 0,
6127
"SIM_VIB_MOT_MAX": 350,
6128
"SIM_GYR1_RND": 20,
6129
})
6130
self.reboot_sitl()
6131
6132
self.takeoff(10, mode="ALT_HOLD")
6133
6134
# find a motor peak
6135
freq, hover_throttle, peakdb = self.hover_and_check_matched_frequency_with_fft(-15, 200, 300)
6136
6137
# now add a dynamic notch and check that the peak is squashed
6138
self.set_parameters({
6139
"INS_LOG_BAT_OPT": 2,
6140
"INS_HNTCH_ENABLE": 1,
6141
"INS_HNTCH_FREQ": freq,
6142
"INS_HNTCH_REF": hover_throttle/100.,
6143
"INS_HNTCH_HMNCS": 5, # first and third harmonic
6144
"INS_HNTCH_ATT": 50,
6145
"INS_HNTCH_BW": freq/2,
6146
})
6147
self.reboot_sitl()
6148
6149
freq, hover_throttle, peakdb1 = \
6150
self.hover_and_check_matched_frequency_with_fft(-10, 20, 350, reverse=True)
6151
6152
# now add double dynamic notches and check that the peak is squashed
6153
self.set_parameter("INS_HNTCH_OPTS", 1)
6154
self.reboot_sitl()
6155
6156
freq, hover_throttle, peakdb2 = \
6157
self.hover_and_check_matched_frequency_with_fft(-15, 20, 350, reverse=True)
6158
6159
# double-notch should do better, but check for within 5%
6160
if peakdb2 * 1.05 > peakdb1:
6161
raise NotAchievedException(
6162
"Double-notch peak was higher than single-notch peak %fdB > %fdB" %
6163
(peakdb2, peakdb1))
6164
6165
# now add triple dynamic notches and check that the peak is squashed
6166
self.set_parameter("INS_HNTCH_OPTS", 16)
6167
self.reboot_sitl()
6168
6169
freq, hover_throttle, peakdb2 = \
6170
self.hover_and_check_matched_frequency_with_fft(-15, 20, 350, reverse=True)
6171
6172
# triple-notch should do better, but check for within 5%
6173
if peakdb2 * 1.05 > peakdb1:
6174
raise NotAchievedException(
6175
"Triple-notch peak was higher than single-notch peak %fdB > %fdB" %
6176
(peakdb2, peakdb1))
6177
6178
except Exception as e:
6179
self.print_exception_caught(e)
6180
ex = e
6181
6182
self.context_pop()
6183
6184
if ex is not None:
6185
raise ex
6186
6187
def DynamicRpmNotches(self):
6188
"""Use dynamic harmonic notch to control motor noise via ESC telemetry."""
6189
self.progress("Flying with ESC telemetry driven dynamic notches")
6190
6191
self.set_rc_default()
6192
self.set_parameters({
6193
"AHRS_EKF_TYPE": 10,
6194
"INS_LOG_BAT_MASK": 3,
6195
"INS_LOG_BAT_OPT": 0,
6196
"INS_GYRO_FILTER": 300, # set gyro filter high so we can observe behaviour
6197
"LOG_BITMASK": 958,
6198
"LOG_DISARMED": 0,
6199
"SIM_VIB_MOT_MAX": 350,
6200
"SIM_GYR1_RND": 20,
6201
"SIM_ESC_TELEM": 1
6202
})
6203
self.reboot_sitl()
6204
6205
self.takeoff(10, mode="ALT_HOLD")
6206
6207
# find a motor peak, the peak is at about 190Hz, so checking between 50 and 320Hz should be safe.
6208
# there is a second harmonic at 380Hz which should be avoided to make the test reliable
6209
# detect at -5dB so we don't pick some random noise as the peak. The actual peak is about +15dB
6210
freq, hover_throttle, peakdb = self.hover_and_check_matched_frequency_with_fft(-5, 50, 320)
6211
6212
# now add a dynamic notch and check that the peak is squashed
6213
self.set_parameters({
6214
"INS_LOG_BAT_OPT": 4,
6215
"INS_HNTCH_ENABLE": 1,
6216
"INS_HNTCH_FREQ": 80,
6217
"INS_HNTCH_REF": 1.0,
6218
"INS_HNTCH_HMNCS": 5, # first and third harmonic
6219
"INS_HNTCH_ATT": 50,
6220
"INS_HNTCH_BW": 40,
6221
"INS_HNTCH_MODE": 3,
6222
})
6223
self.reboot_sitl()
6224
6225
# -10dB is pretty conservative - actual is about -25dB
6226
freq, hover_throttle, peakdb1, psd = \
6227
self.hover_and_check_matched_frequency_with_fft_and_psd(-10, 50, 320, reverse=True, instance=2)
6228
# find the noise at the motor frequency
6229
esc_hz = self.get_average_esc_frequency()
6230
esc_peakdb1 = psd["X"][int(esc_hz)]
6231
6232
# now add notch-per motor and check that the peak is squashed
6233
self.set_parameter("INS_HNTCH_OPTS", 2)
6234
self.reboot_sitl()
6235
6236
freq, hover_throttle, peakdb2, psd = \
6237
self.hover_and_check_matched_frequency_with_fft_and_psd(-10, 50, 320, reverse=True, instance=2)
6238
# find the noise at the motor frequency
6239
esc_hz = self.get_average_esc_frequency()
6240
esc_peakdb2 = psd["X"][int(esc_hz)]
6241
6242
# notch-per-motor will be better at the average ESC frequency
6243
if esc_peakdb2 > esc_peakdb1:
6244
raise NotAchievedException(
6245
"Notch-per-motor peak was higher than single-notch peak %fdB > %fdB" %
6246
(esc_peakdb2, esc_peakdb1))
6247
6248
# check that the noise is being squashed at all. this needs to be an aggresive check so that failure happens easily
6249
# testing shows this to be -58dB on average
6250
if esc_peakdb2 > -25:
6251
raise NotAchievedException(
6252
"Notch-per-motor had a peak of %fdB there should be none" % esc_peakdb2)
6253
6254
# Now do it again for an octacopter
6255
self.context_push()
6256
ex = None
6257
try:
6258
self.progress("Flying Octacopter with ESC telemetry driven dynamic notches")
6259
self.set_parameter("INS_HNTCH_OPTS", 0)
6260
self.customise_SITL_commandline(
6261
[],
6262
defaults_filepath=','.join(self.model_defaults_filepath("octa")),
6263
model="octa"
6264
)
6265
freq, hover_throttle, peakdb1, psd = \
6266
self.hover_and_check_matched_frequency_with_fft_and_psd(-10, 50, 320, reverse=True, instance=2)
6267
# find the noise at the motor frequency
6268
esc_hz = self.get_average_esc_frequency()
6269
esc_peakdb1 = psd["X"][int(esc_hz)]
6270
6271
# now add notch-per motor and check that the peak is squashed
6272
self.set_parameter("INS_HNTCH_HMNCS", 1)
6273
self.set_parameter("INS_HNTCH_OPTS", 2)
6274
self.reboot_sitl()
6275
6276
freq, hover_throttle, peakdb2, psd = \
6277
self.hover_and_check_matched_frequency_with_fft_and_psd(-15, 50, 320, reverse=True, instance=2)
6278
# find the noise at the motor frequency
6279
esc_hz = self.get_average_esc_frequency()
6280
esc_peakdb2 = psd["X"][int(esc_hz)]
6281
6282
# notch-per-motor will be better at the average ESC frequency
6283
if esc_peakdb2 > esc_peakdb1:
6284
raise NotAchievedException(
6285
"Notch-per-motor peak was higher than single-notch peak %fdB > %fdB" %
6286
(esc_peakdb2, esc_peakdb1))
6287
6288
except Exception as e:
6289
self.print_exception_caught(e)
6290
ex = e
6291
self.context_pop()
6292
self.reboot_sitl()
6293
if ex is not None:
6294
raise ex
6295
6296
def DynamicRpmNotchesRateThread(self):
6297
"""Use dynamic harmonic notch to control motor noise via ESC telemetry."""
6298
self.progress("Flying with ESC telemetry driven dynamic notches")
6299
self.context_push()
6300
self.set_rc_default()
6301
self.set_parameters({
6302
"AHRS_EKF_TYPE": 10,
6303
"INS_LOG_BAT_MASK": 3,
6304
"INS_LOG_BAT_OPT": 0,
6305
"INS_GYRO_FILTER": 300, # set gyro filter high so we can observe behaviour
6306
"LOG_BITMASK": 959,
6307
"LOG_DISARMED": 0,
6308
"SIM_VIB_MOT_MAX": 350,
6309
"SIM_GYR1_RND": 20,
6310
"SIM_ESC_TELEM": 1,
6311
"FSTRATE_ENABLE": 1
6312
})
6313
self.reboot_sitl()
6314
6315
self.takeoff(10, mode="ALT_HOLD")
6316
6317
# find a motor peak, the peak is at about 190Hz, so checking between 50 and 320Hz should be safe.
6318
# there is a second harmonic at 380Hz which should be avoided to make the test reliable
6319
# detect at -5dB so we don't pick some random noise as the peak. The actual peak is about +15dB
6320
freq, hover_throttle, peakdb = self.hover_and_check_matched_frequency_with_fft(-5, 50, 320)
6321
6322
# now add a dynamic notch and check that the peak is squashed
6323
self.set_parameters({
6324
"INS_LOG_BAT_OPT": 4,
6325
"INS_HNTCH_ENABLE": 1,
6326
"INS_HNTCH_FREQ": 80,
6327
"INS_HNTCH_REF": 1.0,
6328
"INS_HNTCH_HMNCS": 5, # first and third harmonic
6329
"INS_HNTCH_ATT": 50,
6330
"INS_HNTCH_BW": 40,
6331
"INS_HNTCH_MODE": 3,
6332
"FSTRATE_ENABLE": 1
6333
})
6334
self.reboot_sitl()
6335
6336
# -10dB is pretty conservative - actual is about -25dB
6337
freq, hover_throttle, peakdb1, psd = \
6338
self.hover_and_check_matched_frequency_with_fft_and_psd(-10, 50, 320, reverse=True, instance=2)
6339
# find the noise at the motor frequency
6340
esc_hz = self.get_average_esc_frequency()
6341
esc_peakdb1 = psd["X"][int(esc_hz)]
6342
6343
# now add notch-per motor and check that the peak is squashed
6344
self.set_parameter("INS_HNTCH_OPTS", 2)
6345
self.reboot_sitl()
6346
6347
freq, hover_throttle, peakdb2, psd = \
6348
self.hover_and_check_matched_frequency_with_fft_and_psd(-10, 50, 320, reverse=True, instance=2)
6349
# find the noise at the motor frequency
6350
esc_hz = self.get_average_esc_frequency()
6351
esc_peakdb2 = psd["X"][int(esc_hz)]
6352
6353
# notch-per-motor will be better at the average ESC frequency
6354
if esc_peakdb2 > esc_peakdb1:
6355
raise NotAchievedException(
6356
"Notch-per-motor peak was higher than single-notch peak %fdB > %fdB" %
6357
(esc_peakdb2, esc_peakdb1))
6358
6359
# check that the noise is being squashed at all. this needs to be an aggresive check so that failure happens easily
6360
# testing shows this to be -58dB on average
6361
if esc_peakdb2 > -25:
6362
raise NotAchievedException(
6363
"Notch-per-motor had a peak of %fdB there should be none" % esc_peakdb2)
6364
self.context_pop()
6365
self.reboot_sitl()
6366
6367
def hover_and_check_matched_frequency(self, dblevel=-15, minhz=200, maxhz=300, fftLength=32, peakhz=None):
6368
'''do a simple up-and-down test flight with current vehicle state.
6369
Check that the onboard filter comes up with the same peak-frequency that
6370
post-processing does.'''
6371
self.takeoff(10, mode="ALT_HOLD")
6372
tstart, tend, hover_throttle = self.hover_for_interval(15)
6373
self.do_RTL()
6374
6375
psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6)
6376
6377
# batch sampler defaults give 1024 fft and sample rate of 1kz so roughly 1hz/bin
6378
scale = 1000. / 1024.
6379
sminhz = int(minhz * scale)
6380
smaxhz = int(maxhz * scale)
6381
freq = psd["F"][numpy.argmax(psd["X"][sminhz:smaxhz]) + sminhz]
6382
peakdb = numpy.amax(psd["X"][sminhz:smaxhz])
6383
6384
self.progress("Post-processing FFT detected motor peak at %fHz/%fdB, throttle %f%%" %
6385
(freq, peakdb, hover_throttle))
6386
6387
if peakdb < dblevel:
6388
raise NotAchievedException(
6389
"Detected motor peak not strong enough; want=%fdB got=%fdB" %
6390
(peakdb, dblevel))
6391
6392
# caller can supply an expected frequency:
6393
if peakhz is not None and abs(freq - peakhz) / peakhz > 0.05:
6394
raise NotAchievedException(
6395
"Post-processing detected motor peak at wrong frequency; want=%fHz got=%fHz" %
6396
(peakhz, freq))
6397
6398
# we have a peak make sure that the onboard filter detected
6399
# something close logging is at 10Hz
6400
6401
# peak within resolution of FFT length
6402
pkAvg, nmessages = self.extract_median_FTN1_PkAvg_from_current_onboard_log(tstart, tend)
6403
self.progress("Onboard-FFT detected motor peak at %fHz (processed %d FTN1 messages)" % (pkAvg, nmessages))
6404
6405
# accuracy is determined by sample rate and fft length, given
6406
# our use of quinn we could probably use half of this
6407
freqDelta = 1000. / fftLength
6408
if abs(pkAvg - freq) > freqDelta:
6409
raise NotAchievedException(
6410
"post-processed FFT does not agree with onboard filter on peak frequency; onboard=%fHz post-processed=%fHz/%fdB" % # noqa
6411
(pkAvg, freq, dblevel)
6412
)
6413
return freq
6414
6415
def extract_median_FTN1_PkAvg_from_current_onboard_log(self, tstart, tend):
6416
'''extracts FTN1 messages from log, returns median of pkAvg values and
6417
the number of samples'''
6418
mlog = self.dfreader_for_current_onboard_log()
6419
freqs = []
6420
while True:
6421
m = mlog.recv_match(
6422
type='FTN1',
6423
blocking=False,
6424
condition="FTN1.TimeUS>%u and FTN1.TimeUS<%u" % (tstart * 1.0e6, tend * 1.0e6))
6425
if m is None:
6426
break
6427
freqs.append(m.PkAvg)
6428
return numpy.median(numpy.asarray(freqs)), len(freqs)
6429
6430
def PIDNotches(self):
6431
"""Use dynamic harmonic notch to control motor noise."""
6432
self.progress("Flying with PID notches")
6433
self.set_parameters({
6434
"FILT1_TYPE": 1,
6435
"AHRS_EKF_TYPE": 10,
6436
"INS_LOG_BAT_MASK": 3,
6437
"INS_LOG_BAT_OPT": 0,
6438
"INS_GYRO_FILTER": 100, # set the gyro filter high so we can observe behaviour
6439
"LOG_BITMASK": 65535,
6440
"LOG_DISARMED": 0,
6441
"SIM_VIB_FREQ_X": 120, # roll
6442
"SIM_VIB_FREQ_Y": 120, # pitch
6443
"SIM_VIB_FREQ_Z": 180, # yaw
6444
"FILT1_NOTCH_FREQ": 120,
6445
"ATC_RAT_RLL_NEF": 1,
6446
"ATC_RAT_PIT_NEF": 1,
6447
"ATC_RAT_YAW_NEF": 1,
6448
"SIM_GYR1_RND": 5,
6449
})
6450
self.reboot_sitl()
6451
6452
self.hover_and_check_matched_frequency_with_fft(dblevel=5, minhz=20, maxhz=350, reverse=True)
6453
6454
def StaticNotches(self):
6455
"""Use static harmonic notch to control motor noise."""
6456
self.progress("Flying with Static notches")
6457
self.set_parameters({
6458
"AHRS_EKF_TYPE": 10,
6459
"INS_LOG_BAT_MASK": 3,
6460
"INS_LOG_BAT_OPT": 4,
6461
"INS_GYRO_FILTER": 100, # set the gyro filter high so we can observe behaviour
6462
"LOG_BITMASK": 65535,
6463
"LOG_DISARMED": 0,
6464
"SIM_VIB_FREQ_X": 120, # roll
6465
"SIM_VIB_FREQ_Y": 120, # pitch
6466
"SIM_VIB_FREQ_Z": 120, # yaw
6467
"SIM_VIB_MOT_MULT": 0,
6468
"SIM_GYR1_RND": 5,
6469
"INS_HNTCH_ENABLE": 1,
6470
"INS_HNTCH_FREQ": 120,
6471
"INS_HNTCH_REF": 1.0,
6472
"INS_HNTCH_HMNCS": 3, # first and second harmonic
6473
"INS_HNTCH_ATT": 50,
6474
"INS_HNTCH_BW": 40,
6475
"INS_HNTCH_MODE": 0, # static notch
6476
})
6477
self.reboot_sitl()
6478
6479
self.hover_and_check_matched_frequency_with_fft(dblevel=-15, minhz=20, maxhz=350, reverse=True, instance=2)
6480
6481
def ThrottleGainBoost(self):
6482
"""Use PD and Angle P boost for anti-gravity."""
6483
# basic gyro sample rate test
6484
self.progress("Flying with Throttle-Gain Boost")
6485
6486
# magic tridge EKF type that dramatically speeds up the test
6487
self.set_parameters({
6488
"AHRS_EKF_TYPE": 10,
6489
"EK2_ENABLE": 0,
6490
"EK3_ENABLE": 0,
6491
"INS_FAST_SAMPLE": 0,
6492
"LOG_BITMASK": 959,
6493
"LOG_DISARMED": 0,
6494
"ATC_THR_G_BOOST": 5.0,
6495
})
6496
6497
self.reboot_sitl()
6498
6499
self.takeoff(10, mode="ALT_HOLD")
6500
hover_time = 15
6501
self.progress("Hovering for %u seconds" % hover_time)
6502
tstart = self.get_sim_time()
6503
while self.get_sim_time_cached() < tstart + hover_time:
6504
self.assert_receive_message('ATTITUDE')
6505
6506
# fly fast forrest!
6507
self.set_rc(3, 1900)
6508
self.set_rc(2, 1200)
6509
self.wait_groundspeed(5, 1000)
6510
self.set_rc(3, 1500)
6511
self.set_rc(2, 1500)
6512
6513
self.do_RTL()
6514
6515
def test_gyro_fft_harmonic(self, averaging):
6516
"""Use dynamic harmonic notch to control motor noise with harmonic matching of the first harmonic."""
6517
# basic gyro sample rate test
6518
self.progress("Flying with gyro FFT harmonic - Gyro sample rate")
6519
self.context_push()
6520
ex = None
6521
# we are dealing with probabalistic scenarios involving threads
6522
try:
6523
self.start_subtest("Hover to calculate approximate hover frequency")
6524
# magic tridge EKF type that dramatically speeds up the test
6525
self.set_parameters({
6526
"AHRS_EKF_TYPE": 10,
6527
"EK2_ENABLE": 0,
6528
"EK3_ENABLE": 0,
6529
"INS_LOG_BAT_MASK": 3,
6530
"INS_LOG_BAT_OPT": 0,
6531
"INS_GYRO_FILTER": 100,
6532
"INS_FAST_SAMPLE": 0,
6533
"LOG_BITMASK": 958,
6534
"LOG_DISARMED": 0,
6535
"SIM_DRIFT_SPEED": 0,
6536
"SIM_DRIFT_TIME": 0,
6537
"FFT_THR_REF": self.get_parameter("MOT_THST_HOVER"),
6538
"SIM_GYR1_RND": 20, # enable a noisy gyro
6539
})
6540
6541
# motor peak enabling FFT will also enable the arming
6542
# check, self-testing the functionality
6543
self.set_parameters({
6544
"FFT_ENABLE": 1,
6545
"FFT_MINHZ": 50,
6546
"FFT_MAXHZ": 450,
6547
"FFT_SNR_REF": 10,
6548
})
6549
if averaging:
6550
self.set_parameter("FFT_NUM_FRAMES", 8)
6551
6552
# Step 1: inject actual motor noise and use the FFT to track it
6553
self.set_parameters({
6554
"SIM_VIB_MOT_MAX": 250, # gives a motor peak at about 175Hz
6555
"FFT_WINDOW_SIZE": 64,
6556
"FFT_WINDOW_OLAP": 0.75,
6557
})
6558
6559
self.reboot_sitl()
6560
freq = self.hover_and_check_matched_frequency(-15, 100, 250, 64)
6561
6562
# Step 2: add a second harmonic and check the first is still tracked
6563
self.start_subtest("Add a fixed frequency harmonic at twice the hover frequency "
6564
"and check the right harmonic is found")
6565
self.set_parameters({
6566
"SIM_VIB_FREQ_X": freq * 2,
6567
"SIM_VIB_FREQ_Y": freq * 2,
6568
"SIM_VIB_FREQ_Z": freq * 2,
6569
"SIM_VIB_MOT_MULT": 0.25, # halve the motor noise so that the higher harmonic dominates
6570
})
6571
self.reboot_sitl()
6572
6573
self.hover_and_check_matched_frequency(-15, 100, 250, 64, None)
6574
6575
# Step 3: switch harmonics mid flight and check for tracking
6576
self.start_subtest("Switch harmonics mid flight and check the right harmonic is found")
6577
self.set_parameter("FFT_HMNC_PEAK", 0)
6578
self.reboot_sitl()
6579
6580
self.takeoff(10, mode="ALT_HOLD")
6581
6582
hover_time = 10
6583
tstart, tend_unused, hover_throttle = self.hover_for_interval(hover_time)
6584
6585
self.progress("Switching motor vibration multiplier")
6586
self.set_parameter("SIM_VIB_MOT_MULT", 5.0)
6587
6588
tstart_unused, tend, hover_throttle = self.hover_for_interval(hover_time)
6589
6590
self.do_RTL()
6591
6592
# peak within resolution of FFT length, the highest energy peak switched but our detection should not
6593
pkAvg, nmessages = self.extract_median_FTN1_PkAvg_from_current_onboard_log(tstart, tend)
6594
6595
freqDelta = 1000. / self.get_parameter("FFT_WINDOW_SIZE")
6596
6597
if abs(pkAvg - freq) > freqDelta:
6598
raise NotAchievedException("FFT did not detect a harmonic motor peak, found %f, wanted %f" % (pkAvg, freq))
6599
6600
# Step 4: dynamic harmonic
6601
self.start_subtest("Enable dynamic harmonics and make sure both frequency peaks are attenuated")
6602
# find a motor peak
6603
freq, hover_throttle, peakdb = self.hover_and_check_matched_frequency_with_fft(-15, 100, 350)
6604
6605
# now add a dynamic notch and check that the peak is squashed
6606
self.set_parameters({
6607
"INS_LOG_BAT_OPT": 2,
6608
"INS_HNTCH_ENABLE": 1,
6609
"INS_HNTCH_HMNCS": 1,
6610
"INS_HNTCH_MODE": 4,
6611
"INS_HNTCH_FREQ": freq,
6612
"INS_HNTCH_REF": hover_throttle/100.0,
6613
"INS_HNTCH_ATT": 100,
6614
"INS_HNTCH_BW": freq/2,
6615
"INS_HNTCH_OPTS": 3,
6616
})
6617
self.reboot_sitl()
6618
6619
# 5db is far in excess of the attenuation that the double dynamic-harmonic notch is able
6620
# to provide (-7dB on average), but without the notch the peak is around 20dB so still a safe test
6621
self.hover_and_check_matched_frequency_with_fft(5, 100, 350, reverse=True)
6622
6623
self.set_parameters({
6624
"SIM_VIB_FREQ_X": 0,
6625
"SIM_VIB_FREQ_Y": 0,
6626
"SIM_VIB_FREQ_Z": 0,
6627
"SIM_VIB_MOT_MULT": 1.0,
6628
})
6629
# prevent update parameters from messing with the settings when we pop the context
6630
self.set_parameter("FFT_ENABLE", 0)
6631
self.reboot_sitl()
6632
6633
except Exception as e:
6634
self.print_exception_caught(e)
6635
ex = e
6636
6637
self.context_pop()
6638
6639
# need a final reboot because weird things happen to your
6640
# vehicle state when switching back from EKF type 10!
6641
self.reboot_sitl()
6642
6643
if ex is not None:
6644
raise ex
6645
6646
def GyroFFTHarmonic(self):
6647
"""Use dynamic harmonic notch to control motor noise with harmonic matching of the first harmonic."""
6648
self.test_gyro_fft_harmonic(False)
6649
6650
def GyroFFTContinuousAveraging(self):
6651
"""Use dynamic harmonic notch with FFT averaging to control motor noise
6652
with harmonic matching of the first harmonic."""
6653
self.test_gyro_fft_harmonic(True)
6654
6655
def GyroFFT(self):
6656
"""Use dynamic harmonic notch to control motor noise."""
6657
# basic gyro sample rate test
6658
self.progress("Flying with gyro FFT - Gyro sample rate")
6659
self.context_push()
6660
6661
ex = None
6662
try:
6663
# magic tridge EKF type that dramatically speeds up the test
6664
self.set_parameters({
6665
"AHRS_EKF_TYPE": 10,
6666
"EK2_ENABLE": 0,
6667
"EK3_ENABLE": 0,
6668
"INS_LOG_BAT_MASK": 3,
6669
"INS_LOG_BAT_OPT": 4,
6670
"INS_GYRO_FILTER": 100,
6671
"INS_FAST_SAMPLE": 0,
6672
"LOG_BITMASK": 958,
6673
"LOG_DISARMED": 0,
6674
"SIM_DRIFT_SPEED": 0,
6675
"SIM_DRIFT_TIME": 0,
6676
"SIM_GYR1_RND": 20, # enable a noisy motor peak
6677
})
6678
# enabling FFT will also enable the arming check,
6679
# self-testing the functionality
6680
self.set_parameters({
6681
"FFT_ENABLE": 1,
6682
"FFT_MINHZ": 50,
6683
"FFT_MAXHZ": 450,
6684
"FFT_SNR_REF": 10,
6685
"FFT_WINDOW_SIZE": 128,
6686
"FFT_WINDOW_OLAP": 0.75,
6687
"FFT_SAMPLE_MODE": 0,
6688
})
6689
6690
# Step 1: inject a very precise noise peak at 250hz and make sure the in-flight fft
6691
# can detect it really accurately. For a 128 FFT the frequency resolution is 8Hz so
6692
# a 250Hz peak should be detectable within 5%
6693
self.start_subtest("Inject noise at 250Hz and check the FFT can find the noise")
6694
self.set_parameters({
6695
"SIM_VIB_FREQ_X": 250,
6696
"SIM_VIB_FREQ_Y": 250,
6697
"SIM_VIB_FREQ_Z": 250,
6698
})
6699
6700
self.reboot_sitl()
6701
6702
# find a motor peak
6703
self.hover_and_check_matched_frequency(-15, 100, 350, 128, 250)
6704
6705
# Step 1b: run the same test with an FFT length of 256 which is needed to flush out a
6706
# whole host of bugs related to uint8_t. This also tests very accurately the frequency resolution
6707
self.set_parameter("FFT_WINDOW_SIZE", 256)
6708
self.start_subtest("Inject noise at 250Hz and check the FFT can find the noise")
6709
6710
self.reboot_sitl()
6711
6712
# find a motor peak
6713
self.hover_and_check_matched_frequency(-15, 100, 350, 256, 250)
6714
self.set_parameter("FFT_WINDOW_SIZE", 128)
6715
6716
# Step 2: inject actual motor noise and use the standard length FFT to track it
6717
self.start_subtest("Hover and check that the FFT can find the motor noise")
6718
self.set_parameters({
6719
"SIM_VIB_FREQ_X": 0,
6720
"SIM_VIB_FREQ_Y": 0,
6721
"SIM_VIB_FREQ_Z": 0,
6722
"SIM_VIB_MOT_MAX": 250, # gives a motor peak at about 175Hz
6723
"FFT_WINDOW_SIZE": 32,
6724
"FFT_WINDOW_OLAP": 0.5,
6725
})
6726
6727
self.reboot_sitl()
6728
freq = self.hover_and_check_matched_frequency(-15, 100, 250, 32)
6729
6730
self.set_parameter("SIM_VIB_MOT_MULT", 1.)
6731
6732
# Step 3: add a FFT dynamic notch and check that the peak is squashed
6733
self.start_subtest("Add a dynamic notch, hover and check that the noise peak is now gone")
6734
self.set_parameters({
6735
"INS_LOG_BAT_OPT": 2,
6736
"INS_HNTCH_ENABLE": 1,
6737
"INS_HNTCH_FREQ": freq,
6738
"INS_HNTCH_REF": 1.0,
6739
"INS_HNTCH_ATT": 50,
6740
"INS_HNTCH_BW": freq/2,
6741
"INS_HNTCH_MODE": 4,
6742
})
6743
self.reboot_sitl()
6744
6745
# do test flight:
6746
self.takeoff(10, mode="ALT_HOLD")
6747
tstart, tend, hover_throttle = self.hover_for_interval(15)
6748
# fly fast forrest!
6749
self.set_rc(3, 1900)
6750
self.set_rc(2, 1200)
6751
self.wait_groundspeed(5, 1000)
6752
self.set_rc(3, 1500)
6753
self.set_rc(2, 1500)
6754
self.do_RTL()
6755
6756
psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6)
6757
6758
# batch sampler defaults give 1024 fft and sample rate of 1kz so roughly 1hz/bin
6759
scale = 1000. / 1024.
6760
sminhz = int(100 * scale)
6761
smaxhz = int(350 * scale)
6762
freq = psd["F"][numpy.argmax(psd["X"][sminhz:smaxhz]) + sminhz]
6763
peakdb = numpy.amax(psd["X"][sminhz:smaxhz])
6764
if peakdb < 0:
6765
self.progress("Did not detect a motor peak, found %fHz at %fdB" % (freq, peakdb))
6766
else:
6767
raise NotAchievedException("Detected %fHz motor peak at %fdB" % (freq, peakdb))
6768
6769
# Step 4: loop sample rate test with larger window
6770
self.start_subtest("Hover and check that the FFT can find the motor noise when running at fast loop rate")
6771
# we are limited to half the loop rate for frequency detection
6772
self.set_parameters({
6773
"FFT_MAXHZ": 185,
6774
"INS_LOG_BAT_OPT": 4,
6775
"SIM_VIB_MOT_MAX": 220,
6776
"FFT_WINDOW_SIZE": 64,
6777
"FFT_WINDOW_OLAP": 0.75,
6778
"FFT_SAMPLE_MODE": 1,
6779
})
6780
self.reboot_sitl()
6781
6782
# do test flight:
6783
self.takeoff(10, mode="ALT_HOLD")
6784
tstart, tend, hover_throttle = self.hover_for_interval(15)
6785
self.do_RTL()
6786
6787
# why are we not checking the results from that flight? -pb20220613
6788
6789
# prevent update parameters from messing with the settings
6790
# when we pop the context
6791
self.set_parameter("FFT_ENABLE", 0)
6792
self.reboot_sitl()
6793
6794
except Exception as e:
6795
self.print_exception_caught(e)
6796
ex = e
6797
6798
self.context_pop()
6799
6800
# must reboot after we move away from EKF type 10 to EKF2 or EKF3
6801
self.reboot_sitl()
6802
6803
if ex is not None:
6804
raise ex
6805
6806
def GyroFFTAverage(self):
6807
"""Use dynamic harmonic notch to control motor noise setup via FFT averaging."""
6808
# basic gyro sample rate test
6809
self.progress("Flying with gyro FFT harmonic - Gyro sample rate")
6810
self.context_push()
6811
ex = None
6812
try:
6813
# Step 1
6814
self.start_subtest("Hover to calculate approximate hover frequency and see that it is tracked")
6815
# magic tridge EKF type that dramatically speeds up the test
6816
self.set_parameters({
6817
"INS_HNTCH_ATT": 100,
6818
"AHRS_EKF_TYPE": 10,
6819
"EK2_ENABLE": 0,
6820
"EK3_ENABLE": 0,
6821
"INS_LOG_BAT_MASK": 3,
6822
"INS_LOG_BAT_OPT": 2,
6823
"INS_GYRO_FILTER": 100,
6824
"INS_FAST_SAMPLE": 0,
6825
"LOG_BITMASK": 958,
6826
"LOG_DISARMED": 0,
6827
"SIM_DRIFT_SPEED": 0,
6828
"SIM_DRIFT_TIME": 0,
6829
"SIM_GYR1_RND": 20, # enable a noisy gyro
6830
})
6831
# motor peak enabling FFT will also enable the arming
6832
# check, self-testing the functionality
6833
self.set_parameters({
6834
"FFT_ENABLE": 1,
6835
"FFT_WINDOW_SIZE": 64, # not the default, but makes the test more reliable
6836
"FFT_SNR_REF": 10,
6837
"FFT_MINHZ": 80,
6838
"FFT_MAXHZ": 450,
6839
})
6840
6841
# Step 1: inject actual motor noise and use the FFT to track it
6842
self.set_parameters({
6843
"SIM_VIB_MOT_MAX": 250, # gives a motor peak at about 175Hz
6844
"RC7_OPTION" : 162, # FFT tune
6845
})
6846
6847
self.reboot_sitl()
6848
6849
# hover and engage FFT tracker
6850
self.takeoff(10, mode="ALT_HOLD")
6851
6852
hover_time = 60
6853
6854
# start the tune
6855
self.set_rc(7, 2000)
6856
6857
tstart, tend, hover_throttle = self.hover_for_interval(hover_time)
6858
6859
# finish the tune
6860
self.set_rc(7, 1000)
6861
6862
psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6)
6863
6864
# batch sampler defaults give 1024 fft and sample rate of 1kz so roughly 1hz/bin
6865
freq = psd["F"][numpy.argmax(psd["X"][50:450]) + 50] * (1000. / 1024.)
6866
6867
detected_ref = self.get_parameter("INS_HNTCH_REF")
6868
detected_freq = self.get_parameter("INS_HNTCH_FREQ")
6869
self.progress("FFT detected parameters were %fHz, ref %f" % (detected_freq, detected_ref))
6870
6871
# approximate the scaled frequency
6872
scaled_freq_at_hover = math.sqrt((hover_throttle / 100.) / detected_ref) * detected_freq
6873
6874
# Check we matched
6875
if abs(scaled_freq_at_hover - freq) / scaled_freq_at_hover > 0.05:
6876
raise NotAchievedException("Detected frequency %fHz did not match required %fHz" %
6877
(scaled_freq_at_hover, freq))
6878
6879
if self.get_parameter("INS_HNTCH_ENABLE") != 1:
6880
raise NotAchievedException("Harmonic notch was not enabled")
6881
6882
# Step 2: now rerun the test and check that the peak is squashed
6883
self.start_subtest("Verify that noise is suppressed by the harmonic notch")
6884
self.hover_and_check_matched_frequency_with_fft(0, 100, 350, reverse=True, takeoff=False)
6885
6886
# reset notch to defaults
6887
self.set_parameters({
6888
"INS_HNTCH_HMNCS": 3.0,
6889
"INS_HNTCH_ENABLE": 0.0,
6890
"INS_HNTCH_REF": 0.0,
6891
"INS_HNTCH_FREQ": 80,
6892
"INS_HNTCH_BW": 40,
6893
"INS_HNTCH_FM_RAT": 1.0
6894
})
6895
6896
# Step 3: add a second harmonic and check the first is still tracked
6897
self.start_subtest("Add a fixed frequency harmonic at twice the hover frequency "
6898
"and check the right harmonic is found")
6899
self.set_parameters({
6900
"SIM_VIB_FREQ_X": detected_freq * 2,
6901
"SIM_VIB_FREQ_Y": detected_freq * 2,
6902
"SIM_VIB_FREQ_Z": detected_freq * 2,
6903
"SIM_VIB_MOT_MULT": 0.25, # halve the motor noise so that the higher harmonic dominates
6904
})
6905
self.reboot_sitl()
6906
6907
# hover and engage FFT tracker
6908
self.takeoff(10, mode="ALT_HOLD")
6909
6910
hover_time = 60
6911
6912
# start the tune
6913
self.set_rc(7, 2000)
6914
6915
tstart, tend, hover_throttle = self.hover_for_interval(hover_time)
6916
6917
# finish the tune
6918
self.set_rc(7, 1000)
6919
6920
self.do_RTL()
6921
6922
detected_ref = self.get_parameter("INS_HNTCH_REF")
6923
detected_freq = self.get_parameter("INS_HNTCH_FREQ")
6924
self.progress("FFT detected parameters were %fHz, ref %f" % (detected_freq, detected_ref))
6925
6926
# approximate the scaled frequency
6927
scaled_freq_at_hover = math.sqrt((hover_throttle / 100.) / detected_ref) * detected_freq
6928
6929
# Check we matched
6930
if abs(scaled_freq_at_hover - freq) / scaled_freq_at_hover > 0.05:
6931
raise NotAchievedException("Detected frequency %fHz did not match required %fHz" %
6932
(scaled_freq_at_hover, freq))
6933
6934
if self.get_parameter("INS_HNTCH_ENABLE") != 1:
6935
raise NotAchievedException("Harmonic notch was not enabled")
6936
6937
self.set_parameters({
6938
"SIM_VIB_FREQ_X": 0,
6939
"SIM_VIB_FREQ_Y": 0,
6940
"SIM_VIB_FREQ_Z": 0,
6941
"SIM_VIB_MOT_MULT": 1.0,
6942
"INS_HNTCH_HMNCS": 3.0,
6943
"INS_HNTCH_ENABLE": 0.0,
6944
"INS_HNTCH_REF": 0.0,
6945
"INS_HNTCH_FREQ": 80,
6946
"INS_HNTCH_BW": 40,
6947
"INS_HNTCH_FM_RAT": 1.0
6948
})
6949
# prevent update parameters from messing with the settings when we pop the context
6950
self.set_parameter("FFT_ENABLE", 0)
6951
self.reboot_sitl()
6952
6953
except Exception as e:
6954
self.print_exception_caught(e)
6955
ex = e
6956
6957
self.context_pop()
6958
6959
# need a final reboot because weird things happen to your
6960
# vehicle state when switching back from EKF type 10!
6961
self.reboot_sitl()
6962
6963
if ex is not None:
6964
raise ex
6965
6966
def GyroFFTPostFilter(self):
6967
"""Use FFT-driven dynamic harmonic notch to control post-RPM filter motor noise."""
6968
# basic gyro sample rate test
6969
self.progress("Flying with gyro FFT post-filter supression - Gyro sample rate")
6970
self.context_push()
6971
ex = None
6972
try:
6973
# This set of parameters creates two noise peaks one at the motor frequency and one at 250Hz
6974
# we then use ESC telemetry to drive the notch to clean up the motor noise and a post-filter
6975
# FFT notch to clean up the remaining 250Hz. If either notch fails then the test will be failed
6976
# due to too much noise being present
6977
self.set_parameters({
6978
"AHRS_EKF_TYPE": 10, # magic tridge EKF type that dramatically speeds up the test
6979
"EK2_ENABLE": 0,
6980
"EK3_ENABLE": 0,
6981
"INS_LOG_BAT_MASK": 3,
6982
"INS_LOG_BAT_OPT": 4,
6983
"INS_GYRO_FILTER": 100,
6984
"INS_FAST_SAMPLE": 3,
6985
"LOG_BITMASK": 958,
6986
"LOG_DISARMED": 0,
6987
"SIM_DRIFT_SPEED": 0,
6988
"SIM_DRIFT_TIME": 0,
6989
"SIM_GYR1_RND": 20, # enable a noisy gyro
6990
"INS_HNTCH_ENABLE": 1,
6991
"INS_HNTCH_FREQ": 80,
6992
"INS_HNTCH_REF": 1.0,
6993
"INS_HNTCH_HMNCS": 1, # first harmonic
6994
"INS_HNTCH_ATT": 50,
6995
"INS_HNTCH_BW": 30,
6996
"INS_HNTCH_MODE": 3, # ESC telemetry
6997
"INS_HNTCH_OPTS": 2, # notch-per-motor
6998
"INS_HNTC2_ENABLE": 1,
6999
"INS_HNTC2_FREQ": 80,
7000
"INS_HNTC2_REF": 1.0,
7001
"INS_HNTC2_HMNCS": 1,
7002
"INS_HNTC2_ATT": 50,
7003
"INS_HNTC2_BW": 40,
7004
"INS_HNTC2_MODE": 4, # in-flight FFT
7005
"INS_HNTC2_OPTS": 18, # triple-notch, notch-per-FFT peak
7006
"FFT_ENABLE": 1,
7007
"FFT_WINDOW_SIZE": 64, # not the default, but makes the test more reliable
7008
"FFT_OPTIONS": 1,
7009
"FFT_MINHZ": 50,
7010
"FFT_MAXHZ": 450,
7011
"SIM_VIB_MOT_MAX": 250, # gives a motor peak at about 145Hz
7012
"SIM_VIB_FREQ_X": 250, # create another peak at 250hz
7013
"SIM_VIB_FREQ_Y": 250,
7014
"SIM_VIB_FREQ_Z": 250,
7015
"SIM_GYR_FILE_RW": 2, # write data to a file
7016
})
7017
self.reboot_sitl()
7018
7019
# do test flight:
7020
self.takeoff(10, mode="ALT_HOLD")
7021
tstart, tend, hover_throttle = self.hover_for_interval(60)
7022
# fly fast forrest!
7023
self.set_rc(3, 1900)
7024
self.set_rc(2, 1200)
7025
self.wait_groundspeed(5, 1000)
7026
self.set_rc(3, 1500)
7027
self.set_rc(2, 1500)
7028
self.do_RTL()
7029
7030
psd = self.mavfft_fttd(1, 2, tstart * 1.0e6, tend * 1.0e6)
7031
7032
# batch sampler defaults give 1024 fft and sample rate of 1kz so roughly 1hz/bin
7033
scale = 1000. / 1024.
7034
sminhz = int(100 * scale)
7035
smaxhz = int(350 * scale)
7036
freq = psd["F"][numpy.argmax(psd["X"][sminhz:smaxhz]) + sminhz]
7037
peakdb = numpy.amax(psd["X"][sminhz:smaxhz])
7038
if peakdb < -5:
7039
self.progress("Did not detect a motor peak, found %fHz at %fdB" % (freq, peakdb))
7040
else:
7041
raise NotAchievedException("Detected %fHz motor peak at %fdB" % (freq, peakdb))
7042
7043
# prevent update parameters from messing with the settings when we pop the context
7044
self.set_parameters({
7045
"SIM_VIB_FREQ_X": 0,
7046
"SIM_VIB_FREQ_Y": 0,
7047
"SIM_VIB_FREQ_Z": 0,
7048
"SIM_VIB_MOT_MULT": 1.0,
7049
"SIM_GYR_FILE_RW": 0, # stop writing data
7050
"FFT_ENABLE": 0,
7051
})
7052
self.reboot_sitl()
7053
7054
except Exception as e:
7055
self.print_exception_caught(e)
7056
ex = e
7057
7058
self.context_pop()
7059
7060
# need a final reboot because weird things happen to your
7061
# vehicle state when switching back from EKF type 10!
7062
self.reboot_sitl()
7063
7064
if ex is not None:
7065
raise ex
7066
7067
def GyroFFTMotorNoiseCheck(self):
7068
"""Use FFT to detect post-filter motor noise."""
7069
# basic gyro sample rate test
7070
self.progress("Flying with FFT motor-noise detection - Gyro sample rate")
7071
self.context_push()
7072
ex = None
7073
try:
7074
# This set of parameters creates two noise peaks one at the motor frequency and one at 250Hz
7075
# we then use ESC telemetry to drive the notch to clean up the motor noise and a post-filter
7076
# FFT notch to clean up the remaining 250Hz. If either notch fails then the test will be failed
7077
# due to too much noise being present
7078
self.set_parameters({
7079
"AHRS_EKF_TYPE": 10, # magic tridge EKF type that dramatically speeds up the test
7080
"EK2_ENABLE": 0,
7081
"EK3_ENABLE": 0,
7082
"INS_LOG_BAT_MASK": 3,
7083
"INS_LOG_BAT_OPT": 4,
7084
"INS_GYRO_FILTER": 100,
7085
"INS_FAST_SAMPLE": 3,
7086
"LOG_BITMASK": 958,
7087
"LOG_DISARMED": 0,
7088
"SIM_DRIFT_SPEED": 0,
7089
"SIM_DRIFT_TIME": 0,
7090
"SIM_GYR1_RND": 200, # enable a noisy gyro
7091
"INS_HNTCH_ENABLE": 1,
7092
"INS_HNTCH_FREQ": 80,
7093
"INS_HNTCH_REF": 1.0,
7094
"INS_HNTCH_HMNCS": 1, # first harmonic
7095
"INS_HNTCH_ATT": 50,
7096
"INS_HNTCH_BW": 30,
7097
"INS_HNTCH_MODE": 3, # ESC telemetry
7098
"INS_HNTCH_OPTS": 2, # notch-per-motor
7099
"INS_HNTC2_ENABLE": 1,
7100
"INS_HNTC2_FREQ": 80,
7101
"INS_HNTC2_REF": 1.0,
7102
"INS_HNTC2_HMNCS": 1,
7103
"INS_HNTC2_ATT": 50,
7104
"INS_HNTC2_BW": 40,
7105
"INS_HNTC2_MODE": 0, # istatic notch
7106
"INS_HNTC2_OPTS": 16, # triple-notch
7107
"FFT_ENABLE": 1,
7108
"FFT_WINDOW_SIZE": 64, # not the default, but makes the test more reliable
7109
"FFT_OPTIONS": 3,
7110
"FFT_MINHZ": 50,
7111
"FFT_MAXHZ": 450,
7112
"SIM_VIB_MOT_MAX": 250, # gives a motor peak at about 145Hz
7113
"SIM_VIB_FREQ_X": 250, # create another peak at 250hz
7114
"SIM_VIB_FREQ_Y": 250,
7115
"SIM_VIB_FREQ_Z": 250,
7116
"SIM_GYR_FILE_RW": 2, # write data to a file
7117
})
7118
self.reboot_sitl()
7119
7120
# do test flight:
7121
self.takeoff(10, mode="ALT_HOLD")
7122
tstart, tend, hover_throttle = self.hover_for_interval(10)
7123
self.wait_statustext("Noise ", timeout=20)
7124
self.set_parameter("SIM_GYR1_RND", 0) # stop noise so that we can get home
7125
self.do_RTL()
7126
7127
# prevent update parameters from messing with the settings when we pop the context
7128
self.set_parameters({
7129
"SIM_VIB_FREQ_X": 0,
7130
"SIM_VIB_FREQ_Y": 0,
7131
"SIM_VIB_FREQ_Z": 0,
7132
"SIM_VIB_MOT_MULT": 1.0,
7133
"SIM_GYR_FILE_RW": 0, # stop writing data
7134
"FFT_ENABLE": 0,
7135
})
7136
self.reboot_sitl()
7137
7138
except Exception as e:
7139
self.print_exception_caught(e)
7140
ex = e
7141
7142
self.context_pop()
7143
7144
# need a final reboot because weird things happen to your
7145
# vehicle state when switching back from EKF type 10!
7146
self.reboot_sitl()
7147
7148
if ex is not None:
7149
raise ex
7150
7151
def BrakeMode(self):
7152
'''Fly Brake Mode'''
7153
# test brake mode
7154
self.progress("Testing brake mode")
7155
self.takeoff(10, mode="LOITER")
7156
7157
self.progress("Ensuring RC inputs have no effect in brake mode")
7158
self.change_mode("STABILIZE")
7159
self.set_rc(3, 1500)
7160
self.set_rc(2, 1200)
7161
self.wait_groundspeed(5, 1000)
7162
7163
self.change_mode("BRAKE")
7164
self.wait_groundspeed(0, 1)
7165
7166
self.set_rc(2, 1500)
7167
7168
self.do_RTL()
7169
self.progress("Ran brake mode")
7170
7171
def fly_guided_move_to(self, destination, timeout=30):
7172
'''move to mavutil.location location; absolute altitude'''
7173
tstart = self.get_sim_time()
7174
self.mav.mav.set_position_target_global_int_send(
7175
0, # timestamp
7176
1, # target system_id
7177
1, # target component id
7178
mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
7179
MAV_POS_TARGET_TYPE_MASK.POS_ONLY | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE, # mask specifying use-only-lat-lon-alt
7180
int(destination.lat * 1e7), # lat
7181
int(destination.lng * 1e7), # lon
7182
destination.alt, # alt
7183
0, # vx
7184
0, # vy
7185
0, # vz
7186
0, # afx
7187
0, # afy
7188
0, # afz
7189
0, # yaw
7190
0, # yawrate
7191
)
7192
while True:
7193
if self.get_sim_time() - tstart > timeout:
7194
raise NotAchievedException()
7195
delta = self.get_distance(self.mav.location(), destination)
7196
self.progress("delta=%f (want <1)" % delta)
7197
if delta < 1:
7198
break
7199
7200
def AltTypes(self):
7201
'''Test Different Altitude Types'''
7202
'''start by disabling GCS failsafe, otherwise we immediately disarm
7203
due to (apparently) not receiving traffic from the GCS for
7204
too long. This is probably a function of --speedup'''
7205
7206
'''this test flies the vehicle somewhere lower than were it started.
7207
It then disarms. It then arms, which should reset home to the
7208
new, lower altitude. This delta should be outside 1m but
7209
within a few metres of the old one.
7210
7211
'''
7212
7213
self.install_terrain_handlers_context()
7214
7215
self.set_parameter("FS_GCS_ENABLE", 0)
7216
self.change_mode('GUIDED')
7217
self.wait_ready_to_arm()
7218
self.arm_vehicle()
7219
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
7220
max_initial_home_alt_m = 500
7221
if m.relative_alt > max_initial_home_alt_m:
7222
raise NotAchievedException("Initial home alt too high (%fm > %fm)" %
7223
(m.relative_alt*1000, max_initial_home_alt_m*1000))
7224
orig_home_offset_mm = m.alt - m.relative_alt
7225
self.user_takeoff(5)
7226
7227
self.progress("Flying to low position")
7228
current_alt = self.mav.location().alt
7229
# 10m delta low_position = mavutil.location(-35.358273, 149.169165, current_alt, 0)
7230
low_position = mavutil.location(-35.36200016, 149.16415599, current_alt, 0)
7231
self.fly_guided_move_to(low_position, timeout=240)
7232
self.change_mode('LAND')
7233
# expecting home to change when disarmed
7234
self.wait_landed_and_disarmed()
7235
# wait a while for home to move (it shouldn't):
7236
self.delay_sim_time(10)
7237
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
7238
new_home_offset_mm = m.alt - m.relative_alt
7239
home_offset_delta_mm = orig_home_offset_mm - new_home_offset_mm
7240
self.progress("new home offset: %f delta=%f" %
7241
(new_home_offset_mm, home_offset_delta_mm))
7242
self.progress("gpi=%s" % str(m))
7243
max_home_offset_delta_mm = 10
7244
if home_offset_delta_mm > max_home_offset_delta_mm:
7245
raise NotAchievedException("Large home offset delta: want<%f got=%f" %
7246
(max_home_offset_delta_mm, home_offset_delta_mm))
7247
self.progress("Ensuring home moves when we arm")
7248
self.change_mode('GUIDED')
7249
self.wait_ready_to_arm()
7250
self.arm_vehicle()
7251
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
7252
post_arming_home_offset_mm = m.alt - m.relative_alt
7253
self.progress("post-arming home offset: %f" % (post_arming_home_offset_mm))
7254
self.progress("gpi=%s" % str(m))
7255
min_post_arming_home_offset_delta_mm = -2500
7256
max_post_arming_home_offset_delta_mm = -4000
7257
delta_between_original_home_alt_offset_and_new_home_alt_offset_mm = post_arming_home_offset_mm - orig_home_offset_mm
7258
self.progress("delta=%f-%f=%f" % (
7259
post_arming_home_offset_mm,
7260
orig_home_offset_mm,
7261
delta_between_original_home_alt_offset_and_new_home_alt_offset_mm))
7262
self.progress("Home moved %fm vertically" % (delta_between_original_home_alt_offset_and_new_home_alt_offset_mm/1000.0))
7263
if delta_between_original_home_alt_offset_and_new_home_alt_offset_mm > min_post_arming_home_offset_delta_mm:
7264
raise NotAchievedException(
7265
"Home did not move vertically on arming: want<=%f got=%f" %
7266
(min_post_arming_home_offset_delta_mm, delta_between_original_home_alt_offset_and_new_home_alt_offset_mm))
7267
if delta_between_original_home_alt_offset_and_new_home_alt_offset_mm < max_post_arming_home_offset_delta_mm:
7268
raise NotAchievedException(
7269
"Home moved too far vertically on arming: want>=%f got=%f" %
7270
(max_post_arming_home_offset_delta_mm, delta_between_original_home_alt_offset_and_new_home_alt_offset_mm))
7271
7272
self.wait_disarmed()
7273
7274
def PrecisionLoiterCompanion(self):
7275
"""Use Companion PrecLand backend precision messages to loiter."""
7276
7277
self.set_parameters({
7278
"PLND_ENABLED": 1,
7279
"PLND_TYPE": 1, # enable companion backend:
7280
"RC7_OPTION": 39, # set up a channel switch to enable precision loiter:
7281
})
7282
self.set_analog_rangefinder_parameters()
7283
self.reboot_sitl()
7284
7285
self.progress("Waiting for location")
7286
self.change_mode('LOITER')
7287
self.wait_ready_to_arm()
7288
7289
# we should be doing precision loiter at this point
7290
start = self.assert_receive_message('LOCAL_POSITION_NED')
7291
7292
self.takeoff(20, mode='ALT_HOLD')
7293
7294
# move away a little
7295
self.set_rc(2, 1550)
7296
self.wait_distance(5, accuracy=1)
7297
self.set_rc(2, 1500)
7298
self.change_mode('LOITER')
7299
7300
# turn precision loiter on:
7301
self.context_collect('STATUSTEXT')
7302
self.set_rc(7, 2000)
7303
7304
# try to drag aircraft to a position 5 metres north-east-east:
7305
self.precision_loiter_to_pos(start.x + 5, start.y + 10, start.z + 10)
7306
self.wait_statustext("PrecLand: Target Found", check_context=True, timeout=10)
7307
self.wait_statustext("PrecLand: Init Complete", check_context=True, timeout=10)
7308
# .... then northwest
7309
self.precision_loiter_to_pos(start.x + 5, start.y - 10, start.z + 10)
7310
7311
self.disarm_vehicle(force=True)
7312
7313
def loiter_requires_position(self):
7314
# ensure we can't switch to LOITER without position
7315
self.progress("Ensure we can't enter LOITER without position")
7316
self.context_push()
7317
self.set_parameters({
7318
"GPS1_TYPE": 2,
7319
"SIM_GPS1_ENABLE": 0,
7320
})
7321
# if there is no GPS at all then we must direct EK3 to not use
7322
# it at all. Otherwise it will never initialise, as it wants
7323
# to calculate the lag and size its delay buffers accordingly.
7324
self.set_parameters({
7325
"EK3_SRC1_POSXY": 0,
7326
"EK3_SRC1_VELZ": 0,
7327
"EK3_SRC1_VELXY": 0,
7328
})
7329
self.reboot_sitl()
7330
self.delay_sim_time(30) # wait for accels/gyros to settle
7331
7332
# check for expected EKF flags
7333
ahrs_ekf_type = self.get_parameter("AHRS_EKF_TYPE")
7334
expected_ekf_flags = (mavutil.mavlink.ESTIMATOR_ATTITUDE |
7335
mavutil.mavlink.ESTIMATOR_VELOCITY_VERT |
7336
mavutil.mavlink.ESTIMATOR_POS_VERT_ABS |
7337
mavutil.mavlink.ESTIMATOR_CONST_POS_MODE)
7338
if ahrs_ekf_type == 2:
7339
expected_ekf_flags = expected_ekf_flags | mavutil.mavlink.ESTIMATOR_PRED_POS_HORIZ_REL
7340
self.wait_ekf_flags(expected_ekf_flags, 0, timeout=120)
7341
7342
# arm in Stabilize and attempt to switch to Loiter
7343
self.change_mode('STABILIZE')
7344
self.arm_vehicle()
7345
self.context_collect('STATUSTEXT')
7346
self.run_cmd_do_set_mode(
7347
"LOITER",
7348
want_result=mavutil.mavlink.MAV_RESULT_FAILED)
7349
self.wait_statustext("requires position", check_context=True)
7350
self.disarm_vehicle()
7351
self.context_pop()
7352
self.reboot_sitl()
7353
7354
def ArmFeatures(self):
7355
'''Arm features'''
7356
self.loiter_requires_position()
7357
7358
super(AutoTestCopter, self).ArmFeatures()
7359
7360
def ParameterChecks(self):
7361
'''Test Arming Parameter Checks'''
7362
self.test_parameter_checks_poscontrol("PSC")
7363
7364
def PosHoldTakeOff(self):
7365
"""ensure vehicle stays put until it is ready to fly"""
7366
self.context_push()
7367
7368
self.set_parameter("PILOT_TKOFF_ALT", 700)
7369
self.change_mode('POSHOLD')
7370
self.set_rc(3, 1000)
7371
self.wait_ready_to_arm()
7372
self.arm_vehicle()
7373
self.delay_sim_time(2)
7374
# check we are still on the ground...
7375
relative_alt = self.get_altitude(relative=True)
7376
if relative_alt > 0.1:
7377
raise NotAchievedException("Took off prematurely")
7378
7379
self.progress("Pushing throttle up")
7380
self.set_rc(3, 1710)
7381
self.delay_sim_time(0.5)
7382
self.progress("Bringing back to hover throttle")
7383
self.set_rc(3, 1500)
7384
7385
# make sure we haven't already reached alt:
7386
relative_alt = self.get_altitude(relative=True)
7387
max_initial_alt = 2.0
7388
if abs(relative_alt) > max_initial_alt:
7389
raise NotAchievedException("Took off too fast (%f > %f" %
7390
(relative_alt, max_initial_alt))
7391
7392
self.progress("Monitoring takeoff-to-alt")
7393
self.wait_altitude(6.9, 8, relative=True, minimum_duration=10)
7394
self.progress("takeoff OK")
7395
7396
self.land_and_disarm()
7397
self.set_rc(8, 1000)
7398
7399
self.context_pop()
7400
7401
def initial_mode(self):
7402
return "STABILIZE"
7403
7404
def initial_mode_switch_mode(self):
7405
return "STABILIZE"
7406
7407
def default_mode(self):
7408
return "STABILIZE"
7409
7410
def rc_defaults(self):
7411
ret = super(AutoTestCopter, self).rc_defaults()
7412
ret[3] = 1000
7413
ret[5] = 1800 # mode switch
7414
return ret
7415
7416
def MANUAL_CONTROL(self):
7417
'''test MANUAL_CONTROL mavlink message'''
7418
self.set_parameter("SYSID_MYGCS", self.mav.source_system)
7419
7420
self.change_mode('STABILIZE')
7421
self.takeoff(10)
7422
7423
tstart = self.get_sim_time_cached()
7424
want_pitch_degrees = -12
7425
while True:
7426
if self.get_sim_time_cached() - tstart > 10:
7427
raise AutoTestTimeoutException("Did not reach pitch")
7428
self.progress("Sending pitch-forward")
7429
self.mav.mav.manual_control_send(
7430
1, # target system
7431
500, # x (pitch)
7432
32767, # y (roll)
7433
32767, # z (thrust)
7434
32767, # r (yaw)
7435
0) # button mask
7436
m = self.mav.recv_match(type='ATTITUDE', blocking=True, timeout=1)
7437
print("m=%s" % str(m))
7438
if m is None:
7439
continue
7440
p = math.degrees(m.pitch)
7441
self.progress("pitch=%f want<=%f" % (p, want_pitch_degrees))
7442
if p <= want_pitch_degrees:
7443
break
7444
self.mav.mav.manual_control_send(
7445
1, # target system
7446
32767, # x (pitch)
7447
32767, # y (roll)
7448
32767, # z (thrust)
7449
32767, # r (yaw)
7450
0) # button mask
7451
self.do_RTL()
7452
7453
def check_avoidance_corners(self):
7454
self.takeoff(10, mode="LOITER")
7455
here = self.mav.location()
7456
self.set_rc(2, 1400)
7457
west_loc = mavutil.location(-35.363007,
7458
149.164911,
7459
here.alt,
7460
0)
7461
self.wait_location(west_loc, accuracy=6)
7462
north_loc = mavutil.location(-35.362908,
7463
149.165051,
7464
here.alt,
7465
0)
7466
self.reach_heading_manual(0)
7467
self.wait_location(north_loc, accuracy=6, timeout=200)
7468
self.reach_heading_manual(90)
7469
east_loc = mavutil.location(-35.363013,
7470
149.165194,
7471
here.alt,
7472
0)
7473
self.wait_location(east_loc, accuracy=6)
7474
self.reach_heading_manual(225)
7475
self.wait_location(west_loc, accuracy=6, timeout=200)
7476
self.set_rc(2, 1500)
7477
self.do_RTL()
7478
7479
def OBSTACLE_DISTANCE_3D_test_angle(self, angle):
7480
now = self.get_sim_time_cached()
7481
7482
distance = 15
7483
right = distance * math.sin(math.radians(angle))
7484
front = distance * math.cos(math.radians(angle))
7485
down = 0
7486
7487
expected_distance_cm = distance * 100
7488
# expected orientation
7489
expected_orientation = int((angle+22.5)/45) % 8
7490
self.progress("Angle %f expected orient %u" %
7491
(angle, expected_orientation))
7492
7493
tstart = self.get_sim_time()
7494
last_send = 0
7495
m = None
7496
while True:
7497
now = self.get_sim_time_cached()
7498
if now - tstart > 100:
7499
raise NotAchievedException("Did not get correct angle back (last-message=%s)" % str(m))
7500
7501
if now - last_send > 0.1:
7502
self.progress("ang=%f sending front=%f right=%f" %
7503
(angle, front, right))
7504
self.mav.mav.obstacle_distance_3d_send(
7505
int(now*1000), # time_boot_ms
7506
mavutil.mavlink.MAV_DISTANCE_SENSOR_LASER,
7507
mavutil.mavlink.MAV_FRAME_BODY_FRD,
7508
65535,
7509
front, # x (m)
7510
right, # y (m)
7511
down, # z (m)
7512
0, # min_distance (m)
7513
20 # max_distance (m)
7514
)
7515
last_send = now
7516
m = self.mav.recv_match(type="DISTANCE_SENSOR",
7517
blocking=True,
7518
timeout=1)
7519
if m is None:
7520
continue
7521
# self.progress("Got (%s)" % str(m))
7522
if m.orientation != expected_orientation:
7523
# self.progress("Wrong orientation (want=%u got=%u)" %
7524
# (expected_orientation, m.orientation))
7525
continue
7526
if abs(m.current_distance - expected_distance_cm) > 1:
7527
# self.progress("Wrong distance (want=%f got=%f)" %
7528
# (expected_distance_cm, m.current_distance))
7529
continue
7530
self.progress("distance-at-angle good")
7531
break
7532
7533
def OBSTACLE_DISTANCE_3D(self):
7534
'''Check round-trip behaviour of distance sensors'''
7535
self.context_push()
7536
self.set_parameters({
7537
"SERIAL5_PROTOCOL": 1,
7538
"PRX1_TYPE": 2,
7539
"SIM_SPEEDUP": 8, # much GCS interaction
7540
})
7541
self.reboot_sitl()
7542
# need yaw estimate to stabilise:
7543
self.wait_ekf_happy(require_absolute=True)
7544
7545
for angle in range(0, 360):
7546
self.OBSTACLE_DISTANCE_3D_test_angle(angle)
7547
7548
self.context_pop()
7549
self.reboot_sitl()
7550
7551
def AC_Avoidance_Proximity(self):
7552
'''Test proximity avoidance slide behaviour'''
7553
7554
self.context_push()
7555
7556
self.load_fence("copter-avoidance-fence.txt")
7557
self.set_parameters({
7558
"FENCE_ENABLE": 1,
7559
"PRX1_TYPE": 10,
7560
"PRX_LOG_RAW": 1,
7561
"RC10_OPTION": 40, # proximity-enable
7562
})
7563
self.reboot_sitl()
7564
self.progress("Enabling proximity")
7565
self.set_rc(10, 2000)
7566
self.check_avoidance_corners()
7567
7568
self.assert_current_onboard_log_contains_message("PRX")
7569
self.assert_current_onboard_log_contains_message("PRXR")
7570
7571
self.disarm_vehicle(force=True)
7572
7573
self.context_pop()
7574
self.reboot_sitl()
7575
7576
def ProximitySensors(self):
7577
'''ensure proximity sensors return appropriate data'''
7578
7579
self.set_parameters({
7580
"SERIAL5_PROTOCOL": 11,
7581
"OA_DB_OUTPUT": 3,
7582
"OA_TYPE": 2,
7583
})
7584
sensors = [ # tuples of name, prx_type
7585
('ld06', 16, {
7586
mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 273,
7587
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 256,
7588
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 1130,
7589
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 696,
7590
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 625,
7591
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 967,
7592
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 760,
7593
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 771,
7594
}),
7595
('sf45b', 8, {
7596
mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 270,
7597
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 258,
7598
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 1146,
7599
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 632,
7600
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 629,
7601
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 972,
7602
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 774,
7603
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 774,
7604
}),
7605
('rplidara2', 5, {
7606
mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 277,
7607
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 256,
7608
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 1130,
7609
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 1288,
7610
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 626,
7611
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 970,
7612
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 762,
7613
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 790,
7614
}),
7615
('terarangertower', 3, {
7616
mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 450,
7617
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 282,
7618
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 450,
7619
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 450,
7620
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 450,
7621
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 450,
7622
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 450,
7623
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 450,
7624
}),
7625
]
7626
7627
# the following is a "magic" location SITL understands which
7628
# has some posts near it:
7629
home_string = "%s,%s,%s,%s" % (51.8752066, 14.6487840, 54.15, 0)
7630
for (name, prx_type, expected_distances) in sensors:
7631
self.start_subtest("Testing %s" % name)
7632
self.set_parameter("PRX1_TYPE", prx_type)
7633
self.customise_SITL_commandline([
7634
"--serial5=sim:%s:" % name,
7635
"--home", home_string,
7636
])
7637
self.wait_ready_to_arm()
7638
expected_distances_copy = copy.copy(expected_distances)
7639
tstart = self.get_sim_time()
7640
failed = False
7641
wants = []
7642
gots = []
7643
epsilon = 20
7644
while True:
7645
if self.get_sim_time_cached() - tstart > 30:
7646
raise AutoTestTimeoutException("Failed to get distances")
7647
if len(expected_distances_copy.keys()) == 0:
7648
break
7649
m = self.assert_receive_message("DISTANCE_SENSOR")
7650
if m.orientation not in expected_distances_copy:
7651
continue
7652
got = m.current_distance
7653
want = expected_distances_copy[m.orientation]
7654
wants.append(want)
7655
gots.append(got)
7656
if abs(want - got) > epsilon:
7657
failed = True
7658
del expected_distances_copy[m.orientation]
7659
if failed:
7660
raise NotAchievedException(
7661
"Distance too great (%s) (want=%s != got=%s)" %
7662
(name, wants, gots))
7663
7664
def AC_Avoidance_Proximity_AVOID_ALT_MIN(self):
7665
'''Test proximity avoidance with AVOID_ALT_MIN'''
7666
self.context_push()
7667
ex = None
7668
try:
7669
self.set_parameters({
7670
"PRX1_TYPE": 2,
7671
"AVOID_ALT_MIN": 10,
7672
})
7673
self.set_analog_rangefinder_parameters()
7674
self.reboot_sitl()
7675
7676
self.change_mode('LOITER')
7677
self.wait_ekf_happy()
7678
7679
tstart = self.get_sim_time()
7680
while True:
7681
if self.armed():
7682
break
7683
if self.get_sim_time_cached() - tstart > 60:
7684
raise AutoTestTimeoutException("Did not arm")
7685
self.mav.mav.distance_sensor_send(
7686
0, # time_boot_ms
7687
10, # min_distance cm
7688
500, # max_distance cm
7689
400, # current_distance cm
7690
mavutil.mavlink.MAV_DISTANCE_SENSOR_LASER, # type
7691
26, # id
7692
mavutil.mavlink.MAV_SENSOR_ROTATION_NONE, # orientation
7693
255 # covariance
7694
)
7695
self.send_mavlink_arm_command()
7696
7697
self.takeoff(15, mode='LOITER')
7698
self.progress("Poking vehicle; should avoid")
7699
7700
def shove(a, b):
7701
self.mav.mav.distance_sensor_send(
7702
0, # time_boot_ms
7703
10, # min_distance cm
7704
500, # max_distance cm
7705
20, # current_distance cm
7706
mavutil.mavlink.MAV_DISTANCE_SENSOR_LASER, # type
7707
21, # id
7708
mavutil.mavlink.MAV_SENSOR_ROTATION_NONE, # orientation
7709
255 # covariance
7710
)
7711
self.wait_speed_vector_bf(
7712
Vector3(-0.4, 0.0, 0.0),
7713
timeout=10,
7714
called_function=shove,
7715
)
7716
7717
self.change_alt(5)
7718
7719
tstart = self.get_sim_time()
7720
while True:
7721
if self.get_sim_time_cached() - tstart > 10:
7722
break
7723
vel = self.get_body_frame_velocity()
7724
if vel.length() > 0.5:
7725
raise NotAchievedException("Moved too much (%s)" %
7726
(str(vel),))
7727
shove(None, None)
7728
7729
except Exception as e:
7730
self.progress("Caught exception: %s" %
7731
self.get_exception_stacktrace(e))
7732
ex = e
7733
self.disarm_vehicle(force=True)
7734
self.context_pop()
7735
self.reboot_sitl()
7736
if ex is not None:
7737
raise ex
7738
7739
def AC_Avoidance_Fence(self):
7740
'''Test fence avoidance slide behaviour'''
7741
self.load_fence("copter-avoidance-fence.txt")
7742
self.set_parameter("FENCE_ENABLE", 1)
7743
self.check_avoidance_corners()
7744
7745
def AvoidanceAltFence(self):
7746
'''Test fence avoidance at minimum and maximum altitude'''
7747
ex = None
7748
try:
7749
self.set_parameters({
7750
"FENCE_ENABLE": 1,
7751
"FENCE_TYPE": 9, # min and max alt fence
7752
"FENCE_ALT_MIN": 10,
7753
"FENCE_ALT_MAX": 30,
7754
})
7755
7756
self.change_mode('LOITER')
7757
self.wait_ekf_happy()
7758
7759
tstart = self.get_sim_time()
7760
self.takeoff(15, mode='LOITER')
7761
self.progress("Increasing throttle, vehicle should stay below 30m")
7762
self.set_rc(3, 1920)
7763
7764
tstart = self.get_sim_time()
7765
while True:
7766
if self.get_sim_time_cached() - tstart > 20:
7767
break
7768
alt = self.get_altitude(relative=True)
7769
self.progress("Altitude %s" % alt)
7770
if alt > 30:
7771
raise NotAchievedException("Breached maximum altitude (%s)" % (str(alt),))
7772
7773
self.progress("Decreasing, vehicle should stay above 10m")
7774
self.set_rc(3, 1080)
7775
tstart = self.get_sim_time()
7776
while True:
7777
if self.get_sim_time_cached() - tstart > 20:
7778
break
7779
alt = self.get_altitude(relative=True)
7780
self.progress("Altitude %s" % alt)
7781
if alt < 10:
7782
raise NotAchievedException("Breached minimum altitude (%s)" % (str(alt),))
7783
7784
except Exception as e:
7785
self.progress("Caught exception: %s" %
7786
self.get_exception_stacktrace(e))
7787
ex = e
7788
self.land_and_disarm()
7789
self.disarm_vehicle(force=True)
7790
if ex is not None:
7791
raise ex
7792
7793
def ModeFollow(self):
7794
'''Fly follow mode'''
7795
foll_ofs_x = 30 # metres
7796
self.set_parameters({
7797
"FOLL_ENABLE": 1,
7798
"FOLL_SYSID": self.mav.source_system,
7799
"FOLL_OFS_X": -foll_ofs_x,
7800
"FOLL_OFS_TYPE": 1, # relative to other vehicle heading
7801
})
7802
self.takeoff(10, mode="LOITER")
7803
self.context_push()
7804
self.set_parameter("SIM_SPEEDUP", 1)
7805
self.change_mode("FOLLOW")
7806
new_loc = self.mav.location()
7807
new_loc_offset_n = 20
7808
new_loc_offset_e = 30
7809
self.location_offset_ne(new_loc, new_loc_offset_n, new_loc_offset_e)
7810
self.progress("new_loc: %s" % str(new_loc))
7811
heading = 0
7812
if self.mavproxy is not None:
7813
self.mavproxy.send("map icon %f %f greenplane %f\n" %
7814
(new_loc.lat, new_loc.lng, heading))
7815
7816
expected_loc = copy.copy(new_loc)
7817
self.location_offset_ne(expected_loc, -foll_ofs_x, 0)
7818
if self.mavproxy is not None:
7819
self.mavproxy.send("map icon %f %f hoop\n" %
7820
(expected_loc.lat, expected_loc.lng))
7821
self.progress("expected_loc: %s" % str(expected_loc))
7822
7823
origin = self.poll_message('GPS_GLOBAL_ORIGIN')
7824
7825
last_sent = 0
7826
tstart = self.get_sim_time()
7827
while True:
7828
now = self.get_sim_time_cached()
7829
if now - tstart > 60:
7830
raise NotAchievedException("Did not FOLLOW")
7831
if now - last_sent > 0.5:
7832
gpi = self.mav.mav.global_position_int_encode(
7833
int(now * 1000), # time_boot_ms
7834
int(new_loc.lat * 1e7),
7835
int(new_loc.lng * 1e7),
7836
int(new_loc.alt * 1000), # alt in mm
7837
int(new_loc.alt * 1000 - origin.altitude), # relative alt - urp.
7838
vx=0,
7839
vy=0,
7840
vz=0,
7841
hdg=heading
7842
)
7843
gpi.pack(self.mav.mav)
7844
self.mav.mav.send(gpi)
7845
self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
7846
pos = self.mav.location()
7847
delta = self.get_distance(expected_loc, pos)
7848
max_delta = 3
7849
self.progress("position delta=%f (want <%f)" % (delta, max_delta))
7850
if delta < max_delta:
7851
break
7852
self.context_pop()
7853
self.do_RTL()
7854
7855
def get_global_position_int(self, timeout=30):
7856
tstart = self.get_sim_time()
7857
while True:
7858
if self.get_sim_time_cached() - tstart > timeout:
7859
raise NotAchievedException("Did not get good global_position_int")
7860
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)
7861
self.progress("GPI: %s" % str(m))
7862
if m is None:
7863
continue
7864
if m.lat != 0 or m.lon != 0:
7865
return m
7866
7867
def BeaconPosition(self):
7868
'''Fly Beacon Position'''
7869
self.reboot_sitl()
7870
7871
self.wait_ready_to_arm(require_absolute=True)
7872
7873
old_pos = self.get_global_position_int()
7874
print("old_pos=%s" % str(old_pos))
7875
7876
self.set_parameters({
7877
"BCN_TYPE": 10,
7878
"BCN_LATITUDE": SITL_START_LOCATION.lat,
7879
"BCN_LONGITUDE": SITL_START_LOCATION.lng,
7880
"BCN_ALT": SITL_START_LOCATION.alt,
7881
"BCN_ORIENT_YAW": 0,
7882
"AVOID_ENABLE": 4,
7883
"GPS1_TYPE": 0,
7884
"EK3_ENABLE": 1,
7885
"EK3_SRC1_POSXY": 4, # Beacon
7886
"EK3_SRC1_POSZ": 1, # Baro
7887
"EK3_SRC1_VELXY": 0, # None
7888
"EK3_SRC1_VELZ": 0, # None
7889
"EK2_ENABLE": 0,
7890
"AHRS_EKF_TYPE": 3,
7891
})
7892
self.reboot_sitl()
7893
7894
# turn off GPS arming checks. This may be considered a
7895
# bug that we need to do this.
7896
old_arming_check = int(self.get_parameter("ARMING_CHECK"))
7897
if old_arming_check == 1:
7898
old_arming_check = 1 ^ 25 - 1
7899
new_arming_check = int(old_arming_check) & ~(1 << 3)
7900
self.set_parameter("ARMING_CHECK", new_arming_check)
7901
7902
self.reboot_sitl()
7903
7904
# require_absolute=True infers a GPS is present
7905
self.wait_ready_to_arm(require_absolute=False)
7906
7907
tstart = self.get_sim_time()
7908
timeout = 20
7909
while True:
7910
if self.get_sim_time_cached() - tstart > timeout:
7911
raise NotAchievedException("Did not get new position like old position")
7912
self.progress("Fetching location")
7913
new_pos = self.get_global_position_int()
7914
pos_delta = self.get_distance_int(old_pos, new_pos)
7915
max_delta = 1
7916
self.progress("delta=%u want <= %u" % (pos_delta, max_delta))
7917
if pos_delta <= max_delta:
7918
break
7919
7920
self.progress("Moving to ensure location is tracked")
7921
self.takeoff(10, mode="STABILIZE")
7922
self.change_mode("CIRCLE")
7923
7924
self.context_push()
7925
validator = vehicle_test_suite.TestSuite.ValidateGlobalPositionIntAgainstSimState(self, max_allowed_divergence=10)
7926
self.install_message_hook_context(validator)
7927
7928
self.delay_sim_time(20)
7929
self.progress("Tracked location just fine")
7930
self.context_pop()
7931
7932
self.change_mode("LOITER")
7933
self.wait_groundspeed(0, 0.3, timeout=120)
7934
self.land_and_disarm()
7935
7936
self.assert_current_onboard_log_contains_message("BCN")
7937
7938
self.disarm_vehicle(force=True)
7939
7940
def AC_Avoidance_Beacon(self):
7941
'''Test beacon avoidance slide behaviour'''
7942
self.context_push()
7943
ex = None
7944
try:
7945
self.set_parameters({
7946
"BCN_TYPE": 10,
7947
"BCN_LATITUDE": int(SITL_START_LOCATION.lat),
7948
"BCN_LONGITUDE": int(SITL_START_LOCATION.lng),
7949
"BCN_ORIENT_YAW": 45,
7950
"AVOID_ENABLE": 4,
7951
})
7952
self.reboot_sitl()
7953
7954
self.takeoff(10, mode="LOITER")
7955
self.set_rc(2, 1400)
7956
here = self.mav.location()
7957
west_loc = mavutil.location(-35.362919, 149.165055, here.alt, 0)
7958
self.wait_location(west_loc, accuracy=1)
7959
self.reach_heading_manual(0)
7960
north_loc = mavutil.location(-35.362881, 149.165103, here.alt, 0)
7961
self.wait_location(north_loc, accuracy=1)
7962
self.set_rc(2, 1500)
7963
self.set_rc(1, 1600)
7964
east_loc = mavutil.location(-35.362986, 149.165227, here.alt, 0)
7965
self.wait_location(east_loc, accuracy=1)
7966
self.set_rc(1, 1500)
7967
self.set_rc(2, 1600)
7968
south_loc = mavutil.location(-35.363025, 149.165182, here.alt, 0)
7969
self.wait_location(south_loc, accuracy=1)
7970
self.set_rc(2, 1500)
7971
self.do_RTL()
7972
7973
except Exception as e:
7974
self.print_exception_caught(e)
7975
ex = e
7976
self.context_pop()
7977
self.clear_fence()
7978
self.disarm_vehicle(force=True)
7979
self.reboot_sitl()
7980
if ex is not None:
7981
raise ex
7982
7983
def BaroWindCorrection(self):
7984
'''Test wind estimation and baro position error compensation'''
7985
self.context_push()
7986
ex = None
7987
try:
7988
self.customise_SITL_commandline(
7989
[],
7990
defaults_filepath=self.model_defaults_filepath('Callisto'),
7991
model="octa-quad:@ROMFS/models/Callisto.json",
7992
wipe=True,
7993
)
7994
wind_spd_truth = 8.0
7995
wind_dir_truth = 90.0
7996
self.set_parameters({
7997
"EK3_ENABLE": 1,
7998
"EK2_ENABLE": 0,
7999
"AHRS_EKF_TYPE": 3,
8000
"BARO1_WCF_ENABLE": 1.000000,
8001
})
8002
self.reboot_sitl()
8003
self.set_parameters({
8004
"BARO1_WCF_FWD": -0.300000,
8005
"BARO1_WCF_BCK": -0.300000,
8006
"BARO1_WCF_RGT": 0.300000,
8007
"BARO1_WCF_LFT": 0.300000,
8008
"BARO1_WCF_UP": 0.300000,
8009
"BARO1_WCF_DN": 0.300000,
8010
"SIM_BARO_WCF_FWD": -0.300000,
8011
"SIM_BARO_WCF_BAK": -0.300000,
8012
"SIM_BARO_WCF_RGT": 0.300000,
8013
"SIM_BARO_WCF_LFT": 0.300000,
8014
"SIM_BARO_WCF_UP": 0.300000,
8015
"SIM_BARO_WCF_DN": 0.300000,
8016
"SIM_WIND_DIR": wind_dir_truth,
8017
"SIM_WIND_SPD": wind_spd_truth,
8018
"SIM_WIND_T": 1.000000,
8019
})
8020
self.reboot_sitl()
8021
8022
# require_absolute=True infers a GPS is present
8023
self.wait_ready_to_arm(require_absolute=False)
8024
8025
self.progress("Climb to 20m in LOITER and yaw spin for 30 seconds")
8026
self.takeoff(10, mode="LOITER")
8027
self.set_rc(4, 1400)
8028
self.delay_sim_time(30)
8029
8030
# check wind esitmates
8031
m = self.mav.recv_match(type='WIND', blocking=True)
8032
speed_error = abs(m.speed - wind_spd_truth)
8033
angle_error = abs(m.direction - wind_dir_truth)
8034
if (speed_error > 1.0):
8035
raise NotAchievedException("Wind speed incorrect - want %f +-1 got %f m/s" % (wind_spd_truth, m.speed))
8036
if (angle_error > 15.0):
8037
raise NotAchievedException(
8038
"Wind direction incorrect - want %f +-15 got %f deg" %
8039
(wind_dir_truth, m.direction))
8040
self.progress("Wind estimate is good, now check height variation for 30 seconds")
8041
8042
# check height stability over another 30 seconds
8043
z_min = 1E6
8044
z_max = -1E6
8045
tstart = self.get_sim_time()
8046
while (self.get_sim_time() < tstart + 30):
8047
m = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True)
8048
if (m.z > z_max):
8049
z_max = m.z
8050
if (m.z < z_min):
8051
z_min = m.z
8052
if (z_max-z_min > 0.5):
8053
raise NotAchievedException("Height variation is excessive")
8054
self.progress("Height variation is good")
8055
8056
self.set_rc(4, 1500)
8057
self.land_and_disarm()
8058
8059
except Exception as e:
8060
self.print_exception_caught(e)
8061
ex = e
8062
self.disarm_vehicle(force=True)
8063
self.reboot_sitl()
8064
self.context_pop()
8065
self.reboot_sitl()
8066
if ex is not None:
8067
raise ex
8068
8069
def wait_generator_speed_and_state(self, rpm_min, rpm_max, want_state, timeout=240):
8070
tstart = self.get_sim_time()
8071
while True:
8072
if self.get_sim_time_cached() - tstart > timeout:
8073
raise NotAchievedException("Did not move to state/speed")
8074
8075
m = self.assert_receive_message("GENERATOR_STATUS", timeout=10)
8076
8077
if m.generator_speed < rpm_min:
8078
self.progress("Too slow (%u<%u)" % (m.generator_speed, rpm_min))
8079
continue
8080
if m.generator_speed > rpm_max:
8081
self.progress("Too fast (%u>%u)" % (m.generator_speed, rpm_max))
8082
continue
8083
if m.status != want_state:
8084
self.progress("Wrong state (got=%u want=%u)" % (m.status, want_state))
8085
break
8086
self.progress("Got generator speed and state")
8087
8088
def RichenPower(self):
8089
'''Test RichenPower generator'''
8090
self.set_parameters({
8091
"SERIAL5_PROTOCOL": 30,
8092
"SIM_RICH_ENABLE": 1,
8093
"SERVO8_FUNCTION": 42,
8094
"SIM_RICH_CTRL": 8,
8095
"RC9_OPTION": 85,
8096
"LOG_DISARMED": 1,
8097
"BATT2_MONITOR": 17,
8098
"GEN_TYPE": 3,
8099
})
8100
self.reboot_sitl()
8101
self.set_rc(9, 1000) # remember this is a switch position - stop
8102
self.customise_SITL_commandline(["--serial5=sim:richenpower"])
8103
self.wait_statustext("requested state is not RUN", timeout=60)
8104
8105
self.set_message_rate_hz("GENERATOR_STATUS", 10)
8106
8107
self.wait_generator_speed_and_state(0, 0, mavutil.mavlink.MAV_GENERATOR_STATUS_FLAG_OFF)
8108
8109
self.context_collect('STATUSTEXT')
8110
self.set_rc(9, 2000) # remember this is a switch position - run
8111
self.wait_statustext("Generator HIGH", check_context=True)
8112
self.set_rc(9, 1000) # remember this is a switch position - stop
8113
self.wait_statustext("requested state is not RUN", timeout=200)
8114
8115
self.set_rc(9, 1500) # remember this is a switch position - idle
8116
self.wait_generator_speed_and_state(3000, 8000, mavutil.mavlink.MAV_GENERATOR_STATUS_FLAG_IDLE)
8117
8118
self.set_rc(9, 2000) # remember this is a switch position - run
8119
# self.wait_generator_speed_and_state(3000, 30000, mavutil.mavlink.MAV_GENERATOR_STATUS_FLAG_WARMING_UP)
8120
8121
self.wait_generator_speed_and_state(8000, 30000, mavutil.mavlink.MAV_GENERATOR_STATUS_FLAG_GENERATING)
8122
8123
bs = self.mav.recv_match(
8124
type="BATTERY_STATUS",
8125
condition="BATTERY_STATUS.id==1", # id is zero-indexed
8126
timeout=1,
8127
blocking=True
8128
)
8129
if bs is None:
8130
raise NotAchievedException("Did not receive BATTERY_STATUS")
8131
self.progress("Received battery status: %s" % str(bs))
8132
want_bs_volt = 50000
8133
if bs.voltages[0] != want_bs_volt:
8134
raise NotAchievedException("Battery voltage not as expected (want=%f) got=(%f)" % (want_bs_volt, bs.voltages[0],))
8135
8136
self.progress("Moving *back* to idle")
8137
self.set_rc(9, 1500) # remember this is a switch position - idle
8138
self.wait_generator_speed_and_state(3000, 10000, mavutil.mavlink.MAV_GENERATOR_STATUS_FLAG_IDLE)
8139
8140
self.progress("Moving *back* to run")
8141
self.set_rc(9, 2000) # remember this is a switch position - run
8142
self.wait_generator_speed_and_state(8000, 30000, mavutil.mavlink.MAV_GENERATOR_STATUS_FLAG_GENERATING)
8143
8144
self.set_message_rate_hz("GENERATOR_STATUS", -1)
8145
self.set_parameter("LOG_DISARMED", 0)
8146
if not self.current_onboard_log_contains_message("GEN"):
8147
raise NotAchievedException("Did not find expected GEN message")
8148
8149
def IE24(self):
8150
'''Test IntelligentEnergy 2.4kWh generator with V1 and V2 telemetry protocols'''
8151
protocol_ver = (1, 2)
8152
for ver in protocol_ver:
8153
self.run_IE24(ver)
8154
8155
def run_IE24(self, proto_ver):
8156
'''Test IntelligentEnergy 2.4kWh generator'''
8157
elec_battery_instance = 2
8158
fuel_battery_instance = 1
8159
self.set_parameters({
8160
"SERIAL5_PROTOCOL": 30,
8161
"SERIAL5_BAUD": 115200,
8162
"GEN_TYPE": 2,
8163
"BATT%u_MONITOR" % (fuel_battery_instance + 1): 18, # fuel-based generator
8164
"BATT%u_MONITOR" % (elec_battery_instance + 1): 17,
8165
"SIM_IE24_ENABLE": proto_ver,
8166
"LOG_DISARMED": 1,
8167
})
8168
8169
self.customise_SITL_commandline(["--serial5=sim:ie24"])
8170
8171
self.start_subtest("Protocol %i: ensure that BATTERY_STATUS for electrical generator message looks right" % proto_ver)
8172
self.start_subsubtest("Protocol %i: Checking original voltage (electrical)" % proto_ver)
8173
# ArduPilot spits out essentially uninitialised battery
8174
# messages until we read things fromthe battery:
8175
self.delay_sim_time(30)
8176
original_elec_m = self.wait_message_field_values('BATTERY_STATUS', {
8177
"charge_state": mavutil.mavlink.MAV_BATTERY_CHARGE_STATE_OK
8178
}, instance=elec_battery_instance)
8179
original_fuel_m = self.wait_message_field_values('BATTERY_STATUS', {
8180
"charge_state": mavutil.mavlink.MAV_BATTERY_CHARGE_STATE_OK
8181
}, instance=fuel_battery_instance)
8182
8183
if original_elec_m.battery_remaining < 90:
8184
raise NotAchievedException("Bad original percentage")
8185
self.start_subsubtest("Ensure percentage is counting down")
8186
self.wait_message_field_values('BATTERY_STATUS', {
8187
"battery_remaining": original_elec_m.battery_remaining - 1,
8188
}, instance=elec_battery_instance)
8189
8190
self.start_subtest("Protocol %i: ensure that BATTERY_STATUS for fuel generator message looks right" % proto_ver)
8191
self.start_subsubtest("Protocol %i: Checking original voltage (fuel)" % proto_ver)
8192
# ArduPilot spits out essentially uninitialised battery
8193
# messages until we read things fromthe battery:
8194
if original_fuel_m.battery_remaining <= 90:
8195
raise NotAchievedException("Bad original percentage (want=>%f got %f" % (90, original_fuel_m.battery_remaining))
8196
self.start_subsubtest("Protocol %i: Ensure percentage is counting down" % proto_ver)
8197
self.wait_message_field_values('BATTERY_STATUS', {
8198
"battery_remaining": original_fuel_m.battery_remaining - 1,
8199
}, instance=fuel_battery_instance)
8200
8201
self.wait_ready_to_arm()
8202
self.arm_vehicle()
8203
self.disarm_vehicle()
8204
8205
# Test for pre-arm check fail when state is not running
8206
self.start_subtest("Protocol %i: Without takeoff generator error should cause failsafe and disarm" % proto_ver)
8207
self.set_parameter("SIM_IE24_STATE", 8)
8208
self.wait_statustext("Status not running", timeout=40)
8209
self.try_arm(result=False,
8210
expect_msg="Status not running")
8211
self.set_parameter("SIM_IE24_STATE", 2) # Explicitly set state to running
8212
8213
# Test that error code does result in failsafe
8214
self.start_subtest("Protocol %i: Without taken off generator error should cause failsafe and disarm" % proto_ver)
8215
self.change_mode("STABILIZE")
8216
self.set_parameter("DISARM_DELAY", 0)
8217
self.arm_vehicle()
8218
self.set_parameter("SIM_IE24_ERROR", 30)
8219
self.disarm_wait(timeout=1)
8220
self.set_parameter("SIM_IE24_ERROR", 0)
8221
self.set_parameter("DISARM_DELAY", 10)
8222
8223
def AuxSwitchOptions(self):
8224
'''Test random aux mode options'''
8225
self.set_parameter("RC7_OPTION", 58) # clear waypoints
8226
self.load_mission("copter_loiter_to_alt.txt")
8227
self.set_rc(7, 1000)
8228
self.assert_mission_count(5)
8229
self.progress("Clear mission")
8230
self.set_rc(7, 2000)
8231
self.delay_sim_time(1) # allow switch to debounce
8232
self.assert_mission_count(0)
8233
self.set_rc(7, 1000)
8234
self.set_parameter("RC7_OPTION", 24) # reset mission
8235
self.delay_sim_time(2)
8236
self.load_mission("copter_loiter_to_alt.txt")
8237
set_wp = 4
8238
self.set_current_waypoint(set_wp)
8239
self.wait_current_waypoint(set_wp, timeout=10)
8240
self.progress("Reset mission")
8241
self.set_rc(7, 2000)
8242
self.delay_sim_time(1)
8243
self.wait_current_waypoint(0, timeout=10)
8244
self.set_rc(7, 1000)
8245
8246
def AuxFunctionsInMission(self):
8247
'''Test use of auxilliary functions in missions'''
8248
self.load_mission("aux_functions.txt")
8249
self.change_mode('LOITER')
8250
self.wait_ready_to_arm()
8251
self.arm_vehicle()
8252
self.change_mode('AUTO')
8253
self.set_rc(3, 1500)
8254
self.wait_mode('ALT_HOLD')
8255
self.change_mode('AUTO')
8256
self.wait_rtl_complete()
8257
8258
def MAV_CMD_AIRFRAME_CONFIGURATION(self):
8259
'''deploy/retract landing gear using mavlink command'''
8260
self.context_push()
8261
self.set_parameters({
8262
"LGR_ENABLE": 1,
8263
"SERVO10_FUNCTION": 29,
8264
"SERVO10_MIN": 1001,
8265
"SERVO10_MAX": 1999,
8266
})
8267
self.reboot_sitl()
8268
8269
# starts loose:
8270
self.wait_servo_channel_value(10, 0)
8271
8272
# 0 is down:
8273
self.start_subtest("Put gear down")
8274
self.run_cmd(mavutil.mavlink.MAV_CMD_AIRFRAME_CONFIGURATION, p2=0)
8275
self.wait_servo_channel_value(10, 1999)
8276
8277
# 1 is up:
8278
self.start_subtest("Put gear up")
8279
self.run_cmd_int(mavutil.mavlink.MAV_CMD_AIRFRAME_CONFIGURATION, p2=1)
8280
self.wait_servo_channel_value(10, 1001)
8281
8282
# 0 is down:
8283
self.start_subtest("Put gear down")
8284
self.run_cmd(mavutil.mavlink.MAV_CMD_AIRFRAME_CONFIGURATION, p2=0)
8285
self.wait_servo_channel_value(10, 1999)
8286
8287
self.context_pop()
8288
self.reboot_sitl()
8289
8290
def WatchAlts(self):
8291
'''Ensure we can monitor different altitudes'''
8292
self.takeoff(30, mode='GUIDED')
8293
self.delay_sim_time(5, reason='let altitude settle')
8294
8295
self.progress("Testing absolute altitudes")
8296
absolute_alt = self.get_altitude(altitude_source='SIM_STATE.alt')
8297
self.progress("absolute_alt=%f" % absolute_alt)
8298
epsilon = 4 # SIM_STATE and vehicle state can be off by a bit...
8299
for source in ['GLOBAL_POSITION_INT.alt', 'SIM_STATE.alt', 'GPS_RAW_INT.alt']:
8300
self.watch_altitude_maintained(
8301
absolute_alt-epsilon,
8302
absolute_alt+epsilon,
8303
altitude_source=source
8304
)
8305
8306
self.progress("Testing absolute altitudes")
8307
relative_alt = self.get_altitude(relative=True)
8308
for source in ['GLOBAL_POSITION_INT.relative_alt']:
8309
self.watch_altitude_maintained(
8310
relative_alt-epsilon,
8311
relative_alt+epsilon,
8312
altitude_source=source
8313
)
8314
8315
self.do_RTL()
8316
8317
def TestTetherStuck(self):
8318
"""Test tethered vehicle stuck because of tether"""
8319
# Enable tether simulation
8320
self.set_parameters({
8321
"SIM_TETH_ENABLE": 1,
8322
})
8323
self.delay_sim_time(2)
8324
self.reboot_sitl()
8325
8326
# Set tether line length
8327
self.set_parameters({
8328
"SIM_TETH_LINELEN": 10,
8329
})
8330
self.delay_sim_time(2)
8331
8332
# Prepare and take off
8333
self.wait_ready_to_arm()
8334
self.arm_vehicle()
8335
self.takeoff(10, mode='LOITER')
8336
8337
# Simulate vehicle getting stuck by increasing RC throttle
8338
self.set_rc(3, 1900)
8339
self.delay_sim_time(5, reason='let tether get stuck')
8340
8341
# Monitor behavior for 10 seconds
8342
tstart = self.get_sim_time()
8343
initial_alt = self.get_altitude()
8344
stuck = True # Assume it's stuck unless proven otherwise
8345
8346
while self.get_sim_time() - tstart < 10:
8347
# Fetch current altitude
8348
current_alt = self.get_altitude()
8349
self.progress(f"current_alt={current_alt}")
8350
8351
# Fetch and log battery status
8352
battery_status = self.mav.recv_match(type='BATTERY_STATUS', blocking=True, timeout=1)
8353
if battery_status:
8354
self.progress(f"Battery: {battery_status}")
8355
8356
# Check if the vehicle is stuck.
8357
# We assume the vehicle is stuck if the current is high and the altitude is not changing
8358
if battery_status and (battery_status.current_battery < 6500 or abs(current_alt - initial_alt) > 2):
8359
stuck = False # Vehicle moved or current is abnormal
8360
break
8361
8362
if not stuck:
8363
raise NotAchievedException("Vehicle did not get stuck as expected")
8364
8365
# Land and disarm the vehicle to ensure we can go down
8366
self.land_and_disarm()
8367
8368
def fly_rangefinder_drivers_fly(self, rangefinders):
8369
'''ensure rangefinder gives height-above-ground'''
8370
self.change_mode('GUIDED')
8371
self.wait_ready_to_arm()
8372
self.arm_vehicle()
8373
expected_alt = 5
8374
self.user_takeoff(alt_min=expected_alt)
8375
rf = self.mav.recv_match(type="RANGEFINDER", timeout=1, blocking=True)
8376
if rf is None:
8377
raise NotAchievedException("Did not receive rangefinder message")
8378
gpi = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)
8379
if gpi is None:
8380
raise NotAchievedException("Did not receive GLOBAL_POSITION_INT message")
8381
if abs(rf.distance - gpi.relative_alt/1000.0) > 1:
8382
raise NotAchievedException(
8383
"rangefinder alt (%s) disagrees with global-position-int.relative_alt (%s)" %
8384
(rf.distance, gpi.relative_alt/1000.0)
8385
)
8386
8387
for i in range(0, len(rangefinders)):
8388
name = rangefinders[i]
8389
self.progress("i=%u (%s)" % (i, name))
8390
ds = self.mav.recv_match(
8391
type="DISTANCE_SENSOR",
8392
timeout=2,
8393
blocking=True,
8394
condition="DISTANCE_SENSOR.id==%u" % i
8395
)
8396
if ds is None:
8397
raise NotAchievedException("Did not receive DISTANCE_SENSOR message for id==%u (%s)" % (i, name))
8398
self.progress("Got: %s" % str(ds))
8399
if abs(ds.current_distance/100.0 - gpi.relative_alt/1000.0) > 1:
8400
raise NotAchievedException(
8401
"distance sensor.current_distance (%f) (%s) disagrees with global-position-int.relative_alt (%s)" %
8402
(ds.current_distance/100.0, name, gpi.relative_alt/1000.0))
8403
8404
self.land_and_disarm()
8405
8406
self.progress("Ensure RFND messages in log")
8407
if not self.current_onboard_log_contains_message("RFND"):
8408
raise NotAchievedException("No RFND messages in log")
8409
8410
def MAVProximity(self):
8411
'''Test MAVLink proximity driver'''
8412
self.start_subtest("Test mavlink proximity sensor using DISTANCE_SENSOR messages") # noqa
8413
self.context_push()
8414
ex = None
8415
try:
8416
self.set_parameter("SERIAL5_PROTOCOL", 1)
8417
self.set_parameter("PRX1_TYPE", 2) # mavlink
8418
self.reboot_sitl()
8419
8420
self.progress("Should be unhealthy while we don't send messages")
8421
self.assert_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_PROXIMITY, True, True, False)
8422
8423
self.progress("Should be healthy while we're sending good messages")
8424
tstart = self.get_sim_time()
8425
while True:
8426
if self.get_sim_time() - tstart > 5:
8427
raise NotAchievedException("Sensor did not come good")
8428
self.mav.mav.distance_sensor_send(
8429
0, # time_boot_ms
8430
10, # min_distance cm
8431
50, # max_distance cm
8432
20, # current_distance cm
8433
mavutil.mavlink.MAV_DISTANCE_SENSOR_LASER, # type
8434
21, # id
8435
mavutil.mavlink.MAV_SENSOR_ROTATION_NONE, # orientation
8436
255 # covariance
8437
)
8438
if self.sensor_has_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_PROXIMITY, True, True, True):
8439
self.progress("Sensor has good state")
8440
break
8441
self.delay_sim_time(0.1)
8442
8443
self.progress("Should be unhealthy again if we stop sending messages")
8444
self.delay_sim_time(1)
8445
self.assert_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_PROXIMITY, True, True, False)
8446
8447
# now make sure we get echoed back the same sorts of things we send:
8448
# distances are in cm
8449
distance_map = {
8450
mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 30,
8451
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 35,
8452
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 20,
8453
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 15,
8454
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 70,
8455
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 80,
8456
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 10,
8457
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 90,
8458
}
8459
8460
wanted_distances = copy.copy(distance_map)
8461
sensor_enum = mavutil.mavlink.enums["MAV_SENSOR_ORIENTATION"]
8462
8463
def my_message_hook(mav, m):
8464
if m.get_type() != 'DISTANCE_SENSOR':
8465
return
8466
self.progress("Got (%s)" % str(m))
8467
want = distance_map[m.orientation]
8468
got = m.current_distance
8469
# ArduPilot's floating point conversions make it imprecise:
8470
delta = abs(want-got)
8471
if delta > 1:
8472
self.progress(
8473
"Wrong distance (%s): want=%f got=%f" %
8474
(sensor_enum[m.orientation].name, want, got))
8475
return
8476
if m.orientation not in wanted_distances:
8477
return
8478
self.progress(
8479
"Correct distance (%s): want=%f got=%f" %
8480
(sensor_enum[m.orientation].name, want, got))
8481
del wanted_distances[m.orientation]
8482
8483
self.install_message_hook_context(my_message_hook)
8484
tstart = self.get_sim_time()
8485
while True:
8486
if self.get_sim_time() - tstart > 5:
8487
raise NotAchievedException("Sensor did not give right distances") # noqa
8488
for (orient, dist) in distance_map.items():
8489
self.mav.mav.distance_sensor_send(
8490
0, # time_boot_ms
8491
10, # min_distance cm
8492
90, # max_distance cm
8493
dist, # current_distance cm
8494
mavutil.mavlink.MAV_DISTANCE_SENSOR_LASER, # type
8495
21, # id
8496
orient, # orientation
8497
255 # covariance
8498
)
8499
self.wait_heartbeat()
8500
if len(wanted_distances.keys()) == 0:
8501
break
8502
except Exception as e:
8503
self.print_exception_caught(e)
8504
ex = e
8505
self.context_pop()
8506
self.reboot_sitl()
8507
if ex is not None:
8508
raise ex
8509
8510
def fly_rangefinder_mavlink_distance_sensor(self):
8511
self.start_subtest("Test mavlink rangefinder using DISTANCE_SENSOR messages")
8512
self.context_push()
8513
self.set_parameters({
8514
"RTL_ALT_TYPE": 0,
8515
"LGR_ENABLE": 1,
8516
"LGR_DEPLOY_ALT": 1,
8517
"LGR_RETRACT_ALT": 10, # metres
8518
"SERVO10_FUNCTION": 29
8519
})
8520
ex = None
8521
try:
8522
self.set_parameter("SERIAL5_PROTOCOL", 1)
8523
self.set_parameter("RNGFND1_TYPE", 10)
8524
self.reboot_sitl()
8525
self.set_parameter("RNGFND1_MAX_CM", 32767)
8526
8527
self.progress("Should be unhealthy while we don't send messages")
8528
self.assert_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION, True, True, False)
8529
8530
self.progress("Should be healthy while we're sending good messages")
8531
tstart = self.get_sim_time()
8532
while True:
8533
if self.get_sim_time() - tstart > 5:
8534
raise NotAchievedException("Sensor did not come good")
8535
self.mav.mav.distance_sensor_send(
8536
0, # time_boot_ms
8537
10, # min_distance
8538
50, # max_distance
8539
20, # current_distance
8540
mavutil.mavlink.MAV_DISTANCE_SENSOR_LASER, # type
8541
21, # id
8542
mavutil.mavlink.MAV_SENSOR_ROTATION_PITCH_270, # orientation
8543
255 # covariance
8544
)
8545
if self.sensor_has_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION, True, True, True):
8546
self.progress("Sensor has good state")
8547
break
8548
self.delay_sim_time(0.1)
8549
8550
self.progress("Should be unhealthy again if we stop sending messages")
8551
self.delay_sim_time(1)
8552
self.assert_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION, True, True, False)
8553
8554
self.progress("Landing gear should deploy with current_distance below min_distance")
8555
self.change_mode('STABILIZE')
8556
timeout = 60
8557
tstart = self.get_sim_time()
8558
while not self.sensor_has_state(mavutil.mavlink.MAV_SYS_STATUS_PREARM_CHECK, True, True, True):
8559
if self.get_sim_time() - tstart > timeout:
8560
raise NotAchievedException("Failed to become armable after %f seconds" % timeout)
8561
self.mav.mav.distance_sensor_send(
8562
0, # time_boot_ms
8563
100, # min_distance (cm)
8564
2500, # max_distance (cm)
8565
200, # current_distance (cm)
8566
mavutil.mavlink.MAV_DISTANCE_SENSOR_LASER, # type
8567
21, # id
8568
mavutil.mavlink.MAV_SENSOR_ROTATION_PITCH_270, # orientation
8569
255 # covariance
8570
)
8571
self.arm_vehicle()
8572
self.delay_sim_time(1) # servo function maps only periodically updated
8573
# self.send_debug_trap()
8574
8575
self.run_cmd(
8576
mavutil.mavlink.MAV_CMD_AIRFRAME_CONFIGURATION,
8577
p2=0, # deploy
8578
)
8579
8580
self.context_collect("STATUSTEXT")
8581
tstart = self.get_sim_time()
8582
while True:
8583
if self.get_sim_time_cached() - tstart > 5:
8584
raise NotAchievedException("Retraction did not happen")
8585
self.mav.mav.distance_sensor_send(
8586
0, # time_boot_ms
8587
100, # min_distance (cm)
8588
6000, # max_distance (cm)
8589
1500, # current_distance (cm)
8590
mavutil.mavlink.MAV_DISTANCE_SENSOR_LASER, # type
8591
21, # id
8592
mavutil.mavlink.MAV_SENSOR_ROTATION_PITCH_270, # orientation
8593
255 # covariance
8594
)
8595
self.delay_sim_time(0.1)
8596
try:
8597
self.wait_text("LandingGear: RETRACT", check_context=True, timeout=0.1)
8598
except Exception:
8599
continue
8600
self.progress("Retracted")
8601
break
8602
# self.send_debug_trap()
8603
while True:
8604
if self.get_sim_time_cached() - tstart > 5:
8605
raise NotAchievedException("Deployment did not happen")
8606
self.progress("Sending distance-sensor message")
8607
self.mav.mav.distance_sensor_send(
8608
0, # time_boot_ms
8609
300, # min_distance
8610
500, # max_distance
8611
250, # current_distance
8612
mavutil.mavlink.MAV_DISTANCE_SENSOR_LASER, # type
8613
21, # id
8614
mavutil.mavlink.MAV_SENSOR_ROTATION_PITCH_270, # orientation
8615
255 # covariance
8616
)
8617
try:
8618
self.wait_text("LandingGear: DEPLOY", check_context=True, timeout=0.1)
8619
except Exception:
8620
continue
8621
self.progress("Deployed")
8622
break
8623
self.disarm_vehicle()
8624
8625
except Exception as e:
8626
self.print_exception_caught(e)
8627
ex = e
8628
self.context_pop()
8629
self.reboot_sitl()
8630
if ex is not None:
8631
raise ex
8632
8633
def GSF(self):
8634
'''test the Gaussian Sum filter'''
8635
self.context_push()
8636
self.set_parameter("EK2_ENABLE", 1)
8637
self.reboot_sitl()
8638
self.takeoff(20, mode='LOITER')
8639
self.set_rc(2, 1400)
8640
self.delay_sim_time(5)
8641
self.set_rc(2, 1500)
8642
self.progress("Path: %s" % self.current_onboard_log_filepath())
8643
dfreader = self.dfreader_for_current_onboard_log()
8644
self.do_RTL()
8645
self.context_pop()
8646
self.reboot_sitl()
8647
8648
# ensure log messages present
8649
want = set(["XKY0", "XKY1", "NKY0", "NKY1"])
8650
still_want = want
8651
while len(still_want):
8652
m = dfreader.recv_match(type=want)
8653
if m is None:
8654
raise NotAchievedException("Did not get %s" % want)
8655
still_want.remove(m.get_type())
8656
8657
def GSF_reset(self):
8658
'''test the Gaussian Sum filter based Emergency reset'''
8659
self.context_push()
8660
self.set_parameters({
8661
"COMPASS_ORIENT": 4, # yaw 180
8662
"COMPASS_USE2": 0, # disable backup compasses to avoid pre-arm failures
8663
"COMPASS_USE3": 0,
8664
})
8665
self.reboot_sitl()
8666
self.change_mode('GUIDED')
8667
self.wait_ready_to_arm()
8668
8669
# record starting position
8670
startpos = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True)
8671
self.progress("startpos=%s" % str(startpos))
8672
8673
# arm vehicle and takeoff to at least 5m
8674
self.arm_vehicle()
8675
expected_alt = 5
8676
self.user_takeoff(alt_min=expected_alt)
8677
8678
# watch for emergency yaw reset
8679
self.wait_text("EKF3 IMU. emergency yaw reset", timeout=5, regex=True)
8680
8681
# record how far vehicle flew off
8682
endpos = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True)
8683
delta_x = endpos.x - startpos.x
8684
delta_y = endpos.y - startpos.y
8685
dist_m = math.sqrt(delta_x*delta_x + delta_y*delta_y)
8686
self.progress("GSF yaw reset triggered at %f meters" % dist_m)
8687
8688
self.do_RTL()
8689
self.context_pop()
8690
self.reboot_sitl()
8691
8692
# ensure vehicle did not fly too far
8693
dist_m_max = 8
8694
if dist_m > dist_m_max:
8695
raise NotAchievedException("GSF reset failed, vehicle flew too far (%f > %f)" % (dist_m, dist_m_max))
8696
8697
def fly_rangefinder_mavlink(self):
8698
self.fly_rangefinder_mavlink_distance_sensor()
8699
8700
# explicit test for the mavlink driver as it doesn't play so nice:
8701
self.set_parameters({
8702
"SERIAL5_PROTOCOL": 1,
8703
"RNGFND1_TYPE": 10,
8704
})
8705
self.customise_SITL_commandline(['--serial5=sim:rf_mavlink'])
8706
8707
self.change_mode('GUIDED')
8708
self.wait_ready_to_arm()
8709
self.arm_vehicle()
8710
expected_alt = 5
8711
self.user_takeoff(alt_min=expected_alt)
8712
8713
tstart = self.get_sim_time()
8714
while True:
8715
if self.get_sim_time() - tstart > 5:
8716
raise NotAchievedException("Mavlink rangefinder not working")
8717
rf = self.mav.recv_match(type="RANGEFINDER", timeout=1, blocking=True)
8718
if rf is None:
8719
raise NotAchievedException("Did not receive rangefinder message")
8720
gpi = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)
8721
if gpi is None:
8722
raise NotAchievedException("Did not receive GLOBAL_POSITION_INT message")
8723
if abs(rf.distance - gpi.relative_alt/1000.0) > 1:
8724
print("rangefinder alt (%s) disagrees with global-position-int.relative_alt (%s)" %
8725
(rf.distance, gpi.relative_alt/1000.0))
8726
continue
8727
8728
ds = self.mav.recv_match(
8729
type="DISTANCE_SENSOR",
8730
timeout=2,
8731
blocking=True,
8732
)
8733
if ds is None:
8734
raise NotAchievedException("Did not receive DISTANCE_SENSOR message")
8735
self.progress("Got: %s" % str(ds))
8736
if abs(ds.current_distance/100.0 - gpi.relative_alt/1000.0) > 1:
8737
print(
8738
"distance sensor.current_distance (%f) disagrees with global-position-int.relative_alt (%s)" %
8739
(ds.current_distance/100.0, gpi.relative_alt/1000.0))
8740
continue
8741
break
8742
self.progress("mavlink rangefinder OK")
8743
self.land_and_disarm()
8744
8745
def MaxBotixI2CXL(self):
8746
'''Test maxbotix rangefinder drivers'''
8747
ex = None
8748
try:
8749
self.context_push()
8750
8751
self.start_subtest("No messages")
8752
rf = self.mav.recv_match(type="DISTANCE_SENSOR", timeout=5, blocking=True)
8753
if rf is not None:
8754
raise NotAchievedException("Receiving DISTANCE_SENSOR when I shouldn't be")
8755
8756
self.start_subtest("Default address")
8757
self.set_parameter("RNGFND1_TYPE", 2) # maxbotix
8758
self.reboot_sitl()
8759
self.do_timesync_roundtrip()
8760
rf = self.mav.recv_match(type="DISTANCE_SENSOR", timeout=5, blocking=True)
8761
self.progress("Got (%s)" % str(rf))
8762
if rf is None:
8763
raise NotAchievedException("Didn't receive DISTANCE_SENSOR when I should've")
8764
8765
self.start_subtest("Explicitly set to default address")
8766
self.set_parameters({
8767
"RNGFND1_TYPE": 2, # maxbotix
8768
"RNGFND1_ADDR": 0x70,
8769
})
8770
self.reboot_sitl()
8771
self.do_timesync_roundtrip()
8772
rf = self.mav.recv_match(type="DISTANCE_SENSOR", timeout=5, blocking=True)
8773
self.progress("Got (%s)" % str(rf))
8774
if rf is None:
8775
raise NotAchievedException("Didn't receive DISTANCE_SENSOR when I should've")
8776
8777
self.start_subtest("Explicitly set to non-default address")
8778
self.set_parameter("RNGFND1_ADDR", 0x71)
8779
self.reboot_sitl()
8780
self.do_timesync_roundtrip()
8781
rf = self.mav.recv_match(type="DISTANCE_SENSOR", timeout=5, blocking=True)
8782
self.progress("Got (%s)" % str(rf))
8783
if rf is None:
8784
raise NotAchievedException("Didn't receive DISTANCE_SENSOR when I should've")
8785
8786
self.start_subtest("Two MaxBotix RangeFinders")
8787
self.set_parameters({
8788
"RNGFND1_TYPE": 2, # maxbotix
8789
"RNGFND1_ADDR": 0x70,
8790
"RNGFND1_MIN_CM": 150,
8791
"RNGFND2_TYPE": 2, # maxbotix
8792
"RNGFND2_ADDR": 0x71,
8793
"RNGFND2_MIN_CM": 250,
8794
})
8795
self.reboot_sitl()
8796
self.do_timesync_roundtrip()
8797
for i in [0, 1]:
8798
rf = self.mav.recv_match(
8799
type="DISTANCE_SENSOR",
8800
timeout=5,
8801
blocking=True,
8802
condition="DISTANCE_SENSOR.id==%u" % i
8803
)
8804
self.progress("Got id==%u (%s)" % (i, str(rf)))
8805
if rf is None:
8806
raise NotAchievedException("Didn't receive DISTANCE_SENSOR when I should've")
8807
expected_dist = 150
8808
if i == 1:
8809
expected_dist = 250
8810
if rf.min_distance != expected_dist:
8811
raise NotAchievedException("Unexpected min_cm (want=%u got=%u)" %
8812
(expected_dist, rf.min_distance))
8813
8814
self.context_pop()
8815
except Exception as e:
8816
self.print_exception_caught(e)
8817
ex = e
8818
8819
self.reboot_sitl()
8820
if ex is not None:
8821
raise ex
8822
8823
def fly_rangefinder_sitl(self):
8824
self.set_parameters({
8825
"RNGFND1_TYPE": 100,
8826
})
8827
self.reboot_sitl()
8828
self.fly_rangefinder_drivers_fly([("unused", "sitl")])
8829
self.wait_disarmed()
8830
8831
def RangeFinderDrivers(self):
8832
'''Test rangefinder drivers'''
8833
self.set_parameters({
8834
"RTL_ALT": 500,
8835
"RTL_ALT_TYPE": 1,
8836
})
8837
drivers = [
8838
("lightwareserial", 8), # autodetected between this and -binary
8839
("lightwareserial-binary", 8),
8840
("USD1_v0", 11),
8841
("USD1_v1", 11),
8842
("leddarone", 12),
8843
("maxsonarseriallv", 13),
8844
("nmea", 17, {"baud": 9600}),
8845
("wasp", 18),
8846
("benewake_tf02", 19),
8847
("blping", 23),
8848
("benewake_tfmini", 20),
8849
("lanbao", 26),
8850
("benewake_tf03", 27),
8851
("gyus42v2", 31),
8852
("teraranger_serial", 35),
8853
("nooploop_tofsense", 37),
8854
("ainsteinlrd1", 42),
8855
("rds02uf", 43),
8856
]
8857
while len(drivers):
8858
do_drivers = drivers[0:3]
8859
drivers = drivers[3:]
8860
command_line_args = []
8861
self.context_push()
8862
for offs in range(3):
8863
serial_num = offs + 4
8864
if len(do_drivers) > offs:
8865
if len(do_drivers[offs]) > 2:
8866
(sim_name, rngfnd_param_value, kwargs) = do_drivers[offs]
8867
else:
8868
(sim_name, rngfnd_param_value) = do_drivers[offs]
8869
kwargs = {}
8870
command_line_args.append("--serial%s=sim:%s" %
8871
(serial_num, sim_name))
8872
sets = {
8873
"SERIAL%u_PROTOCOL" % serial_num: 9, # rangefinder
8874
"RNGFND%u_TYPE" % (offs+1): rngfnd_param_value,
8875
}
8876
if "baud" in kwargs:
8877
sets["SERIAL%u_BAUD" % serial_num] = kwargs["baud"]
8878
self.set_parameters(sets)
8879
self.customise_SITL_commandline(command_line_args)
8880
self.fly_rangefinder_drivers_fly([x[0] for x in do_drivers])
8881
self.context_pop()
8882
8883
self.fly_rangefinder_mavlink()
8884
self.fly_rangefinder_sitl() # i.e. type 100
8885
8886
i2c_drivers = [
8887
("maxbotixi2cxl", 2),
8888
]
8889
while len(i2c_drivers):
8890
do_drivers = i2c_drivers[0:9]
8891
i2c_drivers = i2c_drivers[9:]
8892
count = 1
8893
p = {}
8894
for d in do_drivers:
8895
(sim_name, rngfnd_param_value) = d
8896
p["RNGFND%u_TYPE" % count] = rngfnd_param_value
8897
count += 1
8898
8899
self.set_parameters(p)
8900
8901
self.reboot_sitl()
8902
self.fly_rangefinder_drivers_fly([x[0] for x in do_drivers])
8903
8904
def RangeFinderDriversMaxAlt(self):
8905
'''test max-height behaviour'''
8906
# lightwareserial goes to 130m when out of range
8907
self.set_parameters({
8908
"SERIAL4_PROTOCOL": 9,
8909
"RNGFND1_TYPE": 8,
8910
"WPNAV_SPEED_UP": 1000, # cm/s
8911
})
8912
self.customise_SITL_commandline([
8913
"--serial4=sim:lightwareserial",
8914
])
8915
self.takeoff(95, mode='GUIDED', timeout=240, max_err=0.5)
8916
self.assert_rangefinder_distance_between(90, 100)
8917
8918
self.wait_rangefinder_distance(90, 100)
8919
8920
rf_bit = mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION
8921
8922
self.assert_sensor_state(rf_bit, present=True, enabled=True, healthy=True)
8923
self.assert_distance_sensor_quality(100)
8924
8925
self.progress("Moving higher to be out of max rangefinder range")
8926
self.fly_guided_move_local(0, 0, 150)
8927
8928
# sensor remains healthy even out-of-range
8929
self.assert_sensor_state(rf_bit, present=True, enabled=True, healthy=True)
8930
8931
self.assert_distance_sensor_quality(1)
8932
8933
self.do_RTL()
8934
8935
def ShipTakeoff(self):
8936
'''Fly Simulated Ship Takeoff'''
8937
# test ship takeoff
8938
self.wait_groundspeed(0, 2)
8939
self.set_parameters({
8940
"SIM_SHIP_ENABLE": 1,
8941
"SIM_SHIP_SPEED": 10,
8942
"SIM_SHIP_DSIZE": 2,
8943
})
8944
self.wait_ready_to_arm()
8945
# we should be moving with the ship
8946
self.wait_groundspeed(9, 11)
8947
self.takeoff(10)
8948
# above ship our speed drops to 0
8949
self.wait_groundspeed(0, 2)
8950
self.land_and_disarm()
8951
# ship will have moved on, so we land on the water which isn't moving
8952
self.wait_groundspeed(0, 2)
8953
8954
def ParameterValidation(self):
8955
'''Test parameters are checked for validity'''
8956
# wait 10 seconds for initialisation
8957
self.delay_sim_time(10)
8958
self.progress("invalid; min must be less than max:")
8959
self.set_parameters({
8960
"MOT_PWM_MIN": 100,
8961
"MOT_PWM_MAX": 50,
8962
})
8963
self.drain_mav()
8964
self.assert_prearm_failure("Motors: Check MOT_PWM_MIN and MOT_PWM_MAX")
8965
self.progress("invalid; min must be less than max (equal case):")
8966
self.set_parameters({
8967
"MOT_PWM_MIN": 100,
8968
"MOT_PWM_MAX": 100,
8969
})
8970
self.drain_mav()
8971
self.assert_prearm_failure("Motors: Check MOT_PWM_MIN and MOT_PWM_MAX")
8972
self.progress("Spin min more than 0.3")
8973
self.set_parameters({
8974
"MOT_PWM_MIN": 1000,
8975
"MOT_PWM_MAX": 2000,
8976
"MOT_SPIN_MIN": 0.5,
8977
})
8978
self.drain_mav()
8979
self.assert_prearm_failure("PreArm: Motors: MOT_SPIN_MIN too high 0.50 > 0.3")
8980
self.progress("Spin arm more than spin min")
8981
self.set_parameters({
8982
"MOT_SPIN_MIN": 0.1,
8983
"MOT_SPIN_ARM": 0.2,
8984
})
8985
self.drain_mav()
8986
self.assert_prearm_failure("PreArm: Motors: MOT_SPIN_ARM > MOT_SPIN_MIN")
8987
8988
def SensorErrorFlags(self):
8989
'''Test we get ERR messages when sensors have issues'''
8990
self.reboot_sitl()
8991
8992
for (param_names, param_value, expected_subsys, expected_ecode, desc) in [
8993
(['SIM_BARO_DISABLE', 'SIM_BAR2_DISABLE'], 1, 18, 4, 'unhealthy'),
8994
(['SIM_BARO_DISABLE', 'SIM_BAR2_DISABLE'], 0, 18, 0, 'healthy'),
8995
(['SIM_MAG1_FAIL', 'SIM_MAG2_FAIL', 'SIM_MAG3_FAIL'], 1, 3, 4, 'unhealthy'),
8996
(['SIM_MAG1_FAIL', 'SIM_MAG2_FAIL', 'SIM_MAG3_FAIL'], 0, 3, 0, 'healthy'),
8997
]:
8998
sp = dict()
8999
for name in param_names:
9000
sp[name] = param_value
9001
self.set_parameters(sp)
9002
self.delay_sim_time(1)
9003
mlog = self.dfreader_for_current_onboard_log()
9004
success = False
9005
while True:
9006
m = mlog.recv_match(type='ERR')
9007
print("Got (%s)" % str(m))
9008
if m is None:
9009
break
9010
if m.Subsys == expected_subsys and m.ECode == expected_ecode: # baro / ecode
9011
success = True
9012
break
9013
if not success:
9014
raise NotAchievedException("Did not find %s log message" % desc)
9015
9016
def AltEstimation(self):
9017
'''Test that Alt Estimation is mandatory for ALT_HOLD'''
9018
self.context_push()
9019
ex = None
9020
try:
9021
# disable barometer so there is no altitude source
9022
self.set_parameters({
9023
"SIM_BARO_DISABLE": 1,
9024
"SIM_BARO2_DISABL": 1,
9025
})
9026
9027
self.wait_gps_disable(position_vertical=True)
9028
9029
# turn off arming checks (mandatory arming checks will still be run)
9030
self.set_parameter("ARMING_CHECK", 0)
9031
9032
# delay 12 sec to allow EKF to lose altitude estimate
9033
self.delay_sim_time(12)
9034
9035
self.change_mode("ALT_HOLD")
9036
self.assert_prearm_failure("Need Alt Estimate")
9037
9038
# force arm vehicle in stabilize to bypass barometer pre-arm checks
9039
self.change_mode("STABILIZE")
9040
self.arm_vehicle()
9041
self.set_rc(3, 1700)
9042
try:
9043
self.change_mode("ALT_HOLD", timeout=10)
9044
except AutoTestTimeoutException:
9045
self.progress("PASS not able to set mode without Position : %s" % "ALT_HOLD")
9046
9047
# check that mode change to ALT_HOLD has failed (it should)
9048
if self.mode_is("ALT_HOLD"):
9049
raise NotAchievedException("Changed to ALT_HOLD with no altitude estimate")
9050
9051
except Exception as e:
9052
self.print_exception_caught(e)
9053
ex = e
9054
self.context_pop()
9055
self.disarm_vehicle(force=True)
9056
if ex is not None:
9057
raise ex
9058
9059
def EKFSource(self):
9060
'''Check EKF Source Prearms work'''
9061
self.context_push()
9062
ex = None
9063
try:
9064
self.set_parameters({
9065
"EK3_ENABLE": 1,
9066
"AHRS_EKF_TYPE": 3,
9067
})
9068
self.wait_ready_to_arm()
9069
9070
self.start_subtest("bad yaw source")
9071
self.set_parameter("EK3_SRC3_YAW", 17)
9072
self.assert_prearm_failure("Check EK3_SRC3_YAW")
9073
9074
self.context_push()
9075
self.start_subtest("missing required yaw source")
9076
self.set_parameters({
9077
"EK3_SRC3_YAW": 3, # External Yaw with Compass Fallback
9078
"COMPASS_USE": 0,
9079
"COMPASS_USE2": 0,
9080
"COMPASS_USE3": 0,
9081
})
9082
self.assert_prearm_failure("EK3 sources require Compass")
9083
self.context_pop()
9084
9085
except Exception as e:
9086
self.disarm_vehicle(force=True)
9087
self.print_exception_caught(e)
9088
ex = e
9089
self.context_pop()
9090
if ex is not None:
9091
raise ex
9092
9093
def test_replay_gps_bit(self):
9094
self.set_parameters({
9095
"LOG_REPLAY": 1,
9096
"LOG_DISARMED": 1,
9097
"EK3_ENABLE": 1,
9098
"EK2_ENABLE": 1,
9099
"AHRS_TRIM_X": 0.01,
9100
"AHRS_TRIM_Y": -0.03,
9101
"GPS2_TYPE": 1,
9102
"GPS1_POS_X": 0.1,
9103
"GPS1_POS_Y": 0.2,
9104
"GPS1_POS_Z": 0.3,
9105
"GPS2_POS_X": -0.1,
9106
"GPS2_POS_Y": -0.02,
9107
"GPS2_POS_Z": -0.31,
9108
"INS_POS1_X": 0.12,
9109
"INS_POS1_Y": 0.14,
9110
"INS_POS1_Z": -0.02,
9111
"INS_POS2_X": 0.07,
9112
"INS_POS2_Y": 0.012,
9113
"INS_POS2_Z": -0.06,
9114
"RNGFND1_TYPE": 1,
9115
"RNGFND1_PIN": 0,
9116
"RNGFND1_SCALING": 30,
9117
"RNGFND1_POS_X": 0.17,
9118
"RNGFND1_POS_Y": -0.07,
9119
"RNGFND1_POS_Z": -0.005,
9120
"SIM_SONAR_SCALE": 30,
9121
"SIM_GPS2_ENABLE": 1,
9122
})
9123
self.reboot_sitl()
9124
9125
self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_LOGGING, True, True, True)
9126
9127
current_log_filepath = self.current_onboard_log_filepath()
9128
self.progress("Current log path: %s" % str(current_log_filepath))
9129
9130
self.change_mode("LOITER")
9131
self.wait_ready_to_arm(require_absolute=True)
9132
self.arm_vehicle()
9133
self.takeoffAndMoveAway()
9134
self.do_RTL()
9135
9136
self.reboot_sitl()
9137
9138
return current_log_filepath
9139
9140
def test_replay_beacon_bit(self):
9141
self.set_parameters({
9142
"LOG_REPLAY": 1,
9143
"LOG_DISARMED": 1,
9144
})
9145
9146
old_onboard_logs = sorted(self.log_list())
9147
self.BeaconPosition()
9148
new_onboard_logs = sorted(self.log_list())
9149
9150
log_difference = [x for x in new_onboard_logs if x not in old_onboard_logs]
9151
return log_difference[2]
9152
9153
def test_replay_optical_flow_bit(self):
9154
self.set_parameters({
9155
"LOG_REPLAY": 1,
9156
"LOG_DISARMED": 1,
9157
})
9158
9159
old_onboard_logs = sorted(self.log_list())
9160
self.OpticalFlowLimits()
9161
new_onboard_logs = sorted(self.log_list())
9162
9163
log_difference = [x for x in new_onboard_logs if x not in old_onboard_logs]
9164
print("log difference: %s" % str(log_difference))
9165
return log_difference[0]
9166
9167
def GPSBlendingLog(self):
9168
'''Test GPS Blending'''
9169
'''ensure we get dataflash log messages for blended instance'''
9170
9171
self.context_push()
9172
9173
ex = None
9174
9175
try:
9176
# configure:
9177
self.set_parameters({
9178
"GPS2_TYPE": 1,
9179
"SIM_GPS2_TYPE": 1,
9180
"SIM_GPS2_ENABLE": 1,
9181
"GPS_AUTO_SWITCH": 2,
9182
})
9183
self.reboot_sitl()
9184
9185
# ensure we're seeing the second GPS:
9186
tstart = self.get_sim_time()
9187
while True:
9188
if self.get_sim_time_cached() - tstart > 60:
9189
raise NotAchievedException("Did not get good GPS2_RAW message")
9190
m = self.mav.recv_match(type='GPS2_RAW', blocking=True, timeout=1)
9191
self.progress("%s" % str(m))
9192
if m is None:
9193
continue
9194
if m.lat == 0:
9195
continue
9196
break
9197
9198
# create a log we can expect blended data to appear in:
9199
self.change_mode('LOITER')
9200
self.wait_ready_to_arm()
9201
self.arm_vehicle()
9202
self.delay_sim_time(5)
9203
self.disarm_vehicle()
9204
9205
# inspect generated log for messages:
9206
dfreader = self.dfreader_for_current_onboard_log()
9207
wanted = set([0, 1, 2])
9208
seen_primary_change = False
9209
while True:
9210
m = dfreader.recv_match(type=["GPS", "EV"]) # disarmed
9211
if m is None:
9212
break
9213
mtype = m.get_type()
9214
if mtype == 'GPS':
9215
try:
9216
wanted.remove(m.I)
9217
except KeyError:
9218
continue
9219
elif mtype == 'EV':
9220
if m.Id == 67: # GPS_PRIMARY_CHANGED
9221
seen_primary_change = True
9222
if len(wanted) == 0 and seen_primary_change:
9223
break
9224
9225
if len(wanted):
9226
raise NotAchievedException("Did not get all three GPS types")
9227
if not seen_primary_change:
9228
raise NotAchievedException("Did not see primary change")
9229
9230
except Exception as e:
9231
self.progress("Caught exception: %s" %
9232
self.get_exception_stacktrace(e))
9233
ex = e
9234
9235
self.context_pop()
9236
9237
self.reboot_sitl()
9238
9239
if ex is not None:
9240
raise ex
9241
9242
def GPSBlending(self):
9243
'''Test GPS Blending'''
9244
'''ensure we get dataflash log messages for blended instance'''
9245
9246
# configure:
9247
self.set_parameters({
9248
"WP_YAW_BEHAVIOR": 0, # do not yaw
9249
"GPS2_TYPE": 1,
9250
"SIM_GPS2_TYPE": 1,
9251
"SIM_GPS2_ENABLE": 1,
9252
"SIM_GPS1_POS_X": 1.0,
9253
"SIM_GPS1_POS_Y": -1.0,
9254
"SIM_GPS2_POS_X": -1.0,
9255
"SIM_GPS2_POS_Y": 1.0,
9256
"GPS_AUTO_SWITCH": 2,
9257
})
9258
self.reboot_sitl()
9259
9260
alt = 10
9261
self.takeoff(alt, mode='GUIDED')
9262
self.fly_guided_move_local(30, 0, alt)
9263
self.fly_guided_move_local(30, 30, alt)
9264
self.fly_guided_move_local(0, 30, alt)
9265
self.fly_guided_move_local(0, 0, alt)
9266
self.change_mode('LAND')
9267
9268
current_log_file = self.dfreader_for_current_onboard_log()
9269
9270
self.wait_disarmed()
9271
9272
# ensure that the blended solution is always about half-way
9273
# between the two GPSs:
9274
current_ts = None
9275
while True:
9276
m = current_log_file.recv_match(type='GPS')
9277
if m is None:
9278
break
9279
if current_ts is None:
9280
if m.I != 0: # noqa
9281
continue
9282
current_ts = m.TimeUS
9283
measurements = {}
9284
if m.TimeUS != current_ts:
9285
current_ts = None
9286
continue
9287
measurements[m.I] = (m.Lat, m.Lng)
9288
if len(measurements) == 3:
9289
# check lat:
9290
for n in 0, 1:
9291
expected_blended = (measurements[0][n] + measurements[1][n])/2
9292
epsilon = 0.0000002
9293
error = abs(measurements[2][n] - expected_blended)
9294
if error > epsilon:
9295
raise NotAchievedException("Blended diverged")
9296
current_ts = None
9297
9298
if len(measurements) != 3:
9299
raise NotAchievedException("Did not see three GPS measurements!")
9300
9301
def GPSWeightedBlending(self):
9302
'''Test GPS Weighted Blending'''
9303
9304
self.context_push()
9305
9306
# configure:
9307
self.set_parameters({
9308
"WP_YAW_BEHAVIOR": 0, # do not yaw
9309
"GPS2_TYPE": 1,
9310
"SIM_GPS2_TYPE": 1,
9311
"SIM_GPS2_ENABLE": 1,
9312
"SIM_GPS1_POS_X": 1.0,
9313
"SIM_GPS1_POS_Y": -1.0,
9314
"SIM_GPS2_POS_X": -1.0,
9315
"SIM_GPS2_POS_Y": 1.0,
9316
"GPS_AUTO_SWITCH": 2,
9317
})
9318
# configure velocity errors such that the 1st GPS should be
9319
# 4/5, second GPS 1/5 of result (0.5**2)/((0.5**2)+(1.0**2))
9320
self.set_parameters({
9321
"SIM_GPS1_VERR_X": 0.3, # m/s
9322
"SIM_GPS1_VERR_Y": 0.4,
9323
"SIM_GPS2_VERR_X": 0.6, # m/s
9324
"SIM_GPS2_VERR_Y": 0.8,
9325
"GPS_BLEND_MASK": 4, # use only speed for blend calculations
9326
})
9327
self.reboot_sitl()
9328
9329
alt = 10
9330
self.takeoff(alt, mode='GUIDED')
9331
self.fly_guided_move_local(30, 0, alt)
9332
self.fly_guided_move_local(30, 30, alt)
9333
self.fly_guided_move_local(0, 30, alt)
9334
self.fly_guided_move_local(0, 0, alt)
9335
self.change_mode('LAND')
9336
9337
current_log_file = self.dfreader_for_current_onboard_log()
9338
9339
self.wait_disarmed()
9340
9341
# ensure that the blended solution is always about half-way
9342
# between the two GPSs:
9343
current_ts = None
9344
while True:
9345
m = current_log_file.recv_match(type='GPS')
9346
if m is None:
9347
break
9348
if current_ts is None:
9349
if m.I != 0: # noqa
9350
continue
9351
current_ts = m.TimeUS
9352
measurements = {}
9353
if m.TimeUS != current_ts:
9354
current_ts = None
9355
continue
9356
measurements[m.I] = (m.Lat, m.Lng, m.Alt)
9357
if len(measurements) == 3:
9358
# check lat:
9359
for n in 0, 1, 2:
9360
expected_blended = 0.8*measurements[0][n] + 0.2*measurements[1][n]
9361
axis_epsilons = [0.0000002, 0.0000002, 0.2]
9362
epsilon = axis_epsilons[n]
9363
error = abs(measurements[2][n] - expected_blended)
9364
if error > epsilon:
9365
raise NotAchievedException(f"Blended diverged {n=} {measurements[0][n]=} {measurements[1][n]=}")
9366
current_ts = None
9367
9368
self.context_pop()
9369
self.reboot_sitl()
9370
9371
def GPSBlendingAffinity(self):
9372
'''test blending when affinity in use'''
9373
# configure:
9374
self.set_parameters({
9375
"WP_YAW_BEHAVIOR": 0, # do not yaw
9376
"GPS2_TYPE": 1,
9377
"SIM_GPS2_TYPE": 1,
9378
"SIM_GPS2_ENABLE": 1,
9379
"SIM_GPS1_POS_X": 1.0,
9380
"SIM_GPS1_POS_Y": -1.0,
9381
"SIM_GPS2_POS_X": -1.0,
9382
"SIM_GPS2_POS_Y": 1.0,
9383
"GPS_AUTO_SWITCH": 2,
9384
9385
"EK3_AFFINITY" : 1,
9386
"EK3_IMU_MASK": 7,
9387
"SIM_IMU_COUNT": 3,
9388
"INS_ACC3OFFS_X": 0.001,
9389
"INS_ACC3OFFS_Y": 0.001,
9390
"INS_ACC3OFFS_Z": 0.001,
9391
})
9392
# force-calibration of accel:
9393
self.run_cmd(mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, p5=76)
9394
self.reboot_sitl()
9395
9396
alt = 10
9397
self.takeoff(alt, mode='GUIDED')
9398
self.fly_guided_move_local(30, 0, alt)
9399
self.fly_guided_move_local(30, 30, alt)
9400
self.fly_guided_move_local(0, 30, alt)
9401
self.fly_guided_move_local(0, 0, alt)
9402
self.change_mode('LAND')
9403
9404
current_log_file = self.dfreader_for_current_onboard_log()
9405
9406
self.wait_disarmed()
9407
9408
# ensure that the blended solution is always about half-way
9409
# between the two GPSs:
9410
current_ts = None
9411
max_errors = [0, 0, 0]
9412
while True:
9413
m = current_log_file.recv_match(type='XKF1')
9414
if m is None:
9415
break
9416
if current_ts is None:
9417
if m.C != 0: # noqa
9418
continue
9419
current_ts = m.TimeUS
9420
measurements = {}
9421
if m.TimeUS != current_ts:
9422
current_ts = None
9423
continue
9424
measurements[m.C] = (m.PN, m.PE, m.PD)
9425
if len(measurements) == 3:
9426
# check lat:
9427
for n in 0, 1, 2:
9428
expected_blended = 0.5*measurements[0][n] + 0.5*measurements[1][n]
9429
axis_epsilons = [0.02, 0.02, 0.03]
9430
epsilon = axis_epsilons[n]
9431
error = abs(measurements[2][n] - expected_blended)
9432
# self.progress(f"{n=} {error=}")
9433
if error > max_errors[n]:
9434
max_errors[n] = error
9435
if error > epsilon:
9436
raise NotAchievedException(f"Blended diverged {n=} {measurements[0][n]=} {measurements[1][n]=} {measurements[2][n]=} {error=}") # noqa:E501
9437
current_ts = None
9438
self.progress(f"{max_errors=}")
9439
9440
def Callisto(self):
9441
'''Test Callisto'''
9442
self.customise_SITL_commandline(
9443
[],
9444
defaults_filepath=self.model_defaults_filepath('Callisto'),
9445
model="octa-quad:@ROMFS/models/Callisto.json",
9446
wipe=True,
9447
)
9448
self.takeoff(10)
9449
self.do_RTL()
9450
9451
def FlyEachFrame(self):
9452
'''Fly each supported internal frame'''
9453
vinfo = vehicleinfo.VehicleInfo()
9454
copter_vinfo_options = vinfo.options[self.vehicleinfo_key()]
9455
known_broken_frames = {
9456
'heli-compound': "wrong binary, different takeoff regime",
9457
'heli-dual': "wrong binary, different takeoff regime",
9458
'heli': "wrong binary, different takeoff regime",
9459
'heli-gas': "wrong binary, different takeoff regime",
9460
'heli-blade360': "wrong binary, different takeoff regime",
9461
"quad-can" : "needs CAN periph",
9462
}
9463
for frame in sorted(copter_vinfo_options["frames"].keys()):
9464
self.start_subtest("Testing frame (%s)" % str(frame))
9465
if frame in known_broken_frames:
9466
self.progress("Actually, no I'm not - it is known-broken (%s)" %
9467
(known_broken_frames[frame]))
9468
continue
9469
frame_bits = copter_vinfo_options["frames"][frame]
9470
print("frame_bits: %s" % str(frame_bits))
9471
if frame_bits.get("external", False):
9472
self.progress("Actually, no I'm not - it is an external simulation")
9473
continue
9474
model = frame_bits.get("model", frame)
9475
defaults = self.model_defaults_filepath(frame)
9476
if not isinstance(defaults, list):
9477
defaults = [defaults]
9478
self.customise_SITL_commandline(
9479
[],
9480
defaults_filepath=defaults,
9481
model=model,
9482
wipe=True,
9483
)
9484
9485
# add a listener that verifies yaw looks good:
9486
def verify_yaw(mav, m):
9487
if m.get_type() != 'ATTITUDE':
9488
return
9489
yawspeed_thresh_rads = math.radians(20)
9490
if m.yawspeed > yawspeed_thresh_rads:
9491
raise NotAchievedException("Excessive yaw on takeoff: %f deg/s > %f deg/s (frame=%s)" %
9492
(math.degrees(m.yawspeed), math.degrees(yawspeed_thresh_rads), frame))
9493
self.context_push()
9494
self.install_message_hook_context(verify_yaw)
9495
self.takeoff(10)
9496
self.context_pop()
9497
self.hover()
9498
self.change_mode('ALT_HOLD')
9499
self.delay_sim_time(1)
9500
9501
def verify_rollpitch(mav, m):
9502
if m.get_type() != 'ATTITUDE':
9503
return
9504
pitch_thresh_rad = math.radians(2)
9505
if m.pitch > pitch_thresh_rad:
9506
raise NotAchievedException("Excessive pitch %f deg > %f deg" %
9507
(math.degrees(m.pitch), math.degrees(pitch_thresh_rad)))
9508
roll_thresh_rad = math.radians(2)
9509
if m.roll > roll_thresh_rad:
9510
raise NotAchievedException("Excessive roll %f deg > %f deg" %
9511
(math.degrees(m.roll), math.degrees(roll_thresh_rad)))
9512
self.context_push()
9513
self.install_message_hook_context(verify_rollpitch)
9514
for i in range(5):
9515
self.set_rc(4, 2000)
9516
self.delay_sim_time(0.5)
9517
self.set_rc(4, 1500)
9518
self.delay_sim_time(5)
9519
self.context_pop()
9520
9521
self.do_RTL()
9522
9523
def Replay(self):
9524
'''test replay correctness'''
9525
self.progress("Building Replay")
9526
util.build_SITL('tool/Replay', clean=False, configure=False)
9527
self.set_parameters({
9528
"LOG_DARM_RATEMAX": 0,
9529
"LOG_FILE_RATEMAX": 0,
9530
})
9531
9532
bits = [
9533
('GPS', self.test_replay_gps_bit),
9534
('Beacon', self.test_replay_beacon_bit),
9535
('OpticalFlow', self.test_replay_optical_flow_bit),
9536
]
9537
for (name, func) in bits:
9538
self.start_subtest("%s" % name)
9539
self.test_replay_bit(func)
9540
9541
def test_replay_bit(self, bit):
9542
9543
self.context_push()
9544
current_log_filepath = bit()
9545
9546
self.progress("Running replay on (%s) (%u bytes)" % (
9547
(current_log_filepath, os.path.getsize(current_log_filepath))
9548
))
9549
9550
self.run_replay(current_log_filepath)
9551
9552
replay_log_filepath = self.current_onboard_log_filepath()
9553
9554
self.context_pop()
9555
9556
self.progress("Replay log path: %s" % str(replay_log_filepath))
9557
9558
check_replay = util.load_local_module("Tools/Replay/check_replay.py")
9559
9560
ok = check_replay.check_log(replay_log_filepath, self.progress, verbose=True)
9561
if not ok:
9562
raise NotAchievedException("check_replay (%s) failed" % current_log_filepath)
9563
9564
def DefaultIntervalsFromFiles(self):
9565
'''Test setting default mavlink message intervals from files'''
9566
ex = None
9567
intervals_filepath = util.reltopdir("message-intervals-chan0.txt")
9568
self.progress("Using filepath (%s)" % intervals_filepath)
9569
try:
9570
with open(intervals_filepath, "w") as f:
9571
f.write("""30 50
9572
28 100
9573
29 200
9574
""")
9575
f.close()
9576
9577
# other tests may have explicitly set rates, so wipe parameters:
9578
def custom_stream_rate_setter():
9579
for stream in mavutil.mavlink.MAV_DATA_STREAM_EXTRA3, mavutil.mavlink.MAV_DATA_STREAM_RC_CHANNELS:
9580
self.set_streamrate(5, stream=stream)
9581
9582
self.customise_SITL_commandline(
9583
[],
9584
wipe=True,
9585
set_streamrate_callback=custom_stream_rate_setter,
9586
)
9587
9588
self.assert_message_rate_hz("ATTITUDE", 20)
9589
self.assert_message_rate_hz("SCALED_PRESSURE", 5)
9590
9591
except Exception as e:
9592
self.print_exception_caught(e)
9593
ex = e
9594
9595
os.unlink(intervals_filepath)
9596
9597
self.reboot_sitl()
9598
9599
if ex is not None:
9600
raise ex
9601
9602
def BaroDrivers(self):
9603
'''Test Baro Drivers'''
9604
sensors = [
9605
("MS5611", 2),
9606
]
9607
for (name, bus) in sensors:
9608
self.context_push()
9609
if bus is not None:
9610
self.set_parameter("BARO_EXT_BUS", bus)
9611
self.set_parameter("BARO_PROBE_EXT", 1 << 2)
9612
self.reboot_sitl()
9613
self.wait_ready_to_arm()
9614
self.arm_vehicle()
9615
9616
# insert listener to compare airspeeds:
9617
messages = [None, None, None]
9618
9619
global count
9620
count = 0
9621
9622
def check_pressure(mav, m):
9623
global count
9624
m_type = m.get_type()
9625
count += 1
9626
# if count > 500:
9627
# if press_abs[0] is None or press_abs[1] is None:
9628
# raise NotAchievedException("Not receiving messages")
9629
if m_type == 'SCALED_PRESSURE3':
9630
off = 2
9631
elif m_type == 'SCALED_PRESSURE2':
9632
off = 1
9633
elif m_type == 'SCALED_PRESSURE':
9634
off = 0
9635
else:
9636
return
9637
9638
messages[off] = m
9639
9640
if None in messages:
9641
return
9642
first = messages[0]
9643
for msg in messages[1:]:
9644
delta_press_abs = abs(first.press_abs - msg.press_abs)
9645
if delta_press_abs > 0.5: # 50 Pa leeway
9646
raise NotAchievedException("Press_Abs mismatch (press1=%s press2=%s)" % (first, msg))
9647
delta_temperature = abs(first.temperature - msg.temperature)
9648
if delta_temperature > 300: # that's 3-degrees leeway
9649
raise NotAchievedException("Temperature mismatch (t1=%s t2=%s)" % (first, msg))
9650
self.install_message_hook_context(check_pressure)
9651
self.fly_mission("copter_mission.txt", strict=False)
9652
if None in messages:
9653
raise NotAchievedException("Missing a message")
9654
9655
self.context_pop()
9656
self.reboot_sitl()
9657
9658
def PositionWhenGPSIsZero(self):
9659
'''Ensure position doesn't zero when GPS lost'''
9660
# https://github.com/ArduPilot/ardupilot/issues/14236
9661
self.progress("arm the vehicle and takeoff in Guided")
9662
self.takeoff(20, mode='GUIDED')
9663
self.progress("fly 50m North (or whatever)")
9664
old_pos = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
9665
self.fly_guided_move_global_relative_alt(50, 0, 20)
9666
self.set_parameter('GPS1_TYPE', 0)
9667
self.drain_mav()
9668
tstart = self.get_sim_time()
9669
while True:
9670
if self.get_sim_time_cached() - tstart > 30 and self.mode_is('LAND'):
9671
self.progress("Bug not reproduced")
9672
break
9673
m = self.assert_receive_message('GLOBAL_POSITION_INT', timeout=1, verbose=True)
9674
pos_delta = self.get_distance_int(old_pos, m)
9675
self.progress("Distance: %f" % pos_delta)
9676
if pos_delta < 5:
9677
raise NotAchievedException("Bug reproduced - returned to near origin")
9678
self.wait_disarmed()
9679
self.reboot_sitl()
9680
9681
def SMART_RTL(self):
9682
'''Check SMART_RTL'''
9683
self.progress("arm the vehicle and takeoff in Guided")
9684
self.takeoff(20, mode='GUIDED')
9685
self.progress("fly around a bit (or whatever)")
9686
locs = [
9687
(50, 0, 20),
9688
(-50, 50, 20),
9689
(-50, 0, 20),
9690
]
9691
for (lat, lng, alt) in locs:
9692
self.fly_guided_move_local(lat, lng, alt)
9693
9694
self.change_mode('SMART_RTL')
9695
for (lat, lng, alt) in reversed(locs):
9696
self.wait_distance_to_local_position(
9697
(lat, lng, -alt),
9698
0,
9699
10,
9700
timeout=60
9701
)
9702
self.wait_disarmed()
9703
9704
def get_ground_effect_duration_from_current_onboard_log(self, bit, ignore_multi=False):
9705
'''returns a duration in seconds we were expecting to interact with
9706
the ground. Will die if there's more than one such block of
9707
time and ignore_multi is not set (will return first duration
9708
otherwise)
9709
'''
9710
ret = []
9711
dfreader = self.dfreader_for_current_onboard_log()
9712
seen_expected_start_TimeUS = None
9713
first = None
9714
last = None
9715
while True:
9716
m = dfreader.recv_match(type="XKF4")
9717
if m is None:
9718
break
9719
last = m
9720
if first is None:
9721
first = m
9722
# self.progress("%s" % str(m))
9723
expected = m.SS & (1 << bit)
9724
if expected:
9725
if seen_expected_start_TimeUS is None:
9726
seen_expected_start_TimeUS = m.TimeUS
9727
continue
9728
else:
9729
if seen_expected_start_TimeUS is not None:
9730
duration = (m.TimeUS - seen_expected_start_TimeUS)/1000000.0
9731
ret.append(duration)
9732
seen_expected_start_TimeUS = None
9733
if seen_expected_start_TimeUS is not None:
9734
duration = (last.TimeUS - seen_expected_start_TimeUS)/1000000.0
9735
ret.append(duration)
9736
return ret
9737
9738
def get_takeoffexpected_durations_from_current_onboard_log(self, ignore_multi=False):
9739
return self.get_ground_effect_duration_from_current_onboard_log(11, ignore_multi=ignore_multi)
9740
9741
def get_touchdownexpected_durations_from_current_onboard_log(self, ignore_multi=False):
9742
return self.get_ground_effect_duration_from_current_onboard_log(12, ignore_multi=ignore_multi)
9743
9744
def ThrowDoubleDrop(self):
9745
'''Test a more complicated drop-mode scenario'''
9746
self.progress("Getting a lift to altitude")
9747
self.set_parameters({
9748
"SIM_SHOVE_Z": -11,
9749
"THROW_TYPE": 1, # drop
9750
"MOT_SPOOL_TIME": 2,
9751
})
9752
self.change_mode('THROW')
9753
self.wait_ready_to_arm()
9754
self.arm_vehicle()
9755
try:
9756
self.set_parameter("SIM_SHOVE_TIME", 30000)
9757
except ValueError:
9758
# the shove resets this to zero
9759
pass
9760
9761
self.wait_altitude(100, 1000, timeout=100, relative=True)
9762
self.context_collect('STATUSTEXT')
9763
self.wait_statustext("throw detected - spooling motors", check_context=True, timeout=10)
9764
self.wait_statustext("throttle is unlimited - uprighting", check_context=True)
9765
self.wait_statustext("uprighted - controlling height", check_context=True)
9766
self.wait_statustext("height achieved - controlling position", check_context=True)
9767
self.progress("Waiting for still")
9768
self.wait_speed_vector(Vector3(0, 0, 0))
9769
self.change_mode('ALT_HOLD')
9770
self.set_rc(3, 1000)
9771
self.wait_disarmed(timeout=90)
9772
self.zero_throttle()
9773
9774
self.progress("second flight")
9775
self.upload_square_mission_items_around_location(self.poll_home_position())
9776
9777
self.set_parameters({
9778
"THROW_NEXTMODE": 3, # auto
9779
})
9780
9781
self.change_mode('THROW')
9782
self.wait_ready_to_arm()
9783
self.arm_vehicle()
9784
try:
9785
self.set_parameter("SIM_SHOVE_TIME", 30000)
9786
except ValueError:
9787
# the shove resets this to zero
9788
pass
9789
9790
self.wait_altitude(100, 1000, timeout=100, relative=True)
9791
self.wait_statustext("throw detected - spooling motors", check_context=True, timeout=10)
9792
self.wait_statustext("throttle is unlimited - uprighting", check_context=True)
9793
self.wait_statustext("uprighted - controlling height", check_context=True)
9794
self.wait_statustext("height achieved - controlling position", check_context=True)
9795
self.wait_mode('AUTO')
9796
self.wait_disarmed(timeout=240)
9797
9798
def GroundEffectCompensation_takeOffExpected(self):
9799
'''Test EKF's handling of takeoff-expected'''
9800
self.change_mode('ALT_HOLD')
9801
self.set_parameter("LOG_FILE_DSRMROT", 1)
9802
self.progress("Making sure we'll have a short log to look at")
9803
self.wait_ready_to_arm()
9804
self.arm_vehicle()
9805
self.disarm_vehicle()
9806
9807
# arm the vehicle and let it disarm normally. This should
9808
# yield a log where the EKF considers a takeoff imminent until
9809
# disarm
9810
self.start_subtest("Check ground effect compensation remains set in EKF while we're at idle on the ground")
9811
self.arm_vehicle()
9812
self.wait_disarmed()
9813
9814
durations = self.get_takeoffexpected_durations_from_current_onboard_log()
9815
duration = durations[0]
9816
want = 9
9817
self.progress("takeoff-expected duration: %fs" % (duration,))
9818
if duration < want: # assumes default 10-second DISARM_DELAY
9819
raise NotAchievedException("Should have been expecting takeoff for longer than %fs (want>%f)" %
9820
(duration, want))
9821
9822
self.start_subtest("takeoffExpected should be false very soon after we launch into the air")
9823
self.takeoff(mode='ALT_HOLD', alt_min=5)
9824
self.change_mode('LAND')
9825
self.wait_disarmed()
9826
durations = self.get_takeoffexpected_durations_from_current_onboard_log(ignore_multi=True)
9827
self.progress("touchdown-durations: %s" % str(durations))
9828
duration = durations[0]
9829
self.progress("takeoff-expected-duration %f" % (duration,))
9830
want_lt = 5
9831
if duration >= want_lt:
9832
raise NotAchievedException("Was expecting takeoff for longer than expected; got=%f want<=%f" %
9833
(duration, want_lt))
9834
9835
def _MAV_CMD_CONDITION_YAW(self, command):
9836
self.start_subtest("absolute")
9837
self.takeoff(20, mode='GUIDED')
9838
9839
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
9840
initial_heading = m.heading
9841
9842
self.progress("Ensuring initial heading is steady")
9843
target = initial_heading
9844
command(
9845
mavutil.mavlink.MAV_CMD_CONDITION_YAW,
9846
p1=target, # target angle
9847
p2=10, # degrees/second
9848
p3=1, # -1 is counter-clockwise, 1 clockwise
9849
p4=0, # 1 for relative, 0 for absolute
9850
)
9851
self.wait_heading(target, minimum_duration=2, timeout=50)
9852
self.wait_yaw_speed(0)
9853
9854
degsecond = 2
9855
9856
def rate_watcher(mav, m):
9857
if m.get_type() != 'ATTITUDE':
9858
return
9859
if abs(math.degrees(m.yawspeed)) > 5*degsecond:
9860
raise NotAchievedException("Moved too fast (%f>%f)" %
9861
(math.degrees(m.yawspeed), 5*degsecond))
9862
self.install_message_hook_context(rate_watcher)
9863
self.progress("Yaw CW 60 degrees")
9864
target = initial_heading + 60
9865
part_way_target = initial_heading + 10
9866
command(
9867
mavutil.mavlink.MAV_CMD_CONDITION_YAW,
9868
p1=target, # target angle
9869
p2=degsecond, # degrees/second
9870
p3=1, # -1 is counter-clockwise, 1 clockwise
9871
p4=0, # 1 for relative, 0 for absolute
9872
)
9873
self.wait_heading(part_way_target)
9874
self.wait_heading(target, minimum_duration=2)
9875
9876
self.progress("Yaw CCW 60 degrees")
9877
target = initial_heading
9878
part_way_target = initial_heading + 30
9879
command(
9880
mavutil.mavlink.MAV_CMD_CONDITION_YAW,
9881
p1=target, # target angle
9882
p2=degsecond, # degrees/second
9883
p3=-1, # -1 is counter-clockwise, 1 clockwise
9884
p4=0, # 1 for relative, 0 for absolute
9885
)
9886
self.wait_heading(part_way_target)
9887
self.wait_heading(target, minimum_duration=2)
9888
9889
self.disarm_vehicle(force=True)
9890
self.reboot_sitl()
9891
9892
def MAV_CMD_CONDITION_YAW(self):
9893
'''Test response to MAV_CMD_CONDITION_YAW via mavlink'''
9894
self.context_push()
9895
self._MAV_CMD_CONDITION_YAW(self.run_cmd_int)
9896
self.context_pop()
9897
self.context_push()
9898
self._MAV_CMD_CONDITION_YAW(self.run_cmd)
9899
self.context_pop()
9900
9901
def GroundEffectCompensation_touchDownExpected(self):
9902
'''Test EKF's handling of touchdown-expected'''
9903
self.zero_throttle()
9904
self.change_mode('ALT_HOLD')
9905
self.set_parameter("LOG_FILE_DSRMROT", 1)
9906
self.progress("Making sure we'll have a short log to look at")
9907
self.wait_ready_to_arm()
9908
self.arm_vehicle()
9909
self.disarm_vehicle()
9910
9911
self.start_subtest("Make sure touchdown-expected duration is about right")
9912
self.takeoff(20, mode='ALT_HOLD')
9913
self.change_mode('LAND')
9914
self.wait_disarmed()
9915
9916
durations = self.get_touchdownexpected_durations_from_current_onboard_log(ignore_multi=True)
9917
self.progress("touchdown-durations: %s" % str(durations))
9918
duration = durations[-1]
9919
expected = 23 # this is the time in the final descent phase of LAND
9920
if abs(duration - expected) > 5:
9921
raise NotAchievedException("Was expecting roughly %fs of touchdown expected, got %f" % (expected, duration))
9922
9923
def upload_square_mission_items_around_location(self, loc):
9924
alt = 20
9925
loc.alt = alt
9926
items = [
9927
(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, alt)
9928
]
9929
9930
for (ofs_n, ofs_e) in (20, 20), (20, -20), (-20, -20), (-20, 20), (20, 20):
9931
items.append((mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, ofs_n, ofs_e, alt))
9932
9933
items.append((mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0))
9934
9935
self.upload_simple_relhome_mission(items)
9936
9937
def RefindGPS(self):
9938
'''Refind the GPS and attempt to RTL rather than continue to land'''
9939
# https://github.com/ArduPilot/ardupilot/issues/14236
9940
self.progress("arm the vehicle and takeoff in Guided")
9941
self.takeoff(50, mode='GUIDED')
9942
self.progress("fly 50m North (or whatever)")
9943
old_pos = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
9944
self.fly_guided_move_global_relative_alt(50, 0, 50)
9945
self.set_parameter('GPS1_TYPE', 0)
9946
self.drain_mav()
9947
tstart = self.get_sim_time()
9948
while True:
9949
if self.get_sim_time_cached() - tstart > 30 and self.mode_is('LAND'):
9950
self.progress("Bug not reproduced")
9951
break
9952
m = self.assert_receive_message('GLOBAL_POSITION_INT', timeout=1, verbose=True)
9953
pos_delta = self.get_distance_int(old_pos, m)
9954
self.progress("Distance: %f" % pos_delta)
9955
if pos_delta < 5:
9956
raise NotAchievedException("Bug reproduced - returned to near origin")
9957
self.set_parameter('GPS1_TYPE', 1)
9958
self.do_RTL()
9959
9960
def GPSForYaw(self):
9961
'''Moving baseline GPS yaw'''
9962
self.load_default_params_file("copter-gps-for-yaw.parm")
9963
self.reboot_sitl()
9964
9965
self.wait_gps_fix_type_gte(6, message_type="GPS2_RAW", verbose=True)
9966
m = self.assert_receive_message("GPS2_RAW", very_verbose=True)
9967
want = 27000
9968
if abs(m.yaw - want) > 500:
9969
raise NotAchievedException("Expected to get GPS-from-yaw (want %f got %f)" % (want, m.yaw))
9970
self.wait_ready_to_arm()
9971
9972
def SMART_RTL_EnterLeave(self):
9973
'''check SmartRTL behaviour when entering/leaving'''
9974
# we had a bug where we would consume points when re-entering smartrtl
9975
9976
self.upload_simple_relhome_mission([
9977
# N E U
9978
(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 10),
9979
(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),
9980
])
9981
self.set_parameter('AUTO_OPTIONS', 3)
9982
self.change_mode('AUTO')
9983
self.wait_ready_to_arm()
9984
self.change_mode('ALT_HOLD')
9985
self.change_mode('SMART_RTL')
9986
self.change_mode('ALT_HOLD')
9987
self.change_mode('SMART_RTL')
9988
9989
def SMART_RTL_Repeat(self):
9990
'''Test whether Smart RTL catches the repeat'''
9991
self.takeoff(alt_min=10, mode='GUIDED')
9992
self.set_rc(3, 1500)
9993
self.change_mode("CIRCLE")
9994
self.delay_sim_time(1300)
9995
self.change_mode("SMART_RTL")
9996
self.wait_disarmed()
9997
9998
def GPSForYawCompassLearn(self):
9999
'''Moving baseline GPS yaw - with compass learning'''
10000
self.context_push()
10001
self.load_default_params_file("copter-gps-for-yaw.parm")
10002
self.set_parameter("EK3_SRC1_YAW", 3) # GPS with compass fallback
10003
self.reboot_sitl()
10004
10005
self.wait_gps_fix_type_gte(6, message_type="GPS2_RAW", verbose=True)
10006
10007
self.wait_ready_to_arm()
10008
10009
self.takeoff(10, mode='GUIDED')
10010
tstart = self.get_sim_time()
10011
compass_learn_set = False
10012
while True:
10013
delta_t = self.get_sim_time_cached() - tstart
10014
if delta_t > 30:
10015
break
10016
if not compass_learn_set and delta_t > 10:
10017
self.set_parameter("COMPASS_LEARN", 3)
10018
compass_learn_set = True
10019
10020
self.check_attitudes_match()
10021
self.delay_sim_time(1)
10022
10023
self.context_pop()
10024
self.reboot_sitl()
10025
10026
def AP_Avoidance(self):
10027
'''ADSB-based avoidance'''
10028
self.set_parameters({
10029
"AVD_ENABLE": 1,
10030
"ADSB_TYPE": 1, # mavlink
10031
"AVD_F_ACTION": 2, # climb or descend
10032
})
10033
self.reboot_sitl()
10034
10035
self.wait_ready_to_arm()
10036
10037
here = self.mav.location()
10038
10039
self.context_push()
10040
10041
self.start_subtest("F_ALT_MIN zero - disabled, can't arm in face of threat")
10042
self.set_parameters({
10043
"AVD_F_ALT_MIN": 0,
10044
})
10045
self.wait_ready_to_arm()
10046
self.test_adsb_send_threatening_adsb_message(here)
10047
self.delay_sim_time(1)
10048
self.try_arm(result=False,
10049
expect_msg="ADSB threat detected")
10050
10051
self.wait_ready_to_arm(timeout=60)
10052
10053
self.context_pop()
10054
10055
self.start_subtest("F_ALT_MIN 16m relative - arm in face of threat")
10056
self.context_push()
10057
self.set_parameters({
10058
"AVD_F_ALT_MIN": int(16 + here.alt),
10059
})
10060
self.wait_ready_to_arm()
10061
self.test_adsb_send_threatening_adsb_message(here)
10062
# self.delay_sim_time(1)
10063
self.arm_vehicle()
10064
self.disarm_vehicle()
10065
self.context_pop()
10066
10067
def PAUSE_CONTINUE(self):
10068
'''Test MAV_CMD_DO_PAUSE_CONTINUE in AUTO mode'''
10069
self.load_mission(filename="copter_mission.txt", strict=False)
10070
self.set_parameter(name="AUTO_OPTIONS", value=3)
10071
self.change_mode(mode="AUTO")
10072
self.wait_ready_to_arm()
10073
self.arm_vehicle()
10074
10075
self.wait_current_waypoint(wpnum=3, timeout=500)
10076
self.send_pause_command()
10077
self.wait_groundspeed(speed_min=0, speed_max=1, minimum_duration=5)
10078
self.send_resume_command()
10079
10080
self.wait_current_waypoint(wpnum=4, timeout=500)
10081
self.send_pause_command()
10082
self.wait_groundspeed(speed_min=0, speed_max=1, minimum_duration=5)
10083
self.send_resume_command()
10084
10085
# sending a pause, or resume, to the aircraft twice, doesn't result in reporting a failure
10086
self.wait_current_waypoint(wpnum=5, timeout=500)
10087
self.send_pause_command()
10088
self.send_pause_command()
10089
self.wait_groundspeed(speed_min=0, speed_max=1, minimum_duration=5)
10090
self.send_resume_command()
10091
self.send_resume_command()
10092
10093
self.wait_disarmed(timeout=500)
10094
10095
def PAUSE_CONTINUE_GUIDED(self):
10096
'''Test MAV_CMD_DO_PAUSE_CONTINUE in GUIDED mode'''
10097
self.start_subtest("Started test for Pause/Continue in GUIDED mode with LOCATION!")
10098
self.change_mode(mode="GUIDED")
10099
self.wait_ready_to_arm()
10100
self.arm_vehicle()
10101
self.set_parameter(name="GUID_TIMEOUT", value=120)
10102
self.user_takeoff(alt_min=30)
10103
10104
# send vehicle to global position target
10105
location = self.home_relative_loc_ne(n=300, e=0)
10106
target_typemask = MAV_POS_TARGET_TYPE_MASK.POS_ONLY
10107
self.mav.mav.set_position_target_global_int_send(
10108
0, # timestamp
10109
1, # target system_id
10110
1, # target component id
10111
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, # relative altitude frame
10112
target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE, # target typemask as pos only
10113
int(location.lat * 1e7), # lat
10114
int(location.lng * 1e7), # lon
10115
30, # alt
10116
0, # vx
10117
0, # vy
10118
0, # vz
10119
0, # afx
10120
0, # afy
10121
0, # afz
10122
0, # yaw
10123
0) # yawrate
10124
10125
self.wait_distance_to_home(distance_min=100, distance_max=150, timeout=120)
10126
self.send_pause_command()
10127
self.wait_groundspeed(speed_min=0, speed_max=1, minimum_duration=5)
10128
self.send_resume_command()
10129
self.wait_location(loc=location, timeout=120)
10130
10131
self.end_subtest("Ended test for Pause/Continue in GUIDED mode with LOCATION!")
10132
self.start_subtest("Started test for Pause/Continue in GUIDED mode with DESTINATION!")
10133
self.guided_achieve_heading(heading=270)
10134
10135
# move vehicle on x direction
10136
location = self.offset_location_ne(location=self.mav.location(), metres_north=0, metres_east=-300)
10137
self.mav.mav.set_position_target_global_int_send(
10138
0, # system time in milliseconds
10139
1, # target system
10140
1, # target component
10141
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, # coordinate frame MAV_FRAME_BODY_NED
10142
MAV_POS_TARGET_TYPE_MASK.POS_ONLY, # type mask (pos only)
10143
int(location.lat*1e7), # position x
10144
int(location.lng*1e7), # position y
10145
30, # position z
10146
0, # velocity x
10147
0, # velocity y
10148
0, # velocity z
10149
0, # accel x
10150
0, # accel y
10151
0, # accel z
10152
0, # yaw
10153
0) # yaw rate
10154
10155
self.wait_location(loc=location, accuracy=200, timeout=120)
10156
self.send_pause_command()
10157
self.wait_groundspeed(speed_min=0, speed_max=1, minimum_duration=5)
10158
self.send_resume_command()
10159
self.wait_location(loc=location, timeout=120)
10160
10161
self.end_subtest("Ended test for Pause/Continue in GUIDED mode with DESTINATION!")
10162
self.start_subtest("Started test for Pause/Continue in GUIDED mode with VELOCITY!")
10163
10164
# give velocity command
10165
vx, vy, vz_up = (5, 5, 0)
10166
self.test_guided_local_velocity_target(vx=vx, vy=vy, vz_up=vz_up, timeout=10)
10167
10168
self.wait_for_local_velocity(vx=vx, vy=vy, vz_up=vz_up, timeout=10)
10169
self.send_pause_command()
10170
self.wait_for_local_velocity(vx=0, vy=0, vz_up=0, timeout=10)
10171
self.send_resume_command()
10172
self.wait_for_local_velocity(vx=vx, vy=vy, vz_up=vz_up, timeout=10)
10173
self.test_guided_local_velocity_target(vx=0, vy=0, vz_up=0, timeout=10)
10174
self.wait_for_local_velocity(vx=0, vy=0, vz_up=0, timeout=10)
10175
10176
self.end_subtest("Ended test for Pause/Continue in GUIDED mode with VELOCITY!")
10177
self.start_subtest("Started test for Pause/Continue in GUIDED mode with ACCELERATION!")
10178
10179
# give acceleration command
10180
ax, ay, az_up = (1, 1, 0)
10181
target_typemask = (MAV_POS_TARGET_TYPE_MASK.POS_IGNORE | MAV_POS_TARGET_TYPE_MASK.VEL_IGNORE |
10182
MAV_POS_TARGET_TYPE_MASK.YAW_IGNORE | MAV_POS_TARGET_TYPE_MASK.YAW_RATE_IGNORE)
10183
self.mav.mav.set_position_target_local_ned_send(
10184
0, # timestamp
10185
1, # target system_id
10186
1, # target component id
10187
mavutil.mavlink.MAV_FRAME_LOCAL_NED,
10188
target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE,
10189
0, # x
10190
0, # y
10191
0, # z
10192
0, # vx
10193
0, # vy
10194
0, # vz
10195
ax, # afx
10196
ay, # afy
10197
-az_up, # afz
10198
0, # yaw
10199
0, # yawrate
10200
)
10201
10202
self.wait_for_local_velocity(vx=5, vy=5, vz_up=0, timeout=10)
10203
self.send_pause_command()
10204
self.wait_for_local_velocity(vx=0, vy=0, vz_up=0, timeout=10)
10205
self.send_resume_command()
10206
self.wait_for_local_velocity(vx=5, vy=5, vz_up=0, timeout=10)
10207
self.test_guided_local_velocity_target(vx=0, vy=0, vz_up=0, timeout=10)
10208
self.wait_for_local_velocity(vx=0, vy=0, vz_up=0, timeout=10)
10209
self.end_subtest("Ended test for Pause/Continue in GUIDED mode with ACCELERATION!")
10210
10211
# start pause/continue subtest with posvelaccel
10212
self.start_subtest("Started test for Pause/Continue in GUIDED mode with POSITION and VELOCITY and ACCELERATION!")
10213
self.guided_achieve_heading(heading=0)
10214
10215
# give posvelaccel command
10216
x, y, z_up = (-300, 0, 30)
10217
target_typemask = (MAV_POS_TARGET_TYPE_MASK.YAW_IGNORE | MAV_POS_TARGET_TYPE_MASK.YAW_RATE_IGNORE)
10218
self.mav.mav.set_position_target_local_ned_send(
10219
0, # timestamp
10220
1, # target system_id
10221
1, # target component id
10222
mavutil.mavlink.MAV_FRAME_LOCAL_NED,
10223
target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE,
10224
x, # x
10225
y, # y
10226
-z_up, # z
10227
0, # vx
10228
0, # vy
10229
0, # vz
10230
0, # afx
10231
0, # afy
10232
0, # afz
10233
0, # yaw
10234
0, # yawrate
10235
)
10236
10237
self.wait_distance_to_local_position(local_position=(x, y, -z_up), distance_min=400, distance_max=450, timeout=120)
10238
self.send_pause_command()
10239
self.wait_for_local_velocity(0, 0, 0, timeout=10)
10240
self.send_resume_command()
10241
self.wait_distance_to_local_position(local_position=(x, y, -z_up), distance_min=0, distance_max=10, timeout=120)
10242
10243
self.end_subtest("Ended test for Pause/Continue in GUIDED mode with POSITION and VELOCITY and ACCELERATION!")
10244
self.do_RTL(timeout=120)
10245
10246
def DO_CHANGE_SPEED(self):
10247
'''Change speed during misison using waypoint items'''
10248
self.load_mission("mission.txt", strict=False)
10249
10250
self.set_parameters({
10251
"AUTO_OPTIONS": 3,
10252
"ANGLE_MAX": 4500,
10253
})
10254
10255
self.change_mode('AUTO')
10256
self.wait_ready_to_arm()
10257
self.arm_vehicle()
10258
10259
self.wait_current_waypoint(1)
10260
self.wait_groundspeed(
10261
3.5, 4.5,
10262
minimum_duration=5,
10263
timeout=60,
10264
)
10265
10266
self.wait_current_waypoint(3)
10267
self.wait_groundspeed(
10268
14.5, 15.5,
10269
minimum_duration=10,
10270
timeout=60,
10271
)
10272
10273
self.wait_current_waypoint(5)
10274
self.wait_groundspeed(
10275
9.5, 11.5,
10276
minimum_duration=10,
10277
timeout=60,
10278
)
10279
10280
self.set_parameter("ANGLE_MAX", 6000)
10281
self.wait_current_waypoint(7)
10282
self.wait_groundspeed(
10283
15.5, 16.5,
10284
minimum_duration=10,
10285
timeout=60,
10286
)
10287
10288
self.wait_disarmed()
10289
10290
def AUTO_LAND_TO_BRAKE(self):
10291
'''ensure terrain altitude is taken into account when braking'''
10292
self.set_parameters({
10293
"PLND_ACC_P_NSE": 2.500000,
10294
"PLND_ALT_MAX": 8.000000,
10295
"PLND_ALT_MIN": 0.750000,
10296
"PLND_BUS": -1,
10297
"PLND_CAM_POS_X": 0.000000,
10298
"PLND_CAM_POS_Y": 0.000000,
10299
"PLND_CAM_POS_Z": 0.000000,
10300
"PLND_ENABLED": 1,
10301
"PLND_EST_TYPE": 1,
10302
"PLND_LAG": 0.020000,
10303
"PLND_LAND_OFS_X": 0.000000,
10304
"PLND_LAND_OFS_Y": 0.000000,
10305
"PLND_OPTIONS": 0,
10306
"PLND_RET_BEHAVE": 0,
10307
"PLND_RET_MAX": 4,
10308
"PLND_STRICT": 1,
10309
"PLND_TIMEOUT": 4.000000,
10310
"PLND_TYPE": 4,
10311
"PLND_XY_DIST_MAX": 2.500000,
10312
"PLND_YAW_ALIGN": 0.000000,
10313
10314
"SIM_PLD_ALT_LMT": 15.000000,
10315
"SIM_PLD_DIST_LMT": 10.000000,
10316
"SIM_PLD_ENABLE": 1,
10317
"SIM_PLD_HEIGHT": 0,
10318
"SIM_PLD_LAT": -20.558929,
10319
"SIM_PLD_LON": -47.415035,
10320
"SIM_PLD_RATE": 100,
10321
"SIM_PLD_TYPE": 1,
10322
"SIM_PLD_YAW": 87,
10323
10324
"SIM_SONAR_SCALE": 12,
10325
})
10326
10327
self.set_analog_rangefinder_parameters()
10328
10329
self.load_mission('mission.txt')
10330
self.customise_SITL_commandline([
10331
"--home", self.sitl_home_string_from_mission("mission.txt"),
10332
])
10333
10334
self.set_parameter('AUTO_OPTIONS', 3)
10335
self.change_mode('AUTO')
10336
self.wait_ready_to_arm()
10337
self.arm_vehicle()
10338
10339
self.wait_current_waypoint(7)
10340
self.wait_altitude(10, 15, relative=True, timeout=60)
10341
self.change_mode('BRAKE')
10342
# monitor altitude here
10343
self.wait_altitude(10, 15, relative=True, minimum_duration=20)
10344
self.change_mode('AUTO')
10345
self.wait_disarmed()
10346
10347
def MAVLandedStateTakeoff(self):
10348
'''check EXTENDED_SYS_STATE message'''
10349
ex = None
10350
try:
10351
self.set_message_rate_hz(id=mavutil.mavlink.MAVLINK_MSG_ID_EXTENDED_SYS_STATE, rate_hz=1)
10352
self.wait_extended_sys_state(vtol_state=mavutil.mavlink.MAV_VTOL_STATE_MC,
10353
landed_state=mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND, timeout=10)
10354
self.load_mission(filename="copter_mission.txt")
10355
self.set_parameter(name="AUTO_OPTIONS", value=3)
10356
self.change_mode(mode="AUTO")
10357
self.wait_ready_to_arm()
10358
self.arm_vehicle()
10359
self.wait_extended_sys_state(vtol_state=mavutil.mavlink.MAV_VTOL_STATE_MC,
10360
landed_state=mavutil.mavlink.MAV_LANDED_STATE_TAKEOFF, timeout=30)
10361
self.wait_extended_sys_state(vtol_state=mavutil.mavlink.MAV_VTOL_STATE_MC,
10362
landed_state=mavutil.mavlink.MAV_LANDED_STATE_IN_AIR, timeout=60)
10363
self.land_and_disarm()
10364
except Exception as e:
10365
self.print_exception_caught(e)
10366
ex = e
10367
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_EXTENDED_SYS_STATE, -1)
10368
if ex is not None:
10369
raise ex
10370
10371
def ATTITUDE_FAST(self):
10372
'''ensure that when ATTITDE_FAST is set we get many messages'''
10373
self.context_push()
10374
ex = None
10375
try:
10376
old = self.get_parameter('LOG_BITMASK')
10377
new = int(old) | (1 << 0) # see defines.h
10378
self.set_parameters({
10379
"LOG_BITMASK": new,
10380
"LOG_DISARMED": 1,
10381
"LOG_DARM_RATEMAX": 0,
10382
"LOG_FILE_RATEMAX": 0,
10383
})
10384
path = self.generate_rate_sample_log()
10385
10386
except Exception as e:
10387
self.print_exception_caught(e)
10388
ex = e
10389
10390
self.context_pop()
10391
10392
self.reboot_sitl()
10393
10394
if ex is not None:
10395
raise ex
10396
10397
self.delay_sim_time(10) # NFI why this is required
10398
10399
self.check_dflog_message_rates(path, {
10400
'ANG': 400,
10401
})
10402
10403
def BaseLoggingRates(self):
10404
'''ensure messages come out at specific rates'''
10405
self.set_parameters({
10406
"LOG_DARM_RATEMAX": 0,
10407
"LOG_FILE_RATEMAX": 0,
10408
})
10409
path = self.generate_rate_sample_log()
10410
self.delay_sim_time(10) # NFI why this is required
10411
self.check_dflog_message_rates(path, {
10412
"ATT": 10,
10413
"IMU": 25,
10414
})
10415
10416
def FETtecESC_flight(self):
10417
'''fly with servo outputs from FETtec ESC'''
10418
self.start_subtest("FETtec ESC flight")
10419
num_wp = self.load_mission("copter_mission.txt", strict=False)
10420
self.fly_loaded_mission(num_wp)
10421
10422
def FETtecESC_esc_power_checks(self):
10423
'''Make sure state machine copes with ESCs rebooting'''
10424
self.start_subtest("FETtec ESC reboot")
10425
self.wait_ready_to_arm()
10426
self.context_collect('STATUSTEXT')
10427
self.progress("Turning off an ESC off ")
10428
mask = int(self.get_parameter("SIM_FTOWESC_POW"))
10429
10430
for mot_id_to_kill in 1, 2:
10431
self.progress("Turning ESC=%u off" % mot_id_to_kill)
10432
self.set_parameter("SIM_FTOWESC_POW", mask & ~(1 << mot_id_to_kill))
10433
self.delay_sim_time(1)
10434
self.assert_prearm_failure("are not running")
10435
self.progress("Turning it back on")
10436
self.set_parameter("SIM_FTOWESC_POW", mask)
10437
self.wait_ready_to_arm()
10438
10439
self.progress("Turning ESC=%u off (again)" % mot_id_to_kill)
10440
self.set_parameter("SIM_FTOWESC_POW", mask & ~(1 << mot_id_to_kill))
10441
self.delay_sim_time(1)
10442
self.assert_prearm_failure("are not running")
10443
self.progress("Turning it back on")
10444
self.set_parameter("SIM_FTOWESC_POW", mask)
10445
self.wait_ready_to_arm()
10446
10447
self.progress("Turning all ESCs off")
10448
self.set_parameter("SIM_FTOWESC_POW", 0)
10449
self.delay_sim_time(1)
10450
self.assert_prearm_failure("are not running")
10451
self.progress("Turning them back on")
10452
self.set_parameter("SIM_FTOWESC_POW", mask)
10453
self.wait_ready_to_arm()
10454
10455
def fettec_assert_bad_mask(self, mask):
10456
'''assert the mask is bad for fettec driver'''
10457
self.start_subsubtest("Checking mask (%s) is bad" % (mask,))
10458
self.context_push()
10459
self.set_parameter("SERVO_FTW_MASK", mask)
10460
self.reboot_sitl()
10461
self.delay_sim_time(12) # allow accels/gyros to be happy
10462
tstart = self.get_sim_time()
10463
while True:
10464
if self.get_sim_time_cached() - tstart > 20:
10465
raise NotAchievedException("Expected mask to be only problem within 20 seconds")
10466
try:
10467
self.assert_prearm_failure("Invalid motor mask")
10468
break
10469
except NotAchievedException:
10470
self.delay_sim_time(1)
10471
self.context_pop()
10472
self.reboot_sitl()
10473
10474
def fettec_assert_good_mask(self, mask):
10475
'''assert the mask is bad for fettec driver'''
10476
self.start_subsubtest("Checking mask (%s) is good" % (mask,))
10477
self.context_push()
10478
self.set_parameter("SERVO_FTW_MASK", mask)
10479
self.reboot_sitl()
10480
self.wait_ready_to_arm()
10481
self.context_pop()
10482
self.reboot_sitl()
10483
10484
def FETtecESC_safety_switch(self):
10485
mot = self.find_first_set_bit(int(self.get_parameter("SERVO_FTW_MASK"))) + 1
10486
self.wait_esc_telem_rpm(mot, 0, 0)
10487
self.wait_ready_to_arm()
10488
self.context_push()
10489
self.set_parameter("DISARM_DELAY", 0)
10490
self.arm_vehicle()
10491
# we have to wait for a while for the arming tone to go out
10492
# before the motors will spin:
10493
self.wait_esc_telem_rpm(
10494
esc=mot,
10495
rpm_min=17640,
10496
rpm_max=17640,
10497
minimum_duration=2,
10498
timeout=5,
10499
)
10500
self.set_safetyswitch_on()
10501
self.wait_esc_telem_rpm(mot, 0, 0)
10502
self.set_safetyswitch_off()
10503
self.wait_esc_telem_rpm(
10504
esc=mot,
10505
rpm_min=17640,
10506
rpm_max=17640,
10507
minimum_duration=2,
10508
timeout=5,
10509
)
10510
self.context_pop()
10511
self.wait_disarmed()
10512
10513
def FETtecESC_btw_mask_checks(self):
10514
'''ensure prearm checks work as expected'''
10515
for bad_mask in [0b1000000000000000, 0b10100000000000000]:
10516
self.fettec_assert_bad_mask(bad_mask)
10517
for good_mask in [0b00001, 0b00101, 0b110000000000]:
10518
self.fettec_assert_good_mask(good_mask)
10519
10520
def FETtecESC(self):
10521
'''Test FETtecESC'''
10522
self.set_parameters({
10523
"SERIAL5_PROTOCOL": 38,
10524
"SERVO_FTW_MASK": 0b11101000,
10525
"SIM_FTOWESC_ENA": 1,
10526
"SERVO1_FUNCTION": 0,
10527
"SERVO2_FUNCTION": 0,
10528
"SERVO3_FUNCTION": 0,
10529
"SERVO4_FUNCTION": 33,
10530
"SERVO5_FUNCTION": 0,
10531
"SERVO6_FUNCTION": 34,
10532
"SERVO7_FUNCTION": 35,
10533
"SERVO8_FUNCTION": 36,
10534
"SIM_ESC_TELEM": 0,
10535
})
10536
self.customise_SITL_commandline(["--serial5=sim:fetteconewireesc"])
10537
self.FETtecESC_safety_switch()
10538
self.FETtecESC_esc_power_checks()
10539
self.FETtecESC_btw_mask_checks()
10540
self.FETtecESC_flight()
10541
10542
def PerfInfo(self):
10543
'''Test Scheduler PerfInfo output'''
10544
self.set_parameter('SCHED_OPTIONS', 1) # enable gathering
10545
# sometimes we need to trigger collection....
10546
content = self.fetch_file_via_ftp("@SYS/tasks.txt")
10547
self.delay_sim_time(5)
10548
content = self.fetch_file_via_ftp("@SYS/tasks.txt")
10549
self.progress("Got content (%s)" % str(content))
10550
10551
lines = content.split("\n")
10552
10553
if not lines[0].startswith("TasksV2"):
10554
raise NotAchievedException("Expected TasksV2 as first line first not (%s)" % lines[0])
10555
# last line is empty, so -2 here
10556
if not lines[-2].startswith("AP_Vehicle::update_arming"):
10557
raise NotAchievedException("Expected EFI last not (%s)" % lines[-2])
10558
10559
def RTL_TO_RALLY(self, target_system=1, target_component=1):
10560
'''Check RTL to rally point'''
10561
self.wait_ready_to_arm()
10562
rally_loc = self.home_relative_loc_ne(50, -25)
10563
rally_alt = 37
10564
items = [
10565
self.mav.mav.mission_item_int_encode(
10566
target_system,
10567
target_component,
10568
0, # seq
10569
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
10570
mavutil.mavlink.MAV_CMD_NAV_RALLY_POINT,
10571
0, # current
10572
0, # autocontinue
10573
0, # p1
10574
0, # p2
10575
0, # p3
10576
0, # p4
10577
int(rally_loc.lat * 1e7), # latitude
10578
int(rally_loc.lng * 1e7), # longitude
10579
rally_alt, # altitude
10580
mavutil.mavlink.MAV_MISSION_TYPE_RALLY),
10581
]
10582
self.upload_using_mission_protocol(
10583
mavutil.mavlink.MAV_MISSION_TYPE_RALLY,
10584
items
10585
)
10586
self.set_parameters({
10587
'RALLY_INCL_HOME': 0,
10588
})
10589
self.takeoff(10)
10590
self.change_mode('RTL')
10591
self.wait_location(rally_loc)
10592
self.assert_altitude(rally_alt, relative=True)
10593
self.progress("Ensuring we're descending")
10594
self.wait_altitude(20, 25, relative=True)
10595
self.change_mode('LOITER')
10596
self.progress("Flying home")
10597
self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_RALLY)
10598
self.change_mode('RTL')
10599
self.wait_disarmed()
10600
self.assert_at_home()
10601
10602
def NoRCOnBootPreArmFailure(self):
10603
'''Ensure we can't arm with no RC on boot if THR_FS_VALUE set'''
10604
self.context_push()
10605
for rc_failure_mode in 1, 2:
10606
self.set_parameters({
10607
"SIM_RC_FAIL": rc_failure_mode,
10608
})
10609
self.reboot_sitl()
10610
if rc_failure_mode == 1:
10611
self.assert_prearm_failure("RC not found",
10612
other_prearm_failures_fatal=False)
10613
elif rc_failure_mode == 2:
10614
self.assert_prearm_failure("Throttle below failsafe",
10615
other_prearm_failures_fatal=False)
10616
self.context_pop()
10617
self.reboot_sitl()
10618
10619
def IMUConsistency(self):
10620
'''test IMUs must be consistent with one another'''
10621
self.wait_ready_to_arm()
10622
10623
self.start_subsubtest("prearm checks for accel inconsistency")
10624
self.context_push()
10625
self.set_parameters({
10626
"SIM_ACC1_BIAS_X": 5,
10627
})
10628
self.assert_prearm_failure("Accels inconsistent")
10629
self.context_pop()
10630
tstart = self.get_sim_time()
10631
self.wait_ready_to_arm()
10632
if self.get_sim_time() - tstart < 8:
10633
raise NotAchievedException("Should take 10 seconds to become armableafter IMU upset")
10634
10635
self.start_subsubtest("prearm checks for gyro inconsistency")
10636
self.context_push()
10637
self.set_parameters({
10638
"SIM_GYR1_BIAS_X": math.radians(10),
10639
})
10640
self.assert_prearm_failure("Gyros inconsistent")
10641
self.context_pop()
10642
tstart = self.get_sim_time()
10643
self.wait_ready_to_arm()
10644
if self.get_sim_time() - tstart < 8:
10645
raise NotAchievedException("Should take 10 seconds to become armableafter IMU upset")
10646
10647
def Sprayer(self):
10648
"""Test sprayer functionality."""
10649
self.context_push()
10650
10651
rc_ch = 9
10652
pump_ch = 5
10653
spinner_ch = 6
10654
pump_ch_min = 1050
10655
pump_ch_trim = 1520
10656
pump_ch_max = 1950
10657
spinner_ch_min = 975
10658
spinner_ch_trim = 1510
10659
spinner_ch_max = 1975
10660
10661
self.set_parameters({
10662
"SPRAY_ENABLE": 1,
10663
10664
"SERVO%u_FUNCTION" % pump_ch: 22,
10665
"SERVO%u_MIN" % pump_ch: pump_ch_min,
10666
"SERVO%u_TRIM" % pump_ch: pump_ch_trim,
10667
"SERVO%u_MAX" % pump_ch: pump_ch_max,
10668
10669
"SERVO%u_FUNCTION" % spinner_ch: 23,
10670
"SERVO%u_MIN" % spinner_ch: spinner_ch_min,
10671
"SERVO%u_TRIM" % spinner_ch: spinner_ch_trim,
10672
"SERVO%u_MAX" % spinner_ch: spinner_ch_max,
10673
10674
"SIM_SPR_ENABLE": 1,
10675
"SIM_SPR_PUMP": pump_ch,
10676
"SIM_SPR_SPIN": spinner_ch,
10677
10678
"RC%u_OPTION" % rc_ch: 15,
10679
"LOG_DISARMED": 1,
10680
})
10681
10682
self.reboot_sitl()
10683
10684
self.wait_ready_to_arm()
10685
self.arm_vehicle()
10686
10687
self.progress("test bootup state - it's zero-output!")
10688
self.wait_servo_channel_value(spinner_ch, 0)
10689
self.wait_servo_channel_value(pump_ch, 0)
10690
10691
self.progress("Enable sprayer")
10692
self.set_rc(rc_ch, 2000)
10693
10694
self.progress("Testing zero-speed state")
10695
self.wait_servo_channel_value(spinner_ch, spinner_ch_min)
10696
self.wait_servo_channel_value(pump_ch, pump_ch_min)
10697
10698
self.progress("Testing turning it off")
10699
self.set_rc(rc_ch, 1000)
10700
self.wait_servo_channel_value(spinner_ch, spinner_ch_min)
10701
self.wait_servo_channel_value(pump_ch, pump_ch_min)
10702
10703
self.progress("Testing turning it back on")
10704
self.set_rc(rc_ch, 2000)
10705
self.wait_servo_channel_value(spinner_ch, spinner_ch_min)
10706
self.wait_servo_channel_value(pump_ch, pump_ch_min)
10707
10708
self.takeoff(30, mode='LOITER')
10709
10710
self.progress("Testing speed-ramping")
10711
self.set_rc(1, 1700) # start driving forward
10712
10713
# this is somewhat empirical...
10714
self.wait_servo_channel_value(
10715
pump_ch,
10716
1458,
10717
timeout=60,
10718
comparator=lambda x, y : abs(x-y) < 5
10719
)
10720
10721
self.progress("Turning it off again")
10722
self.set_rc(rc_ch, 1000)
10723
self.wait_servo_channel_value(spinner_ch, spinner_ch_min)
10724
self.wait_servo_channel_value(pump_ch, pump_ch_min)
10725
10726
self.start_subtest("Checking mavlink commands")
10727
self.progress("Starting Sprayer")
10728
self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_SPRAYER, p1=1)
10729
10730
self.progress("Testing speed-ramping")
10731
self.wait_servo_channel_value(
10732
pump_ch,
10733
1458,
10734
timeout=60,
10735
comparator=lambda x, y : abs(x-y) < 5
10736
)
10737
10738
self.start_subtest("Stopping Sprayer")
10739
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SPRAYER, p1=0)
10740
10741
self.wait_servo_channel_value(pump_ch, pump_ch_min)
10742
10743
self.disarm_vehicle(force=True)
10744
10745
self.context_pop()
10746
10747
self.reboot_sitl()
10748
10749
self.progress("Sprayer OK")
10750
10751
def tests1a(self):
10752
'''return list of all tests'''
10753
ret = super(AutoTestCopter, self).tests() # about 5 mins and ~20 initial tests from autotest/vehicle_test_suite.py
10754
ret.extend([
10755
self.NavDelayTakeoffAbsTime,
10756
self.NavDelayAbsTime,
10757
self.NavDelay,
10758
self.GuidedSubModeChange,
10759
self.MAV_CMD_CONDITION_YAW,
10760
self.LoiterToAlt,
10761
self.PayloadPlaceMission,
10762
self.PrecisionLoiterCompanion,
10763
self.Landing,
10764
self.PrecisionLanding,
10765
self.SetModesViaModeSwitch,
10766
self.SetModesViaAuxSwitch,
10767
self.AuxSwitchOptions,
10768
self.AuxFunctionsInMission,
10769
self.AutoTune,
10770
self.AutoTuneYawD,
10771
self.NoRCOnBootPreArmFailure,
10772
])
10773
return ret
10774
10775
def tests1b(self):
10776
'''return list of all tests'''
10777
ret = ([
10778
self.ThrowMode,
10779
self.BrakeMode,
10780
self.RecordThenPlayMission,
10781
self.ThrottleFailsafe,
10782
self.ThrottleFailsafePassthrough,
10783
self.GCSFailsafe,
10784
self.CustomController,
10785
])
10786
return ret
10787
10788
def tests1c(self):
10789
'''return list of all tests'''
10790
ret = ([
10791
self.BatteryFailsafe,
10792
self.BatteryMissing,
10793
self.VibrationFailsafe,
10794
self.EK3AccelBias,
10795
self.StabilityPatch,
10796
self.OBSTACLE_DISTANCE_3D,
10797
self.AC_Avoidance_Proximity,
10798
self.AC_Avoidance_Proximity_AVOID_ALT_MIN,
10799
self.AC_Avoidance_Fence,
10800
self.AC_Avoidance_Beacon,
10801
self.AvoidanceAltFence,
10802
self.BaroWindCorrection,
10803
self.SetpointGlobalPos,
10804
self.ThrowDoubleDrop,
10805
self.SetpointGlobalVel,
10806
self.SetpointBadVel,
10807
self.SplineTerrain,
10808
self.TakeoffCheck,
10809
self.GainBackoffTakeoff,
10810
])
10811
return ret
10812
10813
def tests1d(self):
10814
'''return list of all tests'''
10815
ret = ([
10816
self.HorizontalFence,
10817
self.HorizontalAvoidFence,
10818
self.MaxAltFence,
10819
self.MaxAltFenceAvoid,
10820
self.MinAltFence,
10821
self.MinAltFenceAvoid,
10822
self.FenceFloorEnabledLanding,
10823
self.FenceFloorAutoDisableLanding,
10824
self.FenceFloorAutoEnableOnArming,
10825
self.AutoTuneSwitch,
10826
self.GPSGlitchLoiter,
10827
self.GPSGlitchLoiter2,
10828
self.GPSGlitchAuto,
10829
self.ModeAltHold,
10830
self.ModeLoiter,
10831
self.SimpleMode,
10832
self.SuperSimpleCircle,
10833
self.ModeCircle,
10834
self.MagFail,
10835
self.OpticalFlow,
10836
self.OpticalFlowLocation,
10837
self.OpticalFlowLimits,
10838
self.OpticalFlowCalibration,
10839
self.MotorFail,
10840
self.ModeFlip,
10841
self.CopterMission,
10842
self.TakeoffAlt,
10843
self.SplineLastWaypoint,
10844
self.Gripper,
10845
self.TestLocalHomePosition,
10846
self.TestGripperMission,
10847
self.VisionPosition,
10848
self.ATTITUDE_FAST,
10849
self.BaseLoggingRates,
10850
self.BodyFrameOdom,
10851
self.GPSViconSwitching,
10852
])
10853
return ret
10854
10855
def tests1e(self):
10856
'''return list of all tests'''
10857
ret = ([
10858
self.BeaconPosition,
10859
self.RTLSpeed,
10860
self.Mount,
10861
self.MountYawVehicleForMountROI,
10862
self.MAV_CMD_DO_MOUNT_CONTROL,
10863
self.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE,
10864
self.AutoYawDO_MOUNT_CONTROL,
10865
self.Button,
10866
self.ShipTakeoff,
10867
self.RangeFinder,
10868
self.BaroDrivers,
10869
self.SurfaceTracking,
10870
self.Parachute,
10871
self.ParameterChecks,
10872
self.ManualThrottleModeChange,
10873
self.MANUAL_CONTROL,
10874
self.ModeZigZag,
10875
self.PosHoldTakeOff,
10876
self.ModeFollow,
10877
self.RangeFinderDrivers,
10878
self.RangeFinderDriversMaxAlt,
10879
self.MaxBotixI2CXL,
10880
self.MAVProximity,
10881
self.ParameterValidation,
10882
self.AltTypes,
10883
self.PAUSE_CONTINUE,
10884
self.PAUSE_CONTINUE_GUIDED,
10885
self.RichenPower,
10886
self.IE24,
10887
self.MAVLandedStateTakeoff,
10888
self.Weathervane,
10889
self.MAV_CMD_AIRFRAME_CONFIGURATION,
10890
self.MAV_CMD_NAV_LOITER_UNLIM,
10891
self.MAV_CMD_NAV_RETURN_TO_LAUNCH,
10892
self.MAV_CMD_NAV_VTOL_LAND,
10893
self.clear_roi,
10894
])
10895
return ret
10896
10897
def tests2a(self):
10898
'''return list of all tests'''
10899
ret = ([
10900
# something about SITLCompassCalibration appears to fail
10901
# this one, so we put it first:
10902
self.FixedYawCalibration,
10903
10904
# we run this single 8min-and-40s test on its own, apart
10905
# from requiring FixedYawCalibration right before it
10906
# because without it, it fails to calibrate this
10907
# autotest appears to interfere with
10908
# FixedYawCalibration, no idea why.
10909
self.SITLCompassCalibration,
10910
])
10911
return ret
10912
10913
def ScriptMountPOI(self):
10914
'''test the MountPOI example script'''
10915
self.context_push()
10916
10917
self.install_terrain_handlers_context()
10918
self.set_parameters({
10919
"SCR_ENABLE": 1,
10920
"RC12_OPTION": 300,
10921
})
10922
self.setup_servo_mount()
10923
self.reboot_sitl()
10924
self.set_rc(6, 1300)
10925
self.install_applet_script_context('mount-poi.lua')
10926
self.reboot_sitl()
10927
self.wait_ready_to_arm()
10928
self.context_collect('STATUSTEXT')
10929
self.set_rc(12, 2000)
10930
self.wait_statustext('POI.*-35.*149', check_context=True, regex=True)
10931
self.set_rc(12, 1000)
10932
10933
self.context_pop()
10934
self.reboot_sitl()
10935
10936
def ScriptCopterPosOffsets(self):
10937
'''test the copter-posoffset.lua example script'''
10938
self.context_push()
10939
10940
# enable scripting and arming/takingoff in Auto mode
10941
self.set_parameters({
10942
"SCR_ENABLE": 1,
10943
"AUTO_OPTIONS": 3,
10944
"RC12_OPTION": 300
10945
})
10946
self.reboot_sitl()
10947
10948
# install copter-posoffset script
10949
self.install_example_script_context('copter-posoffset.lua')
10950
self.reboot_sitl()
10951
10952
# create simple mission with a single takeoff command
10953
self.upload_simple_relhome_mission([
10954
(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 20)
10955
])
10956
10957
# switch to loiter to wait for position estimate (aka GPS lock)
10958
self.change_mode('LOITER')
10959
self.wait_ready_to_arm()
10960
10961
# arm and takeoff in Auto mode
10962
self.change_mode('AUTO')
10963
self.arm_vehicle()
10964
10965
# wait for vehicle to climb to at least 10m
10966
self.wait_altitude(8, 12, relative=True)
10967
10968
# add position offset to East and confirm vehicle moves
10969
self.set_parameter("PSC_OFS_POS_E", 20)
10970
self.set_rc(12, 2000)
10971
self.wait_distance(18)
10972
10973
# remove position offset and wait for vehicle to return home
10974
self.set_parameter("PSC_OFS_POS_E", 0)
10975
self.wait_distance_to_home(distance_min=0, distance_max=4, timeout=20)
10976
10977
# add velocity offset and confirm vehicle moves
10978
self.set_parameter("PSC_OFS_VEL_N", 5)
10979
self.wait_groundspeed(4.8, 5.2, minimum_duration=5, timeout=20)
10980
10981
# remove velocity offset and switch to RTL
10982
self.set_parameter("PSC_OFS_VEL_N", 0)
10983
self.set_rc(12, 1000)
10984
self.do_RTL()
10985
self.context_pop()
10986
self.reboot_sitl()
10987
10988
def AHRSTrimLand(self):
10989
'''test land detector with significant AHRS trim'''
10990
self.context_push()
10991
self.set_parameters({
10992
"SIM_ACC_TRIM_X": 0.12,
10993
"AHRS_TRIM_X": 0.12,
10994
})
10995
self.reboot_sitl()
10996
self.wait_ready_to_arm()
10997
self.takeoff(alt_min=20, mode='LOITER')
10998
self.do_RTL()
10999
self.context_pop()
11000
self.reboot_sitl()
11001
11002
def GainBackoffTakeoff(self):
11003
'''test gain backoff on takeoff'''
11004
self.context_push()
11005
self.progress("Test gains are fully backed off")
11006
self.set_parameters({
11007
"ATC_LAND_R_MULT": 0.0,
11008
"ATC_LAND_P_MULT": 0.0,
11009
"ATC_LAND_Y_MULT": 0.0,
11010
"GCS_PID_MASK" : 7,
11011
"LOG_BITMASK": 180222,
11012
})
11013
self.reboot_sitl()
11014
self.wait_ready_to_arm()
11015
self.change_mode('ALT_HOLD')
11016
11017
class ValidatePDZero(vehicle_test_suite.TestSuite.MessageHook):
11018
'''asserts correct values in PID_TUNING'''
11019
11020
def __init__(self, suite, axis):
11021
super(ValidatePDZero, self).__init__(suite)
11022
self.pid_tuning_count = 0
11023
self.p_sum = 0
11024
self.d_sum = 0
11025
self.i_sum = 0
11026
self.axis = axis
11027
11028
def hook_removed(self):
11029
if self.pid_tuning_count == 0:
11030
raise NotAchievedException("Did not get PID_TUNING")
11031
if self.i_sum == 0:
11032
raise ValueError("I sum is zero")
11033
print(f"ValidatePDZero: PID_TUNING count: {self.pid_tuning_count}")
11034
11035
def process(self, mav, m):
11036
if m.get_type() != 'PID_TUNING' or m.axis != self.axis:
11037
return
11038
self.pid_tuning_count += 1
11039
self.p_sum += m.P
11040
self.d_sum += m.D
11041
self.i_sum += m.I
11042
if self.p_sum > 0:
11043
raise ValueError("P sum is not zero")
11044
if self.d_sum > 0:
11045
raise ValueError("D sum is not zero")
11046
11047
self.progress("Check that PD values are zero")
11048
self.install_message_hook_context(ValidatePDZero(self, mavutil.mavlink.PID_TUNING_ROLL))
11049
self.install_message_hook_context(ValidatePDZero(self, mavutil.mavlink.PID_TUNING_PITCH))
11050
self.install_message_hook_context(ValidatePDZero(self, mavutil.mavlink.PID_TUNING_YAW))
11051
# until the context pop happens, all received PID_TUNINGS will be verified as good
11052
self.arm_vehicle()
11053
self.set_rc(3, 1500)
11054
self.delay_sim_time(2)
11055
self.set_rc(2, 1250)
11056
self.delay_sim_time(5)
11057
self.assert_receive_message('PID_TUNING', timeout=5)
11058
self.set_rc_default()
11059
self.zero_throttle()
11060
self.disarm_vehicle()
11061
self.context_pop()
11062
11063
self.context_push()
11064
self.progress("Test gains are not backed off")
11065
self.set_parameters({
11066
"ATC_LAND_R_MULT": 1.0,
11067
"ATC_LAND_P_MULT": 1.0,
11068
"ATC_LAND_Y_MULT": 1.0,
11069
"GCS_PID_MASK" : 7,
11070
"LOG_BITMASK": 180222,
11071
})
11072
self.reboot_sitl()
11073
self.wait_ready_to_arm()
11074
self.change_mode('ALT_HOLD')
11075
11076
class ValidatePDNonZero(vehicle_test_suite.TestSuite.MessageHook):
11077
'''asserts correct values in PID_TUNING'''
11078
11079
def __init__(self, suite, axis):
11080
super(ValidatePDNonZero, self).__init__(suite)
11081
self.pid_tuning_count = 0
11082
self.p_sum = 0
11083
self.d_sum = 0
11084
self.i_sum = 0
11085
self.axis = axis
11086
11087
def hook_removed(self):
11088
if self.pid_tuning_count == 0:
11089
raise NotAchievedException("Did not get PID_TUNING")
11090
if self.p_sum == 0:
11091
raise ValueError("P sum is zero")
11092
if self.i_sum == 0:
11093
raise ValueError("I sum is zero")
11094
print(f"ValidatePDNonZero: PID_TUNING count: {self.pid_tuning_count}")
11095
11096
def process(self, mav, m):
11097
if m.get_type() != 'PID_TUNING' or m.axis != self.axis:
11098
return
11099
self.pid_tuning_count += 1
11100
self.p_sum += m.P
11101
self.d_sum += m.D
11102
self.i_sum += m.I
11103
11104
self.progress("Check that PD values are non-zero")
11105
self.install_message_hook_context(ValidatePDNonZero(self, mavutil.mavlink.PID_TUNING_ROLL))
11106
self.install_message_hook_context(ValidatePDNonZero(self, mavutil.mavlink.PID_TUNING_PITCH))
11107
self.install_message_hook_context(ValidatePDNonZero(self, mavutil.mavlink.PID_TUNING_YAW))
11108
# until the context pop happens, all received PID_TUNINGS will be verified as good
11109
self.arm_vehicle()
11110
self.set_rc(3, 1500)
11111
self.delay_sim_time(2)
11112
self.set_rc(2, 1250)
11113
self.delay_sim_time(5)
11114
self.assert_receive_message('PID_TUNING', timeout=5)
11115
self.set_rc_default()
11116
self.zero_throttle()
11117
self.disarm_vehicle()
11118
11119
self.context_pop()
11120
self.reboot_sitl()
11121
11122
def turn_safety_x(self, value):
11123
self.mav.mav.set_mode_send(
11124
self.mav.target_system,
11125
mavutil.mavlink.MAV_MODE_FLAG_DECODE_POSITION_SAFETY,
11126
value)
11127
11128
def turn_safety_off(self):
11129
self.turn_safety_x(0)
11130
11131
def turn_safety_on(self):
11132
self.turn_safety_x(1)
11133
11134
def SafetySwitch(self):
11135
'''test safety switch behaviour'''
11136
self.wait_ready_to_arm()
11137
11138
self.turn_safety_on()
11139
self.assert_prearm_failure("safety switch")
11140
11141
self.turn_safety_off()
11142
self.wait_ready_to_arm()
11143
11144
self.takeoff(2, mode='LOITER')
11145
self.turn_safety_on()
11146
11147
self.wait_servo_channel_value(1, 0)
11148
self.turn_safety_off()
11149
11150
self.change_mode('LAND')
11151
self.wait_disarmed()
11152
11153
# test turning safty on/off using explicit MAVLink command:
11154
self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_SET_SAFETY_SWITCH_STATE, mavutil.mavlink.SAFETY_SWITCH_STATE_SAFE)
11155
self.assert_prearm_failure("safety switch")
11156
self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_SET_SAFETY_SWITCH_STATE, mavutil.mavlink.SAFETY_SWITCH_STATE_DANGEROUS)
11157
self.wait_ready_to_arm()
11158
11159
def ArmSwitchAfterReboot(self):
11160
'''test that the arming switch does not trigger after a reboot'''
11161
self.wait_ready_to_arm()
11162
self.set_parameters({
11163
"RC8_OPTION": 153,
11164
})
11165
self.set_rc(8, 2000)
11166
self.wait_armed()
11167
self.disarm_vehicle()
11168
self.context_collect('STATUSTEXT')
11169
self.reboot_sitl()
11170
11171
tstart = self.get_sim_time()
11172
while True:
11173
if self.get_sim_time_cached() - tstart > 60:
11174
break
11175
if self.armed():
11176
raise NotAchievedException("Armed after reboot with switch high")
11177
armmsg = self.statustext_in_collections('Arm: ')
11178
if armmsg is not None:
11179
raise NotAchievedException("statustext(%s) means we tried to arm" % armmsg.text)
11180
self.progress("Did not arm via arming switfch after a reboot")
11181
11182
def GuidedYawRate(self):
11183
'''ensuer guided yaw rate is not affected by rate of sewt-attitude messages'''
11184
self.takeoff(30, mode='GUIDED')
11185
rates = {}
11186
for rate in 1, 10:
11187
# command huge yaw rate for a while
11188
tstart = self.get_sim_time()
11189
interval = 1/rate
11190
yawspeed_rads_sum = 0
11191
yawspeed_rads_count = 0
11192
last_sent = 0
11193
while True:
11194
self.drain_mav()
11195
tnow = self.get_sim_time_cached()
11196
if tnow - last_sent > interval:
11197
self.do_yaw_rate(60) # this is... unlikely
11198
last_sent = tnow
11199
if tnow - tstart < 5: # let it spin up to speed first
11200
continue
11201
yawspeed_rads_sum += self.mav.messages['ATTITUDE'].yawspeed
11202
yawspeed_rads_count += 1
11203
if tnow - tstart > 15: # 10 seconds of measurements
11204
break
11205
yawspeed_degs = math.degrees(yawspeed_rads_sum / yawspeed_rads_count)
11206
rates[rate] = yawspeed_degs
11207
self.progress("Input rate %u hz: average yaw rate %f deg/s" % (rate, yawspeed_degs))
11208
11209
if rates[10] < rates[1] * 0.95:
11210
raise NotAchievedException("Guided yaw rate slower for higher rate updates")
11211
11212
self.do_RTL()
11213
11214
def test_rplidar(self, sim_device_name, expected_distances):
11215
'''plonks a Copter with a RPLidarA2 in the middle of a simulated field
11216
of posts and checks that the measurements are what we expect.'''
11217
self.set_parameters({
11218
"SERIAL5_PROTOCOL": 11,
11219
"PRX1_TYPE": 5,
11220
})
11221
self.customise_SITL_commandline([
11222
"--serial5=sim:%s:" % sim_device_name,
11223
"--home", "51.8752066,14.6487840,0,0", # SITL has "posts" here
11224
])
11225
11226
self.wait_ready_to_arm()
11227
11228
wanting_distances = copy.copy(expected_distances)
11229
tstart = self.get_sim_time()
11230
timeout = 60
11231
while True:
11232
now = self.get_sim_time_cached()
11233
if now - tstart > timeout:
11234
raise NotAchievedException("Did not get all distances")
11235
m = self.mav.recv_match(type="DISTANCE_SENSOR",
11236
blocking=True,
11237
timeout=1)
11238
if m is None:
11239
continue
11240
self.progress("Got (%s)" % str(m))
11241
if m.orientation not in wanting_distances:
11242
continue
11243
if abs(m.current_distance - wanting_distances[m.orientation]) > 5:
11244
self.progress("Wrong distance orient=%u want=%u got=%u" %
11245
(m.orientation,
11246
wanting_distances[m.orientation],
11247
m.current_distance))
11248
continue
11249
self.progress("Correct distance for orient %u (want=%u got=%u)" %
11250
(m.orientation,
11251
wanting_distances[m.orientation],
11252
m.current_distance))
11253
del wanting_distances[m.orientation]
11254
if len(wanting_distances.items()) == 0:
11255
break
11256
11257
def RPLidarA2(self):
11258
'''test Raspberry Pi Lidar A2'''
11259
expected_distances = {
11260
mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 276,
11261
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 256,
11262
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 1130,
11263
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 1286,
11264
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 626,
11265
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 971,
11266
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 762,
11267
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 792,
11268
}
11269
11270
self.test_rplidar("rplidara2", expected_distances)
11271
11272
def RPLidarA1(self):
11273
'''test Raspberry Pi Lidar A1'''
11274
return # we don't send distances when too long?
11275
expected_distances = {
11276
mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 276,
11277
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 256,
11278
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 800,
11279
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 800,
11280
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 626,
11281
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 800,
11282
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 762,
11283
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 792,
11284
}
11285
11286
self.test_rplidar("rplidara1", expected_distances)
11287
11288
def BrakeZ(self):
11289
'''check jerk limit correct in Brake mode'''
11290
self.set_parameter('PSC_JERK_Z', 3)
11291
self.takeoff(50, mode='GUIDED')
11292
vx, vy, vz_up = (0, 0, -1)
11293
self.test_guided_local_velocity_target(vx=vx, vy=vy, vz_up=vz_up, timeout=10)
11294
11295
self.wait_for_local_velocity(vx=vx, vy=vy, vz_up=vz_up, timeout=10)
11296
self.change_mode('BRAKE')
11297
self.wait_for_local_velocity(vx=0, vy=0, vz_up=0, timeout=10)
11298
self.land_and_disarm()
11299
11300
def MISSION_START(self):
11301
'''test mavlink command MAV_CMD_MISSION_START'''
11302
self.upload_simple_relhome_mission([
11303
(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 200),
11304
(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),
11305
])
11306
for command in self.run_cmd, self.run_cmd_int:
11307
self.change_mode('LOITER')
11308
self.set_current_waypoint(1)
11309
self.wait_ready_to_arm()
11310
self.arm_vehicle()
11311
self.change_mode('AUTO')
11312
command(mavutil.mavlink.MAV_CMD_MISSION_START)
11313
self.wait_altitude(20, 1000, relative=True)
11314
self.change_mode('RTL')
11315
self.wait_disarmed()
11316
11317
def DO_CHANGE_SPEED_in_guided(self):
11318
'''test Copter DO_CHANGE_SPEED handling in guided mode'''
11319
self.takeoff(20, mode='GUIDED')
11320
11321
new_loc = self.mav.location()
11322
new_loc_offset_n = 2000
11323
new_loc_offset_e = 0
11324
self.location_offset_ne(new_loc, new_loc_offset_n, new_loc_offset_e)
11325
11326
second_loc_offset_n = -1000
11327
second_loc_offset_e = 0
11328
second_loc = self.mav.location()
11329
self.location_offset_ne(second_loc, second_loc_offset_n, second_loc_offset_e)
11330
11331
# for run_cmd we fly away from home
11332
for (tloc, command) in (new_loc, self.run_cmd), (second_loc, self.run_cmd_int):
11333
self.run_cmd_int(
11334
mavutil.mavlink.MAV_CMD_DO_REPOSITION,
11335
p1=-1, # "default"
11336
p2=0, # flags; none supplied here
11337
p3=0, # loiter radius for planes, zero ignored
11338
p4=float("nan"), # nan means do whatever you want to do
11339
p5=int(tloc.lat * 1e7),
11340
p6=int(tloc.lng * 1e7),
11341
p7=tloc.alt,
11342
frame=mavutil.mavlink.MAV_FRAME_GLOBAL,
11343
)
11344
for speed in [2, 10, 4]:
11345
command(
11346
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED,
11347
p1=1, # groundspeed,
11348
p2=speed,
11349
p3=-1, # throttle, -1 is no-change
11350
p4=0, # absolute value, not relative
11351
)
11352
self.wait_groundspeed(speed-0.2, speed+0.2, minimum_duration=10, timeout=20)
11353
11354
# we've made random changes to vehicle guided speeds above;
11355
# reboot vehicle to reset those:
11356
self.disarm_vehicle(force=True)
11357
self.reboot_sitl()
11358
11359
def _MAV_CMD_DO_FLIGHTTERMINATION(self, command):
11360
self.set_parameters({
11361
"SYSID_MYGCS": self.mav.source_system,
11362
"DISARM_DELAY": 0,
11363
})
11364
self.wait_ready_to_arm()
11365
self.arm_vehicle()
11366
self.context_collect('STATUSTEXT')
11367
command(mavutil.mavlink.MAV_CMD_DO_FLIGHTTERMINATION, p1=1)
11368
self.wait_disarmed()
11369
self.reboot_sitl()
11370
11371
def MAV_CMD_DO_FLIGHTTERMINATION(self):
11372
'''test MAV_CMD_DO_FLIGHTTERMINATION works on Copter'''
11373
self._MAV_CMD_DO_FLIGHTTERMINATION(self.run_cmd)
11374
self._MAV_CMD_DO_FLIGHTTERMINATION(self.run_cmd_int)
11375
11376
def MAV_CMD_NAV_LOITER_UNLIM(self):
11377
'''ensure MAV_CMD_NAV_LOITER_UNLIM via mavlink works'''
11378
for command in self.run_cmd, self.run_cmd_int:
11379
self.change_mode('STABILIZE')
11380
command(mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM)
11381
self.wait_mode('LOITER')
11382
11383
def MAV_CMD_NAV_RETURN_TO_LAUNCH(self):
11384
'''ensure MAV_CMD_NAV_RETURN_TO_LAUNCH via mavlink works'''
11385
for command in self.run_cmd, self.run_cmd_int:
11386
self.change_mode('STABILIZE')
11387
command(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH)
11388
self.wait_mode('RTL')
11389
11390
def MAV_CMD_NAV_VTOL_LAND(self):
11391
'''ensure MAV_CMD_NAV_LAND via mavlink works'''
11392
for command in self.run_cmd, self.run_cmd_int:
11393
self.change_mode('STABILIZE')
11394
command(mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND)
11395
self.wait_mode('LAND')
11396
self.change_mode('STABILIZE')
11397
command(mavutil.mavlink.MAV_CMD_NAV_LAND)
11398
self.wait_mode('LAND')
11399
11400
def clear_roi(self):
11401
'''ensure three commands that clear ROI are equivalent'''
11402
11403
self.upload_simple_relhome_mission([
11404
(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 20),
11405
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 20),
11406
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 200, 0, 20), # directly North, i.e. 0 degrees
11407
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 400, 0, 20), # directly North, i.e. 0 degrees
11408
])
11409
11410
self.set_parameter("AUTO_OPTIONS", 3)
11411
self.change_mode('AUTO')
11412
self.wait_ready_to_arm()
11413
self.arm_vehicle()
11414
home_loc = self.mav.location()
11415
11416
cmd_ids = [
11417
mavutil.mavlink.MAV_CMD_DO_SET_ROI,
11418
mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION,
11419
mavutil.mavlink.MAV_CMD_DO_SET_ROI_NONE,
11420
]
11421
for command in self.run_cmd, self.run_cmd_int:
11422
for cmd_id in cmd_ids:
11423
self.wait_waypoint(2, 2)
11424
11425
# Set an ROI at the Home location, expect to point at Home
11426
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION, p5=home_loc.lat, p6=home_loc.lng, p7=home_loc.alt)
11427
self.wait_heading(180)
11428
11429
# Clear the ROI, expect to point at the next Waypoint
11430
self.progress("Clear ROI using %s(%d)" % (command.__name__, cmd_id))
11431
command(cmd_id)
11432
self.wait_heading(0)
11433
11434
self.wait_waypoint(4, 4)
11435
self.set_current_waypoint_using_mav_cmd_do_set_mission_current(seq=2)
11436
11437
self.land_and_disarm()
11438
11439
def start_flying_simple_rehome_mission(self, items):
11440
'''uploads items, changes mode to auto, waits ready to arm and arms
11441
vehicle. If the first item it a takeoff you can expect the
11442
vehicle to fly after this method returns
11443
'''
11444
11445
self.upload_simple_relhome_mission(items)
11446
11447
self.set_parameter("AUTO_OPTIONS", 3)
11448
self.change_mode('AUTO')
11449
self.wait_ready_to_arm()
11450
11451
self.arm_vehicle()
11452
11453
def _MAV_CMD_DO_LAND_START(self, run_cmd):
11454
alt = 5
11455
self.start_flying_simple_rehome_mission([
11456
(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, alt),
11457
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 200, 0, alt),
11458
(mavutil.mavlink.MAV_CMD_NAV_LAND, 0, 0, 0),
11459
(mavutil.mavlink.MAV_CMD_DO_LAND_START, 0, 0, alt),
11460
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 200, 2000, alt),
11461
(mavutil.mavlink.MAV_CMD_NAV_LAND, 0, 0, 0),
11462
])
11463
11464
self.wait_current_waypoint(2)
11465
run_cmd(mavutil.mavlink.MAV_CMD_DO_LAND_START)
11466
self.wait_current_waypoint(5)
11467
# we pretend to be in RTL mode while doing this:
11468
self.wait_mode("AUTO_RTL")
11469
self.do_RTL()
11470
11471
def MAV_CMD_DO_LAND_START(self):
11472
'''test handling of mavlink-received MAV_CMD_DO_LAND_START command'''
11473
self._MAV_CMD_DO_LAND_START(self.run_cmd)
11474
self.zero_throttle()
11475
self._MAV_CMD_DO_LAND_START(self.run_cmd_int)
11476
11477
def _MAV_CMD_SET_EKF_SOURCE_SET(self, run_cmd):
11478
run_cmd(
11479
mavutil.mavlink.MAV_CMD_SET_EKF_SOURCE_SET,
11480
17,
11481
want_result=mavutil.mavlink.MAV_RESULT_DENIED,
11482
)
11483
11484
self.change_mode('LOITER')
11485
self.wait_ready_to_arm()
11486
11487
run_cmd(mavutil.mavlink.MAV_CMD_SET_EKF_SOURCE_SET, 2)
11488
11489
self.assert_prearm_failure('Need Position Estimate')
11490
run_cmd(mavutil.mavlink.MAV_CMD_SET_EKF_SOURCE_SET, 1)
11491
11492
self.wait_ready_to_arm()
11493
11494
def MAV_CMD_SET_EKF_SOURCE_SET(self):
11495
'''test setting of source sets using mavlink command'''
11496
self._MAV_CMD_SET_EKF_SOURCE_SET(self.run_cmd)
11497
self._MAV_CMD_SET_EKF_SOURCE_SET(self.run_cmd_int)
11498
11499
def MAV_CMD_NAV_TAKEOFF(self):
11500
'''test issuing takeoff command via mavlink'''
11501
self.change_mode('GUIDED')
11502
self.wait_ready_to_arm()
11503
11504
self.arm_vehicle()
11505
self.run_cmd(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, p7=5)
11506
self.wait_altitude(4.5, 5.5, minimum_duration=5, relative=True)
11507
self.change_mode('LAND')
11508
self.wait_disarmed()
11509
11510
self.start_subtest("Check NAV_TAKEOFF is above home location, not current location")
11511
# reset home 20 metres above current location
11512
current_alt_abs = self.get_altitude(relative=False)
11513
11514
loc = self.mav.location()
11515
11516
home_z_ofs = 20
11517
self.run_cmd(
11518
mavutil.mavlink.MAV_CMD_DO_SET_HOME,
11519
p5=loc.lat,
11520
p6=loc.lng,
11521
p7=current_alt_abs + home_z_ofs,
11522
)
11523
11524
self.change_mode('GUIDED')
11525
self.arm_vehicle()
11526
takeoff_alt = 5
11527
self.run_cmd(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, p7=takeoff_alt)
11528
self.wait_altitude(
11529
current_alt_abs + home_z_ofs + takeoff_alt - 0.5,
11530
current_alt_abs + home_z_ofs + takeoff_alt + 0.5,
11531
minimum_duration=5,
11532
relative=False,
11533
)
11534
self.change_mode('LAND')
11535
self.wait_disarmed()
11536
11537
self.reboot_sitl() # unlock home position
11538
11539
def MAV_CMD_NAV_TAKEOFF_command_int(self):
11540
'''test issuing takeoff command via mavlink and command_int'''
11541
self.change_mode('GUIDED')
11542
self.wait_ready_to_arm()
11543
11544
self.start_subtest("Check NAV_TAKEOFF is above home location, not current location")
11545
# reset home 20 metres above current location
11546
current_alt_abs = self.get_altitude(relative=False)
11547
11548
loc = self.mav.location()
11549
11550
home_z_ofs = 20
11551
self.run_cmd(
11552
mavutil.mavlink.MAV_CMD_DO_SET_HOME,
11553
p5=loc.lat,
11554
p6=loc.lng,
11555
p7=current_alt_abs + home_z_ofs,
11556
)
11557
11558
self.change_mode('GUIDED')
11559
self.arm_vehicle()
11560
takeoff_alt = 5
11561
self.run_cmd_int(
11562
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
11563
p7=takeoff_alt,
11564
frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
11565
)
11566
self.wait_altitude(
11567
current_alt_abs + home_z_ofs + takeoff_alt - 0.5,
11568
current_alt_abs + home_z_ofs + takeoff_alt + 0.5,
11569
minimum_duration=5,
11570
relative=False,
11571
)
11572
self.change_mode('LAND')
11573
self.wait_disarmed()
11574
11575
self.reboot_sitl() # unlock home position
11576
11577
def Ch6TuningWPSpeed(self):
11578
'''test waypoint speed can be changed via Ch6 tuning knob'''
11579
self.set_parameters({
11580
"RC6_OPTION": 219, # RC6 used for tuning
11581
"TUNE": 10, # 10 is waypoint speed
11582
"TUNE_MIN": 0.02, # 20cm/s
11583
"TUNE_MAX": 1000, # 10m/s
11584
"AUTO_OPTIONS": 3,
11585
})
11586
self.set_rc(6, 2000)
11587
11588
self.upload_simple_relhome_mission([
11589
(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 20),
11590
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2000, 0, 20),
11591
(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),
11592
])
11593
self.change_mode('AUTO')
11594
11595
self.wait_ready_to_arm()
11596
11597
self.arm_vehicle()
11598
11599
self.wait_groundspeed(9.5, 10.5, minimum_duration=5)
11600
11601
self.set_rc(6, 1500)
11602
self.wait_groundspeed(4.5, 5.5, minimum_duration=5)
11603
11604
self.set_rc(6, 2000)
11605
self.wait_groundspeed(9.5, 10.5, minimum_duration=5)
11606
11607
self.set_rc(6, 1300)
11608
self.wait_groundspeed(2.5, 3.5, minimum_duration=5)
11609
11610
self.do_RTL()
11611
11612
def PILOT_THR_BHV(self):
11613
'''test the PILOT_THR_BHV parameter'''
11614
self.start_subtest("Test default behaviour, no disarm on land")
11615
self.set_parameters({
11616
"DISARM_DELAY": 0,
11617
})
11618
self.takeoff(2, mode='GUIDED')
11619
self.set_rc(3, 1500)
11620
self.change_mode('LOITER')
11621
self.set_rc(3, 1300)
11622
11623
maintain_armed = WaitAndMaintainArmed(self, minimum_duration=20)
11624
maintain_armed.run()
11625
11626
self.start_subtest("Test THR_BEHAVE_DISARM_ON_LAND_DETECT")
11627
self.set_parameters({
11628
"PILOT_THR_BHV": 4, # Disarm on land detection
11629
})
11630
self.zero_throttle()
11631
self.takeoff(2, mode='GUIDED')
11632
self.set_rc(3, 1500)
11633
self.change_mode('LOITER')
11634
self.set_rc(3, 1300)
11635
11636
self.wait_disarmed()
11637
11638
def CameraLogMessages(self):
11639
'''ensure Camera log messages are good'''
11640
self.set_parameter("RC12_OPTION", 9) # CameraTrigger
11641
self.set_parameter("CAM1_TYPE", 1) # Camera with servo trigger
11642
self.reboot_sitl() # needed for RC12_OPTION to take effect
11643
11644
gpis = []
11645
gps_raws = []
11646
11647
self.takeoff(10, mode='GUIDED')
11648
self.set_rc(12, 2000)
11649
gpis.append(self.assert_receive_message('GLOBAL_POSITION_INT'))
11650
gps_raws.append(self.assert_receive_message('GPS_RAW_INT'))
11651
self.set_rc(12, 1000)
11652
11653
self.fly_guided_move_local(0, 0, 20)
11654
11655
self.set_rc(12, 2000)
11656
gpis.append(self.assert_receive_message('GLOBAL_POSITION_INT'))
11657
gps_raws.append(self.assert_receive_message('GPS_RAW_INT'))
11658
self.set_rc(12, 1000)
11659
11660
dfreader = self.dfreader_for_current_onboard_log()
11661
self.do_RTL()
11662
11663
for i in range(len(gpis)):
11664
gpi = gpis[i]
11665
gps_raw = gps_raws[i]
11666
m = dfreader.recv_match(type="CAM")
11667
11668
things = [
11669
["absalt", gpi.alt*0.001, m.Alt],
11670
["relalt", gpi.relative_alt*0.001, m.RelAlt],
11671
["gpsalt", gps_raw.alt*0.001, m.GPSAlt], # use GPS_RAW here?
11672
]
11673
for (name, want, got) in things:
11674
if abs(got - want) > 1:
11675
raise NotAchievedException(f"Incorrect {name} {want=} {got=}")
11676
self.progress(f"{name} {want=} {got=}")
11677
11678
want = gpi.relative_alt*0.001
11679
got = m.RelAlt
11680
if abs(got - want) > 1:
11681
raise NotAchievedException(f"Incorrect relalt {want=} {got=}")
11682
11683
def LoiterToGuidedHomeVSOrigin(self):
11684
'''test moving from guided to loiter mode when home is a different alt
11685
to origin'''
11686
self.set_parameters({
11687
"TERRAIN_ENABLE": 1,
11688
"SIM_TERRAIN": 1,
11689
})
11690
self.takeoff(10, mode='GUIDED')
11691
here = self.mav.location()
11692
self.set_home(here)
11693
self.change_mode('LOITER')
11694
self.wait_altitude(here.alt-1, here.alt+1, minimum_duration=10)
11695
self.disarm_vehicle(force=True)
11696
self.reboot_sitl() # to "unstick" home
11697
11698
def GuidedModeThrust(self):
11699
'''test handling of option-bit-3, where mavlink commands are
11700
intrepreted as thrust not climb rate'''
11701
self.set_parameter('GUID_OPTIONS', 8)
11702
self.change_mode('GUIDED')
11703
self.wait_ready_to_arm()
11704
self.arm_vehicle()
11705
self.mav.mav.set_attitude_target_send(
11706
0, # time_boot_ms
11707
1, # target sysid
11708
1, # target compid
11709
0, # bitmask of things to ignore
11710
mavextra.euler_to_quat([0, 0, 0]), # att
11711
0, # roll rate (rad/s)
11712
0, # pitch rate (rad/s)
11713
0, # yaw rate (rad/s)
11714
0.5
11715
) # thrust, 0 to 1
11716
self.wait_altitude(0.5, 100, relative=True, timeout=10)
11717
self.do_RTL()
11718
11719
def AutoRTL(self):
11720
'''Test Auto RTL mode using do land start and return path start mission items'''
11721
alt = 50
11722
guided_loc = self.home_relative_loc_ne(1000, 0)
11723
guided_loc.alt += alt
11724
11725
# Arm, take off and fly to guided location
11726
self.takeoff(mode='GUIDED')
11727
self.fly_guided_move_to(guided_loc, timeout=240)
11728
11729
# Try auto RTL mode, should fail with no mission
11730
try:
11731
self.change_mode('AUTO_RTL', timeout=10)
11732
raise NotAchievedException("Should not change mode with no mission")
11733
except WaitModeTimeout:
11734
pass
11735
except Exception as e:
11736
raise e
11737
11738
# pymavlink does not understand the new return path command yet, at some point it will
11739
cmd_return_path_start = 188 # mavutil.mavlink.MAV_CMD_DO_RETURN_PATH_START
11740
11741
# Do land start and do return path should both fail as commands too
11742
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_LAND_START, want_result=mavutil.mavlink.MAV_RESULT_FAILED)
11743
self.run_cmd(cmd_return_path_start, want_result=mavutil.mavlink.MAV_RESULT_FAILED)
11744
11745
# Load mission with do land start
11746
self.upload_simple_relhome_mission([
11747
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1000, 0, alt), # 1
11748
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 750, 0, alt), # 2
11749
self.create_MISSION_ITEM_INT(mavutil.mavlink.MAV_CMD_DO_LAND_START), # 3
11750
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 500, 0, alt), # 4
11751
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 250, 0, alt), # 5
11752
])
11753
11754
# Return path should still fail
11755
self.run_cmd(cmd_return_path_start, want_result=mavutil.mavlink.MAV_RESULT_FAILED)
11756
11757
# Do land start should jump to the waypoint following the item
11758
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_LAND_START, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED)
11759
self.drain_mav()
11760
self.assert_current_waypoint(4)
11761
11762
# Back to guided location
11763
self.change_mode('GUIDED')
11764
self.fly_guided_move_to(guided_loc)
11765
11766
# mode change to Auto RTL should do the same
11767
self.change_mode('AUTO_RTL')
11768
self.drain_mav()
11769
self.assert_current_waypoint(4)
11770
11771
# Back to guided location
11772
self.change_mode('GUIDED')
11773
self.fly_guided_move_to(guided_loc)
11774
11775
# Add a return path item
11776
self.upload_simple_relhome_mission([
11777
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 1250, 0, alt), # 1
11778
self.create_MISSION_ITEM_INT(cmd_return_path_start), # 2
11779
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 900, 0, alt), # 3
11780
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 750, 0, alt), # 4
11781
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 550, 0, alt), # 5
11782
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 500, 0, alt), # 6
11783
self.create_MISSION_ITEM_INT(mavutil.mavlink.MAV_CMD_DO_LAND_START), # 7
11784
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 250, 0, alt), # 8
11785
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, -250, 0, alt), # 9
11786
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, -500, 0, alt), # 10
11787
])
11788
11789
# Return path should now work
11790
self.run_cmd(cmd_return_path_start, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED)
11791
self.drain_mav()
11792
self.assert_current_waypoint(3)
11793
11794
# Back to guided location
11795
self.change_mode('GUIDED')
11796
self.fly_guided_move_to(guided_loc)
11797
11798
# mode change to Auto RTL should join the return path
11799
self.change_mode('AUTO_RTL')
11800
self.drain_mav()
11801
self.assert_current_waypoint(3)
11802
11803
# do land start should still work
11804
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_LAND_START, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED)
11805
self.drain_mav()
11806
self.assert_current_waypoint(8)
11807
11808
# Move a bit closer in guided
11809
return_path_test = self.home_relative_loc_ne(600, 0)
11810
return_path_test.alt += alt
11811
self.change_mode('GUIDED')
11812
self.fly_guided_move_to(return_path_test, timeout=100)
11813
11814
# check the mission is joined further along
11815
self.run_cmd(cmd_return_path_start, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED)
11816
self.drain_mav()
11817
self.assert_current_waypoint(5)
11818
11819
# fly over home
11820
home = self.home_relative_loc_ne(0, 0)
11821
home.alt += alt
11822
self.change_mode('GUIDED')
11823
self.fly_guided_move_to(home, timeout=140)
11824
11825
# Should never join return path after do land start
11826
self.run_cmd(cmd_return_path_start, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED)
11827
self.drain_mav()
11828
self.assert_current_waypoint(6)
11829
11830
# Done
11831
self.land_and_disarm()
11832
11833
def EK3_OGN_HGT_MASK(self):
11834
'''test baraometer-alt-compensation based on long-term GPS readings'''
11835
self.context_push()
11836
self.set_parameters({
11837
'EK3_OGN_HGT_MASK': 1, # compensate baro drift using GPS
11838
})
11839
self.reboot_sitl()
11840
11841
expected_alt = 10
11842
11843
self.change_mode('GUIDED')
11844
self.wait_ready_to_arm()
11845
current_alt = self.get_altitude()
11846
11847
expected_alt_abs = current_alt + expected_alt
11848
11849
self.takeoff(expected_alt, mode='GUIDED')
11850
self.delay_sim_time(5)
11851
11852
self.set_parameter("SIM_BARO_DRIFT", 0.01) # 1cm/second
11853
11854
def check_altitude(mav, m):
11855
m_type = m.get_type()
11856
epsilon = 10 # in metres
11857
if m_type == 'GPS_RAW_INT':
11858
got_gps_alt = m.alt * 0.001
11859
if abs(expected_alt_abs - got_gps_alt) > epsilon:
11860
raise NotAchievedException(f"Bad GPS altitude (got={got_gps_alt} want={expected_alt_abs})")
11861
elif m_type == 'GLOBAL_POSITION_INT':
11862
got_canonical_alt = m.relative_alt * 0.001
11863
if abs(expected_alt - got_canonical_alt) > epsilon:
11864
raise NotAchievedException(f"Bad canonical altitude (got={got_canonical_alt} want={expected_alt})")
11865
11866
self.install_message_hook_context(check_altitude)
11867
11868
self.delay_sim_time(1500)
11869
11870
self.disarm_vehicle(force=True)
11871
11872
self.context_pop()
11873
11874
self.reboot_sitl(force=True)
11875
11876
def GuidedForceArm(self):
11877
'''ensure Guided acts appropriately when force-armed'''
11878
self.set_parameters({
11879
"EK3_SRC2_VELXY": 5,
11880
"SIM_GPS1_ENABLE": 0,
11881
})
11882
self.load_default_params_file("copter-optflow.parm")
11883
self.reboot_sitl()
11884
self.delay_sim_time(30)
11885
self.change_mode('GUIDED')
11886
self.arm_vehicle(force=True)
11887
self.takeoff(20, mode='GUIDED')
11888
location = self.offset_location_ne(self.sim_location(), metres_north=0, metres_east=-300)
11889
self.progress("Ensure we don't move for 10 seconds")
11890
tstart = self.get_sim_time()
11891
startpos = self.sim_location_int()
11892
while True:
11893
now = self.get_sim_time_cached()
11894
if now - tstart > 10:
11895
break
11896
self.send_set_position_target_global_int(int(location.lat*1e7), int(location.lng*1e7), 10)
11897
dist = self.get_distance_int(startpos, self.sim_location_int())
11898
if dist > 10:
11899
raise NotAchievedException("Wandered too far from start position")
11900
self.delay_sim_time(1)
11901
11902
self.disarm_vehicle(force=True)
11903
self.reboot_sitl()
11904
11905
def EK3_OGN_HGT_MASK_climbing(self):
11906
'''check combination of height bits doesn't cause climb'''
11907
self.context_push()
11908
self.set_parameters({
11909
'EK3_OGN_HGT_MASK': 5,
11910
})
11911
self.reboot_sitl()
11912
11913
expected_alt = 10
11914
11915
self.change_mode('GUIDED')
11916
self.wait_ready_to_arm()
11917
current_alt = self.get_altitude()
11918
11919
expected_alt_abs = current_alt + expected_alt
11920
11921
self.takeoff(expected_alt, mode='GUIDED')
11922
self.delay_sim_time(5)
11923
11924
def check_altitude(mav, m):
11925
m_type = m.get_type()
11926
epsilon = 10 # in metres
11927
if m_type == 'GPS_RAW_INT':
11928
got_gps_alt = m.alt * 0.001
11929
if abs(expected_alt_abs - got_gps_alt) > epsilon:
11930
raise NotAchievedException(f"Bad GPS altitude (got={got_gps_alt} want={expected_alt_abs})")
11931
elif m_type == 'GLOBAL_POSITION_INT':
11932
if abs(expected_alt - m.relative_alt * 0.001) > epsilon:
11933
raise NotAchievedException("Bad canonical altitude")
11934
11935
self.install_message_hook_context(check_altitude)
11936
11937
self.delay_sim_time(1500)
11938
11939
self.disarm_vehicle(force=True)
11940
11941
self.context_pop()
11942
self.reboot_sitl(force=True)
11943
11944
def GuidedWeatherVane(self):
11945
'''check Copter Guided mode weathervane option'''
11946
self.set_parameters({
11947
'SIM_WIND_SPD': 10,
11948
'SIM_WIND_DIR': 90,
11949
'WVANE_ENABLE': 1,
11950
})
11951
self.takeoff(20, mode='GUIDED')
11952
self.guided_achieve_heading(0)
11953
11954
self.set_parameter("GUID_OPTIONS", 128)
11955
self.wait_heading(90, timeout=60, minimum_duration=10)
11956
self.do_RTL()
11957
11958
def Clamp(self):
11959
'''test Copter docking clamp'''
11960
clamp_ch = 11
11961
self.set_parameters({
11962
"SIM_CLAMP_CH": clamp_ch,
11963
})
11964
11965
self.takeoff(1, mode='LOITER')
11966
11967
self.context_push()
11968
self.context_collect('STATUSTEXT')
11969
self.progress("Ensure can't take off with clamp in place")
11970
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=11, p2=2000)
11971
self.wait_statustext("SITL: Clamp: grabbed vehicle", check_context=True)
11972
self.arm_vehicle()
11973
self.set_rc(3, 2000)
11974
self.wait_altitude(0, 5, minimum_duration=5, relative=True)
11975
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=11, p2=1000)
11976
self.wait_statustext("SITL: Clamp: released vehicle", check_context=True)
11977
self.wait_altitude(5, 5000, minimum_duration=1, relative=True)
11978
self.do_RTL()
11979
self.set_rc(3, 1000)
11980
self.change_mode('LOITER')
11981
self.context_pop()
11982
11983
self.progress("Same again for repeatability")
11984
self.context_push()
11985
self.context_collect('STATUSTEXT')
11986
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=11, p2=2000)
11987
self.wait_statustext("SITL: Clamp: grabbed vehicle", check_context=True)
11988
self.arm_vehicle()
11989
self.set_rc(3, 2000)
11990
self.wait_altitude(0, 1, minimum_duration=5, relative=True)
11991
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=11, p2=1000)
11992
self.wait_statustext("SITL: Clamp: released vehicle", check_context=True)
11993
self.wait_altitude(5, 5000, minimum_duration=1, relative=True)
11994
self.do_RTL()
11995
self.set_rc(3, 1000)
11996
self.change_mode('LOITER')
11997
self.context_pop()
11998
11999
here = self.mav.location()
12000
loc = self.offset_location_ne(here, 10, 0)
12001
self.takeoff(5, mode='GUIDED')
12002
self.send_do_reposition(loc, frame=mavutil.mavlink.MAV_FRAME_GLOBAL)
12003
self.wait_location(loc, timeout=120)
12004
self.land_and_disarm()
12005
12006
# explicitly set home so we RTL to the right spot
12007
self.set_home(here)
12008
12009
self.context_push()
12010
self.context_collect('STATUSTEXT')
12011
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=11, p2=2000)
12012
self.wait_statustext("SITL: Clamp: missed vehicle", check_context=True)
12013
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=11, p2=1000)
12014
self.context_pop()
12015
12016
self.takeoff(5, mode='GUIDED')
12017
self.do_RTL()
12018
12019
self.takeoff(5, mode='GUIDED')
12020
self.land_and_disarm()
12021
12022
self.context_push()
12023
self.context_collect('STATUSTEXT')
12024
self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=11, p2=2000)
12025
self.wait_statustext("SITL: Clamp: grabbed vehicle", check_context=True)
12026
self.context_pop()
12027
12028
self.reboot_sitl() # because we set home
12029
12030
def GripperReleaseOnThrustLoss(self):
12031
'''tests that gripper is released on thrust loss if option set'''
12032
12033
self.context_push()
12034
self.set_servo_gripper_parameters()
12035
self.reboot_sitl()
12036
12037
self.takeoff(30, mode='LOITER')
12038
self.context_push()
12039
self.context_collect('STATUSTEXT')
12040
self.set_parameters({
12041
"SIM_ENGINE_MUL": 0.5,
12042
"SIM_ENGINE_FAIL": 1 << 1, # motor 2
12043
"FLIGHT_OPTIONS": 4,
12044
})
12045
12046
self.wait_statustext("Gripper Load Released", timeout=60)
12047
self.context_pop()
12048
12049
self.do_RTL()
12050
self.context_pop()
12051
self.reboot_sitl()
12052
12053
def assert_home_position_not_set(self):
12054
try:
12055
self.poll_home_position()
12056
except NotAchievedException:
12057
return
12058
12059
# if home.lng != 0: etc
12060
12061
raise NotAchievedException("Home is set when it shouldn't be")
12062
12063
def REQUIRE_POSITION_FOR_ARMING(self):
12064
'''check FlightOption::REQUIRE_POSITION_FOR_ARMING works'''
12065
self.context_push()
12066
self.set_parameters({
12067
"SIM_GPS1_NUMSATS": 3, # EKF does not like < 6
12068
})
12069
self.reboot_sitl()
12070
self.change_mode('STABILIZE')
12071
self.wait_prearm_sys_status_healthy()
12072
self.assert_home_position_not_set()
12073
self.arm_vehicle()
12074
self.disarm_vehicle()
12075
self.change_mode('LOITER')
12076
self.assert_prearm_failure("waiting for home", other_prearm_failures_fatal=False)
12077
12078
self.change_mode('STABILIZE')
12079
self.set_parameters({
12080
"FLIGHT_OPTIONS": 8,
12081
})
12082
self.assert_prearm_failure("Need Position Estimate", other_prearm_failures_fatal=False)
12083
self.context_pop()
12084
self.reboot_sitl()
12085
12086
def AutoContinueOnRCFailsafe(self):
12087
'''check LOITER when entered after RC failsafe is ignored in auto'''
12088
self.set_parameters({
12089
"FS_OPTIONS": 1, # 1 is "RC continue if in auto"
12090
})
12091
12092
self.upload_simple_relhome_mission([
12093
# N E U
12094
(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 10),
12095
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 20, 0, 10),
12096
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 40, 0, 10),
12097
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 120, 0, 10),
12098
])
12099
12100
self.takeoff(mode='LOITER')
12101
self.set_rc(1, 1200)
12102
self.delay_sim_time(1) # build up some pilot desired stuff
12103
self.change_mode('AUTO')
12104
self.wait_waypoint(2, 2)
12105
self.set_parameters({
12106
'SIM_RC_FAIL': 1,
12107
})
12108
# self.set_rc(1, 1500) # note we are still in RC fail!
12109
self.wait_waypoint(3, 3)
12110
self.assert_mode_is('AUTO')
12111
self.change_mode('LOITER')
12112
self.wait_groundspeed(0, 0.1, minimum_duration=30, timeout=450)
12113
self.do_RTL()
12114
12115
def MissionRTLYawBehaviour(self):
12116
'''check end-of-mission yaw behaviour'''
12117
self.set_parameters({
12118
"AUTO_OPTIONS": 3,
12119
})
12120
12121
self.start_subtest("behaviour with WP_YAW_BEHAVE set to next-waypoint-except-RTL")
12122
self.upload_simple_relhome_mission([
12123
# N E U
12124
(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 10),
12125
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 20, 0, 10),
12126
(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),
12127
])
12128
self.change_mode('AUTO')
12129
self.wait_ready_to_arm()
12130
original_heading = self.get_heading()
12131
if abs(original_heading) < 5:
12132
raise NotAchievedException(f"Bad original heading {original_heading}")
12133
self.arm_vehicle()
12134
self.wait_current_waypoint(3)
12135
self.wait_rtl_complete()
12136
self.wait_disarmed()
12137
if abs(self.get_heading()) > 5:
12138
raise NotAchievedException("Should have yaw zero without option")
12139
12140
# must change out of auto and back in again to reset state machine:
12141
self.change_mode('LOITER')
12142
self.change_mode('AUTO')
12143
12144
self.start_subtest("behaviour with WP_YAW_BEHAVE set to next-waypoint")
12145
self.upload_simple_relhome_mission([
12146
# N E U
12147
(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 10),
12148
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 20, 20),
12149
(mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),
12150
])
12151
self.set_parameters({
12152
"WP_YAW_BEHAVIOR": 1, # look at next waypoint (including in RTL)
12153
})
12154
self.change_mode('AUTO')
12155
self.wait_ready_to_arm()
12156
original_heading = self.get_heading()
12157
if abs(original_heading) > 1:
12158
raise NotAchievedException("Bad original heading")
12159
self.arm_vehicle()
12160
self.wait_current_waypoint(3)
12161
self.wait_rtl_complete()
12162
self.wait_disarmed()
12163
new_heading = self.get_heading()
12164
if abs(new_heading - original_heading) > 5:
12165
raise NotAchievedException(f"Should return to original heading want={original_heading} got={new_heading}")
12166
12167
def BatteryInternalUseOnly(self):
12168
'''batteries marked as internal use only should not appear over mavlink'''
12169
self.set_parameters({
12170
"BATT_MONITOR": 4, # 4 is analog volt+curr
12171
"BATT2_MONITOR": 4,
12172
})
12173
self.reboot_sitl()
12174
self.wait_message_field_values('BATTERY_STATUS', {
12175
"id": 0,
12176
})
12177
self.wait_message_field_values('BATTERY_STATUS', {
12178
"id": 1,
12179
})
12180
self.progress("Making battery private")
12181
self.set_parameters({
12182
"BATT_OPTIONS": 256,
12183
})
12184
self.wait_message_field_values('BATTERY_STATUS', {
12185
"id": 1,
12186
})
12187
for i in range(10):
12188
self.assert_received_message_field_values('BATTERY_STATUS', {
12189
"id": 1
12190
})
12191
12192
def MAV_CMD_MISSION_START_p1_p2(self):
12193
'''make sure we deny MAV_CMD_MISSION_START if either p1 or p2 non-zero'''
12194
self.upload_simple_relhome_mission([
12195
(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 20),
12196
])
12197
self.set_parameters({
12198
"AUTO_OPTIONS": 3,
12199
})
12200
self.change_mode('AUTO')
12201
self.wait_ready_to_arm()
12202
12203
self.run_cmd(
12204
mavutil.mavlink.MAV_CMD_MISSION_START,
12205
p1=1,
12206
want_result=mavutil.mavlink.MAV_RESULT_DENIED,
12207
)
12208
12209
self.run_cmd(
12210
mavutil.mavlink.MAV_CMD_MISSION_START,
12211
p2=1,
12212
want_result=mavutil.mavlink.MAV_RESULT_DENIED,
12213
)
12214
12215
self.run_cmd(
12216
mavutil.mavlink.MAV_CMD_MISSION_START,
12217
p1=1,
12218
p2=1,
12219
want_result=mavutil.mavlink.MAV_RESULT_DENIED,
12220
)
12221
12222
def ScriptingAHRSSource(self):
12223
'''test ahrs-source.lua script'''
12224
self.install_example_script_context("ahrs-source.lua")
12225
self.set_parameters({
12226
"RC10_OPTION": 90,
12227
"SCR_ENABLE": 1,
12228
"SCR_USER1": 10, # something else
12229
"SCR_USER2": 10, # GPS something
12230
"SCR_USER3": 0.2, # ExtNav innovation
12231
})
12232
self.set_rc(10, 2000)
12233
self.reboot_sitl()
12234
self.context_collect('STATUSTEXT')
12235
self.set_rc(10, 1000)
12236
self.wait_statustext('Using EKF Source Set 1', check_context=True)
12237
self.set_rc(10, 1500)
12238
self.wait_statustext('Using EKF Source Set 2', check_context=True)
12239
self.set_rc(10, 2000)
12240
self.wait_statustext('Using EKF Source Set 3', check_context=True)
12241
12242
def CommonOrigin(self):
12243
"""Test common origin between EKF2 and EKF3"""
12244
self.context_push()
12245
12246
# start on EKF2
12247
self.set_parameters({
12248
'AHRS_EKF_TYPE': 2,
12249
'EK2_ENABLE': 1,
12250
'EK3_CHECK_SCALE': 1, # make EK3 slow to get origin
12251
})
12252
self.reboot_sitl()
12253
12254
self.context_collect('STATUSTEXT')
12255
12256
self.wait_statustext("EKF2 IMU0 origin set", timeout=60, check_context=True)
12257
self.wait_statustext("EKF2 IMU0 is using GPS", timeout=60, check_context=True)
12258
self.wait_statustext("EKF2 active", timeout=60, check_context=True)
12259
12260
self.context_collect('GPS_GLOBAL_ORIGIN')
12261
12262
# get EKF2 origin
12263
self.run_cmd(mavutil.mavlink.MAV_CMD_GET_HOME_POSITION)
12264
ek2_origin = self.assert_receive_message('GPS_GLOBAL_ORIGIN', check_context=True)
12265
12266
# switch to EKF3
12267
self.set_parameters({
12268
'SIM_GPS1_GLTCH_X' : 0.001, # about 100m
12269
'EK3_CHECK_SCALE' : 100,
12270
'AHRS_EKF_TYPE' : 3})
12271
12272
self.wait_statustext("EKF3 IMU0 is using GPS", timeout=60, check_context=True)
12273
self.wait_statustext("EKF3 active", timeout=60, check_context=True)
12274
12275
self.run_cmd(mavutil.mavlink.MAV_CMD_GET_HOME_POSITION)
12276
ek3_origin = self.assert_receive_message('GPS_GLOBAL_ORIGIN', check_context=True)
12277
12278
self.progress("Checking origins")
12279
if ek2_origin.time_usec == ek3_origin.time_usec:
12280
raise NotAchievedException("Did not get a new GPS_GLOBAL_ORIGIN message")
12281
12282
print(ek2_origin, ek3_origin)
12283
12284
if (ek2_origin.latitude != ek3_origin.latitude or
12285
ek2_origin.longitude != ek3_origin.longitude or
12286
ek2_origin.altitude != ek3_origin.altitude):
12287
raise NotAchievedException("Did not get matching EK2 and EK3 origins")
12288
12289
self.context_pop()
12290
12291
# restart GPS driver
12292
self.reboot_sitl()
12293
12294
def ScriptingFlipMode(self):
12295
'''test adding custom mode from scripting'''
12296
# Really it would be nice to check for the AVAILABLE_MODES message, but pymavlink does not understand them yet.
12297
12298
# enable scripting and install flip script
12299
self.set_parameters({
12300
"SCR_ENABLE": 1,
12301
})
12302
self.install_example_script_context('Flip_Mode.lua')
12303
self.reboot_sitl()
12304
12305
# Takeoff in loiter
12306
self.takeoff(10, mode="LOITER")
12307
12308
# Try and switch to flip, should not be posible from loiter
12309
try:
12310
self.change_mode(100, timeout=10)
12311
except AutoTestTimeoutException:
12312
self.progress("PASS not able to enter from loiter")
12313
12314
# Should be alowd to enter from alt hold
12315
self.change_mode("ALT_HOLD")
12316
self.change_mode(100)
12317
12318
# Should return to previous mode after flipping
12319
self.wait_mode("ALT_HOLD")
12320
12321
# Test done
12322
self.land_and_disarm()
12323
12324
def RTLYaw(self):
12325
'''test that vehicle yaws to original heading on RTL'''
12326
# 0 is WP_YAW_BEHAVIOR_NONE
12327
# 1 is WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP
12328
# 2 is WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL
12329
# 3 is WP_YAW_BEHAVIOR_LOOK_AHEAD
12330
for behaviour in 1, 3:
12331
self.set_parameters({
12332
'WP_YAW_BEHAVIOR': behaviour,
12333
})
12334
self.change_mode('GUIDED')
12335
original_heading = self.get_heading()
12336
target_heading = 100
12337
if original_heading - target_heading < 90:
12338
raise NotAchievedException("Bad initial heading")
12339
self.takeoff(10, mode='GUIDED')
12340
self.guided_achieve_heading(target_heading)
12341
self.change_mode('RTL')
12342
self.wait_heading(original_heading)
12343
self.wait_disarmed()
12344
12345
def tests2b(self): # this block currently around 9.5mins here
12346
'''return list of all tests'''
12347
ret = ([
12348
self.MotorVibration,
12349
Test(self.DynamicNotches, attempts=4),
12350
self.PositionWhenGPSIsZero,
12351
self.DynamicRpmNotches, # Do not add attempts to this - failure is sign of a bug
12352
self.DynamicRpmNotchesRateThread,
12353
self.PIDNotches,
12354
self.StaticNotches,
12355
self.RefindGPS,
12356
Test(self.GyroFFT, attempts=1, speedup=8),
12357
Test(self.GyroFFTHarmonic, attempts=4, speedup=8),
12358
Test(self.GyroFFTAverage, attempts=1, speedup=8),
12359
Test(self.GyroFFTContinuousAveraging, attempts=4, speedup=8),
12360
self.GyroFFTPostFilter,
12361
self.GyroFFTMotorNoiseCheck,
12362
self.CompassReordering,
12363
self.CRSF,
12364
self.MotorTest,
12365
self.AltEstimation,
12366
self.EKFSource,
12367
self.GSF,
12368
self.GSF_reset,
12369
self.AP_Avoidance,
12370
self.SMART_RTL,
12371
self.SMART_RTL_EnterLeave,
12372
self.SMART_RTL_Repeat,
12373
self.RTL_TO_RALLY,
12374
self.RTLYaw,
12375
self.FlyEachFrame,
12376
self.GPSBlending,
12377
self.GPSWeightedBlending,
12378
self.GPSBlendingLog,
12379
self.GPSBlendingAffinity,
12380
self.DataFlash,
12381
Test(self.DataFlashErase, attempts=8),
12382
self.Callisto,
12383
self.PerfInfo,
12384
self.Replay,
12385
self.FETtecESC,
12386
self.ProximitySensors,
12387
self.GroundEffectCompensation_touchDownExpected,
12388
self.GroundEffectCompensation_takeOffExpected,
12389
self.DO_CHANGE_SPEED,
12390
self.MISSION_START,
12391
self.AUTO_LAND_TO_BRAKE,
12392
self.WPNAV_SPEED,
12393
self.WPNAV_SPEED_UP,
12394
self.WPNAV_SPEED_DN,
12395
self.DO_WINCH,
12396
self.SensorErrorFlags,
12397
self.GPSForYaw,
12398
self.DefaultIntervalsFromFiles,
12399
self.GPSTypes,
12400
self.MultipleGPS,
12401
self.WatchAlts,
12402
self.GuidedEKFLaneChange,
12403
self.Sprayer,
12404
self.AutoContinueOnRCFailsafe,
12405
self.EK3_RNG_USE_HGT,
12406
self.TerrainDBPreArm,
12407
self.ThrottleGainBoost,
12408
self.ScriptMountPOI,
12409
self.ScriptCopterPosOffsets,
12410
self.MountSolo,
12411
self.FlyMissionTwice,
12412
self.FlyMissionTwiceWithReset,
12413
self.MissionIndexValidity,
12414
self.InvalidJumpTags,
12415
self.IMUConsistency,
12416
self.AHRSTrimLand,
12417
self.IBus,
12418
self.GuidedYawRate,
12419
self.NoArmWithoutMissionItems,
12420
self.DO_CHANGE_SPEED_in_guided,
12421
self.ArmSwitchAfterReboot,
12422
self.RPLidarA1,
12423
self.RPLidarA2,
12424
self.SafetySwitch,
12425
self.BrakeZ,
12426
self.MAV_CMD_DO_FLIGHTTERMINATION,
12427
self.MAV_CMD_DO_LAND_START,
12428
self.MAV_CMD_SET_EKF_SOURCE_SET,
12429
self.MAV_CMD_NAV_TAKEOFF,
12430
self.MAV_CMD_NAV_TAKEOFF_command_int,
12431
self.Ch6TuningWPSpeed,
12432
self.PILOT_THR_BHV,
12433
self.GPSForYawCompassLearn,
12434
self.CameraLogMessages,
12435
self.LoiterToGuidedHomeVSOrigin,
12436
self.GuidedModeThrust,
12437
self.CompassMot,
12438
self.AutoRTL,
12439
self.EK3_OGN_HGT_MASK_climbing,
12440
self.EK3_OGN_HGT_MASK,
12441
self.FarOrigin,
12442
self.GuidedForceArm,
12443
self.GuidedWeatherVane,
12444
self.Clamp,
12445
self.GripperReleaseOnThrustLoss,
12446
self.REQUIRE_POSITION_FOR_ARMING,
12447
self.LoggingFormat,
12448
self.MissionRTLYawBehaviour,
12449
self.BatteryInternalUseOnly,
12450
self.MAV_CMD_MISSION_START_p1_p2,
12451
self.ScriptingAHRSSource,
12452
self.CommonOrigin,
12453
self.TestTetherStuck,
12454
self.ScriptingFlipMode,
12455
])
12456
return ret
12457
12458
def testcan(self):
12459
ret = ([
12460
self.CANGPSCopterMission,
12461
self.TestLogDownloadMAVProxyCAN,
12462
])
12463
return ret
12464
12465
def BattCANSplitAuxInfo(self):
12466
'''test CAN battery periphs'''
12467
self.start_subtest("Swap UAVCAN backend at runtime")
12468
self.set_parameters({
12469
"CAN_P1_DRIVER": 1,
12470
"BATT_MONITOR": 4, # 4 is ananlog volt+curr
12471
"BATT2_MONITOR": 8, # 8 is UAVCAN_BatteryInfo
12472
"BATT_SERIAL_NUM": 0,
12473
"BATT2_SERIAL_NUM": 0,
12474
"BATT_OPTIONS": 128, # allow split auxinfo
12475
"BATT2_OPTIONS": 128, # allow split auxinfo
12476
})
12477
self.reboot_sitl()
12478
self.delay_sim_time(2)
12479
self.set_parameters({
12480
"BATT_MONITOR": 8, # 8 is UAVCAN_BatteryInfo
12481
"BATT2_MONITOR": 4, # 8 is UAVCAN_BatteryInfo
12482
})
12483
self.delay_sim_time(2)
12484
self.set_parameters({
12485
"BATT_MONITOR": 4, # 8 is UAVCAN_BatteryInfo
12486
"BATT2_MONITOR": 8, # 8 is UAVCAN_BatteryInfo
12487
})
12488
self.delay_sim_time(2)
12489
self.set_parameters({
12490
"BATT_MONITOR": 8, # 8 is UAVCAN_BatteryInfo
12491
"BATT2_MONITOR": 4, # 8 is UAVCAN_BatteryInfo
12492
})
12493
self.delay_sim_time(2)
12494
12495
def BattCANReplaceRuntime(self):
12496
'''test CAN battery periphs'''
12497
self.start_subtest("Replace UAVCAN backend at runtime")
12498
self.set_parameters({
12499
"CAN_P1_DRIVER": 1,
12500
"BATT_MONITOR": 11, # 4 is ananlog volt+curr
12501
})
12502
self.reboot_sitl()
12503
self.delay_sim_time(2)
12504
self.set_parameters({
12505
"BATT_MONITOR": 8, # 4 is UAVCAN batterinfo
12506
})
12507
self.delay_sim_time(2)
12508
12509
def testcanbatt(self):
12510
ret = ([
12511
self.BattCANReplaceRuntime,
12512
self.BattCANSplitAuxInfo,
12513
])
12514
return ret
12515
12516
def tests(self):
12517
ret = []
12518
ret.extend(self.tests1a())
12519
ret.extend(self.tests1b())
12520
ret.extend(self.tests1c())
12521
ret.extend(self.tests1d())
12522
ret.extend(self.tests1e())
12523
ret.extend(self.tests2a())
12524
ret.extend(self.tests2b())
12525
return ret
12526
12527
def disabled_tests(self):
12528
return {
12529
"Parachute": "See https://github.com/ArduPilot/ardupilot/issues/4702",
12530
"AltEstimation": "See https://github.com/ArduPilot/ardupilot/issues/15191",
12531
"GroundEffectCompensation_takeOffExpected": "Flapping",
12532
"GroundEffectCompensation_touchDownExpected": "Flapping",
12533
"FlyMissionTwice": "See https://github.com/ArduPilot/ardupilot/pull/18561",
12534
"GPSForYawCompassLearn": "Vehicle currently crashed in spectacular fashion",
12535
"CompassMot": "Cuases an arithmetic exception in the EKF",
12536
"SMART_RTL_EnterLeave": "Causes a panic",
12537
"SMART_RTL_Repeat": "Currently fails due to issue with loop detection",
12538
}
12539
12540
12541
class AutoTestCopterTests1a(AutoTestCopter):
12542
def tests(self):
12543
return self.tests1a()
12544
12545
12546
class AutoTestCopterTests1b(AutoTestCopter):
12547
def tests(self):
12548
return self.tests1b()
12549
12550
12551
class AutoTestCopterTests1c(AutoTestCopter):
12552
def tests(self):
12553
return self.tests1c()
12554
12555
12556
class AutoTestCopterTests1d(AutoTestCopter):
12557
def tests(self):
12558
return self.tests1d()
12559
12560
12561
class AutoTestCopterTests1e(AutoTestCopter):
12562
def tests(self):
12563
return self.tests1e()
12564
12565
12566
class AutoTestCopterTests2a(AutoTestCopter):
12567
def tests(self):
12568
return self.tests2a()
12569
12570
12571
class AutoTestCopterTests2b(AutoTestCopter):
12572
def tests(self):
12573
return self.tests2b()
12574
12575
12576
class AutoTestCAN(AutoTestCopter):
12577
12578
def tests(self):
12579
return self.testcan()
12580
12581
12582
class AutoTestBattCAN(AutoTestCopter):
12583
12584
def tests(self):
12585
return self.testcanbatt()
12586
12587