UCLA IEEE / AIviate

Code for an autonomous plane I'm building. Includes process scheduling, process communication, and hardware sensor interfacing (via I2C). NOTE: currently in development, source code will be updated frequently.

Dependencies:   mbed

steps.cpp@4:44a5b1e8fd27, 2013-11-02 (annotated)

Committer:
teamgoat
Date:
Sat Nov 02 08:47:14 2013 +0000
Revision:
4:44a5b1e8fd27
Parent:
3:f9e18a9cd9af 
added process memory and communication via output buffer

Who changed what in which revision?

UserRevisionLine numberNew contents of line
teamgoat 0:0c627ff4c5ed 1 #include "steps.h"
teamgoat 0:0c627ff4c5ed 2
teamgoat 0:0c627ff4c5ed 3 Serial pc(USBTX, USBRX);
teamgoat 0:0c627ff4c5ed 4
teamgoat 0:0c627ff4c5ed 5 // in the future, change get_sensor_data to append the sensor data to a rolling list
teamgoat 4:44a5b1e8fd27 6 int get_sensor_data(int pid)
teamgoat 0:0c627ff4c5ed 7 {
teamgoat 2:452dd766d212 8 struct sensor s;
teamgoat 4:44a5b1e8fd27 9
teamgoat 4:44a5b1e8fd27 10 // retrieve process memory pointer
teamgoat 4:44a5b1e8fd27 11 char* mem = get_output(pid);
teamgoat 4:44a5b1e8fd27 12 int gx=0, gy=0, gz=0;
teamgoat 4:44a5b1e8fd27 13 if (sscanf(mem, "gx0 %i gy0 %i gz0 %i;\r\n", &gx, &gy, &gz) != 3)
teamgoat 4:44a5b1e8fd27 14 {
teamgoat 4:44a5b1e8fd27 15 // probably first time running process, retrieve data from init_sensors and store in current process output buffer
teamgoat 4:44a5b1e8fd27 16
teamgoat 4:44a5b1e8fd27 17 }
teamgoat 4:44a5b1e8fd27 18 else
teamgoat 4:44a5b1e8fd27 19 {
teamgoat 4:44a5b1e8fd27 20 s.gx0 = gx;
teamgoat 4:44a5b1e8fd27 21 s.gy0 = gy;
teamgoat 4:44a5b1e8fd27 22 s.gz0 = gz;
teamgoat 4:44a5b1e8fd27 23 }
teamgoat 4:44a5b1e8fd27 24
teamgoat 4:44a5b1e8fd27 25
teamgoat 0:0c627ff4c5ed 26 if (read_accelerometer(&s) == 0)
teamgoat 0:0c627ff4c5ed 27 {
teamgoat 4:44a5b1e8fd27 28 if (USBDEBUG)
teamgoat 4:44a5b1e8fd27 29 pc.printf("Error in get_sensor_data while reading from accel!\r\n");
teamgoat 4:44a5b1e8fd27 30 return 1;
teamgoat 0:0c627ff4c5ed 31 }
teamgoat 3:f9e18a9cd9af 32 if (read_gyro(&s) == 0)
teamgoat 3:f9e18a9cd9af 33 {
teamgoat 4:44a5b1e8fd27 34 if (USBDEBUG)
teamgoat 4:44a5b1e8fd27 35 pc.printf("Error in get_sensor_data while reading from gyro!\r\n");
teamgoat 4:44a5b1e8fd27 36 return 1;
teamgoat 3:f9e18a9cd9af 37 }
teamgoat 4:44a5b1e8fd27 38 if (USBDEBUG)
teamgoat 4:44a5b1e8fd27 39 pc.printf("Ax: %i Ay: %i Az: %i Gx: %i Gy: %i Gz: %i\r\n", s.ax, s.ay, s.az, s.gx, s.gy, s.gz);
teamgoat 4:44a5b1e8fd27 40 return 1;
teamgoat 2:452dd766d212 41 }
teamgoat 2:452dd766d212 42
teamgoat 4:44a5b1e8fd27 43 int init_sensors(int pid)
teamgoat 2:452dd766d212 44 {
teamgoat 2:452dd766d212 45 // create config struct
teamgoat 2:452dd766d212 46 struct config c;
teamgoat 2:452dd766d212 47
teamgoat 2:452dd766d212 48 // set configurations
teamgoat 2:452dd766d212 49 c.frequency = 10000;
teamgoat 2:452dd766d212 50
teamgoat 2:452dd766d212 51 // pass configuration struct to configuration routine
teamgoat 2:452dd766d212 52 int ret = config_gy80(&c);
teamgoat 2:452dd766d212 53 if (ret == 0)
teamgoat 2:452dd766d212 54 {
teamgoat 4:44a5b1e8fd27 55 if (USBDEBUG)
teamgoat 4:44a5b1e8fd27 56 pc.printf("Error configuring sensors\r\n");
teamgoat 2:452dd766d212 57 }
teamgoat 4:44a5b1e8fd27 58
teamgoat 4:44a5b1e8fd27 59 // accumulations of gyro values (for zeroing)
teamgoat 4:44a5b1e8fd27 60 int gx = 0, gy = 0, gz = 0;
teamgoat 4:44a5b1e8fd27 61 struct sensor s;
teamgoat 4:44a5b1e8fd27 62 int numzerotrials = 10;
teamgoat 4:44a5b1e8fd27 63 for (int i=0; i<numzerotrials; i++)
teamgoat 4:44a5b1e8fd27 64 {
teamgoat 4:44a5b1e8fd27 65 if (read_gyro(&s) == 0)
teamgoat 4:44a5b1e8fd27 66 {
teamgoat 4:44a5b1e8fd27 67 if (USBDEBUG)
teamgoat 4:44a5b1e8fd27 68 pc.printf("Error in collecting zero-level data from gyro (init_sensors)\r\n");
teamgoat 4:44a5b1e8fd27 69 return 1;
teamgoat 4:44a5b1e8fd27 70 }
teamgoat 4:44a5b1e8fd27 71 gx += s.gx;
teamgoat 4:44a5b1e8fd27 72 gy += s.gy;
teamgoat 4:44a5b1e8fd27 73 gz += s.gz;
teamgoat 4:44a5b1e8fd27 74 }
teamgoat 4:44a5b1e8fd27 75
teamgoat 4:44a5b1e8fd27 76 gx /= numzerotrials;
teamgoat 4:44a5b1e8fd27 77 gy /= numzerotrials;
teamgoat 4:44a5b1e8fd27 78 gz /= numzerotrials;
teamgoat 4:44a5b1e8fd27 79
teamgoat 4:44a5b1e8fd27 80 char * output = get_output(pid);
teamgoat 4:44a5b1e8fd27 81 sprintf(output, "gx0 %i gy0 %i gz0 %i;\r\n", gx, gy, gz);
teamgoat 4:44a5b1e8fd27 82
teamgoat 4:44a5b1e8fd27 83 schedule_proc("RETDATA", &get_sensor_data);
teamgoat 4:44a5b1e8fd27 84 return 0;
teamgoat 4:44a5b1e8fd27 85 }