Revision 16:4c4b75824efc, committed 2018-11-29

Comitter:

shimniok

Date:

Thu Nov 29 17:13:50 2018 +0000

Parent:

15:01fb4916a5cd

Child:

17:08f6ee55abbe

Commit message:

Updated SystemState.h, added SystemState.cpp

Changed in this revision

--- /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;
+}

--- a/SystemState.h	Thu Nov 29 17:11:34 2018 +0000
+++ b/SystemState.h	Thu Nov 29 17:13:50 2018 +0000
@@ -1,12 +1,14 @@
 #ifndef _SYSTEMSTATE_H
 #define _SYSTEMSTATE_H
 
-#define SSBUF 0x3F   // 0b0000 0011 1111 1111
+#define SSBUF 32 // must be 2^n
 
 /** System State is the main mechanism for communicating current realtime system state to
  * the rest of the system for logging, data display, etc.
  */
 
+#include <stdbool.h>
+
 /* struct systemState
  * structure containing system sensor data
  ****** System Status
@@ -82,6 +84,20 @@
     float gbias;
     float errHeading;
     float steerAngle;
+    float LABrg;
+    float LAx;
+    float LAy;
 } SystemState;
 
-#endif
\ No newline at end of file
+void state_clear( SystemState *s );
+bool fifo_init(void);
+void fifo_reset(void);
+bool fifo_available(void);
+bool fifo_push(SystemState *s);
+SystemState *fifo_first(void);
+SystemState *fifo_last(void);
+SystemState *fifo_pull(void);
+int fifo_getInState(void);
+int fifo_getOutState(void);
+
+#endif