Diff: SystemState.cpp

Revision:

16:4c4b75824efc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SystemState.cpp	Thu Nov 29 17:13:50 2018 +0000
@@ -0,0 +1,96 @@
+/*
+ * SystemState.c
+ *
+ *  Created on: Jan 1, 2014
+ *      Author: mes
+ */
+
+#include "mbed.h"
+#include <stdlib.h>
+#include <string.h>
+#include "SystemState.h"
+
+volatile int inState = 0; // push pointer
+volatile int outState = 0; // pull pointer
+SystemState *state;
+SystemState mystate[SSBUF];
+
+void state_clear( SystemState *s )
+{
+    s->millis = 0;
+    s->current = s->voltage = 0.0;
+    s->g[0] = s->g[1] = s->g[2] = 0;
+    s->gyro[0] = s->gyro[1] = s->gyro[2] = 0;
+    s->gTemp = 0;
+    s->a[0] = s->a[1] = s->a[2] = 0;
+    s->m[0] = s->m[1] = s->m[2] = 0;
+    s->gHeading = s->cHeading = 0.0;
+    s->gpsLatitude = s->gpsLongitude = s->gpsCourse_deg = s->gpsSpeed_mps = s->gpsHDOP = 0.0;
+    s->lrEncDistance = s->rrEncDistance = 0.0;
+    s->lrEncSpeed = s->rrEncSpeed = s->encHeading = 0.0;
+    s->estHeading = s->estLatitude = s->estLongitude = 0.0;
+    s->estX = s->estY = 0.0;
+    s->nextWaypoint = 0;
+    s->bearing = s->distance = 0.0;
+    s->LABrg = s->LAx = s->LAy = 0.0;
+}
+
+bool fifo_init() {
+    // Allocate memory for system state buffer
+    // We're doing this to (hopefully) save some flash size
+    //state = (SystemState *) malloc(SSBUF*sizeof(SystemState));
+    state = mystate;
+    fifo_reset();
+    return (state != NULL);
+}
+
+void fifo_reset() {
+    // initialize in/out pointers
+    __disable_irq();
+    inState = outState = 0;
+    __enable_irq();
+}
+
+bool fifo_available() {
+    return (inState != outState);
+}
+
+bool fifo_push(SystemState *s) {
+    __disable_irq();
+    inState++;                      // Get next state struct in the buffer
+    inState &= (SSBUF-1);           // Wrap around
+    __enable_irq();
+    memcpy((void *) &state[inState], (void *) s, sizeof(SystemState));
+
+    return (inState != outState);
+}
+
+SystemState *fifo_pull() {
+    SystemState *s = NULL;
+
+    if (fifo_available()) {
+        __disable_irq();
+        outState++;                     // increment
+        outState &= (SSBUF-1);          // wrap
+        s = fifo_last();
+        __enable_irq();
+    }
+
+    return s;
+}
+
+SystemState *fifo_first() {
+    return &state[inState];
+}
+
+SystemState *fifo_last() {
+    return &state[outState];
+}
+
+int fifo_getInState() {
+    return inState;
+}
+
+int fifo_getOutState() {
+    return outState;
+}