view main/robots/odr-imu/canmsgs.cpp @ 251:87864f3a89e4 main

IMU module code.
author Bob Cook <bob@bobcookdev.com>
date Mon, 07 Sep 2015 14:49:45 -0700
parents 1fd327ac6ad8
children
line wrap: on
line source

// ----------------------------------------------------------------------------------------
//
//  Copyright (C) 2015 Bob Cook
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
// 
//  CAN related functions for the Outdoor Robot IMU 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 "project_defs.h"

#include "func.h"

#include "packages/avr/can/m1.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_send_emergency()
{
    return m1can_send( can_build_message_id( can_node_sensor_imu,
                                             can_node_broadcast,
                                             can_dataid_emergency ), 0, 0 );
}

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

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

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

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

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

bool canmsg_send_imu_roll( float roll )
{
    uint32_t msgid;
    msgid = can_build_message_id( can_node_sensor_imu,
                                  can_node_broadcast,
                                  can_dataid_imu_roll );

    can_data_imu_data data;

    roll *= static_cast< float >( can_data_imu_multiplier );
    data.data = static_cast< int32_t >( roll );

    return m1can_send( msgid, reinterpret_cast< uint8_t* >( &data ), sizeof( data ) );
}

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

bool canmsg_send_imu_pitch( float pitch )
{
    uint32_t msgid;
    msgid = can_build_message_id( can_node_sensor_imu,
                                  can_node_broadcast,
                                  can_dataid_imu_pitch );

    can_data_imu_data data;

    pitch *= static_cast< float >( can_data_imu_multiplier );
    data.data = static_cast< int32_t >( pitch );

    return m1can_send( msgid, reinterpret_cast< uint8_t* >( &data ), sizeof( data ) );
}

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

bool canmsg_send_imu_yaw( float yaw )
{
    uint32_t msgid;
    msgid = can_build_message_id( can_node_sensor_imu,
                                  can_node_broadcast,
                                  can_dataid_imu_yaw );

    can_data_imu_data data;

    yaw *= static_cast< float >( can_data_imu_multiplier );
    data.data = static_cast< int32_t >( yaw );

    return m1can_send( msgid, reinterpret_cast< uint8_t* >( &data ), sizeof( data ) );
}

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

bool canmsg_send_imu_health( uint32_t health )
{
    uint32_t msgid;
    msgid = can_build_message_id( can_node_sensor_imu,
                                  can_node_broadcast,
                                  can_dataid_imu_health );

    can_data_imu_health data;
    data.health = health;

    return m1can_send( msgid, reinterpret_cast< uint8_t* >( &data ), sizeof( data ) );
}

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

static void update_imu_yaw_adj( const can_data_imu_data* data )
{
    float adjustment = static_cast< float >( data->data );
    adjustment /= static_cast< float >( can_data_imu_multiplier );

    imu_set_yaw_adjustment( adjustment );
}

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

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_sensor_imu )
        {
            continue; // not for us, try again
        }

        switch ( dataid )
        {
            case can_dataid_emergency:
                status_got_emergency( srcnode );
                break;

            case can_dataid_all_clear:
                status_got_all_clear( srcnode );
                break;

            case can_dataid_heartbeat:
            case can_dataid_odrmgr_update:
                status_got_heartbeat( srcnode );
                break;

            case can_dataid_imu_zero_gyros:
                imu_zero_gyros();
                break;

            case can_dataid_imu_yaw_adj:
                update_imu_yaw_adj(
                        reinterpret_cast< const can_data_imu_data* >( &recvdata ) );
                break;
        }
    }

    return true; // no errors
}

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