view main/packages/common/chrobotics/um7.cpp @ 245:44333100915a main

Initial checkin for the UM7 support code
author Bob Cook <bob@bobcookdev.com>
date Thu, 06 Aug 2015 21:47:44 -0700
parents
children c36e268a624d
line wrap: on
line source

// ----------------------------------------------------------------------------------------
//
//  common/chrobotics/um7.cpp
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
// 
//  Implements a state machine for parsing packets from the ChRobotics UM7.
//
//  Copyright (c) 2015 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.
//  
//  The ChRobotics UM7 documentation can be found at this link:
//
//      http://www.chrobotics.com/docs/UM7_Datasheet.pdf
//
// ----------------------------------------------------------------------------------------

#include "um7.h"

#include <stdio.h>

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

static uint8_t g_error_count;
static uint8_t g_packet_data[ 64 ];
static uint8_t g_packet_address;
static uint8_t g_packet_size;

const uint8_t state_packet_start_1   = 0;  // wait for 's'
const uint8_t state_packet_start_2   = 1;  // expecting 'n'
const uint8_t state_packet_start_3   = 2;  // expecting 'p'
const uint8_t state_recv_packet_type = 3;  // packet type byte
const uint8_t state_recv_address     = 4;  // register address
const uint8_t state_recv_data        = 5;  // data bytes
const uint8_t state_recv_chksum_hi   = 6;  // checksum high byte
const uint8_t state_recv_chksum_lo   = 7;  // checksum low byte

const uint8_t packet_type_has_data = ( 1 << 7 );
const uint8_t packet_type_is_batch = ( 1 << 6 );

// mask the input byte then shift right by the specified amount
const uint8_t packet_type_batch_len_mask   = 0x3C; 
const uint8_t packet_type_batch_len_offset = 2;

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

bool um7_parse( uint8_t c )
{
    static uint8_t  bytes_to_recv;
    static uint16_t calc_checksum;
    static uint16_t recv_checksum;
    static uint8_t  state = state_packet_start_1;

    if ( state != state_packet_start_1 )
        printf( "%02x ", c );

    switch ( state )
    {
        case state_packet_start_1:

            if ( c == 's' )
            {
                state = state_packet_start_2;
                bytes_to_recv = 0;
                calc_checksum = 's' + 'n' + 'p';
                g_packet_size = 0;
        printf( "%02x ", c );
            }

            break;

        case state_packet_start_2:

            if ( c == 'n' )
            {
                state = state_packet_start_3;
            }
            else
            {
                state = state_packet_start_1;
            }

            break;

        case state_packet_start_3:

            if ( c == 'p' )
            {
                state = state_recv_packet_type;
            }
            else
            {
                state = state_packet_start_1;
            }

            break;

        case state_recv_packet_type:

            calc_checksum += c;

            if ( c & packet_type_has_data )
            {
                bytes_to_recv = 4;

                if ( c & packet_type_is_batch )
                {
                    c &= packet_type_batch_len_mask;
                    c >>= packet_type_batch_len_offset;
                    bytes_to_recv *= c;
                }
            }
            else
            {
                bytes_to_recv = 0;
            }
            printf( "bytes_to_recv %d\n", bytes_to_recv );

            state = state_recv_address;
            break;

        case state_recv_address:

            g_packet_address = c;
            calc_checksum += c;

            if ( bytes_to_recv > 0 )
            {
                state = state_recv_data;
            }
            else
            {
                state = state_recv_chksum_hi;
            }
            printf( "g_packet_address %d\n", g_packet_address );

            break;

        case state_recv_data:

            calc_checksum += c;
            g_packet_data[ g_packet_size++ ] = c;
            
            if ( --bytes_to_recv == 0 )
            {
                state = state_recv_chksum_hi;
                printf( "--- end of data ---\n" );
            }

            break;

        case state_recv_chksum_hi:

            recv_checksum = c;
            recv_checksum <<= 8;
            state = state_recv_chksum_lo;
            break;

        case state_recv_chksum_lo:

            recv_checksum |= c;
            state = state_packet_start_1;

            if ( calc_checksum == recv_checksum )
            {
                printf( "--- checksum ok %04x\n", calc_checksum );
                return true;
            }
            else
            {
                g_error_count++;
                printf( "--- checksum error: recv %04x calc %04x\n", recv_checksum, calc_checksum );
                printf( "    g_packet_address %d\n", g_packet_address );
                printf( "    " );
                for ( uint8_t i = 0; i < g_packet_size; ++i )
                {
                    printf( "%02x ", g_packet_data[ i ] );
                }
                printf( "\n" );
                printf( "    " );
                for ( uint8_t i = 0; i < g_packet_size; ++i )
                {
                    if ( g_packet_data[ i ] > 32 && g_packet_data[ i ] < 127 )
                    {
                        printf( " %c ", g_packet_data[ i ] );
                    }
                    else
                    {
                        printf( " . " );
                    }
                }
                printf( "\n" );
            }

            break;
 
        default:

            // state machine error, start over at the next sentence
            state = state_packet_start_1;
            break;
    }

    return false;
}

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

uint8_t um7_error_count()
{
    return g_error_count;
}
    
// ----------------------------------------------------------------------------------------

uint8_t um7_packet_address()
{
    return g_packet_address;
}

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

uint8_t um7_packet_length()
{
    return g_packet_size;
}

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

int8_t  um7_packet_value_s08( uint8_t offset )
{
    return static_cast< int8_t >( g_packet_data[ offset ] );
}

uint8_t  um7_packet_value_u08( uint8_t offset )
{
    return g_packet_data[ offset ];
}

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

int16_t um7_packet_value_s16( uint8_t offset )
{
    uint16_t result = g_packet_data[ offset ];
    result <<= 8;
    result |= g_packet_data[ offset + 1 ];
    return static_cast< int16_t >( result );
}

uint16_t um7_packet_value_u16( uint8_t offset )
{
    uint16_t result = g_packet_data[ offset ];
    result <<= 8;
    result |= g_packet_data[ offset + 1 ];
    return result;
}

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

int32_t um7_packet_value_s32( uint8_t offset )
{
    uint32_t result = g_packet_data[ offset ];
    result <<= 8;
    result |= g_packet_data[ offset + 1 ];
    result <<= 8;
    result |= g_packet_data[ offset + 2 ];
    result <<= 8;
    result |= g_packet_data[ offset + 3 ];
    return static_cast< int32_t >( result );
}

uint32_t um7_packet_value_u32( uint8_t offset )
{
    uint32_t result = g_packet_data[ offset ];
    result <<= 8;
    result |= g_packet_data[ offset + 1 ];
    result <<= 8;
    result |= g_packet_data[ offset + 2 ];
    result <<= 8;
    result |= g_packet_data[ offset + 3 ];
    return result;
}

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

uint8_t um7_create_packet( uint8_t*       buffer,
                           uint8_t        address,
                           const uint8_t* data,
                           uint8_t        data_len )
{
    // create the header

    *buffer++ = 's';
    *buffer++ = 'n';
    *buffer++ = 'p';

    uint8_t packet_type = 0;

    if ( data_len > 0 )
    {
        packet_type |= data_len;
        packet_type <<= packet_type_batch_len_offset;
        packet_type &= packet_type_batch_len_mask;
        packet_type |= packet_type_has_data;
    }
    *buffer++ = packet_type;
    *buffer++ = address;

    uint16_t checksum   = 's' + 'n' + 'p' + packet_type + address;
    uint8_t  buf_length = 5 + data_len;

    // copy the data

    while ( data_len > 0 )
    {
        *buffer++ = *data++;
        --data_len;
    }

    // write the checksum

    *buffer++   = checksum >> 8;
    *buffer     = checksum & 0x0ff;
    buf_length += 2;

    return buf_length;
}

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