view main/robots/odr/ImuReader.cpp @ 221:8e6ca2ead01a main

Latest updates.
author Bob Cook <bob@bobcookdev.com>
date Mon, 09 Sep 2013 20:03:31 -0700
parents 070201175cc6
children 50414c680910
line wrap: on
line source

// ----------------------------------------------------------------------------------------
//
//  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 <algorithm>
#include <cmath>
#include <stdexcept>
#include <utility>
#include <vector>

#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 ) );

    if ( tcgetattr( m_serialPortFd, &settings ) < 0 )
    {
        log.error(
            Poco::Logger::format(
                "ImuReader::configureSerialPort() cannot get config on port \"$0\": $1",
                m_serialPortPath,
                std::string( strerror( errno ) ) ) );

        closeSerialPort();
        return false;
    }

    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;
}

// ----------------------------------------------------------------------------------------
//  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.
//
//  For values that aren't approaching 180, a standard average is ok. But for the values
//  on either side of the 180/-180 boundry we compute the average delta from 180/-180.

static double computeAverageYaw( const std::vector< double >& samples )
{
    bool valuesAreNearMax = false;
    bool valuesAreNearNegMax = false;

    for ( std::vector< double >::const_iterator itr = samples.begin();
          itr != samples.end();
          ++itr )
    {
        if ( *itr > (  180.0 - 10.0 ) ) valuesAreNearMax = true;
        if ( *itr < ( -180.0 + 10.0 ) ) valuesAreNearNegMax = true;
    }

    if ( valuesAreNearMax && valuesAreNearNegMax )
    {
        // wrap-around case computes the delta from 180/-180

        double deltaValuesSum = 0.0;

        for ( std::vector< double >::const_iterator itr = samples.begin();
              itr != samples.end();
              ++itr )
        {
            if ( *itr < 0.0 )
            {
                deltaValuesSum += ( -180.0 - *itr );
            }
            else
            {
                deltaValuesSum += ( 180.0 - *itr );
            }
        }

        if ( deltaValuesSum < 0.0 )
        {
            return -180.0 - ( deltaValuesSum / samples.size() );
        }
        else
        {
            return 180.0 - ( deltaValuesSum / samples.size() );
        }
    }

    // just compute a regular average

    double valuesSum = 0.0;

    for ( std::vector< double >::const_iterator itr = samples.begin();
          itr != samples.end();
          ++itr )
    {
        valuesSum += *itr;
    }

    return valuesSum / samples.size();
}

// ----------------------------------------------------------------------------------------

void ImuReader::run()
{
    Poco::Logger& log = Poco::Logger::get( m_loggerName );

    for ( ;; )
    {
        log.information( std::string( "ImuReader::run() starting" ) );

        // initialize the calibration table

        initYawCalibrationTable();

        try
        {
            // to keep ourselves sane, start by assuming the IMU is offline

            Scoreboard::imuIsOffline();

            // now try to find it on the serial port

            if ( ! configureSerialPort() )
            {
                Poco::Thread::sleep( 10000 ); // 10 sec
                continue;
            }

            resetParseState();

            // keep the last 10 values to average together (approx 0.5 seconds elapsed)

            std::vector< double > valuesYaw;
            valuesYaw.reserve( 10 );

            // timeout after 10 consecutive runs of not getting data

            int timeoutNoData = 0;

            // loop while we are still getting data

            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, drop out and reinit the port
                }

                if ( length == 0 )
                {
                    if ( ++timeoutNoData == 10 )
                    {
                        Scoreboard::imuIsOffline();
                    }
                    continue;
                }

                // got some bytes, try to parse them

                timeoutNoData = 0;

                if ( processImuData( buffer, length ) )
                {
                    // new values available, save them

                    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

                    if ( valuesYaw.size() == 10 )
                    {
                        double averageYaw = computeAverageYaw( valuesYaw );

                        Scoreboard::imuUpdate( m_parseValueRoll,
                                               m_parseValuePitch,
                                               averageYaw );

                        // clear the vector, to accumulate more samples

                        valuesYaw.clear();
                    }
                }
            }
        }
        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 );
    }
}

// ----------------------------------------------------------------------------------------