changeset 228:2429999d8731 main

Merge/commit from upstream.
author Bob Cook <bob@bobcookdev.com>
date Mon, 07 Jul 2014 18:59:11 -0700
parents 03064d391e79 (current diff) 74381e5dd204 (diff)
children 79283dc7dc54
files main/packages/common/can/can_messages.h
diffstat 23 files changed, 976 insertions(+), 175 deletions(-) [+]
line wrap: on
line diff
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/demos/avr/adafruit-lcd-1480/jamfile	Mon Jul 07 18:59:11 2014 -0700
@@ -0,0 +1,44 @@
+# -----------------------------------------------------------------------------------------
+#
+#   Copyright (C) 2014 Bob Cook
+#
+#   Bob Cook Development, Robotics Library
+#   http://www.bobcookdev.com/rl/
+#    
+#   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 $(TRACE) { echo "trace /demos/avr/adafruit-lcd-1480/jamfile" ; }
+
+SubDir TOP demos avr adafruit-lcd-1480 ;
+
+# -----------------------------------------------------------------------------------------
+
+avr_executable
+    adafruit-lcd-1480 atmega16m1
+    : main.cpp
+      packages.avr.device.pkg
+      packages.avr.lcd.ili9340.pkg
+      packages.common.font.pkg
+      packages.common.util.pkg
+    ;
+
+# -----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/demos/avr/adafruit-lcd-1480/main.cpp	Mon Jul 07 18:59:11 2014 -0700
@@ -0,0 +1,165 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2014 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Demonstration of the Adafruit 2.2" TFT LCD display (product #1480)
+//
+//  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/interrupt.h>
+#include <avr/io.h>
+#include <avr/sleep.h>
+
+#include "project_defs.h"
+
+#include "packages/avr/device/int_helpers.h"
+#include "packages/avr/device/spinwait.h"
+
+#include "packages/avr/lcd/ili9340/ili9340.h"
+
+#include "packages/common/util/printutils.h"
+
+// always after other includes
+#include "packages/avr/device/workaround34734.h"
+
+// ----------------------------------------------------------------------------------------
+
+inline void led_red_on()
+{
+    PORTB &= ~( 1 << PB5 );
+}
+
+inline void led_red_off()
+{
+    PORTB |= ( 1 << PB5 );
+}
+
+inline void led_green_on()
+{
+    PORTC &= ~( 1 << PC0 );
+    PORTC &= ~( 1 << PC7 );
+}
+
+inline void led_green_off()
+{
+    PORTC |= ( 1 << PC0 );
+    PORTC |= ( 1 << PC7 );
+}
+
+inline void leds_off()
+{
+    PORTB |= ( 1 << PB5 );
+    PORTC |= ( 1 << PC0 );
+    PORTC |= ( 1 << PC7 );
+}
+
+// ----------------------------------------------------------------------------------------
+
+void hw_init()
+{
+    //--    Turn off all interrupts.
+
+    cli();
+    interrupts_clear_all();
+
+    //--    Indicator LEDs on PORTB.5, PORTC.0
+
+    DDRB |= ( 1 << PB5 );
+    DDRC |= ( 1 << PC0 );
+    DDRC |= ( 1 << PC7 );
+    leds_off();
+
+    //--    Set the SS pin as an output; this prevents the SPI machinery from blocking.
+
+    DDRD |= ( 1 << PD3 );
+
+    //--    Blink the LEDs just to say hello.
+    
+    for ( uint8_t i = 0; i < 5; i++ )
+    {
+        led_red_on();
+        led_green_on();
+        spinwait_delay_ms( 75 );
+        leds_off();
+        spinwait_delay_ms( 75 );
+    }
+
+    //--    Re-enable interrupts.
+
+    sei();
+
+    //--    Initialize the display package.
+
+    ili9340_init();
+
+    PORTC |= ( 1 << PC1 ); // backlight
+}
+
+// ----------------------------------------------------------------------------------------
+
+int main()
+{
+    //--    Initialize the hardware and send a "hello we started" message.
+
+    hw_init();
+
+    led_green_on();
+       
+    //--    Draw some stuff.
+
+    ili9340_fill_screen( ili9340_color_blue );
+
+    ili9340_write( 'A', 1, 1, ili9340_color_white, ili9340_color_blue );
+    ili9340_write( 'B', 2, 1, ili9340_color_white, ili9340_color_blue );
+
+    ili9340_write_pgm( PSTR("Adafruit 2.2\" TFT LCD"),
+                       3, 5,
+                       ili9340_color_white, ili9340_color_blue );
+
+    for ( uint8_t i = 0; i < 200; ++i )
+    {
+        char number_str[ 16 ];
+        char* eos = print_bits( number_str, i );
+        *eos = '\0';
+
+        ili9340_write( number_str,
+                       10, 6,
+                       ili9340_color_white, ili9340_color_blue );
+
+        //spinwait_delay_ms( 250 );
+    }
+
+    for ( ;; )
+    {
+        led_red_on();
+        spinwait_delay_ms( 1000 );
+        led_red_off();
+        spinwait_delay_ms( 1000 );
+    }
+
+    return 0;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/demos/avr/adafruit-lcd-1480/project_defs.h	Mon Jul 07 18:59:11 2014 -0700
@@ -0,0 +1,79 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 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
+//  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( PROJECT_DEFS_H )
+#define PROJECT_DEFS_H
+
+// ----------------------------------------------------------------------------------------
+//  packages/avr/common
+
+#define PRJ_CPU_FREQ    16000000L
+
+// ----------------------------------------------------------------------------------------
+//  packages/common/font
+
+#define PRJ_FONT_8X12_ENABLE
+
+// ----------------------------------------------------------------------------------------
+//  packages/avr/common/util
+
+#define PRJ_PRINTDEC_ENABLE_OUTPUT_TO_BUFFER
+#define PRJ_PRINTHEX_ENABLE_OUTPUT_TO_BUFFER
+#define PRJ_PRINTBITS_ENABLE_OUTPUT_TO_BUFFER
+
+// ----------------------------------------------------------------------------------------
+//  packages/avr/device/spi
+
+#define PRJ_SPI_ENABLE_MASTER
+#define PRJ_SPI_BUS_MODE_0
+#define PRJ_SPI_DATA_DIRECTION_MSB
+#define PRJ_SPI_CLOCK_FACTOR_DIV_2
+
+// ----------------------------------------------------------------------------------------
+//  packages/avr/lcd/ili9340
+
+#define PRJ_ILI9340_ENABLE
+
+#define PRJ_ILI9340_SELECT_PORT  PORTD
+#define PRJ_ILI9340_SELECT_PIN   PD5
+#define PRJ_ILI9340_SELECT_DDR   DDRD
+
+#define PRJ_ILI9340_COMMAND_PORT  PORTB
+#define PRJ_ILI9340_COMMAND_PIN   PB2
+#define PRJ_ILI9340_COMMAND_DDR   DDRB
+
+#define PRJ_ILI9340_USE_HW_RESET
+#define PRJ_ILI9340_RESET_PORT   PORTD
+#define PRJ_ILI9340_RESET_PIN    PD7
+#define PRJ_ILI9340_RESET_DDR    DDRD
+
+#define PRJ_ILI9340_DISPLAY_ORIENTATION_180
+#define PRJ_ILI9340_INC_WRITE_CHAR
+#define PRJ_ILI9340_INC_WRITE_CSTRINGS
+#define PRJ_ILI9340_INC_WRITE_PGM_CSTRINGS
+#define PRJ_ILI9340_INC_DRAWING
+
+#endif // #if !defined( PROJECT_DEFS_H )
+// ----------------------------------------------------------------------------------------
+
--- a/main/packages/avr/can/m1.cpp	Mon Jul 07 18:52:16 2014 -0700
+++ b/main/packages/avr/can/m1.cpp	Mon Jul 07 18:59:11 2014 -0700
@@ -81,6 +81,8 @@
 static cbuffer< m1can_data_t, uint8_t, PRJ_M1CAN_RX_BUFFER_SIZE > g_rx_buffer;
 static cbuffer< m1can_data_t, uint8_t, PRJ_M1CAN_TX_BUFFER_SIZE > g_tx_buffer;
 
+volatile uint8_t g_status;
+
 // ----------------------------------------------------------------------------------------
 //  mapping the four register bytes to the uint32_t CAN identifier; this assumes the
 //  correct CANPAGE has been selected previously.
@@ -136,9 +138,16 @@
         }
     }
 
-    //--    Add it to the queue.
+    //--    Add it to the queue, if there is space. Otherwise discard the overflow.
 
-    g_rx_buffer.push( msg );
+    if ( g_tx_buffer.is_full() )
+    {
+        g_status |= m1can_status_overflow;
+    }
+    else
+    {
+        g_rx_buffer.push( msg );
+    }
 }
 
 // ----------------------------------------------------------------------------------------
@@ -487,7 +496,7 @@
     
     msg.id     = can_id;
     msg.length = 0xff; // request, not an actual message
-    
+
     g_tx_buffer.push( msg );
 
     return true;
@@ -495,6 +504,15 @@
 
 // ----------------------------------------------------------------------------------------
 
+uint8_t m1can_status()
+{
+    uint8_t result = g_status;
+    g_status = m1can_status_ok;
+    return result;
+}
+
+// ----------------------------------------------------------------------------------------
+
 #if defined( PRJ_M1CAN_ENABLE_DEBUG )
     
 static void print_register( const prog_char* label, uint8_t value )
--- a/main/packages/avr/can/m1.h	Mon Jul 07 18:52:16 2014 -0700
+++ b/main/packages/avr/can/m1.h	Mon Jul 07 18:59:11 2014 -0700
@@ -7,7 +7,7 @@
 //    
 //  This file implements CAN support for the mega(16,32,64)m1 chip.
 //
-//  Copyright (C) 2012 Bob Cook
+//  Copyright (C) 2012-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
@@ -120,6 +120,20 @@
 bool m1can_request( const uint32_t& can_id );
 
 // ----------------------------------------------------------------------------------------
+//  m1can_status
+//
+//      Return the current status of the controller. May return more than one item.
+//
+//      Internal status is cleared after this call, such that the next call will return
+//      status = ok until another issue occurs.
+//
+
+static const uint8_t m1can_status_ok       = 0x00;
+static const uint8_t m1can_status_overflow = ( 1 << 0 );
+
+uint8_t m1can_status();
+
+// ----------------------------------------------------------------------------------------
 //  m1can_debug_print_registers
 //
 //      Print the current state of the mega(16,32,64)m1 CAN h/w registers to stdout.
--- a/main/packages/avr/lcd/ili9340/ili9340.cpp	Mon Jul 07 18:52:16 2014 -0700
+++ b/main/packages/avr/lcd/ili9340/ili9340.cpp	Mon Jul 07 18:59:11 2014 -0700
@@ -110,6 +110,10 @@
 #endif
 #endif
 
+#if !defined( PRJ_FONT_8X12_ENABLE )
+#error PRJ_FONT_8X12_ENABLE is required!
+#endif
+
 // ----------------------------------------------------------------------------------------
 
 #if defined( PRJ_ILI9340_DISPLAY_ORIENTATION_0 ) \
--- a/main/packages/avr/lcd/ili9340/ili9340.h	Mon Jul 07 18:52:16 2014 -0700
+++ b/main/packages/avr/lcd/ili9340/ili9340.h	Mon Jul 07 18:59:11 2014 -0700
@@ -130,8 +130,8 @@
 //      Write a character to the display in the *character* row and column. The maximum
 //      number of characters depends on the orientation:
 //      
-//          horizontal: 53 characters wide x 30 characters high
-//          vertical:   40 characters wide x 40 characters high
+//          horizontal: 40 characters wide x 20 characters high
+//          vertical:   30 characters wide x 26 characters high
 //
 //      This function is only available if PRJ_ILI9340_INC_WRITE_CHAR is defined.
 //
@@ -148,8 +148,7 @@
 // ----------------------------------------------------------------------------------------
 //  ili9340_write
 //
-//      Write a string to the display in the *character* row and column. The display
-//      can support 16 rows by 21 columns.
+//      Write a string to the display in the *character* row and column.
 //
 //      This function is only available if PRJ_ILI9340_INC_WRITE_CSTRINGS is defined.
 //
@@ -167,7 +166,6 @@
 //  ili9340_write_pgm
 //
 //      Write a string from flash memory to the display in the *character* row and column.
-//      The display can support 16 rows by 21 columns.
 //
 //      This function is only available if PRJ_ILI9340_INC_WRITE_PGM_CSTRINGS is defined.
 //
@@ -184,7 +182,7 @@
 // ----------------------------------------------------------------------------------------
 //  ili9340_set_pixel
 //
-//      Paint a specific pixel. The display supports 240 rows by 320 columns.
+//      Paint a specific pixel.
 //
 //      This function is only available if PRJ_ILI9340_INC_SET_PIXEL is defined.
 //
@@ -255,10 +253,10 @@
 
 void ili9340_imagemask
     (
-        uint16_t             x,
-        uint16_t             y,
-        uint16_t             w,
-        uint16_t             h,
+        uint16_t            x,
+        uint16_t            y,
+        uint16_t            w,
+        uint16_t            h,
         ili9340_color_t     fg_color,
         ili9340_color_t     bg_color,
         const prog_uint8_t* pixels
@@ -276,10 +274,10 @@
 
 void ili9340_colorimage
     (
-        uint16_t             x,
-        uint16_t             y,
-        uint16_t             w,
-        uint16_t             h,
+        uint16_t            x,
+        uint16_t            y,
+        uint16_t            w,
+        uint16_t            h,
         const prog_uint8_t* pixels
     );
 
--- a/main/packages/avr/lcd/jamdefs	Mon Jul 07 18:52:16 2014 -0700
+++ b/main/packages/avr/lcd/jamdefs	Mon Jul 07 18:59:11 2014 -0700
@@ -5,7 +5,7 @@
 #   Bob Cook Development, Robotics Library
 #   http://www.bobcookdev.com/rl/
 #
-#   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
@@ -31,7 +31,7 @@
 
 # -----------------------------------------------------------------------------------------
 
-DefinePackages TOP packages avr lcd : cfax hd44780 sfe569 ;
+DefinePackages TOP packages avr lcd : cfax hd44780 ili9340 sfe569 ;
 
 # -----------------------------------------------------------------------------------------
 
--- a/main/packages/common/can/can_messages.h	Mon Jul 07 18:52:16 2014 -0700
+++ b/main/packages/common/can/can_messages.h	Mon Jul 07 18:59:11 2014 -0700
@@ -86,6 +86,16 @@
 
 const uint16_t can_dataid_odr_start_pgm = 0x0014;
 
+const uint8_t odr_pgm_wander        = 0x01;
+const uint8_t odr_pgm_straight_line = 0x02;
+const uint8_t odr_pgm_small_box     = 0x03;
+
+typedef struct
+{
+    uint8_t pgm_id;
+
+}   can_data_pgm_selection;
+
 // ----------------------------------------------------------------------------------------
 
 const uint16_t can_dataid_imu_roll  = 0x0020;
--- a/main/robots/odr-gps/canmsgs.cpp	Mon Jul 07 18:52:16 2014 -0700
+++ b/main/robots/odr-gps/canmsgs.cpp	Mon Jul 07 18:59:11 2014 -0700
@@ -96,7 +96,7 @@
     uint32_t msgid;
     msgid = can_build_message_id( can_node_sensor_gps,
                                   can_node_broadcast,
-                                  can_dataid_latitude );
+                                  can_dataid_gps_latitude );
 
     can_data_gps_data data;
 
@@ -116,7 +116,7 @@
     uint32_t msgid;
     msgid = can_build_message_id( can_node_sensor_gps,
                                   can_node_broadcast,
-                                  can_dataid_longitude );
+                                  can_dataid_gps_longitude );
 
     can_data_gps_data data;
 
--- a/main/robots/odr-gps/project_defs.h	Mon Jul 07 18:52:16 2014 -0700
+++ b/main/robots/odr-gps/project_defs.h	Mon Jul 07 18:59:11 2014 -0700
@@ -1,6 +1,6 @@
 // ----------------------------------------------------------------------------------------
 //
-//  Copyright (C) 2013 Bob Cook
+//  Copyright (C) 2013-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,8 +35,7 @@
 
 #define PRJ_M1CAN_ENABLE
 #define PRJ_M1CAN_CANBUS_250_KHZ
-#define PRJ_M1CAN_TX_BUFFER_SIZE  8
-#define PRJ_M1CAN_RX_BUFFER_SIZE  4
+#define PRJ_M1CAN_TX_BUFFER_SIZE  16
 
 // ----------------------------------------------------------------------------------------
 //  packages/avr/device
--- a/main/robots/odr-motion/main.cpp	Mon Jul 07 18:52:16 2014 -0700
+++ b/main/robots/odr-motion/main.cpp	Mon Jul 07 18:59:11 2014 -0700
@@ -199,7 +199,7 @@
     //
     //      encoder update: every 10 milliseconds
     //      led blink pattern: 60 milliseconds on, 150 off, 60 on
-    //      check ESTOP status: every 100 milliseconds
+    //      check emergency status: every 100 milliseconds
     //      adjust the motor speeds every 500 milliseconds
     //      update the system status every 500 milliseconds
     //      check motor controller: every 1.25 seconds
--- a/main/robots/odr-motion/project_defs.h	Mon Jul 07 18:52:16 2014 -0700
+++ b/main/robots/odr-motion/project_defs.h	Mon Jul 07 18:59:11 2014 -0700
@@ -1,6 +1,6 @@
 // ----------------------------------------------------------------------------------------
 //
-//  Copyright (C) 2013 Bob Cook
+//  Copyright (C) 2013-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
 
 // ----------------------------------------------------------------------------------------
 //  packages/avr/device
--- a/main/robots/odr-sonar-front/canmsgs.cpp	Mon Jul 07 18:52:16 2014 -0700
+++ b/main/robots/odr-sonar-front/canmsgs.cpp	Mon Jul 07 18:59:11 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;
@@ -101,30 +126,45 @@
     uint8_t  recvdata[ 8 ];
     uint8_t  recvlen = sizeof( recvdata );
 
-    if ( ! m1can_read( &recvid, &recvrequest, recvdata, &recvlen ) )
-    {
-        return true; // no messages, everything ok
-    }
-
-    uint8_t  srcnode;
-    uint8_t  dstnode;
-    uint16_t dataid;
-    can_parse_message_id( recvid, &srcnode, &dstnode, &dataid );
-
-    if ( dstnode != can_node_broadcast && dstnode != can_node_odr_sonar_front )
+    for ( ;; )
     {
-        return true; // not for us, everything ok
-    }
+        if ( ! m1can_read( &recvid, &recvrequest, recvdata, &recvlen ) )
+        {
+            return true; // no messages, everything ok
+        }
+
+        uint8_t  srcnode;
+        uint8_t  dstnode;
+        uint16_t dataid;
+        can_parse_message_id( recvid, &srcnode, &dstnode, &dataid );
+
+        if ( dstnode != can_node_broadcast && dstnode != can_node_odr_sonar_front )
+        {
+            return true; // not for us, everything ok
+        }
 
-    switch ( dataid )
-    {
-        case can_dataid_sonar_front_enable:
-            sonar_start();
-            break;
+        switch ( dataid )
+        {
+                case can_dataid_heartbeat:
+                    status_got_heartbeat( srcnode );
+                    break;
+
+                case can_dataid_emergency:
+                    status_got_emergency( srcnode );
+                    break;
 
-        case can_dataid_sonar_front_disable:
-            sonar_stop();
-            break;
+                case can_dataid_all_clear:
+                    status_got_all_clear( srcnode );
+                    break;
+
+            case can_dataid_sonar_front_enable:
+                sonar_start();
+                break;
+
+            case can_dataid_sonar_front_disable:
+                sonar_stop();
+                break;
+        }
     }
 
     return true; // no errors
--- a/main/robots/odr-sonar-front/func.h	Mon Jul 07 18:52:16 2014 -0700
+++ b/main/robots/odr-sonar-front/func.h	Mon Jul 07 18:59:11 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,26 @@
 void sonar_stop();
 bool sonar_is_running();
 bool sonar_send_update();
+bool sonar_fault();
+
+// ----------------------------------------------------------------------------------------
+
+void status_init();
+void status_update();
+
+void status_set_emergency();
+bool status_is_emergency();
+
+void status_got_emergency( uint8_t node );
+void status_got_all_clear( uint8_t node );
+void status_got_heartbeat( uint8_t node );
 
 // ----------------------------------------------------------------------------------------
 
 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	Mon Jul 07 18:52:16 2014 -0700
+++ b/main/robots/odr-sonar-front/jamfile	Mon Jul 07 18:59:11 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	Mon Jul 07 18:59:11 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	Mon Jul 07 18:59:11 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	Mon Jul 07 18:59:11 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	Mon Jul 07 18:52:16 2014 -0700
+++ b/main/robots/odr-sonar-front/main.cpp	Mon Jul 07 18:59:11 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,111 +34,24 @@
 #include "project_defs.h"
 
 #include "func.h"
+#include "leds.h"
 
 #include "packages/avr/device/int_helpers.h"
 #include "packages/avr/device/spinwait.h"
 
-#include "packages/common/filt/filtbuf_u16.h"
 #include "packages/common/util/misc.h"
 
 // ----------------------------------------------------------------------------------------
 
-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,21 +173,42 @@
 {
     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 100 milliseconds
+    //      force sonar update: every 1.25 seconds
+    //      check emergency status: every 100 milliseconds
     
+    if ( count % 10 == 0 )
+    {
+        g_triggers |= trigger_check_emergency;
+        g_triggers |= trigger_send_sonar_update;
+    }
+
     switch ( ++count )
     {
-        case 50:
-        case 100:
-        case 150:
-        case 200:
-            g_triggers |= trigger_send_sonar_update;
+        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 250:
             g_triggers |= trigger_send_sonar_status;
-            g_triggers |= trigger_send_sonar_update;
             count = 0;
             break;
     }
@@ -294,11 +228,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 )
@@ -310,50 +264,57 @@
                 ++can_comm_errors;
             }
         }
-
+        
         //--    Time to send the status message?
 
         if ( g_triggers & trigger_send_sonar_status )
         {
             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	Mon Jul 07 18:52:16 2014 -0700
+++ b/main/robots/odr-sonar-front/project_defs.h	Mon Jul 07 18:59:11 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	Mon Jul 07 18:52:16 2014 -0700
+++ b/main/robots/odr-sonar-front/sonar.cpp	Mon Jul 07 18:59:11 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,23 @@
 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;
+
+static const uint8_t sensor_state_idle      = 0;
+static const uint8_t sensor_state_pinging   = 1;
+static const uint8_t sensor_state_receiving = 2;
+static const uint8_t sensor_state_received  = 3;
+
+volatile uint8_t  g_sensor_state;
+volatile uint8_t  g_sensor_failures;
+
 // ----------------------------------------------------------------------------------------
 
 void sonar_init()
@@ -89,6 +99,22 @@
 
 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_state == sensor_state_pinging )
+    {
+        if ( g_sensor_failures < 200 )
+        {
+            ++g_sensor_failures;
+        }
+    }
+
+    //--    Reset the sensor state to idle; tracking the state allaws the detection of
+    //      unresponsive sensors.
+
+    g_sensor_state = sensor_state_idle;
+
     //--    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.
@@ -102,24 +128,28 @@
             // switch PORTD.6 to an output, set it high
             DDRD  |= ( 1 << PD6 );
             PORTD |= ( 1 << PD6 );
+            g_sensor_state = sensor_state_pinging;
             break;
 
         case sensor_index_center_left:
             // switch PORTD.5 to an output, set it high
             DDRD  |= ( 1 << PD5 );
             PORTD |= ( 1 << PD5 );
+            g_sensor_state = sensor_state_pinging;
             break;
 
         case sensor_index_center_right:
             // switch PORTB.6 to an output, set it high
             DDRB  |= ( 1 << PB6 );
             PORTB |= ( 1 << PB6 );
+            g_sensor_state = sensor_state_pinging;
             break;
 
         case sensor_index_right:
             // switch PORTB.7 to an output, set it high
             DDRB  |= ( 1 << PB7 );
             PORTB |= ( 1 << PB7 );
+            g_sensor_state = sensor_state_pinging;
             break;
     }
 
@@ -173,6 +203,7 @@
         {
             // rising edge => hold-off period over, reset timer
             TCNT1H = 0x00; TCNT1L = 0x00;
+            g_sensor_state = sensor_state_receiving;
         }
         else
         {
@@ -180,6 +211,7 @@
             uint16_t sample = TCNT1L;
             sample |= static_cast< uint16_t >( TCNT1H ) << 8;
             g_sensor_center_right = static_cast< uint8_t >( sample / 291 );
+            g_sensor_state = sensor_state_received;
             // disable pin-change interrupt
             PCMSK0 &= ~( 1 << PCINT6 );
         }
@@ -190,6 +222,7 @@
         {
             // rising edge => hold-off period over, reset timer
             TCNT1H = 0x00; TCNT1L = 0x00;
+            g_sensor_state = sensor_state_receiving;
         }
         else
         {
@@ -197,6 +230,7 @@
             uint16_t sample = TCNT1L;
             sample |= static_cast< uint16_t >( TCNT1H ) << 8;
             g_sensor_right = static_cast< uint8_t >( sample / 300 );
+            g_sensor_state = sensor_state_received;
             // disable pin-change interrupt
             PCMSK0 &= ~( 1 << PCINT7 );
         }
@@ -215,6 +249,7 @@
         {
             // rising edge => hold-off period over, reset timer
             TCNT1H = 0x00; TCNT1L = 0x00;
+            g_sensor_state = sensor_state_receiving;
         }
         else
         {
@@ -222,6 +257,7 @@
             uint16_t sample = TCNT1L;
             sample |= static_cast< uint16_t >( TCNT1H ) << 8;
             g_sensor_left = static_cast< uint8_t >( sample / 291 );
+            g_sensor_state = sensor_state_received;
             // disable pin-change interrupt
             PCMSK2 &= ~( 1 << PCINT22 );
         }
@@ -232,6 +268,7 @@
         {
             // rising edge => hold-off period over, reset timer
             TCNT1H = 0x00; TCNT1L = 0x00;
+            g_sensor_state = sensor_state_receiving;
         }
         else
         {
@@ -239,6 +276,7 @@
             uint16_t sample = TCNT1L;
             sample |= static_cast< uint16_t >( TCNT1H ) << 8;
             g_sensor_center_left = static_cast< uint8_t >( sample / 291 );
+            g_sensor_state = sensor_state_received;
             // disable pin-change interrupt
             PCMSK2 &= ~( 1 << PCINT21 );
         }
@@ -249,6 +287,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 );
@@ -272,6 +314,30 @@
 
 // ----------------------------------------------------------------------------------------
 
+static uint8_t compute_min_sensor_value()
+{
+    uint8_t curr_min_sensor_value = g_sensor_left;
+
+    if ( g_sensor_center_left < curr_min_sensor_value )
+    {
+        curr_min_sensor_value = g_sensor_center_left;
+    }
+
+    if ( g_sensor_center_right < curr_min_sensor_value )
+    {
+        curr_min_sensor_value = g_sensor_center_right;
+    }
+
+    if ( g_sensor_right < curr_min_sensor_value )
+    {
+        curr_min_sensor_value = g_sensor_right;
+    }
+
+    return curr_min_sensor_value;
+}
+
+// ----------------------------------------------------------------------------------------
+
 bool sonar_send_update()
 {
     //--    Only send data if the sonar machinery is running.
@@ -281,6 +347,53 @@
         return true; // no error
     }
 
+    //--    Always send if the current minimum sensor value is significantly different
+    //      than the previous minimum. Otherwise, only send every tenth call, or more
+    //      frequently for shorter distances. Assumes this function is called at 10 Hz.
+
+    static uint8_t s_last_min_sensor_value = 127; // starts as arbitrarily high
+    static uint8_t s_countdown_to_send     = 10;  // send when equal to zero
+
+    uint8_t curr_minimum = compute_min_sensor_value();
+    bool    send_update  = false;
+
+    if ( s_last_min_sensor_value < 5 )
+    {
+        send_update = ( s_last_min_sensor_value > curr_minimum );
+    }
+    else
+    {
+        send_update = ( s_last_min_sensor_value - 5 > curr_minimum );
+    }
+
+    s_last_min_sensor_value = curr_minimum;
+
+    --s_countdown_to_send;
+
+    if ( s_countdown_to_send == 0 )
+    {
+        send_update = true;
+    }
+
+    if ( ! send_update )
+    {
+        return true; // no error
+    }
+
+    //--    Reset the countdown timer. It doesn't matter why we are sending, just
+    //      don't want to send too rapidly, unnecessarily.
+
+    if ( curr_minimum > 20 )
+    {
+        s_countdown_to_send = 10;
+    }
+    else
+    {
+        s_countdown_to_send = util_max( ( curr_minimum / 2 ), 2 );
+    }
+
+    //--    Send the four sensor range values.
+
     return canmsg_send_sonar_front( g_sensor_left,
                                     g_sensor_center_left,
                                     g_sensor_center_right,
@@ -289,3 +402,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	Mon Jul 07 18:59:11 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;
+}
+
+// ----------------------------------------------------------------------------------------
+
+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;
+}
+
+// ----------------------------------------------------------------------------------------
+
+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;
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+