view main/robots/odr-display/canmsgs.cpp @ 235:8af48f7c2f34 main

Initial commit for the odr-display node.
author Bob Cook <bob@bobcookdev.com>
date Sat, 12 Jul 2014 13:20:14 -0700
parents
children 43f43ebe35df
line wrap: on
line source

// ----------------------------------------------------------------------------------------
//
//  Copyright (C) 2013-2014 Bob Cook
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
// 
//  CAN related functions for the Outdoor Robot user interface node.
//
//  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 <avr/interrupt.h>
#include <avr/io.h>

#include "project_defs.h"
#include "func.h"
#include "leds.h"

#include "packages/avr/can/m1.h"
#include "packages/avr/device/spinwait.h"

#include "packages/common/can/can_helpers.h"
#include "packages/common/can/can_messages.h"
#include "packages/common/can/can_nodes.h"

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

bool canmsg_init()
{
    return m1can_init();
}

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

bool canmsg_did_error_occur()
{
    return ( m1can_status() != m1can_status_ok );
}

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

bool canmsg_send_emergency()
{
    return m1can_send( can_build_message_id( can_node_odr_display,
                                             can_node_broadcast,
                                             can_dataid_emergency ), 0, 0 );
}

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

bool canmsg_send_all_clear()
{
    return m1can_send( can_build_message_id( can_node_odr_display,
                                             can_node_broadcast,
                                             can_dataid_all_clear ), 0, 0 );
}

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

bool canmsg_send_heartbeat()
{
    return m1can_send( can_build_message_id( can_node_odr_display,
                                             can_node_broadcast,
                                             can_dataid_heartbeat ), 0, 0 );
}

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

#if 0
bool canmsg_send_command( uint8_t cmd, uint8_t data_1, uint8_t data_2 )
{
    uint32_t msgid = can_build_message_id( can_node_odr_display,
                                           can_node_broadcast,
                                           can_dataid_odr_display_cmd );

    can_data_odrctl_button info;

    info.button_1 = button_is_1_down();
    info.button_2 = button_is_2_down();

    if ( ! m1can_send( msgid, reinterpret_cast< uint8_t* >( &info ), sizeof( info ) ) )
    {
        return false;
    }
    
    return true;
}
#endif

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

static void update_wheel_status( const can_data_wheel_status* data )
{
    display_update_wheel_speed( data->actual_rpm_front, data->actual_rpm_rear );
    display_update_wheel_position( data->angle_front, data->angle_rear );
}

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

static void update_sonar_front( const can_data_sonar_front* data )
{
    display_update_sonar_front( data->left,
                                data->center_left,
                                data->center_right,
                                data->right );
}

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

static void update_yaw( const can_data_imu_data* data )
{
    display_update_yaw( data->data );
}

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

static void update_pitch( const can_data_imu_data* data )
{
    display_update_pitch( data->data );
}

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

static void update_roll( const can_data_imu_data* data )
{
    display_update_roll( data->data );
}

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

static void update_latitude( const can_data_gps_data* data )
{
    display_update_latitude( data->degrees, data->min_thousandths );
}

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

static void update_longitude( const can_data_gps_data* data )
{
    display_update_longitude( data->degrees, data->min_thousandths );
}

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

static void update_gps_fix( const can_data_gps_fix* data )
{
    status_got_gps_fix( data->satellites > 0 );
    display_update_gps_fix( data->satellites );
}

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

static void got_mgr_update( const can_data_odrmgr_update* data, uint8_t length )
{
    status_got_mgr_update();

    if ( length > 0 )
    {
        display_set_mgr_status( data->msg, length );
    }
}

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

bool canmsg_process_pending()
{
    uint32_t recvid;
    bool     recvrequest = false;
    uint8_t  recvdata[ 8 ];
    uint8_t  recvlen = sizeof( recvdata );

    for ( ;; )
    {
        if ( ! m1can_read( &recvid, &recvrequest, recvdata, &recvlen ) )
        {
            return true; // no messages, everything ok
        }

        uint8_t  srcnode;
        uint8_t  dstnode;
        uint16_t dataid;
        can_parse_message_id( recvid, &srcnode, &dstnode, &dataid );

        if ( dstnode != can_node_broadcast && dstnode != can_node_odr_display )
        {
            continue; // not for us, everything ok, loop around and try again
        }

        switch ( dataid )
        {
            case can_dataid_heartbeat:
                status_got_heartbeat( srcnode );
                break;

            case can_dataid_emergency:
                status_got_emergency( srcnode );
                break;

            case can_dataid_all_clear:
                status_got_all_clear( srcnode );
                break;

            case can_dataid_odrmgr_update:
                got_mgr_update(
                        reinterpret_cast< can_data_odrmgr_update* >( &recvdata ), recvlen );
                break;

            case can_dataid_wheel_status:
                update_wheel_status( reinterpret_cast< can_data_wheel_status* >( &recvdata ) );
                break;

            case can_dataid_imu_yaw:
                update_yaw( reinterpret_cast< can_data_imu_data* >( &recvdata ) );
                break;

            case can_dataid_imu_pitch:
                update_pitch( reinterpret_cast< can_data_imu_data* >( &recvdata ) );
                break;

            case can_dataid_imu_roll:
                update_roll( reinterpret_cast< can_data_imu_data* >( &recvdata ) );
                break;

            case can_dataid_gps_latitude:
                update_latitude( reinterpret_cast< can_data_gps_data* >( &recvdata ) );
                break;

            case can_dataid_gps_longitude:
                update_longitude( reinterpret_cast< can_data_gps_data* >( &recvdata ) );
                break;

            case can_dataid_gps_fix:
                update_gps_fix( reinterpret_cast< can_data_gps_fix* >( &recvdata ) );
                break;

            case can_dataid_origin_lat:
                update_latitude( reinterpret_cast< can_data_gps_data* >( &recvdata ) );
                break;

            case can_dataid_origin_long:
                update_longitude( reinterpret_cast< can_data_gps_data* >( &recvdata ) );
                break;

            case can_dataid_target_lat:
                update_latitude( reinterpret_cast< can_data_gps_data* >( &recvdata ) );
                break;

            case can_dataid_target_long:
                update_longitude( reinterpret_cast< can_data_gps_data* >( &recvdata ) );
                break;

            case can_dataid_sonar_front_state_enabled:
                status_got_sonar_front_enabled();
                break;

            case can_dataid_sonar_front_state_disabled:
                status_got_sonar_front_disabled();
                break;

            case can_dataid_sonar_front:
                update_sonar_front( reinterpret_cast< can_data_sonar_front* >( &recvdata ) );
                break;
        }
    }

    return true; // no errors
}

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