API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Sep 29th, 2016
58
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 11.74 KB | None | 0 0
raw download clone embed print report
  1. /*------------------------------------------------------------------
  2. * in4073.c -- test QR engines and sensors
  3. *
  4. * reads ae[0-3] uart rx queue
  5. * (q,w,e,r increment, a,s,d,f decrement)
  6. *
  7. * prints timestamp, ae[0-3], sensors to uart tx queue
  8. *
  9. * I. Protonotarios
  10. * Embedded Software Lab
  11. *
  12. * June 2016
  13. *------------------------------------------------------------------
  14. */
  15.  
  16. #include "in4073.h"
  17.  
  18. uint8_t qr_mode = 0;
  19. uint32_t flash_address_p = 0x000000; // Address can be between 0x000000 and 0x01FFFF
  20. /*------------------------------------------------------------------
  21. * process_key -- process command keys
  22. *------------------------------------------------------------------
  23. */
  24. void process_key(uint8_t c)
  25. {
  26. switch (c)
  27. {
  28. case 'q':
  29. ae_keyboard[0] += 10;
  30. break;
  31. case 'a':
  32. ae_keyboard[0] -= 10;
  33. if (ae_keyboard[0] < 0) ae_keyboard[0] = 0;
  34. break;
  35. case 'w':
  36. ae_keyboard[1] += 10;
  37. break;
  38. case 's':
  39. ae_keyboard[1] -= 10;
  40. if (ae_keyboard[1] < 0) ae_keyboard[1] = 0;
  41. break;
  42. case 'e':
  43. ae_keyboard[2] += 10;
  44. break;
  45. case 'd':
  46. ae_keyboard[2] -= 10;
  47. if (ae_keyboard[2] < 0) ae_keyboard[2] = 0;
  48. break;
  49. case 'r':
  50. ae_keyboard[3] += 10;
  51. break;
  52. case 'f':
  53. ae_keyboard[3] -= 10;
  54. if (ae_keyboard[3] < 0) ae_keyboard[3] = 0;
  55. break;
  56. case 27:
  57. demo_done = true;
  58. break;
  59. default:
  60. nrf_gpio_pin_toggle(RED);
  61. }
  62. }
  63.  
  64. /* Alex: processing joystick angles */
  65. void process_joystick() {
  66. if (roll > 0) {
  67. ae_joystick[1] = roll * 10;
  68. } else {
  69. ae_joystick[3] = roll * 10;
  70. }
  71. if (pitch > 0) {
  72. ae_joystick[0] = pitch * 10;
  73. } else {
  74. ae_joystick[2] = pitch * 10;
  75. }
  76. if (yaw > 0) {
  77. ae_joystick[1] = yaw * 10;
  78. ae_joystick[3] = yaw * 10;
  79. } else {
  80. ae_joystick[0] = yaw * 10;
  81. ae_joystick[2] = yaw * 10;
  82. }
  83. }
  84.  
  85. /* Alex: get a single bit from a byte
  86. * Return 0 or 1
  87. */
  88. uint8_t get_bit(uint8_t byte, uint8_t pos) {
  89. // Hurray for unsafe operations!
  90. return (byte & (1 << pos)) > 0 ? 1 : 0;
  91. }
  92.  
  93. /* Alex: read the message buffer based on the receiving packet format
  94. * Package format: preamble, modes, keyboard, throttle, roll, pitch, yaw, CRC
  95. */
  96. void parse_buffer() {
  97. // Ignore preamble
  98. uint8_t mode = get_bit(msg_buf[1], 0);
  99. mode += get_bit(msg_buf[1], 1) * 2;
  100. uint8_t raw_height_mode = get_bit(msg_buf[1], 2);
  101. switch (mode) {
  102. case 0:
  103. if (raw_height_mode == 0)
  104. qr_mode = 1; // panic
  105. else
  106. qr_mode = 2; // calibration
  107. case 1: qr_mode = 3; // manual
  108. case 2: qr_mode = 4; // yaw control
  109. case 3: qr_mode = 5; // full control
  110. }
  111. // Skip 3 0's'
  112. if (get_bit(msg_buf[1], 3) == 1) {
  113. process_key('O');
  114. }
  115. if (get_bit(msg_buf[1], 4) == 1) {
  116. process_key('L');
  117. }
  118. if (get_bit(msg_buf[1], 5) == 1) {
  119. process_key('A');
  120. }
  121. if (get_bit(msg_buf[1], 6) == 1) {
  122. process_key('Z');
  123. }
  124. if (get_bit(msg_buf[1], 7) == 1) {
  125. process_key('Q');
  126. }
  127. if (get_bit(msg_buf[2], 0) == 1) {
  128. process_key('W');
  129. }
  130. if (get_bit(msg_buf[2], 1) == 1) {
  131. process_key('U');
  132. }
  133. if (get_bit(msg_buf[2], 2) == 1) {
  134. process_key('J');
  135. }
  136. if (get_bit(msg_buf[2], 3) == 1) {
  137. process_key('I');
  138. }
  139. if (get_bit(msg_buf[2], 4) == 1) {
  140. process_key('K');
  141. }
  142. throttle = (get_bit(msg_buf[2], 5) << 5) + (get_bit(msg_buf[2], 6) << 4) + (get_bit(msg_buf[2], 7) << 3) +
  143. (get_bit(msg_buf[3], 0) << 2) + (get_bit(msg_buf[3], 1) << 1) + get_bit(msg_buf[3], 2);
  144. roll = -1 * get_bit(msg_buf[3], 3) * (
  145. (get_bit(msg_buf[3], 4) << 5) + (get_bit(msg_buf[3], 5) << 4) + (get_bit(msg_buf[3], 6) << 3) + (get_bit(msg_buf[3], 7) << 2) +
  146. (get_bit(msg_buf[4], 0) << 1) + get_bit(msg_buf[4], 1)
  147. );
  148. pitch = -1 * get_bit(msg_buf[4], 2) * (
  149. (get_bit(msg_buf[4], 3) << 5) + (get_bit(msg_buf[4], 4) << 4) + (get_bit(msg_buf[4], 5) << 3) + (get_bit(msg_buf[4], 6) << 2) +
  150. (get_bit(msg_buf[5], 7) << 1) + get_bit(msg_buf[5], 0)
  151. );
  152. yaw = -1 * get_bit(msg_buf[5], 1) * (
  153. (get_bit(msg_buf[5], 2) << 5) + (get_bit(msg_buf[5], 3) << 4) + (get_bit(msg_buf[5], 4) << 3) + (get_bit(msg_buf[5], 5) << 2) +
  154. (get_bit(msg_buf[5], 6) << 1) + get_bit(msg_buf[5], 7)
  155. );
  156. }
  157.  
  158. /* Alex & Petros: read out queue and call CRC */
  159. void buffer(uint8_t byte)
  160. {
  161. // if (stream_is_tracked) {
  162. // if (bytes_read < RECEIVE_MESSAGE_LENGTH - 1) {
  163. msg_buf[bytes_read] = byte;
  164. // } else {
  165. // received_crc_value = byte;
  166. // }
  167.  
  168. // if (bytes_read == RECEIVE_MESSAGE_LENGTH - 1) {
  169. // crc_value = do_crc(msg_buf); // calculates crc once the buffer is ready
  170. // }
  171. // } else {
  172. // if (byte == 0xFF) {
  173. // stream_is_tracked = true;
  174. // bytes_read = 0;
  175. // }
  176. // }
  177. }
  178.  
  179. void CalulateTable_CRC8()
  180. {
  181. const uint8_t generator = 0x1D;
  182. /* iterate over all byte values 0 - 255 */
  183. for (uint16_t divident = 0; divident < 256; divident++) {
  184. uint8_t currByte = (uint8_t)divident;
  185. /* calculate the CRC-8 value for current byte */
  186. for (uint8_t bit = 0; bit < 8; bit++) {
  187. if ((currByte & 0x80) != 0) {
  188. currByte <<= 1;
  189. currByte ^= generator;
  190. }
  191. else {
  192. currByte <<= 1;
  193. }
  194. }
  195. /* store CRC value in lookup table */
  196. crctable[divident] = currByte;
  197. }
  198. printf("Calculated CRC table");
  199. }
  200.  
  201. uint8_t Compute_CRC8(uint8_t* bytes)
  202. {
  203. uint8_t crc = 0;
  204. for (uint8_t i = 0; i < RECEIVE_MESSAGE_LENGTH - 1; i++) {
  205. /* XOR-in next input byte */
  206. uint8_t data = (uint8_t)(bytes[i] ^ crc);
  207. /* get current CRC value = remainder */
  208. crc = (uint8_t)(crctable[data]);
  209. }
  210.  
  211. return crc;
  212. }
  213.  
  214. /* Petros: CRC-8, crc value generation */
  215. uint8_t do_crc(uint8_t* package)
  216. {
  217. uint8_t crc = Compute_CRC8(package);
  218.  
  219. return crc;
  220. }
  221.  
  222. /* Petros: check if sender's crc matches receiver's */
  223. // TODO: check the sizeof(crc of sent msg); possible missmatch of quad/pc_crc
  224. bool match_crc(uint8_t quad_crc, uint8_t pc_crc) // change if unint8_t crc
  225. {
  226. if (quad_crc == pc_crc) {
  227. return true;
  228. } else {
  229. printf("Bad CRC");
  230. return false;
  231. }
  232. }
  233.  
  234. /* Alex: convert int16_t to uint8_t for logging purposes */
  235. void convert_uint32_to_uint8(uint32_t x, uint8_t* buf)
  236. {
  237. buf[0] = (x & 0x000000ff);
  238. buf[1] = (x & 0x0000ff00) >> 8;
  239. buf[2] = (x & 0x00ff0000) >> 16;
  240. buf[3] = (x & 0xff000000) >> 24;
  241. }
  242.  
  243. /* Alex: convert int16_t to uint8_t for logging purposes */
  244. void convert_int16_to_uint8(int16_t x, uint8_t* buf)
  245. {
  246. buf[0]= x & 0xff; // caps at 256 (2^8)
  247. buf[1]= (x >> 8); // multiplier of 256
  248. }
  249.  
  250. /* Alex: write bytes to flash */
  251. void write_log(uint8_t* bytes, uint32_t length)
  252. {
  253. // printf("Logging %lu bytes at address %lu \r\n", length, flash_address_p);
  254. bool success = flash_write_bytes(flash_address_p, bytes, length);
  255. flash_address_p += length;
  256.  
  257. if (!success) {
  258. printf("Bad writing of %lu bytes at address %lu", length, flash_address_p);
  259. }
  260. }
  261.  
  262. /* Alex: read bytes from flash, output as the logged value with printf */
  263. uint32_t read_log(uint32_t address, uint32_t length)
  264. {
  265. uint8_t buf[length];
  266. bool success = flash_read_bytes(address, buf, length);
  267. if (success == false) {
  268. printf("BAD READING!");
  269. return 0;
  270. } else {
  271. uint32_t val = 0;
  272. for (uint8_t i = 0; i < length; i++) {
  273. val += buf[i] << (8 * i);
  274. }
  275. // printf("%d", val);
  276. return val;
  277. }
  278. }
  279.  
  280. /*------------------------------------------------------------------
  281. * main -- do the test
  282. *------------------------------------------------------------------
  283. */
  284. int main(void)
  285. {
  286. uart_init();
  287. gpio_init();
  288. timers_init();
  289. adc_init();
  290. twi_init();
  291. imu_init(true, 100);
  292. baro_init();
  293. spi_flash_init();
  294. CalulateTable_CRC8(); // It initializes the CRC-8 table
  295. // ble_init();
  296.  
  297.  
  298. // uint8_t dummy_msg[RECEIVE_MESSAGE_LENGTH] = {0xFF, 0x01, 0x02, 0x03, 0x04, 0x05, 123};
  299.  
  300. uint32_t counter = 0;
  301. // uint8_t buf_int16[2];
  302. // uint8_t buf_int32[4];
  303. int ro,pi,ya;
  304. int thr;
  305. int mode1, mode2, mode3;
  306. int keypress[10];
  307. crc_value = 0; // Petros: most probably it is unint8_t
  308.  
  309. demo_done = false;
  310. stream_is_tracked = false;
  311. bytes_read = 0;
  312.  
  313. // init keyboard/joystick values
  314. for (uint8_t i = 0; i < 4; i++) {
  315. ae_keyboard[i] = 0;
  316. ae_joystick[i] = 0;
  317. }
  318.  
  319. // dummy_msg[6] = do_crc(dummy_msg);
  320. while (!demo_done) {
  321. // Petros: Write in buffer the message received
  322. /* INFO: if (true) then it loads bytes to buffer until
  323. the length is reached. */
  324. if (rx_queue.count) {
  325. while (rx_queue.count && bytes_read != RECEIVE_MESSAGE_LENGTH) {
  326. buffer((uint8_t)dequeue(&rx_queue));
  327. ++bytes_read;
  328. }
  329.  
  330. scanf(msg_buf, " \r %1d %1d %1d %1d %1d %1d %1d %1d %1d %1d %1d %1d %1d %1d %2d %2d %2d %3d",
  331. &mode1, &mode2, &mode3, &keypress[0], &keypress[1], &keypress[2], &keypress[3], &keypress[4],
  332. &keypress[5], &keypress[6], &keypress[7], &keypress[8], &keypress[9], &thr, &ro, &pi, &ya, &received_crc_value);
  333.  
  334. for (uint8_t i = 0; i < 10; i++) {
  335. process_key(keypress[i]);
  336. }
  337.  
  338. throttle = thr * 10;
  339. roll = ro * 10;
  340. pitch = pi * 10;
  341. yaw = ya * 10;
  342. }
  343.  
  344. // Petros: it enters the if-statement only if matching CRCs are found
  345. if (check_timer_flag())
  346. {
  347. if (counter++%20 == 0)
  348. nrf_gpio_pin_toggle(BLUE);
  349.  
  350. // Total throttle = keyboard + joystick + throttle
  351. for (uint8_t i = 0; i < 4; i++) {
  352. ae[i] = ae_keyboard[i] + ae_joystick[i] + throttle * 10;
  353. }
  354.  
  355. adc_request_sample();
  356. read_baro();
  357.  
  358. uint32_t timestamp = get_time_us();
  359.  
  360. printf("%10ld | ", timestamp);
  361. printf("%3d %3d %3d %3d | ",ae[0],ae[1],ae[2],ae[3]);
  362. printf("%6d %6d %6d | ", phi, theta, psi);
  363. printf("%6d %6d %6d | ", sp, sq, sr);
  364. printf("%4d | %4ld | %6ld \r\n", bat_volt, temperature, pressure);
  365.  
  366. // convert_uint32_to_uint8(timestamp, buf_int32);
  367. // write_log(buf_int32, 4);
  368. // for (uint8_t i = 0; i < 4; i++) {
  369. // convert_int16_to_uint8(ae[i], buf_int16);
  370. // write_log(buf_int16, 2);
  371. // }
  372. // convert_int16_to_uint8(phi, buf_int16);
  373. // write_log(buf_int16, 2);
  374. // convert_int16_to_uint8(theta, buf_int16);
  375. // write_log(buf_int16, 2);
  376. // convert_int16_to_uint8(psi, buf_int16);
  377. // write_log(buf_int16, 2);
  378. // convert_int16_to_uint8(sp, buf_int16);
  379. // write_log(buf_int16, 2);
  380. // convert_int16_to_uint8(sq, buf_int16);
  381. // write_log(buf_int16, 2);
  382. // convert_int16_to_uint8(sr, buf_int16);
  383. // write_log(buf_int16, 2);
  384. // convert_int16_to_uint8(bat_volt, buf_int16);
  385. // write_log(buf_int16, 2);
  386. // convert_uint32_to_uint8(temperature, buf_int32);
  387. // write_log(buf_int32, 4);
  388. // convert_uint32_to_uint8(pressure, buf_int32);
  389. // write_log(buf_int32, 4);
  390.  
  391. clear_timer_flag();
  392. } else
  393. continue;
  394.  
  395. if (check_sensor_int_flag()) {
  396. get_dmp_data();
  397. run_filters_and_control();
  398.  
  399. clear_sensor_int_flag();
  400. }
  401. }
  402.  
  403. // Read all stored logs
  404. // 30 bytes per log
  405. uint32_t count = 0x000000;
  406. while (count <= flash_address_p) {
  407. printf("%10ld", read_log(count, 4)); printf(" | "); count += 4;
  408. for (uint8_t i = 0; i < 4; i++) {
  409. printf("%3ld", read_log(count, 2));; printf(" "); count += 2;
  410. }
  411. printf("| ");
  412. printf("%6ld", read_log(count, 2)); printf(" | "); count += 2;
  413. printf("%6ld", read_log(count, 2)); printf(" | "); count += 2;
  414. printf("%6ld", read_log(count, 2)); printf(" | "); count += 2;
  415. printf("%6ld", read_log(count, 2)); printf(" | "); count += 2;
  416. printf("%6ld", read_log(count, 2)); printf(" | "); count += 2;
  417. printf("%6ld", read_log(count, 2)); printf(" | "); count += 2;
  418. printf("%4ld", read_log(count, 2)); printf(" | "); count += 2;
  419. printf("%4ld", read_log(count, 4)); printf(" | "); count += 4;
  420. printf("%6ld", read_log(count, 4)); printf(" | "); count += 4;
  421. printf("\r\n");
  422. nrf_delay_ms(10);
  423. }
  424.  
  425. printf("\n\t Goodbye \n\n");
  426.  
  427. nrf_delay_ms(100);
  428.  
  429. NVIC_SystemReset();
  430. }
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!