/
mavproxy_cameraview.py
148 lines (135 loc) · 6.51 KB
/
mavproxy_cameraview.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
#!/usr/bin/env python
'''
camera view module
Malcolm Gill
Feb 2014
'''
import math
from MAVProxy.modules.mavproxy_map import mp_slipmap
from MAVProxy.modules.lib import mp_util
from MAVProxy.modules.lib import mp_settings
from cuav.lib import cuav_util
from cuav.camera.cam_params import CameraParams
# documented in common.xml, can't find these constants in code
scale_latlon = 1e-7
scale_hdg = 1e-2
scale_relative_alt = 1e-3
from MAVProxy.modules.lib import mp_module
class CameraViewModule(mp_module.MPModule):
def __init__(self, mpstate):
super(CameraViewModule, self).__init__(mpstate, "cameraview")
self.add_command('cameraview', self.cmd_cameraview, "camera view")
self.roll = 0
self.pitch = 0
self.yaw = 0
self.mount_roll = 0
self.mount_pitch = 0
self.mount_yaw = 0
self.height = 0
self.lat = 0
self.lon = 0
self.home_height = 0
self.hdg = 0
self.camera_params = CameraParams() # TODO how to get actual camera params
self.view_settings = mp_settings.MPSettings(
[ ('r', float, 0.5),
('g', float, 0.5),
('b', float, 1.0),
])
self.update_col()
def update_col(self):
self.col = tuple(int(255*c) for c in (self.view_settings.r, self.view_settings.g, self.view_settings.b))
def cmd_cameraview(self, args):
'''camera view commands'''
state = self
if args and args[0] == 'set':
if len(args) < 3:
state.view_settings.show_all()
else:
state.view_settings.set(args[1], args[2])
state.update_col()
else:
print('usage: cameraview set')
def unload(self):
'''unload module'''
pass
def scale_rc(self, servo, min, max, param):
'''scale a PWM value'''
# default to servo range of 1000 to 2000
min_pwm = self.get_mav_param('%s_MIN' % param, 0)
max_pwm = self.get_mav_param('%s_MAX' % param, 0)
if min_pwm == 0 or max_pwm == 0:
return 0
if max_pwm == min_pwm:
p = 0.0
else:
p = (servo-min_pwm) / float(max_pwm-min_pwm)
v = min + p*(max-min)
if v < min:
v = min
if v > max:
v = max
return v
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
state = self
mtype = m.get_type()
if mtype == 'GLOBAL_POSITION_INT':
state.lat, state.lon = m.lat*scale_latlon, m.lon*scale_latlon
state.hdg = m.hdg*scale_hdg
agl = self.module('terrain').ElevationModel.GetElevation(state.lat, state.lon)
if agl is not None:
state.height = m.relative_alt*scale_relative_alt + state.home_height - agl
elif mtype == 'ATTITUDE':
state.roll, state.pitch, state.yaw = math.degrees(m.roll), math.degrees(m.pitch), math.degrees(m.yaw)
elif mtype in ['GPS_RAW', 'GPS_RAW_INT']:
if self.module('wp').wploader.count() > 0:
home = self.module('wp').wploader.wp(0).x, self.module('wp').wploader.wp(0).y
else:
home = [self.master.field('HOME', c)*scale_latlon for c in ['lat', 'lon']]
old = state.home_height # TODO TMP
agl = self.module('terrain').ElevationModel.GetElevation(*home)
if agl is None:
return
state.home_height = agl
# TODO TMP
if state.home_height != old:
# tridge said to get home pos from wploader,
# but this is not the same as from master() below...!!
# using master() gives the right coordinates
# (i.e. matches GLOBAL_POSITION_INT coords, and $IMHOME in sim_arduplane.sh)
# and wploader is a bit off
print('home height changed from',old,'to',state.home_height)
elif mtype == 'SERVO_OUTPUT_RAW':
for (axis, attr) in [('ROLL', 'mount_roll'), ('TILT', 'mount_pitch'), ('PAN', 'mount_yaw')]:
channel = int(self.get_mav_param('MNT_RC_IN_{0}'.format(axis), 0))
if self.get_mav_param('MNT_STAB_{0}'.format(axis), 0) and channel:
# enabled stabilisation on this axis
# TODO just guessing that RC_IN_ROLL gives the servo number, but no idea if this is really the case
servo = 'servo{0}_raw'.format(channel)
centidegrees = self.scale_rc(getattr(m, servo),
self.get_mav_param('MNT_ANGMIN_{0}'.format(axis[:3])),
self.get_mav_param('MNT_ANGMAX_{0}'.format(axis[:3])),
param='RC{0}'.format(channel))
setattr(state, attr, centidegrees*0.01)
#state.mount_roll = min(max(-state.roll,-45),45)#TODO TMP
#state.mount_yaw = min(max(-state.yaw,-45),45)#TODO TMP
#state.mount_pitch = min(max(-state.pitch,-45),45)#TODO TMP
else:
return
if self.mpstate.map: # if the map module is loaded, redraw polygon
# get rid of the old polygon
self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('CameraView'))
# camera view polygon determined by projecting corner pixels of the image onto the ground
pixel_positions = [cuav_util.pixel_position(px[0],px[1], state.height, state.pitch+state.mount_pitch, state.roll+state.mount_roll, state.yaw+state.mount_yaw, state.camera_params) for px in [(0,0), (state.camera_params.xresolution,0), (state.camera_params.xresolution,state.camera_params.yresolution), (0,state.camera_params.yresolution)]]
if any(pixel_position is None for pixel_position in pixel_positions):
# at least one of the pixels is not on the ground
# so it doesn't make sense to try to draw the polygon
return
gps_positions = [mp_util.gps_newpos(state.lat, state.lon, math.degrees(math.atan2(*pixel_position)), math.hypot(*pixel_position)) for pixel_position in pixel_positions]
# draw new polygon
self.mpstate.map.add_object(mp_slipmap.SlipPolygon('cameraview', gps_positions+[gps_positions[0]], # append first element to close polygon
layer='CameraView', linewidth=2, colour=state.col))
def init(mpstate):
'''initialise module'''
return CameraViewModule(mpstate)