@@ -380,6 +380,52 @@ def decorator(fn):
380
380
self .on_attribute (name , fn )
381
381
return decorator
382
382
383
+ class Channels (dict ):
384
+ """
385
+ Implements RC object.
386
+ """
387
+
388
+ def __init__ (self , vehicle , count ):
389
+ self ._count = count
390
+ self ._readonly = False
391
+ self ._overrides = {}
392
+ for k in range (0 , count ):
393
+ self [k + 1 ] = None
394
+ self ._readonly = True
395
+
396
+ @property
397
+ def count (self ):
398
+ return self ._count
399
+
400
+ def __setitem__ (self , key , value ):
401
+ if self ._readonly :
402
+ raise TypeError ('__setitem__ is not supported on Channels object' )
403
+ return dict .__setitem__ (self , key , value )
404
+
405
+ def __len__ (self ):
406
+ return self ._count
407
+
408
+ def _update_channel (self , channel , value ):
409
+ # If we have channels on different ports, we expand the Channels
410
+ # object to support them.
411
+ channel = str (channel )
412
+ self ._readonly = False
413
+ self [channel ] = value
414
+ self ._readonly = True
415
+ self ._count = max (self ._count , int (channel ))
416
+
417
+ @property
418
+ def overrides (self ):
419
+ return copy .copy (self ._overrides )
420
+
421
+ @overrides .setter
422
+ def overrides (self , newch ):
423
+ self ._overrides = {}
424
+ for k , v in newch .iteritems ():
425
+ if v :
426
+ self ._overrides [str (k )] = v
427
+ self ._master .mav .rc_channels_override_send (0 , 0 , * list (self ._overrides .values ()))
428
+
383
429
class Vehicle (HasObservers ):
384
430
"""
385
431
The main vehicle API
@@ -656,14 +702,15 @@ def listener(self, name, m):
656
702
self ._mount_yaw = m .pointing_c / 100
657
703
self ._notify_attribute_listeners ('mount' )
658
704
659
- self ._rc_readback = {}
705
+ # All keys are strings.
706
+ self ._channels = Channels (self , 8 )
660
707
661
708
@self .message_listener ('RC_CHANNELS_RAW' )
662
709
def listener (self , name , m ):
663
710
def set_rc (chnum , v ):
664
711
'''Private utility for handling rc channel messages'''
665
712
# use port to allow ch nums greater than 8
666
- self ._rc_readback [ str (m .port * 8 + chnum )] = v
713
+ self ._channels . _update_channel ( str (m .port * 8 + chnum ), v )
667
714
668
715
set_rc (1 , m .chan1_raw )
669
716
set_rc (2 , m .chan2_raw )
@@ -1028,21 +1075,8 @@ def ekf_ok(self):
1028
1075
return self ._ekf_poshorizabs or self ._ekf_predposhorizabs
1029
1076
1030
1077
@property
1031
- def channel_override (self ):
1032
- overrides = self .__rc .override
1033
- # Only return entries that have a non zero override
1034
- return dict ((str (num + 1 ), overrides [num ]) for num in range (8 ) if overrides [num ] != 0 )
1035
-
1036
- @channel_override .setter
1037
- def channel_override (self , newch ):
1038
- overrides = self .__rc .override
1039
- for k , v in newch .iteritems ():
1040
- overrides [int (k ) - 1 ] = v
1041
- self .__rc .set_override (overrides )
1042
-
1043
- @property
1044
- def channel_readback (self ):
1045
- return copy .copy (self ._rc_readback )
1078
+ def channels (self ):
1079
+ return self ._channels
1046
1080
1047
1081
@property
1048
1082
def home_location (self ):
0 commit comments