view main/robots/odr-sim/ControllerStatus.cpp @ 231:783f69f37c64 main

Rework the simulator to better reflect the new/different nodes in the real hardware.
author Bob Cook <bob@bobcookdev.com>
date Sat, 12 Jul 2014 17:22:55 -0700
parents 44140fb96102
children
line wrap: on
line source

// ----------------------------------------------------------------------------------------
//
//  robots/odr-sim/ControllerStatus.cpp
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
// 
//  Thread that periodically sends the controller "status" message.
//
//  Copyright (c) 2011-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 "ControllerStatus.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"

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

Poco::RWLock ControllerStatus::sm_rwLock;
bool         ControllerStatus::sm_isDisplayAlive;
bool         ControllerStatus::sm_isGpsAlive;
bool         ControllerStatus::sm_isImuAlive;
bool         ControllerStatus::sm_isMotionAlive;
bool         ControllerStatus::sm_isSonarFrontAlive;

bool         ControllerStatus::sm_isEstopActive;
bool         ControllerStatus::sm_isMotorCtlActive;
bool         ControllerStatus::sm_isButtonOneDown;
bool         ControllerStatus::sm_isButtonTwoDown;

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

void ControllerStatus::setDisplayAlive( bool alive )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
    sm_isDisplayAlive = alive;
}

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

void ControllerStatus::setGpsAlive( bool alive )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
    sm_isGpsAlive = alive;
}

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

void ControllerStatus::setImuAlive( bool alive )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
    sm_isImuAlive = alive;
}

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

void ControllerStatus::setMotionAlive( bool alive )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
    sm_isMotionAlive = alive;
}

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

void ControllerStatus::setSonarFrontAlive( bool alive )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
    sm_isSonarFrontAlive = alive;
}

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

void ControllerStatus::sendButtonStatusMessage()
{
    uint32_t msgid = can_build_message_id( can_node_odr_controller,
                                           can_node_broadcast,
                                           can_dataid_odrctl_button );

    can_data_odrctl_button info;

    info.button_1 = sm_isButtonOneDown;
    info.button_2 = sm_isButtonTwoDown;

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

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

void ControllerStatus::setEstopActive( bool active )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
    sm_isEstopActive = active;
}

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

void ControllerStatus::setMotorCtlActive( bool active )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
    sm_isMotorCtlActive = active;
}

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

void ControllerStatus::setButtonOne( bool down )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );

    if ( down != sm_isButtonOneDown )
    {
        sm_isButtonOneDown = down;
        sendButtonStatusMessage();
    }
}

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

void ControllerStatus::setButtonTwo( bool down )
{
    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );

    if ( down != sm_isButtonTwoDown )
    {
        sm_isButtonTwoDown = down;
        sendButtonStatusMessage();
    }
}

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

ControllerStatus::ControllerStatus()
    : Poco::Runnable(),
      m_quitEvent( true /* auto-reset */ )
{
}

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

ControllerStatus::~ControllerStatus()
{
}

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

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

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

static void sendHeartbeatForNode( uint8_t theNode )
{
    uint32_t msgid = can_build_message_id( theNode,
                                           can_node_broadcast,
                                           can_dataid_heartbeat );

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

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

void ControllerStatus::run()
{
    for ( ;; )
    {
        try
        {
            Poco::RWLock::ScopedReadLock lock( sm_rwLock );

            if ( sm_isDisplayAlive )
            {
                sendHeartbeatForNode( can_node_odr_display );
            }

            if ( sm_isGpsAlive )
            {
                sendHeartbeatForNode( can_node_sensor_gps );
            }

            if ( sm_isImuAlive )
            {
                sendHeartbeatForNode( can_node_odr_display );
            }

            if ( sm_isMotionAlive )
            {
                sendHeartbeatForNode( can_node_odr_motion );
            }

            if ( sm_isSonarFrontAlive )
            {
                sendHeartbeatForNode( can_node_odr_sonar_front );
            }

        }
        catch ( ... )
        {
            // nothing to do, but don't stop the thread
        }

        if ( m_quitEvent.tryWait( 500 ) ) // 500 ms == 0.5s
        {
            return;
        }
    }
}

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