├── FreeSerif.ttf ├── LICENSE ├── PiDashCam ├── __init__.py ├── __init__.pyc ├── adxl345.py ├── adxl345.pyc ├── bmp085.py ├── bmp085.pyc ├── camera.py ├── camera.pyc ├── config.py ├── config.pyc ├── gpspoller.py ├── gpspoller.pyc ├── gy80.py ├── gy80.pyc ├── hmc5883l.py ├── hmc5883l.pyc ├── i2cutils.py ├── i2cutils.pyc ├── l3g4200d.py ├── l3g4200d.pyc ├── quaternions.py └── quaternions.pyc ├── README.md ├── assets ├── asset_1.jpg ├── asset_2.jpg ├── asset_3.jpg └── asset_4.jpg └── pidashcam.py /FreeSerif.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/michelep/PiDashCam/86c128bd0f78899ae418401955de956ad5407bdc/FreeSerif.ttf -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 Michele "O-Zone" 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /PiDashCam/__init__.py: -------------------------------------------------------------------------------- 1 | # Is a module 2 | -------------------------------------------------------------------------------- /PiDashCam/__init__.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/michelep/PiDashCam/86c128bd0f78899ae418401955de956ad5407bdc/PiDashCam/__init__.pyc -------------------------------------------------------------------------------- /PiDashCam/adxl345.py: -------------------------------------------------------------------------------- 1 | import math 2 | import time 3 | import smbus 4 | 5 | import i2cutils as I2CUtils 6 | 7 | class ADXL345(object): 8 | ''' 9 | Simple ADXL345 implementation 10 | Datasheet: http://www.analog.com/en/mems-sensors/mems-inertial-sensors/adxl345/products/product.html 11 | ''' 12 | 13 | POWER_CTL = 0x2d 14 | DATA_FORMAT = 0x31 15 | FIFO_CTL = 0x38 16 | 17 | AFS_2g = 0 18 | AFS_4g = 1 19 | AFS_8g = 2 20 | AFS_16g = 3 21 | 22 | ACCEL_START_BLOCK = 0x32 23 | ACCEL_XOUT_H = 1 24 | ACCEL_XOUT_L = 0 25 | ACCEL_YOUT_H = 3 26 | ACCEL_YOUT_L = 2 27 | ACCEL_ZOUT_H = 5 28 | ACCEL_ZOUT_L = 4 29 | 30 | ACCEL_SCALE = 0.004 # Always set to this as we are using FULL_RES 31 | 32 | def __init__(self, bus, address, name, afs_scale=AFS_2g): 33 | ''' 34 | Constructor 35 | ''' 36 | 37 | self.bus = bus 38 | self.address = address 39 | self.name = name 40 | 41 | self.afs_scale = afs_scale 42 | 43 | self.raw_accel_data = [0, 0, 0, 0, 0, 0] 44 | 45 | self.accel_raw_x = 0 46 | self.accel_raw_y = 0 47 | self.accel_raw_z = 0 48 | 49 | self.accel_scaled_x = 0 50 | self.accel_scaled_y = 0 51 | self.accel_scaled_z = 0 52 | 53 | self.pitch = 0.0 54 | self.roll = 0.0 55 | self.last_time = time.time() 56 | self.time_diff = 0 57 | 58 | 59 | # Wake up the device 60 | I2CUtils.i2c_write_byte(self.bus, self.address,ADXL345.POWER_CTL, 0b00001000) 61 | 62 | # Set data to FULL_RES and user defined scale 63 | data_format = 1 << 3 | afs_scale 64 | I2CUtils.i2c_write_byte(self.bus, self.address,ADXL345.DATA_FORMAT, data_format) 65 | 66 | # Disable FIFO mode 67 | I2CUtils.i2c_write_byte(self.bus, self.address,ADXL345.FIFO_CTL, 0b00000000) 68 | 69 | def read_raw_data(self): 70 | self.raw_accel_data = I2CUtils.i2c_read_block(self.bus, self.address, ADXL345.ACCEL_START_BLOCK, 6) 71 | 72 | self.accel_raw_x = I2CUtils.twos_compliment(self.raw_accel_data[ADXL345.ACCEL_XOUT_H], self.raw_accel_data[ADXL345.ACCEL_XOUT_L]) 73 | self.accel_raw_y = I2CUtils.twos_compliment(self.raw_accel_data[ADXL345.ACCEL_YOUT_H], self.raw_accel_data[ADXL345.ACCEL_YOUT_L]) 74 | self.accel_raw_z = I2CUtils.twos_compliment(self.raw_accel_data[ADXL345.ACCEL_ZOUT_H], self.raw_accel_data[ADXL345.ACCEL_ZOUT_L]) 75 | 76 | self.accel_scaled_x = self.accel_raw_x * ADXL345.ACCEL_SCALE 77 | self.accel_scaled_y = self.accel_raw_y * ADXL345.ACCEL_SCALE 78 | self.accel_scaled_z = self.accel_raw_z * ADXL345.ACCEL_SCALE 79 | 80 | def distance(self, x, y): 81 | '''Returns the distance between two point in 2d space''' 82 | return math.sqrt((x * x) + (y * y)) 83 | 84 | def read_x_rotation(self, x, y, z): 85 | '''Returns the rotation around the X axis in radians''' 86 | return (math.atan2(y, self.distance(x, z))) 87 | 88 | def read_y_rotation(self, x, y, z): 89 | '''Returns the rotation around the Y axis in radians''' 90 | return (-math.atan2(x, self.distance(y, z))) 91 | 92 | def read_raw_accel_x(self): 93 | '''Return the RAW X accelerometer value''' 94 | return self.accel_raw_x 95 | 96 | def read_raw_accel_y(self): 97 | '''Return the RAW Y accelerometer value''' 98 | return self.accel_raw_y 99 | 100 | def read_raw_accel_z(self): 101 | '''Return the RAW Z accelerometer value''' 102 | return self.accel_raw_z 103 | 104 | def read_scaled_accel_x(self): 105 | '''Return the SCALED X accelerometer value''' 106 | return self.accel_scaled_x 107 | 108 | def read_scaled_accel_y(self): 109 | '''Return the SCALED Y accelerometer value''' 110 | return self.accel_scaled_y 111 | 112 | def read_scaled_accel_z(self): 113 | '''Return the SCALED Z accelerometer value''' 114 | return self.accel_scaled_z 115 | 116 | 117 | def read_pitch(self): 118 | '''Calculate the current pitch value in radians''' 119 | x = self.read_scaled_accel_x() 120 | y = self.read_scaled_accel_y() 121 | z = self.read_scaled_accel_z() 122 | return self.read_x_rotation(x, y, z) 123 | 124 | def read_roll(self): 125 | '''Calculate the current roll value in radians''' 126 | x = self.read_scaled_accel_x() 127 | y = self.read_scaled_accel_y() 128 | z = self.read_scaled_accel_z() 129 | return self.read_y_rotation(x, y, z) 130 | 131 | if __name__ == "__main__": 132 | bus = smbus.SMBus(I2CUtils.i2c_raspberry_pi_bus_number()) 133 | adxl345=ADXL345(bus, 0x53, "accel") 134 | adxl345.read_raw_data() 135 | print adxl345.read_scaled_accel_x() 136 | print adxl345.read_scaled_accel_y() 137 | print adxl345.read_scaled_accel_z() 138 | -------------------------------------------------------------------------------- /PiDashCam/adxl345.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/michelep/PiDashCam/86c128bd0f78899ae418401955de956ad5407bdc/PiDashCam/adxl345.pyc -------------------------------------------------------------------------------- /PiDashCam/bmp085.py: -------------------------------------------------------------------------------- 1 | import i2cutils as I2CUtils 2 | 3 | import smbus 4 | import time 5 | 6 | import threading 7 | from config import Config 8 | 9 | 10 | class BMP085(object): 11 | ''' 12 | Simple BMP085 implementation 13 | Datasheet: http://www.adafruit.com/datasheets/BMP085_DataSheet_Rev.1.0_01July2008.pdf 14 | ''' 15 | 16 | CALIB_BLOCK_ADDRESS = 0xAA 17 | CALIB_BLOCK_SIZE = 22 18 | 19 | def __init__(self, bus, address, name, oss=3): 20 | ''' 21 | Constructor 22 | ''' 23 | self.bus = bus 24 | self.address = address 25 | self.name = name 26 | 27 | self.calibration = I2CUtils.i2c_read_block(bus, address, BMP085.CALIB_BLOCK_ADDRESS, BMP085.CALIB_BLOCK_SIZE) 28 | self.oss = oss 29 | self.temp_wait_period = 0.004 30 | self.pressure_wait_period = 0.0255 # Conversion time 31 | 32 | def twos_compliment(self, val): 33 | if (val >= 0x8000): 34 | return -((0xffff - val) + 1) 35 | else: 36 | return val 37 | 38 | def get_word(self, array, index, twos): 39 | val = (array[index] << 8) + array[index + 1] 40 | if twos: 41 | return self.twos_compliment(val) 42 | else: 43 | return val 44 | 45 | def calculate(self): 46 | 47 | # The sensor has a block of factory set calibration values we need to read 48 | # these are then used in a length calculation to get the temperature and pressure 49 | # copy these into convenience variables 50 | ac1 = self.get_word(self.calibration, 0, True) 51 | ac2 = self.get_word(self.calibration, 2, True) 52 | ac3 = self.get_word(self.calibration, 4, True) 53 | ac4 = self.get_word(self.calibration, 6, False) 54 | ac5 = self.get_word(self.calibration, 8, False) 55 | ac6 = self.get_word(self.calibration, 10, False) 56 | b1 = self.get_word(self.calibration, 12, True) 57 | b2 = self.get_word(self.calibration, 14, True) 58 | mb = self.get_word(self.calibration, 16, True) 59 | mc = self.get_word(self.calibration, 18, True) 60 | md = self.get_word(self.calibration, 20, True) 61 | oss = self.oss 62 | 63 | # This code is a direct translation from the datasheet 64 | # and should be optimised for real world use 65 | 66 | # Read raw temperature 67 | I2CUtils.i2c_write_byte(self.bus, self.address, 0xF4, 0x2E) # Tell the sensor to take a temperature reading 68 | time.sleep(self.temp_wait_period) # Wait for the conversion to take place 69 | temp_raw = I2CUtils.i2c_read_word_signed(self.bus, self.address, 0xF6) 70 | 71 | I2CUtils.i2c_write_byte(self.bus, self.address, 0xF4, 0x34 + (self.oss << 6)) # Tell the sensor to take a pressure reading 72 | time.sleep(self.pressure_wait_period) # Wait for the conversion to take place 73 | pressure_raw = ((I2CUtils.i2c_read_byte(self.bus, self.address, 0xF6) << 16) \ 74 | + (I2CUtils.i2c_read_byte(self.bus, self.address, 0xF7) << 8) \ 75 | + (I2CUtils.i2c_read_byte(self.bus, self.address, 0xF8))) >> (8 - self.oss) 76 | 77 | 78 | # Calculate temperature 79 | x1 = ((temp_raw - ac6) * ac5) / 32768 80 | x2 = (mc * 2048) / (x1 + md) 81 | b5 = x1 + x2 82 | t = (b5 + 8) / 16 83 | 84 | # Now calculate the pressure 85 | b6 = b5 - 4000 86 | x1 = (b2 * (b6 * b6 >> 12)) >> 11 87 | x2 = ac2 * b6 >> 11 88 | x3 = x1 + x2 89 | b3 = (((ac1 * 4 + x3) << oss) + 2) >> 2 90 | 91 | x1 = (ac3 * b6) >> 13 92 | x2 = (b1 * (b6 * b6 >> 12)) >> 16 93 | x3 = ((x1 + x2) + 2) >> 2 94 | b4 = ac4 * (x3 + 32768) >> 15 95 | b7 = (pressure_raw - b3) * (50000 >> oss) 96 | if (b7 < 0x80000000): 97 | p = (b7 * 2) / b4 98 | else: 99 | p = (b7 / b4) * 2 100 | x1 = (p >> 8) * (p >> 8) 101 | x1 = (x1 * 3038) >> 16 102 | x2 = (-7357 * p) >> 16 103 | p = p + ((x1 + x2 + 3791) >> 4) 104 | return(t / 10., p / 100.) 105 | 106 | def read_pressure(self): 107 | (temperature, pressure) = self.calculate() 108 | return pressure 109 | 110 | def read_temperature(self): 111 | (temperature, pressure) = self.calculate() 112 | return temperature 113 | 114 | def read_temperature_and_pressure(self): 115 | return self.calculate() 116 | 117 | class BMP085Poller(threading.Thread): 118 | def __init__(self): 119 | Config.log.debug('[INIT] BMP085 thread') 120 | threading.Thread.__init__(self) 121 | bus = smbus.SMBus(1) 122 | Config.bmp085 = BMP085(bus, 0x77 , "BMP085") 123 | self.running = True #setting the thread running to true 124 | 125 | def run(self): 126 | while self.running: 127 | t,p = Config.bmp085.read_temperature_and_pressure() 128 | Config.log.debug("Temp: %s C at %s mbar"%(t,p)) 129 | time.sleep(5) 130 | 131 | if __name__ == "__main__": 132 | bus = smbus.SMBus(1) 133 | bmp085 = BMP085(bus, 0x77 , "BMP085") 134 | print bmp085.read_temperature_and_pressure() 135 | 136 | 137 | 138 | -------------------------------------------------------------------------------- /PiDashCam/bmp085.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/michelep/PiDashCam/86c128bd0f78899ae418401955de956ad5407bdc/PiDashCam/bmp085.pyc -------------------------------------------------------------------------------- /PiDashCam/camera.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # PyDashCam 4 | # 5 | # 6 | import io 7 | import random 8 | import picamera 9 | # from PIL import Image, ImageDraw, ImageFont 10 | 11 | import threading 12 | from config import Config 13 | 14 | # Video Resolution 15 | VIDEO_HEIGHT = 720 16 | VIDEO_WIDTH = 1280 17 | 18 | class Camera(threading.Thread): 19 | def __init__(self): 20 | threading.Thread.__init__(self) 21 | Config.log.debug('[INIT] Camera thread') 22 | self.camera = picamera.PiCamera() 23 | self.camera.resolution = (VIDEO_WIDTH, VIDEO_HEIGHT) 24 | self.camera.framerate = 30 25 | self.stream = picamera.PiCameraCircularIO(self.camera, seconds=20) 26 | self.camera.start_recording(self.stream, format='h264') 27 | self.running = True 28 | 29 | def write_video(self): 30 | Config.log.debug('Writing video to disk') 31 | with self.stream.lock: 32 | # Find the first header frame in the video 33 | for frame in self.stream.frames: 34 | if frame.frame_type == picamera.PiVideoFrameType.sps_header: 35 | self.stream.seek(frame.position) 36 | break 37 | # Write the rest of the stream to disk 38 | # with io.open('./cap/motion.h264', 'wb') as output: 39 | # output.write(self.stream.read()) 40 | 41 | def exit(self): 42 | self.camera.stop_recording() 43 | 44 | def run(self): 45 | while self.running: 46 | self.camera.wait_recording(1) 47 | self.camera.wait_recording(10) 48 | self.write_video() 49 | -------------------------------------------------------------------------------- /PiDashCam/camera.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/michelep/PiDashCam/86c128bd0f78899ae418401955de956ad5407bdc/PiDashCam/camera.pyc -------------------------------------------------------------------------------- /PiDashCam/config.py: -------------------------------------------------------------------------------- 1 | # 2 | # 3 | # 4 | 5 | class Config: 6 | pass -------------------------------------------------------------------------------- /PiDashCam/config.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/michelep/PiDashCam/86c128bd0f78899ae418401955de956ad5407bdc/PiDashCam/config.pyc -------------------------------------------------------------------------------- /PiDashCam/gpspoller.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python 2 | # Written by Dan Mandle http://dan.mandle.me September 2012 3 | # License: GPL 2.0 4 | 5 | import os 6 | from gps import * 7 | from time import * 8 | import time 9 | import threading 10 | from config import Config 11 | 12 | class GpsPoller(threading.Thread): 13 | def __init__(self): 14 | Config.log.debug('[INIT] GPS thread') 15 | threading.Thread.__init__(self) 16 | Config.gpsd = gps(mode=WATCH_ENABLE) #starting the stream of info 17 | self.current_value = None 18 | self.running = True #setting the thread running to true 19 | 20 | def run(self): 21 | while self.running: 22 | Config.gpsd.next() #this will continue to loop and grab EACH set of gpsd info to clear the buffer 23 | 24 | 25 | if __name__ == '__main__': 26 | gpsp = GpsPoller() # create the thread 27 | try: 28 | gpsp.start() # start it up 29 | while True: 30 | #It may take a second or two to get good data 31 | #print gpsd.fix.latitude,', ',gpsd.fix.longitude,' Time: ',gpsd.utc 32 | 33 | os.system('clear') 34 | 35 | print 36 | print ' GPS reading' 37 | print '----------------------------------------' 38 | print 'latitude ' , gpsd.fix.latitude 39 | print 'longitude ' , gpsd.fix.longitude 40 | print 'time utc ' , gpsd.utc,' + ', gpsd.fix.time 41 | print 'altitude (m)' , gpsd.fix.altitude 42 | print 'eps ' , gpsd.fix.eps 43 | print 'epx ' , gpsd.fix.epx 44 | print 'epv ' , gpsd.fix.epv 45 | print 'ept ' , gpsd.fix.ept 46 | print 'speed (m/s) ' , gpsd.fix.speed 47 | print 'climb ' , gpsd.fix.climb 48 | print 'track ' , gpsd.fix.track 49 | print 'mode ' , gpsd.fix.mode 50 | print 51 | print 'sats ' , gpsd.satellites 52 | 53 | if gpsd.utc != None and gpsd.utc != '': 54 | tzhour = int(gpsd.utc[11:13])+TIMEZ 55 | if (tzhour>23): 56 | tzhour = (int(gpsd.utc[11:13])+TIMEZ)-24 57 | print str(tzhour) 58 | gpstime = gpsd.utc[0:4] + gpsd.utc[5:7] + gpsd.utc[8:10] + ' ' + str(tzhour) + gpsd.utc[13:19] 59 | print 'Setting system time to GPS time...' 60 | os.system('sudo date --set="%s"' % gpstime) 61 | 62 | time.sleep(5) #set to whatever 63 | 64 | except (KeyboardInterrupt, SystemExit): #when you press ctrl+c 65 | print "\nKilling Thread..." 66 | gpsp.running = False 67 | gpsp.join() # wait for the thread to finish what it's doing 68 | print "Done.\nExiting." -------------------------------------------------------------------------------- /PiDashCam/gpspoller.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/michelep/PiDashCam/86c128bd0f78899ae418401955de956ad5407bdc/PiDashCam/gpspoller.pyc -------------------------------------------------------------------------------- /PiDashCam/gy80.py: -------------------------------------------------------------------------------- 1 | #!/usb/bin/env python 2 | # 3 | # - HMC5883L (3-Axis Digital Compass / vector magnetometer), I2C Address 0x1E 4 | # - ADXL345 (3-Axis Digital Accelerometer), I2C Address 0x53 5 | # - L3G4200D (3-Axis Angular Rate Sensor / Gyro), I2C Address 0x69 6 | # - BMP085 (Barometric Pressure / Temperature Sensor), I2C Address 0x77 7 | # 8 | from __future__ import print_function 9 | 10 | import sys 11 | from time import sleep, time 12 | from math import pi, sin, cos, asin, acos, atan2, sqrt 13 | import numpy as np 14 | import smbus 15 | import threading 16 | from config import Config 17 | 18 | try: 19 | from adxl345 import ADXL345 20 | from hmc5883l import HMC5883L 21 | from bmp085 import BMP085 22 | from l3g4200d import L3G4200D 23 | except ImportError: 24 | sys.stderr.write("Ensure adxl345.py, hmc5883l.py bmp085.py, l3g4200d.py and i2cutils.py are present and importable\n") 25 | sys.exit(1) 26 | 27 | #Local imports 28 | from quaternions import _check_close 29 | from quaternions import quaternion_to_rotation_matrix_rows, quaternion_from_rotation_matrix_rows 30 | from quaternions import quaternion_from_axis_angle 31 | from quaternions import quaternion_from_euler_angles, quaternion_to_euler_angles 32 | from quaternions import quaternion_multiply, quaternion_normalise 33 | 34 | 35 | class GY80(object): 36 | def __init__(self, bus=None): 37 | if bus is None: 38 | bus = smbus.SMBus(1) 39 | 40 | #Default ADXL345 range +/- 2g is ideal for telescope use 41 | self.accel = ADXL345(bus, 0x53, name="accel") 42 | self.gyro = L3G4200D(bus, 0x69, name="gyro") 43 | self.compass = HMC5883L(bus, 0x1e, name="compass") 44 | self.barometer = BMP085(bus, 0x77, name="barometer") 45 | 46 | self._last_gyro_time = 0 #needed for interpreting gyro 47 | self.read_gyro_delta() #Discard first reading 48 | q_start = self.current_orientation_quaternion_mag_acc_only() 49 | self._q_start = q_start 50 | self._current_hybrid_orientation_q = q_start 51 | self._current_gyro_only_q = q_start 52 | 53 | def update(self): 54 | """Read the current sensor values & store them for smoothing. No return value.""" 55 | t = time() 56 | delta_t = t - self._last_gyro_time 57 | if delta_t < 0.020: 58 | #Want at least 20ms of data 59 | return 60 | v_gyro = np.array(self.read_gyro(), np.float) 61 | v_acc = np.array(self.read_accel(), np.float) 62 | v_mag = np.array(self.read_compass(), np.float) 63 | self._last_gyro_time = t 64 | 65 | #Gyro only quaternion calculation (expected to drift) 66 | rot_mag = sqrt(sum(v_gyro**2)) 67 | v_rotation = v_gyro / rot_mag 68 | q_rotation = quaternion_from_axis_angle(v_rotation, rot_mag * delta_t) 69 | self._current_gyro_only_q = quaternion_multiply(self._current_gyro_only_q, q_rotation) 70 | self._current_hybrid_orientation_q = quaternion_multiply(self._current_hybrid_orientation_q, q_rotation) 71 | 72 | if abs(sqrt(sum(v_acc**2)) - 1) < 0.3: 73 | #Approx 1g, should be stationary, and can use this for down axis... 74 | v_down = v_acc * -1.0 75 | v_east = np.cross(v_down, v_mag) 76 | v_north = np.cross(v_east, v_down) 77 | v_down /= sqrt((v_down**2).sum()) 78 | v_east /= sqrt((v_east**2).sum()) 79 | v_north /= sqrt((v_north**2).sum()) 80 | #Complementary Filter 81 | #Combine (noisy) orientation from acc/mag, 2% 82 | #with (drifting) orientation from gyro, 98% 83 | q_mag_acc = quaternion_from_rotation_matrix_rows(v_north, v_east, v_down) 84 | self._current_hybrid_orientation_q = tuple(0.02*a + 0.98*b for a, b in 85 | zip(q_mag_acc, self._current_hybrid_orientation_q)) 86 | 87 | 88 | #1st order approximation of quaternion for this rotation (v_rotation, delta_t) 89 | #using small angle approximation, cos(theta) = 1, sin(theta) = theta 90 | #w, x, y, z = (1, v_rotation[0] * delta_t/2, v_rotation[1] *delta_t/2, v_rotation[2] * delta_t/2) 91 | #q_rotation = (1, v_rotation[0] * delta_t/2, v_rotation[1] *delta_t/2, v_rotation[2] * delta_t/2) 92 | return 93 | 94 | def current_orientation_quaternion_hybrid(self): 95 | """Current orientation using North, East, Down (NED) frame of reference.""" 96 | self.update() 97 | return self._current_hybrid_orientation_q 98 | 99 | def current_orientation_quaternion_mag_acc_only(self): 100 | """Current orientation using North, East, Down (NED) frame of reference.""" 101 | #Can't use v_mag directly as North since it will usually not be 102 | #quite horizontal (requiring tilt compensation), establish this 103 | #using the up/down axis from the accelerometer. 104 | #Note assumes starting at rest so only acceleration is gravity. 105 | v_acc = np.array(self.read_accel(), np.float) 106 | v_mag = np.array(self.read_compass(), np.float) 107 | return self._quaternion_from_acc_mag(v_acc, v_mag) 108 | 109 | def _quaternion_from_acc_mag(self, v_acc, v_mag): 110 | v_down = v_acc * -1.0 #(sign change depends on sensor design?) 111 | v_east = np.cross(v_down, v_mag) 112 | v_north = np.cross(v_east, v_down) 113 | #Normalise the vectors... 114 | v_down /= sqrt((v_down ** 2).sum()) 115 | v_east /= sqrt((v_east ** 2).sum()) 116 | v_north /= sqrt((v_north ** 2).sum()) 117 | return quaternion_from_rotation_matrix_rows(v_north, v_east, v_down) 118 | 119 | def current_orientation_euler_angles_hybrid(self): 120 | """Current orientation using yaw, pitch, roll (radians) using sensor's frame.""" 121 | return quaternion_to_euler_angles(*self.current_orientation_quaternion_hybrid()) 122 | 123 | def current_orientation_euler_angles_mag_acc_only(self): 124 | """Current orientation using yaw, pitch, roll (radians) using sensor's frame.""" 125 | return quaternion_to_euler_angles(*self.current_orientation_quaternion_mag_acc_only()) 126 | 127 | def read_accel(self, scaled=True): 128 | """Returns an X, Y, Z tuple; if scaled in units of gravity.""" 129 | accel = self.accel 130 | accel.read_raw_data() 131 | if scaled: 132 | return accel.accel_scaled_x, accel.accel_scaled_y, accel.accel_scaled_z 133 | else: 134 | return accel.accel_raw_x, accel.accel_raw_y, accel.accel_raw_z 135 | 136 | def read_gyro(self, scaled=True): 137 | """Returns an X, Y, Z tuple; If scaled uses radians/second. 138 | WARNING: Calling this method directly will interfere with the higher-level 139 | methods like ``read_gyro_delta`` which integrate the gyroscope readings to 140 | track orientation (it will miss out on the rotation reported in this call). 141 | """ 142 | gyro = self.gyro 143 | gyro.read_raw_data() 144 | if scaled: 145 | return gyro.gyro_scaled_x, gyro.gyro_scaled_y, gyro.gyro_scaled_z 146 | else: 147 | return gyro.gyro_raw_x, gyro.gyro_raw_y, gyro.gyro_raw_z 148 | 149 | def read_gyro_delta(self): 150 | """Returns an X, Y, Z tuple - radians since last call.""" 151 | g = self.gyro 152 | t = time() 153 | g.read_raw_data() 154 | d = np.array([g.gyro_scaled_x, g.gyro_scaled_y, g.gyro_scaled_z], np.float) / (t - self._last_gyro_time) 155 | self._last_gyro_time = t 156 | return d 157 | 158 | def read_compass(self, scaled=True): 159 | """Returns an X, Y, Z tuple.""" 160 | compass = self.compass 161 | compass.read_raw_data() 162 | if scaled: 163 | return compass.scaled_x, compass.scaled_y, compass.scaled_z 164 | else: 165 | return compass.raw_x, compass.raw_y, compass.raw_z 166 | 167 | 168 | class GY80Poller(threading.Thread): 169 | def __init__(self): 170 | Config.log.debug('[INIT] GY80 thread') 171 | threading.Thread.__init__(self) 172 | Config.gy80 = GY80() 173 | x, y, z = Config.gy80.read_accel() 174 | self.running = True #setting the thread running to true 175 | 176 | def run(self): 177 | while self.running: 178 | Config.gy80.update() 179 | w, x, y, z = Config.gy80._current_hybrid_orientation_q 180 | 181 | yaw, pitch, roll = quaternion_to_euler_angles(w, x, y, z) 182 | Config.log.debug("Gyroscope/Accl/Comp q (%0.2f, %0.2f, %0.2f, %0.2f) yaw %0.1f, pitch %0.2f, roll %0.1f (degrees)" % (w, x, y, z, 183 | yaw * 180.0 / pi, 184 | pitch * 180.0 / pi, 185 | roll * 180.0 / pi)) 186 | sleep(5) 187 | 188 | if __name__ == "__main__": 189 | print("Starting...") 190 | imu = GY80() 191 | 192 | #Sanity test: 193 | x, y, z = imu.read_accel() 194 | g = sqrt(x*x + y*y + z*z) 195 | print("Magnitude of acceleration %0.2fg (%0.2f %0.2f %0.2f)" % (g, x, y, z)) 196 | if abs(g - 1) > 0.3: 197 | sys.stderr.write("Not starting from rest, acceleration %0.2f\n" % g) 198 | sys.exit(1) 199 | print("Starting q by acc/mag (%0.2f, %0.2f, %0.2f, %0.2f)" % imu._q_start) 200 | 201 | try: 202 | while True: 203 | print() 204 | imu.update() 205 | #w, x, y, z = imu.current_orientation_quaternion_hybrid() 206 | w, x, y, z = imu._current_hybrid_orientation_q 207 | #print("Gyroscope/Accl/Comp q (%0.2f, %0.2f, %0.2f, %0.2f)" % (w, x, y, z)) 208 | yaw, pitch, roll = quaternion_to_euler_angles(w, x, y, z) 209 | print("Gyroscope/Accl/Comp q (%0.2f, %0.2f, %0.2f, %0.2f), " 210 | "yaw %0.1f, pitch %0.2f, roll %0.1f (degrees)" % (w, x, y, z, 211 | yaw * 180.0 / pi, 212 | pitch * 180.0 / pi, 213 | roll * 180.0 / pi)) 214 | 215 | w, x, y, z = imu._current_gyro_only_q 216 | #print("Gyro-only quaternion (%0.2f, %0.2f, %0.2f, %0.2f)" % (w, x, y, z)) 217 | yaw, pitch, roll = quaternion_to_euler_angles(w, x, y, z) 218 | print("Gyro-only quaternion (%0.2f, %0.2f, %0.2f, %0.2f), " 219 | "yaw %0.1f, pitch %0.2f, roll %0.1f (degrees)" % (w, x, y, z, 220 | yaw * 180.0 / pi, 221 | pitch * 180.0 / pi, 222 | roll * 180.0 / pi)) 223 | 224 | w, x, y, z = imu.current_orientation_quaternion_mag_acc_only() 225 | #print("Accel/Comp quaternion (%0.2f, %0.2f, %0.2f, %0.2f)" % (w, x, y, z)) 226 | yaw, pitch, roll = quaternion_to_euler_angles(w, x, y, z) 227 | print("Accel/Comp quaternion (%0.2f, %0.2f, %0.2f, %0.2f), " 228 | "yaw %0.1f, pitch %0.2f, roll %0.1f (degrees)" % (w, x, y, z, 229 | yaw * 180.0 / pi, 230 | pitch * 180.0 / pi, 231 | roll * 180.0 / pi)) 232 | sleep(0.25) 233 | except KeyboardInterrupt: 234 | print() 235 | pass 236 | print("Done") 237 | 238 | -------------------------------------------------------------------------------- /PiDashCam/gy80.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/michelep/PiDashCam/86c128bd0f78899ae418401955de956ad5407bdc/PiDashCam/gy80.pyc -------------------------------------------------------------------------------- /PiDashCam/hmc5883l.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | import i2cutils as I2CUtils 4 | 5 | class HMC5883L(object): 6 | ''' 7 | Simple HMC5883L implementation 8 | ''' 9 | TWO_PI = 2 * math.pi 10 | 11 | CONF_REG_A = 0 12 | CONF_REG_B = 1 13 | MODE_REG = 2 14 | DATA_START_BLOCK = 3 15 | DATA_XOUT_H = 0 16 | DATA_XOUT_L = 1 17 | DATA_ZOUT_H = 2 18 | DATA_ZOUT_L = 3 19 | DATA_YOUT_H = 4 20 | DATA_YOUT_L = 5 21 | 22 | SAMPLE_RATE = { 0 : 0.75, 1 : 1.5, 2 : 3, 3 : 7.5, 4 : 15, 5 : 30, 6 : 75, 7 :-1 } 23 | 24 | SAMPLE_MODE = { 0 : "CONTINUOUS", 1 : "SINGLE", 2 : "IDLE", 3 : "IDLE" } 25 | 26 | GAIN_SCALE = { 27 | 0 : [ 0.88, 1370, 0.73 ], 28 | 1 : [ 1.30, 1090, 0.92 ], 29 | 2 : [ 1.90, 820, 1.22 ], 30 | 3 : [ 2.50, 660, 1.52 ], 31 | 4 : [ 4.00, 440, 2.27 ], 32 | 5 : [ 4.70, 390, 2.56 ], 33 | 6 : [ 5.60, 330, 3.03 ], 34 | 7 : [ 8.10, 230, 4.35 ] 35 | } 36 | 37 | 38 | def __init__(self, bus, address, name, samples=3, rate=4, gain=1, sampling_mode=0, x_offset=0, y_offset=0, z_offset=0): 39 | self.bus = bus 40 | self.address = address 41 | self.name = name 42 | self.samples = samples 43 | self.gain = gain 44 | self.sampling_mode = sampling_mode 45 | 46 | self.x_offset = x_offset 47 | self.y_offset = y_offset 48 | self.z_offset = z_offset 49 | 50 | # Set the number of samples 51 | conf_a = (samples << 5) + (rate << 2) 52 | I2CUtils.i2c_write_byte(self.bus, self.address, HMC5883L.CONF_REG_A, conf_a) 53 | 54 | # Set the gain 55 | conf_b = gain << 5 56 | I2CUtils.i2c_write_byte(self.bus, self.address, HMC5883L.CONF_REG_B, conf_b) 57 | 58 | # Set the operation mode 59 | I2CUtils.i2c_write_byte(self.bus, self.address, HMC5883L.MODE_REG, self.sampling_mode) 60 | 61 | self.raw_data = [0, 0, 0, 0, 0, 0] 62 | 63 | # Now read all the values as the first read after a gain change returns the old value 64 | self.read_raw_data() 65 | 66 | def read_raw_data(self): 67 | ''' 68 | Read the raw data from the sensor, scale it appropriately and store for later use 69 | ''' 70 | self.raw_data = I2CUtils.i2c_read_block(self.bus, self.address, HMC5883L.DATA_START_BLOCK, 6) 71 | self.raw_x = I2CUtils.twos_compliment(self.raw_data[HMC5883L.DATA_XOUT_H], self.raw_data[HMC5883L.DATA_XOUT_L]) - self.x_offset 72 | self.raw_y = I2CUtils.twos_compliment(self.raw_data[HMC5883L.DATA_YOUT_H], self.raw_data[HMC5883L.DATA_YOUT_L]) - self.y_offset 73 | self.raw_z = I2CUtils.twos_compliment(self.raw_data[HMC5883L.DATA_ZOUT_H], self.raw_data[HMC5883L.DATA_ZOUT_L]) - self.z_offset 74 | 75 | self.scaled_x = self.raw_x * HMC5883L.GAIN_SCALE[self.gain][2] 76 | self.scaled_y = self.raw_y * HMC5883L.GAIN_SCALE[self.gain][2] 77 | self.scaled_z = self.raw_z * HMC5883L.GAIN_SCALE[self.gain][2] 78 | 79 | def read_bearing(self): 80 | ''' 81 | Read a bearing from the sensor assuming the sensor is level 82 | ''' 83 | self.read_raw_data() 84 | 85 | bearing = math.atan2(self.read_scaled_y(), self.read_scaled_x()) 86 | if bearing < 0: 87 | return bearing + (HMC5883L.TWO_PI) 88 | else: 89 | return bearing 90 | 91 | def read_compensated_bearing(self, pitch, roll): 92 | ''' 93 | Calculate a bearing taking in to account the current pitch and roll of the device as supplied as parameters 94 | ''' 95 | self.read_raw_data() 96 | cos_pitch = (math.cos(pitch)) 97 | sin_pitch = (math.sin(pitch)) 98 | 99 | cos_roll = (math.cos(roll)) 100 | sin_roll = (math.sin(roll)) 101 | 102 | Xh = (self.scaled_x * cos_roll) + (self.scaled_z * sin_roll) 103 | Yh = (self.scaled_x * sin_pitch * sin_roll) + (self.scaled_y * cos_pitch) - (self.scaled_z * sin_pitch * cos_roll) 104 | 105 | bearing = math.atan2(Yh, Xh) 106 | if bearing < 0: 107 | return bearing + (HMC5883L.TWO_PI) 108 | else: 109 | return bearing 110 | 111 | def set_offsets(self, x_offset, y_offset, z_offset): 112 | self.x_offset = x_offset 113 | self.y_offset = y_offset 114 | self.z_offset = z_offset 115 | 116 | def read_raw_x(self): 117 | return self.raw_x 118 | 119 | def read_raw_y(self): 120 | return self.raw_y 121 | 122 | def read_raw_z(self): 123 | return self.raw_z 124 | 125 | def read_scaled_x(self): 126 | return self.scaled_x 127 | 128 | def read_scaled_y(self): 129 | return self.scaled_y 130 | 131 | def read_scaled_z(self): 132 | return self.scaled_z 133 | -------------------------------------------------------------------------------- /PiDashCam/hmc5883l.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/michelep/PiDashCam/86c128bd0f78899ae418401955de956ad5407bdc/PiDashCam/hmc5883l.pyc -------------------------------------------------------------------------------- /PiDashCam/i2cutils.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | def i2c_raspberry_pi_bus_number(): 4 | """Returns Raspberry Pi I2C bus number (integer, 0 or 1). 5 | 6 | Looks at `/proc/cpuinfo` to identify if this is a revised model 7 | of the Raspberry Pi (with 512MB of RAM) using `/dev/i2c-1`, or 8 | the original version (with 256MB or RAM) using `/dev/i2c-0`. 9 | """ 10 | cpuinfo = 1 11 | with open('/proc/cpuinfo','r') as f: 12 | for line in f: 13 | if line.startswith('Revision'): 14 | cpuinfo = line.strip()[-1:] 15 | return (1 if (cpuinfo >'3') else 0) 16 | 17 | def i2c_read_byte(bus, address, register): 18 | return bus.read_byte_data(address, register) 19 | 20 | def i2c_read_word_unsigned(bus, address, register): 21 | high = bus.read_byte_data(address, register) 22 | low = bus.read_byte_data(address, register+1) 23 | return (high << 8) + low 24 | 25 | def i2c_read_word_signed(bus, address, register): 26 | value = i2c_read_word_unsigned(bus, address, register) 27 | if (value >= 0x8000): 28 | return -((0xffff - value) + 1) 29 | else: 30 | return value 31 | 32 | def i2c_write_byte(bus, address, register, value): 33 | bus.write_byte_data(address, register, value) 34 | 35 | def i2c_read_block(bus, address, start, length): 36 | return bus.read_i2c_block_data(address, start, length) 37 | 38 | def twos_compliment(high_byte, low_byte): 39 | value = (high_byte << 8) + low_byte 40 | if (value >= 0x8000): 41 | return -((0xffff - value) + 1) 42 | else: 43 | return value 44 | 45 | if __name__ == "__main__": 46 | print i2c_raspberry_pi_bus_number() -------------------------------------------------------------------------------- /PiDashCam/i2cutils.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/michelep/PiDashCam/86c128bd0f78899ae418401955de956ad5407bdc/PiDashCam/i2cutils.pyc -------------------------------------------------------------------------------- /PiDashCam/l3g4200d.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | import i2cutils as I2CUtils 4 | 5 | class L3G4200D(object): 6 | ''' 7 | Simple L3G4200D implementation 8 | Datasheet: http://www.st.com/web/catalog/sense_power/FM89/SC1288/PF250373 9 | ''' 10 | 11 | CTRL_REG1 = 0x20 12 | CTRL_REG2 = 0x21 13 | CTRL_REG3 = 0x22 14 | CTRL_REG4 = 0x23 15 | CTRL_REG5 = 0x24 16 | 17 | GYRO_START_BLOCK = 0x28 18 | GYRO_XOUT_H = 0x28 19 | GYRO_XOUT_L = 0x29 20 | GYRO_YOUT_H = 0x2a 21 | GYRO_YOUT_L = 0x2b 22 | GYRO_ZOUT_H = 0x2c 23 | GYRO_ZOUT_L = 0x2d 24 | 25 | FS_250 = 250 26 | FS_500 = 500 27 | FS_2000 = 2000 28 | 29 | FS_250_SCALE = 8.75 / 1000 # milli degrees per second page 10 of the datasheet 30 | FS_500_SCALE = 17.50 / 1000 # milli degrees per second page 10 of the datasheet 31 | FS_2000_SCALE = 70.00 / 1000 # milli degrees per second page 10 of the datasheet 32 | 33 | 34 | GYRO_SCALE = { FS_250 : [FS_250_SCALE, 0], FS_500 : [FS_500_SCALE, 1], FS_2000 : [FS_2000_SCALE, 10] } 35 | 36 | def __init__(self, bus, address, name, fs_scale=FS_250): 37 | ''' 38 | Constructor 39 | ''' 40 | 41 | self.bus = bus 42 | self.address = address 43 | self.name = name 44 | self.fs_scale = fs_scale 45 | 46 | self.gyro_raw_x = 0 47 | self.gyro_raw_y = 0 48 | self.gyro_raw_z = 0 49 | 50 | self.gyro_scaled_x = 0 51 | self.gyro_scaled_y = 0 52 | self.gyro_scaled_z = 0 53 | 54 | self.raw_temp = 0 55 | self.scaled_temp = 0 56 | 57 | # Wake up the deice and get output for each of the three axes,X, Y & Z 58 | I2CUtils.i2c_write_byte(self.bus, self.address, L3G4200D.CTRL_REG1, 0b00001111) 59 | 60 | # Select Big endian so we can use existing I2C library and include the scaling 61 | ctrl_reg4 = 1 << 6 | L3G4200D.GYRO_SCALE[fs_scale][1] << 4 62 | I2CUtils.i2c_write_byte(self.bus, self.address, L3G4200D.CTRL_REG4, ctrl_reg4) 63 | 64 | def read_raw_data(self): 65 | 66 | self.gyro_raw_x = I2CUtils.i2c_read_word_signed(self.bus, self.address, L3G4200D.GYRO_XOUT_H) 67 | self.gyro_raw_y = I2CUtils.i2c_read_word_signed(self.bus, self.address, L3G4200D.GYRO_YOUT_H) 68 | self.gyro_raw_z = I2CUtils.i2c_read_word_signed(self.bus, self.address, L3G4200D.GYRO_ZOUT_H) 69 | 70 | # We convert these to radians for consistency and so we can easily combine later in the filter 71 | self.gyro_scaled_x = math.radians(self.gyro_raw_x * L3G4200D.GYRO_SCALE[self.fs_scale][0]) 72 | self.gyro_scaled_y = math.radians(self.gyro_raw_y * L3G4200D.GYRO_SCALE[self.fs_scale][0]) 73 | self.gyro_scaled_z = math.radians(self.gyro_raw_z * L3G4200D.GYRO_SCALE[self.fs_scale][0]) 74 | 75 | def read_raw_gyro_x(self): 76 | '''Return the RAW X gyro value''' 77 | return self.gyro_raw_x 78 | 79 | def read_raw_gyro_y(self): 80 | '''Return the RAW Y gyro value''' 81 | return self.gyro_raw_y 82 | 83 | def read_raw_gyro_z(self): 84 | '''Return the RAW Z gyro value''' 85 | return self.gyro_raw_z 86 | 87 | def read_scaled_gyro_x(self): 88 | '''Return the SCALED X gyro value in radians/second''' 89 | return self.gyro_scaled_x 90 | 91 | def read_scaled_gyro_y(self): 92 | '''Return the SCALED Y gyro value in radians/second''' 93 | return self.gyro_scaled_y 94 | 95 | def read_scaled_gyro_z(self): 96 | '''Return the SCALED Z gyro value in radians/second''' 97 | return self.gyro_scaled_z 98 | -------------------------------------------------------------------------------- /PiDashCam/l3g4200d.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/michelep/PiDashCam/86c128bd0f78899ae418401955de956ad5407bdc/PiDashCam/l3g4200d.pyc -------------------------------------------------------------------------------- /PiDashCam/quaternions.py: -------------------------------------------------------------------------------- 1 | """Crude code for quaternions in Python. 2 | 3 | TODO - Define a quaternion class? 4 | """ 5 | 6 | from __future__ import print_function 7 | 8 | from math import pi, sin, cos, asin, acos, atan2, sqrt 9 | 10 | def _check_close(a, b, error=0.0001): 11 | if isinstance(a, (tuple, list)): 12 | assert isinstance(b, (tuple, list)) 13 | assert len(a) == len(b) 14 | for a1, b1 in zip(a, b): 15 | diff = abs(a1-b1) 16 | if diff > error: 17 | raise ValueError("%s vs %s, for %s vs %s difference %s > %s" 18 | % (a, b, a1, b1, diff, error)) 19 | return 20 | diff = abs(a-b) 21 | if diff > error: 22 | raise ValueError("%s vs %s, difference %s > %s" 23 | % (a, b, diff, error)) 24 | 25 | def quaternion_mgnitude(w, x, y, z): 26 | return sqrt(w*w + x*x + y*y + z*z) 27 | 28 | def quaternion_normalise(w, x, y, z): 29 | mag = sqrt(w*w + x*x + y*y + z*z) 30 | return w/mag, x/mag, y/mag, z/mag 31 | 32 | def quaternion_from_axis_angle(vector, theta): 33 | sin_half_theta = sin(theta/2) 34 | return cos(theta/2), vector[0]*sin_half_theta, vector[1]*sin_half_theta, vector[2]*sin_half_theta 35 | 36 | #TODO - Write quaternion_to_axis_angle and cross-validate 37 | 38 | def quaternion_to_rotation_matrix_rows(w, x, y, z): 39 | """Returns a tuple of three rows which make up a 3x3 rotatation matrix. 40 | 41 | It is trival to turn this into a NumPy array/matrix if desired.""" 42 | x2 = x*x 43 | y2 = y*2 44 | z2 = z*2 45 | row0 = (1 - 2*y2 - 2*z2, 46 | 2*x*y - 2*w*z, 47 | 2*x*z + 2*w*y) 48 | row1 = (2*x*y + 2*w*z, 49 | 1 - 2*x2 - 2*z2, 50 | 2*y*z - 2*w*x) 51 | row2 = (2*x*z - 2*w*y, 52 | 2*y*z + 2*w*x, 53 | 1 - 2*x2 - 2*y2) 54 | return row0, row1, row2 55 | 56 | def quaternion_from_rotation_matrix_rows(row0, row1, row2): 57 | #No point merging three rows into a 3x3 matrix if just want quaternion 58 | #Based on several sources including the C++ implementation here: 59 | #http://www.camelsoftware.com/firetail/blog/uncategorized/quaternion-based-ahrs-using-altimu-10-arduino/ 60 | #http://www.camelsoftware.com/firetail/blog/c/imu-maths/ 61 | trace = row0[0] + row1[1] + row2[2] 62 | if trace > row2[2]: 63 | S = sqrt(1.0 + trace) * 2 64 | w = 0.25 * S 65 | x = (row2[1] - row1[2]) / S 66 | y = (row0[2] - row2[0]) / S 67 | z = (row1[0] - row0[1]) / S 68 | elif row0[0] < row1[1] and row0[0] < row2[2]: 69 | S = sqrt(1.0 + row0[0] - row1[1] - row2[2]) * 2 70 | w = (row2[1] - row1[2]) / S 71 | x = 0.25 * S 72 | y = (row0[1] + row1[0]) / S 73 | z = (row0[2] + row2[0]) / S 74 | elif row1[1] < row2[2]: 75 | S = sqrt(1.0 + row1[1] - row0[0] - row2[2]) * 2 76 | w = (row0[2] - row2[0]) / S 77 | x = (row0[1] + row1[0]) / S 78 | y = 0.25 * S 79 | z = (row1[2] + row2[1]) / S 80 | else: 81 | S = sqrt(1.0 + row2[2] - row0[0] - row1[1]) * 2 82 | w = (row1[0] - row0[1]) / S 83 | x = (row0[2] + row2[0]) / S 84 | y = (row1[2] + row2[1]) / S 85 | z = 0.25 * S 86 | return w, x, y, z 87 | 88 | 89 | #TODO - Double check which angles exactly have I calculated (which frame etc)? 90 | def quaternion_from_euler_angles(yaw, pitch, roll): 91 | """Returns (w, x, y, z) quaternion from angles in radians. 92 | 93 | Assuming angles given in the moving frame of reference of the sensor, 94 | not a fixed Earth bound observer. 95 | """ 96 | #Roll = phi, pitch = theta, yaw = psi 97 | return (cos(roll/2)*cos(pitch/2)*cos(yaw/2) + sin(roll/2)*sin(pitch/2)*sin(yaw/2), 98 | sin(roll/2)*cos(pitch/2)*cos(yaw/2) - cos(roll/2)*sin(pitch/2)*sin(yaw/2), 99 | cos(roll/2)*sin(pitch/2)*cos(yaw/2) + sin(roll/2)*cos(pitch/2)*sin(yaw/2), 100 | cos(roll/2)*cos(pitch/2)*sin(yaw/2) - sin(roll/2)*sin(pitch/2)*cos(yaw/2)) 101 | 102 | def quaternion_to_euler_angles(w, x, y, z): 103 | """Returns angles about Z, Y, X axes in radians (yaw, pitch, roll). 104 | 105 | Using moving frame of reference of the sensor, not the fixed frame of 106 | an Earth bound observer.. 107 | """ 108 | w2 = w*w 109 | x2 = x*x 110 | y2 = y*y 111 | z2 = z*z 112 | return (atan2(2.0 * (x*y + z*w), (w2 + x2 - y2 - z2)), # -pi to pi 113 | asin(2.0 * (w*y - x*z) / (w2 + x2 + y2 + z2)), # -pi/2 to +pi/2 114 | atan2(2.0 * (y*z + x*w), (w2 - x2 - y2 + z2))) # -pi to pi 115 | 116 | _check_close(quaternion_to_euler_angles(0, 1, 0, 0), (0, 0, pi)) 117 | _check_close(quaternion_to_euler_angles(0,-1, 0, 0), (0, 0, pi)) 118 | _check_close(quaternion_from_euler_angles(0, 0, pi), (0, 1, 0, 0)) 119 | 120 | _check_close(quaternion_to_euler_angles(0, 0, 1, 0), (pi, 0, pi)) 121 | _check_close(quaternion_to_euler_angles(0, 0,-1, 0), (pi, 0, pi)) 122 | _check_close(quaternion_from_euler_angles(pi, 0, pi), (0, 0, 1, 0)) 123 | 124 | _check_close(quaternion_to_euler_angles(0, 0, 0, 1), (pi, 0, 0)) 125 | _check_close(quaternion_to_euler_angles(0, 0, 0,-1), (pi, 0, 0)) 126 | _check_close(quaternion_from_euler_angles(pi, 0, 0), (0, 0, 0, 1)) 127 | 128 | _check_close(quaternion_to_euler_angles(0, 0, 0.5*sqrt(2), 0.5*sqrt(2)), (pi, 0, pi/2)) 129 | _check_close(quaternion_from_euler_angles(pi, 0, pi/2), (0, 0, 0.5*sqrt(2), 0.5*sqrt(2))) 130 | 131 | _check_close(quaternion_to_euler_angles(0, 0.5*sqrt(2), 0, 0.5*sqrt(2)), (0, -pi/2, 0)) 132 | _check_close(quaternion_to_euler_angles(0.5*sqrt(2), 0,-0.5*sqrt(2), 0), (0, -pi/2, 0)) 133 | _check_close(quaternion_from_euler_angles(0, -pi/2, 0), (0.5*sqrt(2), 0, -0.5*sqrt(2), 0)) 134 | 135 | _check_close(quaternion_to_euler_angles(0, 1, 1, 0), (pi/2, 0, pi)) #Not normalised 136 | _check_close(quaternion_to_euler_angles(0, 0.5*sqrt(2), 0.5*sqrt(2), 0), (pi/2, 0, pi)) 137 | _check_close(quaternion_from_euler_angles(pi/2, 0, pi), (0, 0.5*sqrt(2), 0.5*sqrt(2), 0)) 138 | 139 | #w, x, y, z = quaternion_from_euler_angles(pi, 0, pi) 140 | #print("quarternion (%0.2f, %0.2f, %0.2f, %0.2f) magnitude %0.2f" % (w, x, y, z, sqrt(w*w + x*x + y*y + z*z))) 141 | 142 | def quaternion_multiply(a, b): 143 | a_w, a_x, a_y, a_z = a 144 | b_w, b_x, b_y, b_z = b 145 | return (a_w*b_w - a_x*b_x - a_y*b_y - a_z*b_z, 146 | a_w*b_x + a_x*b_w + a_y*b_z - a_z*b_y, 147 | a_w*b_y - a_x*b_z + a_y*b_w + a_z*b_x, 148 | a_w*b_z + a_x*b_y - a_y*b_x + a_z*b_w) 149 | 150 | _check_close(quaternion_multiply((0, 0, 0, 1), (0, 0, 1, 0)), (0, -1, 0, 0)) 151 | 152 | def quaternion_scalar_multiply(q, s): 153 | w, x, y, z = q 154 | return (w*s, x*s, y*s, z*q) 155 | -------------------------------------------------------------------------------- /PiDashCam/quaternions.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/michelep/PiDashCam/86c128bd0f78899ae418401955de956ad5407bdc/PiDashCam/quaternions.pyc -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PiDashCam 2 | Raspberry Pi Zero W dash camera for your auto 3 | 4 | !!! PLEASE NOTE: CODE IS NOT READY YET !!! 5 | 6 | ## BoM - Bill of Materials 7 | 8 | * [Raspberry Pi Zero W](https://www.raspberrypi.org/products/raspberry-pi-zero-w/) 9 | * [Raspberry Pi Zero \ Zero W V1.3 Mini Camera 5MP 720P / 1080P Mini Camera Module for Raspberry Pi Zero](https://www.aliexpress.com/item/Raspberry-Pi-Zero-Zero-W-V1-3-Mini-Camera-5MP-720P-1080P-Mini-Camera-Module-for/32810927440.html) 10 | * [Ublox-NEO-6M-GPS-Module](https://it.aliexpress.com/item/Free-Shipping-Ublox-NEO-6M-GPS-Module-with-EEPROM-for-MWC-AeroQuad-with-Antenna-for-Flight/32391262594.html) 11 | * [GY-80 - Multi Sensor Board - 3 Axis Gyro -3 Axis Accelerometer - 3 Axis Magnetometer - Barometer - Thermometer](http://selfbuilt.net/shop/gy-80-inertial-management-unit) 12 | 13 | Soldering iron, board, wires and all other stuff needed to build a simple circuit... 14 | 15 | ## How first prototype looks like... 16 | 17 | ![Overview](assets/asset_1.jpg "Overview") 18 | 19 | ![Overview](assets/asset_2.jpg "Overview") 20 | 21 | ![Overview](assets/asset_3.jpg "Overview") 22 | 23 | ![Overview](assets/asset_4.jpg "Prototype wiring") 24 | 25 | ## Author 26 | 27 | PiDashCam was made by Michele Pinassi 28 | 29 | ## License 30 | 31 | PiDashCam is under MIT license. No any warranty. Please use responsibly. 32 | -------------------------------------------------------------------------------- /assets/asset_1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/michelep/PiDashCam/86c128bd0f78899ae418401955de956ad5407bdc/assets/asset_1.jpg -------------------------------------------------------------------------------- /assets/asset_2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/michelep/PiDashCam/86c128bd0f78899ae418401955de956ad5407bdc/assets/asset_2.jpg -------------------------------------------------------------------------------- /assets/asset_3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/michelep/PiDashCam/86c128bd0f78899ae418401955de956ad5407bdc/assets/asset_3.jpg -------------------------------------------------------------------------------- /assets/asset_4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/michelep/PiDashCam/86c128bd0f78899ae418401955de956ad5407bdc/assets/asset_4.jpg -------------------------------------------------------------------------------- /pidashcam.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # PiDashCam 4 | # 5 | # Michele Pinassi 6 | # 7 | 8 | import os 9 | import time 10 | import logging 11 | import signal 12 | import sys 13 | 14 | import pygame 15 | 16 | from PiDashCam.config import Config 17 | from PiDashCam.gpspoller import GpsPoller 18 | from PiDashCam.camera import Camera 19 | from PiDashCam.gy80 import GY80Poller 20 | from PiDashCam.bmp085 import BMP085Poller 21 | 22 | logging.basicConfig(filename='pidashcam.log', level=logging.DEBUG, format='%(asctime)s.%(msecs)03d %(levelname)s - %(funcName)s: %(message)s', datefmt="%Y-%m-%d %H:%M:%S") 23 | Config.log = logging.getLogger(__file__) 24 | 25 | Config.gpsd = None 26 | gpsTime = False 27 | TIMEZ = 2 28 | Config.isRun = True 29 | 30 | BLACK = ( 0, 0, 0) 31 | WHITE = (255, 255, 255) 32 | BLUE = ( 0, 0, 255) 33 | GREEN = ( 0, 255, 0) 34 | RED = (255, 0, 0) 35 | 36 | def exit_handler(signal, frame): 37 | if(Config.isRun): 38 | Config.isRun=False 39 | print "[INFO] exitHandler(). Please wait..." 40 | Config.log.info('PiDashCam EXIT Handler') 41 | 42 | signal.signal(signal.SIGTERM, exit_handler) 43 | signal.signal(signal.SIGINT, exit_handler) 44 | signal.signal(signal.SIGTSTP, exit_handler) 45 | 46 | if __name__ == "__main__": 47 | Config.log.info('PiDashCam START') 48 | 49 | os.environ['SDL_VIDEODRIVER'] = 'fbcon' 50 | os.environ["SDL_FBDEV"] = "/dev/fb0" 51 | 52 | print("[INFO] Initializing display...") 53 | 54 | try: 55 | pygame.init() 56 | pygame.display.init() 57 | pygame.font.init() 58 | 59 | display_info = pygame.display.Info() 60 | 61 | print display_info 62 | 63 | screen = pygame.display.set_mode([display_info.current_w,display_info.current_h], pygame.FULLSCREEN) 64 | 65 | font = pygame.font.Font(None, 10) 66 | clock = pygame.time.Clock() 67 | except: 68 | print "[FATAL] Error initilizing display" 69 | Config.log.error("Unable to initialize display") 70 | exit() 71 | 72 | print("[INFO] Display done. Initializing modules...") 73 | 74 | screen.fill(BLACK) 75 | 76 | text = font.render("PiDashCam initializing...", True, WHITE) 77 | screen.blit(text, [10, 10]) 78 | 79 | pygame.display.flip() 80 | 81 | # Launch threads... 82 | threads = [] 83 | 84 | gps_thread = GpsPoller() 85 | threads.append(gps_thread) 86 | 87 | camera_thread = Camera() 88 | threads.append(camera_thread) 89 | 90 | gy80_thread = GY80Poller() 91 | threads.append(gy80_thread) 92 | 93 | bmp085_thread = BMP085Poller() 94 | threads.append(bmp085_thread) 95 | 96 | try: 97 | for t in threads: 98 | t.start() 99 | except: 100 | print "[FATAL] Something wrong starting threads..." 101 | exit() 102 | 103 | print "[INIT] Modules intialized. Go." 104 | 105 | screen.fill(BLACK) 106 | pygame.draw.line(screen, GREEN, [0, 50], [640,50], 3) 107 | 108 | while Config.isRun: 109 | for event in pygame.event.get(): 110 | print event 111 | if event.type == pygame.QUIT: 112 | Config.isRun = False 113 | 114 | # Update screen 115 | text = font.render("lat:%s lng:%s alt:%s"%(Config.gpsd.fix.latitude,Config.gpsd.fix.longitude,Config.gpsd.fix.altitude), True, WHITE) 116 | screen.blit(text,[10,10]) 117 | 118 | # time.sleep(1) 119 | # Config.log.debug("lat: %s lng: %s alt: %s"%(Config.gpsd.fix.latitude,Config.gpsd.fix.longitude,Config.gpsd.fix.altitude)) 120 | 121 | # Sync time if needed 122 | if Config.gpsd.utc != None and Config.gpsd.utc != '' and gpsTime != True: 123 | tzhour = int(Config.gpsd.utc[11:13])+TIMEZ 124 | if (tzhour>23): 125 | tzhour = (int(Config.gpsd.utc[11:13])+TIMEZ)-24 126 | gps_time = Config.gpsd.utc[0:4] + Config.gpsd.utc[5:7] + Config.gpsd.utc[8:10] + ' ' + str(tzhour) + Config.gpsd.utc[13:19] 127 | Config.log.info("Setting system time to GPS time (%s)..."%str(tzhour)) 128 | os.system('sudo date --set="%s"' % gps_time) 129 | gpsTime = True 130 | 131 | # Update the screen 132 | clock.tick(60) 133 | pygame.display.flip() 134 | 135 | pygame.quit() 136 | 137 | print "[INFO] Quitting. Killing Threads..." 138 | 139 | for t in threads: 140 | t.running = False 141 | t.join() 142 | 143 | --------------------------------------------------------------------------------