Michael Shimniok / Mbed 2 deprecated DataBus

Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.

Dependencies:   mbed Watchdog SDFileSystem DigoleSerialDisp

SystemState.cpp@25:bb5356402687, 2018-11-30 (annotated)

Committer:
shimniok
Date:
Fri Nov 30 16:11:53 2018 +0000
Revision:
25:bb5356402687
Parent:
16:4c4b75824efc 
Initial publish of revised version.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 16:4c4b75824efc 1 /*
shimniok 16:4c4b75824efc 2 * SystemState.c
shimniok 16:4c4b75824efc 3 *
shimniok 16:4c4b75824efc 4 * Created on: Jan 1, 2014
shimniok 16:4c4b75824efc 5 * Author: mes
shimniok 16:4c4b75824efc 6 */
shimniok 16:4c4b75824efc 7
shimniok 16:4c4b75824efc 8 #include "mbed.h"
shimniok 16:4c4b75824efc 9 #include <stdlib.h>
shimniok 16:4c4b75824efc 10 #include <string.h>
shimniok 16:4c4b75824efc 11 #include "SystemState.h"
shimniok 16:4c4b75824efc 12
shimniok 16:4c4b75824efc 13 volatile int inState = 0; // push pointer
shimniok 16:4c4b75824efc 14 volatile int outState = 0; // pull pointer
shimniok 16:4c4b75824efc 15 SystemState *state;
shimniok 16:4c4b75824efc 16 SystemState mystate[SSBUF];
shimniok 16:4c4b75824efc 17
shimniok 16:4c4b75824efc 18 void state_clear( SystemState *s )
shimniok 16:4c4b75824efc 19 {
shimniok 16:4c4b75824efc 20 s->millis = 0;
shimniok 16:4c4b75824efc 21 s->current = s->voltage = 0.0;
shimniok 16:4c4b75824efc 22 s->g[0] = s->g[1] = s->g[2] = 0;
shimniok 16:4c4b75824efc 23 s->gyro[0] = s->gyro[1] = s->gyro[2] = 0;
shimniok 16:4c4b75824efc 24 s->gTemp = 0;
shimniok 16:4c4b75824efc 25 s->a[0] = s->a[1] = s->a[2] = 0;
shimniok 16:4c4b75824efc 26 s->m[0] = s->m[1] = s->m[2] = 0;
shimniok 16:4c4b75824efc 27 s->gHeading = s->cHeading = 0.0;
shimniok 16:4c4b75824efc 28 s->gpsLatitude = s->gpsLongitude = s->gpsCourse_deg = s->gpsSpeed_mps = s->gpsHDOP = 0.0;
shimniok 16:4c4b75824efc 29 s->lrEncDistance = s->rrEncDistance = 0.0;
shimniok 16:4c4b75824efc 30 s->lrEncSpeed = s->rrEncSpeed = s->encHeading = 0.0;
shimniok 16:4c4b75824efc 31 s->estHeading = s->estLatitude = s->estLongitude = 0.0;
shimniok 16:4c4b75824efc 32 s->estX = s->estY = 0.0;
shimniok 16:4c4b75824efc 33 s->nextWaypoint = 0;
shimniok 16:4c4b75824efc 34 s->bearing = s->distance = 0.0;
shimniok 16:4c4b75824efc 35 s->LABrg = s->LAx = s->LAy = 0.0;
shimniok 16:4c4b75824efc 36 }
shimniok 16:4c4b75824efc 37
shimniok 16:4c4b75824efc 38 bool fifo_init() {
shimniok 16:4c4b75824efc 39 // Allocate memory for system state buffer
shimniok 16:4c4b75824efc 40 // We're doing this to (hopefully) save some flash size
shimniok 16:4c4b75824efc 41 //state = (SystemState *) malloc(SSBUF*sizeof(SystemState));
shimniok 16:4c4b75824efc 42 state = mystate;
shimniok 16:4c4b75824efc 43 fifo_reset();
shimniok 16:4c4b75824efc 44 return (state != NULL);
shimniok 16:4c4b75824efc 45 }
shimniok 16:4c4b75824efc 46
shimniok 16:4c4b75824efc 47 void fifo_reset() {
shimniok 16:4c4b75824efc 48 // initialize in/out pointers
shimniok 16:4c4b75824efc 49 __disable_irq();
shimniok 16:4c4b75824efc 50 inState = outState = 0;
shimniok 16:4c4b75824efc 51 __enable_irq();
shimniok 16:4c4b75824efc 52 }
shimniok 16:4c4b75824efc 53
shimniok 16:4c4b75824efc 54 bool fifo_available() {
shimniok 16:4c4b75824efc 55 return (inState != outState);
shimniok 16:4c4b75824efc 56 }
shimniok 16:4c4b75824efc 57
shimniok 16:4c4b75824efc 58 bool fifo_push(SystemState *s) {
shimniok 16:4c4b75824efc 59 __disable_irq();
shimniok 16:4c4b75824efc 60 inState++; // Get next state struct in the buffer
shimniok 16:4c4b75824efc 61 inState &= (SSBUF-1); // Wrap around
shimniok 16:4c4b75824efc 62 __enable_irq();
shimniok 16:4c4b75824efc 63 memcpy((void *) &state[inState], (void *) s, sizeof(SystemState));
shimniok 16:4c4b75824efc 64
shimniok 16:4c4b75824efc 65 return (inState != outState);
shimniok 16:4c4b75824efc 66 }
shimniok 16:4c4b75824efc 67
shimniok 16:4c4b75824efc 68 SystemState *fifo_pull() {
shimniok 16:4c4b75824efc 69 SystemState *s = NULL;
shimniok 16:4c4b75824efc 70
shimniok 16:4c4b75824efc 71 if (fifo_available()) {
shimniok 16:4c4b75824efc 72 __disable_irq();
shimniok 16:4c4b75824efc 73 outState++; // increment
shimniok 16:4c4b75824efc 74 outState &= (SSBUF-1); // wrap
shimniok 16:4c4b75824efc 75 s = fifo_last();
shimniok 16:4c4b75824efc 76 __enable_irq();
shimniok 16:4c4b75824efc 77 }
shimniok 16:4c4b75824efc 78
shimniok 16:4c4b75824efc 79 return s;
shimniok 16:4c4b75824efc 80 }
shimniok 16:4c4b75824efc 81
shimniok 16:4c4b75824efc 82 SystemState *fifo_first() {
shimniok 16:4c4b75824efc 83 return &state[inState];
shimniok 16:4c4b75824efc 84 }
shimniok 16:4c4b75824efc 85
shimniok 16:4c4b75824efc 86 SystemState *fifo_last() {
shimniok 16:4c4b75824efc 87 return &state[outState];
shimniok 16:4c4b75824efc 88 }
shimniok 16:4c4b75824efc 89
shimniok 16:4c4b75824efc 90 int fifo_getInState() {
shimniok 16:4c4b75824efc 91 return inState;
shimniok 16:4c4b75824efc 92 }
shimniok 16:4c4b75824efc 93
shimniok 16:4c4b75824efc 94 int fifo_getOutState() {
shimniok 16:4c4b75824efc 95 return outState;
shimniok 16:4c4b75824efc 96 }