00001
00002 """
00003 * dial.py
00004 *
00005 * Created on: 1 Nov 2010
00006 * Author: Duncan Law
00007 *
00008 * Copyright (C) 2010 Duncan Law
00009 * This program is free software; you can redistribute it and/or modify
00010 * it under the terms of the GNU General Public License as published by
00011 * the Free Software Foundation; either version 2 of the License, or
00012 * (at your option) any later version.
00013 *
00014 * This program is distributed in the hope that it will be useful,
00015 * but WITHOUT ANY WARRANTY; without even the implied warranty of
00016 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00017 * GNU General Public License for more details.
00018 *
00019 * You should have received a copy of the GNU General Public License
00020 * along with this program; if not, write to the Free Software
00021 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
00022
00023
00024 * Thanks to Chootair at http://www.codeproject.com/Members/Chootair
00025 * for the inspiration and the artwork that i based this code on.
00026 * His full work is intended for C# and can be found here:
00027 * http://www.codeproject.com/KB/miscctrl/Avionic_Instruments.aspx
00028
00029
00030
00031
00032 * Requires pySerial and pyGame to run.
00033 * http://pyserial.sourceforge.net
00034 * http://www.pygame.org
00035
00036 """
00037
00038 import math
00039 import serial
00040 import pygame
00041 from pygame.locals import *
00042 from sensor_msgs.msg import Imu
00043
00044 pygame.init()
00045 import sys
00046 import rospkg
00047 import rospy
00048 from rospy import Subscriber
00049 from tf.transformations import euler_from_quaternion
00050
00051 REV_MODE = -1
00052
00053 class Dial:
00054 """
00055 Generic dial type.
00056 """
00057
00058 def __init__(self, image, frameImage, x=0, y=0, w=0, h=0):
00059 """
00060 x,y = coordinates of top left of dial.
00061 w,h = Width and Height of dial.
00062 """
00063 self.x = x
00064 self.y = y
00065 self.image = image
00066 self.frameImage = frameImage
00067 self.dial = pygame.Surface(self.frameImage.get_rect()[2:4])
00068 self.dial.fill(0xFFFF00)
00069 if (w == 0):
00070 w = self.frameImage.get_rect()[2]
00071 if (h == 0):
00072 h = self.frameImage.get_rect()[3]
00073 self.w = w
00074 self.h = h
00075 self.pos = self.dial.get_rect()
00076 self.pos = self.pos.move(x, y)
00077
00078 def position(self, x, y):
00079 """
00080 Reposition top,left of dial at x,y.
00081 """
00082 self.x = x
00083 self.y = y
00084 self.pos[0] = x
00085 self.pos[1] = y
00086
00087 def position_center(self, x, y):
00088 """
00089 Reposition centre of dial at x,y.
00090 """
00091 self.x = x
00092 self.y = y
00093 self.pos[0] = x - self.pos[2] / 2
00094 self.pos[1] = y - self.pos[3] / 2
00095
00096 def rotate(self, image, angle):
00097 """
00098 Rotate supplied image by "angle" degrees.
00099 This rotates round the centre of the image.
00100 If you need to offset the centre, resize the image using self.clip.
00101 This is used to rotate dial needles and probably doesn't need to be used externally.
00102 """
00103 tmpImage = pygame.transform.rotate(image, angle)
00104 imageCentreX = tmpImage.get_rect()[0] + tmpImage.get_rect()[2] / 2
00105 imageCentreY = tmpImage.get_rect()[1] + tmpImage.get_rect()[3] / 2
00106
00107 targetWidth = tmpImage.get_rect()[2]
00108 targetHeight = tmpImage.get_rect()[3]
00109
00110 imageOut = pygame.Surface((targetWidth, targetHeight))
00111 imageOut.fill(0xFFFF00)
00112 imageOut.set_colorkey(0xFFFF00)
00113 imageOut.blit(tmpImage, (0, 0),
00114 pygame.Rect(imageCentreX - targetWidth / 2, imageCentreY - targetHeight / 2, targetWidth,
00115 targetHeight))
00116 return imageOut
00117
00118 def clip(self, image, x=0, y=0, w=0, h=0, oX=0, oY=0):
00119 """
00120 Cuts out a part of the needle image at x,y position to the correct size (w,h).
00121 This is put on to "imageOut" at an offset of oX,oY if required.
00122 This is used to centre dial needles and probably doesn't need to be used externally.
00123 """
00124 if (w == 0):
00125 w = image.get_rect()[2]
00126 if (h == 0):
00127 h = image.get_rect()[3]
00128 needleW = w + 2 * math.sqrt(oX * oX)
00129 needleH = h + 2 * math.sqrt(oY * oY)
00130 imageOut = pygame.Surface((needleW, needleH))
00131 imageOut.fill(0xFFFF00)
00132 imageOut.set_colorkey(0xFFFF00)
00133 imageOut.blit(image, (needleW / 2 - w / 2 + oX, needleH / 2 - h / 2 + oY), pygame.Rect(x, y, w, h))
00134 return imageOut
00135
00136 def overlay(self, image, x, y, r=0):
00137 """
00138 Overlays one image on top of another using 0xFFFF00 (Yellow) as the overlay colour.
00139 """
00140 x -= (image.get_rect()[2] - self.dial.get_rect()[2]) / 2
00141 y -= (image.get_rect()[3] - self.dial.get_rect()[3]) / 2
00142 image.set_colorkey(0xFFFF00)
00143 self.dial.blit(image, (x, y))
00144
00145
00146 class Horizon(Dial):
00147 """
00148 Artificial horizon dial.
00149 """
00150
00151 def __init__(self, x=0, y=0, w=0, h=0):
00152 """
00153 Initialise dial at x,y.
00154 Default size of 300px can be overidden using w,h.
00155 """
00156 self.image = pygame.image.load('%s/scripts/cockpit/resources/Horizon_GroundSky.png' % pkg).convert()
00157 self.frameImage = pygame.image.load('%s/scripts/cockpit/resources/Horizon_Background.png' % pkg).convert()
00158 self.maquetteImage = pygame.image.load('%s/scripts/cockpit/resources/Maquette_Avion.png' % pkg).convert()
00159 Dial.__init__(self, self.image, self.frameImage, x, y, w, h)
00160
00161 def update(self, screen, angleX, angleY):
00162 """
00163 Called to update an Artificial horizon dial.
00164 "angleX" and "angleY" are the inputs.
00165 "screen" is the surface to draw the dial on.
00166 """
00167 angleX %= 360
00168 angleY %= 360
00169 if (angleX > 180):
00170 angleX -= 360
00171 if (angleY > 90) and (angleY < 270):
00172 angleY = 180 - angleY
00173 elif (angleY > 270):
00174 angleY -= 360
00175 tmpImage = self.clip(self.image, 0, (59 - angleY) * 720 / 180, 250, 250)
00176 tmpImage = self.rotate(tmpImage, angleX)
00177 self.overlay(tmpImage, 0, 0)
00178 self.overlay(self.frameImage, 0, 0)
00179 self.overlay(self.maquetteImage, 0, 0)
00180 self.dial.set_colorkey(0xFFFF00)
00181 screen.blit(pygame.transform.scale(self.dial, (self.w, self.h)), self.pos)
00182
00183
00184 class TurnCoord(Dial):
00185 """
00186 Turn Coordinator dial.
00187 """
00188
00189 def __init__(self, x=0, y=0, w=0, h=0):
00190 """
00191 Initialise dial at x,y.
00192 Default size of 300px can be overidden using w,h.
00193 """
00194 self.image = pygame.image.load('%s/scripts/cockpit/resources/TurnCoordinatorAircraft.png' % pkg).convert()
00195 self.frameImage = pygame.image.load('%s/scripts/cockpit/resources/TurnCoordinator_Background.png' % pkg).convert()
00196 self.marks = pygame.image.load('%s/scripts/cockpit/resources/TurnCoordinatorMarks.png' % pkg).convert()
00197 self.ball = pygame.image.load('%s/scripts/cockpit/resources/TurnCoordinatorBall.png' % pkg).convert()
00198 Dial.__init__(self, self.image, self.frameImage, x, y, w, h)
00199
00200 def update(self, screen, angleX, angleY):
00201 """
00202 Called to update a Turn Coordinator dial.
00203 "angleX" and "angleY" are the inputs.
00204 "screen" is the surface to draw the dial on.
00205 """
00206 angleX %= 360
00207 angleY %= 360
00208 if (angleX > 180):
00209 angleX -= 360
00210 if (angleY > 180):
00211 angleY -= 360
00212 if (angleY > 14):
00213 angleY = 14
00214 if (angleY < -14):
00215 angleY = -14
00216 tmpImage = self.clip(self.image, 0, 0, 0, 0, 0, -12)
00217 tmpImage = self.rotate(tmpImage, angleX)
00218 self.overlay(self.frameImage, 0, 0)
00219 self.overlay(tmpImage, 0, 0)
00220 tmpImage = self.clip(self.marks, 0, 0, 0, 0, 0, 0)
00221 self.overlay(tmpImage, 0, 80)
00222 tmpImage = self.clip(self.ball, 0, 0, 0, 0, 0, 300)
00223 tmpImage = self.rotate(tmpImage, angleY)
00224 self.overlay(tmpImage, 0, -220)
00225 self.dial.set_colorkey(0xFFFF00)
00226 screen.blit(pygame.transform.scale(self.dial, (self.w, self.h)), self.pos)
00227
00228
00229 class Generic(Dial):
00230 """
00231 Generic Dial. This is built on by other dials.
00232 """
00233
00234 def __init__(self, x=0, y=0, w=0, h=0):
00235 """
00236 Initialise dial at x,y.
00237 Default size of 300px can be overidden using w,h.
00238 """
00239 self.image = pygame.image.load('%s/scripts/cockpit/resources/AirSpeedNeedle.png' % pkg).convert()
00240 self.frameImage = pygame.image.load('%s/scripts/cockpit/resources/Indicator_Background.png' % pkg).convert()
00241 Dial.__init__(self, self.image, self.frameImage, x, y, w, h)
00242
00243 def update(self, screen, angleX, iconLayer=0):
00244 """
00245 Called to update a Generic dial.
00246 "angleX" and "angleY" are the inputs.
00247 "screen" is the surface to draw the dial on.
00248 """
00249 angleX %= 360
00250 angleX = 360 - angleX
00251 tmpImage = self.clip(self.image, 0, 0, 0, 0, 0, -35)
00252 tmpImage = self.rotate(tmpImage, angleX)
00253 self.overlay(self.frameImage, 0, 0)
00254 if iconLayer:
00255 self.overlay(iconLayer[0], iconLayer[1], iconLayer[2])
00256 self.overlay(tmpImage, 0, 0)
00257 self.dial.set_colorkey(0xFFFF00)
00258 screen.blit(pygame.transform.scale(self.dial, (self.w, self.h)), self.pos)
00259
00260
00261 class Battery(Generic):
00262 """
00263 Battery dial.
00264 """
00265
00266 def __init__(self, x=0, y=0, w=0, h=0):
00267 """
00268 Initialise dial at x,y.
00269 Default size of 300px can be overidden using w,h.
00270 """
00271 self.icon = pygame.image.load('%s/scripts/cockpit/resources/battery2.png' % pkg).convert()
00272 Generic.__init__(self, x, y, w, h)
00273 self.frameImage = pygame.image.load('%s/scripts/cockpit/resources/ledgend.png' % pkg).convert()
00274
00275 def update(self, screen, angleX):
00276 """
00277 Called to update a Battery dial.
00278 "angleX" is the input.
00279 "screen" is the surface to draw the dial on.
00280 """
00281 if angleX > 100:
00282 angleX = 100
00283 elif angleX < 0:
00284 angleX = 0
00285 angleX *= 2.7
00286 angleX -= 135
00287 Generic.update(self, screen, angleX, (self.icon, 0, 100))
00288
00289
00290 class RfSignal(Generic):
00291 """
00292 RF Signal dial.
00293 """
00294
00295 def __init__(self, x=0, y=0, w=0, h=0):
00296 """
00297 Initialise dial at x,y.
00298 Default size of 300px can be overidden using w,h.
00299 """
00300 self.image = pygame.Surface((0, 0))
00301 self.frameImage = pygame.image.load('%s/scripts/cockpit/resources/RF_Dial_Background.png' % pkg).convert()
00302 Dial.__init__(self, self.image, self.frameImage, x, y, w, h)
00303
00304 def update(self, screen, inputA, inputB, scanPos):
00305 """
00306 "screen" is the surface to draw the dial on.
00307 """
00308
00309 top = self.dial.get_rect()[0] + 60
00310 left = self.dial.get_rect()[1] + 30
00311 bottom = self.dial.get_rect()[0] + self.dial.get_rect()[2] - 60
00312 right = self.dial.get_rect()[1] + self.dial.get_rect()[3] - 30
00313 height = bottom - top
00314 middle = height / 2 + top
00315
00316 scanPos %= right - 30
00317 scanPos += 30
00318 inputA %= 100
00319 inputB %= 100
00320 inputA = height * inputA / 200
00321 inputB = height * inputB / 200
00322
00323 pygame.draw.line(self.dial, 0xFFFFFF, (scanPos, top), (scanPos, bottom), 1)
00324 pygame.draw.line(self.dial, 0x222222, (scanPos - 1, top), (scanPos - 1, bottom), 1)
00325
00326 pygame.draw.line(self.dial, 0x00FFFF, (scanPos - 1, middle - inputA), (scanPos - 1, middle), 4)
00327 pygame.draw.line(self.dial, 0xFF00FF, (scanPos - 1, bottom - inputB), (scanPos - 1, bottom), 4)
00328 pygame.draw.line(self.dial, 0xFFFF00, (scanPos - 1, middle), (scanPos - 1, middle))
00329
00330 self.overlay(self.frameImage, 0, 0)
00331
00332 self.dial.set_colorkey(0xFFFF00)
00333 screen.blit(pygame.transform.scale(self.dial, (self.w, self.h)), self.pos)
00334
00335
00336 class TxSerial:
00337 """
00338 Wrapper round pyserial.
00339 """
00340
00341 def __init__(self, port, baud, testing=0):
00342 """
00343 Open serial port if possible.
00344 Exit program if not.
00345 "testing" = 1 prevents the program teminating if valid serial port is not found.
00346 """
00347 self.testing = testing
00348 try:
00349 self.serial = serial.Serial(port, baud, timeout=1)
00350 except serial.SerialException:
00351 print
00352 print "Usage: " + sys.argv[0] + " [SERIAL_DEVICE]"
00353 print " Opens SERIAL_DEVICE and lisens for telemitery data."
00354 print " If SERIAL_DEVICE not specified, /dev/ttyUSB0 will be tried."
00355 print "Usage: " + sys.argv[0] + " test"
00356 print " Uses dummy data for testing purposes."
00357 print
00358 if (not testing):
00359 sys.exit()
00360
00361 def readline(self):
00362 """
00363 Returns data from serial port
00364 or dummy data if "testing"=1.
00365 """
00366 if (not self.testing):
00367 line = self.serial.readline()
00368 else:
00369 line = ''
00370 rf_data = 0
00371 if (len(line) == 35):
00372 rf_data = {'RX_RSSI': int(line[1:3], 16), 'RX_fr_sucsess': int(line[4:6], 16), \
00373 'RX_fr_con_err': int(line[7:9], 16), 'RX_batt_volt': int(line[10:12], 16), \
00374 'RX_batt_cur': int(line[13:15], 16), 'TX_batt_volt': int(line[16:18], 16), \
00375 'TX_fr_sucsess': int(line[19:21], 16), 'RX_accel_x': int(line[22:24], 16), \
00376 'RX_accel_y': int(line[25:27], 16), 'RX_est_x': int(line[28:30], 16), \
00377 'RX_est_y': int(line[31:33], 16)}
00378 elif (self.testing):
00379
00380 rf_data = {'RX_RSSI': 0, 'RX_fr_sucsess': 0, 'RX_fr_con_err': 0, 'RX_batt_volt': 0, \
00381 'RX_batt_cur': 0, 'TX_batt_volt': 0, 'TX_fr_sucsess': 0, 'RX_accel_x': 0, \
00382 'RX_accel_y': 0, 'RX_est_x': 0, 'RX_est_y': 0}
00383 return rf_data
00384
00385 def close(self):
00386 """
00387 Close serial port.
00388 """
00389 if ((not self.testing) and self.serial.isOpen()):
00390 self.serial.close()
00391
00392
00393 class ImuRead:
00394 def __init__(self):
00395 self._roll = 0.0
00396 self._pitch = 0.0
00397
00398 self._scaleForPitch = float(sys.argv[2])
00399 self._scaleForRoll = float(sys.argv[3])
00400 self._offsetForRoll = float(sys.argv[4])
00401 self._offsetForPitch = float(sys.argv[5])
00402 self._revMode = int(sys.argv[6]) == REV_MODE
00403 Subscriber(sys.argv[1], Imu, self.imuCallback)
00404
00405 def imuCallback(self, msg):
00406 q = msg.orientation
00407
00408 roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
00409 roll = (roll*180/math.pi) + 180
00410 pitch = (pitch*180/math.pi)
00411
00412 if roll > 180.0:
00413 roll -= 360
00414 if not self._revMode:
00415 self._roll = roll
00416 self._pitch = pitch
00417 else:
00418 self._roll = pitch
00419 self._pitch = roll
00420
00421 def getRoll(self):
00422 return math.floor(self._scaleForRoll * self._roll / 90 + 127) + self._offsetForRoll
00423
00424 def getPitch(self):
00425 return math.floor(self._scaleForPitch * self._pitch / 90 + 127) + self._offsetForPitch
00426
00427 pkg = rospkg.RosPack().get_path('ric_board')
00428 rospy.init_node('ric_artificial_horizon')
00429 imu = ImuRead()
00430
00431 baud = 115200
00432
00433
00434
00435 screen = pygame.display.set_mode((640, 480))
00436 screen.fill(0x222222)
00437
00438
00439 horizon = Horizon(170, 180)
00440 turn = TurnCoord(20, 180, 150, 150)
00441 throttle = Generic(470, 255, 75, 75)
00442 RXbattery = Battery(470, 180, 75, 75)
00443 TXbattery = Battery(545, 180, 75, 75)
00444 rfSignal = RfSignal(470, 330, 150, 150)
00445
00446
00447 a = 0
00448 while 1:
00449
00450 for event in pygame.event.get():
00451 if event.type == QUIT:
00452 print "Exiting...."
00453 sys.exit()
00454
00455
00456 curPos = pygame.mouse.get_pos()
00457 rf_data = {'RX_RSSI': 0, 'RX_fr_sucsess': 0, 'RX_fr_con_err': 0, 'RX_batt_volt': 0, \
00458 'RX_batt_cur': 0, 'TX_batt_volt': 0, 'TX_fr_sucsess': 0, 'RX_accel_x': a, \
00459 'RX_accel_y': 0, 'RX_est_x': imu.getRoll(), 'RX_est_y': imu.getPitch()}
00460
00461 pygame.time.delay(100)
00462
00463 if rf_data:
00464 a += 1
00465
00466
00467 horizon.update(screen, 127 - rf_data['RX_est_x'], 127 - rf_data['RX_est_y'])
00468 turn.update(screen, (rf_data['RX_est_x'] - 127) / 2, (127 - rf_data['RX_accel_x']) / 4)
00469 throttle.update(screen, rf_data['RX_batt_cur'])
00470 RXbattery.update(screen, (rf_data['RX_batt_volt'] - 115))
00471 TXbattery.update(screen, 12.5 * (rf_data['TX_batt_volt'] - 105))
00472 rfSignal.update(screen, rf_data['RX_fr_sucsess'], rf_data['TX_fr_sucsess'], a)
00473
00474 pygame.display.update()