changeset 221:8e6ca2ead01a main

Latest updates.
author Bob Cook <bob@bobcookdev.com>
date Mon, 09 Sep 2013 20:03:31 -0700
parents 15d96880b4ba
children 8170f8bde4d1
files main/robots/odr/AvoidBySonarTask.cpp main/robots/odr/CruisingTask.cpp main/robots/odr/ImuReader.cpp main/robots/odr/NavigateTask.cpp main/robots/odr/ODRApp.cpp main/robots/odr/Scoreboard.cpp main/robots/odr/Scoreboard.h main/robots/odr/SquareCourseTask.cpp main/robots/odr/SquareCourseTask.h main/robots/odr/jamfile
diffstat 10 files changed, 591 insertions(+), 6 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr/AvoidBySonarTask.cpp	Sat Aug 24 16:57:22 2013 -0700
+++ b/main/robots/odr/AvoidBySonarTask.cpp	Mon Sep 09 20:03:31 2013 -0700
@@ -201,6 +201,8 @@
         return; // stuck!
     }
 
+    int8_t adjustedSpeed = Scoreboard::navMaximumSpeed();
+
     if ( m_frontCenterLeft < CENTER_AVOIDANCE_ZONE
                                          && m_frontCenterRight < CENTER_AVOIDANCE_ZONE )
     {
@@ -208,39 +210,46 @@
         {
             int8_t turn = CalculateTurnFromSideSensor( m_frontLeft );
             MotorsAndServos::servoPositions( -turn, turn );
+            adjustedSpeed -= ( 2 * turn );
         }
         else
         {
             int8_t turn = CalculateTurnFromSideSensor( m_frontRight );
             MotorsAndServos::servoPositions( turn, -turn );
+            adjustedSpeed -= ( 2 * turn );
         }
     }
     else if ( m_frontCenterLeft < CENTER_AVOIDANCE_ZONE )
     {
         int8_t turn = CalculateTurnFromCenterSensor( m_frontCenterLeft );
         MotorsAndServos::servoPositions( -turn, turn );
+        adjustedSpeed -= turn;
     }
     else if ( m_frontCenterRight < CENTER_AVOIDANCE_ZONE )
     {
         int8_t turn = CalculateTurnFromCenterSensor( m_frontCenterRight );
         MotorsAndServos::servoPositions( turn, -turn );
+        adjustedSpeed -= turn;
     }
     else if ( m_frontLeft < SIDE_AVOIDANCE_ZONE )
     {
         int8_t turn = CalculateTurnFromSideSensor( m_frontLeft );
         MotorsAndServos::servoPositions( -turn, turn );
+        adjustedSpeed -= turn;
     }
     else if ( m_frontRight < SIDE_AVOIDANCE_ZONE )
     {
         int8_t turn = CalculateTurnFromSideSensor( m_frontRight );
         MotorsAndServos::servoPositions( turn, -turn );
+        adjustedSpeed -= turn;
     }
     else
     {
         MotorsAndServos::servoPositions( 0, 0 ); // unexpected...
     }
 
-    MotorsAndServos::motorSpeeds( 15, 15 );
+    if ( adjustedSpeed < 5 ) { adjustedSpeed = 5; }
+    MotorsAndServos::motorSpeeds( adjustedSpeed, adjustedSpeed );
 }
 
 // ----------------------------------------------------------------------------------------
--- a/main/robots/odr/CruisingTask.cpp	Sat Aug 24 16:57:22 2013 -0700
+++ b/main/robots/odr/CruisingTask.cpp	Mon Sep 09 20:03:31 2013 -0700
@@ -73,7 +73,9 @@
     }
 
     MotorsAndServos::servoPositions( 0, 0 );
-    MotorsAndServos::motorSpeeds( 15, 15 );
+
+    int8_t speed = Scoreboard::navMaximumSpeed();
+    MotorsAndServos::motorSpeeds( speed, speed );
 }
 
 // ----------------------------------------------------------------------------------------
--- a/main/robots/odr/ImuReader.cpp	Sat Aug 24 16:57:22 2013 -0700
+++ b/main/robots/odr/ImuReader.cpp	Mon Sep 09 20:03:31 2013 -0700
@@ -40,7 +40,11 @@
 #include <termios.h>
 #include <unistd.h>
 
+#include <algorithm>
+#include <cmath>
 #include <stdexcept>
+#include <utility>
+#include <vector>
 
 #include <Poco/Exception.h>
 #include <Poco/Logger.h>
@@ -350,6 +354,176 @@
 }
 
 // ----------------------------------------------------------------------------------------
+//  Compute the calibrated yaw value from the uncalibrated (raw) input. This method uses
+//  data derived experimentally on a rotating table.
+//
+//  This data is in the range of 0 to 180, -180 to 0.
+//
+
+static int yaw_uncalibrated_values[ 36 ] = {
+      38,   46,   54,   61,   68,   76,   83,   90,   98,  105,
+     113,  121,  128,  137,  145,  154,  163,  172, -177, -166,
+    -155, -142, -129, -114,  -98,  -83,  -67,  -53,  -39,  -27,
+     -15,   -5,    4,   13,   22,   30
+};
+
+static double yaw_uncalibrated_offset = 18.0;
+
+typedef std::pair< double, double > yaw_cal_table_item_t;
+typedef std::vector< yaw_cal_table_item_t > yaw_cal_table_t;
+
+static yaw_cal_table_t yaw_cal_table;
+
+// ----------------------------------------------------------------------------------------
+//  Initialize the yaw calibration table using the translation data. This results in a
+//  table mapping between uncalibrated values and the corresponding "actual" value.
+//
+//  The table contains values in the range of 0 to 360, sorted by the uncalibrated values.
+
+static bool yaw_cal_table_item_sorter( yaw_cal_table_item_t a, yaw_cal_table_item_t b )
+{
+    return a.first < b.first;
+}
+
+static void initYawCalibrationTable()
+{
+    yaw_cal_table.clear();
+
+    for ( size_t i = 0; i < array_sizeof( yaw_uncalibrated_values ); ++i )
+    {
+        double v = static_cast< double >( yaw_uncalibrated_values[ i ] );
+        double a = yaw_uncalibrated_offset + ( static_cast< double >( i ) * 10.0 );
+
+        // adjust the uncalibrated data from the range of [-180,180] to [0,360]
+        v += 180.0;
+
+        yaw_cal_table.push_back( std::make_pair( v, a ) );
+    }
+
+    std::sort( yaw_cal_table.begin(), yaw_cal_table.end(), yaw_cal_table_item_sorter );
+}
+
+// ----------------------------------------------------------------------------------------
+
+static double getUncalibratedYawValueAtIndex( int index )
+{
+    while ( index < 0 )
+    {
+        index += yaw_cal_table.size();
+    }
+
+    index %= yaw_cal_table.size();
+
+    return yaw_cal_table[ index ].first;
+}
+
+// ----------------------------------------------------------------------------------------
+
+static double getCalibratedYawValueAtIndex( int index )
+{
+    while ( index < 0 )
+    {
+        index += yaw_cal_table.size();
+    }
+
+    index %= yaw_cal_table.size();
+
+    return yaw_cal_table[ index ].second;
+}
+
+// ----------------------------------------------------------------------------------------
+//  Translate the uncalibrated yaw value to a calibrated value. This is done by iterating
+//  through the calibration table to find the two values on either side of the input
+//  value then interpolating (linerally) between the two bounding points.
+//
+//  The only tricky consideration is where the two bounding points are on either side of
+//  the 0 degree (or 180 degree) boundry. To make this easier (only a single point to
+//  be concerned about) we do all the calculations in the range of [0, 360] then check
+//  for the "wrap-around" at the zero point.
+
+static double calibrateYawValue( double uncalibratedYaw )
+{
+    //--    Adjust the uncalibrated data from the range of [-180,180] to [0,360].
+
+    double inputUncalValue = uncalibratedYaw + 180.0;
+
+    //--    Find the index of the two bounding points around the uncalibrated value.
+
+    int indexUncalHi = 0;
+
+    for ( ; indexUncalHi < 36; ++indexUncalHi )
+    {
+        if ( getUncalibratedYawValueAtIndex( indexUncalHi ) > inputUncalValue ) break;
+    }
+
+    int indexUncalLo = indexUncalHi - 1;
+
+    double uncalHi = getUncalibratedYawValueAtIndex( indexUncalHi );
+    double uncalLo = getUncalibratedYawValueAtIndex( indexUncalLo );
+
+    //--    The case of two points on opposite sides of the origin (zero point) is slightly
+    //      more complicated.
+
+    if ( uncalHi - uncalLo > 180.0 || uncalLo - uncalHi > 180.0 )
+    {
+        //--    Wrap-around the zero point; shift to negative coordinates as needed and
+        //      recompute the fraction.
+
+        if ( uncalLo > 180.0 )
+        {
+            uncalLo -= 360.0;
+        }
+        else
+        {
+            uncalHi -= 360.0;
+        }
+
+        if ( inputUncalValue > 180.0 )
+        {
+            inputUncalValue -= 360.0;
+        }
+    }
+
+    double uncalFract = std::abs( inputUncalValue - uncalLo ) / std::abs( uncalLo - uncalHi );
+
+    //--    Fetch the calibrated values for interpolation.
+
+    double calLo = getCalibratedYawValueAtIndex( indexUncalLo );
+    double calHi = getCalibratedYawValueAtIndex( indexUncalHi );
+
+    //--    The case of two points on opposite sides of the origin (zero point) is slightly
+    //      more complicated.
+
+    if ( calHi - calLo > 180.0 || calLo - calHi > 180.0 )
+    {
+        //--    Wrap-around the zero point; shift to negative coordinates as needed.
+
+        if ( calLo > 180.0 )
+        {
+            calLo -= 360.0;
+        }
+        else
+        {
+            calHi -= 360.0;
+        }
+    }
+
+    if ( calLo > calHi )
+    {
+        std::swap( calLo, calHi );
+    }
+
+    double result = ( ( calHi - calLo ) * uncalFract ) + calLo;
+    result = ::fmod( result, 360.0 );
+
+    //--    Adjust the calibrated result from the range of [0, 360] to [-180,180].
+
+    result -= 180.0;
+
+    return result;
+}
+
+// ----------------------------------------------------------------------------------------
 //  Yaw values range from 0 to 180 then from -180 to 0 again. This means the values near
 //  zero are very close together (as the sensor turns) but so are values at 180/-180.
 //
@@ -423,6 +597,10 @@
     {
         log.information( std::string( "ImuReader::run() starting" ) );
 
+        // initialize the calibration table
+
+        initYawCalibrationTable();
+
         try
         {
             // to keep ourselves sane, start by assuming the IMU is offline
@@ -483,7 +661,7 @@
                 {
                     // new values available, save them
 
-                    valuesYaw.push_back( m_parseValueYaw );
+                    valuesYaw.push_back( calibrateYawValue( m_parseValueYaw ) );
 
                     // if we've reached 10 values, compute the average and update the
                     // scoreboard; then clear the vector making room for new values
--- a/main/robots/odr/NavigateTask.cpp	Sat Aug 24 16:57:22 2013 -0700
+++ b/main/robots/odr/NavigateTask.cpp	Mon Sep 09 20:03:31 2013 -0700
@@ -125,15 +125,19 @@
         servoPosition = 12;
     }
 
+#if 0
     log().information(
         Poco::Logger::format(
             "NavigateTask: heading: $0 diff: $1 steer: $2",
             Poco::NumberFormatter::format( Scoreboard::imuYaw() ),
             Poco::NumberFormatter::format( headingDiff ),
             Poco::NumberFormatter::format( servoPosition ) ) );
+#endif
 
     MotorsAndServos::servoPositions( -servoPosition, servoPosition );
-    MotorsAndServos::motorSpeeds( 15, 15 );
+
+    int8_t speed = Scoreboard::navMaximumSpeed();
+    MotorsAndServos::motorSpeeds( speed, speed );
 }
 
 // ----------------------------------------------------------------------------------------
--- a/main/robots/odr/ODRApp.cpp	Sat Aug 24 16:57:22 2013 -0700
+++ b/main/robots/odr/ODRApp.cpp	Mon Sep 09 20:03:31 2013 -0700
@@ -53,6 +53,7 @@
 #include "SafetyTask.h"
 #include "Scoreboard.h"
 #include "Sonar.h"
+#include "SquareCourseTask.h"
 #include "StartupTask.h"
 #include "TaskObject.h"
 
@@ -171,8 +172,11 @@
     m_tasks.push_back(
             Poco::SharedPtr< TaskObject >( new AvoidBySonarTask( logger().name() ) ) );
 
-//    m_tasks.push_back(
-//            Poco::SharedPtr< TaskObject >( new NavigateTask( logger().name() ) ) );
+    m_tasks.push_back(
+            Poco::SharedPtr< TaskObject >( new SquareCourseTask( logger().name() ) ) );
+
+    m_tasks.push_back(
+            Poco::SharedPtr< TaskObject >( new NavigateTask( logger().name() ) ) );
 
     m_tasks.push_back(
             Poco::SharedPtr< TaskObject >( new CruisingTask( logger().name() ) ) );
--- a/main/robots/odr/Scoreboard.cpp	Sat Aug 24 16:57:22 2013 -0700
+++ b/main/robots/odr/Scoreboard.cpp	Mon Sep 09 20:03:31 2013 -0700
@@ -62,12 +62,27 @@
 uint8_t         Scoreboard::sm_sonarFrontCenterLeft;
 uint8_t         Scoreboard::sm_sonarFrontCenterRight;
 uint8_t         Scoreboard::sm_sonarFrontRight;
+bool            Scoreboard::sm_isGpsAlive;
+bool            Scoreboard::sm_gpsHasFix;
+std::list< double > Scoreboard::sm_gpsLatitudes;
+double                Scoreboard::sm_gpsLatitudeAvg;
+std::list< double > Scoreboard::sm_gpsLongitudes;
+double                Scoreboard::sm_gpsLongitudeAvg;
 bool            Scoreboard::sm_isImuAlive;
 Poco::Timestamp Scoreboard::sm_imuLastUpdate;
 double          Scoreboard::sm_imuRoll;
 double          Scoreboard::sm_imuPitch;
 double          Scoreboard::sm_imuYaw;
 double          Scoreboard::sm_navTargetHeading = 100.0;
+int8_t          Scoreboard::sm_navMaximumSpeed = 30;
+
+// ----------------------------------------------------------------------------------------
+
+bool Scoreboard::isGpsSensorAlive()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_isGpsAlive;
+}
 
 // ----------------------------------------------------------------------------------------
 
@@ -188,6 +203,38 @@
 
 // ----------------------------------------------------------------------------------------
 
+bool Scoreboard::isGpsAlive()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_isGpsAlive;
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool Scoreboard::gpsHasFix()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_gpsHasFix;
+}
+
+// ----------------------------------------------------------------------------------------
+
+double Scoreboard::gpsLatitude()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_gpsLatitudeAvg;
+}
+
+// ----------------------------------------------------------------------------------------
+
+double Scoreboard::gpsLongitude()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_gpsLongitudeAvg;
+}
+
+// ----------------------------------------------------------------------------------------
+
 bool Scoreboard::isImuAlive()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
@@ -242,6 +289,22 @@
 
 // ----------------------------------------------------------------------------------------
 
+int8_t Scoreboard::navMaximumSpeed()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_navMaximumSpeed;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::navSetMaximumSpeed( int8_t max )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_navMaximumSpeed = max;
+}
+
+// ----------------------------------------------------------------------------------------
+
 double Scoreboard::navTargetHeading()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
@@ -262,16 +325,27 @@
     : Poco::Runnable(),
       m_loggerName( loggerName ),
       m_quitEvent( false /* not auto-reset */ ),
+      m_timerLogStatus(),
       m_timerSendManagerUpdate(),
+      m_timerGpsUpdate(),
       m_timerMotionUpdate(),
       m_timerControllerUpdate(),
       m_timerSonarFrontUpdate(),
       m_timerSendImuDataUpdate()
 {
+    m_timerLogStatus.setPeriodicInterval( 5000 );
+    m_timerLogStatus.start(
+            Poco::TimerCallback< Scoreboard >( *this, &Scoreboard::logSystemStatus ) );
+
     m_timerSendManagerUpdate.setPeriodicInterval( 2500 );
     m_timerSendManagerUpdate.start(
             Poco::TimerCallback< Scoreboard >( *this, &Scoreboard::sendManagerUpdate ) );
 
+    m_timerGpsUpdate.setPeriodicInterval( 5000 );
+    m_timerGpsUpdate.start(
+            Poco::TimerCallback< Scoreboard >(
+                    *this, &Scoreboard::timeoutGpsSensorUpdate ) );
+
     m_timerMotionUpdate.setPeriodicInterval( 5000 );
     m_timerMotionUpdate.start(
             Poco::TimerCallback< Scoreboard >(
@@ -307,6 +381,35 @@
 
 // ----------------------------------------------------------------------------------------
 
+void Scoreboard::logSystemStatus( Poco::Timer& timer )
+{
+    Poco::Logger& log = Poco::Logger::get( m_loggerName );
+
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+
+    if ( sm_isGpsAlive )
+    {
+        if ( sm_gpsHasFix )
+        {
+            log.information(
+                Poco::Logger::format(
+                    "Scoreboard: GPS latitude: $0 longitude: $1",
+                    Poco::NumberFormatter::format( sm_gpsLatitudeAvg ),
+                    Poco::NumberFormatter::format( sm_gpsLongitudeAvg ) ) );
+        }
+        else
+        {
+            log.information( "Scoreboard: GPS has no position fix" );
+        }
+    }
+    else
+    {
+        log.information( "Scoreboard: GPS is not alive" );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
 void Scoreboard::sendManagerUpdate( Poco::Timer& timer )
 {
     Poco::Logger& log = Poco::Logger::get( m_loggerName );
@@ -356,6 +459,14 @@
 
 // ----------------------------------------------------------------------------------------
 
+void Scoreboard::timeoutGpsSensorUpdate( Poco::Timer& timer )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_isGpsAlive = false;
+}
+
+// ----------------------------------------------------------------------------------------
+
 void Scoreboard::timeoutMotionUpdate( Poco::Timer& timer )
 {
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
@@ -478,6 +589,16 @@
             m_timerControllerUpdate.restart();
             sm_isControllerAlive = true;
             break;
+
+        case can_node_odr_motion:
+            m_timerMotionUpdate.restart();
+            sm_isMotionAlive = true;
+            break;
+
+        case can_node_sensor_gps:
+            m_timerGpsUpdate.restart();
+            sm_isGpsAlive = true;
+            break;
     }
 }
 
@@ -538,6 +659,79 @@
 
 // ----------------------------------------------------------------------------------------
 
+void Scoreboard::recvGpsFixInfo( CANMessage* msg )
+{
+    can_data_gps_fix info;
+    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
+
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_gpsHasFix = info.satellites > 0;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::recvGpsLatitude( CANMessage* msg )
+{
+    can_data_gps_data info;
+    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
+
+    double lat = static_cast< double >( info.degrees );
+    double min = static_cast< double >( info.min_thousandths )
+               / static_cast< double >( can_data_gps_min_multiplier );
+    lat += ( min * 60.0L );
+    
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+
+    sm_gpsLatitudes.push_back( lat );
+    while ( sm_gpsLatitudes.size() > 10 )
+    {
+        sm_gpsLatitudes.pop_front();
+    }
+
+    std::list< double >::iterator itr = sm_gpsLatitudes.begin();
+    double latsTotal = 0.0;
+
+    while ( itr != sm_gpsLatitudes.end() )
+    {
+        latsTotal += *itr++;
+    }
+
+    sm_gpsLatitudeAvg = latsTotal / sm_gpsLatitudes.size();
+}
+
+// ----------------------------------------------------------------------------------------
+
+void Scoreboard::recvGpsLongitude( CANMessage* msg )
+{
+    can_data_gps_data info;
+    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
+
+    double lon = static_cast< double >( info.degrees );
+    double min = static_cast< double >( info.min_thousandths )
+               / static_cast< double >( can_data_gps_min_multiplier );
+    lon += ( min * 60.0L );
+    
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+
+    sm_gpsLongitudes.push_back( lon );
+    while ( sm_gpsLongitudes.size() > 10 )
+    {
+        sm_gpsLongitudes.pop_front();
+    }
+
+    std::list< double >::iterator itr = sm_gpsLongitudes.begin();
+    double lonsTotal = 0.0;
+
+    while ( itr != sm_gpsLongitudes.end() )
+    {
+        lonsTotal += *itr++;
+    }
+
+    sm_gpsLongitudeAvg = lonsTotal / sm_gpsLongitudes.size();
+}
+
+// ----------------------------------------------------------------------------------------
+
 void Scoreboard::run()
 {
     Poco::Logger& log = Poco::Logger::get( m_loggerName );
@@ -604,6 +798,18 @@
                         recvSonarFrontStatus( true /* enabled */ );
                         recvSonarFrontValues( msg );
                         break;
+
+                    case can_dataid_gps_fix:
+                        recvGpsFixInfo( msg );
+                        break;
+
+                    case can_dataid_latitude:
+                        recvGpsLatitude( msg );
+                        break;
+
+                    case can_dataid_longitude:
+                        recvGpsLongitude( msg );
+                        break;
                 }
             }
         }
--- a/main/robots/odr/Scoreboard.h	Sat Aug 24 16:57:22 2013 -0700
+++ b/main/robots/odr/Scoreboard.h	Mon Sep 09 20:03:31 2013 -0700
@@ -33,6 +33,7 @@
 #define BCDRL_ROBOTS_ODR_SCOREBOARD_H
 
 #include <stdint.h>
+#include <list>
 
 #include <Poco/Event.h>
 #include <Poco/Runnable.h>
@@ -47,6 +48,9 @@
 class Scoreboard : public Poco::Runnable
 {
     public:
+        // odr-gps
+        static bool isGpsSensorAlive();
+
         // odr-motion
         static bool isMotionAlive();
 
@@ -65,6 +69,12 @@
         static uint8_t sonarFrontCenterRight();
         static uint8_t sonarFrontRight();
 
+        // odr-gps
+        static bool   isGpsAlive();
+        static bool   gpsHasFix();
+        static double gpsLatitude();
+        static double gpsLongitude();
+
         // imu
         static bool   isImuAlive();
         static double imuRoll();
@@ -74,6 +84,8 @@
         static void   imuIsOffline();
 
         // navigation
+        static int8_t navMaximumSpeed();
+        static void   navSetMaximumSpeed( int8_t max );
         static double navTargetHeading();
         static void   navSetTargetHeading( double heading );
 
@@ -100,24 +112,35 @@
         static uint8_t         sm_sonarFrontCenterLeft;
         static uint8_t         sm_sonarFrontCenterRight;
         static uint8_t         sm_sonarFrontRight;
+        static bool            sm_isGpsAlive;
+        static bool            sm_gpsHasFix;
+        static std::list< double > sm_gpsLatitudes;
+        static double                sm_gpsLatitudeAvg;
+        static std::list< double > sm_gpsLongitudes;
+        static double                sm_gpsLongitudeAvg;
         static bool            sm_isImuAlive;
         static Poco::Timestamp sm_imuLastUpdate;
         static double          sm_imuRoll;
         static double          sm_imuPitch;
         static double          sm_imuYaw;
+        static int8_t          sm_navMaximumSpeed;
         static double          sm_navTargetHeading;
 
     private:
         std::string m_loggerName;
         Poco::Event m_quitEvent;
+        Poco::Timer m_timerLogStatus;
         Poco::Timer m_timerSendManagerUpdate;
+        Poco::Timer m_timerGpsUpdate;
         Poco::Timer m_timerMotionUpdate;
         Poco::Timer m_timerControllerUpdate;
         Poco::Timer m_timerSonarFrontUpdate;
         Poco::Timer m_timerSendImuDataUpdate;
 
     private:
+        void logSystemStatus( Poco::Timer& timer );
         void sendManagerUpdate( Poco::Timer& timer );
+        void timeoutGpsSensorUpdate( Poco::Timer& timer );
         void timeoutMotionUpdate( Poco::Timer& timer );
         void timeoutControllerUpdate( Poco::Timer& timer );
         void timeoutSonarFrontUpdate( Poco::Timer& timer );
@@ -129,6 +152,9 @@
         void recvButtonUpdate( CANMessage* msg );
         void recvSonarFrontStatus( bool enabled );
         void recvSonarFrontValues( CANMessage* msg );
+        void recvGpsFixInfo( CANMessage* msg );
+        void recvGpsLatitude( CANMessage* msg );
+        void recvGpsLongitude( CANMessage* msg );
 };
 
 // ----------------------------------------------------------------------------------------
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/SquareCourseTask.cpp	Mon Sep 09 20:03:31 2013 -0700
@@ -0,0 +1,99 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/SquareCourseTask.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Subsumption task to navigate in a square shape.
+//
+//  Copyright (c) 2013 Bob Cook
+//
+//  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.
+//
+// ----------------------------------------------------------------------------------------
+
+#include "SquareCourseTask.h"
+
+#include <cmath>
+
+#include <Poco/Logger.h>
+#include <Poco/NumberFormatter.h>
+
+#include "MotorsAndServos.h"
+#include "Scoreboard.h"
+
+// ----------------------------------------------------------------------------------------
+
+static const Poco::Timestamp::TimeDiff sRunTimeUntilDirectionChange
+                                                      = Poco::Timestamp::resolution() * 60;
+
+// ----------------------------------------------------------------------------------------
+
+SquareCourseTask::SquareCourseTask( const std::string& loggerName )
+    : TaskObject( loggerName ),
+      m_heading( 0.0 ),
+      m_nextSwitchTime()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+SquareCourseTask::~SquareCourseTask()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+void SquareCourseTask::update()
+{
+    // nothing to do here
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool SquareCourseTask::wantsControl()
+{
+    if ( m_nextSwitchTime.isElapsed( sRunTimeUntilDirectionChange ) )
+    {
+        m_nextSwitchTime.update();
+
+        m_heading += 90.0;
+        if ( m_heading > 359.0 ) { m_heading = 0.0; }
+
+        log().information(
+            Poco::Logger::format(
+                "SquareCourseTask: new heading $0",
+                Poco::NumberFormatter::format( m_heading ) ) );
+
+        Scoreboard::navSetTargetHeading( m_heading );
+    }
+
+    return false;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void SquareCourseTask::takeControl()
+{
+    // nothing to do here
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/SquareCourseTask.h	Mon Sep 09 20:03:31 2013 -0700
@@ -0,0 +1,56 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/SquareCourseTask.h
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Subsumption task to navigate in a square shape.
+//
+//  Copyright (c) 2013 Bob Cook
+//
+//  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.
+//
+// ----------------------------------------------------------------------------------------
+
+#ifndef BCDRL_ROBOTS_ODR_SQUARECOURSETASK_H
+#define BCDRL_ROBOTS_ODR_SQUARECOURSETASK_H
+
+#include "TaskObject.h"
+
+// ----------------------------------------------------------------------------------------
+
+class SquareCourseTask : public TaskObject
+{
+    public:
+        SquareCourseTask( const std::string& loggerName );
+        virtual ~SquareCourseTask();
+        virtual void update();
+        virtual bool wantsControl();
+        virtual void takeControl();
+
+    private:
+        double          m_heading;
+        Poco::Timestamp m_nextSwitchTime;
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_ODR_SQUARECOURSETASK_H
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr/jamfile	Sat Aug 24 16:57:22 2013 -0700
+++ b/main/robots/odr/jamfile	Mon Sep 09 20:03:31 2013 -0700
@@ -40,6 +40,7 @@
     ImuReader.cpp MotorsAndServos.cpp Scoreboard.cpp Sonar.cpp
     TaskObject.cpp
     AvoidBySonarTask.cpp CruisingTask.cpp NavigateTask.cpp SafetyTask.cpp StartupTask.cpp
+    SquareCourseTask.cpp
     packages.common.can.pkg
     packages.linux.can.pkg
     ;