changeset 168:cf51b02db081 main

Show the IMU values on the display.
author Bob Cook <bob@bobcookdev.com>
date Tue, 01 Jan 2013 11:32:19 -0800
parents 827befd71be0
children 61495fdd34c5
files main/robots/odr-controller/canmsgs.cpp main/robots/odr-controller/display.cpp main/robots/odr-controller/func.h
diffstat 3 files changed, 86 insertions(+), 20 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr-controller/canmsgs.cpp	Tue Jan 01 11:28:23 2013 -0800
+++ b/main/robots/odr-controller/canmsgs.cpp	Tue Jan 01 11:32:19 2013 -0800
@@ -145,6 +145,20 @@
 
 // ----------------------------------------------------------------------------------------
 
+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 got_mgr_update( const can_data_odrmgr_update* data, uint8_t length )
 {
     status_got_mgr_update();
@@ -202,6 +216,14 @@
             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_sonar_front_state_enabled:
             status_got_sonar_front_enabled();
             break;
--- a/main/robots/odr-controller/display.cpp	Tue Jan 01 11:28:23 2013 -0800
+++ b/main/robots/odr-controller/display.cpp	Tue Jan 01 11:32:19 2013 -0800
@@ -32,6 +32,7 @@
 #include "func.h"
 
 #include "packages/common/util/printutils.h"
+#include "packages/common/util/misc.h"
 
 #include "packages/avr/lcd/sfe569/sfe569.h"
 
@@ -55,7 +56,7 @@
 //      7-|     front sonar     |-
 //      8-| 0000 0000 0000 0000 |-
 //      9-|                     |-
-//     10-|    yaw: +000.000    |-
+//     10-| Y+000 P+00.0 R+00.0 |-
 //      1-|                     |-
 //      2-|                     |-
 //      3-|                     |-
@@ -177,7 +178,7 @@
     sfe569_write_pgm( PSTR("motor f:      r:"), 0, 4, sfe569_color_white, sfe569_color_blue );
     sfe569_write_pgm( PSTR("servo f:      r:"), 0, 5, sfe569_color_white, sfe569_color_blue );
     sfe569_write_pgm( PSTR("front sonar"), 5, 7, sfe569_color_white, sfe569_color_blue );
-    sfe569_write_pgm( PSTR("yaw:  ---.---"), 4, 10, sfe569_color_white, sfe569_color_blue );
+    sfe569_write_pgm( PSTR("Y---- P----- R-----"), 1, 10, sfe569_color_white, sfe569_color_blue );
 }
 
 // ----------------------------------------------------------------------------------------
@@ -251,15 +252,6 @@
 
 // ----------------------------------------------------------------------------------------
 
-static void update_yaw_info()
-{
-
-//     10-|    yaw: +000.000    |-
-//
-}
-
-// ----------------------------------------------------------------------------------------
-
 void display_refresh()
 {
     update_heartbeat_flag();
@@ -357,13 +349,11 @@
 }
 
 // ----------------------------------------------------------------------------------------
+//  format the IMU value as "+123.4" in the given buf (which should be 7 bytes minimum)
 
-void display_update_yaw( int32_t value )
+static void format_imu_value( int32_t value, char* buf )
 {
-    // the value is scaled by 10,000, so +123.4567 becomes 1,234,567.
-
-    char  buf[ 8 ];
-    char* p = buf + array_sizeof( buf ) - 1;
+    char* p = buf + 6; // sign, three digits, decimal pt, one digit, null
     *p-- = '\0';
 
     *buf = '+';
@@ -374,22 +364,74 @@
         value *= -1;
     }
 
-    value /= 10; // discard least significant digit
+    // value is scaled by 10,000, so +123.4567 becomes 1,234,567
+    value /= 1000; // discard three least significant digits
 
-    for ( uint8_t i = 0; i < 6; ++i )
+    for ( uint8_t i = 0; i < 4; ++i )
     {
         int32_t d = value % 10;
         *p-- = ( '0' + d );
         value /= 10;
 
-        if ( i == 2 )
+        if ( i == 0 )
         {
             *p-- = '.';
         }
     }
 
-    sfe569_write( buf, 9, 10, sfe569_color_white, sfe569_color_blue );
+    // remove the leading zeros, if any
+
+    if ( buf[ 1 ] == '0' )
+    {
+        buf[ 1 ] = ' ';
+
+        if ( buf[ 2 ] == '0' )
+        {
+            buf[ 2 ] = ' ';
+        }
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+void display_update_yaw( int32_t value )
+{
+    // yaw is displayed as a whole integer, dropping the fractional portion
+
+    char  buf[ 8 ];
+    format_imu_value( value, buf );
+    buf[ 4 ] = '\0';
+
+    sfe569_write( buf, 2, 10, sfe569_color_white, sfe569_color_blue );
 }
 
 // ----------------------------------------------------------------------------------------
 
+void display_update_pitch( int32_t value )
+{
+    // pitch is displayed as a the least two significant integral digits plus one digit
+    // from the fraction e.g. "+xx.y"
+
+    char  buf[ 8 ];
+    format_imu_value( value, buf );
+    buf[ 1 ] = buf[ 0 ];
+
+    sfe569_write( &buf[ 1 ], 8, 10, sfe569_color_white, sfe569_color_blue );
+}
+
+// ----------------------------------------------------------------------------------------
+
+void display_update_roll( int32_t value )
+{
+    // roll is displayed as a the least two significant integral digits plus one digit
+    // from the fraction e.g. "+xx.y"
+
+    char  buf[ 8 ];
+    format_imu_value( value, buf );
+    buf[ 1 ] = buf[ 0 ];
+
+    sfe569_write( &buf[ 1 ], 15, 10, sfe569_color_white, sfe569_color_blue );
+}
+
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr-controller/func.h	Tue Jan 01 11:28:23 2013 -0800
+++ b/main/robots/odr-controller/func.h	Tue Jan 01 11:32:19 2013 -0800
@@ -59,6 +59,8 @@
 void display_update_sonar_front( uint16_t l, uint16_t c_l, uint16_t c_r, uint16_t r );
 
 void display_update_yaw( int32_t value );
+void display_update_pitch( int32_t value );
+void display_update_roll( int32_t value );
 
 void display_update_button_1( bool down );
 void display_update_button_2( bool down );