changeset 248:1fd327ac6ad8 main

Checkpoint for the IMU node firmware.
author Bob Cook <bob@bobcookdev.com>
date Mon, 24 Aug 2015 20:51:16 -0700
parents caf5ac88d09c
children c36e268a624d
files main/robots/odr-imu/canmsgs.cpp main/robots/odr-imu/func.h main/robots/odr-imu/imu.cpp main/robots/odr-imu/jamfile main/robots/odr-imu/main.cpp main/robots/odr-imu/project_defs.h main/robots/odr-imu/status.cpp
diffstat 7 files changed, 270 insertions(+), 84 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr-imu/canmsgs.cpp	Sat Aug 22 13:48:35 2015 -0700
+++ b/main/robots/odr-imu/canmsgs.cpp	Mon Aug 24 20:51:16 2015 -0700
@@ -75,26 +75,82 @@
 
 // ----------------------------------------------------------------------------------------
 
-bool canmsg_send_gps_latitude( int16_t degrees, uint16_t minutes, uint16_t min_fract )
+bool canmsg_send_imu_roll( float roll )
+{
+    uint32_t msgid;
+    msgid = can_build_message_id( can_node_sensor_imu,
+                                  can_node_broadcast,
+                                  can_dataid_imu_roll );
+
+    can_data_imu_data data;
+
+    roll *= static_cast< float >( can_data_imu_multiplier );
+    data.data = static_cast< int32_t >( roll );
+
+    return m1can_send( msgid, reinterpret_cast< uint8_t* >( &data ), sizeof( data ) );
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool canmsg_send_imu_pitch( float pitch )
 {
     uint32_t msgid;
-    msgid = can_build_message_id( can_node_sensor_gps,
+    msgid = can_build_message_id( can_node_sensor_imu,
                                   can_node_broadcast,
-                                  can_dataid_gps_latitude );
-
-    can_data_gps_data data;
+                                  can_dataid_imu_pitch );
 
-    data.degrees          = degrees;
-    data.min_thousandths  = minutes;
-    data.min_thousandths *= can_data_gps_min_multiplier;
-    data.min_thousandths += ( min_fract / // convert between units
-                ( nmea_fractional_minutes_multiplier / can_data_gps_min_multiplier ) );
+    can_data_imu_data data;
+
+    pitch *= static_cast< float >( can_data_imu_multiplier );
+    data.data = static_cast< int32_t >( pitch );
 
     return m1can_send( msgid, reinterpret_cast< uint8_t* >( &data ), sizeof( data ) );
 }
 
 // ----------------------------------------------------------------------------------------
 
+bool canmsg_send_imu_yaw( float yaw )
+{
+    uint32_t msgid;
+    msgid = can_build_message_id( can_node_sensor_imu,
+                                  can_node_broadcast,
+                                  can_dataid_imu_yaw );
+
+    can_data_imu_data data;
+
+    yaw *= static_cast< float >( can_data_imu_multiplier );
+    data.data = static_cast< int32_t >( yaw );
+
+    return m1can_send( msgid, reinterpret_cast< uint8_t* >( &data ), sizeof( data ) );
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool canmsg_send_imu_health( uint32_t health )
+{
+    uint32_t msgid;
+    msgid = can_build_message_id( can_node_sensor_imu,
+                                  can_node_broadcast,
+                                  can_dataid_imu_health );
+
+    can_data_imu_health data;
+    data.health = health;
+
+    return m1can_send( msgid, reinterpret_cast< uint8_t* >( &data ), sizeof( data ) );
+}
+
+// ----------------------------------------------------------------------------------------
+
+static void update_imu_yaw_adj( const can_data_imu_data* data )
+{
+    float adjustment = static_cast< float >( data->data );
+    adjustment /= static_cast< float >( can_data_imu_multiplier );
+
+    imu_set_yaw_adjustment( adjustment );
+}
+
+// ----------------------------------------------------------------------------------------
+
 bool canmsg_process_pending()
 {
     uint32_t recvid;
@@ -133,6 +189,15 @@
             case can_dataid_odrmgr_update:
                 status_got_heartbeat( srcnode );
                 break;
+
+            case can_dataid_imu_zero_gyros:
+                imu_zero_gyros();
+                break;
+
+            case can_dataid_imu_yaw_adj:
+                update_imu_yaw_adj(
+                        reinterpret_cast< const can_data_imu_data* >( &recvdata ) );
+                break;
         }
     }
 
--- a/main/robots/odr-imu/func.h	Sat Aug 22 13:48:35 2015 -0700
+++ b/main/robots/odr-imu/func.h	Mon Aug 24 20:51:16 2015 -0700
@@ -1,6 +1,6 @@
 // ----------------------------------------------------------------------------------------
 //
-//  Copyright (C) 2012-2013 Bob Cook
+//  Copyright (C) 2015 Bob Cook
 //    
 //  Bob Cook Development, Robotics Library
 //  http://www.bobcookdev.com/rl/
@@ -42,10 +42,17 @@
 void status_got_emergency( uint8_t node );
 void status_got_all_clear( uint8_t node );
 void status_got_heartbeat( uint8_t node );
-void status_got_gps_sentence();
+void status_got_imu_packet();
 
 bool status_is_emergency();
-bool status_is_gps_active();
+bool status_is_imu_active();
+
+// ----------------------------------------------------------------------------------------
+
+void imu_init();
+void imu_zero_gyros();
+void imu_set_yaw_adjustment( float adj );
+void imu_process_pending();
 
 // ----------------------------------------------------------------------------------------
 
@@ -53,9 +60,10 @@
 bool canmsg_send_emergency();
 bool canmsg_send_all_clear();
 bool canmsg_send_heartbeat();
-bool canmsg_send_gps_fix_info( uint8_t satellites );
-bool canmsg_send_gps_latitude( int16_t degrees, uint16_t minutes, uint16_t min_fract );
-bool canmsg_send_gps_longitude( int16_t degrees, uint16_t minutes, uint16_t min_fract );
+bool canmsg_send_imu_roll( float roll );
+bool canmsg_send_imu_pitch( float pitch );
+bool canmsg_send_imu_yaw( float yaw );
+bool canmsg_send_imu_health( uint32_t health );
 bool canmsg_process_pending();
 
 // ----------------------------------------------------------------------------------------
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-imu/imu.cpp	Mon Aug 24 20:51:16 2015 -0700
@@ -0,0 +1,150 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2015 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Functions for the Outdoor Robot IMU node to communicate with the UM7 module.
+//
+//  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/eeprom.h>
+
+#include "project_defs.h"
+
+#include "func.h"
+
+#include "packages/avr/device/spinwait.h"
+#include "packages/avr/device/uart.h"
+
+#include "packages/common/chrobotics/um7.h"
+#include "packages/common/chrobotics/um7_data.h"
+
+// ----------------------------------------------------------------------------------------
+
+static float* eeprom_yaw_adjustment_address = reinterpret_cast< float* >( 0x04 );
+
+float g_yaw_adjustment = 0.0;
+
+// ----------------------------------------------------------------------------------------
+
+void imu_init()
+{
+    uart0_init();
+    uart0_set_baudrate( 115200 );
+
+    g_yaw_adjustment = eeprom_read_float( eeprom_yaw_adjustment_address );
+}
+
+// ----------------------------------------------------------------------------------------
+
+void imu_zero_gyros()
+{
+    uint8_t um7_packet[ um7_min_packet_size ];
+    uint8_t pkt_len = um7_create_packet( um7_packet, um7_cmd_zero_gyros, NULL, 0 );
+    uart0_write( um7_packet, pkt_len );
+}
+
+// ----------------------------------------------------------------------------------------
+
+void imu_set_yaw_adjustment( float adj )
+{
+    g_yaw_adjustment = adj;
+    eeprom_update_float( eeprom_yaw_adjustment_address, adj );
+}
+
+// ----------------------------------------------------------------------------------------
+
+static void recv_euler_phi_theta()
+{
+    //--    The sensor is physically 180 degrees from its true direction for yaw, and 
+    //      the pitch and roll are reversed. Fix this in software.
+    //
+    //      For yaw we want to output the range of [-180,180] rather than [0,360]
+
+    float pitch = um7_packet_value_s16( 0 ) / 91.02222;
+    float roll  = um7_packet_value_s16( 2 ) / 91.02222;
+    float yaw   = um7_packet_value_s16( 4 ) / 91.02222;
+
+    roll  *= -1.0;
+    pitch *= -1.0;
+
+    yaw += g_yaw_adjustment; // magnetic declination correction
+    if ( yaw > 360.0 ) yaw -= 360.0;
+    if ( yaw < 0.0 ) yaw += 360.0;
+    yaw -= 180.0;
+
+    canmsg_send_imu_pitch( pitch );
+    canmsg_send_imu_roll( roll );
+    canmsg_send_imu_yaw( yaw );
+}
+
+// ----------------------------------------------------------------------------------------
+
+static void recv_health()
+{
+    canmsg_send_imu_health( um7_packet_value_u32( 0 ) );
+}
+
+// ----------------------------------------------------------------------------------------
+// return true if a full packet is found
+
+static bool recv_imu_packet()
+{
+    for ( uint8_t i = 0; i < 16; ++i )
+    {
+        if ( ! uart0_is_char_available() )
+        {
+            return false;
+        }
+
+        if ( um7_parse( uart0_read() ) )
+        {
+            status_got_imu_packet();
+            return true;
+        }
+    }
+
+    return false;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void imu_process_pending()
+{
+    if ( ! recv_imu_packet() )
+    {
+        return;
+    }
+
+    if ( um7_packet_address() == um7_addr_dreg_euler_phi_theta )
+    {
+        recv_euler_phi_theta();
+    }
+    else if ( um7_packet_address() == um7_addr_dreg_health )
+    {
+        recv_health();
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr-imu/jamfile	Sat Aug 22 13:48:35 2015 -0700
+++ b/main/robots/odr-imu/jamfile	Mon Aug 24 20:51:16 2015 -0700
@@ -35,11 +35,13 @@
     odr-imu atmega16m1
     : main.cpp
       canmsgs.cpp
+      imu.cpp
       leds.cpp
       status.cpp
       packages.avr.can.pkg
       packages.avr.device.pkg
       packages.common.can.pkg
+      packages.common.chrobotics.pkg
       packages.common.util.pkg
     ;
 
--- a/main/robots/odr-imu/main.cpp	Sat Aug 22 13:48:35 2015 -0700
+++ b/main/robots/odr-imu/main.cpp	Mon Aug 24 20:51:16 2015 -0700
@@ -40,6 +40,8 @@
 #include "packages/avr/device/spinwait.h"
 #include "packages/avr/device/uart.h"
 
+#include "packages/common/chrobotics/um7.h"
+#include "packages/common/chrobotics/um7_data.h"
 #include "packages/common/util/misc.h"
 
 // always after other includes
@@ -56,11 +58,11 @@
 
 // ----------------------------------------------------------------------------------------
 
-static const uint8_t odrgps_fatal_error_can_init = 1;
-static const uint8_t odrgps_fatal_error_can_comm = 2;
-static const uint8_t odrgps_fatal_error_exiting  = 9;
+static const uint8_t odrimu_fatal_error_can_init = 1;
+static const uint8_t odrimu_fatal_error_can_comm = 2;
+static const uint8_t odrimu_fatal_error_exiting  = 9;
 
-void odrgps_fatal_error( uint8_t fault )
+void odrimu_fatal_error( uint8_t fault )
 {
     cli(); // no more interrupts
     DDRB = 0; DDRC = 0; DDRD = 0; // everything is an input
@@ -104,7 +106,7 @@
 
 // ----------------------------------------------------------------------------------------
 
-void odrgps_hw_init()
+void odrimu_hw_init()
 {
     //--    Turn off all interrupts.
 
@@ -155,16 +157,15 @@
 
     status_init();
 
-    //--    Initialize the serial port to the GPS unit, 4800 bps.
+    //--    Initialize the IMU.
 
-    uart0_init();
-    uart0_set_baudrate( 4800 );
+    imu_init();
 
     //--    Initialize the CAN related functions and hardware.
 
     if ( ! canmsg_init() )
     {
-        odrgps_fatal_error( odrgps_fatal_error_can_init );
+        odrimu_fatal_error( odrimu_fatal_error_can_init );
     }
 
     //--    When we sleep we want the "idle" mode e.g. wake up on any interrupt.
@@ -236,7 +237,7 @@
 {
     //--    Initialize the hardware and send a "hello we started" message.
 
-    odrgps_hw_init();
+    odrimu_hw_init();
     canmsg_send_all_clear();
 
     //--    Loop forever responding to CAN messages and system status.
@@ -254,54 +255,15 @@
 
 //        sleep_mode();
 
-        //--    Character available from the GPS module?
-
-        if ( uart0_is_char_available() )
-        {
-            if ( nmea_parse( uart0_read() ) )
-            {
-                //--    NMEA sentence parsed successfully.
-
-                led_green_2_toggle();
-                status_got_gps_sentence();
-
-                uint8_t satellites;
-
-                if ( nmea_position_fix( &satellites ) )
-                {
-                    if ( ! canmsg_send_gps_fix_info( satellites ) )
-                    {
-                        ++can_comm_errors;
-                    }
-
-                    int16_t  degrees;
-                    uint16_t minutes;
-                    uint16_t min_fract;
+        //--    Process data coming from the IMU module.
 
-                    nmea_latitude( &degrees, &minutes, &min_fract );
-
-                    if ( ! canmsg_send_gps_latitude( degrees, minutes, min_fract ) )
-                    {
-                        ++can_comm_errors;
-                    }
-
-                    nmea_longitude( &degrees, &minutes, &min_fract );
-
-                    if ( ! canmsg_send_gps_longitude( degrees, minutes, min_fract ) )
-                    {
-                        ++can_comm_errors;
-                    }
-                }
-                else
-                {
-                    if ( ! canmsg_send_gps_fix_info( 0 ) )
-                    {
-                        ++can_comm_errors;
-                    }
-                }
-            }
+        imu_process_pending();
+        
+        if ( status_is_imu_active() )
+        {
+            led_green_2_on();
         }
-        else if ( ! status_is_gps_active() )
+        else
         {
             led_green_2_off();
         }
@@ -356,7 +318,7 @@
 
         if ( can_comm_errors > 50 )
         {
-            odrgps_fatal_error( odrgps_fatal_error_can_comm );
+            odrimu_fatal_error( odrimu_fatal_error_can_comm );
         }
         else if ( can_comm_errors > 10 )
         {
@@ -373,7 +335,7 @@
         }
     }
 
-    odrgps_fatal_error( odrgps_fatal_error_exiting );
+    odrimu_fatal_error( odrimu_fatal_error_exiting );
     return 0;
 }
 
--- a/main/robots/odr-imu/project_defs.h	Sat Aug 22 13:48:35 2015 -0700
+++ b/main/robots/odr-imu/project_defs.h	Mon Aug 24 20:51:16 2015 -0700
@@ -35,12 +35,15 @@
 
 #define PRJ_M1CAN_ENABLE
 #define PRJ_M1CAN_CANBUS_500_KHZ
-#define PRJ_M1CAN_TX_BUFFER_SIZE  8
+#define PRJ_M1CAN_TX_BUFFER_SIZE    4
 
 // ----------------------------------------------------------------------------------------
 //  packages/avr/device
 
-#define PRJ_UART0_USE_POLLED_MODE
+#define PRJ_UART0_USE_INTERRUPT_MODE
+#define PRJ_UART0_INITIAL_BAUCRATE    115200
+#define PRJ_UART0_BUFFER_SIZE         32
+#define PRJ_UART0_EXTENDED_READ_WRITE
 
 
 #endif // #if !defined( PROJECT_DEFS_H )
--- a/main/robots/odr-imu/status.cpp	Sat Aug 22 13:48:35 2015 -0700
+++ b/main/robots/odr-imu/status.cpp	Mon Aug 24 20:51:16 2015 -0700
@@ -75,10 +75,6 @@
 
 //        case can_node_odr_sonar_front:
 //            break;
-
-//        case can_node_gps:
-//            g_beats_without_gps = 0;
-//            break;
     }
 }
 
@@ -123,8 +119,8 @@
         return true;
     }
 
-    // more than five seconds == down
-    if ( g_beats_without_imu > 11 )
+    // more than two seconds == down
+    if ( g_beats_without_imu > 5 )
     {
         return true;
     }
@@ -136,8 +132,8 @@
 
 bool status_is_imu_active()
 {
-    // more than five seconds == down
-    return ( g_beats_without_imu < 11 );
+    // more than two seconds == down
+    return ( g_beats_without_imu < 5 );
 }
 
 // ----------------------------------------------------------------------------------------