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.h@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 | 0:a6a169de725f | 1 | #ifndef _SYSTEMSTATE_H |
shimniok | 0:a6a169de725f | 2 | #define _SYSTEMSTATE_H |
shimniok | 0:a6a169de725f | 3 | |
shimniok | 16:4c4b75824efc | 4 | #define SSBUF 32 // must be 2^n |
shimniok | 0:a6a169de725f | 5 | |
shimniok | 0:a6a169de725f | 6 | /** System State is the main mechanism for communicating current realtime system state to |
shimniok | 0:a6a169de725f | 7 | * the rest of the system for logging, data display, etc. |
shimniok | 0:a6a169de725f | 8 | */ |
shimniok | 0:a6a169de725f | 9 | |
shimniok | 16:4c4b75824efc | 10 | #include <stdbool.h> |
shimniok | 16:4c4b75824efc | 11 | |
shimniok | 0:a6a169de725f | 12 | /* struct systemState |
shimniok | 0:a6a169de725f | 13 | * structure containing system sensor data |
shimniok | 0:a6a169de725f | 14 | ****** System Status |
shimniok | 0:a6a169de725f | 15 | * millis number of milliseconds since epoch (or startup) |
shimniok | 0:a6a169de725f | 16 | * current current draw in amps |
shimniok | 0:a6a169de725f | 17 | * voltage voltage in volts |
shimniok | 0:a6a169de725f | 18 | ****** Data reported by IMU |
shimniok | 0:a6a169de725f | 19 | * g[3] raw 3-axis gyro values; if using 1-axis, then store data in gx |
shimniok | 0:a6a169de725f | 20 | * gTemp Gyro temperature |
shimniok | 0:a6a169de725f | 21 | * a[3] raw 3-axis accelerometer values |
shimniok | 0:a6a169de725f | 22 | * m[3] raw 3-axis magnetometer values; if using 2d then store data in mx and my |
shimniok | 0:a6a169de725f | 23 | * gHeading independently calculated gyro heading in degrees |
shimniok | 0:a6a169de725f | 24 | * cHeading independently calculated compass heading in degrees |
shimniok | 0:a6a169de725f | 25 | ****** AHRS Estimates |
shimniok | 0:a6a169de725f | 26 | * roll, pitch, yaw estimated attitude in degrees relative to the world frame |
shimniok | 0:a6a169de725f | 27 | ****** Data reported by GPS |
shimniok | 0:a6a169de725f | 28 | * gpsLatitude raw GPS latitude in fractional degrees (e.g., 39.123456) |
shimniok | 0:a6a169de725f | 29 | * gpsLongitude raw GPS longitude in fractional degrees (e.g., -104.123456 |
shimniok | 0:a6a169de725f | 30 | * gpsCourse_deg raw GPS course in degrees |
shimniok | 0:a6a169de725f | 31 | * gpsSpeed_mps raw GPS speed in m/s |
shimniok | 0:a6a169de725f | 32 | * gpsHDOP raw GPS Horizontal Dilution of Precision |
shimniok | 0:a6a169de725f | 33 | * gpsSats raw GPS Satellite fix count |
shimniok | 0:a6a169de725f | 34 | ****** Odometry data |
shimniok | 0:a6a169de725f | 35 | * lrEncDistance left rear encoder distance since last log update |
shimniok | 0:a6a169de725f | 36 | * rrEncDistance right rear encoder distance since last log update |
shimniok | 0:a6a169de725f | 37 | * lrEncSpeed left rear encoder speed |
shimniok | 0:a6a169de725f | 38 | * rrEncSpeed right rear encoder speed |
shimniok | 0:a6a169de725f | 39 | * encHeading estimated heading based on encoder readings |
shimniok | 0:a6a169de725f | 40 | ****** Estimated Position and Heading |
shimniok | 1:cb84b477886c | 41 | * estLagHeading estimated heading in degrees, lagged to sync with gps |
shimniok | 1:cb84b477886c | 42 | * estHeading estimated current heading |
shimniok | 0:a6a169de725f | 43 | * estLatitude estimated latitude in fractional degrees (e.g., 39.123456) |
shimniok | 0:a6a169de725f | 44 | * estLongitude estimated longitude in fractional degrees (e.g., -104.123456) |
shimniok | 0:a6a169de725f | 45 | * estNorthing some algorithms use UTM. Estimated UTM northing |
shimniok | 0:a6a169de725f | 46 | * estEasting estimated UTM easting |
shimniok | 0:a6a169de725f | 47 | * estX, estY some algorithms use simple x, y distance from origin (meters) |
shimniok | 0:a6a169de725f | 48 | ****** Waypoint data |
shimniok | 0:a6a169de725f | 49 | * nextWaypoint integer ID of the next waypoint |
shimniok | 0:a6a169de725f | 50 | * bearing estimated bearing to next waypoint in degrees |
shimniok | 0:a6a169de725f | 51 | * distance estimated distance to next waypoint in meters |
shimniok | 0:a6a169de725f | 52 | ****** Control data |
shimniok | 0:a6a169de725f | 53 | * throttle raw servo setting(units?) |
shimniok | 0:a6a169de725f | 54 | * steering raw servo setting(units?) |
shimniok | 0:a6a169de725f | 55 | */ |
shimniok | 0:a6a169de725f | 56 | typedef struct { |
shimniok | 0:a6a169de725f | 57 | unsigned int millis; |
shimniok | 0:a6a169de725f | 58 | float current, voltage; |
shimniok | 0:a6a169de725f | 59 | int g[3]; |
shimniok | 3:42f3821c4e54 | 60 | float gyro[3]; |
shimniok | 0:a6a169de725f | 61 | int gTemp; |
shimniok | 0:a6a169de725f | 62 | int a[3]; |
shimniok | 0:a6a169de725f | 63 | int m[3]; |
shimniok | 0:a6a169de725f | 64 | float gHeading; |
shimniok | 0:a6a169de725f | 65 | float cHeading; |
shimniok | 0:a6a169de725f | 66 | //float roll, pitch, yaw; |
shimniok | 0:a6a169de725f | 67 | double gpsLatitude; |
shimniok | 0:a6a169de725f | 68 | double gpsLongitude; |
shimniok | 0:a6a169de725f | 69 | float gpsCourse_deg; |
shimniok | 0:a6a169de725f | 70 | float gpsSpeed_mps; |
shimniok | 0:a6a169de725f | 71 | float gpsHDOP; |
shimniok | 0:a6a169de725f | 72 | int gpsSats; |
shimniok | 0:a6a169de725f | 73 | float lrEncDistance, rrEncDistance; |
shimniok | 0:a6a169de725f | 74 | float lrEncSpeed, rrEncSpeed; |
shimniok | 0:a6a169de725f | 75 | float encHeading; |
shimniok | 0:a6a169de725f | 76 | float estHeading; |
shimniok | 1:cb84b477886c | 77 | float estLagHeading; |
shimniok | 0:a6a169de725f | 78 | double estLatitude, estLongitude; |
shimniok | 0:a6a169de725f | 79 | //double estNorthing, estEasting; |
shimniok | 0:a6a169de725f | 80 | float estX, estY; |
shimniok | 0:a6a169de725f | 81 | unsigned short nextWaypoint; |
shimniok | 0:a6a169de725f | 82 | float bearing; |
shimniok | 0:a6a169de725f | 83 | float distance; |
shimniok | 0:a6a169de725f | 84 | float gbias; |
shimniok | 3:42f3821c4e54 | 85 | float errHeading; |
shimniok | 1:cb84b477886c | 86 | float steerAngle; |
shimniok | 16:4c4b75824efc | 87 | float LABrg; |
shimniok | 16:4c4b75824efc | 88 | float LAx; |
shimniok | 16:4c4b75824efc | 89 | float LAy; |
shimniok | 0:a6a169de725f | 90 | } SystemState; |
shimniok | 0:a6a169de725f | 91 | |
shimniok | 16:4c4b75824efc | 92 | void state_clear( SystemState *s ); |
shimniok | 16:4c4b75824efc | 93 | bool fifo_init(void); |
shimniok | 16:4c4b75824efc | 94 | void fifo_reset(void); |
shimniok | 16:4c4b75824efc | 95 | bool fifo_available(void); |
shimniok | 16:4c4b75824efc | 96 | bool fifo_push(SystemState *s); |
shimniok | 16:4c4b75824efc | 97 | SystemState *fifo_first(void); |
shimniok | 16:4c4b75824efc | 98 | SystemState *fifo_last(void); |
shimniok | 16:4c4b75824efc | 99 | SystemState *fifo_pull(void); |
shimniok | 16:4c4b75824efc | 100 | int fifo_getInState(void); |
shimniok | 16:4c4b75824efc | 101 | int fifo_getOutState(void); |
shimniok | 16:4c4b75824efc | 102 | |
shimniok | 16:4c4b75824efc | 103 | #endif |