changeset 249:c36e268a624d main

Checkpoint for the UM7 parser, data structure definitions.
author Bob Cook <bob@bobcookdev.com>
date Mon, 24 Aug 2015 20:57:04 -0700
parents 1fd327ac6ad8
children ce9081347cd2
files main/packages/common/chrobotics/um7.cpp main/packages/common/chrobotics/um7.h main/packages/common/chrobotics/um7_data.h
diffstat 3 files changed, 107 insertions(+), 61 deletions(-) [+]
line wrap: on
line diff
--- a/main/packages/common/chrobotics/um7.cpp	Mon Aug 24 20:51:16 2015 -0700
+++ b/main/packages/common/chrobotics/um7.cpp	Mon Aug 24 20:57:04 2015 -0700
@@ -35,12 +35,12 @@
 
 #include "um7.h"
 
-#include <stdio.h>
-
 // ----------------------------------------------------------------------------------------
 
+static const uint8_t um7_max_data_size = 64;
+
 static uint8_t g_error_count;
-static uint8_t g_packet_data[ 64 ];
+static uint8_t g_packet_data[ um7_max_data_size ];
 static uint8_t g_packet_address;
 static uint8_t g_packet_size;
 
@@ -69,9 +69,6 @@
     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:
@@ -82,7 +79,6 @@
                 bytes_to_recv = 0;
                 calc_checksum = 's' + 'n' + 'p';
                 g_packet_size = 0;
-        printf( "%02x ", c );
             }
 
             break;
@@ -132,7 +128,6 @@
             {
                 bytes_to_recv = 0;
             }
-            printf( "bytes_to_recv %d\n", bytes_to_recv );
 
             state = state_recv_address;
             break;
@@ -150,7 +145,6 @@
             {
                 state = state_recv_chksum_hi;
             }
-            printf( "g_packet_address %d\n", g_packet_address );
 
             break;
 
@@ -162,7 +156,6 @@
             if ( --bytes_to_recv == 0 )
             {
                 state = state_recv_chksum_hi;
-                printf( "--- end of data ---\n" );
             }
 
             break;
@@ -181,33 +174,11 @@
 
             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;
@@ -301,6 +272,23 @@
 
 // ----------------------------------------------------------------------------------------
 
+uint8_t um7_packet_data( uint8_t* buffer, uint8_t buffer_len )
+{
+    uint8_t *source_data  = g_packet_data;
+    uint8_t bytes_to_copy = g_packet_size;
+
+    while ( bytes_to_copy > 0 && buffer_len > 0 )
+    {
+        *buffer++ = *source_data++;
+        --bytes_to_copy;
+        --buffer_len;
+    }
+
+    return ( g_packet_size - bytes_to_copy );
+}
+
+// ----------------------------------------------------------------------------------------
+
 uint8_t um7_create_packet( uint8_t*       buffer,
                            uint8_t        address,
                            const uint8_t* data,
@@ -314,13 +302,18 @@
 
     uint8_t packet_type = 0;
 
-    if ( data_len > 0 )
+    if ( data_len > 4 ) // more than 4 bytes means its a batch operation
     {
         packet_type |= data_len;
         packet_type <<= packet_type_batch_len_offset;
         packet_type &= packet_type_batch_len_mask;
+    }
+    
+    if ( data_len > 0 )
+    {
         packet_type |= packet_type_has_data;
     }
+
     *buffer++ = packet_type;
     *buffer++ = address;
 
@@ -331,6 +324,7 @@
 
     while ( data_len > 0 )
     {
+        checksum += *data;
         *buffer++ = *data++;
         --data_len;
     }
--- a/main/packages/common/chrobotics/um7.h	Mon Aug 24 20:51:16 2015 -0700
+++ b/main/packages/common/chrobotics/um7.h	Mon Aug 24 20:57:04 2015 -0700
@@ -93,6 +93,18 @@
 uint32_t um7_packet_value_u32( uint8_t offset );
 
 // ----------------------------------------------------------------------------------------
+//  um7_packet_data
+//
+//      Retrieve the entire data from the completed packet. The caller must provide a
+//      buffer large enough to contain the full packet data. Returns the number of bytes
+//      copied into the caller's buffer.
+//
+//      This value will only be meaningful if um7_parse() returns true.
+//
+
+uint8_t um7_packet_data( uint8_t* buffer, uint8_t buffer_len );
+
+// ----------------------------------------------------------------------------------------
 //  um7_create_packet
 //
 //      Construct a full packet ready to be sent to the UM7. The supplied buffer must
@@ -102,6 +114,9 @@
 //      Returns the number of bytes constructed in the ouput buffer.
 //
 
+static const uint8_t um7_min_packet_size =  7; // zero data
+static const uint8_t um7_max_packet_size = 71; // maximum data
+
 uint8_t um7_create_packet( uint8_t*       buffer,
                            uint8_t        address,
                            const uint8_t* data,
--- a/main/packages/common/chrobotics/um7_data.h	Mon Aug 24 20:51:16 2015 -0700
+++ b/main/packages/common/chrobotics/um7_data.h	Mon Aug 24 20:57:04 2015 -0700
@@ -41,14 +41,8 @@
 // ----------------------------------------------------------------------------------------
 
 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
+typedef struct
 {
     uint8_t raw_accel_rate;
     uint8_t raw_gyro_rate;
@@ -57,7 +51,11 @@
 
 } um7_creg_com_rates1;
 
-struct
+// ----------------------------------------------------------------------------------------
+
+const uint8_t um7_addr_creg_com_rates2 = 0x02;
+
+typedef struct
 {
     uint8_t temp_rate;
     uint8_t reserved1;
@@ -66,7 +64,11 @@
 
 } um7_creg_com_rates2;
 
-struct
+// ----------------------------------------------------------------------------------------
+
+const uint8_t um7_addr_creg_com_rates3 = 0x03;
+
+typedef struct
 {
     uint8_t proc_accel_rate;
     uint8_t proc_gyro_rate;
@@ -75,7 +77,11 @@
 
 } um7_creg_com_rates3;
 
-struct
+// ----------------------------------------------------------------------------------------
+
+const uint8_t um7_addr_creg_com_rates4 = 0x04;
+
+typedef struct
 {
     uint8_t reserved1;
     uint8_t reserved2;
@@ -84,7 +90,11 @@
 
 } um7_creg_com_rates4;
 
-struct
+// ----------------------------------------------------------------------------------------
+
+const uint8_t um7_addr_creg_com_rates5 = 0x05;
+
+typedef struct
 {
     uint8_t quat_rate;
     uint8_t euler_rate;
@@ -93,17 +103,30 @@
 
 } um7_creg_com_rates5;
 
-struct
+// ----------------------------------------------------------------------------------------
+
+const uint8_t um7_addr_creg_com_rates6 = 0x06;
+
+typedef struct
 {
     uint8_t  pose_rate;
-    uint8_t  reserved1 : 4;
-    uint8_t  health_rate : 4;
+    uint8_t  reserved1_and_health_rate;
     uint16_t reserved2;
 
+    inline void set_health_rate( uint8_t rate )
+    {
+        this->reserved1_and_health_rate = ( rate & 0x0f );
+    };
+
 } um7_creg_com_rates6;
 
-struct
+// ----------------------------------------------------------------------------------------
+
+const uint8_t um7_addr_creg_com_rates7 = 0x07;
+
+typedef struct
 {
+    // TODO: these bitfield decls probably don't work, gcc does not guarantee order
     uint8_t health_rate : 4;
     uint8_t pose_rate : 4;
     uint8_t attitude_rate : 4;
@@ -119,14 +142,17 @@
 
 const uint8_t um7_addr_dreg_health = 0x55;
 
-struct 
+const uint32_t um7_dreg_health_mask_ovf = 0x01 << 8;
+
+typedef struct 
 {
+    // TODO: these bitfield decls probably don't work, gcc does not guarantee order
     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  reserved2 : 2;
     uint8_t  mg_n : 1;
     uint8_t  acc_n : 1;
     uint8_t  accel : 1;
@@ -134,45 +160,51 @@
     uint8_t  mag : 1;
     uint8_t  gps : 1;
 
-} um7_dreg_health;
+} um7_dreg_health_t;
 
 // ----------------------------------------------------------------------------------------
 
-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;
+const uint8_t um7_addr_dreg_euler_phi_theta = 0x70;
+const uint8_t um7_addr_dreg_euler_psi       = 0x71;
 
-struct
+typedef struct
 {
     uint16_t phi;
     uint16_t theta;
 
 } um7_dreg_euler_phi_theta;
 
-struct
+typedef struct
 {
     uint16_t psi;
     uint16_t reserved;
 
 } um7_dreg_euler_psi;
 
-struct
+// ----------------------------------------------------------------------------------------
+
+const uint8_t um7_addr_dreg_euler_phi_theta_dot = 0x72;
+const uint8_t um7_addr_dreg_euler_psi_dot       = 0x73;
+
+typedef struct
 {
     uint16_t roll_rate;
     uint16_t pitch_rate;
 
 } um7_dreg_euler_phi_theta_dot;
 
-struct
+typedef struct
 {
     uint16_t yaw_rate;
     uint16_t reserved;
 
 } um7_dreg_euler_psi_dot;
 
-struct
+// ----------------------------------------------------------------------------------------
+
+const uint8_t um7_addr_dreg_euler_time = 0x74;
+
+typedef struct
 {
     uint32_t euler_time;
 
@@ -180,6 +212,11 @@
 
 // ----------------------------------------------------------------------------------------
 
+const uint8_t um7_cmd_get_fw_revision = 0xAA;
+const uint8_t um7_cmd_zero_gyros      = 0xAD;
+
+// ----------------------------------------------------------------------------------------
+
 #if !defined( __AVR__ )
 #pragma pack(pop)
 #endif