view main/robots/cantool/CmdListen.cpp @ 223:5f11f6622bc4 main

Write out the GPS messages.
author Bob Cook <bob@bobcookdev.com>
date Thu, 17 Apr 2014 22:01:14 -0700
parents 15d96880b4ba
children 79283dc7dc54
line wrap: on
line source

// ----------------------------------------------------------------------------------------
//
//  robots/cantool/CmdListen.cpp
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
//
//  Application object implementation for demonstrating the CANSocket class.    
//
//  Copyright (c) 2012-2013 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/NumberFormatter.h>

#include "Commands.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"

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

static void PrintHelp()
{
    std::cerr << "error: listen" << std::endl;
    std::cerr << "       (this command takes no parameters)" << std::endl;
}

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

static const char* NodeName( uint8_t node )
{
    switch ( node )
    {
        case can_node_odr_manager:
            return "odr-manager";

        case can_node_odr_controller:
            return "odr-controller";

        case can_node_odr_motion:
            return "odr-motion";

        case can_node_odr_sonar_front:
            return "odr-sonar-front";

        case can_node_sensor_gps:
            return "sensor-gps";

        default:
            return "(unknown node)";
    }
}

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

static void MsgGpsLatitude( const CANMessage* msg )
{
    can_data_gps_data info;
    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );

    int    deg = info.degrees;
    double min = static_cast< double >( info.min_thousandths )
               / can_data_gps_min_multiplier;

    std::cout << "gps latitude:  " << Poco::NumberFormatter::format( deg )
        << " " << Poco::NumberFormatter::format( min, 4 )
        << std::endl;
}

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

static void MsgGpsLongitude( const CANMessage* msg )
{
    can_data_gps_data info;
    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );

    int    deg = info.degrees;
    double min = static_cast< double >( info.min_thousandths )
               / can_data_gps_min_multiplier;

    std::cout << "gps longitude: " << Poco::NumberFormatter::format( deg )
        << " " << Poco::NumberFormatter::format( min, 4 )
        << std::endl;
}

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

static void MsgGpsFix( const CANMessage* msg )
{
    can_data_gps_fix info;
    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );

    std::cout << "gps fix (" << Poco::NumberFormatter::format( info.satellites )
        << " satellites)" << std::endl;
}

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

static void MsgSonarFrontUpdate( const CANMessage* msg )
{
    can_data_sonar_front info;
    msg->msgData( reinterpret_cast< uint8_t* >( &info ), sizeof( info ) );

    std::cout << "sonar front: " << Poco::NumberFormatter::format0( info.left, 3 )
        << " " << Poco::NumberFormatter::format0( info.center_left, 3 )
        << " " << Poco::NumberFormatter::format0( info.center_right, 3 )
        << " " << Poco::NumberFormatter::format0( info.right, 3 )
        << std::endl;
}

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

void CommandListen( const std::vector< std::string >& args )
{
    if ( args.size() > 1 )
    {
        PrintHelp();
        return;
    }

    std::cout << "listening (q to stop):" << std::endl;

    for ( ;; )
    {
//        if ( getchar() == 'q' )
//        {
//            return;
//        }

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

        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:
                std::cout << "emergency from " << NodeName( srcNode ) << std::endl;
                break;

            case can_dataid_all_clear:
                std::cout << "all-clear from " << NodeName( srcNode ) << std::endl;
                break;

            case can_dataid_heartbeat:
                std::cout << "heartbeat from " << NodeName( srcNode ) << std::endl;
                break;

            case can_dataid_latitude:
                MsgGpsLatitude( msg );
                std::cout << "message: " << CANMessage::asText( msg ) << std::endl;
                break;

            case can_dataid_longitude:
                MsgGpsLongitude( msg );
                std::cout << "message: " << CANMessage::asText( msg ) << std::endl;
                break;

            case can_dataid_gps_fix:
                MsgGpsFix( msg );
                break;

            case can_dataid_sonar_front:
                MsgSonarFrontUpdate( msg );
                break;

            case can_dataid_sonar_front_enable:
                std::cout << "sonar front: enable" << std::endl;
                break;

            case can_dataid_sonar_front_disable:
                std::cout << "sonar front: disable" << std::endl;
                break;
                
            default:
                std::cout << "message: " << CANMessage::asText( msg ) << std::endl;
                break;
        }

    }
}

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