python/examples/example.py

   1 ##
   2 ##############################################################################
   3 #
   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
  10 #
  11 #############################################################################/
  12 #
  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.
  17 #
  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.
  22 #
  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
  26 #
  27 #
  28
  29
  30 import logging
  31 import serial
  32 import traceback
  33 import sys
  34
  35 from librepilot.uavtalk.uavobject import *
  36 from librepilot.uavtalk.uavtalk import *
  37 from librepilot.uavtalk.objectManager import *
  38 from librepilot.uavtalk.connectionManager import *
  39
  40
  41 def _hex02(value):
  42     return "%02X" % value
  43
  44
  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
  52
  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")
  62
  63         print "Creating UavTalk"
  64         self.uavTalk = UavTalk(serPort, None)
  65
  66         print "Starting ObjectManager"
  67         self.objMan = ObjManager(self.uavTalk)
  68         self.objMan.importDefinitions()
  69
  70         print "Starting UavTalk"
  71         self.uavTalk.start()
  72
  73         print "Starting ConnectionManager"
  74         self.connMan = ConnectionManager(self.uavTalk, self.objMan)
  75
  76         print "Connecting...",
  77         self.connMan.connect()
  78         print "Connected"
  79
  80         print "Getting all Data"
  81         self.objMan.requestAllObjUpdate()
  82
  83     # print "SN:",
  84     # sn = self.objMan.FirmwareIAPObj.CPUSerial.value
  85     # print "".join(map(_hex02, sn))
  86
  87     def stop(self):
  88         if self.uavTalk:
  89             print "Stopping UavTalk"
  90             self.uavTalk.stop()
  91
  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()
  97
  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)
 103
 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()
 109
 110         while True:
 111             self.objMan.AttitudeState.waitUpdate()
 112             self._onAttitudeUpdate(self.objMan.AttitudeState)
 113
 114     def showAttitudeViaGet(self):
 115         while True:
 116             self.objMan.AttitudeState.getUpdate()
 117             self._onAttitudeUpdate(self.objMan.AttitudeState)
 118
 119     def _onAttitudeUpdate(self, args):
 120         self.nbUpdates += 1
 121
 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
 127
 128         if self.nbUpdates & 1:
 129             dot = "."
 130         else:
 131             dot = " "
 132
 133         print " %s Rate: %02.1f Hz  " % (dot, self.updateRate),
 134
 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) + " "
 138
 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",
 146
 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()
 151
 152         while True:
 153             self.objMan.ActuatorCommand.Channel.value[0] = 1000
 154             self.objMan.ActuatorCommand.updated()
 155             time.sleep(1)
 156
 157             self.objMan.ActuatorCommand.Channel.value[0] = 2000
 158             self.objMan.ActuatorCommand.updated()
 159             time.sleep(1)
 160
 161
 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()
 168
 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)
 177
 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()
 182
 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)
 192
 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)
 205
 206         self.objMan.ManualControlCommand.Pitch.value = 0
 207         self.objMan.ManualControlCommand.updated()
 208         time.sleep(0.5)
 209
 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)
 218
 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)
 223
 224         self.objMan.ManualControlCommand.Roll.value = 0
 225         self.objMan.ManualControlCommand.updated()
 226         time.sleep(0.5)
 227
 228         print "Setting Throttle to minimum"
 229         self.objMan.ManualControlCommand.Throttle.value = -1
 230         self.objMan.ManualControlCommand.updated()
 231         time.sleep(1)
 232
 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)
 239
 240         self.objMan.ManualControlCommand.Yaw.value = 0
 241         self.objMan.ManualControlCommand.updated()
 242         time.sleep(1)
 243
 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()
 249
 250
 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
 264
 265
 266 if __name__ == '__main__':
 267
 268     if len(sys.argv) != 3:
 269         print "ERROR: Incorrect number of arguments"
 270         printUsage()
 271         sys.exit(2)
 272
 273     port, option = sys.argv[1:]
 274
 275     if option not in ["o", "w", "g", "s", "i"]:
 276         print "ERROR: Invalid option"
 277         printUsage()
 278         sys.exit(2)
 279
 280     # Log everything, and send it to stderr.
 281     logging.basicConfig(level=logging.INFO)
 282
 283     try:
 284         demo = UavtalkDemo()
 285         demo.setup(port)
 286
 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
 297
 298     except KeyboardInterrupt:
 299         pass
 300     except Exception, e:
 301         print
 302         print "An error occured: ", e
 303         print
 304         traceback.print_exc()
 305
 306     print
 307
 308     try:
 309         demo.stop()
 310     except Exception:
 311         pass