SystemState.cpp

00001 /*
00002  * SystemState.c
00003  *
00004  *  Created on: Jan 1, 2014
00005  *      Author: mes
00006  */
00007 
00008 #include "mbed.h"
00009 #include <stdlib.h>
00010 #include <string.h>
00011 #include "SystemState.h"
00012 
00013 volatile int inState = 0; // push pointer
00014 volatile int outState = 0; // pull pointer
00015 SystemState *state;
00016 SystemState mystate[SSBUF];
00017 
00018 void state_clear( SystemState *s )
00019 {
00020     s->millis = 0;
00021     s->current = s->voltage = 0.0;
00022     s->g[0] = s->g[1] = s->g[2] = 0;
00023     s->gyro[0] = s->gyro[1] = s->gyro[2] = 0;
00024     s->gTemp = 0;
00025     s->a[0] = s->a[1] = s->a[2] = 0;
00026     s->m[0] = s->m[1] = s->m[2] = 0;
00027     s->gHeading = s->cHeading = 0.0;
00028     s->gpsLatitude = s->gpsLongitude = s->gpsCourse_deg = s->gpsSpeed_mps = s->gpsHDOP = 0.0;
00029     s->lrEncDistance = s->rrEncDistance = 0.0;
00030     s->lrEncSpeed = s->rrEncSpeed = s->encHeading = 0.0;
00031     s->estHeading = s->estLatitude = s->estLongitude = 0.0;
00032     s->estX = s->estY = 0.0;
00033     s->nextWaypoint = 0;
00034     s->bearing = s->distance = 0.0;
00035     s->LABrg = s->LAx = s->LAy = 0.0;
00036 }
00037 
00038 bool fifo_init() {
00039     // Allocate memory for system state buffer
00040     // We're doing this to (hopefully) save some flash size
00041     //state = (SystemState *) malloc(SSBUF*sizeof(SystemState));
00042     state = mystate;
00043     fifo_reset();
00044     return (state != NULL);
00045 }
00046 
00047 void fifo_reset() {
00048     // initialize in/out pointers
00049     __disable_irq();
00050     inState = outState = 0;
00051     __enable_irq();
00052 }
00053 
00054 bool fifo_available() {
00055     return (inState != outState);
00056 }
00057 
00058 bool fifo_push(SystemState *s) {
00059     __disable_irq();
00060     inState++;                      // Get next state struct in the buffer
00061     inState &= (SSBUF-1);           // Wrap around
00062     __enable_irq();
00063     memcpy((void *) &state[inState], (void *) s, sizeof(SystemState));
00064 
00065     return (inState != outState);
00066 }
00067 
00068 SystemState *fifo_pull() {
00069     SystemState *s = NULL;
00070 
00071     if (fifo_available()) {
00072         __disable_irq();
00073         outState++;                     // increment
00074         outState &= (SSBUF-1);          // wrap
00075         s = fifo_last();
00076         __enable_irq();
00077     }
00078 
00079     return s;
00080 }
00081 
00082 SystemState *fifo_first() {
00083     return &state[inState];
00084 }
00085 
00086 SystemState *fifo_last() {
00087     return &state[outState];
00088 }
00089 
00090 int fifo_getInState() {
00091     return inState;
00092 }
00093 
00094 int fifo_getOutState() {
00095     return outState;
00096 }