UCLA IEEE
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.
Revision 4:44a5b1e8fd27, committed 2013-11-02
- Comitter:
- teamgoat
- Date:
- Sat Nov 02 08:47:14 2013 +0000
- Parent:
- 3:f9e18a9cd9af
- Commit message:
- added process memory and communication via output buffer
Changed in this revision
diff -r f9e18a9cd9af -r 44a5b1e8fd27 control.cpp
--- a/control.cpp Fri Nov 01 01:23:04 2013 +0000
+++ b/control.cpp Sat Nov 02 08:47:14 2013 +0000
@@ -19,13 +19,18 @@
void init()
{
// initialize i2c sensors
- init_sensors();
+ schedule_proc("INITSENSE", &init_sensors);
// set initial processes
procs[0].status = READY;
procs[0].start = &get_sensor_data;
return;
}
+char* get_output(int pid)
+{
+ return procs[pid].output;
+}
+
void schedule()
{
for (int i=0; i<MAXPROC; i++)
@@ -33,9 +38,37 @@
process proc = procs[i];
if(proc.status == READY)
{
- proc.start();
+ int ret = proc.start(i);
+ // if the process returns 0, it means don't run again
+ if (ret == 0)
+ {
+ // set proc.status to ZOMBIE
+ // ZOMBIE means process
+ // is no longer active, but
+ // it's output buffer is still
+ // needed.
+ proc.status = ZOMBIE;
+ }
return;
}
}
}
-
\ No newline at end of file
+
+int schedule_proc(char *sid, int (*funcptr)(int))
+{
+ for (int i=0; i<MAXPROC; i++)
+ {
+ process proc = procs[i];
+ if(proc.status == EMPTY)
+ {
+ proc.status = READY;
+ proc.start = funcptr;
+ strncpy(proc.sid, sid, MAX_SID_LEN-1);
+ // null terminate proc.sid
+ proc.sid[MAX_SID_LEN-1] = 0;
+ return i;
+ }
+ }
+ // if no open processes, return -1
+ return -1;
+}
\ No newline at end of file
diff -r f9e18a9cd9af -r 44a5b1e8fd27 control.h
--- a/control.h Fri Nov 01 01:23:04 2013 +0000
+++ b/control.h Sat Nov 02 08:47:14 2013 +0000
@@ -1,15 +1,13 @@
#ifndef CONTROL_H
#define CONTROL_H
-void schedule();
-void init();
+#define OUTPUT_SIZE 512
+#define MAX_SID_LEN 32
-typedef struct
-{
- unsigned int status;
- unsigned int block_pid;
- void (*start)(void);
-} process;
+char* get_output(int);
+void init();
+void schedule();
+int schedule_proc(char *sid, int (*funcptr)(int));
typedef enum
{
@@ -19,4 +17,16 @@
EMPTY
} proc_stat;
+typedef struct
+{
+ // process status
+ unsigned int status;
+ // if status == BLOCKED, wait till block_pid's status is ZOMBIE
+ unsigned int block_pid;
+ // function. each "process" is a function with a number of properties and storage space
+ int (*start)(int);
+ char sid[MAX_SID_LEN];
+ char output[OUTPUT_SIZE];
+} process;
+
#endif //CONTROL_H
\ No newline at end of file
diff -r f9e18a9cd9af -r 44a5b1e8fd27 sensor.cpp
--- a/sensor.cpp Fri Nov 01 01:23:04 2013 +0000
+++ b/sensor.cpp Sat Nov 02 08:47:14 2013 +0000
@@ -206,7 +206,7 @@
}
return 8;
}
-int config_gyro(void)
+int config_gyro()
{
// turn on the gyro via i2c
int ret = gyro_turnon();
diff -r f9e18a9cd9af -r 44a5b1e8fd27 sensor.h
--- a/sensor.h Fri Nov 01 01:23:04 2013 +0000
+++ b/sensor.h Sat Nov 02 08:47:14 2013 +0000
@@ -22,6 +22,7 @@
{
int16_t ax, ay, az;
int16_t gx, gy, gz;
+ int16_t gx0, gy0, gz0;
char raw_data[6];
};
@@ -50,7 +51,7 @@
int read_barometer(void);
int config_accelerometer(void);
-int config_gyro(void);
+int config_gyro();
int config_compass(void);
int config_barometer(void);
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;
+}
diff -r f9e18a9cd9af -r 44a5b1e8fd27 steps.h --- a/steps.h Fri Nov 01 01:23:04 2013 +0000 +++ b/steps.h Sat Nov 02 08:47:14 2013 +0000 @@ -1,9 +1,11 @@ #ifndef STEPS_H #define STEPS_H #include "sensor.h" - +#include "control.h" -void init_sensors(); -void get_sensor_data(); +#define USBDEBUG 1 + +int init_sensors(int); +int get_sensor_data(int); #endif //STEPS_H \ No newline at end of file