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.
Diff: steps.cpp
- Revision:
- 4:44a5b1e8fd27
- Parent:
- 3:f9e18a9cd9af
diff -r f9e18a9cd9af -r 44a5b1e8fd27 steps.cpp --- a/steps.cpp Fri Nov 01 01:23:04 2013 +0000 +++ b/steps.cpp Sat Nov 02 08:47:14 2013 +0000 @@ -3,24 +3,44 @@ Serial pc(USBTX, USBRX); // in the future, change get_sensor_data to append the sensor data to a rolling list -void get_sensor_data() +int get_sensor_data(int pid) { struct sensor s; + + // retrieve process memory pointer + char* mem = get_output(pid); + int gx=0, gy=0, gz=0; + if (sscanf(mem, "gx0 %i gy0 %i gz0 %i;\r\n", &gx, &gy, &gz) != 3) + { + // probably first time running process, retrieve data from init_sensors and store in current process output buffer + + } + else + { + s.gx0 = gx; + s.gy0 = gy; + s.gz0 = gz; + } + + if (read_accelerometer(&s) == 0) { - pc.printf("Error in get_sensor_data while reading from accel!\r\n"); - return; + if (USBDEBUG) + pc.printf("Error in get_sensor_data while reading from accel!\r\n"); + return 1; } if (read_gyro(&s) == 0) { - pc.printf("Error in get_sensor_data while reading from gyro!\r\n"); - return; + if (USBDEBUG) + pc.printf("Error in get_sensor_data while reading from gyro!\r\n"); + return 1; } - 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); - return; + if (USBDEBUG) + 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); + return 1; } -void init_sensors() +int init_sensors(int pid) { // create config struct struct config c; @@ -32,6 +52,34 @@ int ret = config_gy80(&c); if (ret == 0) { - pc.printf("Error configuring sensors\r\n"); + if (USBDEBUG) + pc.printf("Error configuring sensors\r\n"); } -} \ No newline at end of file + + // accumulations of gyro values (for zeroing) + int gx = 0, gy = 0, gz = 0; + struct sensor s; + int numzerotrials = 10; + for (int i=0; i<numzerotrials; i++) + { + if (read_gyro(&s) == 0) + { + if (USBDEBUG) + pc.printf("Error in collecting zero-level data from gyro (init_sensors)\r\n"); + return 1; + } + gx += s.gx; + gy += s.gy; + gz += s.gz; + } + + gx /= numzerotrials; + gy /= numzerotrials; + gz /= numzerotrials; + + char * output = get_output(pid); + sprintf(output, "gx0 %i gy0 %i gz0 %i;\r\n", gx, gy, gz); + + schedule_proc("RETDATA", &get_sensor_data); + return 0; +}