changeset 158:7fac5705d135 main

Adding the display value for IMU Yaw.
author Bob Cook <bob@bobcookdev.com>
date Sat, 08 Sep 2012 13:19:43 -0700
parents 104286325311
children 19626dab8d31
files main/robots/odr-controller/canmsgs.cpp main/robots/odr-controller/display.cpp main/robots/odr-controller/func.h
diffstat 3 files changed, 61 insertions(+), 3 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr-controller/canmsgs.cpp	Sat Sep 08 13:18:55 2012 -0700
+++ b/main/robots/odr-controller/canmsgs.cpp	Sat Sep 08 13:19:43 2012 -0700
@@ -138,6 +138,13 @@
 
 // ----------------------------------------------------------------------------------------
 
+static void update_yaw( const can_data_imu_data* data )
+{
+    display_update_yaw( data->data );
+}
+
+// ----------------------------------------------------------------------------------------
+
 static void got_mgr_update( const can_data_odrmgr_update* data, uint8_t length )
 {
     status_got_mgr_update();
@@ -191,6 +198,10 @@
             update_motors( reinterpret_cast< can_data_motor_speed* >( &recvdata ) );
             break;
 
+        case can_dataid_imu_yaw:
+            update_yaw( 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	Sat Sep 08 13:18:55 2012 -0700
+++ b/main/robots/odr-controller/display.cpp	Sat Sep 08 13:19:43 2012 -0700
@@ -1,6 +1,6 @@
 // ----------------------------------------------------------------------------------------
 //
-//  Copyright (C) 2011 Bob Cook
+//  Copyright (C) 2012 Bob Cook
 //    
 //  Bob Cook Development, Robotics Library
 //  http://www.bobcookdev.com/rl/
@@ -55,7 +55,7 @@
 //      7-|     front sonar     |-
 //      8-| 0000 0000 0000 0000 |-
 //      9-|                     |-
-//     10-|                     |-
+//     10-|    yaw: +000.000    |-
 //      1-|                     |-
 //      2-|                     |-
 //      3-|                     |-
@@ -177,6 +177,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 );
 }
 
 // ----------------------------------------------------------------------------------------
@@ -250,6 +251,15 @@
 
 // ----------------------------------------------------------------------------------------
 
+static void update_yaw_info()
+{
+
+//     10-|    yaw: +000.000    |-
+//
+}
+
+// ----------------------------------------------------------------------------------------
+
 void display_refresh()
 {
     update_heartbeat_flag();
@@ -348,3 +358,38 @@
 
 // ----------------------------------------------------------------------------------------
 
+void display_update_yaw( int32_t value )
+{
+    // the value is scaled by 10,000, so +123.4567 becomes 1,234,567.
+
+    char  buf[ 8 ];
+    char* p = buf + array_sizeof( buf ) - 1;
+    *p-- = '\0';
+
+    *buf = '+';
+
+    if ( value < 0 )
+    {
+        *buf = '-';
+        value *= -1;
+    }
+
+    value /= 10; // discard least significant digit
+
+    for ( uint8_t i = 0; i < 6; ++i )
+    {
+        int32_t d = value % 10;
+        *p-- = ( '0' + d );
+        value /= 10;
+
+        if ( i == 2 )
+        {
+            *p-- = '.';
+        }
+    }
+
+    sfe569_write( buf, 9, 10, sfe569_color_white, sfe569_color_blue );
+}
+
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr-controller/func.h	Sat Sep 08 13:18:55 2012 -0700
+++ b/main/robots/odr-controller/func.h	Sat Sep 08 13:19:43 2012 -0700
@@ -1,6 +1,6 @@
 // ----------------------------------------------------------------------------------------
 //
-//  Copyright (C) 2011 Bob Cook
+//  Copyright (C) 2012 Bob Cook
 //    
 //  Bob Cook Development, Robotics Library
 //  http://www.bobcookdev.com/rl/
@@ -58,6 +58,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_button_1( bool down );
 void display_update_button_2( bool down );