@@ -56,6 +56,9 @@ class Result(Enum):
56
56
CANCELLED
57
57
Calibration process was cancelled
58
58
59
+ FAILED_ARMED
60
+ Calibration process failed since the vehicle is armed
61
+
59
62
"""
60
63
61
64
@@ -69,6 +72,7 @@ class Result(Enum):
69
72
COMMAND_DENIED = 7
70
73
TIMEOUT = 8
71
74
CANCELLED = 9
75
+ FAILED_ARMED = 10
72
76
73
77
def translate_to_rpc (self , rpcResult ):
74
78
if self == CalibrationResult .Result .UNKNOWN :
@@ -91,6 +95,8 @@ def translate_to_rpc(self, rpcResult):
91
95
return calibration_pb2 .CalibrationResult .RESULT_TIMEOUT
92
96
if self == CalibrationResult .Result .CANCELLED :
93
97
return calibration_pb2 .CalibrationResult .RESULT_CANCELLED
98
+ if self == CalibrationResult .Result .FAILED_ARMED :
99
+ return calibration_pb2 .CalibrationResult .RESULT_FAILED_ARMED
94
100
95
101
@staticmethod
96
102
def translate_from_rpc (rpc_enum_value ):
@@ -115,6 +121,8 @@ def translate_from_rpc(rpc_enum_value):
115
121
return CalibrationResult .Result .TIMEOUT
116
122
if rpc_enum_value == calibration_pb2 .CalibrationResult .RESULT_CANCELLED :
117
123
return CalibrationResult .Result .CANCELLED
124
+ if rpc_enum_value == calibration_pb2 .CalibrationResult .RESULT_FAILED_ARMED :
125
+ return CalibrationResult .Result .FAILED_ARMED
118
126
119
127
def __str__ (self ):
120
128
return self .name
@@ -401,7 +409,7 @@ async def calibrate_accelerometer(self):
401
409
402
410
async def calibrate_magnetometer (self ):
403
411
"""
404
- Perform magnetometer caliration .
412
+ Perform magnetometer calibration .
405
413
406
414
Yields
407
415
-------
@@ -439,6 +447,46 @@ async def calibrate_magnetometer(self):
439
447
finally :
440
448
calibrate_magnetometer_stream .cancel ()
441
449
450
+ async def calibrate_level_horizon (self ):
451
+ """
452
+ Perform board level horizon calibration.
453
+
454
+ Yields
455
+ -------
456
+ progress_data : ProgressData
457
+ Progress data
458
+
459
+ Raises
460
+ ------
461
+ CalibrationError
462
+ If the request fails. The error contains the reason for the failure.
463
+ """
464
+
465
+ request = calibration_pb2 .SubscribeCalibrateLevelHorizonRequest ()
466
+ calibrate_level_horizon_stream = self ._stub .SubscribeCalibrateLevelHorizon (request )
467
+
468
+ try :
469
+ async for response in calibrate_level_horizon_stream :
470
+
471
+ result = self ._extract_result (response )
472
+
473
+ success_codes = [CalibrationResult .Result .SUCCESS ]
474
+ if 'NEXT' in [return_code .name for return_code in CalibrationResult .Result ]:
475
+ success_codes .append (CalibrationResult .Result .NEXT )
476
+
477
+ if result .result not in success_codes :
478
+ raise CalibrationError (result , "calibrate_level_horizon()" )
479
+
480
+ if result .result is CalibrationResult .Result .SUCCESS :
481
+ calibrate_level_horizon_stream .cancel ();
482
+ return
483
+
484
+
485
+
486
+ yield ProgressData .translate_from_rpc (response .progress_data )
487
+ finally :
488
+ calibrate_level_horizon_stream .cancel ()
489
+
442
490
async def calibrate_gimbal_accelerometer (self ):
443
491
"""
444
492
Perform gimbal accelerometer calibration.
0 commit comments