API tools faq
paste
Advertisement
Guest User

Program a RasPi quadcopter

a guest
Jul 1st, 2015
1,984
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 76.33 KB | None | 0 0
raw download clone embed print report
  1. #!/usr/bin/env python
  2. from __future__ import division
  3. import signal
  4. import socket
  5. import time
  6. import string
  7. import sys
  8. import getopt
  9. import time
  10. import math
  11. import threading
  12. from array import *
  13. import smbus
  14. import select
  15. import os
  16. import struct
  17. import logging
  18. from RPIO import PWM
  19. import RPIO
  20. import subprocess
  21. from datetime import datetime
  22. import shutil
  23. import ctypes
  24. from ctypes.util import find_library
  25.  
  26. ############################################################################################
  27. #
  28. # Adafruit i2c interface plus performance / error handling changes
  29. #
  30. ############################################################################################
  31. class I2C:
  32.  
  33. def __init__(self, address, bus=smbus.SMBus(1)):
  34. self.address = address
  35. self.bus = bus
  36.  
  37. def reverseByteOrder(self, data):
  38. "Reverses the byte order of an int (16-bit) or long (32-bit) value"
  39. # Courtesy Vishal Sapre
  40. dstr = hex(data)[2:].replace('L','')
  41. byteCount = len(dstr[::2])
  42. val = 0
  43. for i, n in enumerate(range(byteCount)):
  44. d = data & 0xFF
  45. val |= (d << (8 * (byteCount - i - 1)))
  46. data >>= 8
  47. return val
  48.  
  49. def write8(self, reg, value):
  50. "Writes an 8-bit value to the specified register/address"
  51. while True:
  52. try:
  53. self.bus.write_byte_data(self.address, reg, value)
  54. logger.debug('I2C: Wrote 0x%02X to register 0x%02X', value, reg)
  55. break
  56. except IOError, err:
  57. logger.exception('Error %d, %s accessing 0x%02X: Check your I2C address', err.errno, err.strerror, self.address)
  58. time.sleep(0.001)
  59.  
  60. def writeList(self, reg, list):
  61. "Writes an array of bytes using I2C format"
  62. while True:
  63. try:
  64. self.bus.write_i2c_block_data(self.address, reg, list)
  65. break
  66. except IOError, err:
  67. logger.exception('Error %d, %s accessing 0x%02X: Check your I2C address', err.errno, err.strerror, self.address)
  68. time.sleep(0.001)
  69.  
  70. def readU8(self, reg):
  71. "Read an unsigned byte from the I2C device"
  72. while True:
  73. try:
  74. result = self.bus.read_byte_data(self.address, reg)
  75. logger.debug('I2C: Device 0x%02X returned 0x%02X from reg 0x%02X', self.address, result & 0xFF, reg)
  76. return result
  77. except IOError, err:
  78. logger.exception('Error %d, %s accessing 0x%02X: Check your I2C address', err.errno, err.strerror, self.address)
  79. time.sleep(0.001)
  80.  
  81. def readS8(self, reg):
  82. "Reads a signed byte from the I2C device"
  83. while True:
  84. try:
  85. result = self.bus.read_byte_data(self.address, reg)
  86. logger.debug('I2C: Device 0x%02X returned 0x%02X from reg 0x%02X', self.address, result & 0xFF, reg)
  87. if (result > 127):
  88. return result - 256
  89. else:
  90. return result
  91. except IOError, err:
  92. logger.exception('Error %d, %s accessing 0x%02X: Check your I2C address', err.errno, err.strerror, self.address)
  93. time.sleep(0.001)
  94.  
  95. def readU16(self, reg):
  96. "Reads an unsigned 16-bit value from the I2C device"
  97. while True:
  98. try:
  99. hibyte = self.bus.read_byte_data(self.address, reg)
  100. result = (hibyte << 8) + self.bus.read_byte_data(self.address, reg+1)
  101. logger.debug('I2C: Device 0x%02X returned 0x%04X from reg 0x%02X', self.address, result & 0xFFFF, reg)
  102. if result == 0x7FFF or result == 0x8000:
  103. logger.critical('I2C read max value')
  104. time.sleep(0.001)
  105. else:
  106. return result
  107. except IOError, err:
  108. logger.exception('Error %d, %s accessing 0x%02X: Check your I2C address', err.errno, err.strerror, self.address)
  109. time.sleep(0.001)
  110.  
  111. def readS16(self, reg):
  112. "Reads a signed 16-bit value from the I2C device"
  113. while True:
  114. try:
  115. hibyte = self.bus.read_byte_data(self.address, reg)
  116. if (hibyte > 127):
  117. hibyte -= 256
  118. result = (hibyte << 8) + self.bus.read_byte_data(self.address, reg+1)
  119. logger.debug('I2C: Device 0x%02X returned 0x%04X from reg 0x%02X', self.address, result & 0xFFFF, reg)
  120. if result == 0x7FFF or result == 0x8000:
  121. logger.critical('I2C read max value')
  122. time.sleep(0.001)
  123. else:
  124. return result
  125. except IOError, err:
  126. logger.exception('Error %d, %s accessing 0x%02X: Check your I2C address', err.errno, err.strerror, self.address)
  127. time.sleep(0.001)
  128.  
  129. def readList(self, reg, length):
  130. "Reads a a byte array value from the I2C device"
  131. while True:
  132. try:
  133. result = self.bus.read_i2c_block_data(self.address, reg, length)
  134. logger.debug('I2C: Device 0x%02X from reg 0x%02X', self.address, reg)
  135. return result
  136. except IOError, err:
  137. logger.exception('Error %d, %s accessing 0x%02X: Check your I2C address', err.errno, err.strerror, self.address)
  138. time.sleep(0.001)
  139.  
  140.  
  141. ############################################################################################
  142. #
  143. # Gyroscope / Accelerometer class for reading position / movement
  144. #
  145. ############################################################################################
  146. class MPU6050 :
  147. i2c = None
  148.  
  149. # Registers/etc.
  150. __MPU6050_RA_XG_OFFS_TC= 0x00 # [7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
  151. __MPU6050_RA_YG_OFFS_TC= 0x01 # [7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
  152. __MPU6050_RA_ZG_OFFS_TC= 0x02 # [7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
  153. __MPU6050_RA_X_FINE_GAIN= 0x03 # [7:0] X_FINE_GAIN
  154. __MPU6050_RA_Y_FINE_GAIN= 0x04 # [7:0] Y_FINE_GAIN
  155. __MPU6050_RA_Z_FINE_GAIN= 0x05 # [7:0] Z_FINE_GAIN
  156. __MPU6050_RA_XA_OFFS_H= 0x06 # [15:0] XA_OFFS
  157. __MPU6050_RA_XA_OFFS_L_TC= 0x07
  158. __MPU6050_RA_YA_OFFS_H= 0x08 # [15:0] YA_OFFS
  159. __MPU6050_RA_YA_OFFS_L_TC= 0x09
  160. __MPU6050_RA_ZA_OFFS_H= 0x0A # [15:0] ZA_OFFS
  161. __MPU6050_RA_ZA_OFFS_L_TC= 0x0B
  162. __MPU6050_RA_XG_OFFS_USRH= 0x13 # [15:0] XG_OFFS_USR
  163. __MPU6050_RA_XG_OFFS_USRL= 0x14
  164. __MPU6050_RA_YG_OFFS_USRH= 0x15 # [15:0] YG_OFFS_USR
  165. __MPU6050_RA_YG_OFFS_USRL= 0x16
  166. __MPU6050_RA_ZG_OFFS_USRH= 0x17 # [15:0] ZG_OFFS_USR
  167. __MPU6050_RA_ZG_OFFS_USRL= 0x18
  168. __MPU6050_RA_SMPLRT_DIV= 0x19
  169. __MPU6050_RA_CONFIG= 0x1A
  170. __MPU6050_RA_GYRO_CONFIG= 0x1B
  171. __MPU6050_RA_ACCEL_CONFIG= 0x1C
  172. __MPU6050_RA_FF_THR= 0x1D
  173. __MPU6050_RA_FF_DUR= 0x1E
  174. __MPU6050_RA_MOT_THR= 0x1F
  175. __MPU6050_RA_MOT_DUR= 0x20
  176. __MPU6050_RA_ZRMOT_THR= 0x21
  177. __MPU6050_RA_ZRMOT_DUR= 0x22
  178. __MPU6050_RA_FIFO_EN= 0x23
  179. __MPU6050_RA_I2C_MST_CTRL= 0x24
  180. __MPU6050_RA_I2C_SLV0_ADDR= 0x25
  181. __MPU6050_RA_I2C_SLV0_REG= 0x26
  182. __MPU6050_RA_I2C_SLV0_CTRL= 0x27
  183. __MPU6050_RA_I2C_SLV1_ADDR= 0x28
  184. __MPU6050_RA_I2C_SLV1_REG= 0x29
  185. __MPU6050_RA_I2C_SLV1_CTRL= 0x2A
  186. __MPU6050_RA_I2C_SLV2_ADDR= 0x2B
  187. __MPU6050_RA_I2C_SLV2_REG= 0x2C
  188. __MPU6050_RA_I2C_SLV2_CTRL= 0x2D
  189. __MPU6050_RA_I2C_SLV3_ADDR= 0x2E
  190. __MPU6050_RA_I2C_SLV3_REG= 0x2F
  191. __MPU6050_RA_I2C_SLV3_CTRL= 0x30
  192. __MPU6050_RA_I2C_SLV4_ADDR= 0x31
  193. __MPU6050_RA_I2C_SLV4_REG= 0x32
  194. __MPU6050_RA_I2C_SLV4_DO= 0x33
  195. __MPU6050_RA_I2C_SLV4_CTRL= 0x34
  196. __MPU6050_RA_I2C_SLV4_DI= 0x35
  197. __MPU6050_RA_I2C_MST_STATUS= 0x36
  198. __MPU6050_RA_INT_PIN_CFG= 0x37
  199. __MPU6050_RA_INT_ENABLE= 0x38
  200. __MPU6050_RA_DMP_INT_STATUS= 0x39
  201. __MPU6050_RA_INT_STATUS= 0x3A
  202. __MPU6050_RA_ACCEL_XOUT_H= 0x3B
  203. __MPU6050_RA_ACCEL_XOUT_L= 0x3C
  204. __MPU6050_RA_ACCEL_YOUT_H= 0x3D
  205. __MPU6050_RA_ACCEL_YOUT_L= 0x3E
  206. __MPU6050_RA_ACCEL_ZOUT_H= 0x3F
  207. __MPU6050_RA_ACCEL_ZOUT_L= 0x40
  208. __MPU6050_RA_TEMP_OUT_H= 0x41
  209. __MPU6050_RA_TEMP_OUT_L= 0x42
  210. __MPU6050_RA_GYRO_XOUT_H= 0x43
  211. __MPU6050_RA_GYRO_XOUT_L= 0x44
  212. __MPU6050_RA_GYRO_YOUT_H= 0x45
  213. __MPU6050_RA_GYRO_YOUT_L= 0x46
  214. __MPU6050_RA_GYRO_ZOUT_H= 0x47
  215. __MPU6050_RA_GYRO_ZOUT_L= 0x48
  216. __MPU6050_RA_EXT_SENS_DATA_00= 0x49
  217. __MPU6050_RA_EXT_SENS_DATA_01= 0x4A
  218. __MPU6050_RA_EXT_SENS_DATA_02= 0x4B
  219. __MPU6050_RA_EXT_SENS_DATA_03= 0x4C
  220. __MPU6050_RA_EXT_SENS_DATA_04= 0x4D
  221. __MPU6050_RA_EXT_SENS_DATA_05= 0x4E
  222. __MPU6050_RA_EXT_SENS_DATA_06= 0x4F
  223. __MPU6050_RA_EXT_SENS_DATA_07= 0x50
  224. __MPU6050_RA_EXT_SENS_DATA_08= 0x51
  225. __MPU6050_RA_EXT_SENS_DATA_09= 0x52
  226. __MPU6050_RA_EXT_SENS_DATA_10= 0x53
  227. __MPU6050_RA_EXT_SENS_DATA_11= 0x54
  228. __MPU6050_RA_EXT_SENS_DATA_12= 0x55
  229. __MPU6050_RA_EXT_SENS_DATA_13= 0x56
  230. __MPU6050_RA_EXT_SENS_DATA_14= 0x57
  231. __MPU6050_RA_EXT_SENS_DATA_15= 0x58
  232. __MPU6050_RA_EXT_SENS_DATA_16= 0x59
  233. __MPU6050_RA_EXT_SENS_DATA_17= 0x5A
  234. __MPU6050_RA_EXT_SENS_DATA_18= 0x5B
  235. __MPU6050_RA_EXT_SENS_DATA_19= 0x5C
  236. __MPU6050_RA_EXT_SENS_DATA_20= 0x5D
  237. __MPU6050_RA_EXT_SENS_DATA_21= 0x5E
  238. __MPU6050_RA_EXT_SENS_DATA_22= 0x5F
  239. __MPU6050_RA_EXT_SENS_DATA_23= 0x60
  240. __MPU6050_RA_MOT_DETECT_STATUS= 0x61
  241. __MPU6050_RA_I2C_SLV0_DO= 0x63
  242. __MPU6050_RA_I2C_SLV1_DO= 0x64
  243. __MPU6050_RA_I2C_SLV2_DO= 0x65
  244. __MPU6050_RA_I2C_SLV3_DO= 0x66
  245. __MPU6050_RA_I2C_MST_DELAY_CTRL= 0x67
  246. __MPU6050_RA_SIGNAL_PATH_RESET= 0x68
  247. __MPU6050_RA_MOT_DETECT_CTRL= 0x69
  248. __MPU6050_RA_USER_CTRL= 0x6A
  249. __MPU6050_RA_PWR_MGMT_1= 0x6B
  250. __MPU6050_RA_PWR_MGMT_2= 0x6C
  251. __MPU6050_RA_BANK_SEL= 0x6D
  252. __MPU6050_RA_MEM_START_ADDR= 0x6E
  253. __MPU6050_RA_MEM_R_W= 0x6F
  254. __MPU6050_RA_DMP_CFG_1= 0x70
  255. __MPU6050_RA_DMP_CFG_2= 0x71
  256. __MPU6050_RA_FIFO_COUNTH= 0x72
  257. __MPU6050_RA_FIFO_COUNTL= 0x73
  258. __MPU6050_RA_FIFO_R_W= 0x74
  259. __MPU6050_RA_WHO_AM_I= 0x75
  260.  
  261. __CALIBRATION_ITERATIONS = 100
  262.  
  263. def __init__(self, address=0x68):
  264. self.i2c = I2C(address)
  265. self.address = address
  266. self.grav_x_offset = 0.0
  267. self.grav_y_offset = 0.0
  268. self.grav_z_offset = 0.0
  269. self.gyro_x_offset = 0.0
  270. self.gyro_y_offset = 0.0
  271. self.gyro_z_offset = 0.0
  272. self.sensor_data = array('B', [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
  273. self.result_array = array('h', [0, 0, 0, 0, 0, 0, 0])
  274.  
  275. logger.info('Reseting MPU-6050')
  276.  
  277. #---------------------------------------------------------------------------
  278. # Reset all registers
  279. #---------------------------------------------------------------------------
  280. logger.debug('Reset all registers')
  281. self.i2c.write8(self.__MPU6050_RA_PWR_MGMT_1, 0x80)
  282. time.sleep(5.0)
  283.  
  284. #---------------------------------------------------------------------------
  285. # Sets sample rate to 1kHz/1+3 = 250Hz
  286. ####### Code currently loops at 170Hz, so 250Hz guarantees fresh data ######
  287. #---------------------------------------------------------------------------
  288. logger.debug('Sample rate 250Hz')
  289. self.i2c.write8(self.__MPU6050_RA_SMPLRT_DIV, 0x03)
  290. time.sleep(0.1)
  291.  
  292. #---------------------------------------------------------------------------
  293. # Sets clock source to gyro reference w/ PLL
  294. #---------------------------------------------------------------------------
  295. logger.debug('Clock gyro PLL')
  296. self.i2c.write8(self.__MPU6050_RA_PWR_MGMT_1, 0x02)
  297. time.sleep(0.1)
  298.  
  299. #---------------------------------------------------------------------------
  300. # Disable FSync, Use of DLPF => 1kHz sample frequency used above divided by the
  301. # sample divide factor.
  302. # 0x01 = 180Hz
  303. # 0x02 = 100Hz
  304. # 0x03 = 45Hz
  305. # 0x04 = 20Hz
  306. # 0x05 = 10Hz
  307. # 0x06 = 5Hz
  308. #---------------------------------------------------------------------------
  309. logger.debug('5Hz DLPF')
  310. self.i2c.write8(self.__MPU6050_RA_CONFIG, 0x06)
  311. time.sleep(0.1)
  312.  
  313. #---------------------------------------------------------------------------
  314. # Disable gyro self tests, scale of
  315. # 0x00 = +/- 250 degrees/s
  316. # 0x08 = +/- 500 degrees/s
  317. # 0x10 = +/- 1000 degrees/s
  318. # 0x18 = +/- 2000 degrees/s
  319. #---------------------------------------------------------------------------
  320. logger.debug('Gyro +/-500 degrees/s')
  321. self.i2c.write8(self.__MPU6050_RA_GYRO_CONFIG, 0x08)
  322. time.sleep(0.1)
  323.  
  324. #---------------------------------------------------------------------------
  325. # Disable accel self tests, scale of +/-2g
  326. # 0x00 = +/- 2g
  327. # 0x08 = +/- 4g
  328. # 0x10 = +/- 8g
  329. # 0x18 = +/- 16g
  330. #---------------------------------------------------------------------------
  331. logger.debug('Accel +/- 2g')
  332. self.i2c.write8(self.__MPU6050_RA_ACCEL_CONFIG, 0x00)
  333. time.sleep(0.1)
  334.  
  335. #---------------------------------------------------------------------------
  336. # Setup INT pin to latch and AUX I2C pass through
  337. #---------------------------------------------------------------------------
  338. logger.debug('Enable interrupt')
  339. self.i2c.write8(self.__MPU6050_RA_INT_PIN_CFG, 0x20)
  340. time.sleep(0.1)
  341.  
  342. #---------------------------------------------------------------------------
  343. # Enable data ready interrupt
  344. #---------------------------------------------------------------------------
  345. logger.debug('Interrupt data ready')
  346. self.i2c.write8(self.__MPU6050_RA_INT_ENABLE, 0x01)
  347. time.sleep(0.1)
  348.  
  349.  
  350. def readSensorsRaw(self):
  351. #---------------------------------------------------------------------------
  352. # Hard loop on the data ready interrupt until it gets set high. This clears
  353. # the interrupt also
  354. #---------------------------------------------------------------------------
  355. while not (self.i2c.readU8(self.__MPU6050_RA_INT_STATUS) == 0x01):
  356. time.sleep(0.001)
  357.  
  358. #---------------------------------------------------------------------------
  359. # For speed of reading, read all the sensors and parse to SHORTs after
  360. #---------------------------------------------------------------------------
  361. sensor_data = self.i2c.readList(self.__MPU6050_RA_ACCEL_XOUT_H, 14)
  362.  
  363. for index in range(0, 14, 2):
  364. if (sensor_data[index] > 127):
  365. sensor_data[index] -= 256
  366. self.result_array[int(index / 2)] = (sensor_data[index] << 8) + sensor_data[index + 1]
  367.  
  368. return self.result_array
  369.  
  370.  
  371. def readSensors(self):
  372. #---------------------------------------------------------------------------
  373. # +/- 2g 2 * 16 bit range for the accelerometer
  374. # +/- 500 degrees per second * 16 bit range for the gyroscope - converted to radians
  375. #---------------------------------------------------------------------------
  376. [ax, ay, az, temp, gx, gy, gz] = self.readSensorsRaw()
  377.  
  378. fax = ax * 4.0 / 65536 - self.grav_x_offset
  379. fay = ay * 4.0 / 65536 - self.grav_y_offset
  380. faz = az * 4.0 / 65536 - self.grav_z_offset
  381.  
  382. fgx = gx * 1000.0 * math.pi / (65536 * 180) - self.gyro_x_offset
  383. fgy = gy * 1000.0 * math.pi / (65536 * 180) - self.gyro_y_offset
  384. fgz = gz * 1000.0 * math.pi / (65536 * 180) - self.gyro_z_offset
  385.  
  386. return fax, fay, faz, fgx, fgy, fgz
  387.  
  388. def calibrateGyros(self):
  389. for loop_count in range(0, self.__CALIBRATION_ITERATIONS):
  390. [ax, ay, az, temp, gx, gy, gz] = self.readSensorsRaw()
  391. self.gyro_x_offset += gx
  392. self.gyro_y_offset += gy
  393. self.gyro_z_offset += gz
  394.  
  395. time.sleep(0.05)
  396.  
  397. self.gyro_x_offset *= 1000.0 * math.pi / (65536 * 180 * self.__CALIBRATION_ITERATIONS)
  398. self.gyro_y_offset *= 1000.0 * math.pi / (65536 * 180 * self.__CALIBRATION_ITERATIONS)
  399. self.gyro_z_offset *= 1000.0 * math.pi / (65536 * 180 * self.__CALIBRATION_ITERATIONS)
  400.  
  401.  
  402. def calibrateGravity(self, file_name):
  403. grav_x_offset = 0
  404. grav_y_offset = 0
  405. grav_z_offset = 0
  406.  
  407. for loop_count in range(0, self.__CALIBRATION_ITERATIONS):
  408. [ax, ay, az, temp, gx, gy, gz] = self.readSensorsRaw()
  409. grav_x_offset += ax
  410. grav_y_offset += ay
  411. grav_z_offset += az
  412.  
  413. time.sleep(0.05)
  414.  
  415. grav_x_offset *= (4.0 / (65536 * self.__CALIBRATION_ITERATIONS))
  416. grav_y_offset *= (4.0 / (65536 * self.__CALIBRATION_ITERATIONS))
  417. grav_z_offset *= (4.0 / (65536 * self.__CALIBRATION_ITERATIONS))
  418.  
  419. #---------------------------------------------------------------------------
  420. # Open the offset config file
  421. #---------------------------------------------------------------------------
  422. cfg_rc = True
  423. try:
  424. with open(file_name, 'w+') as cfg_file:
  425. cfg_file.write('%f\n' % grav_x_offset)
  426. cfg_file.write('%f\n' % grav_y_offset)
  427. cfg_file.write('%f\n' % grav_z_offset)
  428. cfg_file.flush()
  429.  
  430. except IOError, err:
  431. logger.critical('Could not open offset config file: %s for writing', file_name)
  432. cfg_rc = False
  433.  
  434. return cfg_rc
  435.  
  436.  
  437. def readGravity(self, file_name):
  438. #---------------------------------------------------------------------------
  439. # Open the Offsets config file, and read the contents
  440. #---------------------------------------------------------------------------
  441. cfg_rc = True
  442. try:
  443. with open(file_name, 'r') as cfg_file:
  444. str_grav_x_offset = cfg_file.readline()
  445. str_grav_y_offset = cfg_file.readline()
  446. str_grav_z_offset = cfg_file.readline()
  447.  
  448. self.grav_x_offset = float(str_grav_x_offset)
  449. self.grav_y_offset = float(str_grav_y_offset)
  450. self.grav_z_offset = float(str_grav_z_offset)
  451.  
  452. except IOError, err:
  453. logger.critical('Could not open offset config file: %s for reading', file_name)
  454. cfg_rc = False
  455.  
  456. return cfg_rc
  457.  
  458. def getEulerAngles(self, fax, fay, faz):
  459. #---------------------------------------------------------------------------
  460. # What's the angle in the x and y plane from horizontal in radians?
  461. # Note fax, fay, fax are all the calibrated outputs reading 0, 0, 0 on
  462. # horizontal ground as a measure of speed in a given direction. For Euler we
  463. # need to re-add gravity of 1g so the sensors read 0, 0, 1 for a horizontal setting
  464. #---------------------------------------------------------------------------
  465. pitch = -math.atan2(fax, faz + 1.0)
  466. roll = math.atan2(fay, faz + 1.0)
  467. tilt = math.atan2(faz + 1.0, math.pow(math.pow(fax, 2) + math.pow(fay, 2), 0.5))
  468.  
  469. return pitch, roll, tilt
  470.  
  471. def readTemp(self):
  472. temp = self.i2c.readS16(self.__MPU6050_RA_TEMP_OUT_H)
  473. temp = (float(temp) / 340) + 36.53
  474. logger.debug('temp = %s oC', temp)
  475. return temp
  476.  
  477.  
  478. ############################################################################################
  479. # PID algorithm to take input accelerometer readings, and target accelermeter requirements, and
  480. # as a result feedback new rotor speeds.
  481. ############################################################################################
  482. class PID:
  483.  
  484. def __init__(self, p_gain, i_gain, d_gain):
  485. self.last_error = 0.0
  486. self.last_time = time.time()
  487. self.p_gain = p_gain
  488. self.i_gain = i_gain
  489. self.d_gain = d_gain
  490.  
  491. self.i_error = 0.0
  492. self.i_err_min = 0.0
  493. self.i_err_max = 0.0
  494. if i_gain != 0.0:
  495. self.i_err_min = -250.0 / i_gain
  496. self.i_err_max = +250.0 / i_gain
  497.  
  498.  
  499. def Compute(self, input, target):
  500.  
  501. now = time.time()
  502. dt = (now - self.last_time)
  503.  
  504. #---------------------------------------------------------------------------
  505. # Error is what the PID alogithm acts upon to derive the output
  506. #---------------------------------------------------------------------------
  507. error = target - input
  508.  
  509. #---------------------------------------------------------------------------
  510. # The proportional term takes the distance between current input and target
  511. # and uses this proportially (based on Kp) to control the ESC pulse width
  512. #---------------------------------------------------------------------------
  513. p_error = error
  514.  
  515. #---------------------------------------------------------------------------
  516. # The integral term sums the errors across many compute calls to allow for
  517. # external factors like wind speed and friction
  518. #---------------------------------------------------------------------------
  519. self.i_error += (error + self.last_error) * dt
  520. if self.i_gain != 0.0 and self.i_error > self.i_err_max:
  521. self.i_error = self.i_err_max
  522. elif self.i_gain != 0.0 and self.i_error < self.i_err_min:
  523. self.i_error = self.i_err_min
  524. i_error = self.i_error
  525.  
  526. #---------------------------------------------------------------------------
  527. # The differential term accounts for the fact that as error approaches 0,
  528. # the output needs to be reduced proportionally to ensure factors such as
  529. # momentum do not cause overshoot.
  530. #---------------------------------------------------------------------------
  531. d_error = (error - self.last_error) / dt
  532.  
  533. #---------------------------------------------------------------------------
  534. # The overall output is the sum of the (P)roportional, (I)ntegral and (D)iffertial terms
  535. #---------------------------------------------------------------------------
  536. p_output = self.p_gain * p_error
  537. i_output = self.i_gain * i_error
  538. d_output = self.d_gain * d_error
  539.  
  540. #---------------------------------------------------------------------------
  541. # Store off last input (for the next differential calulation) and time for calculating the integral value
  542. #---------------------------------------------------------------------------
  543. self.last_error = error
  544. self.last_time = now
  545.  
  546. #---------------------------------------------------------------------------
  547. # Return the output, which has been tuned to be the increment / decrement in ESC PWM
  548. #---------------------------------------------------------------------------
  549. return p_output, i_output, d_output
  550.  
  551. ############################################################################################
  552. #
  553. # Class for managing each blade + motor configuration via its ESC
  554. #
  555. ############################################################################################
  556. class ESC:
  557. pwm = None
  558.  
  559. def __init__(self, pin, location, rotation, name):
  560.  
  561. #---------------------------------------------------------------------------
  562. # The GPIO BCM numbered pin providing PWM signal for this ESC
  563. #---------------------------------------------------------------------------
  564. self.bcm_pin = pin
  565.  
  566. #---------------------------------------------------------------------------
  567. # The location on the quad, and the direction of the motor controlled by this ESC
  568. #---------------------------------------------------------------------------
  569. self.motor_location = location
  570. self.motor_rotation = rotation
  571.  
  572. #---------------------------------------------------------------------------
  573. # The PWM pulse width range required by this ESC in microseconds
  574. #---------------------------------------------------------------------------
  575. self.min_pulse_width = 1000
  576. self.max_pulse_width = 2000
  577.  
  578. #---------------------------------------------------------------------------
  579. # The PWM pulse range required by this ESC
  580. #---------------------------------------------------------------------------
  581. self.current_pulse_width = self.min_pulse_width
  582. self.name = name
  583.  
  584. #---------------------------------------------------------------------------
  585. # Initialize the RPIO DMA PWM
  586. #---------------------------------------------------------------------------
  587. if not PWM.is_setup():
  588. PWM.setup(1) # 1us increment
  589. PWM.init_channel(RPIO_DMA_CHANNEL, 3000) # 3ms carrier period
  590. PWM.add_channel_pulse(RPIO_DMA_CHANNEL, self.bcm_pin, 0, self.current_pulse_width)
  591.  
  592.  
  593. def update(self, spin_rate):
  594. self.current_pulse_width = int(self.min_pulse_width + spin_rate)
  595.  
  596. if self.current_pulse_width < self.min_pulse_width:
  597. self.current_pulse_width = self.min_pulse_width
  598. if self.current_pulse_width > self.max_pulse_width:
  599. self.current_pulse_width = self.max_pulse_width
  600.  
  601. PWM.add_channel_pulse(RPIO_DMA_CHANNEL, self.bcm_pin, 0, self.current_pulse_width)
  602.  
  603.  
  604. ############################################################################################
  605. #
  606. # GPIO pins initialization for /OE on the PWM device and the status LED
  607. #
  608. ############################################################################################
  609. def RpioSetup():
  610. RPIO.setmode(RPIO.BCM)
  611.  
  612. #-----------------------------------------------------------------------------------
  613. # Set the beeper output LOW
  614. #-----------------------------------------------------------------------------------
  615. logger.info('Set status sounder pin %s as out', RPIO_STATUS_SOUNDER)
  616. RPIO.setup(RPIO_STATUS_SOUNDER, RPIO.OUT, RPIO.LOW)
  617.  
  618. #-----------------------------------------------------------------------------------
  619. # Set the MPU6050 interrupt input
  620. #-----------------------------------------------------------------------------------
  621. logger.info('Setup MPU6050 interrupt input %s', RPIO_SENSOR_DATA_RDY)
  622. RPIO.setup(RPIO_SENSOR_DATA_RDY, RPIO.IN, RPIO.PUD_DOWN)
  623.  
  624. ############################################################################################
  625. #
  626. # Parse the received commmand to convert it to the equivalent directional / operational command
  627. # The message format is a basic TLV with header and footer:
  628. # Type: 1 byte
  629. # Len: 1 byte - currently always 4 - includes this header
  630. # Data: 2 bytes
  631. #
  632. ############################################################################################
  633. class TLVSTREAM():
  634.  
  635. __CTRL_CMD_ABORT = 0
  636. __CTRL_CMD_TAKEOFF = 1
  637. __CTRL_CMD_LANDING = 2
  638. __CTRL_CMD_HOVER = 3
  639. __CTRL_CMD_UP_DOWN = 4
  640. __CTRL_CMD_FWD_RVS = 5
  641. __CTRL_CMD_LEFT_RIGHT = 6
  642. __CTRL_CMD_KEEPALIVE = 7
  643. __CTRL_CMD_DATA_ACK = 8
  644.  
  645. def __init__(self):
  646. self.cache = ""
  647.  
  648. def Parser(recv_buff):
  649. self.cache += resv_buff
  650. send_buff = ""
  651.  
  652. eax_speed_target = 0.0
  653. eay_speed_target = 0.0
  654. eaz_speed_target = 0.0
  655.  
  656. #---------------------------------------------------------------------------
  657. # Parse the message TLVs - assume no exception
  658. #---------------------------------------------------------------------------
  659. if len(self.cache) >= 4:
  660. type, length, msg_id = struct.unpack_from('!BBH', self.cache, 0)
  661.  
  662. #-----------------------------------------------------------
  663. # If we have a complete TLV, parse it
  664. #-----------------------------------------------------------
  665. if len(self.cache) >= 4 + length:
  666.  
  667. #---------------------------------------------------------------------------
  668. # If we've received a valid message type, then respond with an ACK
  669. #---------------------------------------------------------------------------
  670. send_buff = struct.pack('!BBH', __CTRL_CMD_DATA_ACK, 4, msg_id)
  671. drc_sck.send(send_buff)
  672. send_buff = ""
  673.  
  674. #---------------------------------------------------------------------------
  675. # If the message content length > 0, unpack that too
  676. #---------------------------------------------------------------------------
  677. if length > 0:
  678. value = struct.unpack_from('!I', self.cache, 4)
  679. logger.info('type %d, length 0, msg_id %d', type, length, msg_id)
  680. else:
  681. logger.info('type %d, length %d, msg_id %d, value %d', type, length, msg_id, value)
  682.  
  683.  
  684. #---------------------------------------------------------------------------
  685. # Enact the command - decide the targets for the PID algorithm
  686. #---------------------------------------------------------------------------
  687. if type == __CTRL_CMD_ABORT:
  688. #----------------------------------------------------------------------------
  689. # Hard shutdown
  690. #----------------------------------------------------------------------------
  691. logger.warning('ABORT')
  692. os.kill(os.getpid(), signal.SIGINT)
  693.  
  694. elif type == __CTRL_CMD_TAKEOFF:
  695. #----------------------------------------------------------------------------
  696. # Spin each blade up to the calibrated 0g point and then increment slight for a while beofre setting back to 0g
  697. #----------------------------------------------------------------------------
  698. logger.info('TAKEOFF')
  699. eax_speed_target = 0.0
  700. eay_speed_target = 0.0
  701.  
  702. #AB: I think this shoud be a manual incremental increase in blade speeds to achieve takeoff before handover to hover
  703. faz_target = 1.01
  704.  
  705. elif type == __CTRL_CMD_LANDING:
  706. #----------------------------------------------------------------------------
  707. # Spin each blade down to the calibrated 0g point and them decrement slightly for a controlled landing
  708. #----------------------------------------------------------------------------
  709. logger.info('LANDING')
  710. eax_speed_target = 0.0
  711. eay_speed_target = 0.0
  712. #AB: Less sure about this one though
  713. eaz_speed_target = 0.99
  714.  
  715. elif type == __CTRL_CMD_HOVER:
  716. #----------------------------------------------------------------------------
  717. # Spin each blade down to the calibrated 0g point
  718. #----------------------------------------------------------------------------
  719. logger.info('HOVER')
  720. eax_speed_target = 0.0
  721. eay_speed_target = 0.0
  722. eaz_speed_target = 1.0
  723.  
  724. elif type == __CTRL_CMD_UP_DOWN:
  725. #----------------------------------------------------------------------------
  726. # Increment the speed of all blades proportially to the command data
  727. #----------------------------------------------------------------------------
  728. logger.info('UP/DOWN %d', int(value))
  729. eax_speed_target = 0.0
  730. eay_speed_target = 0.0
  731. eaz_speed_target = 1.0 + (float(value * 0.05) / 128)
  732.  
  733. elif type == __CTRL_CMD_FWD_RVS:
  734. #----------------------------------------------------------------------------
  735. # ????????????????
  736. #----------------------------------------------------------------------------
  737. logger.info('FWD/RVS %d', int(value))
  738.  
  739. elif type == __CTRL_CMD_LEFT_RIGHT:
  740. #----------------------------------------------------------------------------
  741. # ????????????????
  742. #----------------------------------------------------------------------------
  743. logger.info('LEFT/RIGHT %d', int(value))
  744.  
  745. elif type == __CTRL_CMD_KEEPALIVE:
  746. #----------------------------------------------------------------------------
  747. # No change to blade power, keep stable at the current setting
  748. #----------------------------------------------------------------------------
  749. logger.debug('KEEPALIVE')
  750. else:
  751. #----------------------------------------------------------------------------
  752. # Unrecognised command - treat as an abort
  753. #----------------------------------------------------------------------------
  754. logger.warning('UNRECOGNISED COMMAND: ABORT')
  755. os.kill(os.getpid(), signal.SIGINT)
  756.  
  757. #---------------------------------------------------------------------------
  758. # TLV is wholly parsed, decrease the cache size
  759. #----------------------------------------------------------------------------
  760. self.cache = self.cache[4 + length : len(self.cache)]
  761. else:
  762. #---------------------------------------------------------------------------
  763. # Insufficient data to form a whole TLV, get out
  764. #---------------------------------------------------------------------------
  765. parsed_tlv = False
  766.  
  767.  
  768. else:
  769. #---------------------------------------------------------------------------
  770. # Insufficient data to parse, leave target unchanged !!!!!!!!!!!!!!!!!!!!!!!!
  771. #---------------------------------------------------------------------------
  772. parsed_tlv = False
  773.  
  774. #---------------------------------------------------------------------------
  775. # No more complete TLVs to part, get out, returning how much data we have dealt with
  776. #---------------------------------------------------------------------------
  777. return eax_speed_target, eay_speed_target, eaz_speed_target
  778.  
  779.  
  780. ############################################################################################
  781. #
  782. # Check CLI validity, set calibrate_sensors / fly or sys.exit(1)
  783. #
  784. ############################################################################################
  785. def CheckCLI(argv):
  786. cli_fly = False
  787. cli_calibrate_gravity = False
  788. cli_video = False
  789. cli_takeoff_speed = 590
  790. cli_vsp_gain = 150.0
  791. cli_vsi_gain = 50.0
  792. cli_vsd_gain = 0.0
  793. cli_hsp_gain = 0.0
  794. cli_hsi_gain = 0.0
  795. cli_hsd_gain = 0.0
  796.  
  797.  
  798. try:
  799. opts, args = getopt.getopt(argv,'fgvat:', ['vsp=', 'vsi=', 'vsd=', 'hsp=', 'hsi=', 'hsd='])
  800. except getopt.GetoptError:
  801. logger.critical('qcpi.py [-f][-t take_off_speed][-g][-v]')
  802. sys.exit(2)
  803.  
  804. for opt, arg in opts:
  805. if opt == '-f':
  806. cli_fly = True
  807.  
  808. elif opt in '-t':
  809. cli_takeoff_speed = int(arg)
  810.  
  811. elif opt in '-v':
  812. cli_video = True
  813.  
  814. elif opt in '-g':
  815. cli_calibrate_gravity = True
  816.  
  817. elif opt in '--vsp':
  818. cli_vsp_gain = float(arg)
  819.  
  820. elif opt in '--vsi':
  821. cli_vsi_gain = float(arg)
  822.  
  823. elif opt in '--vsd':
  824. cli_vsd_gain = float(arg)
  825.  
  826. elif opt in '--hsp':
  827. cli_hsp_gain = float(arg)
  828.  
  829. elif opt in '--hsi':
  830. cli_hsi_gain = float(arg)
  831.  
  832. elif opt in '--hsd':
  833. cli_hsd_gain = float(arg)
  834.  
  835.  
  836. if not cli_calibrate_gravity and not cli_fly:
  837. logger.critical('Must specify -f or -g')
  838. logger.critical(' qcpi.py [-f] [-t speed] [-c] [-v]')
  839. logger.critical(' -f set whether to fly')
  840. logger.critical(' -t set the takeoff speed for manual takeoff')
  841. logger.critical(' -g calibrate and save the gravity offsets')
  842. logger.critical(' -v video the flight')
  843. logger.critical(' --vsp set vertical speed PID P gain')
  844. logger.critical(' --vsi set vertical speed PID P gain')
  845. logger.critical(' --vsd set vertical speed PID P gain')
  846. logger.critical(' --hsp set horizontal speed PID P gain')
  847. logger.critical(' --hsi set horizontal speed PID I gain')
  848. logger.critical(' --hsd set horizontal speed PID D gain')
  849. sys.exit(2)
  850.  
  851. if cli_fly and (cli_takeoff_speed < 0 or cli_takeoff_speed > 1000):
  852. logger.critical('Test speed must lie in the following range')
  853. logger.critical('0 <= test speed <= 1000')
  854. sys.exit(2)
  855.  
  856. # if (math.copysign(1.0, cli_hsp_gain) != math.copysign(1.0, cli_hsi_gain)) and (math.copysign(1.0, cli_hsp_gain) != math.copysign(1.0, cli_hsd_gain)):
  857. # logger.critical("All horizonatal gains must have the same sign")
  858. # sys.exit(2)
  859. #
  860. # if (math.copysign(1.0, cli_vsp_gain) != math.copysign(1.0, cli_vsi_gain)) and (math.copysign(1.0, cli_vsp_gain) != math.copysign(1.0, cli_vsd_gain)):
  861. # logger.critical("All vertical gains must have the same sign")
  862. # sys.exit(2)
  863.  
  864. return cli_calibrate_gravity, cli_fly, cli_takeoff_speed, cli_video, cli_vsp_gain, cli_vsi_gain, cli_vsd_gain, cli_hsp_gain, cli_hsi_gain, cli_hsd_gain
  865.  
  866. ############################################################################################
  867. #
  868. # Count down beeper
  869. #
  870. ############################################################################################
  871. def CountdownBeep(num_beeps):
  872. for beep in range(0, num_beeps):
  873. RPIO.output(RPIO_STATUS_SOUNDER, RPIO.HIGH)
  874. time.sleep(0.25)
  875. RPIO.output(RPIO_STATUS_SOUNDER, RPIO.LOW)
  876. time.sleep(0.25)
  877. time.sleep(2.0)
  878.  
  879. ############################################################################################
  880. #
  881. # Shutdown triggered by early Ctrl-C or end of script
  882. #
  883. ############################################################################################
  884. def CleanShutdown():
  885. global esc_list
  886. global shoot_video
  887. global video
  888.  
  889. #-----------------------------------------------------------------------------------
  890. # Time for teddy bye byes
  891. #-----------------------------------------------------------------------------------
  892. for esc in esc_list:
  893. logger.info('Stop blade %d spinning', esc_index)
  894. esc.update(0)
  895.  
  896. #-----------------------------------------------------------------------------------
  897. # Stop the video if it's running
  898. #-----------------------------------------------------------------------------------
  899. if shoot_video:
  900. video.send_signal(signal.SIGINT)
  901.  
  902. #-----------------------------------------------------------------------------------
  903. # Copy logs from /dev/shm (shared / virtual memory) to the Logs directory.
  904. #-----------------------------------------------------------------------------------
  905. now = datetime.now()
  906. now_string = now.strftime("%y%m%d-%H:%M:%S")
  907. log_file_name = "qcstats" + now_string + ".csv"
  908. shutil.move("/dev/shm/qclogs", log_file_name)
  909.  
  910. #-----------------------------------------------------------------------------------
  911. # Clean up PWM / GPIO
  912. #-----------------------------------------------------------------------------------
  913. PWM.cleanup()
  914. RPIO.output(RPIO_STATUS_SOUNDER, RPIO.LOW)
  915. RPIO.cleanup()
  916.  
  917. #-----------------------------------------------------------------------------------
  918. # Unlock memory we've used from RAM
  919. #-----------------------------------------------------------------------------------
  920. munlockall()
  921.  
  922. sys.exit(0)
  923.  
  924. ############################################################################################
  925. #
  926. # Signal handler for Ctrl-C => next FSM transition if running else stop
  927. #
  928. ############################################################################################
  929. def SignalHandler(signal, frame):
  930. global loop_count
  931. global fsm_input
  932. global INPUT_SIGNAL
  933.  
  934. if loop_count > 0:
  935. fsm_input = INPUT_SIGNAL
  936. else:
  937. CleanShutdown()
  938.  
  939. ############################################################################################
  940. #
  941. # Main
  942. #
  943. ############################################################################################
  944.  
  945. #-------------------------------------------------------------------------------------------
  946. # Lock code permanently in memory - no swapping to disk
  947. #-------------------------------------------------------------------------------------------
  948. MCL_CURRENT = 1
  949. MCL_FUTURE = 2
  950. def mlockall(flags = MCL_CURRENT| MCL_FUTURE):
  951. result = libc.mlockall(flags)
  952. if result != 0:
  953. raise Exception("cannot lock memmory, errno=%s" % ctypes.get_errno())
  954.  
  955. def munlockall():
  956. result = libc.munlockall()
  957. if result != 0:
  958. raise Exception("cannot lock memmory, errno=%s" % ctypes.get_errno())
  959.  
  960.  
  961. libc_name = ctypes.util.find_library("c")
  962. libc = ctypes.CDLL(libc_name, use_errno=True)
  963. mlockall()
  964.  
  965. #-------------------------------------------------------------------------------------------
  966. # Set up the global constants
  967. #-------------------------------------------------------------------------------------------
  968. G_FORCE = 9.80665
  969.  
  970. RPIO_DMA_CHANNEL = 1
  971.  
  972. use_sockets = False
  973.  
  974. ESC_BCM_BL = 22
  975. ESC_BCM_FL = 17
  976. ESC_BCM_FR = 18
  977. ESC_BCM_BR = 23
  978.  
  979. MOTOR_LOCATION_FRONT = 0b00000000
  980. MOTOR_LOCATION_BACK = 0b00000010
  981. MOTOR_LOCATION_LEFT = 0b00000000
  982. MOTOR_LOCATION_RIGHT = 0b00000001
  983.  
  984. MOTOR_ROTATION_CW = 1
  985. MOTOR_ROTATION_ACW = 2
  986.  
  987. NUM_SOCK = 5
  988. RC_SILENCE_LIMIT = 10
  989.  
  990. #-------------------------------------------------------------------------------------------
  991. # Through testing, take-off happens @ 587
  992. #-------------------------------------------------------------------------------------------
  993. TAKEOFF_READY_RATE = 590
  994.  
  995. #-------------------------------------------------------------------------------------------
  996. # Set the BCM outputs assigned to LED and sensor interrupt
  997. #-------------------------------------------------------------------------------------------
  998. RPIO_STATUS_SOUNDER = 27
  999. RPIO_SENSOR_DATA_RDY = 25
  1000.  
  1001. silent_scan_count = 0
  1002.  
  1003. #-------------------------------------------------------------------------------------------
  1004. # Set up the base logging
  1005. #-------------------------------------------------------------------------------------------
  1006. logger = logging.getLogger('QC logger')
  1007. logger.setLevel(logging.INFO)
  1008.  
  1009. #-------------------------------------------------------------------------------------------
  1010. # Create file and console logger handlers
  1011. #-------------------------------------------------------------------------------------------
  1012. file_handler = logging.FileHandler("/dev/shm/qclogs", 'w')
  1013. file_handler.setLevel(logging.WARNING)
  1014.  
  1015. console_handler = logging.StreamHandler()
  1016. console_handler.setLevel(logging.CRITICAL)
  1017.  
  1018. #-------------------------------------------------------------------------------------------
  1019. # Create a formatter and add it to both handlers
  1020. #-------------------------------------------------------------------------------------------
  1021. console_formatter = logging.Formatter('%(message)s')
  1022. console_handler.setFormatter(console_formatter)
  1023.  
  1024. file_formatter = logging.Formatter('[%(levelname)s] (%(threadName)-10s) %(funcName)s %(lineno)d %(message)s')
  1025. file_handler.setFormatter(file_formatter)
  1026.  
  1027. #-------------------------------------------------------------------------------------------
  1028. # Add both handlers to the logger
  1029. #-------------------------------------------------------------------------------------------
  1030. logger.addHandler(console_handler)
  1031. logger.addHandler(file_handler)
  1032.  
  1033. #-------------------------------------------------------------------------------------------
  1034. # Check the command line to see if we are calibrating or flying - if neither are set, CheckCLI sys.exit(0)s
  1035. #-------------------------------------------------------------------------------------------
  1036. calibrate_gravity, flying, takeoff_speed, shoot_video, vsp_gain, vsi_gain, vsd_gain, hsp_gain, hsi_gain, hsd_gain = CheckCLI(sys.argv[1:])
  1037. logger.critical("calibrate_gravity = %s, fly = %s, takeoff_speed = %d, shoot_video = %s, vsp_gain = %f, vsi_gain = %f, vsd_gain= %f, hsp_gain = %f, hsi_gain = %f, hsd_gain = %f", calibrate_gravity, flying, takeoff_speed, shoot_video, vsp_gain, vsi_gain, vsd_gain, hsp_gain, hsi_gain, hsd_gain)
  1038.  
  1039. #-------------------------------------------------------------------------------------------
  1040. # Initialize the gyroscope / accelerometer I2C object
  1041. #-------------------------------------------------------------------------------------------
  1042. mpu6050 = MPU6050(0x68)
  1043.  
  1044. #-------------------------------------------------------------------------------------------
  1045. # Calibrate to accelometer for exact gravity
  1046. #-------------------------------------------------------------------------------------------
  1047. if calibrate_gravity:
  1048. if not mpu6050.calibrateGravity('./qcgravity.cfg'):
  1049. print 'Gravity normalization error'
  1050. sys.exit(1)
  1051. sys.exit(0)
  1052.  
  1053. else:
  1054. if not mpu6050.readGravity('./qcgravity.cfg'):
  1055. print 'Gravity config error'
  1056. print '- try running qcpi -c on a flat, horizontal surface first'
  1057. sys.exit(1)
  1058.  
  1059.  
  1060. #-------------------------------------------------------------------------------------------
  1061. # From hereonin we're in flight mode. First task is to shut the ESCs up.
  1062. #-------------------------------------------------------------------------------------------
  1063. #-------------------------------------------------------------------------------------------
  1064. # Assign motor properties to each ESC
  1065. #-------------------------------------------------------------------------------------------
  1066. pin_list = [ESC_BCM_FL, ESC_BCM_FR, ESC_BCM_BL, ESC_BCM_BR]
  1067. location_list = [MOTOR_LOCATION_FRONT | MOTOR_LOCATION_LEFT, MOTOR_LOCATION_FRONT | MOTOR_LOCATION_RIGHT, MOTOR_LOCATION_BACK | MOTOR_LOCATION_LEFT, MOTOR_LOCATION_BACK | MOTOR_LOCATION_RIGHT]
  1068. rotation_list = [MOTOR_ROTATION_ACW, MOTOR_ROTATION_CW, MOTOR_ROTATION_CW, MOTOR_ROTATION_ACW]
  1069. name_list = ['front left', 'front right', 'back left', 'back right']
  1070.  
  1071. #-------------------------------------------------------------------------------------------
  1072. # Prime the ESCs with the default 0 spin rotors
  1073. #-------------------------------------------------------------------------------------------
  1074. esc_list = []
  1075. for esc_index in range(0, 4):
  1076. esc = ESC(pin_list[esc_index], location_list[esc_index], rotation_list[esc_index], name_list[esc_index])
  1077. esc_list.append(esc)
  1078.  
  1079. #-------------------------------------------------------------------------------------------
  1080. # Now with some peace and quiet, enable RPIO for beeper and MPU 6050 interrupts
  1081. #-------------------------------------------------------------------------------------------
  1082. RpioSetup()
  1083.  
  1084. #-------------------------------------------------------------------------------------------
  1085. # Countdown: 5 beeps prior to gyro calibration
  1086. #-------------------------------------------------------------------------------------------
  1087. CountdownBeep(5)
  1088.  
  1089. #-------------------------------------------------------------------------------------------
  1090. # Calibrate the gyros
  1091. #-------------------------------------------------------------------------------------------
  1092. mpu6050.calibrateGyros()
  1093.  
  1094. #-------------------------------------------------------------------------------------------
  1095. # Countdown: 4 beeps prior to waiting for RC connection
  1096. #-------------------------------------------------------------------------------------------
  1097. CountdownBeep(4)
  1098.  
  1099. #-------------------------------------------------------------------------------------------
  1100. # Wait pending a sockets connection with the RC if required
  1101. #-------------------------------------------------------------------------------------------
  1102. inputs = []
  1103. outputs = []
  1104. if use_sockets:
  1105. try:
  1106. serversock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
  1107. except socket.error as msg:
  1108. serversock = None
  1109.  
  1110. try:
  1111. # serversock.setblocking(False) # <=====???????????
  1112. serversock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
  1113. serversock.bind((socket.gethostname(), 12345))
  1114. logger.info('Listening as host %s', socket.gethostname())
  1115. serversock.listen(NUM_SOCK)
  1116. except socket.error as msg:
  1117. serversock.close()
  1118. serversock = None
  1119.  
  1120. if serversock is None:
  1121. sys.exit(1)
  1122.  
  1123. #-----------------------------------------------------------------------------------
  1124. # Wait to accept a connection from the QC RC
  1125. #-----------------------------------------------------------------------------------
  1126. drc_socket, drc_addr = serversock.accept()
  1127. drc_socket.setblocking(False)
  1128. inputs = [drc_socket]
  1129. logger.info('Connected to %s', drc_socket.getpeername())
  1130.  
  1131. #-------------------------------------------------------------------------------------------
  1132. # Now we have a connection, power up the TCP data TLV parsing engine
  1133. #-------------------------------------------------------------------------------------------
  1134. tlvstream = TLVSTREAM()
  1135.  
  1136. #-------------------------------------------------------------------------------------------
  1137. # Countdown: 3 beeps for successful RC connection
  1138. #-------------------------------------------------------------------------------------------
  1139. CountdownBeep(3)
  1140.  
  1141. #---------------------------------------------------------------------------
  1142. # Set the signal handler here so the spin can be cancelled when loop_count = 0
  1143. # or moved on when > 0. Prior to this point Ctrl-C does what you'd expect
  1144. #---------------------------------------------------------------------------
  1145. loop_count = 0
  1146. signal.signal(signal.SIGINT, SignalHandler)
  1147.  
  1148. #-------------------------------------------------------------------------------------------
  1149. # Countdown: Start the video
  1150. #-------------------------------------------------------------------------------------------
  1151. CountdownBeep(2)
  1152.  
  1153. #-------------------------------------------------------------------------------------------
  1154. # Start up the video camera if required - this runs from take-off through to shutdown automatically.
  1155. # Run it in its own process group so that Ctrl-C for QC doesn't get through and stop the video
  1156. #-------------------------------------------------------------------------------------------
  1157. def Daemonize():
  1158. os.setpgrp()
  1159.  
  1160. if shoot_video:
  1161. now = datetime.now()
  1162. now_string = now.strftime("%y%m%d-%H:%M:%S")
  1163. video = subprocess.Popen(["raspivid", "-rot", "180", "-w", "1280", "-h", "720", "-o", "/home/pi/Videos/qcvid_" + now_string + ".h264", "-n", "-t", "0", "-fps", "30", "-b", "5000000"], preexec_fn = Daemonize)
  1164.  
  1165. #-------------------------------------------------------------------------------------------
  1166. # Countdown: Get those blades spinning
  1167. #-------------------------------------------------------------------------------------------
  1168. CountdownBeep(1)
  1169.  
  1170. #------------------------------------------------------------------------------------------
  1171. # Set up the bits of state setup before takeoff
  1172. #-------------------------------------------------------------------------------------------
  1173. keep_looping = True
  1174. delta_time = 0.0
  1175.  
  1176. i_pitch = 0.0
  1177. i_roll = 0.0
  1178. i_yaw = 0.0
  1179.  
  1180. prev_c_pitch = 0.0
  1181. prev_c_roll = 0.0
  1182.  
  1183. eax_speed = 0.0
  1184. eay_speed = 0.0
  1185. eaz_speed = 0.0
  1186.  
  1187. eax_speed_target = 0.0
  1188. eay_speed_target = 0.0
  1189. eaz_speed_target = 0.0
  1190.  
  1191. yaw_angle_target = 0.0
  1192.  
  1193. INPUT_NONE = 0
  1194. INPUT_TAKEOFF = 1
  1195. INPUT_HOVER = 2
  1196. INPUT_LAND = 3
  1197. INPUT_STOP = 4
  1198. INPUT_SIGNAL = 4
  1199. fsm_input = INPUT_NONE
  1200.  
  1201. STATE_OFF = 0
  1202. STATE_ASCENDING = 1
  1203. STATE_HOVERING = 2
  1204. STATE_DESCENDING = 3
  1205. fsm_state = STATE_OFF
  1206.  
  1207.  
  1208. #-------------------------------------------------------------------------------------------
  1209. # Bring the ESCs up to just under takeoff speed
  1210. #-------------------------------------------------------------------------------------------
  1211. for beep_count in range(0, TAKEOFF_READY_RATE, 10):
  1212. for esc in esc_list:
  1213. #---------------------------------------------------------------------------
  1214. # Spin up to just under take-off / hover speeds
  1215. #---------------------------------------------------------------------------
  1216. esc.update(beep_count);
  1217.  
  1218. RPIO.output(RPIO_STATUS_SOUNDER, not RPIO.input(RPIO_STATUS_SOUNDER))
  1219. time.sleep(0.01)
  1220.  
  1221. #-------------------------------------------------------------------------------------------
  1222. # Tuning: Set up the PID gains - some are hard coded, some come from the CLI parameters
  1223. #-------------------------------------------------------------------------------------------
  1224.  
  1225. #-------------------------------------------------------------------------------------------
  1226. # The earth X axis speed controls forward / backward speed
  1227. #-------------------------------------------------------------------------------------------
  1228. PID_EAX_SPEED_P_GAIN = hsp_gain
  1229. PID_EAX_SPEED_I_GAIN = hsi_gain
  1230. PID_EAX_SPEED_D_GAIN = hsd_gain
  1231.  
  1232. #-------------------------------------------------------------------------------------------
  1233. # The earth Y axis speed controls left / right speed
  1234. #-------------------------------------------------------------------------------------------
  1235. PID_EAY_SPEED_P_GAIN = hsp_gain
  1236. PID_EAY_SPEED_I_GAIN = hsi_gain
  1237. PID_EAY_SPEED_D_GAIN = hsd_gain
  1238.  
  1239. #-------------------------------------------------------------------------------------------
  1240. # The earth Z axis speed controls rise / fall speed
  1241. #-------------------------------------------------------------------------------------------
  1242. PID_EAZ_SPEED_P_GAIN = vsp_gain
  1243. PID_EAZ_SPEED_I_GAIN = vsi_gain
  1244. PID_EAZ_SPEED_D_GAIN = vsd_gain
  1245.  
  1246. #-------------------------------------------------------------------------------------------
  1247. # The PITCH ANGLE PID controls stable rotation speed about the Y-axis
  1248. #-------------------------------------------------------------------------------------------
  1249. PID_PITCH_ANGLE_P_GAIN = 2.5
  1250. PID_PITCH_ANGLE_I_GAIN = 0.0
  1251. PID_PITCH_ANGLE_D_GAIN = 0.1
  1252.  
  1253. #-------------------------------------------------------------------------------------------
  1254. # The ROLL ANGLE PID controls stable rotation speed about the X-axis
  1255. #-------------------------------------------------------------------------------------------
  1256. PID_ROLL_ANGLE_P_GAIN = 2.5
  1257. PID_ROLL_ANGLE_I_GAIN = 0.0
  1258. PID_ROLL_ANGLE_D_GAIN = 0.1
  1259.  
  1260. #-------------------------------------------------------------------------------------------
  1261. # The YAW ANGLE PID controls stable rotation speed about the Z-axis
  1262. #-------------------------------------------------------------------------------------------
  1263. PID_YAW_ANGLE_P_GAIN = 2.5
  1264. PID_YAW_ANGLE_I_GAIN = 5.0
  1265. PID_YAW_ANGLE_D_GAIN = 0.1
  1266.  
  1267. #-------------------------------------------------------------------------------------------
  1268. # The PITCH RATE PID controls stable rotation speed about the Y-axis
  1269. #-------------------------------------------------------------------------------------------
  1270. PID_PITCH_RATE_P_GAIN = 150 # Was 2.5 when using degrees rather than radians
  1271. PID_PITCH_RATE_I_GAIN = 300 # Was 5.0 when using degrees rather than radians
  1272. PID_PITCH_RATE_D_GAIN = 5.75 # Was 0.1 when using degrees rather than radians
  1273.  
  1274. #-------------------------------------------------------------------------------------------
  1275. # The ROLL RATE PID controls stable rotation speed about the X-axis
  1276. #-------------------------------------------------------------------------------------------
  1277. PID_ROLL_RATE_P_GAIN = 150 # Was 2.5 when using degrees rather than radians
  1278. PID_ROLL_RATE_I_GAIN = 300 # Was 5.0 when using degrees rather than radians
  1279. PID_ROLL_RATE_D_GAIN = 5.75 # Was 0.1 when using degrees rather than radians
  1280.  
  1281. #-------------------------------------------------------------------------------------------
  1282. # The YAW RATE PID controls stable rotation speed about the Z-axis
  1283. #-------------------------------------------------------------------------------------------
  1284. PID_YAW_RATE_P_GAIN = 150 # Was 2.5 when using degrees rather than radians
  1285. PID_YAW_RATE_I_GAIN = 300 # Was 5.0 when using degrees rather than radians
  1286. PID_YAW_RATE_D_GAIN = 5.75 # Was 0.1 when using degrees rather than radians
  1287.  
  1288. #-------------------------------------------------------------------------------------------
  1289. # Enable time dependent factors PIDs - everything beyond here and "while keep_looping:" is time
  1290. # critical and should be kept to an absolute minimum.
  1291. #-------------------------------------------------------------------------------------------
  1292. pitch_angle_pid = PID(PID_PITCH_ANGLE_P_GAIN, PID_PITCH_ANGLE_I_GAIN, PID_PITCH_ANGLE_D_GAIN)
  1293. roll_angle_pid = PID(PID_ROLL_ANGLE_P_GAIN, PID_ROLL_ANGLE_I_GAIN, PID_ROLL_ANGLE_D_GAIN)
  1294. yaw_angle_pid = PID(PID_YAW_ANGLE_P_GAIN, PID_YAW_ANGLE_I_GAIN, PID_YAW_ANGLE_D_GAIN)
  1295.  
  1296. pitch_rate_pid = PID(PID_PITCH_RATE_P_GAIN, PID_PITCH_RATE_I_GAIN, PID_PITCH_RATE_D_GAIN)
  1297. roll_rate_pid = PID(PID_ROLL_RATE_P_GAIN, PID_ROLL_RATE_I_GAIN, PID_ROLL_RATE_D_GAIN)
  1298. yaw_rate_pid = PID(PID_YAW_RATE_P_GAIN, PID_YAW_RATE_I_GAIN, PID_YAW_RATE_D_GAIN)
  1299.  
  1300. eax_speed_pid = PID(PID_EAX_SPEED_P_GAIN, PID_EAX_SPEED_I_GAIN, PID_EAX_SPEED_D_GAIN)
  1301. eay_speed_pid = PID(PID_EAY_SPEED_P_GAIN, PID_EAY_SPEED_I_GAIN, PID_EAY_SPEED_D_GAIN)
  1302. eaz_speed_pid = PID(PID_EAZ_SPEED_P_GAIN, PID_EAZ_SPEED_I_GAIN, PID_EAZ_SPEED_D_GAIN)
  1303.  
  1304. #-------------------------------------------------------------------------------------------
  1305. # Diagnostic statistics log header
  1306. #-------------------------------------------------------------------------------------------
  1307. logger.warning(', Time, DT, Loop, fgx, fgy, fgz, fax, fay, faz, c pitch, c roll, i yaw, e tilt, evz, prp, pri, prd, pap, pai, pad, rrp, rri, rrd, rap, rai, rad, yrp, yri, yrd, yap, yai, yap, exp, exi, exd, eyp, eyi, eyd, ezp, ezi, ezd, pitch target, roll target, yaw target, pitch out, roll_out, yaw out, vert speed out, FL spin, FR spin, BL spin, BR spin')
  1308.  
  1309. time_handling_fsm = 0.0
  1310. time_handling_sensors = 0.0
  1311. time_handling_eangles = 0.0
  1312. time_handling_iangles = 0.0
  1313. time_handling_angles_filter = 0.0
  1314. time_handling_axes_shift = 0.0
  1315. time_handling_speed_pids = 0.0
  1316. time_handling_angle_pids = 0.0
  1317. time_handling_pid_outputs = 0.0
  1318. time_handling_diagnostics = 0.0
  1319.  
  1320. elapsed_time = 0.0
  1321. start_time = time.time()
  1322. last_log_time = start_time
  1323. current_time = start_time
  1324. prev_sample_time = current_time
  1325.  
  1326. while keep_looping:
  1327. #-----------------------------------------------------------------------------------
  1328. # Update the elapsed time since start, the time for the last iteration, and
  1329. # set the next sleep time to compensate for any overrun in scheduling.
  1330. #-----------------------------------------------------------------------------------
  1331. current_time = time.time()
  1332. delta_time = current_time - start_time - elapsed_time
  1333. elapsed_time = current_time - start_time
  1334. loop_count += 1
  1335.  
  1336. #===================================================================================
  1337. # Interpreter: FSM inputs are mostly generated on a timer for testing; the exceptions are
  1338. # - SIGNAL generated by a Ctrl-C. These produce the targets for the PIDs. In this
  1339. # case, only the vertical speed target is modified - the horizontal X and Y speed targets
  1340. # are configured higher up to be 0.0 to create a stable hover regardless of take-off conditions
  1341. # of any external factors such as wind or weight balance.
  1342. #===================================================================================
  1343. if fsm_input != INPUT_SIGNAL:
  1344. if elapsed_time >= 0.0:
  1345. fsm_input = INPUT_TAKEOFF
  1346.  
  1347. if elapsed_time >= 3.0:
  1348. fsm_input = INPUT_LAND
  1349. # fsm_input = INPUT_HOVER
  1350.  
  1351. # if elapsed_time >= 6.0:
  1352. # fsm_input = INPUT_LAND
  1353.  
  1354. if elapsed_time >= 8.0:
  1355. fsm_input = INPUT_STOP
  1356.  
  1357. if fsm_state == STATE_OFF and fsm_input == INPUT_TAKEOFF:
  1358. logger.critical('#AB: ASCENDING')
  1359. fsm_state = STATE_ASCENDING
  1360. fsm_input = INPUT_NONE
  1361. RPIO.output(RPIO_STATUS_SOUNDER, RPIO.HIGH)
  1362.  
  1363. #---------------------AUTONOMOUS VERTICAL TAKE-OFF SPEED--------------------
  1364. eaz_speed_target = 0.5
  1365. #---------------------AUTONOMOUS VERTICAL TAKE-OFF SPEED--------------------
  1366.  
  1367.  
  1368.  
  1369. # elif fsm_state == STATE_ASCENDING and (fsm_input == INPUT_HOVER or fsm_input == INPUT_SIGNAL):
  1370. # logger.critical('#AB: HOVERING')
  1371. # fsm_state = STATE_HOVERING
  1372. # fsm_input = INPUT_NONE
  1373. # RPIO.output(RPIO_STATUS_SOUNDER, RPIO.LOW)
  1374. #
  1375. # #-----------------------AUTONOMOUS VERTICAL HOVER SPEED---------------------
  1376. # eaz_speed_target = 0.0
  1377. # #-----------------------AUTONOMOUS VERTICAL HOVER SPEED---------------------
  1378.  
  1379.  
  1380.  
  1381. elif fsm_state == STATE_ASCENDING and (fsm_input == INPUT_LAND or fsm_input == INPUT_SIGNAL):
  1382. # elif fsm_state == STATE_HOVERING and (fsm_input == INPUT_LAND or fsm_input == INPUT_SIGNAL):
  1383. logger.critical('#AB: DESCENDING')
  1384. fsm_state = STATE_DESCENDING
  1385. fsm_input = INPUT_NONE
  1386. RPIO.output(RPIO_STATUS_SOUNDER, RPIO.HIGH)
  1387.  
  1388. #----------------------AUTONOMOUS VERTICAL LANDING SPEED--------------------
  1389. eaz_speed_target = -0.35
  1390. #----------------------AUTONOMOUS VERTICAL LANDING SPEED--------------------
  1391.  
  1392.  
  1393.  
  1394. elif fsm_state == STATE_DESCENDING and (fsm_input == INPUT_STOP or fsm_input == INPUT_SIGNAL):
  1395. logger.critical('#AB: LANDED')
  1396. fsm_state = STATE_OFF
  1397. fsm_input = INPUT_NONE
  1398. keep_looping = False
  1399. takeoff_speed = 0
  1400. RPIO.output(RPIO_STATUS_SOUNDER, RPIO.LOW)
  1401.  
  1402. #---------------------AUTONOMOUS VERTICAL PIN-DOWN SPEED--------------------
  1403. eaz_speed_target = -0.10
  1404. #---------------------AUTONOMOUS VERTICAL PIN_DOWN SPEED--------------------
  1405.  
  1406. #-----------------------------------------------------------------------------------
  1407. # Track proportion of time handling FSM
  1408. #-----------------------------------------------------------------------------------
  1409. sample_time = time.time()
  1410. time_handling_fsm += sample_time - prev_sample_time
  1411. prev_sample_time = sample_time
  1412.  
  1413. # #---------------------------------------------------------------------------
  1414. # # Make sure the TLV stream is empty from the last run before extracting more RC command data
  1415. # #---------------------------------------------------------------------------
  1416. # if use_sockets:
  1417. #
  1418. # #---------------------------------------------------------------------------
  1419. # # Select on waiting for a command, or a hello
  1420. # #---------------------------------------------------------------------------
  1421. # readable, writeable, exceptional = select.select(inputs, outputs, inputs, sleep_time)
  1422. #
  1423. # #-----------------------------------------------------------------------------------
  1424. # # HELLO timeout - check for receipt and send
  1425. # #-----------------------------------------------------------------------------------
  1426. # if not (readable or writeable or exceptional):
  1427. #
  1428. # #---------------------------------------------------------------------------
  1429. # # The timer's popped, which means we've received nothing in the last KEEPALIVE_TIMER seconds.
  1430. # # For safety's sake, commit suicide.
  1431. # #---------------------------------------------------------------------------
  1432. # silent_scan_count += 1
  1433. # if silent_scan_count == RC_SILENCE_LIMIT:
  1434. # #-----------------------------------------------------------
  1435. # # We've not receive a message from RC for 10 scans, close the socket and
  1436. # # enforce an automatic landing
  1437. # #-----------------------------------------------------------
  1438. # logger.error('No message from RC for 10 scans')
  1439. # drc_sck.shutdown(socket.SHUT_RDWR)
  1440. # drc_sck.close()
  1441. #
  1442. # eax_speed_target = 0.0
  1443. # eay_speed_target = 0.0
  1444. # eaz_speed_target = 0.95
  1445. #
  1446. # use_sockets = False
  1447. #
  1448. # break
  1449. #
  1450. # #-----------------------------------------------------------------------------------
  1451. # # Now check whether we have errors on anything
  1452. # #-----------------------------------------------------------------------------------
  1453. # for drc_sck in exceptional:
  1454. #
  1455. # #---------------------------------------------------------------------------
  1456. # # Don't care about the details, set auto-landing
  1457. # #---------------------------------------------------------------------------
  1458. # logger.error('Select socket error')
  1459. # drc_sck.shutdown(socket.SHUT_RDWR)
  1460. # drc_sck.close()
  1461. #
  1462. # eax_speed_target = 0.0
  1463. # eay_speed_target = 0.0
  1464. # eaz_speed_target = 0.95
  1465. #
  1466. # use_sockets = False
  1467. #
  1468. # break
  1469. #
  1470. # #-----------------------------------------------------------------------------------
  1471. # # Now check whether we have received anything
  1472. # #-----------------------------------------------------------------------------------
  1473. # for drc_sck in readable:
  1474. #
  1475. # #---------------------------------------------------------------------------
  1476. # # Check to see what we've received
  1477. # #---------------------------------------------------------------------------
  1478. # drc_data = drc_sck.recv(4096)
  1479. # if not drc_data:
  1480. # #-------------------------------------------------------------------
  1481. # # Client shutdown processing
  1482. # #-------------------------------------------------------------------
  1483. # logger.error('0 data received')
  1484. # drc_sck.shutdown(socket.SHUT_RDWR)
  1485. # drc_sck.close()
  1486. #
  1487. # eax_speed_target = 0.0
  1488. # eay_speed_target = 0.0
  1489. # eaz_speed_target = 0.95
  1490. #
  1491. # use_sockets = False
  1492. #
  1493. # break
  1494. #
  1495. # #-------------------------------------------------------------------
  1496. # # Parse the control message
  1497. # #-------------------------------------------------------------------
  1498. # eax_speed_target, eay_speed_target, eaz_speed_target = tlvstream.Parse(drc_data)
  1499. #
  1500. # #-------------------------------------------------------------------
  1501. # # Cycle through each PID applying the appropriate new targets
  1502. # #-------------------------------------------------------------------
  1503. # if eax_speed_target == 0 and eay_speed_target == 0 and eaz_speed_target == 0:
  1504. # logger.warning('Nothing parsed!')
  1505. # silent_scan_count += 1
  1506. # if silent_scan_count == RC_SILENCE_LIMIT:
  1507. # #-----------------------------------------------------------
  1508. # # We've not receive a message from RC for 10 scans, close the socket and
  1509. # # enforce an automatic landing
  1510. # #-----------------------------------------------------------
  1511. # logger.error('No message from RC for 10 scans')
  1512. # drc_sck.shutdown(socket.SHUT_RDWR)
  1513. # drc_sck.close()
  1514. #
  1515. # eax_speed_target = 0.0
  1516. # eay_speed_target = 0.0
  1517. # eaz_speed_target = 0.95
  1518. #
  1519. # use_sockets = False
  1520. # break
  1521. # else:
  1522. # silent_scan_count = 0
  1523. # else:
  1524. # #-----------------------------------------------------------------------------------
  1525. # # Now check whether we have received anything
  1526. # #-----------------------------------------------------------------------------------
  1527. # time.sleep(sleep_time)
  1528. #
  1529. # #-----------------------------------------------------------------------------------
  1530. # # Simulate acclerometer targets for testing purposes - HOVER
  1531. # #-----------------------------------------------------------------------------------
  1532. # if silent_scan_count >= RC_SILENCE_LIMIT:
  1533. # eax_speed_target = 0.0
  1534. # eay_speed_target = 0.0
  1535. # eaz_speed_target = 0.95
  1536.  
  1537.  
  1538. #===================================================================================
  1539. # Inputs: Read the data from the accelerometer and gyro
  1540. #===================================================================================
  1541. [fax, fay, faz, fgx, fgy, fgz] = mpu6050.readSensors()
  1542.  
  1543. #-----------------------------------------------------------------------------------
  1544. # Track proportion of time handling sensors
  1545. #-----------------------------------------------------------------------------------
  1546. sample_time = time.time()
  1547. time_handling_sensors += sample_time - prev_sample_time
  1548. prev_sample_time = sample_time
  1549.  
  1550. #===================================================================================
  1551. # Angles: Get the Euler angles in radians
  1552. #===================================================================================
  1553. e_pitch, e_roll, e_tilt = mpu6050.getEulerAngles(fax, fay, faz)
  1554.  
  1555. #-----------------------------------------------------------------------------------
  1556. # Track proportion of time handling euler angles
  1557. #-----------------------------------------------------------------------------------
  1558. sample_time = time.time()
  1559. time_handling_eangles += sample_time - prev_sample_time
  1560. prev_sample_time = sample_time
  1561.  
  1562. #-----------------------------------------------------------------------------------
  1563. # Integrate the gyros angular velocity to determine absolute angle of tilt in radians
  1564. #-----------------------------------------------------------------------------------
  1565. i_pitch += fgy * delta_time
  1566. i_roll += fgx * delta_time
  1567. i_yaw += fgz * delta_time
  1568.  
  1569. #-----------------------------------------------------------------------------------
  1570. # Track proportion of time handling integrated angles
  1571. #-----------------------------------------------------------------------------------
  1572. sample_time = time.time()
  1573. time_handling_iangles += sample_time - prev_sample_time
  1574. prev_sample_time = sample_time
  1575.  
  1576. #===================================================================================
  1577. # Filter: Apply complementary filter to ensure long-term accuracy of pitch / roll angles
  1578. # tau is the handover period of 0.1s (1 / frequency) that the integrated gyro high pass
  1579. # filter is taken over by the accelerometer Euler low-pass filter. The combination of
  1580. # tau plus the time increment (delta_time) then provides a fraction to mix the two angles sources.
  1581. #===================================================================================
  1582. tau = 0.1
  1583. tau_fraction = tau / (tau + delta_time)
  1584.  
  1585. c_pitch = tau_fraction * (prev_c_pitch + fgy * delta_time) + (1 - tau_fraction) * e_pitch
  1586. prev_c_pitch = c_pitch
  1587.  
  1588. c_roll = tau_fraction * (prev_c_roll + fgx * delta_time) + (1 - tau_fraction) * e_roll
  1589. prev_c_roll = c_roll
  1590.  
  1591. #-----------------------------------------------------------------------------------
  1592. # Choose the best measure of the angles
  1593. #-----------------------------------------------------------------------------------
  1594. pitch_angle = c_pitch
  1595. roll_angle = c_roll
  1596. yaw_angle = i_yaw
  1597.  
  1598. #-----------------------------------------------------------------------------------
  1599. # Track proportion of time handling angle filter
  1600. #-----------------------------------------------------------------------------------
  1601. sample_time = time.time()
  1602. time_handling_angles_filter += sample_time - prev_sample_time
  1603. prev_sample_time = sample_time
  1604.  
  1605. #===================================================================================
  1606. # Axes: Convert the accelerometers' g force to earth coordinates, then integrate to
  1607. # convert to speeds in earth's X and Y axes
  1608. #===================================================================================
  1609. eax = fax * math.cos(pitch_angle)
  1610. eay = fay * math.cos(roll_angle)
  1611. eaz = faz * math.cos(pitch_angle) * math.cos(roll_angle)
  1612.  
  1613. eax_speed += eax * delta_time * G_FORCE
  1614. eay_speed += eay * delta_time * G_FORCE
  1615. eaz_speed += eaz * delta_time * G_FORCE
  1616.  
  1617. #-----------------------------------------------------------------------------------
  1618. # Track proportion of time handling sensor angles
  1619. #-----------------------------------------------------------------------------------
  1620. sample_time = time.time()
  1621. time_handling_axes_shift += sample_time - prev_sample_time
  1622. prev_sample_time = sample_time
  1623.  
  1624. #===================================================================================
  1625. # PIDs: Run the horizontal speed PIDs each rotation axis to determine targets for angle PID
  1626. # and the verical speed PID to control height.
  1627. #===================================================================================
  1628. [p_out, i_out, d_out] = eax_speed_pid.Compute(eax_speed, eax_speed_target)
  1629. eax_diags = "%f, %f, %f" % (p_out, i_out, d_out)
  1630. pitch_angle_target = + p_out + i_out + d_out
  1631.  
  1632. [p_out, i_out, d_out] = eay_speed_pid.Compute(eay_speed, eay_speed_target)
  1633. eay_diags = "%f, %f, %f" % (p_out, i_out, d_out)
  1634. roll_angle_target = - p_out - i_out - d_out
  1635.  
  1636. [p_out, i_out, d_out] = eaz_speed_pid.Compute(eaz_speed, eaz_speed_target)
  1637. eaz_diags = "%f, %f, %f" % (p_out, i_out, d_out)
  1638. eaz_speed_out = p_out + i_out + d_out
  1639.  
  1640. #-----------------------------------------------------------------------------------
  1641. # Track proportion of time handling speed PIDs
  1642. #-----------------------------------------------------------------------------------
  1643. sample_time = time.time()
  1644. time_handling_speed_pids += sample_time - prev_sample_time
  1645. prev_sample_time = sample_time
  1646.  
  1647. #-----------------------------------------------------------------------------------
  1648. # Run the absolute angle PIDs each rotation axis.
  1649. #-----------------------------------------------------------------------------------
  1650. [p_out, i_out, d_out] = pitch_angle_pid.Compute(pitch_angle, pitch_angle_target)
  1651. pa_diags = "%f, %f, %f" % (p_out, i_out, d_out)
  1652. pitch_rate_target = p_out + i_out + d_out
  1653. [p_out, i_out, d_out] = roll_angle_pid.Compute(roll_angle, roll_angle_target)
  1654. ra_diags = "%f, %f, %f" % (p_out, i_out, d_out)
  1655. roll_rate_target = p_out + i_out + d_out
  1656. [p_out, i_out, d_out] = yaw_angle_pid.Compute(yaw_angle, yaw_angle_target)
  1657. ya_diags = "%f, %f, %f" % (p_out, i_out, d_out)
  1658. yaw_rate_target = p_out + i_out + d_out
  1659.  
  1660. #-----------------------------------------------------------------------------------
  1661. # Run the angular rate PIDs each rotation axis.
  1662. #-----------------------------------------------------------------------------------
  1663. [p_out, i_out, d_out] = pitch_rate_pid.Compute(fgy, pitch_rate_target)
  1664. pr_diags = "%f, %f, %f" % (p_out, i_out, d_out)
  1665. pitch_out = p_out + i_out + d_out
  1666. [p_out, i_out, d_out] = roll_rate_pid.Compute(fgx, roll_rate_target)
  1667. rr_diags = "%f, %f, %f" % (p_out, i_out, d_out)
  1668. roll_out = p_out + i_out + d_out
  1669. [p_out, i_out, d_out] = yaw_rate_pid.Compute(fgz, yaw_rate_target)
  1670. yr_diags = "%f, %f, %f" % (p_out, i_out, d_out)
  1671. yaw_out = p_out + i_out + d_out
  1672.  
  1673. #-----------------------------------------------------------------------------------
  1674. # Track proportion of time handling angle PIDs
  1675. #-----------------------------------------------------------------------------------
  1676. sample_time = time.time()
  1677. time_handling_angle_pids += sample_time - prev_sample_time
  1678. prev_sample_time = sample_time
  1679.  
  1680. #===================================================================================
  1681. # Mixer: Walk through the ESCs, and depending on their location, apply the output accordingly
  1682. #===================================================================================
  1683. pitch_out = int(round(pitch_out / 2))
  1684. roll_out = int(round(roll_out / 2))
  1685. yaw_out = int(round(yaw_out / 2))
  1686. vert_out = takeoff_speed + eaz_speed_out
  1687.  
  1688. for esc in esc_list:
  1689. #---------------------------------------------------------------------------
  1690. # Update all blades' power in accordance with the z error
  1691. #---------------------------------------------------------------------------
  1692. delta_spin = vert_out
  1693.  
  1694. #---------------------------------------------------------------------------
  1695. # For a left downwards roll, the x gyro goes negative, so the PID error is positive,
  1696. # meaning PID output is positive, meaning this needs to be added to the left blades
  1697. # and subtracted from the right.
  1698. #---------------------------------------------------------------------------
  1699. if esc.motor_location & MOTOR_LOCATION_RIGHT:
  1700. delta_spin -= roll_out
  1701. else:
  1702. delta_spin += roll_out
  1703.  
  1704. #---------------------------------------------------------------------------
  1705. # For a forward downwards pitch, the y gyro goes positive, so the PID error is
  1706. # negative, meaning PID output is negative, meaning this needs to be subtracted
  1707. # from the front blades and added to the back.
  1708. #---------------------------------------------------------------------------
  1709. if esc.motor_location & MOTOR_LOCATION_BACK:
  1710. delta_spin += pitch_out
  1711. else:
  1712. delta_spin -= pitch_out
  1713.  
  1714. #---------------------------------------------------------------------------
  1715. # An excess CW rotating of the front-right and back-left (FR & BL) blades
  1716. # results in an ACW rotation of the quadcopter body. The z gyro produces
  1717. # a positive output as a result. This then leads to the PID error
  1718. # (target - gyro) being negative, meaning PID output is negative
  1719. # (assuming positive gains). Since the PID output needs to reduce the
  1720. # over-enthusiastic CW rotation of the FR & BL blades, the negative PID
  1721. # output needs to be added to those blades (thus slowing their rotation)
  1722. # and subtracted from the ACW FL & BR blades (thus speeding them up) to
  1723. # compensate for the yaw.
  1724. #---------------------------------------------------------------------------
  1725. if esc.motor_rotation == MOTOR_ROTATION_CW:
  1726. delta_spin += yaw_out
  1727. else:
  1728. delta_spin -= yaw_out
  1729.  
  1730. #---------------------------------------------------------------------------
  1731. # Apply the blended outputs to the esc PWM signal
  1732. #---------------------------------------------------------------------------
  1733. esc.update(delta_spin)
  1734.  
  1735. #-----------------------------------------------------------------------------------
  1736. # Track proportion of time applying PID outputs
  1737. #-----------------------------------------------------------------------------------
  1738. sample_time = time.time()
  1739. time_handling_pid_outputs += sample_time - prev_sample_time
  1740. prev_sample_time = sample_time
  1741.  
  1742. #-----------------------------------------------------------------------------------
  1743. # Diagnostic statistics log - every 0.1s
  1744. #-----------------------------------------------------------------------------------
  1745. if current_time - last_log_time > 0.1:
  1746. logger.warning(', %f, %f, %d, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %s, %s, %s, %s, %s, %s, %s, %s, %s, %f, %f, %f, %d, %d, %d, %d, %d, %d, %d, %d', elapsed_time, delta_time, loop_count, fgx, fgy, fgz, fax, fay, faz, math.degrees(c_pitch), math.degrees(c_roll), math.degrees(i_yaw), math.degrees(e_tilt), eaz_speed, pr_diags, pa_diags, rr_diags, ra_diags, yr_diags, ya_diags, eax_diags, eay_diags, eaz_diags, pitch_rate_target, roll_rate_target, yaw_rate_target, pitch_out, roll_out, yaw_out, eaz_speed_out, esc_list[0].current_pulse_width, esc_list[1].current_pulse_width, esc_list[2].current_pulse_width, esc_list[3].current_pulse_width)
  1747. last_log_time = current_time
  1748.  
  1749. #-----------------------------------------------------------------------------------
  1750. # Track proportion of time applying PID outputs
  1751. #-----------------------------------------------------------------------------------
  1752. sample_time = time.time()
  1753. time_handling_diagnostics += sample_time - prev_sample_time
  1754. prev_sample_time = sample_time
  1755.  
  1756. #-------------------------------------------------------------------------------------------
  1757. # Stop the blades during shutdown analysis
  1758. #-------------------------------------------------------------------------------------------
  1759. for esc in esc_list:
  1760. esc.update(0)
  1761.  
  1762. #-------------------------------------------------------------------------------------------
  1763. # Dump the loops per second
  1764. #-------------------------------------------------------------------------------------------
  1765. logger.critical("loop speed %f loops per second", loop_count / elapsed_time)
  1766.  
  1767. #-------------------------------------------------------------------------------------------
  1768. # Dump the percentage time handling each step
  1769. #-------------------------------------------------------------------------------------------
  1770. logger.critical("%% fsm: %f", time_handling_fsm / elapsed_time * 100.0)
  1771. logger.critical("%% sensors: %f", time_handling_sensors / elapsed_time * 100.0)
  1772. logger.critical("%% eangles: %f", time_handling_eangles / elapsed_time * 100.0)
  1773. logger.critical("%% iangles: %f", time_handling_iangles / elapsed_time * 100.0)
  1774. logger.critical("%% angles_filter: %f", time_handling_angles_filter / elapsed_time * 100.0)
  1775. logger.critical("%% axes_shift: %f", time_handling_axes_shift / elapsed_time * 100.0)
  1776. logger.critical("%% speed_pids: %f", time_handling_speed_pids / elapsed_time * 100.0)
  1777. logger.critical("%% angle_pids: %f", time_handling_angle_pids / elapsed_time * 100.0)
  1778. logger.critical("%% pid_outputs: %f", time_handling_pid_outputs / elapsed_time * 100.0)
  1779. logger.critical("%% pid_diagnosticss: %f", time_handling_diagnostics / elapsed_time * 100.0)
  1780.  
  1781.  
  1782. #-------------------------------------------------------------------------------------------
  1783. # Time for telly bye byes
  1784. #-------------------------------------------------------------------------------------------
  1785. CleanShutdown()
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!