Revision 2:fbc6e3cf3ed8, committed 2013-06-06

Comitter:

shimniok

Date:

Thu Jun 06 13:40:23 2013 +0000

Parent:

1:cb84b477886c

Child:

3:42f3821c4e54

Commit message:

Sort-of working version, still some errors with estimation. Added clamp() function.

Changed in this revision

--- a/Actuators/Steering/Steering.cpp	Tue May 28 13:58:35 2013 +0000
+++ b/Actuators/Steering/Steering.cpp	Thu Jun 06 13:40:23 2013 +0000
@@ -6,10 +6,10 @@
 /** create a new steering calculator for a particular vehicle
  *
  */
-Steering::Steering(float wheelbase, float track)
-    : _wheelbase(wheelbase)
-    , _track(track)
-    , _intercept(2.0)
+Steering::Steering(float wheelbase, float track):
+    _wheelbase(wheelbase),
+    _track(track),
+    _intercept(2.0)
 {
 }
 
@@ -101,6 +101,7 @@
     return 0;
 }
 
+//static int skip=0;
 
 float Steering::pathPursuitSA(float hdg, float Bx, float By, float Ax, float Ay, float Cx, float Cy)
 {
@@ -116,31 +117,37 @@
     // along the path by the lookahead distance
     float legLength = sqrtf(Lx*Lx + Ly*Ly); // ||L||
     float proj = (Lx*Rx + Ly*Ry)/legLength; // R dot L/||L||, projection magnitude, bot vector onto leg vector
-    float LAx = (proj+_intercept)*Lx/legLength + Ax; // find projection point + lookahead, along leg
-    float LAy = (proj+_intercept)*Ly/legLength + Ay;
+    float LAx = (proj + _intercept)*Lx/legLength; // find projection point + lookahead, along leg, relative to Bx
+    float LAy = (proj + _intercept)*Ly/legLength;
     // Compute a circle that is tangential to bot heading and intercepts bot
     // and goal point (LAx,LAy), the intercept circle. Then compute the steering
     // angle to trace that circle. (x,y because 0 deg points up not right)
-    float brg = 180*atan2(LAx-Rx,LAy-Ry)/PI;
+    float brg = angle_degrees(atan2(LAx-Rx,LAy-Ry));
+    if (brg >= 360.0) brg -= 360.0;
+    if (brg < 0) brg += 360.0;
     // would be nice to add in some noise to heading info
-    float relBrg = brg-hdg;
+    float relBrg = brg - hdg;
     if (relBrg < -180.0) relBrg += 360.0;
     if (relBrg >= 180.0) relBrg -= 360.0;
-    // I haven't had time to work out why the equation is asymmetrical, that is, 
-    // negative angle produces slightly less steering angle. So instead, just use
-    // absolute value and restore sign later.
-    float sign = (relBrg < 0) ? -1 : 1;
     // The equation peaks out at 90* so clamp theta artifically to 90, so that
     // if theta is actually > 90, we select max steering
     if (fabs(relBrg) > 90.0) relBrg = 90.0;
     // Compute radius based on intercept distance and specified angle
-    float radius = _intercept/(2*sin(fabs(relBrg)*PI/180));
+    float radius = _intercept/(2*sin(angle_radians(fabs(relBrg))));
     // optionally, limit radius min/max
     // Now compute the steering angle to achieve the circle of 
     // Steering angle is based on wheelbase and track width
-    SA = sign * angle_degrees(asin(_wheelbase / (radius - _track/2)));
-
-    //fprintf(stdout, "R(%.4f,%.4f) A(%.4f,%.4f) C(%.4f,%.4f) L(%.4f,%.4f) b=%.2f rb=%.2f sa=%.2f\n", Bx, By, Ax, Ay, Cx, Cy, Lx, Ly, brg, relBrg, SA);
+    SA = angle_degrees(asin(_wheelbase / (radius - _track/2)));
+    // I haven't had time to work out why the equation is asymmetrical, that is, 
+    // negative angle produces slightly less steering angle.
+    if (relBrg < 0) SA = -SA; 
+    
+    /*
+    if (++skip >= 20) {
+        fprintf(stdout, "R(%.4f,%.4f) A(%.4f,%.4f) C(%.4f,%.4f) LA(%.4f,%.4f) h=%.2f b=%.2f rb=%.2f sa=%.2f\n", Bx, By, Ax, Ay, Cx, Cy, LAx, LAy, hdg, brg, relBrg, SA);
+        skip = 0;
+    }
+    */
         
     return SA;
 }

--- a/Actuators/Steering/Steering.h	Tue May 28 13:58:35 2013 +0000
+++ b/Actuators/Steering/Steering.h	Thu Jun 06 13:40:23 2013 +0000
@@ -1,5 +1,7 @@
+#ifndef __STEERING_H
+#define __STEERING_H
 
-#define PI 3.141592653589
+#include "globals.h"
 
 /** A class for managing steering angle calculations based on current and desired heading
  *  and specified intercept distance along the new path.  
@@ -67,4 +69,6 @@
     float _wheelbase;
     float _track;
     float _intercept;
-};
\ No newline at end of file
+};
+
+#endif
\ No newline at end of file

--- a/Estimation/CartPosition/CartPosition.cpp	Tue May 28 13:58:35 2013 +0000
+++ b/Estimation/CartPosition/CartPosition.cpp	Thu Jun 06 13:40:23 2013 +0000
@@ -1,7 +1,7 @@
 #include "mbed.h" // debug
 #include "CartPosition.h"
 
-#define PI 3.1415926535897932
+#include "globals.h"
 
 CartPosition::CartPosition(void)
 {

--- a/Estimation/kalman.cpp	Tue May 28 13:58:35 2013 +0000
+++ b/Estimation/kalman.cpp	Thu Jun 06 13:40:23 2013 +0000
@@ -14,8 +14,8 @@
 static float H[4]={ 1, 0, 0, 1 };           // Observer matrix maps measurements to state transition
 float K[4]={ 0, 0, 0, 0 };                  // Kalman gain
 static float P[4]={ 1000, 0, 0, 1000 };     // Covariance matrix
-static float R[4]={ 3, 0, 0, 0.03 };        // Measurement noise, hdg, hdg rate
-static float Q[4]={ 0.01, 0, 0, 0.01 };     // Process noise matrix
+static float R[4]={ 100, 0, 0, 0.03 };        // Measurement noise, hdg, hdg rate
+static float Q[4]={ 100, 0, 0, 0.01 };        // Process noise matrix
 static float I[4]={ 1, 0, 0, 1 };           // Identity matrix
 
 float kfGetX(int i)

--- a/Sensors/GPS/Sirf3.cpp	Tue May 28 13:58:35 2013 +0000
+++ b/Sensors/GPS/Sirf3.cpp	Thu Jun 06 13:40:23 2013 +0000
@@ -6,7 +6,6 @@
 Sirf3::Sirf3(PinName tx, PinName rx):
     serial(tx, rx)
 {
-    setBaud(4800);
     enable();
 }
 
@@ -28,11 +27,13 @@
 void Sirf3::enable(void)
 {
     reset_available();
+    setBaud(4800);
     serial.attach(this, &Sirf3::recv_handler, Serial::RxIrq);
 }
 
 void Sirf3::disable(void)
 {
+    setBaud(4800);
     serial.attach(NULL, Serial::RxIrq);
 }
 
@@ -41,8 +42,13 @@
  */
 void Sirf3::enableVerbose(void)
 {
+    setBaud(4800);
     gsaMessage(true);
+    ggaMessage(true);
+    gllMessage(false);
     gsvMessage(true);
+    rmcMessage(true);
+    vtgMessage(false);
 }
 
 /**
@@ -50,8 +56,27 @@
  */
 void Sirf3::disableVerbose(void)
 {
-    gsaMessage(false);
+    setBaud(4800);
+    gsaMessage(true);
+    ggaMessage(true);
+    gllMessage(false);
     gsvMessage(false);
+    rmcMessage(true);
+    vtgMessage(false);
+}
+
+void Sirf3::setUpdateRate(int rate)
+{
+    // We're stuck at 1Hz
+    return;
+}
+
+int Sirf3::getAvailable(void)
+{
+    int answer = 0x00;
+    if (nmea.gga_ready()) answer |= 0x01;
+    if (nmea.rmc_ready()) answer |= 0x02;
+    return answer;
 }
 
 double Sirf3::latitude(void)
@@ -107,6 +132,28 @@
     }
 }
 
+void Sirf3::ggaMessage(bool enable)
+{
+    if (enable) {
+        serial.printf("$PSRF103,00,00,01,01*25\r\n");     // Enable GGA
+    } else {
+        serial.printf("$PSRF103,00,00,00,01*24\r\n");     // Disable GGA
+    }
+
+    return;
+}
+
+void Sirf3::gllMessage(bool enable)
+{
+    if (enable) {
+        serial.printf("$PSRF103,01,00,01,01*24\r\n");     // Enable GLL
+    } else {
+        serial.printf("$PSRF103,01,00,00,01*25\r\n");     // Disable GLL
+    }
+
+    return;
+}
+
 void Sirf3::gsaMessage(bool enable)
 {
     if (enable) {
@@ -129,3 +176,24 @@
     return;
 }
 
+void Sirf3::rmcMessage(bool enable)
+{
+    if (enable) {
+        serial.printf("$PSRF103,04,00,01,01*21\r\n");     // Enable RMC
+    } else {
+        serial.printf("$PSRF103,04,00,00,01*20\r\n");     // Disable RMC
+    }
+
+    return;
+}
+
+void Sirf3::vtgMessage(bool enable)
+{
+    if (enable) {
+        serial.printf("$PSRF103,05,00,01,01*20\r\n");     // Enable VTG
+    } else {
+        serial.printf("$PSRF103,05,00,00,01*21\r\n");     // Disable VTG
+    }
+
+    return;
+}

--- a/Sensors/GPS/Sirf3.h	Tue May 28 13:58:35 2013 +0000
+++ b/Sensors/GPS/Sirf3.h	Thu Jun 06 13:40:23 2013 +0000
@@ -48,6 +48,17 @@
     virtual void disableVerbose(void);
 
     /**
+     * stub function for compatibility
+     */
+    int getAvailable(void);
+
+    /**
+     * stub function for compatibility
+     * Configure GPS update rate (doesn't do anything on Sirf3)
+     */
+    void setUpdateRate(int rate);
+
+    /**
      * get latitude
      */
     virtual double latitude(void);
@@ -90,8 +101,12 @@
 
 private:
     void recv_handler(void);
+    void ggaMessage(bool enable);
     void gsaMessage(bool enable);
     void gsvMessage(bool enable);
+    void gllMessage(bool enable);
+    void rmcMessage(bool enable);
+    void vtgMessage(bool enable);
     Serial serial;
     TinyGPS nmea;
 };

--- a/Sensors/GPS/TinyGPS/TinyGPS.h	Tue May 28 13:58:35 2013 +0000
+++ b/Sensors/GPS/TinyGPS/TinyGPS.h	Thu Jun 06 13:40:23 2013 +0000
@@ -197,7 +197,11 @@
     /** determine if GSV sentence parsed since last reset_ready()
      */
     inline bool gsv_ready() { return _gsv_ready; }
+
+    inline bool gga_ready() { return _gga_ready; }
     
+    inline bool rmc_ready() { return _rmc_ready; }
+   
     /** Reset the ready flags for all the parsed sentences
      */
     inline void reset_ready() { _gsv_ready = _rmc_ready = _gga_ready = false; }

--- a/Sensors/GPS/Venus638flpx.cpp	Tue May 28 13:58:35 2013 +0000
+++ b/Sensors/GPS/Venus638flpx.cpp	Thu Jun 06 13:40:23 2013 +0000
@@ -167,6 +167,7 @@
 
 int Venus638flpx::getAvailable(void)
 {
+    // TODO 2 not sure what to do here
     return 0xff;
 }
 

--- a/Sensors/Sensors.cpp	Tue May 28 13:58:35 2013 +0000
+++ b/Sensors/Sensors.cpp	Thu Jun 06 13:40:23 2013 +0000
@@ -39,8 +39,7 @@
 ///////////////////// MAGNETOMETER CALIBRATION
 
 Sensors::Sensors():
-    //gps(p9, p10),              // Ublox6 GPS
-    gps(p26, p25),               // Venus GPS
+    gps(p26, p25),
     _voltage(p19),               // Voltage from sensor board
     _current(p20),               // Current from sensor board
     _left(p30),                  // left wheel encoder
@@ -111,7 +110,7 @@
     // TODO: get rid of state variable
     lrEncDistance  = (WHEEL_CIRC / WHEEL_STRIPES) * (double) leftCount;
     rrEncDistance = (WHEEL_CIRC / WHEEL_STRIPES) * (double) rightCount;
-    encDistance = (lrEncDistance + rrEncDistance) / 2.0;
+    //encDistance = (lrEncDistance + rrEncDistance) / 2.0;
     // compute speed from time between ticks
     int leftTime = _left.readTime();
     int rightTime = _right.readTime();

--- a/Sensors/Sensors.h	Tue May 28 13:58:35 2013 +0000
+++ b/Sensors/Sensors.h	Thu Jun 06 13:40:23 2013 +0000
@@ -32,6 +32,7 @@
 
 /** Sensor interface library abstracts sensor drivers, next step to a pluggable architecture */
 
+#include "Sirf3.h"
 #include "Ublox6.h"
 #include "Venus638flpx.h"
 #include "L3G4200D.h"
@@ -117,8 +118,9 @@
     float encDistance;                      // encoder distance since last check
     float encSpeed;                         // encoder calculated speed
 
-    Venus638flpx gps;                       // Venus GPS
-    //Ublox6 gps;                             // Ublox6 GPS
+    //Sirf3 gps;                              // Pharos SiRF III GPS
+    //Venus638flpx gps;                       // Venus GPS
+    Ublox6 gps;                             // Ublox6 GPS
     AnalogIn _voltage;                      // Voltage from sensor board
     AnalogIn _current;                      // Current from sensor board
     IncrementalEncoder _left;               // Left wheel encoder

--- a/globals.h	Tue May 28 13:58:35 2013 +0000
+++ b/globals.h	Thu Jun 06 13:40:23 2013 +0000
@@ -3,6 +3,8 @@
 
 /** Global parameters */
 
+#define PI 3.1415926535897932
+
 // Waypoint queue parameters
 #define MAXWPT    10
 #define ENDWPT    9999.0

--- a/logging/b64.cpp	Tue May 28 13:58:35 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,140 +0,0 @@
-/*********************************************************************
-
-MODULE NAME:    b64.c
-
-AUTHOR:         Bob Trower 08/04/01, Michael Shimniok 04/04/13
-
-COPYRIGHT:      Copyright (c) Trantor Standard Systems Inc., 2001
-
-NOTES:          This source code may be used as you wish, subject to
-                the MIT license.  See the LICENCE section below.
-
-                Canonical source should be at:
-                    http://base64.sourceforge.net
-
-DESCRIPTION:    Implements Base64 Content-Transfer-Encoding standard
-                described in RFC1113 (http://www.faqs.org/rfcs/rfc1113.html).
-
-                Groups of 3 binary bytes from a binary stream are coded as
-                groups of 4 printable bytes in a text stream.
-*********************************************************************/
-
-#include <stdio.h>
-#include <stdlib.h>
-#include "b64.h"
-
-/**
- * Translation Table as described in RFC1113
- */
-static const char cb64[]="ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
-
-/**
- * Translation Table to decode (created by author)
- */
-//static const char cd64[]="|$$$}rstuvwxyz{$$$$$$$>?@ABCDEFGHIJKLMNOPQRSTUVW$$$$$$XYZ[\\]^_`abcdefghijklmnopq";
-
-/**
- * returnable errors
- *
- * Error codes returned to the operating system.
- *
- */
-#define B64_SYNTAX_ERROR        1
-#define B64_FILE_ERROR          2
-#define B64_FILE_IO_ERROR       3
-#define B64_ERROR_OUT_CLOSE     4
-#define B64_LINE_SIZE_TO_MIN    5
-#define B64_SYNTAX_TOOMANYARGS  6
-
-/**
- * encodeblock
- *
- * encode 3 8-bit binary bytes as 4 '6-bit' characters
- */
-void encodeblock( const unsigned char *in, unsigned char *out, const int len )
-{
-    out[0] = (unsigned char) cb64[ (int)(in[0] >> 2) ];
-    out[1] = (unsigned char) cb64[ (int)(((in[0] & 0x03) << 4) | ((in[1] & 0xf0) >> 4)) ];
-    out[2] = (unsigned char) (len > 1 ? cb64[ (int)(((in[1] & 0x0f) << 2) | ((in[2] & 0xc0) >> 6)) ] : '=');
-    out[3] = (unsigned char) (len > 2 ? cb64[ (int)(in[2] & 0x3f) ] : '=');
-}
-
-/**
- * encodeblock
- *
- * encode a buffer of binary data into lines of '6-bit' characters
- */
-int encode( const unsigned char *sin, const int length, unsigned char *sout, const int linesize )
-{
-    unsigned char in[3];
-    int i;
-    int j; // j is the sin position index
-    int len, blocksout = 0;
-    int retcode = 0;
-
-    in[0] = 0;
-    j = 0;
-    while( j < length ) {
-        // Get a block of three bytes to encode
-        // If there's < 3 bytes left in the string,
-        // pad with '\0'
-        len = 0;
-        for (i=0; i < 3; i++, j++) {
-            if (j < length) {
-                len++;
-                in[i] = sin[j];
-            } else {
-                in[i] = '\0';
-            }
-        }
-        //printf("2. in=%02x %02x %02x j=%u length=%u\n", in[0], in[1], in[2], j, length);
-        encodeblock( in, sout, len );
-        //printf("4. sout=%c%c%c%c\n", sout[0], sout[1], sout[2], sout[3]);
-        sout += 4;
-        blocksout++;
-        if( linesize && blocksout >= (linesize/4) ) {
-            *sout++ = '\n';
-            blocksout = 0;
-        }
-    }
-    *sout = '\0';
-    return( retcode );
-}
-
-/******************************************************************************
-LICENCE:        Copyright (c) 2001 Bob Trower, Trantor Standard Systems Inc.
-
-                Permission is hereby granted, free of charge, to any person
-                obtaining a copy of this software and associated
-                documentation files (the "Software"), to deal in the
-                Software without restriction, including without limitation
-                the rights to use, copy, modify, merge, publish, distribute,
-                sublicense, and/or sell copies of the Software, and to
-                permit persons to whom the Software is furnished to do so,
-                subject to the following conditions:
-
-                The above copyright notice and this permission notice shall
-                be included in all copies or substantial portions of the
-                Software.
-
-                THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY
-                KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
-                WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
-                PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS
-                OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
-                OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
-                OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
-                SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
-
-VERSION HISTORY:
-                Bob Trower 08/04/01 -- Create Version 0.00.00B
-                Bob Trower 08/17/01 -- Correct documentation, messages.
-                                    -- Correct help for linesize syntax.
-                                    -- Force error on too many arguments.
-                Bob Trower 08/19/01 -- Add sourceforge.net reference to
-                                       help screen prior to release.
-                Bob Trower 10/22/04 -- Cosmetics for package/release.
-                Bob Trower 02/28/08 -- More Cosmetics for package/release.
-                Bob Trower 02/14/11 -- Cast altered to fix warning in VS6.
-
-*****************************************************************************/

--- a/logging/b64.h	Tue May 28 13:58:35 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,77 +0,0 @@
-/*********************************************************************
-
-MODULE NAME:    b64.c
-
-AUTHOR:         Bob Trower 08/04/01, Michael Shimniok 04/04/13
-
-COPYRIGHT:      Copyright (c) Trantor Standard Systems Inc., 2001
-
-NOTES:          This source code may be used as you wish, subject to
-                the MIT license.  See the LICENCE section below.
-
-                Canonical source should be at:
-                    http://base64.sourceforge.net
-
-DESCRIPTION:    Implements Base64 Content-Transfer-Encoding standard
-                described in RFC1113 (http://www.faqs.org/rfcs/rfc1113.html).
-
-                Groups of 3 binary bytes from a binary stream are coded as
-                groups of 4 printable bytes in a text stream.
-*********************************************************************/
-
-#ifndef __B64_H
-#define __B64_H
-
-/**
- * encodeblock
- *
- * encode 3 8-bit binary bytes as 4 '6-bit' characters
- */
-void encodeblock( const unsigned char *in, unsigned char *out, const int len );
-
-/**
- * encode
- *
- * encode a buffer of binary data into lines of '6-bit' characters
- */
-int encode( const unsigned char *sin, const int length, unsigned char *sout, const int linesize );
-
-#endif
-
-/******************************************************************************
-LICENCE:        Copyright (c) 2001 Bob Trower, Trantor Standard Systems Inc.
-
-                Permission is hereby granted, free of charge, to any person
-                obtaining a copy of this software and associated
-                documentation files (the "Software"), to deal in the
-                Software without restriction, including without limitation
-                the rights to use, copy, modify, merge, publish, distribute,
-                sublicense, and/or sell copies of the Software, and to
-                permit persons to whom the Software is furnished to do so,
-                subject to the following conditions:
-
-                The above copyright notice and this permission notice shall
-                be included in all copies or substantial portions of the
-                Software.
-
-                THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY
-                KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
-                WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
-                PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS
-                OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
-                OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
-                OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
-                SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
-
-VERSION HISTORY:
-                Bob Trower 08/04/01 -- Create Version 0.00.00B
-                Bob Trower 08/17/01 -- Correct documentation, messages.
-                                    -- Correct help for linesize syntax.
-                                    -- Force error on too many arguments.
-                Bob Trower 08/19/01 -- Add sourceforge.net reference to
-                                       help screen prior to release.
-                Bob Trower 10/22/04 -- Cosmetics for package/release.
-                Bob Trower 02/28/08 -- More Cosmetics for package/release.
-                Bob Trower 02/14/11 -- Cast altered to fix warning in VS6.
-
-*****************************************************************************/

--- a/logging/logging.cpp	Tue May 28 13:58:35 2013 +0000
+++ b/logging/logging.cpp	Thu Jun 06 13:40:23 2013 +0000
@@ -1,7 +1,6 @@
 #include "logging.h"
 #include "SDHCFileSystem.h"
 #include "SerialGraphicLCD.h"
-#include "b64.h"
 
 extern Serial pc;
 extern SerialGraphicLCD lcd;
@@ -212,7 +211,7 @@
     fputc('\n',logp);
 
     t2 = logtimer.read_us();
-    fprintf(stdout, "%d\n", t2-t1);
+    //fprintf(stdout, "%d\n", t2-t1);
 
     return;
 }

--- a/main.cpp	Tue May 28 13:58:35 2013 +0000
+++ b/main.cpp	Thu Jun 06 13:40:23 2013 +0000
@@ -55,6 +55,7 @@
 #define GYRO_CHAN        5
 
 // Chassis specific parameters
+// TODO 1 put WHEEL_CIRC, WHEELBASE, and TRACK in config.txt
 #define WHEEL_CIRC 0.321537             // m; calibrated with 4 12.236m runs. Measured 13.125" or 0.333375m circumference
 #define WHEELBASE  0.290
 #define TRACK      0.280
@@ -169,23 +170,31 @@
 {
     // Let's try setting priorities...
     NVIC_SetPriority(TIMER3_IRQn, 0);   // updater running off Ticker
-    NVIC_SetPriority(DMA_IRQn, 1);
-    NVIC_SetPriority(EINT0_IRQn, 5);    // wheel encoders
-    NVIC_SetPriority(EINT1_IRQn, 5);    // wheel encoders
-    NVIC_SetPriority(EINT2_IRQn, 5);    // wheel encoders
-    NVIC_SetPriority(EINT3_IRQn, 5);    // wheel encoders
-    NVIC_SetPriority(UART1_IRQn, 10);   // GPS p25,p26
-    NVIC_SetPriority(I2C0_IRQn, 20);    // sensors?
-    NVIC_SetPriority(I2C1_IRQn, 20);    // sensors?
-    NVIC_SetPriority(I2C2_IRQn, 20);    // sensors?
-    NVIC_SetPriority(UART3_IRQn, 30);   // LCD p17,p18
-    NVIC_SetPriority(UART0_IRQn, 30);   // USB
-    NVIC_SetPriority(ADC_IRQn, 40);     // Voltage/current
-    NVIC_SetPriority(TIMER0_IRQn, 255); // unused(?)
-    NVIC_SetPriority(TIMER1_IRQn, 255); // unused(?)
-    NVIC_SetPriority(TIMER2_IRQn, 255); // unused(?)
-    NVIC_SetPriority(SPI_IRQn, 255);    // uSD card, logging
+    NVIC_SetPriority(DMA_IRQn, 0);
+    NVIC_SetPriority(EINT0_IRQn, 0);    // wheel encoders
+    NVIC_SetPriority(EINT1_IRQn, 0);    // wheel encoders
+    NVIC_SetPriority(EINT2_IRQn, 0);    // wheel encoders
+    NVIC_SetPriority(EINT3_IRQn, 0);    // wheel encoders
+    NVIC_SetPriority(UART0_IRQn, 5);   // USB
+    NVIC_SetPriority(UART1_IRQn, 10);
+    NVIC_SetPriority(UART2_IRQn, 10);
+    NVIC_SetPriority(UART3_IRQn, 10);
+    NVIC_SetPriority(I2C0_IRQn, 10);    // sensors?
+    NVIC_SetPriority(I2C1_IRQn, 10);    // sensors?
+    NVIC_SetPriority(I2C2_IRQn, 10);    // sensors?
+    NVIC_SetPriority(ADC_IRQn, 10);     // Voltage/current
+    NVIC_SetPriority(TIMER0_IRQn, 10); // unused(?)
+    NVIC_SetPriority(TIMER1_IRQn, 10); // unused(?)
+    NVIC_SetPriority(TIMER2_IRQn, 10); // unused(?)
+    NVIC_SetPriority(SPI_IRQn, 10);    // uSD card, logging
 
+    // Something here is jacking up the I2C stuff
+    // Also when initializing with ESC powered, it causes motor to run which
+    // totally jacks up everything (noise?)
+    initSteering();
+    initThrottle();
+    // initFlasher();                                   // Initialize autonomous mode flasher
+    
     // Send data back to the PC
     pc.baud(115200);
     fprintf(stdout, "Data Bus 2013\n");
@@ -211,13 +220,6 @@
     if (config.load())                          // Load various configurable parameters, e.g., waypoints, declination, etc.
         confStatus = 1;
 
-    // Something here is jacking up the I2C stuff
-    // Also when initializing with ESC powered, it causes motor to run which
-    // totally jacks up everything (noise?)
-    initSteering();
-    initThrottle();
-    // initFlasher();                                   // Initialize autonomous mode flasher
-        
     sensors.Compass_Calibrate(config.magOffset, config.magScale);
     //pc.printf("Declination: %.1f\n", config.declination);
     pc.printf("Speed: escZero=%d escMax=%d top=%.1f turn=%.1f Kp=%.4f Ki=%.4f Kd=%.4f\n", 
@@ -313,7 +315,7 @@
             display.update(s);
             nextUpdate = thisUpdate + 2000;
             // TODO move this statistic into display class
-            fprintf(stdout, "update time: %d\n", getUpdateTime());
+            //fprintf(stdout, "update time: %d\n", getUpdateTime());
         }
         
         if (keypad.pressed) {
@@ -359,10 +361,9 @@
             fprintf(stdout, "%d) Display AHRS\n", i++);
             fprintf(stdout, "%d) Mavlink mode\n", i++);
             fprintf(stdout, "%d) Shell\n", i++);
-            fprintf(stdout, "%d) Bridge serial to 2nd GPS\n", i++);
             fprintf(stdout, "R) Reset\n");
             fprintf(stdout, "\nSelect from the above: ");
-            fflush(stdout);
+            //fflush(stdout);
             printMenu = false;
         }
 
@@ -840,7 +841,8 @@
     sensors.gps.enableVerbose();
     wait(2.0);
     //dev = &gps;
-    serial.attach(NULL, Serial::RxIrq);
+    sensors.gps.disable();
+    serial.baud(4800);
     while (!done) {
         if (pc.readable()) {
             x = pc.getc();
@@ -935,6 +937,8 @@
                     sensors.gps.latitude(), sensors.gps.longitude(), sensors.gps.heading_deg(), 
                     sensors.gps.speed_mps(), sensors.gps.hdop(), sensors.gps.sat_count(),
                     (unsigned char) sensors.gps.getAvailable() );
+                fprintf(stdout, "brg=%6.2f d=%8.4f sa=%6.2f\n", 
+                    state[inState].bearing, state[inState].distance, state[inState].steerAngle);
                 /*
                 fprintf(stdout, "gps2=(%.6f, %.6f, %.1f, %.1f, %.1f, %d) %02x\n", 
                     gps2.latitude(), gps2.longitude(), gps2.heading_deg(), gps2.speed_mps(), gps2.hdop(), gps2.sat_count(),

--- a/updater.cpp	Tue May 28 13:58:35 2013 +0000
+++ b/updater.cpp	Thu Jun 06 13:40:23 2013 +0000
@@ -1,4 +1,6 @@
 #include "mbed.h"
+#include "util.h"
+#include "globals.h"
 #include "updater.h"
 #include "Config.h"
 #include "Actuators.h"
@@ -25,6 +27,7 @@
 //#define LOG_SKIP 1  // 50ms, 20hz, log entry entered into fifo
 
 // The following is for main loop at 10ms = 100hz
+#define HDG_LAG 100
 #define CTRL_SKIP 5 // 50ms (20hz), control update
 #define MAG_SKIP 2  // 20ms (50hz), magnetometer update
 #define LOG_SKIP 2  // 20ms (50hz), log entry entered into fifo
@@ -98,10 +101,10 @@
     float dt;       // delta time
 } history[MAXHIST]; // fifo for sensor data, position, heading, dt
 
-int hCount;         // history counter; one > 100, we can go back in time to reference gyro history
+int hCount;         // history counter; one > HDG_LAG, we can go back in time to reference gyro history
 int now = 0;        // fifo input index, latest entry
 int prev = 0;       // previous fifo iput index, next to latest entry
-int lag = 0;        // fifo output index, entry from 1 second ago (100 entries prior)
+int lag = 0;        // fifo output index, entry from 1 second ago (HDG_LAG entries prior)
 int lagPrev = 0;    // previous fifo output index, 101 entries prior
 
 /** attach update to Ticker */
@@ -122,7 +125,7 @@
     return;
 }
 
-/** instruct the controller to arm, begin estimation */
+/** instruct the controller to start running */
 void beginRun()
 {
     go = true;
@@ -298,15 +301,16 @@
     history[now].dist = (sensors.lrEncDistance + sensors.rrEncDistance) / 2.0; // current distance traveled
     history[now].gyro = sensors.gyro[_z_];  // current raw gyro data
     history[now].dt = dt; // current dt, to address jitter
-    history[now].hdg = history[prev].hdg + dt*history[now].gyro; // compute current heading from current gyro
-    if (history[now].hdg >= 360.0) history[now].hdg -= 360.0;
-    if (history[now].hdg < 0)      history[now].hdg += 360.0;
+    history[now].hdg = history[prev].hdg + dt*(history[now].gyro - gyroBias); // compute current heading from current gyro
+    clamp(history[now].hdg, 0, 360.0);
+    //if (history[now].hdg >= 360.0) history[now].hdg -= 360.0;
+    //if (history[now].hdg < 0)      history[now].hdg += 360.0;
     float r = PI/180 * history[now].hdg;
     history[now].x = history[prev].x + history[now].dist * sin(r);    // update current position
     history[now].y = history[prev].y + history[now].dist * cos(r);
 
     // We can't do anything until the history buffer is full
-    if (hCount < 100) {
+    if (hCount < HDG_LAG) {
         estLagHeading = history[now].hdg;
         // Until the fifo is full, only keep track of current gyro heading
         hCount++; // after n iterations the fifo will be full
@@ -354,25 +358,33 @@
         // Heading is easy. Original heading - estimated heading gives a tiny error. Add this to all the historical
         // heading data up to present.
         //
-        // For position re-calculation, we iterate 100 times up to present record. Haversine is way, way too slow,
+        // For position re-calculation, we iterate HDG_LAG times up to present record. Haversine is way, way too slow,
         // trig calcs is marginal. Update rate is 10ms and we can't hog more than maybe 2-3ms as the outer
         // loop has logging work to do. Rotating each point is faster; pre-calculate sin/cos, etc. for rotation
         // matrix.
-        //
-        // initialize these once
-        errAngle = (estLagHeading - history[lag].hdg);
-        if (errAngle <= -180.0) errAngle += 360.0;
-        if (errAngle > 180) errAngle -= 360.0;
+
+        // Low pass filter the error correction.  Multiplying by 0.01, it takes HDG_LAG updates to correct a 
+        // consistent error; that's 0.10s/0.01 = 1 sec.  0.005 is 2 sec, 0.0025 is 4 sec, etc.
+        errAngle = (estLagHeading - history[lag].hdg); 
+        // low pass filter error angle:
+        clamp(errAngle, -180.0, 180.0);
+        //if (errAngle > 180.0) errAngle -= 360.0;
+        //if (errAngle <= -180.0) errAngle += 360.0;
+        errAngle *= 0.01; // multiply only after clamping to -180:180 range.
 
         //fprintf(stdout, "%d %.2f, %.2f, %.4f %.4f\n", lag, estLagHeading, history[lag].hdg, estLagHeading - history[lag].hdg, errAngle);
         float cosA = cos(errAngle * PI / 180.0);
         float sinA = sin(errAngle * PI / 180.0);
         // Update position & heading from history[lag] through history[now]
         int i = lag;
-        for (int j=0; j < 100; j++) {
+        // TODO 2 parameterize heading lag -- for uBlox it seems to be ~ 600ms, for Venus, about 1000ms
+        for (int j=0; j < HDG_LAG; j++) {
             //fprintf(stdout, "i=%d n=%d l=%d\n", i, now, lag);
             // Correct gyro-calculated headings from past to present including history[lag].hdg
             history[i].hdg += errAngle;
+            clamp(history[i].hdg, 0, 360.0);
+            //if (history[i].hdg >= 360.0) history[i].hdg -= 360.0;
+            //if (history[i].hdg < 0) history[i].hdg += 360.0;
             // Rotate x, y by errAngle around pivot point; no need to rotate pivot point (j=0)
             if (j > 0) {
                 float dx = history[i].x-history[lag].x;
@@ -399,9 +411,9 @@
     float prevDistance = cartHere.distanceTo(config.cwpt[lastWaypoint]);
     double relativeBrg = bearing - history[now].hdg;
     // if correction angle is < -180, express as negative degree
-    // TODO 3 turn this into a function
-    if (relativeBrg < -180.0) relativeBrg += 360.0;
-    if (relativeBrg > 180.0)  relativeBrg -= 360.0;
+    clamp(relativeBrg, -180.0, 180.0);
+    //if (relativeBrg < -180.0) relativeBrg += 360.0;
+    //if (relativeBrg > 180.0)  relativeBrg -= 360.0;
 
     // if within config.waypointDist distance threshold move to next waypoint
     // TODO 3 figure out how to output feedback on wpt arrival external to this function
@@ -410,10 +422,10 @@
         // if we're within brakeDist of next or previous waypoint, run @ turn speed
         // This would normally mean we run at turn speed until we're brakeDist away
         // from waypoint 0, but we trick the algorithm by initializing prevWaypoint to waypoint 1
-        if (distance < config.brakeDist || prevDistance < config.brakeDist) {
+        if ( (thisTime - timeZero) < 3000 ) {
+            setSpeed( config.startSpeed );
+        } else if (distance < config.brakeDist || prevDistance < config.brakeDist) {
             setSpeed( config.turnSpeed );
-        } else if ( (thisTime - timeZero) < 1000 ) {
-            setSpeed( config.startSpeed );
         } else {
             setSpeed( config.topSpeed );
         }
@@ -568,7 +580,7 @@
     state[inState].bearing = bearing;
     state[inState].distance = distance;
     state[inState].nextWaypoint = nextWaypoint;
-    //state[inState].gbias = gyroBias;
+    state[inState].gbias = gyroBias;
     state[inState].errAngle = biasErrAngle;
     //state[inState].leftRanger = sensors.leftRanger;
     //state[inState].rightRanger = sensors.rightRanger;

--- a/util.cpp	Tue May 28 13:58:35 2013 +0000
+++ b/util.cpp	Thu Jun 06 13:40:23 2013 +0000
@@ -1,3 +1,14 @@
+/** 
+ * Clamp a value (angle) between min (non-inclusive) and max (inclusive)
+ * e.g. clamp(v, 0, 360) or clamp(v, -180, 180)
+ */
+float clamp(float v, float min, float max) 
+{
+    float mod = (max - min);
+    if (v >= max) float -= mod;
+    if (v < min) float += mod;
+}
+
 // convert character to an int
 //
 int ctoi(char c)

--- a/util.h	Tue May 28 13:58:35 2013 +0000
+++ b/util.h	Thu Jun 06 13:40:23 2013 +0000
@@ -12,10 +12,15 @@
 #define M_PI 3.14159265358979323846
 #endif
 
+/** Clamp value between min (non-inclusive) and max (inclusive) */
+float clamp(float v, float min, float max);
+
 /** Convert char to integer */
 int ctoi(char c);
+
 /** Convert string to floating point */
 double cvstof(char *s);
+
 /** Tokenize a string 
  * @param s is the string to tokenize
  * @param max is the maximum number of characters