changeset 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 d1e26bffa503
children 603e27d44005
files main/packages/common/chrobotics/jamfile main/packages/common/chrobotics/um7.cpp main/packages/common/chrobotics/um7.h main/packages/common/chrobotics/um7_proto.h
diffstat 4 files changed, 691 insertions(+), 0 deletions(-) [+]
line wrap: on
line diff
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/packages/common/chrobotics/jamfile	Thu Aug 06 21:47:44 2015 -0700
@@ -0,0 +1,41 @@
+# -----------------------------------------------------------------------------------------
+#
+#   common/chrobotics/jamfile
+#
+#   Bob Cook Development, Robotics Library
+#   http://www.bobcookdev.com/rl/
+#
+#   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.
+#   
+# -----------------------------------------------------------------------------------------
+
+if $(TRACE) { echo "trace /packages/common/chrobotics/jamfile" ; }
+
+SubDir TOP packages common chrobotics ;
+
+# -----------------------------------------------------------------------------------------
+
+PackageSources
+    um7.cpp
+    ;
+    
+# -----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/packages/common/chrobotics/um7.cpp	Thu Aug 06 21:47:44 2015 -0700
@@ -0,0 +1,348 @@
+// ----------------------------------------------------------------------------------------
+//
+//  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;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/packages/common/chrobotics/um7.h	Thu Aug 06 21:47:44 2015 -0700
@@ -0,0 +1,113 @@
+// ----------------------------------------------------------------------------------------
+//
+//  common/chrobotics/um7.h
+//    
+//  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.
+//  
+//  Note: this package requires 73 bytes of RAM for state information.
+//
+// ----------------------------------------------------------------------------------------
+
+#if !defined( BCDRL_COMMON_CHROBOTICS_UM7_H )
+#define BCDRL_COMMON_CHROBOTICS_UM7_H
+
+#include <stdint.h>
+
+// ----------------------------------------------------------------------------------------
+//  um7_parse
+//
+//      Accept another byte for the UM7 packet parsing state machine.
+//
+//      Returns true when a full packet is correctly decoded.
+//
+
+bool um7_parse( uint8_t byte );
+
+// ----------------------------------------------------------------------------------------
+//  um7_error_count
+//
+//      Returns the accumulated parse error count.
+//
+
+uint8_t um7_error_count();
+
+// ----------------------------------------------------------------------------------------
+//  um7_packet_address
+//
+//      Return the address byte from the completed packet.
+//
+//      This value will only be meaningful if um7_parse() returns true.
+//
+
+uint8_t um7_packet_address();
+
+// ----------------------------------------------------------------------------------------
+//  um7_packet_length
+//
+//      Return the number of bytes received in the packet date.
+//
+//      This value will only be meaningful if um7_parse() returns true.
+//
+
+uint8_t um7_packet_length();
+
+// ----------------------------------------------------------------------------------------
+//  um7_packet_value_*
+//
+//      Retrieve a value of a specifed size from the completed packet. The offset is the
+//      the number of bytes from the start of the packet data.
+//
+//      This value will only be meaningful if um7_parse() returns true.
+//
+
+int8_t   um7_packet_value_s08( uint8_t offset );
+uint8_t  um7_packet_value_u08( uint8_t offset );
+
+int16_t  um7_packet_value_s16( uint8_t offset );
+uint16_t um7_packet_value_u16( uint8_t offset );
+
+int32_t  um7_packet_value_s32( uint8_t offset );
+uint32_t um7_packet_value_u32( uint8_t offset );
+
+// ----------------------------------------------------------------------------------------
+//  um7_create_packet
+//
+//      Construct a full packet ready to be sent to the UM7. The supplied buffer must
+//      be at least seven bytes larger than the input data. Its ok to provide NULL for
+//      the data ptr if the data length is zero. The maximum data length is 64 bytes.
+//
+//      Returns the number of bytes constructed in the ouput buffer.
+//
+
+uint8_t um7_create_packet( uint8_t*       buffer,
+                           uint8_t        address,
+                           const uint8_t* data,
+                           uint8_t        data_len );
+
+
+#endif  // #if defined( BCDRL_COMMON_CHROBOTICS_UM7_H )
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/packages/common/chrobotics/um7_proto.h	Thu Aug 06 21:47:44 2015 -0700
@@ -0,0 +1,189 @@
+// ----------------------------------------------------------------------------------------
+//
+//  common/chrobotics/um7_proto.h
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Structure definitions for the binary protocol of 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.
+//
+// ----------------------------------------------------------------------------------------
+
+#if !defined( BCDRL_COMMON_CHROBOTICS_UM7_PROTO_H )
+#define BCDRL_COMMON_CHROBOTICS_UM7_PROTO_H
+
+#include <stdint.h>
+
+#if !defined( __AVR__ )
+#pragma pack(push,1)
+#endif
+
+// ----------------------------------------------------------------------------------------
+
+const uint8_t um7_addr_creg_com_rates1 = 0x01;
+const uint8_t um7_addr_creg_com_rates2 = 0x02;
+const uint8_t um7_addr_creg_com_rates3 = 0x03;
+const uint8_t um7_addr_creg_com_rates4 = 0x04;
+const uint8_t um7_addr_creg_com_rates5 = 0x05;
+const uint8_t um7_addr_creg_com_rates6 = 0x06;
+const uint8_t um7_addr_creg_com_rates7 = 0x07;
+
+struct
+{
+    uint8_t raw_accel_rate;
+    uint8_t raw_gyro_rate;
+    uint8_t raw_mag_rate;
+    uint8_t reserved;
+
+} um7_creg_com_rates1;
+
+struct
+{
+    uint8_t temp_rate;
+    uint8_t reserved1;
+    uint8_t reserved2;
+    uint8_t all_raw_rate;
+
+} um7_creg_com_rates2;
+
+struct
+{
+    uint8_t proc_accel_rate;
+    uint8_t proc_gyro_rate;
+    uint8_t proc_mag_rate;
+    uint8_t reserved;
+
+} um7_creg_com_rates3;
+
+struct
+{
+    uint8_t reserved1;
+    uint8_t reserved2;
+    uint8_t reserved3;
+    uint8_t all_proc_rate;
+
+} um7_creg_com_rates4;
+
+struct
+{
+    uint8_t quat_rate;
+    uint8_t euler_rate;
+    uint8_t position_rate;
+    uint8_t velocity_rate;
+
+} um7_creg_com_rates5;
+
+struct
+{
+    uint8_t  pose_rate;
+    uint8_t  reserved1 : 4;
+    uint8_t  health_rate : 4;
+    uint16_t reserved2;
+
+} um7_creg_com_rates6;
+
+struct
+{
+    uint8_t health_rate : 4;
+    uint8_t pose_rate : 4;
+    uint8_t attitude_rate : 4;
+    uint8_t sensor_rate : 4;
+    uint8_t rates_rate : 4;
+    uint8_t gps_pose_rate : 4;
+    uint8_t quat_rate : 4;
+    uint8_t reserved1 : 4;
+
+} um7_creg_com_rates7;
+
+// ----------------------------------------------------------------------------------------
+
+const uint8_t um7_addr_dreg_health = 0x55;
+
+struct 
+{
+    uint8_t  sats_used : 6;
+    uint16_t hdop : 10;
+    uint8_t  sats_in_view : 6;
+    uint8_t  reserved1 : 1;
+    uint8_t  ovf : 1;
+    uint8_t  reserved2 : 1;
+    uint8_t  mg_n : 1;
+    uint8_t  acc_n : 1;
+    uint8_t  accel : 1;
+    uint8_t  gyro : 1;
+    uint8_t  mag : 1;
+    uint8_t  gps : 1;
+
+} um7_dreg_health;
+
+// ----------------------------------------------------------------------------------------
+
+const uint8_t um7_addr_dreg_euler_phi_theta     = 0x70;
+const uint8_t um7_addr_dreg_euler_psi           = 0x71;
+const uint8_t um7_addr_dreg_euler_phi_theta_dot = 0x72;
+const uint8_t um7_addr_dreg_euler_psi_dot       = 0x73;
+const uint8_t um7_addr_dreg_euler_time          = 0x74;
+
+struct
+{
+    uint16_t phi;
+    uint16_t theta;
+
+} um7_dreg_euler_phi_theta;
+
+struct
+{
+    uint16_t psi;
+    uint16_t reserved;
+
+} um7_dreg_euler_psi;
+
+struct
+{
+    uint16_t roll_rate;
+    uint16_t pitch_rate;
+
+} um7_dreg_euler_phi_theta_dot;
+
+struct
+{
+    uint16_t yaw_rate;
+    uint16_t reserved;
+
+} um7_dreg_euler_psi_dot;
+
+struct
+{
+    uint32_t euler_time;
+
+} um7_dreg_euler_time;
+
+// ----------------------------------------------------------------------------------------
+
+#if !defined( __AVR__ )
+#pragma pack(pop)
+#endif
+
+#endif  // #if defined( BCDRL_COMMON_CHROBOTICS_UM7_PROTO_H )
+// ----------------------------------------------------------------------------------------
+