view main/robots/odr/ImuApp.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
children
line wrap: on
line source

// ----------------------------------------------------------------------------------------
//
//  robots/odr/ImuApp.cpp
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
//
//  Application object for the IMU sensor bridge.
//
//  Copyright (c) 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 <iostream>
#include <memory>
#include <stdio.h>
#include <string>
#include <vector>

#include <Poco/Logger.h>
#include <Poco/Thread.h>
#include <Poco/Util/Application.h>
#include <Poco/Util/HelpFormatter.h>
#include <Poco/Util/LoggingSubsystem.h>
#include <Poco/Util/Option.h>
#include <Poco/Util/OptionSet.h>

#include "ImuApp.h"

#include "ImuReader.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"
#include "packages/linux/can/CANMsgProcessor.h"

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

void ImuApp::sendImuDataUpdate( Poco::Timer& timer )
{
    // if the IMU is not online then we don't send any messages

    if ( ! ImuReader::isImuAlive() )
    {
        return;
    }

    // YAW

    uint32_t imudatamsgid = can_build_message_id(
                    can_node_odr_manager, can_node_broadcast, can_dataid_imu_yaw );

    can_data_imu_data data;
    data.data = static_cast< int32_t >( ImuReader::imuYaw() * can_data_imu_multiplier );

    CANMessage::QueueToSend(
            new CANMessage( imudatamsgid,
                            reinterpret_cast< uint8_t* >( &data ),
                            sizeof( data ) ) );

    // PITCH

    imudatamsgid = can_build_message_id(
                    can_node_odr_manager, can_node_broadcast, can_dataid_imu_pitch );

    data.data = static_cast< int32_t >( ImuReader::imuPitch() * can_data_imu_multiplier );

    CANMessage::QueueToSend(
            new CANMessage( imudatamsgid,
                            reinterpret_cast< uint8_t* >( &data ),
                            sizeof( data ) ) );

    // ROLL

    imudatamsgid = can_build_message_id(
                    can_node_odr_manager, can_node_broadcast, can_dataid_imu_roll );

    data.data = static_cast< int32_t >( ImuReader::imuRoll() * can_data_imu_multiplier );

    CANMessage::QueueToSend(
            new CANMessage( imudatamsgid,
                            reinterpret_cast< uint8_t* >( &data ),
                            sizeof( data ) ) );
}

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

ImuApp::ImuApp()
    : m_helpRequested( false ),
      m_runloopActive( false ),
      m_timerSendImuDataUpdate()
{
}

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

ImuApp::~ImuApp()
{
}

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

void ImuApp::initialize( Poco::Util::Application& self )
{
    loadConfiguration();
    Poco::Util::ServerApplication::initialize( self );

    addSubsystem( new Poco::Util::LoggingSubsystem() );
}

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

void ImuApp::uninitialize()
{
    Poco::Util::ServerApplication::uninitialize();
}

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

void ImuApp::defineOptions( Poco::Util::OptionSet& options )
{
    Poco::Util::ServerApplication::defineOptions( options );

    options.addOption(
        Poco::Util::Option( "help", "h", "display argument help information" )
            .required( false )
            .repeatable( false )
            .callback( Poco::Util::OptionCallback<ImuApp>( this, &ImuApp::handleHelp ) ) );
}

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

void ImuApp::handleHelp( const std::string& name, const std::string& value )
{
    Poco::Util::HelpFormatter helpFormatter( options() );
    helpFormatter.setCommand( commandName() );
    helpFormatter.format( std::cout );
    stopOptionsProcessing();
    m_helpRequested = true;
}

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

int ImuApp::main( const std::vector<std::string>& args )
{
    if ( m_helpRequested )
    {
        return Poco::Util::Application::EXIT_OK;
    }

    loadConfiguration();

    logger().information( "------------------------------------------------------" );
    logger().information( "ImuApp::main() started" );

    std::string canInterfaceName = config().getString( "can.interfaceName", "can0" );
    logger().information( "using CANbus interface name \"" + canInterfaceName + "\"" );

    Poco::Thread    canMsgProcessorThread;
    CANMsgProcessor canMsgProcessor( canInterfaceName, logger().name() );
    canMsgProcessor.logIncomingMessages( config().getBool( "can.logIncoming", "no" ) );
    canMsgProcessor.logOutgoingMessages( config().getBool( "can.logOutgoing", "no" ) );
    canMsgProcessorThread.start( canMsgProcessor );

    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_timerSendImuDataUpdate.setPeriodicInterval( 750 /* milliseconds */ );
    m_timerSendImuDataUpdate.start(
            Poco::TimerCallback< ImuApp >( *this, &ImuApp::sendImuDataUpdate ) );

    m_runloopActive = true;
    while ( m_runloopActive ) Poco::Thread::sleep( 5000 /* milliseconds */ );

    imuReader.timeToQuit();
    imuReaderThread.join();

    canMsgProcessor.timeToQuit();
    canMsgProcessorThread.join();

    logger().information( "ImuApp::main() stopping" );

    return Poco::Util::Application::EXIT_OK;
}

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

int main( int argc, char** argv )
{
    int result = -1;

    try
    {
        ImuApp app;
        result = app.run( argc, argv );
    }
    catch ( const Poco::Exception& ex )
    {
        std::cerr << "Failed to start application, Poco::Exception: "
            << ex.displayText() << std::endl;
    }
    catch ( const std::exception& ex )
    {
        std::cerr << "Failed to start application, std::exception: "
            << ex.what() << std::endl;
    }
    catch ( ... )
    {
        std::cerr << "Failed to start application, unknown exception" << std::endl;
    }

    return result;
}

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