changeset 218:7411b6a1dba2 main

Checkpoint for refactor to bring the implementation into line with odr-display.
author Bob Cook <bob@bobcookdev.com>
date Thu, 03 Jul 2014 19:46:07 -0700
parents d1e6f4dad86a
children 0c26906b7ebf
files main/robots/odr-sonar-front/canmsgs.cpp main/robots/odr-sonar-front/func.h main/robots/odr-sonar-front/jamfile main/robots/odr-sonar-front/leds.cpp main/robots/odr-sonar-front/leds.h main/robots/odr-sonar-front/leds.inl main/robots/odr-sonar-front/main.cpp main/robots/odr-sonar-front/project_defs.h main/robots/odr-sonar-front/sonar.cpp main/robots/odr-sonar-front/status.cpp
diffstat 10 files changed, 492 insertions(+), 118 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr-sonar-front/canmsgs.cpp	Sat Jun 28 16:04:13 2014 -0700
+++ b/main/robots/odr-sonar-front/canmsgs.cpp	Thu Jul 03 19:46:07 2014 -0700
@@ -1,6 +1,6 @@
 // ----------------------------------------------------------------------------------------
 //
-//  Copyright (C) 2011 Bob Cook
+//  Copyright (C) 2011-2014 Bob Cook
 //    
 //  Bob Cook Development, Robotics Library
 //  http://www.bobcookdev.com/rl/
@@ -48,6 +48,31 @@
 
 // ----------------------------------------------------------------------------------------
 
+bool canmsg_did_error_occur()
+{
+    return ( m1can_status() != m1can_status_ok );
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool canmsg_send_emergency()
+{
+    return m1can_send( can_build_message_id( can_node_odr_sonar_front,
+                                             can_node_broadcast,
+                                             can_dataid_emergency ), 0, 0 );
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool canmsg_send_all_clear()
+{
+    return m1can_send( can_build_message_id( can_node_odr_sonar_front,
+                                             can_node_broadcast,
+                                             can_dataid_all_clear ), 0, 0 );
+}
+
+// ----------------------------------------------------------------------------------------
+
 bool canmsg_send_status( bool enabled )
 {
     uint32_t msgid;
--- a/main/robots/odr-sonar-front/func.h	Sat Jun 28 16:04:13 2014 -0700
+++ b/main/robots/odr-sonar-front/func.h	Thu Jul 03 19:46:07 2014 -0700
@@ -1,6 +1,6 @@
 // ----------------------------------------------------------------------------------------
 //
-//  Copyright (C) 2011 Bob Cook
+//  Copyright (C) 2011-2014 Bob Cook
 //    
 //  Bob Cook Development, Robotics Library
 //  http://www.bobcookdev.com/rl/
@@ -39,10 +39,27 @@
 void sonar_stop();
 bool sonar_is_running();
 bool sonar_send_update();
+bool sonar_fault();
+
+// ----------------------------------------------------------------------------------------
+
+void status_init();
+void status_update();
+
+void status_set_emergency();
+
+void status_got_emergency( uint8_t node );
+void status_got_all_clear( uint8_t node );
+void status_got_heartbeat( uint8_t node );
+
+bool status_is_emergency();
 
 // ----------------------------------------------------------------------------------------
 
 bool canmsg_init();
+bool canmsg_did_error_occur();
+bool canmsg_send_emergency();
+bool canmsg_send_all_clear();
 bool canmsg_send_status( bool enabled );
 bool canmsg_send_sonar_front( uint8_t l, uint8_t cl, uint8_t rl, uint8_t r );
 bool canmsg_process_pending();
--- a/main/robots/odr-sonar-front/jamfile	Sat Jun 28 16:04:13 2014 -0700
+++ b/main/robots/odr-sonar-front/jamfile	Thu Jul 03 19:46:07 2014 -0700
@@ -1,6 +1,6 @@
 # -----------------------------------------------------------------------------------------
 #
-#   Copyright (C) 2011 Bob Cook
+#   Copyright (C) 2011-2014 Bob Cook
 #
 #   Bob Cook Development, Robotics Library
 #   http://www.bobcookdev.com/rl/
@@ -33,7 +33,7 @@
 
 avr_executable
     odr-sonar-front atmega16m1
-    : main.cpp canmsgs.cpp sonar.cpp
+    : main.cpp canmsgs.cpp leds.cpp sonar.cpp status.cpp
       packages.avr.can.pkg
       packages.avr.device.pkg
       packages.common.can.pkg
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-sonar-front/leds.cpp	Thu Jul 03 19:46:07 2014 -0700
@@ -0,0 +1,42 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2014 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  LED functions for the Outdoor Robot sensor node.
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+// 
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+// 
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+// ----------------------------------------------------------------------------------------
+
+#include <avr/io.h>
+
+// ----------------------------------------------------------------------------------------
+
+void leds_off()
+{
+    PORTB |= ( 1 << PB2 );
+    PORTC |= ( 1 << PC4 ) | ( 1 << PC5 );
+    PORTD |= ( 1 << PD7 );
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-sonar-front/leds.h	Thu Jul 03 19:46:07 2014 -0700
@@ -0,0 +1,67 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2014 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  LED functions for the Outdoor Robot sensor node.
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+// 
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+// 
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+// ----------------------------------------------------------------------------------------
+
+#if !defined( LEDS_H )
+#define LEDS_H
+
+// ----------------------------------------------------------------------------------------
+
+void leds_off();
+
+// ----------------------------------------------------------------------------------------
+
+void led_red_off();
+void led_red_on();
+void led_red_toggle();
+
+// ----------------------------------------------------------------------------------------
+
+void led_yellow_off();
+void led_yellow_on();
+void led_yellow_toggle();
+
+// ----------------------------------------------------------------------------------------
+
+void led_green_1_off();
+void led_green_1_on();
+void led_green_1_toggle();
+
+// ----------------------------------------------------------------------------------------
+
+void led_green_2_off();
+void led_green_2_on();
+void led_green_2_toggle();
+
+// ----------------------------------------------------------------------------------------
+
+#include "leds.inl"
+
+// ----------------------------------------------------------------------------------------
+#endif // #if !defined( LEDS_H )
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-sonar-front/leds.inl	Thu Jul 03 19:46:07 2014 -0700
@@ -0,0 +1,111 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2014 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  LED functions for the Outdoor Robot sensor node.
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+// 
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+// 
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+// ----------------------------------------------------------------------------------------
+
+#include <avr/io.h>
+
+inline void led_red_off() __attribute__((always_inline));
+inline void led_red_off()
+{
+    PORTD |= ( 1 << PD7 );
+}
+
+inline void led_red_on() __attribute__((always_inline));
+inline void led_red_on()
+{
+    PORTD &= ~( 1 << PD7 );
+}
+
+inline void led_red_toggle() __attribute__((always_inline));
+inline void led_red_toggle()
+{
+    PORTD ^= ( 1 << PD7 );
+}
+
+// ----------------------------------------------------------------------------------------
+
+inline void led_yellow_off() __attribute__((always_inline));
+inline void led_yellow_off()
+{
+    PORTB |= ( 1 << PB2 );
+}
+
+inline void led_yellow_on() __attribute__((always_inline));
+inline void led_yellow_on()
+{
+    PORTB &= ~( 1 << PB2 );
+}
+    
+inline void led_yellow_toggle() __attribute__((always_inline));
+inline void led_yellow_toggle()
+{
+    PORTB ^= ( 1 << PB2 );
+}
+    
+// ----------------------------------------------------------------------------------------
+
+inline void led_green_2_off() __attribute__((always_inline));
+inline void led_green_2_off()
+{
+    PORTC |= ( 1 << PC4 );
+}
+
+inline void led_green_2_on() __attribute__((always_inline));
+inline void led_green_2_on()
+{
+    PORTC &= ~( 1 << PC4 );
+}
+    
+inline void led_green_2_toggle() __attribute__((always_inline));
+inline void led_green_2_toggle()
+{
+    PORTC ^= ( 1 << PC4 );
+}
+    
+// ----------------------------------------------------------------------------------------
+
+inline void led_green_1_off() __attribute__((always_inline));
+inline void led_green_1_off()
+{
+    PORTC |= ( 1 << PC5 );
+}
+
+inline void led_green_1_on() __attribute__((always_inline));
+inline void led_green_1_on()
+{
+    PORTC &= ~( 1 << PC5 );
+}
+
+inline void led_green_1_toggle() __attribute__((always_inline));
+inline void led_green_1_toggle()
+{
+    PORTC ^= ( 1 << PC5 );
+}
+
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr-sonar-front/main.cpp	Sat Jun 28 16:04:13 2014 -0700
+++ b/main/robots/odr-sonar-front/main.cpp	Thu Jul 03 19:46:07 2014 -0700
@@ -1,6 +1,6 @@
 // ----------------------------------------------------------------------------------------
 //
-//  Copyright (C) 2012 Bob Cook
+//  Copyright (C) 2012-2014 Bob Cook
 //    
 //  Bob Cook Development, Robotics Library
 //  http://www.bobcookdev.com/rl/
@@ -34,6 +34,7 @@
 #include "project_defs.h"
 
 #include "func.h"
+#include "leds.h"
 
 #include "packages/avr/device/int_helpers.h"
 #include "packages/avr/device/spinwait.h"
@@ -43,102 +44,15 @@
 
 // ----------------------------------------------------------------------------------------
 
-static const uint8_t trigger_send_sonar_status = ( 1 << 0 );
-static const uint8_t trigger_send_sonar_update = ( 1 << 1 );
+static const uint8_t trigger_main_loop_ok      = ( 1 << 0 );
+static const uint8_t trigger_send_sonar_status = ( 1 << 1 );
+static const uint8_t trigger_send_sonar_update = ( 1 << 2 );
+static const uint8_t trigger_check_emergency   = ( 1 << 3 );
 
 static volatile uint8_t g_triggers;
 
 // ----------------------------------------------------------------------------------------
 
-static void leds_off()
-{
-    PORTB |= ( 1 << PB2 );
-    PORTC |= ( 1 << PC4 ) | ( 1 << PC5 );
-    PORTD |= ( 1 << PD7 );
-}
-
-// ----------------------------------------------------------------------------------------
-
-static inline void led_red_off() __attribute__((always_inline));
-static inline void led_red_off()
-{
-    PORTD |= ( 1 << PD7 );
-}
-
-static inline void led_red_on() __attribute__((always_inline));
-static inline void led_red_on()
-{
-    PORTD &= ~( 1 << PD7 );
-}
-
-static inline void led_red_toggle() __attribute__((always_inline));
-static inline void led_red_toggle()
-{
-    PORTD ^= ( 1 << PD7 );
-}
-
-// ----------------------------------------------------------------------------------------
-
-static inline void led_yellow_off() __attribute__((always_inline));
-static inline void led_yellow_off()
-{
-    PORTB |= ( 1 << PB2 );
-}
-
-static inline void led_yellow_on() __attribute__((always_inline));
-static inline void led_yellow_on()
-{
-    PORTB &= ~( 1 << PB2 );
-}
-    
-static inline void led_yellow_toggle() __attribute__((always_inline));
-static inline void led_yellow_toggle()
-{
-    PORTB ^= ( 1 << PB2 );
-}
-    
-// ----------------------------------------------------------------------------------------
-
-static inline void led_green_2_off() __attribute__((always_inline));
-static inline void led_green_2_off()
-{
-    PORTC |= ( 1 << PC4 );
-}
-
-static inline void led_green_2_on() __attribute__((always_inline));
-static inline void led_green_2_on()
-{
-    PORTC &= ~( 1 << PC4 );
-}
-    
-static inline void led_green_2_toggle() __attribute__((always_inline));
-static inline void led_green_2_toggle()
-{
-    PORTC ^= ( 1 << PC4 );
-}
-    
-// ----------------------------------------------------------------------------------------
-
-static inline void led_green_1_off() __attribute__((always_inline));
-static inline void led_green_1_off()
-{
-    PORTC |= ( 1 << PC5 );
-}
-
-static inline void led_green_1_on() __attribute__((always_inline));
-static inline void led_green_1_on()
-{
-    PORTC &= ~( 1 << PC5 );
-}
-
-static inline void led_green_1_toggle() __attribute__((always_inline));
-static inline void led_green_1_toggle()
-{
-    PORTC ^= ( 1 << PC5 );
-}
-    
-// ----------------------------------------------------------------------------------------
-
 static const uint8_t odrson_fatal_error_can_init = 1;
 static const uint8_t odrson_fatal_error_can_comm = 2;
 static const uint8_t odrson_fatal_error_exiting  = 9;
@@ -260,11 +174,38 @@
 {
     static uint8_t count = 0;
 
-    // sonar status: every 2.5 seconds
-    // sonar update: every 0.5 seconds
+    //--    The trigger_main_loop_ok flag indicates the main loop is "alive" e.g. still
+    //      running. We only blink the "heartbeat" LED if it continues to toggle.
+    //
+    //      led blink pattern: 60 milliseconds on, 150 off, 60 on
+    //      sonar status: every 2.5 seconds
+    //      sonar update: every 0.5 seconds
+    //      check emergency status: every 100 milliseconds
     
+    if ( count % 10 == 0 )
+    {
+        g_triggers |= trigger_check_emergency;
+    }
+
     switch ( ++count )
     {
+        case 10:
+            if ( g_triggers & trigger_main_loop_ok ) { led_green_1_on(); }
+            break;
+
+        case 16:
+            if ( g_triggers & trigger_main_loop_ok ) { led_green_1_off(); }
+            break;
+
+        case 31:
+            if ( g_triggers & trigger_main_loop_ok ) { led_green_1_on(); }
+            break;
+
+        case 37:
+            if ( g_triggers & trigger_main_loop_ok ) { led_green_1_off(); }
+            g_triggers &= ~trigger_main_loop_ok;
+            break;
+
         case 50:
         case 100:
         case 150:
@@ -294,11 +235,31 @@
 
     for ( ;; )
     {
+        //--    The main loop is still running.
+
+        g_triggers |= trigger_main_loop_ok;
+
         //--    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();
 
+        //--    Check for an emergency situation?
+
+        if ( g_triggers & trigger_check_emergency )
+        {
+            g_triggers &= ~trigger_check_emergency;
+
+            if ( status_is_emergency() )
+            {
+                led_red_on();
+            }
+            else
+            {
+                led_red_off();
+            }
+        }
+
         //--    Time to send a sonar update message?
 
         if ( g_triggers & trigger_send_sonar_update )
@@ -317,43 +278,50 @@
         {
             g_triggers &= ~trigger_send_sonar_status;
 
-            led_green_1_on();
-            spinwait_delay_ms( 60 );
-            led_green_1_off();
-            spinwait_delay_ms( 150 );
-            led_green_1_on();
-            spinwait_delay_ms( 60 );
-            led_green_1_off();
-
             if ( ! canmsg_send_status( sonar_is_running() ) )
             {
                 ++can_comm_errors;
             }
+
+            if ( sonar_fault() )
+            {
+                status_set_emergency();
+
+                if ( ! canmsg_send_emergency() )
+                {
+                    ++can_comm_errors;
+                }
+            }
+            else if ( status_is_emergency() )
+            {
+                if ( ! canmsg_send_all_clear() )
+                {
+                    ++can_comm_errors;
+                }
+            }
         }
 
         //--    Any pending CAN messages to receive/process?
 
         if ( ! canmsg_process_pending() )
         {
-            ++can_comm_errors;
+            ++can_comm_errors;  // cannot overflow due to check below
         }
-        else
+
+        if ( canmsg_did_error_occur() )
         {
-            if ( can_comm_errors > 0 )
-            {
-                --can_comm_errors;
-            }
+            ++can_comm_errors;  // cannot overflow due to check below
         }
 
         //--    Report warning and/or error conditions.
 
         bool warning = false;
 
-        if ( can_comm_errors > 50 )
+        if ( can_comm_errors > 10 )
         {
             odrson_fatal_error( odrson_fatal_error_can_comm );
         }
-        else if ( can_comm_errors > 10 )
+        else if ( can_comm_errors > 0 )
         {
             warning = true;
         }
--- a/main/robots/odr-sonar-front/project_defs.h	Sat Jun 28 16:04:13 2014 -0700
+++ b/main/robots/odr-sonar-front/project_defs.h	Thu Jul 03 19:46:07 2014 -0700
@@ -1,6 +1,6 @@
 // ----------------------------------------------------------------------------------------
 //
-//  Copyright (C) 2011 Bob Cook
+//  Copyright (C) 2011-2014 Bob Cook
 //
 //  Permission is hereby granted, free of charge, to any person obtaining a copy
 //  of this software and associated documentation files (the "Software"), to deal
@@ -35,7 +35,7 @@
 
 #define PRJ_M1CAN_ENABLE
 #define PRJ_M1CAN_CANBUS_250_KHZ
-#define PRJ_M1CAN_TX_BUFFER_SIZE  8
+#define PRJ_M1CAN_TX_BUFFER_SIZE  16
 
 
 #endif // #if !defined( PROJECT_DEFS_H )
--- a/main/robots/odr-sonar-front/sonar.cpp	Sat Jun 28 16:04:13 2014 -0700
+++ b/main/robots/odr-sonar-front/sonar.cpp	Thu Jul 03 19:46:07 2014 -0700
@@ -1,6 +1,6 @@
 // ----------------------------------------------------------------------------------------
 //
-//  Copyright (C) 2011-2013 Bob Cook
+//  Copyright (C) 2011-2014 Bob Cook
 //    
 //  Bob Cook Development, Robotics Library
 //  http://www.bobcookdev.com/rl/
@@ -51,13 +51,16 @@
 static const uint8_t sensor_index_right        = ( 3 * sensor_index_offset );
 static const uint8_t sensor_index_max_value    = ( 4 * sensor_index_offset );
 
-volatile uint8_t   g_curr_sensor_index;
+volatile uint8_t  g_curr_sensor_index;
 
 volatile uint8_t  g_sensor_left;               // pin PD6
 volatile uint8_t  g_sensor_center_left;        // pin PD5
 volatile uint8_t  g_sensor_center_right;       // pin PB6
 volatile uint8_t  g_sensor_right;              // pin PB7
 
+volatile bool     g_sensor_triggered;
+volatile uint8_t  g_sensor_failures;
+
 // ----------------------------------------------------------------------------------------
 
 void sonar_init()
@@ -89,6 +92,17 @@
 
 ISR( TIMER1_OVF_vect )
 {
+    //--    Check that we got a valid sample for the previous attempt. If not, we may have
+    //      a dead sensor. If this happens too often we have an emergency situation.
+
+    if ( ! g_sensor_triggered )
+    {
+        if ( g_sensor_failures < 200 )
+        {
+            ++g_sensor_failures;
+        }
+    }
+
     //--    We need to advance to the next sensor then trigger an output pulse. After
     //      the trigger we switch the pin to an input for sensing the hold-off period
     //      and the reception of the final result.
@@ -96,6 +110,8 @@
     g_curr_sensor_index++;
     g_curr_sensor_index %= sensor_index_max_value;
 
+    g_sensor_triggered = false;
+
     switch ( g_curr_sensor_index )
     {
         case sensor_index_left:
@@ -180,6 +196,7 @@
             uint16_t sample = TCNT1L;
             sample |= static_cast< uint16_t >( TCNT1H ) << 8;
             g_sensor_center_right = static_cast< uint8_t >( sample / 291 );
+            g_sensor_triggered = true;
             // disable pin-change interrupt
             PCMSK0 &= ~( 1 << PCINT6 );
         }
@@ -197,6 +214,7 @@
             uint16_t sample = TCNT1L;
             sample |= static_cast< uint16_t >( TCNT1H ) << 8;
             g_sensor_right = static_cast< uint8_t >( sample / 300 );
+            g_sensor_triggered = true;
             // disable pin-change interrupt
             PCMSK0 &= ~( 1 << PCINT7 );
         }
@@ -222,6 +240,7 @@
             uint16_t sample = TCNT1L;
             sample |= static_cast< uint16_t >( TCNT1H ) << 8;
             g_sensor_left = static_cast< uint8_t >( sample / 291 );
+            g_sensor_triggered = true;
             // disable pin-change interrupt
             PCMSK2 &= ~( 1 << PCINT22 );
         }
@@ -239,6 +258,7 @@
             uint16_t sample = TCNT1L;
             sample |= static_cast< uint16_t >( TCNT1H ) << 8;
             g_sensor_center_left = static_cast< uint8_t >( sample / 291 );
+            g_sensor_triggered = true;
             // disable pin-change interrupt
             PCMSK2 &= ~( 1 << PCINT21 );
         }
@@ -249,6 +269,10 @@
 
 void sonar_start()
 {
+    //--    Reset the sensor failure counter, things might be better now.
+
+    g_sensor_failures = 0;
+
     //--    Enable the interrupt on Timer1, which starts the machinery.
 
     TIMSK1 |= ( 1 << TOIE1 );
@@ -289,3 +313,10 @@
 
 // ----------------------------------------------------------------------------------------
 
+bool sonar_fault()
+{
+    return ( g_sensor_failures > 10 );
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-sonar-front/status.cpp	Thu Jul 03 19:46:07 2014 -0700
@@ -0,0 +1,113 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2011-2014 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Status display functions for the Outdoor Robot sensor node.
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+// 
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+// 
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+//  There is an implicit assumption our "heartbeat" is 2 Hz (twice per second).
+//
+// ----------------------------------------------------------------------------------------
+
+#include "func.h"
+#include "leds.h"
+
+#include <avr/io.h>
+
+#include "packages/common/can/can_nodes.h"
+
+// ----------------------------------------------------------------------------------------
+//  "beats" is 500ms (approximately)
+
+static volatile uint8_t g_beats_without_mgr;
+
+static volatile uint8_t g_status_flags;
+
+static const uint8_t status_flag_emergency_active = ( 1 << 0 );
+
+// ----------------------------------------------------------------------------------------
+
+void status_init()
+{
+    // initalize the heartbeat watchdog assuming no contact yet
+    g_beats_without_mgr = 100;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void status_update()
+{
+    if ( g_beats_without_mgr < 200 ) { ++g_beats_without_mgr; }
+}
+
+// ----------------------------------------------------------------------------------------
+
+void status_got_heartbeat( uint8_t source )
+{
+    switch ( source )
+    {
+        case can_node_odr_manager:
+            g_beats_without_mgr = 0;
+            break;
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+void status_set_emergency()
+{
+    g_status_flags |= status_flag_emergency_active;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void status_got_emergency( uint8_t source )
+{
+    g_status_flags |= status_flag_emergency_active;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void status_got_all_clear( uint8_t source )
+{
+    if ( source == can_node_odr_manager )
+    {
+        g_status_flags &= ~status_flag_emergency_active;
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool status_is_emergency()
+{
+    // more than two seconds == down
+    if ( g_beats_without_mgr > 5 )
+    {
+        return true;
+    }
+
+    return ( g_status_flags & status_flag_emergency_active ) > 0;
+}
+
+// ----------------------------------------------------------------------------------------
+