Skip to content

Commit e07328b

Browse files
pietrodnpeterbarker
pietrodn
authored and
peterbarker
committed
Add commands and tests for sensor calibration
1 parent b448fd9 commit e07328b

File tree

3 files changed

+261
-0
lines changed

3 files changed

+261
-0
lines changed

‎dronekit/__init__.py‎

Lines changed: 119 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2398,6 +2398,125 @@ def reboot(self):
23982398

23992399
self.send_mavlink(reboot_msg)
24002400

2401+
def calibrate_gyro(self):
2402+
"""Request gyroscope calibration."""
2403+
2404+
calibration_command = self.message_factory.command_long_encode(
2405+
0, 0, # target_system, target_component
2406+
mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, # command
2407+
0, # confirmation
2408+
1, # param 1, 1: gyro calibration, 3: gyro temperature calibration
2409+
0, # param 2, 1: magnetometer calibration
2410+
0, # param 3, 1: ground pressure calibration
2411+
0, # param 4, 1: radio RC calibration, 2: RC trim calibration
2412+
0, # param 5, 1: accelerometer calibration, 2: board level calibration, 3: accelerometer temperature calibration, 4: simple accelerometer calibration
2413+
0, # param 6, 2: airspeed calibration
2414+
0, # param 7, 1: ESC calibration, 3: barometer temperature calibration
2415+
)
2416+
self.send_mavlink(calibration_command)
2417+
2418+
def calibrate_magnetometer(self):
2419+
"""Request magnetometer calibration."""
2420+
2421+
# APM requires the MAV_CMD_DO_START_MAG_CAL command, only present in the APM MAVLink dialect
2422+
if self._autopilot_type == mavutil.mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA:
2423+
calibration_command = self.message_factory.command_long_encode(
2424+
0, 0, # target_system, target_component
2425+
mavutil.mavlink.MAV_CMD_DO_START_MAG_CAL, # command
2426+
0, # confirmation
2427+
0, # param 1, uint8_t bitmask of magnetometers (0 means all).
2428+
1, # param 2, Automatically retry on failure (0=no retry, 1=retry).
2429+
1, # param 3, Save without user input (0=require input, 1=autosave).
2430+
0, # param 4, Delay (seconds).
2431+
1, # param 5, Autoreboot (0=user reboot, 1=autoreboot).
2432+
0, # param 6, Empty.
2433+
0, # param 7, Empty.
2434+
)
2435+
else:
2436+
calibration_command = self.message_factory.command_long_encode(
2437+
0, 0, # target_system, target_component
2438+
mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, # command
2439+
0, # confirmation
2440+
0, # param 1, 1: gyro calibration, 3: gyro temperature calibration
2441+
1, # param 2, 1: magnetometer calibration
2442+
0, # param 3, 1: ground pressure calibration
2443+
0, # param 4, 1: radio RC calibration, 2: RC trim calibration
2444+
0, # param 5, 1: accelerometer calibration, 2: board level calibration, 3: accelerometer temperature calibration, 4: simple accelerometer calibration
2445+
0, # param 6, 2: airspeed calibration
2446+
0, # param 7, 1: ESC calibration, 3: barometer temperature calibration
2447+
)
2448+
2449+
self._logger.critical(calibration_command)
2450+
self.send_mavlink(calibration_command)
2451+
2452+
def calibrate_accelerometer(self):
2453+
"""Request accelerometer calibration."""
2454+
2455+
calibration_command = self.message_factory.command_long_encode(
2456+
0, 0, # target_system, target_component
2457+
mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, # command
2458+
0, # confirmation
2459+
0, # param 1, 1: gyro calibration, 3: gyro temperature calibration
2460+
0, # param 2, 1: magnetometer calibration
2461+
0, # param 3, 1: ground pressure calibration
2462+
0, # param 4, 1: radio RC calibration, 2: RC trim calibration
2463+
1, # param 5, 1: accelerometer calibration, 2: board level calibration, 3: accelerometer temperature calibration, 4: simple accelerometer calibration
2464+
0, # param 6, 2: airspeed calibration
2465+
0, # param 7, 1: ESC calibration, 3: barometer temperature calibration
2466+
)
2467+
self.send_mavlink(calibration_command)
2468+
2469+
def calibrate_accelerometer_simple(self):
2470+
"""Request simple accelerometer calibration."""
2471+
2472+
calibration_command = self.message_factory.command_long_encode(
2473+
0, 0, # target_system, target_component
2474+
mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, # command
2475+
0, # confirmation
2476+
0, # param 1, 1: gyro calibration, 3: gyro temperature calibration
2477+
0, # param 2, 1: magnetometer calibration
2478+
0, # param 3, 1: ground pressure calibration
2479+
0, # param 4, 1: radio RC calibration, 2: RC trim calibration
2480+
4, # param 5, 1: accelerometer calibration, 2: board level calibration, 3: accelerometer temperature calibration, 4: simple accelerometer calibration
2481+
0, # param 6, 2: airspeed calibration
2482+
0, # param 7, 1: ESC calibration, 3: barometer temperature calibration
2483+
)
2484+
self.send_mavlink(calibration_command)
2485+
2486+
def calibrate_board_level(self):
2487+
"""Request board level calibration."""
2488+
2489+
calibration_command = self.message_factory.command_long_encode(
2490+
0, 0, # target_system, target_component
2491+
mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, # command
2492+
0, # confirmation
2493+
0, # param 1, 1: gyro calibration, 3: gyro temperature calibration
2494+
0, # param 2, 1: magnetometer calibration
2495+
0, # param 3, 1: ground pressure calibration
2496+
0, # param 4, 1: radio RC calibration, 2: RC trim calibration
2497+
2, # param 5, 1: accelerometer calibration, 2: board level calibration, 3: accelerometer temperature calibration, 4: simple accelerometer calibration
2498+
0, # param 6, 2: airspeed calibration
2499+
0, # param 7, 1: ESC calibration, 3: barometer temperature calibration
2500+
)
2501+
self.send_mavlink(calibration_command)
2502+
2503+
def calibrate_barometer(self):
2504+
"""Request barometer calibration."""
2505+
2506+
calibration_command = self.message_factory.command_long_encode(
2507+
0, 0, # target_system, target_component
2508+
mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, # command
2509+
0, # confirmation
2510+
0, # param 1, 1: gyro calibration, 3: gyro temperature calibration
2511+
0, # param 2, 1: magnetometer calibration
2512+
1, # param 3, 1: ground pressure calibration
2513+
0, # param 4, 1: radio RC calibration, 2: RC trim calibration
2514+
0, # param 5, 1: accelerometer calibration, 2: board level calibration, 3: accelerometer temperature calibration, 4: simple accelerometer calibration
2515+
0, # param 6, 2: airspeed calibration
2516+
0, # param 7, 1: ESC calibration, 3: barometer temperature calibration
2517+
)
2518+
self.send_mavlink(calibration_command)
2519+
24012520

24022521
class Gimbal(object):
24032522
"""

‎dronekit/test/sitl/__init__.py‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
import time
2+
from contextlib import contextmanager
3+
from nose.tools import assert_equal
4+
from pymavlink import mavutil
5+
6+
7+
@contextmanager
8+
def assert_command_ack(
9+
vehicle,
10+
command_type,
11+
ack_result=mavutil.mavlink.MAV_RESULT_ACCEPTED,
12+
timeout=10
13+
):
14+
"""Context manager to assert that:
15+
16+
1) exactly one COMMAND_ACK is received from a Vehicle;
17+
2) for a specific command type;
18+
3) with the given result;
19+
4) within a timeout (in seconds).
20+
21+
For example:
22+
23+
.. code-block:: python
24+
25+
with assert_command_ack(vehicle, mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, timeout=30):
26+
vehicle.calibrate_gyro()
27+
28+
"""
29+
30+
acks = []
31+
32+
def on_ack(self, name, message):
33+
if message.command == command_type:
34+
acks.append(message)
35+
36+
vehicle.add_message_listener('COMMAND_ACK', on_ack)
37+
38+
yield
39+
40+
start_time = time.time()
41+
while not acks and time.time() - start_time < timeout:
42+
time.sleep(0.1)
43+
vehicle.remove_message_listener('COMMAND_ACK', on_ack)
44+
45+
assert_equal(1, len(acks)) # one and only one ACK
46+
assert_equal(command_type, acks[0].command) # for the correct command
47+
assert_equal(ack_result, acks[0].result) # the result must be successful

‎dronekit/test/sitl/test_sensor_calibration.py‎

Lines changed: 95 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,95 @@
1+
from pymavlink import mavutil
2+
3+
from dronekit import connect
4+
from dronekit.test import with_sitl
5+
6+
from dronekit.test.sitl import assert_command_ack
7+
8+
9+
@with_sitl
10+
def test_gyro_calibration(connpath):
11+
"""Request gyroscope calibration, and check for the COMMAND_ACK."""
12+
13+
vehicle = connect(connpath, wait_ready=True)
14+
15+
with assert_command_ack(vehicle, mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, timeout=30):
16+
vehicle.calibrate_gyro()
17+
18+
vehicle.close()
19+
20+
21+
@with_sitl
22+
def test_magnetometer_calibration(connpath):
23+
"""Request magnetometer calibration, and check for the COMMAND_ACK."""
24+
25+
vehicle = connect(connpath, wait_ready=True)
26+
27+
with assert_command_ack(
28+
vehicle,
29+
mavutil.mavlink.MAV_CMD_DO_START_MAG_CAL,
30+
timeout=30,
31+
ack_result=mavutil.mavlink.MAV_RESULT_UNSUPPORTED, # TODO: change when APM is upgraded
32+
):
33+
vehicle.calibrate_magnetometer()
34+
35+
vehicle.close()
36+
37+
38+
@with_sitl
39+
def test_simple_accelerometer_calibration(connpath):
40+
"""Request simple accelerometer calibration, and check for the COMMAND_ACK."""
41+
42+
vehicle = connect(connpath, wait_ready=True)
43+
44+
with assert_command_ack(
45+
vehicle,
46+
mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION,
47+
timeout=30,
48+
ack_result=mavutil.mavlink.MAV_RESULT_FAILED, # TODO: change when APM is upgraded
49+
):
50+
vehicle.calibrate_accelerometer_simple()
51+
52+
vehicle.close()
53+
54+
55+
@with_sitl
56+
def test_accelerometer_calibration(connpath):
57+
"""Request accelerometer calibration, and check for the COMMAND_ACK."""
58+
59+
vehicle = connect(connpath, wait_ready=True)
60+
61+
# The calibration is expected to fail because in the SITL we don't tilt the Vehicle.
62+
# We just check that the command isn't denied or unsupported.
63+
with assert_command_ack(
64+
vehicle,
65+
mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION,
66+
timeout=30,
67+
ack_result=mavutil.mavlink.MAV_RESULT_FAILED,
68+
):
69+
vehicle.calibrate_accelerometer()
70+
71+
vehicle.close()
72+
73+
74+
@with_sitl
75+
def test_board_level_calibration(connpath):
76+
"""Request board level calibration, and check for the COMMAND_ACK."""
77+
78+
vehicle = connect(connpath, wait_ready=True)
79+
80+
with assert_command_ack(vehicle, mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, timeout=30):
81+
vehicle.calibrate_board_level()
82+
83+
vehicle.close()
84+
85+
86+
@with_sitl
87+
def test_barometer_calibration(connpath):
88+
"""Request barometer calibration, and check for the COMMAND_ACK."""
89+
90+
vehicle = connect(connpath, wait_ready=True)
91+
92+
with assert_command_ack(vehicle, mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, timeout=30):
93+
vehicle.calibrate_barometer()
94+
95+
vehicle.close()

0 commit comments

Comments
 (0)