view main/robots/odr/Scoreboard.cpp @ 230:50414c680910 main

Split the IMU reader out to a separate application. Rework the scoreboard accordingly. Also remove all the legacy "odr-controller" logic, and improve the management of "emergency" situations for all nodes.
author Bob Cook <bob@bobcookdev.com>
date Sat, 12 Jul 2014 17:21:10 -0700
parents 03064d391e79
children b9288cad0bfb
line wrap: on
line source

// ----------------------------------------------------------------------------------------
//
//  robots/odr/Scoreboard.cpp
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
// 
//  Global thread-safe state derived from incoming CAN messages and other internal data.
//
//  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
//  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 "Scoreboard.h"

#include <stdexcept>

#include <Poco/Exception.h>
#include <Poco/Logger.h>
#include <Poco/NumberFormatter.h>
#include <Poco/Thread.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"

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

// scoreboard lock
Poco::RWLock Scoreboard::sm_rwLock;

// overall emergency state
bool Scoreboard::sm_isEmergencyActive;

// message broadcast
std::string     Scoreboard::sm_msgLastText;
Poco::Timestamp Scoreboard::sm_msgLastUpdate;

// odr-display
bool Scoreboard::sm_DisplayEmergency;
bool Scoreboard::sm_isDisplayAlive;

// odr-gps
bool            Scoreboard::sm_GpsEmergency;
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;

// odr-motion
bool Scoreboard::sm_MotionEmergency;
bool Scoreboard::sm_isMotionAlive;

// odr-sonar-front
bool    Scoreboard::sm_SonarFrontEmergency;
bool    Scoreboard::sm_isSonarFrontAlive;
bool    Scoreboard::sm_isSonarFrontEnabled;
uint8_t Scoreboard::sm_sonarFrontLeft;
uint8_t Scoreboard::sm_sonarFrontCenterLeft;
uint8_t Scoreboard::sm_sonarFrontCenterRight;
uint8_t Scoreboard::sm_sonarFrontRight;

// imu
bool   Scoreboard::sm_isImuAlive;
double Scoreboard::sm_imuRoll;
double Scoreboard::sm_imuPitch;
double Scoreboard::sm_imuYaw;

// program
bool    Scoreboard::sm_hasActiveProgram;
uint8_t Scoreboard::sm_activeProgram;

// navigation
int8_t Scoreboard::sm_navMaximumSpeed;
double Scoreboard::sm_navTargetHeading;

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

bool Scoreboard::isEmergencyActive()
{
    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
    
    sm_isEmergencyActive = sm_DisplayEmergency    | ! sm_isDisplayAlive
                         | sm_GpsEmergency        | ! sm_isGpsAlive
                         | sm_MotionEmergency     | ! sm_isMotionAlive
                         | sm_SonarFrontEmergency | ! sm_isSonarFrontAlive
                         ;

    return sm_isEmergencyActive;
}

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

bool Scoreboard::isDisplayAlive()
{
    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
    return sm_isDisplayAlive;
}

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

bool Scoreboard::isGpsAlive()
{
    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
    return sm_isGpsAlive;
}

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

bool Scoreboard::isMotionAlive()
{
    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
    return sm_isMotionAlive;
}

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

bool Scoreboard::isSonarFrontAlive()
{
    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
    return sm_isSonarFrontAlive;
}

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

void Scoreboard::sendManagerMessage( const char* msg )
{
    // don't repeat the same message more than once per five seconds

    static const Poco::Timestamp::TimeDiff FiveSeconds = Poco::Timestamp::resolution() * 5;

    if ( sm_msgLastText == msg )
    {
        if ( sm_msgLastUpdate.elapsed() <= FiveSeconds )
        {
            return;
        }
    }

    // either a new message, or time to send it again

    sm_msgLastText = msg;
    sm_msgLastUpdate.update();

    uint32_t updatemsgid = can_build_message_id(
                    can_node_odr_manager, can_node_broadcast, can_dataid_odrmgr_update );

    can_data_odrmgr_update update;

    uint8_t len = 0;
    for ( ; len < sizeof( update.msg ); ++len )
    {
        if ( *msg == '\0' ) break;
        update.msg[len] = *msg++;
    }

    CANMessage::QueueToSend(
            new CANMessage( updatemsgid, reinterpret_cast< uint8_t* >( &update ), len ) );
}

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

bool Scoreboard::isSonarFrontEnabled()
{
    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
    return sm_isSonarFrontEnabled;
}

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

uint8_t Scoreboard::sonarFrontLeft()
{
    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
    return sm_sonarFrontLeft;
}

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

uint8_t Scoreboard::sonarFrontCenterLeft()
{
    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
    return sm_sonarFrontCenterLeft;
}

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

uint8_t Scoreboard::sonarFrontCenterRight()
{
    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
    return sm_sonarFrontCenterRight;
}

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

uint8_t Scoreboard::sonarFrontRight()
{
    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
    return sm_sonarFrontRight;
}

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

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 );
    return sm_isImuAlive;
}

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

double Scoreboard::imuRoll()
{
    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
    return sm_imuRoll;
}

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

double Scoreboard::imuPitch()
{
    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
    return sm_imuPitch;
}

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

double Scoreboard::imuYaw()
{
    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
    return sm_imuYaw;
}

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

bool Scoreboard::isProgramRunning()
{
    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
    return sm_hasActiveProgram;
}

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

uint8_t Scoreboard::activeProgram()
{
    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
    if ( ! sm_hasActiveProgram )
    {
        return 0;
    }
    return sm_activeProgram;
}

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

void Scoreboard::setActiveProgram( uint8_t program )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
    sm_hasActiveProgram = true;
    sm_activeProgram    = program;
}

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

void Scoreboard::stopActiveProgram()
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
    sm_hasActiveProgram = false;
}

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

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 );
    return sm_navTargetHeading;
}

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

void Scoreboard::navSetTargetHeading( double heading )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
    sm_navTargetHeading = heading;
}

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

Scoreboard::Scoreboard( const std::string& loggerName )
    : Poco::Runnable(),
      m_loggerName( loggerName ),
      m_quitEvent( false /* not auto-reset */ ),
      m_timerLogStatus(),
      m_timerSendManagerUpdate(),
      m_timerDisplayTimeout(),
      m_timerGpsTimeout(),
      m_timerImuTimeout(),
      m_timerMotionTimeout(),
      m_timerSonarFrontTimeout()
{
    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_timerDisplayTimeout.setPeriodicInterval( 5000 );
    m_timerDisplayTimeout.start(
            Poco::TimerCallback< Scoreboard >(
                    *this, &Scoreboard::timeoutDisplayNode ) );

    m_timerGpsTimeout.setPeriodicInterval( 5000 );
    m_timerGpsTimeout.start(
            Poco::TimerCallback< Scoreboard >(
                    *this, &Scoreboard::timeoutGpsSensorNode ) );

    m_timerImuTimeout.setPeriodicInterval( 5000 );
    m_timerImuTimeout.start(
            Poco::TimerCallback< Scoreboard >(
                    *this, &Scoreboard::timeoutImuNode ) );

    m_timerMotionTimeout.setPeriodicInterval( 5000 );
    m_timerMotionTimeout.start(
            Poco::TimerCallback< Scoreboard >(
                    *this, &Scoreboard::timeoutMotionNode ) );

    m_timerSonarFrontTimeout.setPeriodicInterval( 5000 );
    m_timerSonarFrontTimeout.start(
            Poco::TimerCallback< Scoreboard >(
                    *this, &Scoreboard::timeoutSonarFrontNode ) );
}

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

Scoreboard::~Scoreboard()
{
}

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

void Scoreboard::timeToQuit()
{
    m_quitEvent.set();
}

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

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(
                    can_node_odr_manager, can_node_broadcast, can_dataid_odrmgr_update );

    CANMessage::QueueToSend( new CANMessage( updatemsgid ) );

    // update the emergency / all-clear status

    if ( Scoreboard::isEmergencyActive() )
    {
        uint32_t msgid = can_build_message_id(
                    can_node_odr_manager, can_node_broadcast, can_dataid_emergency );

        CANMessage::QueueToSend( new CANMessage( msgid ) );
    }
    else
    {
        uint32_t msgid = can_build_message_id(
                    can_node_odr_manager, can_node_broadcast, can_dataid_all_clear );

        CANMessage::QueueToSend( new CANMessage( msgid ) );
    }

    // monitor the length of pending CAN messages, log when it looks too long

    unsigned send_q_len = CANMessage::SendQueueSize();
    unsigned recv_q_len = CANMessage::ReceiveQueueSize();

    if ( send_q_len > 5 || recv_q_len > 5 )
    {
        log.information(
            Poco::Logger::format(
                "Scoreboard: CAN queue length: send $0 / recv $1",
                Poco::NumberFormatter::format( send_q_len ),
                Poco::NumberFormatter::format( recv_q_len ) ) );
    }
}

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

void Scoreboard::timeoutDisplayNode( Poco::Timer& timer )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
    sm_isDisplayAlive = false;
}

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

void Scoreboard::timeoutGpsSensorNode( Poco::Timer& timer )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
    sm_isGpsAlive = false;
}

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

void Scoreboard::timeoutImuNode( Poco::Timer& timer )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
    sm_isImuAlive = false;
}

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

void Scoreboard::timeoutMotionNode( Poco::Timer& timer )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
    sm_isMotionAlive = false;
}

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

void Scoreboard::timeoutSonarFrontNode( Poco::Timer& timer )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
    sm_isSonarFrontAlive = false;
}

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

void Scoreboard::recvEmergency( uint8_t srcNode )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );

    sm_isEmergencyActive = true;

    switch ( srcNode )
    {
        case can_node_odr_display:
            sm_DisplayEmergency = true;
            break;

        case can_node_sensor_gps:
            sm_GpsEmergency = true;
            break;

        case can_node_odr_motion:
            sm_MotionEmergency = true;
            break;

        case can_node_odr_sonar_front:
            sm_SonarFrontEmergency = true;
            break;
    }
}

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

void Scoreboard::recvAllClear( uint8_t srcNode )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );

    switch ( srcNode )
    {
        case can_node_odr_display:
            sm_DisplayEmergency = false;
            break;

        case can_node_sensor_gps:
            sm_GpsEmergency = false;
            break;

        case can_node_odr_motion:
            sm_MotionEmergency = false;
            break;

        case can_node_odr_sonar_front:
            sm_SonarFrontEmergency = false;
            break;
    }
}

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

void Scoreboard::recvHeartbeat( uint8_t srcNode )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );

    switch ( srcNode )
    {
        case can_node_odr_display:
            m_timerDisplayTimeout.restart();
            sm_isDisplayAlive = true;
            break;

        case can_node_sensor_gps:
            m_timerGpsTimeout.restart();
            sm_isGpsAlive = true;
            break;

        case can_node_odr_motion:
            m_timerMotionTimeout.restart();
            sm_isMotionAlive = true;
            break;
    }
}

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

void Scoreboard::recvSonarFrontStatus( bool enabled )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );

    m_timerSonarFrontTimeout.restart();
    sm_isSonarFrontAlive = true;

    sm_isSonarFrontEnabled = enabled;
}

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

void Scoreboard::recvSonarFrontValues( CANMessage* msg )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );

    can_data_sonar_front info;
    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );

    sm_sonarFrontLeft        = info.left;
    sm_sonarFrontCenterLeft  = info.center_left;
    sm_sonarFrontCenterRight = info.center_right;
    sm_sonarFrontRight       = info.right;
}

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

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::recvImuRoll( CANMessage* msg )
{
    can_data_imu_data info;
    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );

    double roll = static_cast< double >( info.data )
                / static_cast< double >( can_data_imu_multiplier );

    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );

    sm_isImuAlive = true;
    m_timerImuTimeout.restart();
    
    sm_imuRoll = roll;
}

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

void Scoreboard::recvImuPitch( CANMessage* msg )
{
    can_data_imu_data info;
    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );

    double pitch = static_cast< double >( info.data )
                 / static_cast< double >( can_data_imu_multiplier );

    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );

    sm_isImuAlive = true;
    m_timerImuTimeout.restart();
    
    sm_imuPitch = pitch;
}

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

void Scoreboard::recvImuYaw( CANMessage* msg )
{
    can_data_imu_data info;
    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );

    double yaw = static_cast< double >( info.data )
               / static_cast< double >( can_data_imu_multiplier );

    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );

    sm_isImuAlive = true;
    m_timerImuTimeout.restart();
    
    sm_imuYaw = yaw;
}

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

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

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

        try
        {
            for ( ;; )
            {
                CANMessage* msg = CANMessage::WaitDequeueReceived( 250 ); // 250 ms

                if ( msg == 0 && m_quitEvent.tryWait( 0 ) )
                {
                    log.information( std::string( "Scoreboard::run() stopping" ) );
                    return;
                }

                if ( msg == 0 )
                {
                    continue;
                }

                uint8_t  srcNode;
                uint8_t  dstNode;
                uint16_t dataId;

                can_parse_message_id( 
                        msg->msgIdentifier(), &srcNode, &dstNode, &dataId );

                switch ( dataId )
                {
                    case can_dataid_emergency:
                        recvEmergency( srcNode );
                        break;

                    case can_dataid_all_clear:
                        recvAllClear( srcNode );
                        break;

                    case can_dataid_heartbeat:
                        recvHeartbeat( srcNode );
                        break;

                    case can_dataid_sonar_front_state_enabled:
                        recvSonarFrontStatus( true /* enabled */ );
                        break;

                    case can_dataid_sonar_front_state_disabled:
                        recvSonarFrontStatus( false /* not enabled */ );
                        break;

                    case can_dataid_sonar_front:
                        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;

                    case can_dataid_imu_roll:
                        recvImuRoll( msg );
                        break;

                    case can_dataid_imu_pitch:
                        recvImuPitch( msg );
                        break;

                    case can_dataid_imu_yaw:
                        recvImuYaw( msg );
                        break;
                }
            }
        }
        catch ( const Poco::Exception& ex )
        {
            log.error(
                Poco::Logger::format(
                    "Scoreboard::run() caught Poco::Exception: $0", ex.displayText() ) );
        }
        catch ( const std::exception& ex )
        {
            log.error(
                Poco::Logger::format(
                    "Scoreboard::run() caught std::exception: $0", ex.what() ) );
        }
        catch ( ... )
        {
            log.error( "Scoreboard::run() caught unknown exception" );
        }

        // sleep for 1/2 second after exception processing, but don't exit
        Poco::Thread::sleep( 500 );
    }
}

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