changeset 146:a01a4a349d58 main

New files to read the IMU via a serial port.
author Bob Cook <bob@bobcookdev.com>
date Sat, 11 Aug 2012 16:06:03 -0700
parents fa0eaf92e263
children 73f4dc874231
files main/robots/odr/AvoidBySonarTask.cpp main/robots/odr/ImuReader.cpp main/robots/odr/ImuReader.h main/robots/odr/ODRApp.cpp main/robots/odr/jamfile main/robots/odr/odr_ubuntu.xml
diffstat 6 files changed, 530 insertions(+), 5 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr/AvoidBySonarTask.cpp	Sat Jul 07 16:57:49 2012 -0700
+++ b/main/robots/odr/AvoidBySonarTask.cpp	Sat Aug 11 16:06:03 2012 -0700
@@ -7,7 +7,7 @@
 //
 //  Virtual base class for a subsumption task object.
 //
-//  Copyright (c) 2011 Bob Cook
+//  Copyright (c) 2012 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,9 +37,9 @@
 
 // ----------------------------------------------------------------------------------------
 
-static const unsigned AVOIDANCE_ZONE = 0x2000;
-static const unsigned MAX_TURN_ZONE  = 0x1000;
-static const unsigned TRAPPED_ZONE   = 0x0600;
+static const unsigned AVOIDANCE_ZONE = 0x1000;
+static const unsigned MAX_TURN_ZONE  = 0x0800;
+static const unsigned TRAPPED_ZONE   = 0x0500;
 
 // ----------------------------------------------------------------------------------------
 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/ImuReader.cpp	Sat Aug 11 16:06:03 2012 -0700
@@ -0,0 +1,417 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/ImuReader.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Runnable object that reads and parses messages from a SparkFun 9-DOF Razor IMU.
+//
+//  Copyright (c) 2012 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 "ImuReader.h"
+
+#include <errno.h>
+#include <fcntl.h>
+#include <string.h>
+#include <sys/select.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <termios.h>
+#include <unistd.h>
+
+#include <stdexcept>
+
+#include <Poco/Exception.h>
+#include <Poco/Logger.h>
+#include <Poco/NumberFormatter.h>
+#include <Poco/NumberParser.h>
+#include <Poco/Thread.h>
+
+#include "Scoreboard.h"
+
+#include "packages/common/can/can_helpers.h"
+#include "packages/common/can/can_messages.h"
+#include "packages/common/can/can_nodes.h"
+
+#include "packages/common/util/misc.h"
+
+#include "packages/linux/can/CANMessage.h"
+
+// ----------------------------------------------------------------------------------------
+
+ImuReader::ImuReader( const std::string& loggerName, const std::string& serialPortPath )
+    : Poco::Runnable(),
+      m_loggerName( loggerName ),
+      m_serialPortPath( serialPortPath ),
+      m_quitEvent( false /* not auto-reset */ ),
+      m_serialPortFd( -1 /* not open */ )
+{
+}
+
+// ----------------------------------------------------------------------------------------
+
+ImuReader::~ImuReader()
+{
+    closeSerialPort();
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuReader::timeToQuit()
+{
+    m_quitEvent.set();
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuReader::closeSerialPort()
+{
+    if ( m_serialPortFd >= 0 )
+    {
+        close( m_serialPortFd );
+        m_serialPortFd = -1;
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool ImuReader::configureSerialPort()
+{
+    Poco::Logger& log = Poco::Logger::get( m_loggerName );
+
+    closeSerialPort();
+
+    m_serialPortFd = open( m_serialPortPath.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK );
+
+    if ( m_serialPortFd < 0 )
+    {
+        log.error(
+            Poco::Logger::format(
+                "ImuReader::configureSerialPort() failed to open port \"$0\": $1",
+                m_serialPortPath,
+                std::string( strerror( errno ) ) ) );
+        return false;
+    }
+
+    // the SparkFun 9-DOF Razor IMU is configured with software from WebBot:
+    // http://webbot.org.uk/iPoint/49.page
+    //
+    // 115200 baud, 8 bits, no stop bits
+
+    termios settings;
+    bzero( &settings, sizeof( settings ) );
+
+    settings.c_iflag = IGNBRK;
+    settings.c_cflag = CREAD | CLOCAL;
+    settings.c_cc[ VMIN ]  = 0; // read doesn't block
+    settings.c_cc[ VTIME ] = 1; // always return within 0.1 seconds
+
+    cfsetspeed( &settings, B115200 );
+
+    if ( tcsetattr( m_serialPortFd, TCSANOW, &settings ) < 0 )
+    {
+        log.error(
+            Poco::Logger::format(
+                "ImuReader::configureSerialPort() failed to configure port \"$0\": $1",
+                m_serialPortPath,
+                std::string( strerror( errno ) ) ) );
+
+        closeSerialPort();
+        return false;
+    }
+
+    log.information( "ImuReader::configureSerialPort() successfully opened \""
+                     + m_serialPortPath + "\"" );
+    return true;
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool ImuReader::readFromSerialPort( char* buffer, size_t* length )
+{
+    fd_set emptyFds;
+    FD_ZERO( &emptyFds );
+
+    fd_set readableFds;
+    FD_ZERO( &readableFds );
+    FD_SET( m_serialPortFd, &readableFds );
+
+    struct timeval tv;
+    tv.tv_sec  = 0;
+    tv.tv_usec = 250 * 1000; // 250ms as microseconds
+
+    int result = select( m_serialPortFd + 1, &readableFds, &emptyFds, &emptyFds, &tv );
+
+    if ( result == 0 )
+    {
+        *length = 0;
+        return true; // timeout ok, but no bytes
+    }
+    else if ( result < 0 )
+    {
+        Poco::Logger& log = Poco::Logger::get( m_loggerName );
+
+        log.error(
+            Poco::Logger::format(
+                "ImuReader::readFromSerialPort() failed to select on port: $0",
+                std::string( strerror( errno ) ) ) );
+
+        return false;
+    }
+
+    ssize_t bytesRead = read( m_serialPortFd, buffer, *length );
+
+    if ( bytesRead < 0 )
+    {
+        if ( errno == EAGAIN || errno == EWOULDBLOCK )
+        {
+            *length = 0;
+            return true; // timeout ok, but no bytes
+        }
+
+        Poco::Logger& log = Poco::Logger::get( m_loggerName );
+
+        log.error(
+            Poco::Logger::format(
+                "ImuReader::readFromSerialPort() failed to read from port: $0",
+                std::string( strerror( errno ) ) ) );
+
+        return false;
+    }
+
+    *length = static_cast< size_t >( bytesRead );
+    return true;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuReader::resetParseState()
+{
+    m_parseState = PARSE_STATE_LOOKING_FOR_START;
+}
+
+// ----------------------------------------------------------------------------------------
+//  the data is in the form of:
+//
+//      !ANG:roll,pitch,yaw,MAG:x,y,z<eoln>
+//
+//  we parse out the roll, pitch, and yaw values, then ignore the rest.
+//
+
+bool ImuReader::processImuData( const char* data, size_t length )
+{
+    bool completedNewValues = false;
+
+    while ( length-- )
+    {
+        char ch = *data++;
+
+        switch ( m_parseState )
+        {
+            case PARSE_STATE_LOOKING_FOR_START:
+                if ( ch == '!' )
+                {
+                    m_parseState = PARSE_STATE_START_SYMBOL_A;
+                }
+                break;
+
+            case PARSE_STATE_START_SYMBOL_A:
+                if ( ch == 'A' )
+                {
+                    m_parseState = PARSE_STATE_START_SYMBOL_N;
+                }
+                else
+                {
+                    m_parseState = PARSE_STATE_LOOKING_FOR_START;
+                }
+                break;
+
+            case PARSE_STATE_START_SYMBOL_N:
+                if ( ch == 'N' )
+                {
+                    m_parseState = PARSE_STATE_START_SYMBOL_G;
+                }
+                else
+                {
+                    m_parseState = PARSE_STATE_LOOKING_FOR_START;
+                }
+                break;
+
+            case PARSE_STATE_START_SYMBOL_G:
+                if ( ch == 'G' )
+                {
+                    m_parseState = PARSE_STATE_START_SYMBOL_COLON;
+                }
+                else
+                {
+                    m_parseState = PARSE_STATE_LOOKING_FOR_START;
+                }
+                break;
+
+            case PARSE_STATE_START_SYMBOL_COLON:
+                if ( ch == ':' )
+                {
+                    m_parseToken.clear();
+                    m_parseState = PARSE_STATE_READ_ROLL;
+                }
+                else
+                {
+                    m_parseState = PARSE_STATE_LOOKING_FOR_START;
+                }
+                break;
+
+            case PARSE_STATE_READ_ROLL:
+                if ( ch == ',' )
+                {
+                    m_parseValueRoll = Poco::NumberParser::parseFloat( m_parseToken );
+                    m_parseToken.clear();
+                    m_parseState = PARSE_STATE_READ_PITCH;
+                }
+                else
+                {
+                    m_parseToken += ch;
+                }
+                break;
+
+            case PARSE_STATE_READ_PITCH:
+                if ( ch == ',' )
+                {
+                    m_parseValuePitch = Poco::NumberParser::parseFloat( m_parseToken );
+                    m_parseToken.clear();
+                    m_parseState = PARSE_STATE_READ_YAW;
+                }
+                else
+                {
+                    m_parseToken += ch;
+                }
+                break;
+
+            case PARSE_STATE_READ_YAW:
+                if ( ch == ',' )
+                {
+                    m_parseValueYaw = Poco::NumberParser::parseFloat( m_parseToken );
+                    m_parseToken.clear();
+                    m_parseState = PARSE_STATE_LOOKING_FOR_START;
+                    completedNewValues = true;;
+                }
+                else
+                {
+                    m_parseToken += ch;
+                }
+                break;
+
+            default:
+                m_parseState = PARSE_STATE_LOOKING_FOR_START;
+                break;
+        }
+
+        // sanity check our working buffer; too big == error
+
+        if ( m_parseToken.length() > 20 )
+        {
+            m_parseState = PARSE_STATE_LOOKING_FOR_START;
+        }
+    }
+    
+    return completedNewValues;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void ImuReader::run()
+{
+    Poco::Logger& log = Poco::Logger::get( m_loggerName );
+
+    for ( ;; )
+    {
+        log.information( std::string( "ImuReader::run() starting" ) );
+
+        try
+        {
+            if ( ! configureSerialPort() )
+            {
+                Poco::Thread::sleep( 10000 ); // 10 sec
+                continue;
+            }
+
+            resetParseState();
+
+            int valuesAvailableCount = 0;
+
+            for ( ;; )
+            {
+                if ( m_quitEvent.tryWait( 0 ) )
+                {
+                    log.information( std::string( "ImuReader::run() stopping" ) );
+                    return;
+                }
+
+                char   buffer[ 64 ];
+                size_t length = array_sizeof( buffer );
+
+                if ( ! readFromSerialPort( buffer, &length ) )
+                {
+                    break; // error
+                }
+
+                if ( processImuData( buffer, length ) )
+                {
+                    // new values available
+                    if ( ++valuesAvailableCount == 10 )
+                    {
+                        valuesAvailableCount = 0;
+                        log.information(
+                            Poco::Logger::format(
+                                "ImuReader found roll: $0 pitch: $1 yaw: $2",
+                                Poco::NumberFormatter::format( m_parseValueRoll ),
+                                Poco::NumberFormatter::format( m_parseValuePitch ),
+                                Poco::NumberFormatter::format( m_parseValueYaw ) ) );
+                    }
+                }
+            }
+        }
+        catch ( const Poco::Exception& ex )
+        {
+            log.error(
+                Poco::Logger::format(
+                    "ImuReader::run() caught Poco::Exception: $0", ex.displayText() ) );
+        }
+        catch ( const std::exception& ex )
+        {
+            log.error(
+                Poco::Logger::format(
+                    "ImuReader::run() caught std::exception: $0", ex.what() ) );
+        }
+        catch ( ... )
+        {
+            log.error( "ImuReader::run() caught unknown exception" );
+        }
+
+        // sleep for 1/2 second after exception processing, but don't exit
+        Poco::Thread::sleep( 500 );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/ImuReader.h	Sat Aug 11 16:06:03 2012 -0700
@@ -0,0 +1,88 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/odr/ImuReader.h
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Runnable object that reads and parses messages from a SparkFun 9-DOF Razor IMU.
+//
+//  Copyright (c) 2012 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_IMUREADER_H
+#define BCDRL_ROBOTS_ODR_IMUREADER_H
+
+#include <stdint.h>
+
+#include <Poco/Event.h>
+#include <Poco/Runnable.h>
+#include <Poco/RWLock.h>
+#include <Poco/Timer.h>
+
+// ----------------------------------------------------------------------------------------
+
+class ImuReader : public Poco::Runnable
+{
+    public:
+        ImuReader( const std::string& loggerName, const std::string& serialPortPath );
+        virtual ~ImuReader();
+        virtual void run();
+        void timeToQuit();
+
+    private:
+        typedef enum
+        {
+            PARSE_STATE_LOOKING_FOR_START,
+            PARSE_STATE_START_SYMBOL_A,
+            PARSE_STATE_START_SYMBOL_N,
+            PARSE_STATE_START_SYMBOL_G,
+            PARSE_STATE_START_SYMBOL_COLON,
+            PARSE_STATE_READ_ROLL,
+            PARSE_STATE_READ_PITCH,
+            PARSE_STATE_READ_YAW
+
+        }   imu_parse_state_t;
+
+    private:
+        std::string       m_loggerName;
+        std::string       m_serialPortPath;
+        Poco::Event       m_quitEvent;
+        int               m_serialPortFd;
+        imu_parse_state_t m_parseState;
+        std::string       m_parseToken;
+        double            m_parseValueRoll;
+        double            m_parseValuePitch;
+        double            m_parseValueYaw;
+
+    private:
+        void closeSerialPort();
+        bool configureSerialPort();
+        bool readFromSerialPort( char* buffer, size_t* length );
+        void resetParseState();
+        bool processImuData( const char* data, size_t length );
+};
+
+// ----------------------------------------------------------------------------------------
+#endif // #ifndef BCDRL_ROBOTS_ODR_IMUREADER_H
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr/ODRApp.cpp	Sat Jul 07 16:57:49 2012 -0700
+++ b/main/robots/odr/ODRApp.cpp	Sat Aug 11 16:06:03 2012 -0700
@@ -47,6 +47,7 @@
 
 #include "AvoidBySonarTask.h"
 #include "CruisingTask.h"
+#include "ImuReader.h"
 #include "MotorsAndServos.h"
 #include "SafetyTask.h"
 #include "Scoreboard.h"
@@ -143,6 +144,21 @@
     Scoreboard   scoreboard( logger().name() );
     scoreboardThread.start( scoreboard );
 
+    std::string imuSerialPortPath = config().getString( "imu.serialPortPath", "" );
+
+    Poco::Thread imuReaderThread;
+    ImuReader    imuReader( logger().name(), imuSerialPortPath );
+
+    if ( ! imuSerialPortPath.empty() )
+    {
+        logger().information( "using IMU serial port \"" + imuSerialPortPath + "\"" );
+        imuReaderThread.start( imuReader );
+    }
+    else
+    {
+        logger().information( "no IMU serial port configured; disabled" );
+    }
+
     m_tasks.clear();
 
     m_tasks.push_back(
--- a/main/robots/odr/jamfile	Sat Jul 07 16:57:49 2012 -0700
+++ b/main/robots/odr/jamfile	Sat Aug 11 16:06:03 2012 -0700
@@ -36,7 +36,8 @@
 
 COMMON_SOURCES = 
     main.cpp
-    ODRApp.cpp MotorsAndServos.cpp Scoreboard.cpp Sonar.cpp
+    ODRApp.cpp
+    ImuReader.cpp MotorsAndServos.cpp Scoreboard.cpp Sonar.cpp
     TaskObject.cpp
     AvoidBySonarTask.cpp CruisingTask.cpp SafetyTask.cpp StartupTask.cpp
     packages.common.can.pkg
--- a/main/robots/odr/odr_ubuntu.xml	Sat Jul 07 16:57:49 2012 -0700
+++ b/main/robots/odr/odr_ubuntu.xml	Sat Aug 11 16:06:03 2012 -0700
@@ -32,5 +32,8 @@
         <logIncoming>no</logIncoming>
         <logOutgoing>no</logOutgoing>
     </can>
+    <imu>
+        <serialPortPath>/dev/ttyUSB0</serialPortPath>
+    </imu>
 </config>