Revision 9:fc3575d2cbbf, committed 2018-12-07

Comitter:

shimniok

Date:

Fri Dec 07 15:58:41 2018 +0000

Parent:

8:ba70bb062aa0

Child:

10:9fb3feb38746

Commit message:

Test gyro reading from shell.

Changed in this revision

--- a/Config.cpp	Wed Dec 05 18:02:32 2018 +0000
+++ b/Config.cpp	Fri Dec 07 15:58:41 2018 +0000
@@ -187,4 +187,19 @@
     }
     
     return status;
+}
+
+
+int Config::get(char *key, double *value)
+{
+    int status = 0;
+    
+    return status;
+}
+
+int Config::get(char *key, int *value)
+{
+    int status = 0;
+    
+    return status;
 }
\ No newline at end of file

--- a/Config.h	Wed Dec 05 18:02:32 2018 +0000
+++ b/Config.h	Fri Dec 07 15:58:41 2018 +0000
@@ -25,6 +25,10 @@
     int assign(char *key, char *value);
     void add(char *key, char format);
     void attach(Callback<void(char *)>, char *key, char format);
+    int get(char *key, double *value);
+    int get(char *key, int *value);
+    template<typename T>
+    T retrieve(char *key);
     
 private:
     static const int MAXBUF=128;

--- a/L3G4200D.lib	Wed Dec 05 18:02:32 2018 +0000
+++ b/L3G4200D.lib	Fri Dec 07 15:58:41 2018 +0000
@@ -1,1 +1,1 @@
-L3G4200D#9db065dfaf7d
+https://os.mbed.com/users/shimniok/code/L3G4200D/#14914cd8fdf3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Pinouts.md	Fri Dec 07 15:58:41 2018 +0000
@@ -0,0 +1,48 @@
+mbed lpc1768 pinouts
+====================
+
+UART
+----
+
+ * UART0 is the USB connection
+ * UART1 
+    * rx = p14 
+    * tx = p13
+ * UART2 
+    * rx = p27
+    * tx = p28
+ * UART3
+    * rx = p18
+    * tx = p17
+
+I2C
+---
+ * sda = p9 
+ * scl = p10
+
+Encoders
+--------
+ * A (Outermost)
+    * ALEFT = p30
+    * ARIGHT = p29
+ * B (Innermost)
+    * LEFT = unavailable
+    * RIGHT = unavailable
+
+
+GPIO
+----
+ * Digital
+    * IO0 = D0 = p11 
+    * IO1 = D1 = p12
+    * IO2 = D2 = P0.21 unavailable
+    * IO3 = D7 = P2.12 unavailable
+    * IO4 = D8 = P2.13 unavailable
+ * PWM 
+    * PW0 P2 = p25
+    * PW1 P1 = p26 
+ * Analog
+    * AN0 = A0 = p15
+    * AN1 = A1 = p16
+    * AN2 = A2 = p19
+    * AN3 = A3 = p20

--- a/SimpleShell.lib	Wed Dec 05 18:02:32 2018 +0000
+++ b/SimpleShell.lib	Fri Dec 07 15:58:41 2018 +0000
@@ -1,1 +1,1 @@
-SimpleShell#8f486f4d29d3
+https://os.mbed.com/users/shimniok/code/SimpleShell/#8f486f4d29d3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SystemState.cpp	Fri Dec 07 15:58:41 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;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SystemState.h	Fri Dec 07 15:58:41 2018 +0000
@@ -0,0 +1,103 @@
+#ifndef _SYSTEMSTATE_H
+#define _SYSTEMSTATE_H
+
+#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
+ * millis               number of milliseconds since epoch (or startup)
+ * current              current draw in amps
+ * voltage              voltage in volts
+ ****** Data reported by IMU
+ * g[3]                 raw 3-axis gyro values; if using 1-axis, then store data in gx
+ * gTemp                Gyro temperature
+ * a[3]                 raw 3-axis accelerometer values
+ * m[3]                 raw 3-axis magnetometer values; if using 2d then store data in mx and my
+ * gHeading             independently calculated gyro heading in degrees
+ * cHeading             independently calculated compass heading in degrees
+ ****** AHRS Estimates
+ * roll, pitch, yaw     estimated attitude in degrees relative to the world frame
+ ****** Data reported by GPS
+ * gpsLatitude          raw GPS latitude in fractional degrees (e.g., 39.123456)
+ * gpsLongitude         raw GPS longitude in fractional degrees (e.g., -104.123456
+ * gpsCourse_deg        raw GPS course in degrees
+ * gpsSpeed_mps         raw GPS speed in m/s
+ * gpsHDOP              raw GPS Horizontal Dilution of Precision
+ * gpsSats              raw GPS Satellite fix count
+ ****** Odometry data
+ * lrEncDistance        left rear encoder distance since last log update
+ * rrEncDistance        right rear encoder distance since last log update
+ * lrEncSpeed           left rear encoder speed 
+ * rrEncSpeed           right rear encoder speed
+ * encHeading           estimated heading based on encoder readings
+ ****** Estimated Position and Heading
+ * estLagHeading        estimated heading in degrees, lagged to sync with gps
+ * estHeading           estimated current heading
+ * estLatitude          estimated latitude in fractional degrees (e.g., 39.123456)
+ * estLongitude         estimated longitude in fractional degrees (e.g., -104.123456)
+ * estNorthing          some algorithms use UTM.  Estimated UTM northing
+ * estEasting           estimated UTM easting
+ * estX, estY           some algorithms use simple x, y distance from origin (meters)
+ ****** Waypoint data
+ * nextWaypoint         integer ID of the next waypoint
+ * bearing              estimated bearing to next waypoint in degrees
+ * distance             estimated distance to next waypoint in meters
+ ****** Control data
+ * throttle             raw servo setting(units?)
+ * steering             raw servo setting(units?)
+ */
+typedef struct {
+    unsigned int millis;
+    float current, voltage;
+    int g[3];
+    float gyro[3];
+    int gTemp;
+    int a[3];
+    int m[3];
+    float gHeading;
+    float cHeading;
+    //float roll, pitch, yaw;
+    double gpsLatitude;
+    double gpsLongitude;
+    float gpsCourse_deg;
+    float gpsSpeed_mps;
+    float gpsHDOP;
+    int gpsSats;
+    float lrEncDistance, rrEncDistance;
+    float lrEncSpeed, rrEncSpeed;
+    float encHeading;
+    float estHeading;
+    float estLagHeading;
+    double estLatitude, estLongitude;
+    //double estNorthing, estEasting;
+    float estX, estY;
+    unsigned short nextWaypoint;
+    float bearing;
+    float distance;
+    float gbias;
+    float errHeading;
+    float steerAngle;
+    float LABrg;
+    float LAx;
+    float LAy;
+} SystemState;
+
+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

--- a/main.cpp	Wed Dec 05 18:02:32 2018 +0000
+++ b/main.cpp	Fri Dec 07 15:58:41 2018 +0000
@@ -11,6 +11,7 @@
 #include "FATFileSystem.h"
 #include "SimpleShell.h"
 #include "Config.h"
+#include "L3G4200D.h"
 
 Serial pc(USBTX, USBRX);
 //SDBlockDevice bd(p5, p6, p7, p8); // MOSI, MISO, CLK, CS
@@ -19,14 +20,38 @@
 Config config;
 SimpleShell sh;
 
+L3G4200D gyro(p9, p10);
 DigitalOut led1(LED1);
 
 Thread thread;
 
+/******** SHELL COMMANDS ********/
+
 void test() {
     printf("Hello world!\n");
 }
 
+void read_gyro() 
+{
+    int g[3];
+    gyro.read(g);
+    printf("Gyro: %d, %d, %d\n", g[0], g[1], g[2]);
+}
+
+
+/******** GYRO UPDATER ********/
+void gyro_update()
+{
+    int g[3];
+ 
+    while (1) {
+        gyro.read(g);
+        // send message / add to status fifo?
+    }
+}
+
+
+/******** MAIN ********/
 
 // main() runs in its own thread in the OS
 int main()
@@ -64,6 +89,7 @@
     }
     
     sh.attach(test, "test");
+    sh.attach(read_gyro, "gyro");
     
     thread.start(callback(&sh, &SimpleShell::run));