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?
User | Revision | Line number | New 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 | } |