changeset 228:2429999d8731 main

Merge/commit from upstream.
author Bob Cook <bob@bobcookdev.com>
date Mon, 07 Jul 2014 18:59:11 -0700
parents 03064d391e79 (diff) 74381e5dd204 (current diff)
children 79283dc7dc54
files main/packages/common/can/can_messages.h
diffstat 23 files changed, 1324 insertions(+), 59 deletions(-) [+]
line wrap: on
line diff
--- a/main/packages/common/can/can_messages.h	Mon Jul 07 18:42:15 2014 -0700
+++ b/main/packages/common/can/can_messages.h	Mon Jul 07 18:59:11 2014 -0700
@@ -34,6 +34,8 @@
 
 #include <stdint.h>
 
+#pragma pack(push,1)
+
 // ----------------------------------------------------------------------------------------
 //  Overall scheme (lower is higher priority):
 // 
@@ -205,6 +207,10 @@
 } can_data_sonar_front;
 
 // ----------------------------------------------------------------------------------------
+
+#pragma pack(pop)
+
+// ----------------------------------------------------------------------------------------
 #endif  // #if !defined( BCDRL_COMMON_CAN_CAN_MESSAGES_H )
 // ----------------------------------------------------------------------------------------
 
--- a/main/robots/cantool/CmdListen.cpp	Mon Jul 07 18:42:15 2014 -0700
+++ b/main/robots/cantool/CmdListen.cpp	Mon Jul 07 18:59:11 2014 -0700
@@ -88,10 +88,11 @@
     msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
 
     int    deg = info.degrees;
-    double min = static_cast< double >( info.min_thousandths ) / can_data_gps_min_multiplier;
+    double min = static_cast< double >( info.min_thousandths )
+               / can_data_gps_min_multiplier;
 
     std::cout << "gps latitude:  " << Poco::NumberFormatter::format( deg )
-        << " " << Poco::NumberFormatter::format( min )
+        << " " << Poco::NumberFormatter::format( min, 4 )
         << std::endl;
 }
 
@@ -103,10 +104,11 @@
     msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );
 
     int    deg = info.degrees;
-    double min = static_cast< double >( info.min_thousandths ) / can_data_gps_min_multiplier;
+    double min = static_cast< double >( info.min_thousandths )
+               / can_data_gps_min_multiplier;
 
     std::cout << "gps longitude: " << Poco::NumberFormatter::format( deg )
-        << " " << Poco::NumberFormatter::format( min )
+        << " " << Poco::NumberFormatter::format( min, 4 )
         << std::endl;
 }
 
@@ -183,10 +185,12 @@
 
             case can_dataid_latitude:
                 MsgGpsLatitude( msg );
+                std::cout << "message: " << CANMessage::asText( msg ) << std::endl;
                 break;
 
             case can_dataid_longitude:
                 MsgGpsLongitude( msg );
+                std::cout << "message: " << CANMessage::asText( msg ) << std::endl;
                 break;
 
             case can_dataid_gps_fix:
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-sim/ImuStatus.cpp	Mon Jul 07 18:59:11 2014 -0700
@@ -0,0 +1,194 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr-sim/ImuStatus.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Thread that periodically sends the IMU details.
+//
+//  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 "ImuStatus.h"
+
+#include "SimDisplay.h"
+
+#include "packages/common/can/can_helpers.h"
+#include "packages/common/can/can_messages.h"
+#include "packages/common/can/can_nodes.h"
+
+#include "packages/linux/can/CANMessage.h"
+
+// ----------------------------------------------------------------------------------------
+
+static void SendImuDataMessages()
+{
+#if 0
+    uint32_t msgid;
+
+    if ( SonarFrontStatus::isSonarFrontEnabled() )
+    {
+        msgid = can_build_message_id( can_node_odr_sonar_front,
+                                      can_node_broadcast,
+                                      can_dataid_sonar_front_state_enabled );
+    }
+    else
+    {
+        msgid = can_build_message_id( can_node_odr_sonar_front,
+                                      can_node_broadcast,
+                                      can_dataid_sonar_front_state_disabled );
+    }
+    CANMessage::QueueToSend( new CANMessage( msgid ) );
+#endif
+}
+
+// ----------------------------------------------------------------------------------------
+
+Poco::RWLock ImuStatus::sm_rwLock;
+bool         ImuStatus::sm_isEnabled = false;
+double       ImuStatus::sm_yaw       = 0.0;
+double       ImuStatus::sm_pitch     = 0.0;
+double       ImuStatus::sm_roll      = 0.0;
+
+// ----------------------------------------------------------------------------------------
+
+bool ImuStatus::isImuEnabled()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_isEnabled;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuStatus::setImuEnabled()
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_isEnabled = true;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuStatus::setImuDisabled()
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_isEnabled = false;
+}
+
+// ----------------------------------------------------------------------------------------
+
+double ImuStatus::imuYaw()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_yaw;
+}
+
+// ----------------------------------------------------------------------------------------
+
+double ImuStatus::imuPitch()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_pitch;
+}
+
+// ----------------------------------------------------------------------------------------
+
+double ImuStatus::imuRoll()
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+    return sm_roll;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuStatus::setImuYaw( double value )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_yaw = value;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuStatus::setImuPitch( double value )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_pitch = value;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuStatus::setImuRoll( double value )
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_roll = value;
+}
+
+// ----------------------------------------------------------------------------------------
+
+ImuStatus::ImuStatus()
+    : Poco::Runnable(),
+      m_quitEvent( true /* auto-reset */ )
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+ImuStatus::~ImuStatus()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuStatus::timeToQuit()
+{
+    m_quitEvent.set();
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuStatus::run()
+{
+    for ( ;; )
+    {
+        try
+        {
+//                SendSonarFrontStatusMessage();
+
+//            if ( isSonarFrontEnabled() )
+//            {
+//                SendSonarFrontValueMessage();
+//            }
+        }
+        catch ( ... )
+        {
+            // nothing to do, but don't stop the thread
+        }
+
+        if ( m_quitEvent.tryWait( 500 ) ) // 500ms = 1/2 second
+        {
+            return;
+        }
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-sim/ImuStatus.h	Mon Jul 07 18:59:11 2014 -0700
@@ -0,0 +1,77 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr-sim/ImuStatus.h
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Thread that periodically sends the IMU details.
+//
+//  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_ODRSIM_IMUSTATUS_H
+#define BCDRL_ROBOTS_ODRSIM_IMUSTATUS_H
+
+#include <Poco/Event.h>
+#include <Poco/Runnable.h>
+#include <Poco/RWLock.h>
+
+// ----------------------------------------------------------------------------------------
+
+class ImuStatus : public Poco::Runnable
+{
+    public:
+        static bool   isImuEnabled();
+        static void   setImuEnabled();
+        static void   setImuDisabled();
+        static double imuYaw();
+        static double imuPitch();
+        static double imuRoll();
+        static void   setImuYaw( double value );
+        static void   setImuPitch( double value );
+        static void   setImuRoll( double value );
+
+    public:
+        ImuStatus();
+        virtual ~ImuStatus();
+        virtual void run();
+        void timeToQuit();
+
+    private:
+        static void sendImuData();
+
+    private:
+        static Poco::RWLock sm_rwLock;
+        static bool         sm_isEnabled;
+        static double       sm_yaw;
+        static double       sm_pitch;
+        static double       sm_roll;
+
+    private:
+        Poco::Event m_quitEvent;
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_ODRSIM_IMUSTATUS_H
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr-sim/Receiver.cpp	Mon Jul 07 18:42:15 2014 -0700
+++ b/main/robots/odr-sim/Receiver.cpp	Mon Jul 07 18:59:11 2014 -0700
@@ -7,7 +7,7 @@
 // 
 //  Update status from incoming CAN messages.
 //
-//  Copyright (c) 2011 Bob Cook
+//  Copyright (c) 2011-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
@@ -91,12 +91,12 @@
 
 void Receiver::recvMotorSpeed( CANMessage* msg )
 {
-    can_data_motor_speed data;
+    can_data_wheel_speed data;
     msg->msgData( reinterpret_cast< uint8_t* >( &data ), sizeof( data ) );
 
     if ( m_display )
     {
-        m_display->updateMotorSpeeds( data.motor_front, data.motor_rear );
+        m_display->updateMotorSpeeds( data.rpm_front, data.rpm_rear );
     }
 }
 
@@ -104,12 +104,12 @@
 
 void Receiver::recvServoPosition( CANMessage* msg )
 {
-    can_data_servo_position data;
+    can_data_wheel_position data;
     msg->msgData( reinterpret_cast< uint8_t* >( &data ), sizeof( data ) );
 
     if ( m_display )
     {
-        m_display->updateServoPositions( data.servo_front, data.servo_rear );
+        m_display->updateServoPositions( data.angle_front, data.angle_rear );
     }
 }
 
@@ -165,11 +165,11 @@
                     recvMgrUpdate( msg );
                     break;
 
-                case can_dataid_motor_speed:
+                case can_dataid_wheel_speed:
                     recvMotorSpeed( msg );
                     break;
 
-                case can_dataid_servo_position:
+                case can_dataid_wheel_position:
                     recvServoPosition( msg );
                     break;
 
--- a/main/robots/odr-sim/SimDisplay.cpp	Mon Jul 07 18:42:15 2014 -0700
+++ b/main/robots/odr-sim/SimDisplay.cpp	Mon Jul 07 18:59:11 2014 -0700
@@ -7,7 +7,7 @@
 //
 //  Window display for the ODR platform simulator.
 //
-//  Copyright (c) 2011 Bob Cook
+//  Copyright (c) 2011-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
@@ -37,6 +37,7 @@
 #include <FL/Fl.H>
 #include <FL/Fl_Box.H>
 #include <FL/Fl_Button.H>
+#include <FL/Fl_Dial.H>
 #include <FL/Fl_Double_Window.H>
 #include <FL/Fl_Hor_Slider.H>
 #include <FL/Fl_Light_Button.H>
@@ -48,6 +49,7 @@
 #include <Poco/Thread.h>
 
 #include "ControllerStatus.h"
+#include "ImuStatus.h"
 #include "SonarFrontStatus.h"
 
 // ----------------------------------------------------------------------------------------
@@ -130,6 +132,89 @@
 
 // ----------------------------------------------------------------------------------------
 
+static void ImuKeepAliveCB( Fl_Widget* widget, void* value )
+{
+    static ImuStatus    s_imuStatusRunnable;
+    static Poco::Thread s_imuStatusThread;
+
+    Fl_Light_Button* button = dynamic_cast< Fl_Light_Button* >( widget );
+    if ( button == 0 )
+    {
+        return; // opps, not what we thought?!
+    }
+
+    if ( button->value() == 0 )
+    {
+        // off
+        s_imuStatusRunnable.timeToQuit();
+        s_imuStatusThread.join();
+    }
+    else
+    {
+        // on
+        s_imuStatusThread.start( s_imuStatusRunnable );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+static void ImuHeadingDialCB( Fl_Widget* widget, void* value )
+{
+    Fl_Dial* dial = dynamic_cast< Fl_Dial* >( widget );
+    if ( dial == 0 )
+    {
+        return; // opps, not what we thought?!
+    }
+
+    ImuStatus::setImuYaw( dial->value() );
+
+    SimDisplay* theDisplay = reinterpret_cast< SimDisplay* >( value );
+    if ( theDisplay )
+    {
+        theDisplay->updateImuValues();
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+static void ImuPitchSliderCB( Fl_Widget* widget, void* value )
+{
+    Fl_Slider* slider = dynamic_cast< Fl_Slider* >( widget );
+    if ( slider == 0 )
+    {
+        return; // opps, not what we thought?!
+    }
+
+    ImuStatus::setImuPitch( slider->value() );
+
+    SimDisplay* theDisplay = reinterpret_cast< SimDisplay* >( value );
+    if ( theDisplay )
+    {
+        theDisplay->updateImuValues();
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+static void ImuRollSliderCB( Fl_Widget* widget, void* value )
+{
+    Fl_Slider* slider = dynamic_cast< Fl_Slider* >( widget );
+    if ( slider == 0 )
+    {
+        return; // opps, not what we thought?!
+    }
+
+    ImuStatus::setImuRoll( slider->value() );
+
+    SimDisplay* theDisplay = reinterpret_cast< SimDisplay* >( value );
+    if ( theDisplay )
+    {
+        theDisplay->updateImuValues();
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
 static void SonarFrontKeepAliveCB( Fl_Widget* widget, void* value )
 {
     static SonarFrontStatus s_sfsRunnable;
@@ -490,11 +575,65 @@
 
 // ----------------------------------------------------------------------------------------
 
+void SimDisplay::createImuControls()
+{
+    m_window->begin();
+
+    m_buttonImuKA = new Fl_Light_Button( 250, 150, 165, 20 );
+    if ( m_buttonImuKA )
+    {
+        m_buttonImuKA->label( "IMU Alive" );
+        m_buttonImuKA->selection_color( FL_GREEN );
+        m_buttonImuKA->callback( &ImuKeepAliveCB );
+    }
+
+    m_textImuValues = new Fl_Box( 250, 180, 165, 20 );
+    if ( m_textImuValues )
+    {
+        m_textImuValues->box( FL_THIN_DOWN_BOX );
+        m_textImuValues->align( FL_ALIGN_CENTER );
+        m_textImuValues->label( "0.0  0.0  0.0" );
+    }
+
+    m_dialImuHeading = new Fl_Dial( 250, 210, 75, 75 );
+    if ( m_dialImuHeading )
+    {
+        m_dialImuHeading->box( FL_ROUND_UP_BOX );
+        m_dialImuHeading->bounds( 0.0, 360.0 );
+        m_dialImuHeading->value( ImuStatus::imuYaw() );
+        m_dialImuHeading->callback( ImuHeadingDialCB, this );
+    }
+
+    m_sliderImuPitch = new Fl_Slider( 335, 210, 35, 60 );
+    if ( m_sliderImuPitch )
+    {
+        m_sliderImuPitch->label( "Pitch" );
+        m_sliderImuPitch->slider( FL_UP_BOX );
+        m_sliderImuPitch->bounds( 0.0, 20.0 );
+        m_sliderImuPitch->value( ImuStatus::imuPitch() );
+        m_sliderImuPitch->callback( ImuPitchSliderCB, this );
+    }
+
+    m_sliderImuRoll = new Fl_Slider( 380, 210, 35, 60 );
+    if ( m_sliderImuRoll )
+    {
+        m_sliderImuRoll->label( "Roll" );
+        m_sliderImuRoll->slider( FL_UP_BOX );
+        m_sliderImuRoll->bounds( 0.0, 20.0 );
+        m_sliderImuRoll->value( ImuStatus::imuRoll() );
+        m_sliderImuRoll->callback( ImuRollSliderCB, this );
+    }
+
+    m_window->end();
+}
+
+// ----------------------------------------------------------------------------------------
+
 void SimDisplay::createSonarFrontControls()
 {
     m_window->begin();
 
-    m_buttonSonarFrontKA = new Fl_Light_Button( 250, 150, 165, 20 );
+    m_buttonSonarFrontKA = new Fl_Light_Button( 250, 310, 165, 20 );
     if ( m_buttonSonarFrontKA )
     {
         m_buttonSonarFrontKA->label( "Sonar Front Alive" );
@@ -502,7 +641,7 @@
         m_buttonSonarFrontKA->callback( &SonarFrontKeepAliveCB );
     }
 
-    m_textSonarFront = new Fl_Box( 250, 180, 165, 20 );
+    m_textSonarFront = new Fl_Box( 250, 340, 165, 20 );
     if ( m_textSonarFront )
     {
         m_textSonarFront->box( FL_THIN_DOWN_BOX );
@@ -510,42 +649,42 @@
         m_textSonarFront->label( "0F00 0F00 0F00 0F00" );
     }
 
-    m_sliderSonarFrontL = new Fl_Slider( 250, 210, 35, 100 );
+    m_sliderSonarFrontL = new Fl_Slider( 250, 370, 35, 100 );
     if ( m_sliderSonarFrontL )
     {
         m_sliderSonarFrontL->label( "Left" );
         m_sliderSonarFrontL->slider( FL_UP_BOX );
-        m_sliderSonarFrontL->bounds( 0x0fff, 0 );
+        m_sliderSonarFrontL->bounds( 0xff, 0 );
         m_sliderSonarFrontL->value( SonarFrontStatus::sonarFrontLeft() );
         m_sliderSonarFrontL->callback( SonarFrontSliderLeftCB, this );
     }
 
-    m_sliderSonarFrontCL = new Fl_Slider( 293, 210, 35, 100 );
+    m_sliderSonarFrontCL = new Fl_Slider( 293, 370, 35, 100 );
     if ( m_sliderSonarFrontCL )
     {
         m_sliderSonarFrontCL->label( "C Left" );
         m_sliderSonarFrontCL->slider( FL_UP_BOX );
-        m_sliderSonarFrontCL->bounds( 0x0fff, 0 );
+        m_sliderSonarFrontCL->bounds( 0xff, 0 );
         m_sliderSonarFrontCL->value( SonarFrontStatus::sonarFrontCenterLeft() );
         m_sliderSonarFrontCL->callback( SonarFrontSliderCenterLeftCB, this );
     }
 
-    m_sliderSonarFrontCR = new Fl_Slider( 336, 210, 35, 100 );
+    m_sliderSonarFrontCR = new Fl_Slider( 336, 370, 35, 100 );
     if ( m_sliderSonarFrontCR )
     {
         m_sliderSonarFrontCR->label( "C Right" );
         m_sliderSonarFrontCR->slider( FL_UP_BOX );
-        m_sliderSonarFrontCR->bounds( 0x0fff, 0 );
+        m_sliderSonarFrontCR->bounds( 0xff, 0 );
         m_sliderSonarFrontCR->value( SonarFrontStatus::sonarFrontCenterRight() );
         m_sliderSonarFrontCR->callback( SonarFrontSliderCenterRightCB, this );
     }
 
-    m_sliderSonarFrontR = new Fl_Slider( 380, 210, 35, 100 );
+    m_sliderSonarFrontR = new Fl_Slider( 380, 370, 35, 100 );
     if ( m_sliderSonarFrontR )
     {
         m_sliderSonarFrontR->label( "Right" );
         m_sliderSonarFrontR->slider( FL_UP_BOX );
-        m_sliderSonarFrontR->bounds( 0x0fff, 0 );
+        m_sliderSonarFrontR->bounds( 0xff, 0 );
         m_sliderSonarFrontR->value( SonarFrontStatus::sonarFrontRight() );
         m_sliderSonarFrontR->callback( SonarFrontSliderRightCB, this );
     }
@@ -565,6 +704,7 @@
         createServoSliders();
         createDisplays();
         createActionButtons();
+        createImuControls();
         createSonarFrontControls();
 
         m_window->show();
@@ -757,19 +897,40 @@
 
 // ----------------------------------------------------------------------------------------
 
+void SimDisplay::updateImuValues()
+{
+    Fl::lock();
+
+    if ( m_textImuValues )
+    {
+        char buf[ 80 ];
+        snprintf( buf, sizeof( buf ), "%3.1lf  %3.1lf  %3.1lf",
+                  ImuStatus::imuYaw(),
+                  ImuStatus::imuPitch(),
+                  ImuStatus::imuRoll() );
+        m_textImuValues->label( buf );
+    }
+
+    Fl::flush();
+
+    Fl::unlock();
+}
+
+// ----------------------------------------------------------------------------------------
+
 void SimDisplay::updateSonarFrontValues()
 {
     Fl::lock();
 
-    unsigned left        = SonarFrontStatus::sonarFrontLeft();
-    unsigned centerLeft  = SonarFrontStatus::sonarFrontCenterLeft();
-    unsigned centerRight = SonarFrontStatus::sonarFrontCenterRight();
-    unsigned right       = SonarFrontStatus::sonarFrontRight();
+    uint8_t left        = SonarFrontStatus::sonarFrontLeft();
+    uint8_t centerLeft  = SonarFrontStatus::sonarFrontCenterLeft();
+    uint8_t centerRight = SonarFrontStatus::sonarFrontCenterRight();
+    uint8_t right       = SonarFrontStatus::sonarFrontRight();
 
     if ( m_textSonarFront )
     {
         char buf[ 80 ];
-        snprintf( buf, sizeof( buf ), "%04X %04X %04X %04X",
+        snprintf( buf, sizeof( buf ), "%02X  %02X  %02X  %02X",
                   left, centerLeft, centerRight, right );
         m_textSonarFront->label( buf );
     }
--- a/main/robots/odr-sim/SimDisplay.h	Mon Jul 07 18:42:15 2014 -0700
+++ b/main/robots/odr-sim/SimDisplay.h	Mon Jul 07 18:59:11 2014 -0700
@@ -37,6 +37,7 @@
 #include <Poco/Timer.h>
 
 class Fl_Box;
+class Fl_Dial;
 class Fl_Double_Window;
 class Fl_Hor_Slider;
 class Fl_Light_Button;
@@ -57,6 +58,7 @@
         void updateManagerMsg( const char* msg, int length );
         void doMgrHeartbeat();
         void resetMgrHeartbeat();
+        void updateImuValues();
         void updateSonarFrontState( bool enabled );
         void updateSonarFrontValues();
 
@@ -78,6 +80,11 @@
         Fl_Light_Button*  m_buttonMotorCtl;
         Fl_Toggle_Button* m_buttonA;
         Fl_Toggle_Button* m_buttonB;
+        Fl_Light_Button*  m_buttonImuKA;
+        Fl_Box*           m_textImuValues;
+        Fl_Dial*          m_dialImuHeading;
+        Fl_Slider*        m_sliderImuPitch;
+        Fl_Slider*        m_sliderImuRoll;
         Fl_Light_Button*  m_buttonSonarFrontKA;
         Fl_Slider*        m_sliderSonarFrontL;
         Fl_Slider*        m_sliderSonarFrontCL;
@@ -92,6 +99,7 @@
         void createServoSliders();
         void createDisplays();
         void createActionButtons();
+        void createImuControls();
         void createSonarFrontControls();
         void timeoutMgrHeartbeat( Poco::Timer& timer );
 };
--- a/main/robots/odr-sim/SonarFrontStatus.cpp	Mon Jul 07 18:42:15 2014 -0700
+++ b/main/robots/odr-sim/SonarFrontStatus.cpp	Mon Jul 07 18:59:11 2014 -0700
@@ -84,10 +84,10 @@
 
 Poco::RWLock SonarFrontStatus::sm_rwLock;
 bool         SonarFrontStatus::sm_isEnabled   = false;
-unsigned     SonarFrontStatus::sm_left        = 0x0f00;
-unsigned     SonarFrontStatus::sm_centerLeft  = 0x0f00;
-unsigned     SonarFrontStatus::sm_centerRight = 0x0f00;
-unsigned     SonarFrontStatus::sm_right       = 0x0f00;
+uint8_t      SonarFrontStatus::sm_left        = 0xdf;
+uint8_t      SonarFrontStatus::sm_centerLeft  = 0xdf;
+uint8_t      SonarFrontStatus::sm_centerRight = 0xdf;
+uint8_t      SonarFrontStatus::sm_right       = 0xdf;
 
 // ----------------------------------------------------------------------------------------
 
@@ -115,7 +115,7 @@
 
 // ----------------------------------------------------------------------------------------
 
-unsigned SonarFrontStatus::sonarFrontLeft()
+uint8_t SonarFrontStatus::sonarFrontLeft()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
     return sm_left;
@@ -123,7 +123,7 @@
 
 // ----------------------------------------------------------------------------------------
 
-unsigned SonarFrontStatus::sonarFrontCenterLeft()
+uint8_t SonarFrontStatus::sonarFrontCenterLeft()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
     return sm_centerLeft;
@@ -131,7 +131,7 @@
 
 // ----------------------------------------------------------------------------------------
 
-unsigned SonarFrontStatus::sonarFrontCenterRight()
+uint8_t SonarFrontStatus::sonarFrontCenterRight()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
     return sm_centerRight;
@@ -139,7 +139,7 @@
 
 // ----------------------------------------------------------------------------------------
 
-unsigned SonarFrontStatus::sonarFrontRight()
+uint8_t SonarFrontStatus::sonarFrontRight()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
     return sm_right;
@@ -147,7 +147,7 @@
 
 // ----------------------------------------------------------------------------------------
 
-void SonarFrontStatus::setSonarFrontLeft( unsigned value )
+void SonarFrontStatus::setSonarFrontLeft( uint8_t value )
 {
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
     sm_left = value;
@@ -155,7 +155,7 @@
 
 // ----------------------------------------------------------------------------------------
 
-void SonarFrontStatus::setSonarFrontCenterLeft( unsigned value )
+void SonarFrontStatus::setSonarFrontCenterLeft( uint8_t value )
 {
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
     sm_centerLeft = value;
@@ -163,7 +163,7 @@
 
 // ----------------------------------------------------------------------------------------
 
-void SonarFrontStatus::setSonarFrontCenterRight( unsigned value )
+void SonarFrontStatus::setSonarFrontCenterRight( uint8_t value )
 {
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
     sm_centerRight = value;
@@ -171,7 +171,7 @@
 
 // ----------------------------------------------------------------------------------------
 
-void SonarFrontStatus::setSonarFrontRight( unsigned value )
+void SonarFrontStatus::setSonarFrontRight( uint8_t value )
 {
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
     sm_right = value;
--- a/main/robots/odr-sim/SonarFrontStatus.h	Mon Jul 07 18:42:15 2014 -0700
+++ b/main/robots/odr-sim/SonarFrontStatus.h	Mon Jul 07 18:59:11 2014 -0700
@@ -32,6 +32,8 @@
 #ifndef BCDRL_ROBOTS_ODRSIM_SONARFRONTSTATUS_H
 #define BCDRL_ROBOTS_ODRSIM_SONARFRONTSTATUS_H
 
+#include <stdint.h>
+
 #include <Poco/Event.h>
 #include <Poco/Runnable.h>
 #include <Poco/RWLock.h>
@@ -44,14 +46,14 @@
         static bool     isSonarFrontEnabled();
         static void     setSonarFrontEnabled();
         static void     setSonarFrontDisabled();
-        static unsigned sonarFrontLeft();
-        static unsigned sonarFrontCenterLeft();
-        static unsigned sonarFrontCenterRight();
-        static unsigned sonarFrontRight();
-        static void     setSonarFrontLeft( unsigned value );
-        static void     setSonarFrontCenterLeft( unsigned value );
-        static void     setSonarFrontCenterRight( unsigned value );
-        static void     setSonarFrontRight( unsigned value );
+        static uint8_t  sonarFrontLeft();
+        static uint8_t  sonarFrontCenterLeft();
+        static uint8_t  sonarFrontCenterRight();
+        static uint8_t  sonarFrontRight();
+        static void     setSonarFrontLeft( uint8_t value );
+        static void     setSonarFrontCenterLeft( uint8_t value );
+        static void     setSonarFrontCenterRight( uint8_t value );
+        static void     setSonarFrontRight( uint8_t value );
 
     public:
         SonarFrontStatus();
@@ -65,10 +67,10 @@
     private:
         static Poco::RWLock sm_rwLock;
         static bool         sm_isEnabled;
-        static unsigned     sm_left;
-        static unsigned     sm_centerLeft;
-        static unsigned     sm_centerRight;
-        static unsigned     sm_right;
+        static uint8_t      sm_left;
+        static uint8_t      sm_centerLeft;
+        static uint8_t      sm_centerRight;
+        static uint8_t      sm_right;
 
     private:
         Poco::Event m_quitEvent;
--- a/main/robots/odr-sim/jamfile	Mon Jul 07 18:42:15 2014 -0700
+++ b/main/robots/odr-sim/jamfile	Mon Jul 07 18:59:11 2014 -0700
@@ -37,6 +37,7 @@
     : main.cpp
       ODRSimApp.cpp
       ControllerStatus.cpp 
+      ImuStatus.cpp
       Receiver.cpp 
       SimDisplay.cpp 
       SonarFrontStatus.cpp
--- a/main/robots/odr/AvoidBySonarTask.cpp	Mon Jul 07 18:42:15 2014 -0700
+++ b/main/robots/odr/AvoidBySonarTask.cpp	Mon Jul 07 18:59:11 2014 -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	Mon Jul 07 18:42:15 2014 -0700
+++ b/main/robots/odr/CruisingTask.cpp	Mon Jul 07 18:59:11 2014 -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	Mon Jul 07 18:42:15 2014 -0700
+++ b/main/robots/odr/ImuReader.cpp	Mon Jul 07 18:59:11 2014 -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	Mon Jul 07 18:42:15 2014 -0700
+++ b/main/robots/odr/NavigateTask.cpp	Mon Jul 07 18:59:11 2014 -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	Mon Jul 07 18:42:15 2014 -0700
+++ b/main/robots/odr/ODRApp.cpp	Mon Jul 07 18:59:11 2014 -0700
@@ -53,8 +53,10 @@
 #include "SafetyTask.h"
 #include "Scoreboard.h"
 #include "Sonar.h"
+#include "SquareCourseTask.h"
 #include "StartupTask.h"
 #include "TaskObject.h"
+#include "TrackWaypointsTask.h"
 
 #include "packages/common/can/can_helpers.h"
 
@@ -171,8 +173,14 @@
     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 TrackWaypointsTask( 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	Mon Jul 07 18:42:15 2014 -0700
+++ b/main/robots/odr/Scoreboard.cpp	Mon Jul 07 18:59:11 2014 -0700
@@ -7,7 +7,7 @@
 // 
 //  Global thread-safe state derived from incoming CAN messages and other internal data.
 //
-//  Copyright (c) 2010-2013 Bob Cook
+//  Copyright (c) 2010-2014 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
@@ -62,19 +62,35 @@
 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;
+}
 
 // ----------------------------------------------------------------------------------------
 
 bool Scoreboard::isControllerAlive()
 {
     Poco::RWLock::ScopedReadLock lock( sm_rwLock );
-    return sm_isControllerAlive;
+    return true; // fake it
+    // return sm_isControllerAlive;
 }
 
 // ----------------------------------------------------------------------------------------
@@ -188,6 +204,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 +290,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 +326,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,10 +382,46 @@
 
 // ----------------------------------------------------------------------------------------
 
+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, 5 ),
+                    Poco::NumberFormatter::format( sm_gpsLongitudeAvg, 5 ) ) );
+        }
+        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 );
 
+    // send a heartbeat as if we are odr-controller (which is deprecated)
+
+    uint32_t hbeatmsgid = can_build_message_id(
+                    can_node_odr_manager, can_node_broadcast, can_dataid_heartbeat );
+
+    CANMessage::QueueToSend( new CANMessage( hbeatmsgid ) );
+
     // send the manager status message
 
     uint32_t updatemsgid = can_build_message_id(
@@ -356,6 +467,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 +597,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 +667,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 +806,18 @@
                         recvSonarFrontStatus( true /* enabled */ );
                         recvSonarFrontValues( msg );
                         break;
+
+                    case can_dataid_gps_fix:
+                        recvGpsFixInfo( msg );
+                        break;
+
+                    case can_dataid_gps_latitude:
+                        recvGpsLatitude( msg );
+                        break;
+
+                    case can_dataid_gps_longitude:
+                        recvGpsLongitude( msg );
+                        break;
                 }
             }
         }
--- a/main/robots/odr/Scoreboard.h	Mon Jul 07 18:42:15 2014 -0700
+++ b/main/robots/odr/Scoreboard.h	Mon Jul 07 18:59:11 2014 -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 Jul 07 18:59:11 2014 -0700
@@ -0,0 +1,97 @@
+// ----------------------------------------------------------------------------------------
+//
+//  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()
+{
+    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 );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool SquareCourseTask::wantsControl()
+{
+    return false; // nothing to do here
+}
+
+// ----------------------------------------------------------------------------------------
+
+void SquareCourseTask::takeControl()
+{
+    // nothing to do here
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/SquareCourseTask.h	Mon Jul 07 18:59:11 2014 -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/StartupTask.cpp	Mon Jul 07 18:42:15 2014 -0700
+++ b/main/robots/odr/StartupTask.cpp	Mon Jul 07 18:59:11 2014 -0700
@@ -137,6 +137,7 @@
 {
     if ( ! m_lastControllerStatus )
     {
+        Scoreboard::sendManagerMessage( "Waiting" );
         return; // nothing can be done until the odr-controller is up
     }
 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/TrackWaypointsTask.cpp	Mon Jul 07 18:59:11 2014 -0700
@@ -0,0 +1,140 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/TrackWaypointsTask.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Subsumption task to navigate to a list of successive GPS locations (waypoints).
+//
+//  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.
+//
+//  The algorithm to compute heading (bearing) and distance between GPS coordinates was
+//  found at this website: http://www.movable-type.co.uk/scripts/latlong.html
+//
+// ----------------------------------------------------------------------------------------
+
+#include "TrackWaypointsTask.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <Poco/Logger.h>
+#include <Poco/NumberFormatter.h>
+
+#include "Scoreboard.h"
+
+// ----------------------------------------------------------------------------------------
+
+static const Poco::Timestamp::TimeDiff sRunTimeUntilWaypointCheck
+                                                      = Poco::Timestamp::resolution() * 5;
+
+// ----------------------------------------------------------------------------------------
+
+TrackWaypointsTask::TrackWaypointsTask( const std::string& loggerName )
+    : TaskObject( loggerName ),
+      m_waypoints(),
+      m_nextUpdateTime()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+TrackWaypointsTask::~TrackWaypointsTask()
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+void TrackWaypointsTask::update()
+{
+    if ( m_nextUpdateTime.isElapsed( sRunTimeUntilWaypointCheck ) )
+    {
+        m_nextUpdateTime.update();
+
+        // decide if we have reached the waypoint
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool TrackWaypointsTask::wantsControl()
+{
+    return false; // nothing to do here
+}
+
+// ----------------------------------------------------------------------------------------
+
+void TrackWaypointsTask::takeControl()
+{
+    // nothing to do here
+}
+
+// ----------------------------------------------------------------------------------------
+
+static double deg_to_rad( double degrees )
+{
+    return degrees * M_PI / 180.0;
+}
+
+// ----------------------------------------------------------------------------------------
+
+static double rad_to_deg( double radians )
+{
+    return radians * 180.0 / M_PI;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void TrackWaypointsTask::refreshWaypointTrack()
+{
+    // compute the desired heading to the waypoint from the current location
+
+    double currLat = Scoreboard::gpsLatitude();
+    double currLon = Scoreboard::gpsLongitude();
+
+    double deltaLonRads  = ::deg_to_rad( m_waypointLongitude - currLon );
+    double currLatRads   = ::deg_to_rad( currLat );
+    double targetLatRads = ::deg_to_rad( m_waypointLatitude );
+
+    double y = ::sin( deltaLonRads ) * ::cos( targetLatRads );
+    double x = ::cos( currLatRads ) * ::sin( targetLatRads ) -
+               ::sin( currLatRads ) * ::cos( targetLatRads ) * ::cos( deltaLonRads );
+
+    m_headingToWaypoint = ::rad_to_deg( ::atan2( y, x ) );
+
+    // compute the distance to the desired waypoint
+
+    static const double R = 6371.0; // km
+
+    double deltaLatRads = ::deg_to_rad( m_waypointLatitude - currLat );
+
+    double a = ::sin( deltaLatRads / 2.0 ) * ::sin( deltaLatRads / 2.0 ) +
+               ::sin( deltaLonRads / 2.0 ) * ::sin( deltaLonRads / 2.0 ) *
+               ::cos( currLatRads ) * ::cos( targetLatRads );
+
+    double c = 2.0 * ::atan2( ::sqrt( a ), ::sqrt( 1.0 - a ) );
+
+    m_distanceToWaypoint = R * c;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/TrackWaypointsTask.h	Mon Jul 07 18:59:11 2014 -0700
@@ -0,0 +1,76 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/TrackWaypointsTask.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_TRACKWAYPOINTSTASK_H
+#define BCDRL_ROBOTS_ODR_TRACKWAYPOINTSTASK_H
+
+#include "TaskObject.h"
+
+#include <vector>
+
+// ----------------------------------------------------------------------------------------
+
+typedef struct
+{
+    double latitude;
+    double longitude;
+   
+} WaypointType;
+
+typedef std::vector< WaypointType > WaypointVectorType;
+
+// ----------------------------------------------------------------------------------------
+
+class TrackWaypointsTask : public TaskObject
+{
+    public:
+        TrackWaypointsTask( const std::string& loggerName );
+        virtual ~TrackWaypointsTask();
+        virtual void update();
+        virtual bool wantsControl();
+        virtual void takeControl();
+
+    private:
+        void refreshWaypointTrack();
+
+    private:
+        WaypointVectorType m_waypoints;
+        Poco::Timestamp    m_nextUpdateTime;
+        double             m_waypointLatitude;
+        double             m_waypointLongitude;
+        double             m_headingToWaypoint;
+        double             m_distanceToWaypoint;
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_ODR_TRACKWAYPOINTSTASK_H
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr/jamfile	Mon Jul 07 18:42:15 2014 -0700
+++ b/main/robots/odr/jamfile	Mon Jul 07 18:59:11 2014 -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 TrackWaypointsTask.cpp
     packages.common.can.pkg
     packages.linux.can.pkg
     ;