changeset 153:cf96319a494f main

Checkpoint commit for the Razor IMU software.
author Bob Cook <bob@bobcookdev.com>
date Tue, 17 Jul 2012 19:44:21 -0700
parents 3ff923cfab5f
children 7374d1cd677f
files main/projects/razor-imu/accel.cpp main/projects/razor-imu/accel.h main/projects/razor-imu/gyros.cpp main/projects/razor-imu/gyros.h main/projects/razor-imu/jamfile main/projects/razor-imu/magneto.cpp main/projects/razor-imu/magneto.h main/projects/razor-imu/main.cpp main/projects/razor-imu/project_defs.h
diffstat 9 files changed, 996 insertions(+), 0 deletions(-) [+]
line wrap: on
line diff
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/projects/razor-imu/accel.cpp	Tue Jul 17 19:44:21 2012 -0700
@@ -0,0 +1,115 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2012 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Functions to read the gyros from the SparkFun 9-DOF "Razor" IMU (SEN-09623)
+//
+//  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 "project_defs.h"
+
+#include "packages/avr/device/spinwait.h"
+#include "packages/avr/device/twi_master.h"
+
+#include "accel.h"
+
+// ----------------------------------------------------------------------------------------
+//  Our accelerometer is a 3-axis digital device accessible via TWI.
+//
+    
+static const uint8_t accelerometer_twi_address = 0x53;
+
+// ----------------------------------------------------------------------------------------
+
+bool accel_init()
+{
+    // set POWER_CTL to "measurement mode"
+
+    spinwait_delay_ms( 5 );
+
+    if ( ! twim_write( accelerometer_twi_address, 0x2D, 0x08 ) )
+    {
+        return false;
+    }
+
+    // set DATA_FMT to "full resolution"
+
+    spinwait_delay_ms( 5 );
+
+    if ( ! twim_write( accelerometer_twi_address, 0x31, 0x08 ) )
+    {
+        return false;
+    }
+
+    // set BW_RATE to "50 Hz"
+
+    spinwait_delay_ms( 5 );
+
+    if ( ! twim_write( accelerometer_twi_address, 0x2C, 0x09 ) )
+    {
+        return false;
+    }
+
+    return true;
+}
+
+// ----------------------------------------------------------------------------------------
+
+static int16_t convert_bytes_to_axis( uint8_t axis_value_hi, uint8_t axis_value_lo )
+{
+    int16_t result = static_cast< int8_t >( axis_value_hi << 8 );
+    result |= static_cast< int8_t >( axis_value_lo );
+    return result;
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool accel_values( int16_t* x, int16_t* y, int16_t* z )
+{
+    // we can read all six register bytes in one operation
+
+    if ( ! twim_write( accelerometer_twi_address, 0x32 ) )
+    {
+        return false;
+    }
+
+    uint8_t values[ 6 ];    // lsb, msb for x, y, z
+
+    if ( ! twim_read( accelerometer_twi_address, values, 6 ) )
+    {
+        return false;
+    }
+
+    *x = convert_bytes_to_axis( values[ 1 ], values[ 0 ] );
+    *y = convert_bytes_to_axis( values[ 3 ], values[ 2 ] );
+    *z = convert_bytes_to_axis( values[ 5 ], values[ 4 ] );
+
+    return true;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/projects/razor-imu/accel.h	Tue Jul 17 19:44:21 2012 -0700
@@ -0,0 +1,43 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2012 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Functions to read the gyros from the SparkFun 9-DOF "Razor" IMU (SEN-09623)
+//
+//  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( BCDRL_PROJECTS_IMU_ACCEL_H )
+#define BCDRL_PROJECTS_IMU_ACCEL_H
+
+#include <stdint.h>
+
+// ----------------------------------------------------------------------------------------
+//  accel_init() assumes twim_init() was previously called.
+//
+
+bool accel_init();
+bool accel_values( int16_t* x, int16_t* y, int16_t* z );
+
+// ----------------------------------------------------------------------------------------
+#endif // #if !defined( BCDRL_PROJECTS_IMU_ACCEL_H )
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/projects/razor-imu/gyros.cpp	Tue Jul 17 19:44:21 2012 -0700
@@ -0,0 +1,164 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2012 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Functions to read the gyros from the SparkFun 9-DOF "Razor" IMU (SEN-09623)
+//
+//  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 "project_defs.h"
+
+#include "packages/avr/device/adc.h"
+#include "packages/avr/device/int_helpers.h"
+
+#include "gyros.h"
+
+// ----------------------------------------------------------------------------------------
+//  Our gyros are LPR530AL and LY530AH, with analog output to:
+//
+//          ADC0: LY530AH z-axis
+//          ADC1: LPR530AL x-axis
+//          ADC2: LPR530AL y-axis
+//
+//  The "zero point" was determined experimentally for my hardware.
+//
+    
+static const uint16_t gyro_axis_zero_x = 256;
+static const uint16_t gyro_axis_zero_y = 255;
+static const uint16_t gyro_axis_zero_z = 261;
+
+static const uint8_t gyro_axis_x = 0;
+static const uint8_t gyro_axis_y = 1;
+static const uint8_t gyro_axis_z = 2;
+static const uint8_t gyro_axis_count = 3;
+
+static const uint8_t gyro_adc_channel_x = 1;
+static const uint8_t gyro_adc_channel_y = 2;
+static const uint8_t gyro_adc_channel_z = 0;
+
+static const uint8_t gyro_raw_samples_to_accumulate = 64;
+
+static uint8_t  g_gyro_current_axis;
+static uint16_t g_gyro_raw_x;
+static uint16_t g_gyro_raw_y;
+static uint16_t g_gyro_raw_z;
+static uint8_t  g_gyro_raw_samples;
+
+static uint16_t g_gyro_current_value_x;
+static uint16_t g_gyro_current_value_y;
+static uint16_t g_gyro_current_value_z;
+
+// ----------------------------------------------------------------------------------------
+
+void gyros_start()
+{
+    adc_init();
+    ADCSRA |= ( 1 << ADIE );
+
+    g_gyro_raw_x = 0;
+    g_gyro_raw_y = 0;
+    g_gyro_raw_z = 0;
+    g_gyro_raw_samples = 0;
+
+    adc_start_read( gyro_adc_channel_x );
+    g_gyro_current_axis = gyro_axis_x;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void gyros_stop()
+{
+    adc_stop();
+}
+
+// ----------------------------------------------------------------------------------------
+
+void gyros_values( int16_t* x, int16_t* y, int16_t* z )
+{
+    uint16_t temp_x, temp_y, temp_z;
+    {
+        guard_disable_interrupts guard;
+
+        temp_x = g_gyro_current_value_x;
+        temp_y = g_gyro_current_value_y;
+        temp_z = g_gyro_current_value_z;
+    }
+
+    *x = static_cast< int16_t >( temp_x ) - gyro_axis_zero_x;
+    *y = static_cast< int16_t >( temp_y ) - gyro_axis_zero_y;
+    *z = static_cast< int16_t >( temp_z ) - gyro_axis_zero_z;
+}
+
+// ----------------------------------------------------------------------------------------
+
+ISR( ADC_vect )
+{
+    // save the current axis value and start the next one
+    
+    switch ( g_gyro_current_axis )
+    {
+        case gyro_axis_x:
+            g_gyro_raw_x += adc_get_result();
+            adc_start_read( gyro_adc_channel_y );
+            g_gyro_current_axis = gyro_axis_y;
+            break;
+
+        case gyro_axis_y:
+            g_gyro_raw_y += adc_get_result();
+            adc_start_read( gyro_adc_channel_z );
+            g_gyro_current_axis = gyro_axis_z;
+            break;
+
+        case gyro_axis_z:
+            g_gyro_raw_z += adc_get_result();
+            adc_start_read( gyro_adc_channel_x );
+            g_gyro_current_axis = gyro_axis_x;
+            ++g_gyro_raw_samples;
+            break;
+
+        default:
+            // this should NEVER happen
+            break;
+    }
+
+    // have enough raw samples to update the results?
+
+    if ( g_gyro_raw_samples == ( gyro_raw_samples_to_accumulate - 1 ) )
+    {
+        g_gyro_current_value_x = g_gyro_raw_x / gyro_raw_samples_to_accumulate;
+        g_gyro_current_value_y = g_gyro_raw_y / gyro_raw_samples_to_accumulate;
+        g_gyro_current_value_z = g_gyro_raw_z / gyro_raw_samples_to_accumulate;
+
+        g_gyro_raw_x = 0;
+        g_gyro_raw_y = 0;
+        g_gyro_raw_z = 0;
+        g_gyro_raw_samples = 0;
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/projects/razor-imu/gyros.h	Tue Jul 17 19:44:21 2012 -0700
@@ -0,0 +1,43 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2012 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Functions to read the gyros from the SparkFun 9-DOF "Razor" IMU (SEN-09623)
+//
+//  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( BCDRL_PROJECTS_RAZOR_IMU_GYROS_H )
+#define BCDRL_PROJECTS_RAZOR_IMU_GYROS_H
+
+#include <stdint.h>
+
+// ----------------------------------------------------------------------------------------
+
+void gyros_start();
+void gyros_stop();
+void gyros_values( int16_t* x, int16_t* y, int16_t* z );
+
+// ----------------------------------------------------------------------------------------
+#endif // #if !defined( BCDRL_PROJECTS_RAZOR_IMU_GYROS_H )
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/projects/razor-imu/jamfile	Tue Jul 17 19:44:21 2012 -0700
@@ -0,0 +1,43 @@
+# -----------------------------------------------------------------------------------------
+#
+#   Copyright (C) 2012 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 /projects/razor-imu/jamfile" ; }
+
+SubDir TOP projects razor-imu ;
+
+# -----------------------------------------------------------------------------------------
+
+avr_executable
+    razor-imu atmega328p
+    : main.cpp accel.cpp gyros.cpp magneto.cpp
+      packages.avr.device.pkg
+      packages.avr.redir.pkg
+      packages.common.util.pkg
+    ;
+
+# -----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/projects/razor-imu/magneto.cpp	Tue Jul 17 19:44:21 2012 -0700
@@ -0,0 +1,113 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2012 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Functions to read the gyros from the SparkFun 9-DOF "Razor" IMU (SEN-09623)
+//
+//  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 "project_defs.h"
+
+#include "packages/avr/device/spinwait.h"
+#include "packages/avr/device/twi_master.h"
+
+#include "magneto.h"
+
+// ----------------------------------------------------------------------------------------
+//  Our magnetometer is a 3-axis digital device accessible via TWI.
+//
+    
+static const uint8_t magnetometer_twi_address = 0x1E;
+
+// ----------------------------------------------------------------------------------------
+
+bool magneto_init()
+{
+    // set MODE to "continuous measurement"
+
+    spinwait_delay_ms( 5 );
+
+    if ( ! twim_write( magnetometer_twi_address, 0x02, 0x00 ) )
+    {
+        return false;
+    }
+
+    // set CONFIGURATION REGISTER A to 50 Hz refresh, normal mode; the static_cast<>
+    // is required because 0x00 might be an integer or ptr, and the compiler complains.
+
+    spinwait_delay_ms( 5 );
+
+    if ( ! twim_write( magnetometer_twi_address, static_cast< uint8_t >( 0x00 ), 0x18 ) )
+    {
+        return false;
+    }
+
+    return true;
+}
+
+// ----------------------------------------------------------------------------------------
+
+static int16_t convert_bytes_to_axis( uint8_t axis_value_hi, uint8_t axis_value_lo )
+{
+    int16_t result = static_cast< int8_t >( axis_value_hi << 8 );
+    result |= static_cast< int8_t >( axis_value_lo );
+    return result;
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool magneto_values( int16_t* x, int16_t* y, int16_t* z )
+{
+    // we can read all six register bytes in one operation
+
+    if ( ! twim_write( magnetometer_twi_address, 0x03 ) )
+    {
+        return false;
+    }
+
+    uint8_t values[ 6 ];    // msb, lsb for x, y, z
+
+    if ( ! twim_read( magnetometer_twi_address, values, 6 ) )
+    {
+        return false;
+    }
+
+    // the internal axis of the HMC5843 are different than the IMU axis:
+    //
+    //      internal Y axis -> -1 * IMU X axis
+    //      internal X axis -> -1 * IMU Y axis
+    //      internal Z axis -> -1 * IMU X axis
+
+    *x = -1 * convert_bytes_to_axis( values[ 2 ], values[ 3 ] );
+    *y = -1 * convert_bytes_to_axis( values[ 0 ], values[ 1 ] );
+    *z = -1 * convert_bytes_to_axis( values[ 4 ], values[ 5 ] );
+    
+    return true;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/projects/razor-imu/magneto.h	Tue Jul 17 19:44:21 2012 -0700
@@ -0,0 +1,43 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2012 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Functions to read the gyros from the SparkFun 9-DOF "Razor" IMU (SEN-09623)
+//
+//  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( BCDRL_PROJECTS_IMU_MAGNETO_H )
+#define BCDRL_PROJECTS_IMU_MAGNETO_H
+
+#include <stdint.h>
+
+// ----------------------------------------------------------------------------------------
+//  magneto_init() assumes twim_init() was previously called.
+//
+
+bool magneto_init();
+bool magneto_values( int16_t* x, int16_t* y, int16_t* z );
+
+// ----------------------------------------------------------------------------------------
+#endif // #if !defined( BCDRL_PROJECTS_IMU_MAGNETO_H )
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/projects/razor-imu/main.cpp	Tue Jul 17 19:44:21 2012 -0700
@@ -0,0 +1,363 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2012 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  SparkFun 9-DOF "Razor" IMU
+//
+//  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.
+//
+//  This is a port of various other projects that improve the SparkFun 9-DOF Razor IMU
+//  but targeting the original product (SEN-09623, which most projects don't support).
+//
+// ----------------------------------------------------------------------------------------
+
+#include <avr/interrupt.h>
+#include <avr/io.h>
+#include <avr/pgmspace.h>
+#include <avr/sleep.h>
+
+#include <ctype.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "project_defs.h"
+
+#include "packages/avr/device/int_helpers.h"
+#include "packages/avr/device/spinwait.h"
+#include "packages/avr/device/twi_master.h"
+
+#include "packages/avr/redir/redir.h"
+
+#include "packages/common/filt/filtbuf_u16.h"
+#include "packages/common/util/misc.h"
+
+// always after other includes
+#include "packages/avr/device/workaround34734.h"
+
+#include "accel.h"
+#include "gyros.h"
+#include "magneto.h"
+
+// ----------------------------------------------------------------------------------------
+
+static inline void led_off() __attribute__((always_inline));
+static inline void led_off()
+{
+    PORTB &= ~( 1 << PB5 );
+}
+
+static inline void led_on() __attribute__((always_inline));
+static inline void led_on()
+{
+    PORTB |= ( 1 << PB5 );
+}
+
+static inline void led_toggle() __attribute__((always_inline));
+static inline void led_toggle()
+{
+    PORTB ^= ( 1 << PB5 );
+}
+
+// ----------------------------------------------------------------------------------------
+#if 0
+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;
+
+void odrson_fatal_error( uint8_t fault )
+{
+    cli(); // no more interrupts
+    DDRB = 0; DDRC = 0; DDRD = 0; // everything is an input
+    DDRB |= ( 1 << PB2 ); // except the leds
+    DDRC |= ( 1 << PC4 ) | ( 1 << PC5 );
+    DDRD |= ( 1 << PD7 );
+    leds_off();
+
+    for ( ;; )
+    {
+        for ( uint8_t i = 0; i < 5; i++ )
+        {
+            led_red_on();
+            spinwait_delay_ms( 125 );
+            led_red_off();
+            spinwait_delay_ms( 75 );
+        }
+
+        spinwait_delay_ms( 125 );
+        led_red_on();
+        led_yellow_on();
+        spinwait_delay_ms( 125 );
+
+        for ( uint8_t i = 0; i < fault; i++ )
+        {
+            led_green_1_on();
+            led_green_2_on();
+            spinwait_delay_ms( 350 );
+            
+            led_green_1_off();
+            led_green_2_off();
+            spinwait_delay_ms( 350 );
+        }
+
+        spinwait_delay_ms( 1000 );
+        
+        led_red_off();
+        led_yellow_off();
+    }
+}
+#endif
+
+// ----------------------------------------------------------------------------------------
+
+void hw_init()
+{
+    //--    Turn off all interrupts.
+
+    cli();
+    interrupts_clear_all();
+
+    //--    Indicator LED on PORTB.5
+
+    DDRB |= ( 1 << PB5 );
+    led_off();
+
+    //--    Blink the LED just to say hello.
+    
+    for ( uint8_t i = 0; i < 5; i++ )
+    {
+        led_on();
+        spinwait_delay_ms( 75 );
+        led_off();
+        spinwait_delay_ms( 75 );
+    }
+
+#if 0
+    //--    Timer0 gives up a periodic "heartbeat" interrupt at ~100 Hz.
+
+    TCCR0A = 0
+
+        | ( 0 << COM0A1 ) | ( 0 << COM0A0 )                 // OC0A disconnected
+        | ( 0 << COM0B1 ) | ( 0 << COM0B0 )                 // OC0B disconnected
+        | ( 1 << WGM01  ) | ( 0 << WGM00  )                 // CTC mode
+        ;
+
+    TCCR0B = 0
+
+        | ( 0 << FOC0A ) | ( 0 << FOC0B )                   // no force output compare
+        | ( 0 << WGM02 )                                    // CTC mode
+        | ( 1 << CS02  ) | ( 0 << CS01  ) | ( 1 << CS00 )   // clk/1024
+        ;
+
+    OCR0A = 156;
+
+    TIMSK0 |= ( 1 << OCIE0A );
+
+    //--    Initialize the CAN related functions and hardware.
+
+    if ( ! canmsg_init() )
+    {
+        odrson_fatal_error( odrson_fatal_error_can_init );
+    }
+
+    //--    Initialize the sonar subsystem.
+
+    sonar_init();
+#endif
+
+    //--    When we sleep we want the "idle" mode e.g. wake up on any interrupt.
+
+//    set_sleep_mode( SLEEP_MODE_IDLE );
+
+    //--    Re-enable interrupts.
+
+    sei();
+
+    //--    Initialize the UART output.
+
+    redir_initialize();
+    printf_P( PSTR("\n\n\nHello World!\n") );
+
+    //--    Initialize the TWI master, to communicate with the ADXL345 and HMC5843.
+
+    twim_init();
+
+    if ( ! accel_init() )
+    {
+        printf_P( PSTR("accel_init() failed\n") );
+    }
+
+    if ( ! magneto_init() )
+    {
+        printf_P( PSTR("magneto_init() failed\n") );
+    }
+
+    //--    Initialize the gyros package, to read from the LPR530AL and LY530AH.
+
+    gyros_start();
+}
+
+// ----------------------------------------------------------------------------------------
+
+ISR( TIMER0_COMPA_vect )
+{
+    static uint8_t count = 0;
+
+    // sonar status: every 2.5 seconds
+    // sonar update: every 0.5 seconds
+    
+    switch ( ++count )
+    {
+        case 50:
+        case 100:
+        case 150:
+        case 200:
+//            g_triggers |= trigger_send_sonar_update;
+            break;
+
+        case 250:
+//            g_triggers |= trigger_send_sonar_status;
+//            g_triggers |= trigger_send_sonar_update;
+            count = 0;
+            break;
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+int main()
+{
+    //--    Initialize the hardware and send a "hello we started" message.
+
+    hw_init();
+
+    led_off();
+
+    for ( ;; )
+    {
+        int16_t gyro_x, gyro_y, gyro_z;
+        gyros_values( &gyro_x, &gyro_y, &gyro_z );
+
+        int16_t accel_x, accel_y, accel_z;
+        accel_values( &accel_x, &accel_y, &accel_z );
+
+        int16_t magneto_x, magneto_y, magneto_z;
+        magneto_values( &magneto_x, &magneto_y, &magneto_z );
+
+        printf_P( PSTR("\rGX: %04d GY: %04d GZ: %04d"
+                       "  AX: %04d AY: %04d AZ: %04d"
+                       "  MX: %04d MY: %04d MZ: %04d"),
+                  gyro_x, gyro_y, gyro_z,
+                  accel_x, accel_y, accel_z,
+                  magneto_x, magneto_y, magneto_z );
+
+        spinwait_delay_ms( 20 );
+    }
+
+#if 0
+    //--    Loop forever responding to CAN messages and system status.
+
+    uint8_t can_comm_errors = 0;
+
+    for ( ;; )
+    {
+        //--    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();
+
+        //--    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;
+            }
+        }
+
+        //--    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;
+            }
+        }
+
+        //--    Any pending CAN messages to receive/process?
+
+        if ( ! canmsg_process_pending() )
+        {
+            ++can_comm_errors;
+        }
+        else
+        {
+            if ( can_comm_errors > 0 )
+            {
+                --can_comm_errors;
+            }
+        }
+
+        //--    Report warning and/or error conditions.
+
+        bool warning = false;
+
+        if ( can_comm_errors > 50 )
+        {
+            odrson_fatal_error( odrson_fatal_error_can_comm );
+        }
+        else if ( can_comm_errors > 10 )
+        {
+            warning = true;
+        }
+
+        if ( warning )
+        {
+            led_yellow_on();
+        }
+        else
+        {
+            led_yellow_off();
+        }
+    }
+
+    odrson_fatal_error( odrson_fatal_error_exiting );
+#endif
+
+    return 0;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/projects/razor-imu/project_defs.h	Tue Jul 17 19:44:21 2012 -0700
@@ -0,0 +1,69 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2012 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    8000000L
+
+// ----------------------------------------------------------------------------------------
+//  packages/avr/device/adc
+
+#define PRJ_ADC_ENABLED
+#define PRJ_ADC_EXTENDED_READ
+
+#define PRJ_ADC_CLOCK_FACTOR_DIV_32
+#define PRJ_ADC_REFERENCE_AREF_IS_AVCC
+
+#define PRJ_ADC_USE_CHANNEL_0
+#define PRJ_ADC_USE_CHANNEL_1
+#define PRJ_ADC_USE_CHANNEL_2
+
+// ----------------------------------------------------------------------------------------
+//  packages/avr/device/twi
+
+#define PRJ_TWI_MASTER_ENABLE
+#define PRJ_TWI_BUS_FREQ       100000L
+
+// ----------------------------------------------------------------------------------------
+//  packages/avr/device/uart
+
+//#define PRJ_UART0_USE_INTERRUPT_MODE
+#define PRJ_UART0_USE_POLLED_MODE
+#define PRJ_UART0_BUFFER_SIZE           16
+#define PRJ_UART0_INITIAL_BAUDRATE      38400
+
+// ----------------------------------------------------------------------------------------
+//  packages/avr/redir
+
+#define PRJ_REDIR_VIA_UART0
+#define PRJ_REDIR_OUTPUT_DO_AUTO_CRLF
+
+
+#endif // #if !defined( PROJECT_DEFS_H )
+// ----------------------------------------------------------------------------------------
+