changeset 121:e65210fe1cb0 main

checkpoint the front-facing sonar firmware.
author Bob Cook <bob@bobcookdev.com>
date Sat, 03 Dec 2011 16:38:46 -0800
parents 9a0d24a0706e
children 0448b2b7c530 03f847019642
files main/robots/odr-sonar-front/canmsgs.cpp main/robots/odr-sonar-front/func.h main/robots/odr-sonar-front/main.cpp main/robots/odr-sonar-front/sonar.cpp
diffstat 4 files changed, 71 insertions(+), 23 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr-sonar-front/canmsgs.cpp	Sat Dec 03 16:33:39 2011 -0800
+++ b/main/robots/odr-sonar-front/canmsgs.cpp	Sat Dec 03 16:38:46 2011 -0800
@@ -37,6 +37,8 @@
 #include "packages/common/can/can_messages.h"
 #include "packages/common/can/can_nodes.h"
 
+#include "packages/avr/device/spinwait.h"
+
 // ----------------------------------------------------------------------------------------
 
 bool canmsg_init()
@@ -46,15 +48,24 @@
 
 // ----------------------------------------------------------------------------------------
 
-bool canmsg_send_heartbeat()
+bool canmsg_send_status( bool enabled )
 {
-    uint32_t msgid = can_build_message_id( can_node_odr_sonar_front,
-                                           can_node_broadcast,
-                                           can_dataid_heartbeat );
+    uint32_t msgid;
 
-    uint32_t data = can_heartbeat_data;
+    if ( enabled )
+    {
+        msgid = can_build_message_id( can_node_odr_sonar_front,
+                                      can_node_broadcast,
+                                      can_dataid_sonar_front_state_enabled );
+    }
+    else
+    {
+        msgid = can_build_message_id( can_node_odr_sonar_front,
+                                      can_node_broadcast,
+                                      can_dataid_sonar_front_state_disabled );
+    }
 
-    return m1can_send( msgid, reinterpret_cast< uint8_t* >( &data ), sizeof( data ) );
+    return m1can_send( msgid, 0, 0 );
 }
 
 // ----------------------------------------------------------------------------------------
@@ -100,11 +111,11 @@
 
     switch ( dataid )
     {
-        case can_dataid_start_sonar_front:
+        case can_dataid_sonar_front_enable:
             sonar_start();
             break;
 
-        case can_dataid_stop_sonar_front:
+        case can_dataid_sonar_front_disable:
             sonar_stop();
             break;
     }
--- a/main/robots/odr-sonar-front/func.h	Sat Dec 03 16:33:39 2011 -0800
+++ b/main/robots/odr-sonar-front/func.h	Sat Dec 03 16:38:46 2011 -0800
@@ -37,12 +37,13 @@
 void sonar_init();
 void sonar_start();
 void sonar_stop();
+bool sonar_is_running();
 bool sonar_send_update();
 
 // ----------------------------------------------------------------------------------------
 
 bool canmsg_init();
-bool canmsg_send_heartbeat();
+bool canmsg_send_status( bool enabled );
 bool canmsg_send_sonar_front( uint16_t left, uint16_t center, uint16_t right );
 bool canmsg_process_pending();
 
--- a/main/robots/odr-sonar-front/main.cpp	Sat Dec 03 16:33:39 2011 -0800
+++ b/main/robots/odr-sonar-front/main.cpp	Sat Dec 03 16:38:46 2011 -0800
@@ -43,13 +43,14 @@
 
 // ----------------------------------------------------------------------------------------
 
-static const uint8_t trigger_send_heartbeat = ( 1 << 0 );
+static const uint8_t trigger_send_sonar_status = ( 1 << 0 );
+static const uint8_t trigger_send_sonar_update = ( 1 << 1 );
 
 static volatile uint8_t g_triggers;
 
 // ----------------------------------------------------------------------------------------
 
-inline void leds_off()
+static void leds_off()
 {
     PORTB |= ( 1 << PB2 );
     PORTC |= ( 1 << PC4 ) | ( 1 << PC5 );
@@ -58,17 +59,18 @@
 
 // ----------------------------------------------------------------------------------------
 
-inline void led_red_off()
+static inline void led_red_off() __attribute__((always_inline));
+static inline void led_red_off()
 {
     PORTD |= ( 1 << PD7 );
 }
 
-inline void led_red_on()
+static inline void led_red_on()
 {
     PORTD &= ~( 1 << PD7 );
 }
 
-inline void led_red_toggle()
+static inline void led_red_toggle()
 {
     PORTD ^= ( 1 << PD7 );
 }
@@ -228,6 +230,10 @@
         odrson_fatal_error( odrson_fatal_error_can_init );
     }
 
+    //--    Initialize the sonar subsystem.
+
+    sonar_init();
+
     //--    When we sleep we want the "idle" mode e.g. wake up on any interrupt.
 
     set_sleep_mode( SLEEP_MODE_IDLE );
@@ -243,10 +249,21 @@
 {
     static uint8_t count = 0;
 
-    if ( ++count > 100 )
+    // sonar status: every 2.5 seconds
+    // sonar update: three times every 2.5 seconds
+    
+    switch ( ++count )
     {
-        g_triggers |= trigger_send_heartbeat;
-        count = 0;
+        case 83:
+        case 167:
+            g_triggers |= trigger_send_sonar_update;
+            break;
+
+        case 250:
+            g_triggers |= trigger_send_sonar_status;
+            g_triggers |= trigger_send_sonar_update;
+            count = 0;
+            break;
     }
 }
 
@@ -269,11 +286,11 @@
 
         sleep_mode();
 
-        //--    Time to send the heartbeat message?
+        //--    Time to send the status message?
 
-        if ( g_triggers & trigger_send_heartbeat )
+        if ( g_triggers & trigger_send_sonar_status )
         {
-            g_triggers &= ~trigger_send_heartbeat;
+            g_triggers &= ~trigger_send_sonar_status;
 
             led_green_1_on();
             spinwait_delay_ms( 60 );
@@ -283,7 +300,19 @@
             spinwait_delay_ms( 60 );
             led_green_1_off();
 
-            if ( ! canmsg_send_heartbeat() )
+            if ( ! canmsg_send_status( sonar_is_running() ) )
+            {
+                ++can_comm_errors;
+            }
+        }
+
+        //--    Time to send a sonar update message?
+
+        if ( g_triggers & trigger_send_sonar_update )
+        {
+            g_triggers &= ~trigger_send_sonar_update;
+
+            if ( ! sonar_send_update() )
             {
                 ++can_comm_errors;
             }
--- a/main/robots/odr-sonar-front/sonar.cpp	Sat Dec 03 16:33:39 2011 -0800
+++ b/main/robots/odr-sonar-front/sonar.cpp	Sat Dec 03 16:38:46 2011 -0800
@@ -222,11 +222,18 @@
 
 // ----------------------------------------------------------------------------------------
 
+bool sonar_is_running()
+{
+    return ( ( TIMSK1 & ( 1 << TOIE1 ) ) != 0 );
+}
+
+// ----------------------------------------------------------------------------------------
+
 bool sonar_send_update()
 {
-    //--    Only send the message if the sonar machinery is active.
+    //--    Only send data if the sonar machinery is running.
     
-    if ( ( TIMSK1 & ( 1 << TOIE1 ) ) == 0 )
+    if ( ! sonar_is_running() )
     {
         return true; // no error
     }