changeset 209:07ffde905dcc main

Latest changes. This is an outdated project so just keeping things in sync.
author Bob Cook <bob@bobcookdev.com>
date Thu, 17 Apr 2014 21:54:03 -0700
parents a99efff0a236
children 0e1a8cacc09d
files main/robots/odr-controller/canmsgs.cpp main/robots/odr-controller/display.cpp main/robots/odr-controller/func.h main/robots/odr-controller/main.cpp main/robots/odr-controller/project_defs.h main/robots/odr-controller/status.cpp
diffstat 6 files changed, 244 insertions(+), 38 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr-controller/canmsgs.cpp	Thu Apr 17 21:52:54 2014 -0700
+++ b/main/robots/odr-controller/canmsgs.cpp	Thu Apr 17 21:54:03 2014 -0700
@@ -32,6 +32,7 @@
 
 #include "project_defs.h"
 #include "func.h"
+#include "leds.h"
 
 #include "packages/avr/can/mcp2515.h"
 #include "packages/avr/device/spinwait.h"
@@ -158,6 +159,30 @@
 
 // ----------------------------------------------------------------------------------------
 
+#if 0
+static void update_latitude( const can_data_gps_data* data )
+{
+    display_update_latitude( data->degrees, data->min_thousandths );
+}
+
+// ----------------------------------------------------------------------------------------
+
+static void update_longitude( const can_data_gps_data* data )
+{
+    display_update_longitude( data->degrees, data->min_thousandths );
+}
+
+// ----------------------------------------------------------------------------------------
+
+static void update_gps_fix( const can_data_gps_fix* data )
+{
+    status_got_gps_fix( data->satellites > 0 );
+    display_update_gps_fix( data->satellites );
+}
+#endif
+
+// ----------------------------------------------------------------------------------------
+
 static void got_mgr_update( const can_data_odrmgr_update* data, uint8_t length )
 {
     status_got_mgr_update();
@@ -192,6 +217,8 @@
         return true; // not for us, everything ok
     }
 
+//    led_yellow_toggle();
+
     switch ( dataid )
     {
         case can_dataid_heartbeat:
@@ -227,6 +254,20 @@
             update_roll( reinterpret_cast< can_data_imu_data* >( &recvdata ) );
             break;
 
+#if 0
+        case can_dataid_latitude:
+            update_latitude( reinterpret_cast< can_data_gps_data* >( &recvdata ) );
+            break;
+
+        case can_dataid_longitude:
+            update_longitude( reinterpret_cast< can_data_gps_data* >( &recvdata ) );
+            break;
+
+        case can_dataid_gps_fix:
+            update_gps_fix( reinterpret_cast< can_data_gps_fix* >( &recvdata ) );
+            break;
+#endif
+
         case can_dataid_sonar_front_state_enabled:
             status_got_sonar_front_enabled();
             break;
--- a/main/robots/odr-controller/display.cpp	Thu Apr 17 21:52:54 2014 -0700
+++ b/main/robots/odr-controller/display.cpp	Thu Apr 17 21:54:03 2014 -0700
@@ -56,11 +56,10 @@
 //      7-|  front rangefinder  |-
 //      8-|   000 000 000 000   |-
 //      9-|                     |-
-//     10-| Y+000 P+00.0 R+00.0 |-
-//      1-| Target:+000 (+000)  |-
-//      1-|                     |-
+//     10-| lat: N 123 45.678 * |-
+//      1-| lon: W 123 45.678   |-
 //      2-|                     |-
-//      3-|                     |-
+//      3-| Y+000 P+00.0 R+00.0 |-
 //      4-|                     |-
 //      5-|ESTOP -A- -B-        |-
 //         ||||||||||||||||||||| 
@@ -137,7 +136,9 @@
     sfe569_write_pgm( PSTR("motion controller"), 2, 4, sfe569_color_white, sfe569_color_blue );
     sfe569_write_pgm( PSTR("f:   /    r:   /"), 1, 5, sfe569_color_white, sfe569_color_blue );
     sfe569_write_pgm( PSTR("front rangefinder"), 2, 7, sfe569_color_white, sfe569_color_blue );
-    sfe569_write_pgm( PSTR("Y---- P----- R-----"), 1, 10, sfe569_color_white, sfe569_color_blue );
+    sfe569_write_pgm( PSTR("lat: - ----------"), 1, 10, sfe569_color_white, sfe569_color_blue );
+    sfe569_write_pgm( PSTR("lon: - ----------"), 1, 11, sfe569_color_white, sfe569_color_blue );
+    sfe569_write_pgm( PSTR("Y---- P----- R-----"), 1, 13, sfe569_color_white, sfe569_color_blue );
 }
 
 // ----------------------------------------------------------------------------------------
@@ -242,12 +243,42 @@
 
 // ----------------------------------------------------------------------------------------
 
+#if 0
+static void update_gps_status()
+{
+    if ( ! status_is_gps_up() )
+    {
+        sfe569_write_pgm( PSTR("   offline    "),
+                          6, 10, sfe569_color_white, sfe569_color_red );
+        sfe569_write_pgm( PSTR("              "),
+                          6, 11, sfe569_color_white, sfe569_color_red );
+    }
+    else if ( ! status_does_gps_have_fix() )
+    {
+        sfe569_write_pgm( PSTR("- ---------- -"),
+                          6, 10, sfe569_color_white, sfe569_color_blue );
+        sfe569_write_pgm( PSTR("- ----------  "),
+                          6, 11, sfe569_color_white, sfe569_color_blue );
+    }
+    else
+    {
+        sfe569_write( ' ',  7, 10, sfe569_color_white, sfe569_color_blue );
+        sfe569_write( ' ',  7, 11, sfe569_color_white, sfe569_color_blue );
+        sfe569_write( ' ', 18, 10, sfe569_color_white, sfe569_color_blue );
+        sfe569_write( ' ', 18, 11, sfe569_color_white, sfe569_color_blue );
+    }
+}
+#endif
+
+// ----------------------------------------------------------------------------------------
+
 void display_refresh()
 {
     update_heartbeat_flag();
     update_estop_flag();
     update_wheel_info();
     update_sonar_front_info();
+//    update_gps_status();
 
     ++g_spinner_count;
     g_spinner_count %= 4;
@@ -413,7 +444,7 @@
     format_imu_value( value, buf );
     buf[ 4 ] = '\0';
 
-    sfe569_write( buf, 2, 10, sfe569_color_white, sfe569_color_blue );
+    sfe569_write( buf, 2, 13, sfe569_color_white, sfe569_color_blue );
 }
 
 // ----------------------------------------------------------------------------------------
@@ -427,7 +458,7 @@
     format_imu_value( value, buf );
     buf[ 1 ] = buf[ 0 ];
 
-    sfe569_write( &buf[ 1 ], 8, 10, sfe569_color_white, sfe569_color_blue );
+    sfe569_write( &buf[ 1 ], 8, 13, sfe569_color_white, sfe569_color_blue );
 }
 
 // ----------------------------------------------------------------------------------------
@@ -441,32 +472,122 @@
     format_imu_value( value, buf );
     buf[ 1 ] = buf[ 0 ];
 
-    sfe569_write( &buf[ 1 ], 15, 10, sfe569_color_white, sfe569_color_blue );
+    sfe569_write( &buf[ 1 ], 15, 13, sfe569_color_white, sfe569_color_blue );
 }
 
 // ----------------------------------------------------------------------------------------
-//      1-| Target:+000 (+000)  |-
-
-void display_update_nav_info( int16_t target, int16_t error )
+//  format the GPS value as "123 45.678" in the given buf (11 bytes minimum)
+#if 0
+static void format_gps_value( int16_t degrees, int32_t min_thousandths, char* buf )
 {
-#if 0
-    char buf[ 8 ];
+    char* p = buf + 3; // three digits for degrees followed by space
+    *p-- = ' ';
 
-    // target is displayed as a whole integer, dropping the fractional portion
+    for ( uint8_t i = 0; i < 3; ++i )
+    {
+        int16_t d = degrees % 10;
+        *p-- = ( '0' + d );
+        degrees /= 10;
+    }
+
+    // remove the leading zeros, if any
+
+    if ( buf[ 0 ] == '0' )
+    {
+        buf[ 0 ] = ' ';
 
-    format_imu_value( target, buf );
-    buf[ 4 ] = '\0';
+        if ( buf[ 1 ] == '0' )
+        {
+            buf[ 1 ] = ' ';
+        }
+    }
 
-    sfe569_write( buf, 8, 11, sfe569_color_white, sfe569_color_blue );
+    // now write the factional min_thousandths value
+
+    p = buf + 10; // degrees, space, two digits, decimal point, three digits, null
+    *p-- = '\0';
 
-    // error is displayed as a whole signed value
+    for ( uint8_t i = 0; i < 5; ++i )
+    {
+        int32_t d = min_thousandths % 10;
+        *p-- = ( '0' + d );
+        min_thousandths /= 10;
 
-    format_imu_value( error, buf );
-    buf[ 4 ] = '\0';
+        if ( i == 2 )
+        {
+            *p-- = '.';
+        }
+    }
 
-    sfe569_write( buf, 8, 11, sfe569_color_white, sfe569_color_blue );
-#endif
+    // remove the leading zero, if any
+
+    if ( buf[ 4 ] == '0' )
+    {
+        buf[ 4 ] = ' ';
+    }
 }
 
 // ----------------------------------------------------------------------------------------
 
+void display_update_latitude( int16_t degrees, int32_t min_thousandths )
+{
+    if ( degrees < 0 )
+    {
+        sfe569_write( 'S', 6, 10, sfe569_color_white, sfe569_color_blue );
+        degrees *= -1;
+    }
+    else
+    {
+        sfe569_write( 'N', 6, 10, sfe569_color_white, sfe569_color_blue );
+    }
+
+    char buf[ 12 ];
+    format_gps_value( degrees, min_thousandths, buf );
+    sfe569_write( buf, 8, 10, sfe569_color_white, sfe569_color_blue );
+}
+
+// ----------------------------------------------------------------------------------------
+
+void display_update_longitude( int16_t degrees, int32_t min_thousandths )
+{
+    if ( degrees < 0 )
+    {
+        sfe569_write( 'W', 6, 11, sfe569_color_white, sfe569_color_blue );
+        degrees *= -1;
+    }
+    else
+    {
+        sfe569_write( 'E', 6, 11, sfe569_color_white, sfe569_color_blue );
+    }
+
+    char buf[ 12 ];
+    format_gps_value( degrees, min_thousandths, buf );
+    sfe569_write( buf, 8, 11, sfe569_color_white, sfe569_color_blue );
+}
+
+// ----------------------------------------------------------------------------------------
+
+void display_update_gps_fix( uint8_t satellites )
+{
+    if ( satellites == 0 )
+    {
+        sfe569_write( '-', 19, 10, sfe569_color_white, sfe569_color_blue );
+        sfe569_write( ' ', 19, 11, sfe569_color_white, sfe569_color_blue );
+        return;
+    }
+
+    sfe569_write( '*', 19, 10, sfe569_color_white, sfe569_color_blue );
+
+    if ( satellites < 10 )
+    {
+        sfe569_write( '0' + satellites, 19, 11, sfe569_color_white, sfe569_color_blue );
+    }
+    else
+    {
+        sfe569_write( '*', 19, 11, sfe569_color_white, sfe569_color_blue );
+    }
+}
+#endif 
+
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr-controller/func.h	Thu Apr 17 21:52:54 2014 -0700
+++ b/main/robots/odr-controller/func.h	Thu Apr 17 21:54:03 2014 -0700
@@ -44,6 +44,7 @@
 void status_got_sonar_front_enabled();
 void status_got_sonar_front_disabled();
 void status_got_rf_message( const uint8_t* data, uint8_t length );
+void status_got_gps_fix( bool has_fix );
 
 bool status_is_emergency();
 bool status_is_mgr_up();
@@ -51,6 +52,8 @@
 bool status_is_motion_ctl_alive();
 bool status_is_sonar_front_up();
 bool status_is_sonar_front_enabled();
+bool status_is_gps_up();
+bool status_does_gps_have_fix();
 
 // ----------------------------------------------------------------------------------------
 
@@ -69,6 +72,10 @@
 void display_update_pitch( int32_t value );
 void display_update_roll( int32_t value );
 
+void display_update_latitude( int16_t degrees, int32_t min_thousandths );
+void display_update_longitude( int16_t degrees, int32_t min_thousandths );
+void display_update_gps_fix( uint8_t satellites );
+
 void display_update_button_1( bool down );
 void display_update_button_2( bool down );
 
--- a/main/robots/odr-controller/main.cpp	Thu Apr 17 21:52:54 2014 -0700
+++ b/main/robots/odr-controller/main.cpp	Thu Apr 17 21:54:03 2014 -0700
@@ -361,8 +361,9 @@
         //--    Sleep until there is a new interrupt (the CAN driver gets one, as does
         //      the periodic heartbeat, so we won't sleep forever).
         
-        sleep_mode();
-        
+        //sleep_mode();
+
+#if 1        
         //--    Check for an emergency situation?
 
         if ( g_triggers & trigger_check_emergency )
@@ -406,6 +407,7 @@
                 ++can_comm_errors;
             }
         }
+#endif
 
         //--    Time to refresh the display?
         
@@ -425,6 +427,7 @@
 
         //--    Time to send the heartbeat message?
 
+#if 0
         if ( g_triggers & trigger_send_heartbeat )
         {
             g_triggers &= ~trigger_send_heartbeat;
@@ -434,6 +437,7 @@
                 ++can_comm_errors;
             }
         }
+#endif
 
         //--    Any pending CAN messages to receive/process?
 
@@ -464,11 +468,11 @@
 
         if ( warning )
         {
-            led_yellow_on();
+//            led_yellow_on();
         }
         else
         {
-            led_yellow_off();
+//            led_yellow_off();
         }
     }
 
--- a/main/robots/odr-controller/project_defs.h	Thu Apr 17 21:52:54 2014 -0700
+++ b/main/robots/odr-controller/project_defs.h	Thu Apr 17 21:54:03 2014 -0700
@@ -116,9 +116,9 @@
 // ----------------------------------------------------------------------------------------
 //  packages/common/pololu/qik2s12v10
 
-#define PRJ_POLOLU_QIK2S12V10_ENABLE
-#define PRJ_POLOLU_QIK2S12V10_INPUT_AVR_UART0
-#define PRJ_POLOLU_QIK2S12V10_UART_BAUDRATE    9600
+//#define PRJ_POLOLU_QIK2S12V10_ENABLE
+//#define PRJ_POLOLU_QIK2S12V10_INPUT_AVR_UART0
+//#define PRJ_POLOLU_QIK2S12V10_UART_BAUDRATE    9600
 //#define PRJ_POLOLU_QIK2S12V10_USING_AUTO_BAUDRATE_DETECTION
 
 #endif // #if !defined( PROJECT_DEFS_H )
--- a/main/robots/odr-controller/status.cpp	Thu Apr 17 21:52:54 2014 -0700
+++ b/main/robots/odr-controller/status.cpp	Thu Apr 17 21:54:03 2014 -0700
@@ -43,12 +43,14 @@
 static volatile uint8_t g_beats_without_estop;
 static volatile uint8_t g_beats_without_motion_ctl;
 static volatile uint8_t g_beats_without_sonarfront;
+static volatile uint8_t g_beats_without_gps;
 
 static volatile uint8_t g_status_flags;
 
 static const uint8_t status_flag_emergency_active    = ( 1 << 0 );
 static const uint8_t status_flag_estop_triggered     = ( 1 << 1 );
 static const uint8_t status_flag_sonar_front_enabled = ( 1 << 2 );
+static const uint8_t status_flag_gps_has_fix         = ( 1 << 3 );
 
 // ----------------------------------------------------------------------------------------
 
@@ -74,7 +76,7 @@
     g_beats_without_estop = 100;
     g_beats_without_sonarfront = 100;
     g_beats_without_motion_ctl = 100;
-    // g_beats_without_gps = 100;
+    g_beats_without_gps = 100;
 }
 
 // ----------------------------------------------------------------------------------------
@@ -94,9 +96,9 @@
         case can_node_odr_sonar_front:
             break;
 
-//        case can_node_gps:
-//            g_beats_without_gps = 0;
-//            break;
+        case can_node_sensor_gps:
+            g_beats_without_gps = 0;
+            break;
     }
 }
 
@@ -154,6 +156,7 @@
     if ( g_beats_without_estop < 200 ) { ++g_beats_without_estop; }
     if ( g_beats_without_sonarfront < 200 ) { ++g_beats_without_sonarfront; }
     if ( g_beats_without_motion_ctl < 200 ) { ++g_beats_without_motion_ctl; }
+    if ( g_beats_without_gps < 200 ) { ++g_beats_without_gps; }
 
     //--    Has the ESTOP condition transitioned? Note we ignore this check if the
     //      override switch has been set.
@@ -167,7 +170,7 @@
 
         if ( status_is_emergency() )
         {
-            canmsg_send_all_clear();
+//            canmsg_send_all_clear();
         }
     }
     else if ( g_status_flags & status_flag_estop_triggered )
@@ -180,12 +183,12 @@
         {
             // switching from ESTOP triggered to not triggered
             g_status_flags &= ~status_flag_estop_triggered;
-            canmsg_send_all_clear();
+//            canmsg_send_all_clear();
         }
         else
         {
             // keep sending the emergency until the ESTOP is not triggered
-            canmsg_send_emergency();
+//            canmsg_send_emergency();
         }
     }
     else
@@ -198,12 +201,12 @@
         {
             // switching from ESTOP not triggered to triggered
             g_status_flags |= status_flag_estop_triggered;
-            canmsg_send_emergency();
+//            canmsg_send_emergency();
         }
         else if ( status_is_emergency() )
         {
             // keep sending the all-clear until we are out of the emergency state
-            canmsg_send_all_clear();
+//            canmsg_send_all_clear();
         }
     }
 }
@@ -296,3 +299,33 @@
 
 // ----------------------------------------------------------------------------------------
 
+bool status_is_gps_up()
+{
+    // more than four seconds == down
+    return ( g_beats_without_gps < 8 );
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool status_does_gps_have_fix()
+{
+    // more than four seconds == down
+    return ( g_status_flags & status_flag_gps_has_fix ) > 0;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void status_got_gps_fix( bool has_fix )
+{
+    if ( has_fix )
+    {
+        g_status_flags |= status_flag_gps_has_fix;
+    }
+    else
+    {
+        g_status_flags &= ~status_flag_gps_has_fix;
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+