python/examples/example_tuning.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
  42 def printUsage():
  43     appName = os.path.basename(sys.argv[0])
  44     print
  45     print "usage:"
  46     print "  %s port " % appName
  47     print "  for example: %s COM30 " % appName
  48     print
  49
  50 if __name__ == '__main__':
  51     try:
  52         if len(sys.argv) != 2:
  53           print "ERROR: Incorrect number of arguments"
  54           printUsage()
  55           sys.exit(2)
  56
  57         port = sys.argv[1]
  58
  59         if port[:3].upper() == "COM":
  60             _port = int(port[3:])-1
  61         else:
  62             _port = port
  63
  64         serPort = serial.Serial(_port, 57600, timeout=.5)
  65         uavTalk = UavTalk(serPort, None)
  66
  67         print "Starting ObjectManager"
  68         objMan = ObjManager(uavTalk)
  69         objMan.importDefinitions()
  70
  71         print "Starting UavTalk"
  72         uavTalk.start()
  73
  74
  75         print "Getting Current Settings:"
  76         for _ in range(2): # Try only twice to get the settings
  77             try:
  78                 time.sleep(1)
  79                 objMan.StabilizationSettingsBank1.getUpdate()
  80             except TimeoutException:
  81                 print "Timeout"
  82                 pass
  83             except KeyboardInterrupt:
  84                 exit(1)
  85
  86         while True:
  87             while True:
  88                 print
  89                 print
  90                 print "q. Quit"
  91                 print "s. Save settings"
  92                 print
  93                 print "1. Tune  Roll  Rate      %2.4f %2.4f %2.4f %2.4f" % tuple(objMan.StabilizationSettingsBank1.RollRatePID.value)
  94                 print "2. Tune  Pitch Rate      %2.4f %2.4f %2.4f %2.4f" % tuple(objMan.StabilizationSettingsBank1.PitchRatePID.value)
  95                 print "3. Tune  Yaw   Rate      %2.4f %2.4f %2.4f %2.4f" % tuple(objMan.StabilizationSettingsBank1.YawRatePID.value)
  96                 print
  97                 print "4. Tune  Roll  Attitude  %2.4f %2.4f %2.4f" % tuple(objMan.StabilizationSettingsBank1.RollPI.value)
  98                 print "5. Tune  Pitch Attitude  %2.4f %2.4f %2.4f" % tuple(objMan.StabilizationSettingsBank1.PitchPI.value)
  99                 print "6. Tune  Yaw   Attitude  %2.4f %2.4f %2.4f" % tuple(objMan.StabilizationSettingsBank1.YawPI.value)
 100                 print
 101                 sel = raw_input()
 102                 if len(sel) != 1:
 103                     continue
 104                 elif sel == "q":
 105                     exit(0)
 106                 elif sel == "s":
 107                     print "Saving ",
 108                     objMan.ObjectPersistence.Operation.value = objectpersistence.OperationField.SAVE
 109                     objMan.ObjectPersistence.Selection.value = objectpersistence.SelectionField.SINGLEOBJECT
 110                     objMan.ObjectPersistence.ObjectID.value = objMan.StabilizationSettingsBank1.objId
 111                     objMan.ObjectPersistence.InstanceID.value = objMan.StabilizationSettingsBank1.instId
 112                     objMan.ObjectPersistence.updated()
 113                     for i in range(10):
 114                         print ".",
 115                         objMan.ObjectPersistence.getUpdate(timeout=1)
 116                         if objMan.ObjectPersistence.Operation.value == objectpersistence.OperationField.COMPLETED:
 117                             print "Done"
 118                             break
 119                     print
 120                 elif sel == "1":
 121                     PI = objMan.StabilizationSettingsBank1.RollRatePID.value
 122                     break
 123                 elif sel == "2":
 124                     PI = objMan.StabilizationSettingsBank1.PitchRatePI.value
 125                     break
 126                 elif sel == "3":
 127                     PI = objMan.StabilizationSettingsBank1.YawRatePI.value
 128                     break
 129                 elif sel == "4":
 130                     PI = objMan.StabilizationSettingsBank1.RollPI.value
 131                     break
 132                 elif sel == "5":
 133                     PI = objMan.StabilizationSettingsBank1.PitchPI.value
 134                     break
 135                 elif sel == "6":
 136                     PI = objMan.StabilizationSettingsBank1.YawPI.value
 137                     break
 138
 139             while True:
 140                 print
 141                 print
 142                 print "q. Quit"
 143                 print
 144                 print "1. tune K       %2.4f" % PI[0]
 145                 print "2. tune I       %2.4f" % PI[1]
 146                 print "3. tune I Limit %2.4f" % PI[2]
 147                 sel = raw_input()
 148                 if len(sel) != 1:
 149                     continue
 150                 elif sel == "q":
 151                     exit(0)
 152                 elif sel == "1":
 153                     PIIndex = 0
 154                     break
 155                 elif sel == "2":
 156                     PIIndex = 1
 157                     break
 158                 elif sel == "3":
 159                     PIIndex = 2
 160                     break
 161
 162             print
 163
 164             while True:
 165                 try:
 166                     print "Current value: %2.4f" % PI[PIIndex]
 167                     print "Tune-range from",
 168                     tuneFrom = float(raw_input())
 169                     print "Tune-range to",
 170                     tuneTo = float(raw_input())
 171                     break
 172                 except Exception:
 173                     pass
 174
 175
 176             print
 177             print
 178             while True:
 179                 try:
 180                     # get update of AccessoryDesired
 181                     objMan.AccessoryDesired.getUpdate(timeout=.5)
 182
 183                     # calculate value out of Accessory0 input (-1 ... +1)
 184                     txControl = objMan.AccessoryDesired.AccessoryVal.value
 185                     value = tuneFrom + ((txControl+1)/2)*(tuneTo-tuneFrom)
 186                     PI[PIIndex] = value
 187                     objMan.StabilizationSettingsBank1.updated()
 188
 189                     print "\r%-1.2f => %2.4f" % (txControl, value),
 190                     time.sleep(.1)
 191
 192                 except TimeoutException:
 193                     print "Timeout \a"
 194             print
 195     except Exception,e:
 196         print
 197         print "An error occured: ", e
 198         print
 199         traceback.print_exc()
 200
 201     try:
 202         print "Stop"
 203         uavTalk.stop()
 204     except Exception:
 205         pass
 206