imu rev1
Fork of AIviate by
control.cpp
- Committer:
- team10
- Date:
- 2013-11-26
- Revision:
- 5:d85bac38cdff
- Parent:
- 4:44a5b1e8fd27
File content as of revision 5:d85bac38cdff:
#include "control.h" #include "sensor.h" #include "steps.h" #include "mbed.h" #include "IMUfilter.h" #define MAXPROC 15 IMUfilter imuFilter(0.1, 10); process procs[MAXPROC] = {0}; Serial pc3(USBTX, USBRX); LocalFileSystem local("local"); int main() { init(); pc3.baud(115200); accelerometer_measure(); gyro_turnon(); sensor ter; int a; while (true) { schedule(); a = read_accelerometer(&ter); a = read_gyro(&ter); wait(1); //Gyro and accelerometer values must be converted to rad/s and m/s/s //before being passed to the filter. /* imuFilter.updateFilter(ter.gx, ter.gy, ter.gz, ter.ax, ter.ay, ter.az); imuFilter.computeEuler(); printf("%f, %f, %f\n", imuFilter.getRoll(), imuFilter.getPitch(), imuFilter.getYaw()); */ } ; } void init() { // initialize i2c 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++) { process proc = procs[i]; if(proc.status == READY) { 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; } } } 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; }