Merged in f5soh/librepilot/update_credits (pull request #529)
[librepilot.git] / python / examples / example.py
bloba43fbbf20bdd79e744c34734726069eb63676864
1 ##
2 ##############################################################################
4 # @file example.py
5 # @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017.
6 # The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
7 # @brief Base classes for python UAVObject
8 #
9 # @see The GNU Public License (GPL) Version 3
11 #############################################################################/
13 # This program is free software; you can redistribute it and/or modify
14 # it under the terms of the GNU General Public License as published by
15 # the Free Software Foundation; either version 3 of the License, or
16 # (at your option) any later version.
18 # This program is distributed in the hope that it will be useful, but
19 # WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
20 # or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
21 # for more details.
23 # You should have received a copy of the GNU General Public License along
24 # with this program; if not, write to the Free Software Foundation, Inc.,
25 # 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
30 import logging
31 import serial
32 import traceback
33 import sys
35 from librepilot.uavtalk.uavobject import *
36 from librepilot.uavtalk.uavtalk import *
37 from librepilot.uavtalk.objectManager import *
38 from librepilot.uavtalk.connectionManager import *
41 def _hex02(value):
42 return "%02X" % value
45 class UavtalkDemo():
46 def __init__(self):
47 self.nbUpdates = 0
48 self.lastRateCalc = time.time()
49 self.updateRate = 0
50 self.objMan = None
51 self.connMan = None
53 def setup(self, port):
54 print "Opening Port \"%s\"" % port
55 if port[:3].upper() == "COM":
56 _port = int(port[3:]) - 1
57 else:
58 _port = port
59 serPort = serial.Serial(_port, 57600, timeout=.5)
60 if not serPort.isOpen():
61 raise IOError("Failed to open serial port")
63 print "Creating UavTalk"
64 self.uavTalk = UavTalk(serPort, None)
66 print "Starting ObjectManager"
67 self.objMan = ObjManager(self.uavTalk)
68 self.objMan.importDefinitions()
70 print "Starting UavTalk"
71 self.uavTalk.start()
73 print "Starting ConnectionManager"
74 self.connMan = ConnectionManager(self.uavTalk, self.objMan)
76 print "Connecting...",
77 self.connMan.connect()
78 print "Connected"
80 print "Getting all Data"
81 self.objMan.requestAllObjUpdate()
83 # print "SN:",
84 # sn = self.objMan.FirmwareIAPObj.CPUSerial.value
85 # print "".join(map(_hex02, sn))
87 def stop(self):
88 if self.uavTalk:
89 print "Stopping UavTalk"
90 self.uavTalk.stop()
92 def showAttitudeViaObserver(self):
93 print "Request fast periodic updates for AttitudeState"
94 self.objMan.AttitudeState.metadata.telemetryUpdateMode = UAVMetaDataObject.UpdateMode.PERIODIC
95 self.objMan.AttitudeState.metadata.telemetryUpdatePeriod.value = 50
96 self.objMan.AttitudeState.metadata.updated()
98 print "Install Observer for AttitudeState updates\n"
99 self.objMan.regObjectObserver(self.objMan.AttitudeState, self, "_onAttitudeUpdate")
100 # Spin until we get interrupted
101 while True:
102 time.sleep(1)
104 def showAttitudeViaWait(self):
105 print "Request fast periodic updates for AttitudeState"
106 self.objMan.AttitudeState.metadata.telemetryUpdateMode = UAVMetaDataObject.UpdateMode.PERIODIC
107 self.objMan.AttitudeState.metadata.telemetryUpdatePeriod.value = 50
108 self.objMan.AttitudeState.metadata.updated()
110 while True:
111 self.objMan.AttitudeState.waitUpdate()
112 self._onAttitudeUpdate(self.objMan.AttitudeState)
114 def showAttitudeViaGet(self):
115 while True:
116 self.objMan.AttitudeState.getUpdate()
117 self._onAttitudeUpdate(self.objMan.AttitudeState)
119 def _onAttitudeUpdate(self, args):
120 self.nbUpdates += 1
122 now = time.time()
123 if now - self.lastRateCalc > 1:
124 self.updateRate = self.nbUpdates / (now - self.lastRateCalc)
125 self.lastRateCalc = now
126 self.nbUpdates = 0
128 if self.nbUpdates & 1:
129 dot = "."
130 else:
131 dot = " "
133 print " %s Rate: %02.1f Hz " % (dot, self.updateRate),
135 roll = self.objMan.AttitudeState.Roll.value
136 print "RPY: %f %f %f " % (self.objMan.AttitudeState.Roll.value, self.objMan.AttitudeState.Pitch.value,
137 self.objMan.AttitudeState.Yaw.value) + " "
139 # return
140 # print "Roll: %f " % roll,
141 # i = roll/90
142 # if i<-1: i=-1
143 # if i>1: i= 1
144 # i = int((i+1)*15)
145 # print "-"*i+"*"+"-"*(30-i)+" \r",
147 def driveServo(self):
148 print "Taking control of self.actuatorCmd"
149 self.objMan.ActuatorCommand.metadata.access = UAVMetaDataObject.Access.READONLY
150 self.objMan.ActuatorCommand.metadata.updated()
152 while True:
153 self.objMan.ActuatorCommand.Channel.value[0] = 1000
154 self.objMan.ActuatorCommand.updated()
155 time.sleep(1)
157 self.objMan.ActuatorCommand.Channel.value[0] = 2000
158 self.objMan.ActuatorCommand.updated()
159 time.sleep(1)
162 def driveInput(self):
163 print "Taking control of self.ManualControl"
164 self.objMan.ManualControlCommand.metadata.access = UAVMetaDataObject.Access.READONLY
165 self.objMan.ManualControlCommand.metadata.updated()
166 self.objMan.ManualControlCommand.Connected.value = True
167 self.objMan.ManualControlCommand.updated()
169 print "Arming board using Yaw right"
170 # FIXME: Seems there is a issue with ArmedField.ARMED, 2 equals to the ARMED state
171 while (self.objMan.FlightStatus.Armed.value != 2):
172 self.objMan.ManualControlCommand.Yaw.value = 1
173 self.objMan.ManualControlCommand.updated()
174 self.objMan.ManualControlCommand.Throttle.value = -1
175 self.objMan.ManualControlCommand.updated()
176 time.sleep(1)
178 if (self.objMan.FlightStatus.Armed.value == 2):
179 print "Board is armed !"
180 self.objMan.ManualControlCommand.Yaw.value = 0
181 self.objMan.ManualControlCommand.updated()
183 print "Applying Throttle"
184 self.objMan.ManualControlCommand.Throttle.value = 0.01 # very small value for safety
185 # Assuming board do not control a helicopter, Thrust value will be equal to Throttle value.
186 # Because a 'high' value can be read from latest real RC input value,
187 # initial value is set now to zero for safety reasons.
188 self.objMan.ManualControlCommand.Thrust.value = 0
189 # self.objMan.ManualControlCommand.Throttle.value = self.objMan.ManualControlCommand.Thrust.value
190 self.objMan.ManualControlCommand.updated()
191 time.sleep(0.3)
193 count = 60
194 print "Moving Pitch input"
195 while (count > 0):
196 count-=1
197 if self.objMan.ManualControlCommand.Pitch.value < 1:
198 self.objMan.ManualControlCommand.Pitch.value += 0.05
199 self.objMan.ManualControlCommand.updated()
200 time.sleep(0.1)
201 if self.objMan.ManualControlCommand.Pitch.value > 1:
202 self.objMan.ManualControlCommand.Pitch.value = 0
203 self.objMan.ManualControlCommand.updated()
204 time.sleep(0.1)
206 self.objMan.ManualControlCommand.Pitch.value = 0
207 self.objMan.ManualControlCommand.updated()
208 time.sleep(0.5)
210 count = 60
211 print "Moving Roll input"
212 while (count > 0):
213 count-=1
214 if self.objMan.ManualControlCommand.Roll.value < 1:
215 self.objMan.ManualControlCommand.Roll.value += 0.05
216 self.objMan.ManualControlCommand.updated()
217 time.sleep(0.1)
219 if self.objMan.ManualControlCommand.Roll.value > 1:
220 self.objMan.ManualControlCommand.Roll.value = 0
221 self.objMan.ManualControlCommand.updated()
222 time.sleep(0.1)
224 self.objMan.ManualControlCommand.Roll.value = 0
225 self.objMan.ManualControlCommand.updated()
226 time.sleep(0.5)
228 print "Setting Throttle to minimum"
229 self.objMan.ManualControlCommand.Throttle.value = -1
230 self.objMan.ManualControlCommand.updated()
231 time.sleep(1)
233 print "Disarming board using Yaw left"
234 # FIXME: Seems there is a issue with ArmedField.DISARMED, 0 equals to the DISARMED state
235 while (self.objMan.FlightStatus.Armed.value != 0):
236 self.objMan.ManualControlCommand.Yaw.value = -1
237 self.objMan.ManualControlCommand.updated()
238 time.sleep(0.3)
240 self.objMan.ManualControlCommand.Yaw.value = 0
241 self.objMan.ManualControlCommand.updated()
242 time.sleep(1)
244 print "Back to self.ManualControl, controlled by RC radio"
245 self.objMan.ManualControlCommand.metadata.access = UAVMetaDataObject.Access.READWRITE
246 self.objMan.ManualControlCommand.metadata.updated()
247 self.objMan.ManualControlCommand.Connected.value = False
248 self.objMan.ManualControlCommand.updated()
251 def printUsage():
252 appName = os.path.basename(sys.argv[0])
253 print
254 print "usage:"
255 print " %s port o|w|g|s|i" % appName
256 print " o: Show Attitude using an \"observer\""
257 print " w: Show Attitude waiting for updates from flight"
258 print " g: Show Attitude performing get operations"
259 print " s: Drive Servo"
260 print " i: Take control over RC input"
261 print
262 print " for example: %s COM30 o" % appName
263 print
266 if __name__ == '__main__':
268 if len(sys.argv) != 3:
269 print "ERROR: Incorrect number of arguments"
270 printUsage()
271 sys.exit(2)
273 port, option = sys.argv[1:]
275 if option not in ["o", "w", "g", "s", "i"]:
276 print "ERROR: Invalid option"
277 printUsage()
278 sys.exit(2)
280 # Log everything, and send it to stderr.
281 logging.basicConfig(level=logging.INFO)
283 try:
284 demo = UavtalkDemo()
285 demo.setup(port)
287 if option == "o":
288 demo.showAttitudeViaObserver() # will not return
289 elif option == "w":
290 demo.showAttitudeViaWait() # will not return
291 if option == "g":
292 demo.showAttitudeViaGet() # will not return
293 if option == "s":
294 demo.driveServo() # will not return
295 if option == "i":
296 demo.driveInput() # will not return
298 except KeyboardInterrupt:
299 pass
300 except Exception, e:
301 print
302 print "An error occured: ", e
303 print
304 traceback.print_exc()
306 print
308 try:
309 demo.stop()
310 except Exception:
311 pass