#!/usr/bin/env python ############################################################################################### ############################################################################################### ## ## ## Hove's Raspberry Pi Python Quadcopter Flight Controller. Open Source @ GitHub ## ## PiStuffing/Quadcopter under GPL for non-commercial application. Any code derived from ## ## this should retain this copyright comment. ## ## ## ## Copyright 2014 Andy Baker (Hove) - andy@pistuffing.co.uk ## ## ## ############################################################################################### ############################################################################################### from __future__ import division import signal import socket import time import string import sys import getopt import math import threading from array import * import smbus import select import os import struct import logging import RPi.GPIO as RPIO from RPIO import PWM import subprocess from datetime import datetime import shutil import ctypes from ctypes.util import find_library import random ############################################################################################ # # Adafruit i2c interface enhanced with performance / error handling enhancements # ############################################################################################ class I2C: def __init__(self, address, bus=smbus.SMBus(1)): self.address = address self.bus = bus self.misses = 0 def reverseByteOrder(self, data): "Reverses the byte order of an int (16-bit) or long (32-bit) value" # Courtesy Vishal Sapre dstr = hex(data)[2:].replace('L','') byteCount = len(dstr[::2]) val = 0 for i, n in enumerate(range(byteCount)): d = data & 0xFF val |= (d << (8 * (byteCount - i - 1))) data >>= 8 return val def write8(self, reg, value): "Writes an 8-bit value to the specified register/address" while True: try: self.bus.write_byte_data(self.address, reg, value) logger.debug('I2C: Wrote 0x%02X to register 0x%02X', value, reg) break except IOError, err: self.misses += 1 logger.exception('Error %d, %s accessing 0x%02X: Check your I2C address', err.errno, err.strerror, self.address) time.sleep(0.0001) def writeList(self, reg, list): "Writes an array of bytes using I2C format" while True: try: self.bus.write_i2c_block_data(self.address, reg, list) break except IOError, err: self.misses += 1 logger.exception('Error %d, %s accessing 0x%02X: Check your I2C address', err.errno, err.strerror, self.address) time.sleep(0.0001) def readU8(self, reg): "Read an unsigned byte from the I2C device" while True: try: result = self.bus.read_byte_data(self.address, reg) logger.debug('I2C: Device 0x%02X returned 0x%02X from reg 0x%02X', self.address, result & 0xFF, reg) return result except IOError, err: self.misses += 1 logger.exception('Error %d, %s accessing 0x%02X: Check your I2C address', err.errno, err.strerror, self.address) time.sleep(0.0001) def readS8(self, reg): "Reads a signed byte from the I2C device" while True: try: result = self.bus.read_byte_data(self.address, reg) logger.debug('I2C: Device 0x%02X returned 0x%02X from reg 0x%02X', self.address, result & 0xFF, reg) if (result > 127): return result - 256 else: return result except IOError, err: self.misses += 1 logger.exception('Error %d, %s accessing 0x%02X: Check your I2C address', err.errno, err.strerror, self.address) time.sleep(0.0001) def readU16(self, reg): "Reads an unsigned 16-bit value from the I2C device" while True: try: hibyte = self.bus.read_byte_data(self.address, reg) result = (hibyte << 8) + self.bus.read_byte_data(self.address, reg+1) logger.debug('I2C: Device 0x%02X returned 0x%04X from reg 0x%02X', self.address, result & 0xFFFF, reg) if result == 0x7FFF or result == 0x8000: logger.critical('I2C read max value') time.sleep(0.0005) else: return result except IOError, err: self.misses += 1 logger.exception('Error %d, %s accessing 0x%02X: Check your I2C address', err.errno, err.strerror, self.address) time.sleep(0.0001) def readS16(self, reg): "Reads a signed 16-bit value from the I2C device" while True: try: hibyte = self.bus.read_byte_data(self.address, reg) if (hibyte > 127): hibyte -= 256 result = (hibyte << 8) + self.bus.read_byte_data(self.address, reg+1) logger.debug('I2C: Device 0x%02X returned 0x%04X from reg 0x%02X', self.address, result & 0xFFFF, reg) if result == 0x7FFF or result == 0x8000: logger.critical('I2C read max value') time.sleep(0.0005) else: return result except IOError, err: self.misses += 1 logger.exception('Error %d, %s accessing 0x%02X: Check your I2C address', err.errno, err.strerror, self.address) time.sleep(0.0001) def readList(self, reg, length): "Reads a a byte array value from the I2C device" while True: try: result = self.bus.read_i2c_block_data(self.address, reg, length) logger.debug('I2C: Device 0x%02X from reg 0x%02X', self.address, reg) return result except IOError, err: self.misses += 1 logger.exception('Error %d, %s accessing 0x%02X: Check your I2C address', err.errno, err.strerror, self.address) time.sleep(0.0001) def getMisses(self): return self.misses ############################################################################################ # # Gyroscope / Accelerometer class for reading position / movement # ############################################################################################ class MPU6050 : i2c = None # Registers/etc. __MPU6050_RA_XG_OFFS_TC= 0x00 # [7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD __MPU6050_RA_YG_OFFS_TC= 0x01 # [7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD __MPU6050_RA_ZG_OFFS_TC= 0x02 # [7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD __MPU6050_RA_X_FINE_GAIN= 0x03 # [7:0] X_FINE_GAIN __MPU6050_RA_Y_FINE_GAIN= 0x04 # [7:0] Y_FINE_GAIN __MPU6050_RA_Z_FINE_GAIN= 0x05 # [7:0] Z_FINE_GAIN __MPU6050_RA_XA_OFFS_H= 0x06 # [15:0] XA_OFFS __MPU6050_RA_XA_OFFS_L_TC= 0x07 __MPU6050_RA_YA_OFFS_H= 0x08 # [15:0] YA_OFFS __MPU6050_RA_YA_OFFS_L_TC= 0x09 __MPU6050_RA_ZA_OFFS_H= 0x0A # [15:0] ZA_OFFS __MPU6050_RA_ZA_OFFS_L_TC= 0x0B __MPU6050_RA_XG_OFFS_USRH= 0x13 # [15:0] XG_OFFS_USR __MPU6050_RA_XG_OFFS_USRL= 0x14 __MPU6050_RA_YG_OFFS_USRH= 0x15 # [15:0] YG_OFFS_USR __MPU6050_RA_YG_OFFS_USRL= 0x16 __MPU6050_RA_ZG_OFFS_USRH= 0x17 # [15:0] ZG_OFFS_USR __MPU6050_RA_ZG_OFFS_USRL= 0x18 __MPU6050_RA_SMPLRT_DIV= 0x19 __MPU6050_RA_CONFIG= 0x1A __MPU6050_RA_GYRO_CONFIG= 0x1B __MPU6050_RA_ACCEL_CONFIG= 0x1C __MPU6050_RA_FF_THR= 0x1D __MPU6050_RA_FF_DUR= 0x1E __MPU6050_RA_MOT_THR= 0x1F __MPU6050_RA_MOT_DUR= 0x20 __MPU6050_RA_ZRMOT_THR= 0x21 __MPU6050_RA_ZRMOT_DUR= 0x22 __MPU6050_RA_FIFO_EN= 0x23 __MPU6050_RA_I2C_MST_CTRL= 0x24 __MPU6050_RA_I2C_SLV0_ADDR= 0x25 __MPU6050_RA_I2C_SLV0_REG= 0x26 __MPU6050_RA_I2C_SLV0_CTRL= 0x27 __MPU6050_RA_I2C_SLV1_ADDR= 0x28 __MPU6050_RA_I2C_SLV1_REG= 0x29 __MPU6050_RA_I2C_SLV1_CTRL= 0x2A __MPU6050_RA_I2C_SLV2_ADDR= 0x2B __MPU6050_RA_I2C_SLV2_REG= 0x2C __MPU6050_RA_I2C_SLV2_CTRL= 0x2D __MPU6050_RA_I2C_SLV3_ADDR= 0x2E __MPU6050_RA_I2C_SLV3_REG= 0x2F __MPU6050_RA_I2C_SLV3_CTRL= 0x30 __MPU6050_RA_I2C_SLV4_ADDR= 0x31 __MPU6050_RA_I2C_SLV4_REG= 0x32 __MPU6050_RA_I2C_SLV4_DO= 0x33 __MPU6050_RA_I2C_SLV4_CTRL= 0x34 __MPU6050_RA_I2C_SLV4_DI= 0x35 __MPU6050_RA_I2C_MST_STATUS= 0x36 __MPU6050_RA_INT_PIN_CFG= 0x37 __MPU6050_RA_INT_ENABLE= 0x38 __MPU6050_RA_DMP_INT_STATUS= 0x39 __MPU6050_RA_INT_STATUS= 0x3A __MPU6050_RA_ACCEL_XOUT_H= 0x3B __MPU6050_RA_ACCEL_XOUT_L= 0x3C __MPU6050_RA_ACCEL_YOUT_H= 0x3D __MPU6050_RA_ACCEL_YOUT_L= 0x3E __MPU6050_RA_ACCEL_ZOUT_H= 0x3F __MPU6050_RA_ACCEL_ZOUT_L= 0x40 __MPU6050_RA_TEMP_OUT_H= 0x41 __MPU6050_RA_TEMP_OUT_L= 0x42 __MPU6050_RA_GYRO_XOUT_H= 0x43 __MPU6050_RA_GYRO_XOUT_L= 0x44 __MPU6050_RA_GYRO_YOUT_H= 0x45 __MPU6050_RA_GYRO_YOUT_L= 0x46 __MPU6050_RA_GYRO_ZOUT_H= 0x47 __MPU6050_RA_GYRO_ZOUT_L= 0x48 __MPU6050_RA_EXT_SENS_DATA_00= 0x49 __MPU6050_RA_EXT_SENS_DATA_01= 0x4A __MPU6050_RA_EXT_SENS_DATA_02= 0x4B __MPU6050_RA_EXT_SENS_DATA_03= 0x4C __MPU6050_RA_EXT_SENS_DATA_04= 0x4D __MPU6050_RA_EXT_SENS_DATA_05= 0x4E __MPU6050_RA_EXT_SENS_DATA_06= 0x4F __MPU6050_RA_EXT_SENS_DATA_07= 0x50 __MPU6050_RA_EXT_SENS_DATA_08= 0x51 __MPU6050_RA_EXT_SENS_DATA_09= 0x52 __MPU6050_RA_EXT_SENS_DATA_10= 0x53 __MPU6050_RA_EXT_SENS_DATA_11= 0x54 __MPU6050_RA_EXT_SENS_DATA_12= 0x55 __MPU6050_RA_EXT_SENS_DATA_13= 0x56 __MPU6050_RA_EXT_SENS_DATA_14= 0x57 __MPU6050_RA_EXT_SENS_DATA_15= 0x58 __MPU6050_RA_EXT_SENS_DATA_16= 0x59 __MPU6050_RA_EXT_SENS_DATA_17= 0x5A __MPU6050_RA_EXT_SENS_DATA_18= 0x5B __MPU6050_RA_EXT_SENS_DATA_19= 0x5C __MPU6050_RA_EXT_SENS_DATA_20= 0x5D __MPU6050_RA_EXT_SENS_DATA_21= 0x5E __MPU6050_RA_EXT_SENS_DATA_22= 0x5F __MPU6050_RA_EXT_SENS_DATA_23= 0x60 __MPU6050_RA_MOT_DETECT_STATUS= 0x61 __MPU6050_RA_I2C_SLV0_DO= 0x63 __MPU6050_RA_I2C_SLV1_DO= 0x64 __MPU6050_RA_I2C_SLV2_DO= 0x65 __MPU6050_RA_I2C_SLV3_DO= 0x66 __MPU6050_RA_I2C_MST_DELAY_CTRL= 0x67 __MPU6050_RA_SIGNAL_PATH_RESET= 0x68 __MPU6050_RA_MOT_DETECT_CTRL= 0x69 __MPU6050_RA_USER_CTRL= 0x6A __MPU6050_RA_PWR_MGMT_1= 0x6B __MPU6050_RA_PWR_MGMT_2= 0x6C __MPU6050_RA_BANK_SEL= 0x6D __MPU6050_RA_MEM_START_ADDR= 0x6E __MPU6050_RA_MEM_R_W= 0x6F __MPU6050_RA_DMP_CFG_1= 0x70 __MPU6050_RA_DMP_CFG_2= 0x71 __MPU6050_RA_FIFO_COUNTH= 0x72 __MPU6050_RA_FIFO_COUNTL= 0x73 __MPU6050_RA_FIFO_R_W= 0x74 __MPU6050_RA_WHO_AM_I= 0x75 __CALIBRATION_ITERATIONS = 50 def __init__(self, address=0x68, dlpf=4): self.i2c = I2C(address) self.address = address self.sensor_data = array('B', [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) self.result_array = array('h', [0, 0, 0, 0, 0, 0, 0]) self.misses = 0 self.gx_offset = 0.0 self.gy_offset = 0.0 self.gz_offset = 0.0 self.ax_offset_m = -0.008672274 self.ax_offset_c = -95.85877449 self.ay_offset_m = 0.001811116 self.ay_offset_c = 152.3654568 self.az_offset_m = 0.086221252 self.az_offset_c = 1245.398862 self.ax_gain_m = -0.000000104005 self.ax_gain_c = 0.994343914 self.ay_gain_m = -0.000000134932 self.ay_gain_c = 0.991553647 self.az_gain_m = -0.000000177587 self.az_gain_c = 0.999063422 logger.info('Reseting MPU-6050') #--------------------------------------------------------------------------- # Ensure chip has completed boot #--------------------------------------------------------------------------- time.sleep(0.5) #--------------------------------------------------------------------------- # Reset all registers #--------------------------------------------------------------------------- logger.debug('Reset all registers') self.i2c.write8(self.__MPU6050_RA_PWR_MGMT_1, 0x80) time.sleep(5.0) #--------------------------------------------------------------------------- # Sets sample rate to 1kHz/1+0 = 1kHz or 1ms #--------------------------------------------------------------------------- logger.debug('Sample rate 1kHz') self.i2c.write8(self.__MPU6050_RA_SMPLRT_DIV, 0x0) time.sleep(0.1) #--------------------------------------------------------------------------- # Sets clock source to gyro reference w/ PLL #--------------------------------------------------------------------------- logger.debug('Clock gyro PLL') self.i2c.write8(self.__MPU6050_RA_PWR_MGMT_1, 0x02) time.sleep(0.1) #--------------------------------------------------------------------------- # Disable FSync, Use of DLPF => 1kHz sample frequency used above divided by the # sample divide factor. # 0x01 = 180Hz # 0x02 = 100Hz # 0x03 = 45Hz # 0x04 = 20Hz # 0x05 = 10Hz # 0x06 = 5Hz #--------------------------------------------------------------------------- logger.debug('configurable DLPF to filter out non-gravitational acceleration for Euler') self.i2c.write8(self.__MPU6050_RA_CONFIG, dlpf) time.sleep(0.1) #--------------------------------------------------------------------------- # Disable gyro self tests, scale of # 0x00 = +/- 250 degrees/s # 0x08 = +/- 500 degrees/s # 0x10 = +/- 1000 degrees/s # 0x18 = +/- 2000 degrees/s # See SCALE_GYRO for converstion from raw data to units of radians per second #--------------------------------------------------------------------------- # int(math.log(degrees / 250, 2)) << 3 logger.debug('Gyro +/-250 degrees/s') self.i2c.write8(self.__MPU6050_RA_GYRO_CONFIG, 0x00) time.sleep(0.1) #--------------------------------------------------------------------------- # Disable accel self tests, scale of +/-2g # 0x00 = +/- 2g # 0x08 = +/- 4g # 0x10 = +/- 8g # 0x18 = +/- 16g # See SCALE_ACCEL for convertion from raw data to units of meters per second squared #--------------------------------------------------------------------------- # int(math.log(g / 2, 2)) << 3 logger.debug('Accel +/- 2g') self.i2c.write8(self.__MPU6050_RA_ACCEL_CONFIG, 0x00) time.sleep(0.1) #--------------------------------------------------------------------------- # Setup INT pin to latch and AUX I2C pass through #--------------------------------------------------------------------------- logger.debug('Enable interrupt') self.i2c.write8(self.__MPU6050_RA_INT_PIN_CFG, 0x10) # 0x10 for edge detection, 0x20 for polling time.sleep(0.1) #--------------------------------------------------------------------------- # Enable data ready interrupt #--------------------------------------------------------------------------- logger.debug('Interrupt data ready') self.i2c.write8(self.__MPU6050_RA_INT_ENABLE, 0x01) time.sleep(0.1) #--------------------------------------------------------------------------- # Check DLPF programming has worked. #--------------------------------------------------------------------------- check_dlpf = self.i2c.readU8(self.__MPU6050_RA_CONFIG) if check_dlpf != dlpf: logger.critical("dlpf_check = %d, dlpf_config = %d", dlpf_check, dlpf_config); CleanShutdown() def readSensorsRaw(self): global time_now #--------------------------------------------------------------------------- # Wait for the data ready interrupt #--------------------------------------------------------------------------- RPIO.edge_detect_wait(RPIO_DATA_READY_INTERRUPT) #--------------------------------------------------------------------------- # For speed of reading, read all the sensors and parse to SHORTs after. This # also ensures a self consistent set of sensor data compared to reading each # individually where the sensor data registers could be updated between reads. #--------------------------------------------------------------------------- sensor_data = self.i2c.readList(self.__MPU6050_RA_ACCEL_XOUT_H, 14) #--------------------------------------------------------------------------- # Time stamp the data for the best integration possible in the main # processing loop #--------------------------------------------------------------------------- time_now = time.time() for index in range(0, 14, 2): if (sensor_data[index] > 127): sensor_data[index] -= 256 self.result_array[int(index / 2)] = (sensor_data[index] << 8) + sensor_data[index + 1] return self.result_array def readSensors(self): #--------------------------------------------------------------------------- # +/- 2g 2 * 16 bit range for the accelerometer # +/- 250 degrees per second * 16 bit range for the gyroscope - converted to radians #--------------------------------------------------------------------------- [ax, ay, az, temp, gx, gy, gz] = self.readSensorsRaw() ax_offset = temp * self.ax_offset_m + self.ax_offset_c ay_offset = temp * self.ay_offset_m + self.ay_offset_c az_offset = temp * self.az_offset_m + self.az_offset_c ax_gain = temp * self.ax_gain_m + self.ax_gain_c ay_gain = temp * self.ay_gain_m + self.ay_gain_c az_gain = temp * self.az_gain_m + self.az_gain_c qax = (ax + ax_offset) * ax_gain qay = (ay + ay_offset) * ay_gain qaz = (az + az_offset) * az_gain qgx = gx - self.gx_offset qgy = gy - self.gy_offset qgz = gz - self.gz_offset return qax, qay, qaz, qgx, -qgy, qgz, temp def getMisses(self): i2c_misses = self.i2c.getMisses() return self.misses, i2c_misses ############################################################################################ # # PID algorithm to take input sensor readings, and target requirements, and # as a result feedback new rotor speeds. # ############################################################################################ class PID: def __init__(self, p_gain, i_gain, d_gain, now): self.last_error = 0.0 self.last_time = now self.p_gain = p_gain self.i_gain = i_gain self.d_gain = d_gain self.i_error = 0.0 self.i_err_min = 0.0 self.i_err_max = 0.0 def Compute(self, input, target, now): dt = (now - self.last_time) #--------------------------------------------------------------------------- # Error is what the PID alogithm acts upon to derive the output #--------------------------------------------------------------------------- error = target - input #--------------------------------------------------------------------------- # The proportional term takes the distance between current input and target # and uses this proportially (based on Kp) to control the ESC pulse width #--------------------------------------------------------------------------- p_error = error #--------------------------------------------------------------------------- # The integral term sums the errors across many compute calls to allow for # external factors like wind speed and friction #--------------------------------------------------------------------------- self.i_error += (error + self.last_error) * dt i_error = self.i_error #--------------------------------------------------------------------------- # The differential term accounts for the fact that as error approaches 0, # the output needs to be reduced proportionally to ensure factors such as # momentum do not cause overshoot. #--------------------------------------------------------------------------- d_error = (error - self.last_error) / dt #--------------------------------------------------------------------------- # The overall output is the sum of the (P)roportional, (I)ntegral and (D)iffertial terms #--------------------------------------------------------------------------- p_output = self.p_gain * p_error i_output = self.i_gain * i_error d_output = self.d_gain * d_error #--------------------------------------------------------------------------- # Store off last input for the next differential calculation and time for next integral calculation #--------------------------------------------------------------------------- self.last_error = error self.last_time = now #--------------------------------------------------------------------------- # Return the output, which has been tuned to be the increment / decrement in ESC PWM #--------------------------------------------------------------------------- return p_output, i_output, d_output ############################################################################################ # # Class for managing each blade + motor configuration via its ESC # ############################################################################################ class THERMOSTAT: def __init__(self, pin): #--------------------------------------------------------------------------- # The GPIO BCM numbered pin providing PWM signal for this ESC #--------------------------------------------------------------------------- self.bcm_pin = pin #--------------------------------------------------------------------------- # Initialize the RPIO DMA PWM for this ESC. #--------------------------------------------------------------------------- PWM.add_channel_pulse(RPIO_DMA_CHANNEL, self.bcm_pin, 0, 0) def update(self, temp_out): temp_out = int(round(temp_out)) if temp_out > 1999: CleanShutdown() sys.exit(0) PWM.add_channel_pulse(RPIO_DMA_CHANNEL, self.bcm_pin, 0, temp_out) ############################################################################################ # # GPIO pins initialization for MPU6050 interrupt, sounder and hardware PWM # def RpioSetup(): RPIO.setmode(RPIO.BCM) #----------------------------------------------------------------------------------- # Set the MPU6050 interrupt input #----------------------------------------------------------------------------------- logger.info('Setup MPU6050 interrupt input %s', RPIO_DATA_READY_INTERRUPT) RPIO.setup(RPIO_DATA_READY_INTERRUPT, RPIO.IN) # , RPIO.PUD_DOWN) RPIO.edge_detect_init(RPIO_DATA_READY_INTERRUPT, RPIO.RISING) #----------------------------------------------------------------------------------- # Set up the globally shared single PWM channel #----------------------------------------------------------------------------------- PWM.set_loglevel(PWM.LOG_LEVEL_ERRORS) PWM.setup(1) # 1us increment PWM.init_channel(RPIO_DMA_CHANNEL, 3000) # 3ms carrier period ############################################################################################ # # GPIO pins cleanup for MPU6050 interrupt, sounder and hardware PWM # ############################################################################################ def RpioCleanup(): PWM.cleanup() RPIO.edge_detect_term(RPIO_DATA_READY_INTERRUPT) RPIO.cleanup() ############################################################################################ # # Check CLI validity, set calibrate_sensors / fly or sys.exit(1) # ############################################################################################ def CheckCLI(argv): cli_tpp_gain = 0.5 cli_tpi_gain = 0.01 cli_tpd_gain = 0.0 #----------------------------------------------------------------------------------- # Right, let's get on with reading the command line and checking consistency #----------------------------------------------------------------------------------- try: opts, args = getopt.getopt(argv,'', ['tp=', 'ti=', 'td=']) except getopt.GetoptError: logger.critical('Incorrect CLI parameter') sys.exit(2) for opt, arg in opts: if opt in '--tp': cli_tp_gain = float(arg) elif opt in '--ti': cli_ti_gain = float(arg) elif opt in '--td': cli_td_gain = float(arg) return cli_tpp_gain, cli_tpi_gain, cli_tpd_gain ############################################################################################ # # Shutdown triggered by early Ctrl-C or end of script # ############################################################################################ def CleanShutdown(): #----------------------------------------------------------------------------------- # Stop the signal handler #----------------------------------------------------------------------------------- signal.signal(signal.SIGINT, signal.SIG_IGN) #----------------------------------------------------------------------------------- # Time for teddy bye byes #----------------------------------------------------------------------------------- thermostat.update(0) #----------------------------------------------------------------------------------- # Copy logs from /dev/shm (shared / virtual memory) to the Logs directory. #----------------------------------------------------------------------------------- now = datetime.now() now_string = now.strftime("%y%m%d-%H:%M:%S") log_file_name = "qcstats" + now_string + ".csv" shutil.move("/dev/shm/qclogs", log_file_name) #----------------------------------------------------------------------------------- # Clean up PWM / GPIO #----------------------------------------------------------------------------------- RpioCleanup() #----------------------------------------------------------------------------------- # Reset the signal handler to default #----------------------------------------------------------------------------------- signal.signal(signal.SIGINT, signal.SIG_DFL) sys.exit(0) ############################################################################################ # # Signal handler for Ctrl-C => next FSM update if running else stop # ############################################################################################ def SignalHandler(signal, frame): global keep_looping if loop_count > 0: keep_looping = False else: CleanShutdown() ############################################################################################ # # Main # ############################################################################################ loop_count = 0 #------------------------------------------------------------------------------------------- # Set the BCM output / intput assigned to LED and sensor interrupt respectively #------------------------------------------------------------------------------------------- RPIO_DMA_CHANNEL = 1 RPIO_DATA_READY_INTERRUPT = 25 RPIO_THERMOSTAT_PWM = 24 #------------------------------------------------------------------------------------------- # Set up the base logging #------------------------------------------------------------------------------------------- logger = logging.getLogger('QC logger') logger.setLevel(logging.INFO) #------------------------------------------------------------------------------------------- # Create file and console logger handlers - the file is written into shared memory and only # dumped to disk / SD card at the end of a flight for performance reasons #------------------------------------------------------------------------------------------- file_handler = logging.FileHandler("/dev/shm/qclogs", 'w') file_handler.setLevel(logging.WARNING) console_handler = logging.StreamHandler() console_handler.setLevel(logging.CRITICAL) #------------------------------------------------------------------------------------------- # Create a formatter and add it to both handlers #------------------------------------------------------------------------------------------- console_formatter = logging.Formatter('%(message)s') console_handler.setFormatter(console_formatter) file_formatter = logging.Formatter('[%(levelname)s] (%(threadName)-10s) %(funcName)s %(lineno)d, %(message)s') file_handler.setFormatter(file_formatter) #------------------------------------------------------------------------------------------- # Add both handlers to the logger #------------------------------------------------------------------------------------------- logger.addHandler(console_handler) logger.addHandler(file_handler) #------------------------------------------------------------------------------------------- # Check the command line for thermostat PID gains #------------------------------------------------------------------------------------------- tpp_gain, tpi_gain, tpd_gain = CheckCLI(sys.argv[1:]) logger.warning('tpp_gain = %f, tpi_gain = %f, tpd_gain = %f', tpp_gain, tpi_gain, tpd_gain) #------------------------------------------------------------------------------------------- # Set up the global constants # - gravity in meters per second squared # - accelerometer in g's # - gyroscope in radians per second #------------------------------------------------------------------------------------------- GRAV_ACCEL = 9.80665 SCALE_GYRO = 500.0 * math.pi / (65536 * 180) SCALE_ACCEL = 4.0 / 65536 TEMP_TARGET = -2220 #------------------------------------------------------------------------------------------- # Enable RPIO for beeper, MPU 6050 interrupts and PWM #------------------------------------------------------------------------------------------- RpioSetup() #------------------------------------------------------------------------------------------- # Initialize the gyroscope / accelerometer I2C object and thermostat PWM #------------------------------------------------------------------------------------------- mpu6050 = MPU6050(0x68) thermostat = THERMOSTAT(RPIO_THERMOSTAT_PWM) signal.signal(signal.SIGINT, SignalHandler) #------------------------------------------------------------------------------------------ # Set up the bits of state setup before takeoff #------------------------------------------------------------------------------------------- keep_looping = True #------------------------------------------------------------------------------------------- # The earth X axis speed controls forward / backward speed #------------------------------------------------------------------------------------------- PID_TEMP_P_GAIN = tpp_gain PID_TEMP_I_GAIN = tpi_gain PID_TEMP_D_GAIN = tpd_gain logger.critical('Thunderbirds are go!') #------------------------------------------------------------------------------------------- # Diagnostic log header #------------------------------------------------------------------------------------------- logger.warning('time, dt, loop, qgx, qgy, qgz, qax, qay, qaz, temp, temp_out') #------------------------------------------------------------------------------------------- # Initialize the #------------------------------------------------------------------------------------------- qax, qay, qaz, qgx, qgy, qgz, temp = mpu6050.readSensors() temp_pid = PID(PID_TEMP_P_GAIN, PID_TEMP_I_GAIN, PID_TEMP_D_GAIN, time_now) start_time = time_now elapsed_time = 0.0 while keep_looping: #=================================================================================== # Sensors: Read the sensor values; note that this also sets the time_now to be as # accurate a time stamp for the sensor data as possible. #=================================================================================== qax, qay, qaz, qgx, qgy, qgz, temp = mpu6050.readSensors() qgx *= SCALE_GYRO qgy *= SCALE_GYRO qgz *= SCALE_GYRO qax *= SCALE_ACCEL qay *= SCALE_ACCEL qaz *= SCALE_ACCEL #----------------------------------------------------------------------------------- # Now we have the sensor snapshot, tidy up the rest of the variable so that processing # takes zero time. #----------------------------------------------------------------------------------- delta_time = time_now - start_time - elapsed_time elapsed_time = time_now - start_time loop_count += 1 tpp_out, tpi_out, tpd_out = temp_pid.Compute(temp, TEMP_TARGET, time_now) temp_out = tpp_out + tpi_out + tpd_out thermostat.update(temp_out) logger.critical('%f, %f, %f, %f, %f, %f, %f, %d, %f %f, %f, %f', elapsed_time, qgx, qgy, qgz, qax, qay, qaz, temp, (temp / 340) + 36.53, tpp_out, tpi_out, tpd_out) time.sleep(0.5) #------------------------------------------------------------------------------------------- # Dump the variety of sensor misses #------------------------------------------------------------------------------------------- mpu6050_misses, i2c_misses = mpu6050.getMisses() logger.critical("mpu6050 %d misses, i2c %d misses", mpu6050_misses, i2c_misses) #------------------------------------------------------------------------------------------- # Time for telly bye byes #------------------------------------------------------------------------------------------- CleanShutdown()