imu rev1
Fork of AIviate by
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
- Child:
- 5:d85bac38cdff
added process memory and communication via output buffer
Who changed what in which revision?
User | Revision | Line number | New 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 | } |