changeset 251:87864f3a89e4 main

IMU module code.
author Bob Cook <bob@bobcookdev.com>
date Mon, 07 Sep 2015 14:49:45 -0700
parents ce9081347cd2
children 1b764c120921
files main/robots/odr-imu/canmsgs.cpp main/robots/odr-imu/func.h main/robots/odr-imu/imu.cpp main/robots/odr-imu/main.cpp main/robots/odr-imu/project_defs.h main/robots/odr-imu/status.cpp
diffstat 6 files changed, 102 insertions(+), 60 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr-imu/canmsgs.cpp	Sun Sep 06 10:53:50 2015 -0700
+++ b/main/robots/odr-imu/canmsgs.cpp	Mon Sep 07 14:49:45 2015 -0700
@@ -37,8 +37,6 @@
 #include "packages/common/can/can_messages.h"
 #include "packages/common/can/can_nodes.h"
 
-#include "packages/avr/device/spinwait.h"
-
 // ----------------------------------------------------------------------------------------
 
 bool canmsg_init()
--- a/main/robots/odr-imu/func.h	Sun Sep 06 10:53:50 2015 -0700
+++ b/main/robots/odr-imu/func.h	Mon Sep 07 14:49:45 2015 -0700
@@ -49,10 +49,16 @@
 
 // ----------------------------------------------------------------------------------------
 
-void imu_init();
-void imu_zero_gyros();
-void imu_set_yaw_adjustment( float adj );
-void imu_process_pending();
+void     imu_init();
+void     imu_zero_gyros();
+void     imu_set_yaw_adjustment( float adj );
+bool     imu_process_pending();
+bool     imu_did_receive_imu_angles();
+bool     imu_did_receive_imu_health();
+float    imu_angle_roll();
+float    imu_angle_pitch();
+float    imu_angle_yaw();
+uint32_t imu_health_info();
 
 // ----------------------------------------------------------------------------------------
 
--- a/main/robots/odr-imu/imu.cpp	Sun Sep 06 10:53:50 2015 -0700
+++ b/main/robots/odr-imu/imu.cpp	Mon Sep 07 14:49:45 2015 -0700
@@ -43,14 +43,14 @@
 
 static float* eeprom_yaw_adjustment_address = reinterpret_cast< float* >( 0x04 );
 
-float g_yaw_adjustment = 0.0;
+static float g_yaw_adjustment = 0.0;
 
 // ----------------------------------------------------------------------------------------
 
 void imu_init()
 {
     uart0_init();
-    uart0_set_baudrate( 115200 );
+    uart0_set_baudrate( 57600 );
 
     g_yaw_adjustment = eeprom_read_float( eeprom_yaw_adjustment_address );
 }
@@ -73,39 +73,6 @@
 }
 
 // ----------------------------------------------------------------------------------------
-
-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()
@@ -129,22 +96,81 @@
 
 // ----------------------------------------------------------------------------------------
 
-void imu_process_pending()
+bool imu_process_pending()
 {
     if ( ! recv_imu_packet() )
     {
-        return;
+        return false;
     }
 
     if ( um7_packet_address() == um7_addr_dreg_euler_phi_theta )
     {
-        recv_euler_phi_theta();
+        return true;
     }
     else if ( um7_packet_address() == um7_addr_dreg_health )
     {
-        recv_health();
+        return true;
     }
+
+    return false; // message, but not one we care about
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool imu_did_receive_imu_angles()
+{
+    return ( um7_packet_address() == um7_addr_dreg_euler_phi_theta );
 }
 
 // ----------------------------------------------------------------------------------------
 
+bool imu_did_receive_imu_health()
+{
+    return ( um7_packet_address() == um7_addr_dreg_health );
+}
+
+// ----------------------------------------------------------------------------------------
+//  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 imu_angle_roll()
+{
+    float roll = um7_packet_value_s16( 2 );
+    roll /= 91.02222;
+    roll *= -1.0;
+
+    return roll;
+}
+
+float imu_angle_pitch()
+{
+    float pitch = um7_packet_value_s16( 0 );
+    pitch /= 91.02222;
+    pitch *= -1.0;
+    
+    return pitch;
+}
+
+float imu_angle_yaw()
+{
+    float yaw = um7_packet_value_s16( 4 );
+    yaw /= 91.02222;
+    yaw += g_yaw_adjustment; // magnetic declination correction
+    if ( yaw > 360.0 ) yaw -= 360.0;
+    if ( yaw < 0.0 ) yaw += 360.0;
+    yaw -= 180.0;
+
+    return yaw;
+}
+
+// ----------------------------------------------------------------------------------------
+
+uint32_t imu_health_info()
+{
+    return um7_packet_value_u32( 0 );
+}
+
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr-imu/main.cpp	Sun Sep 06 10:53:50 2015 -0700
+++ b/main/robots/odr-imu/main.cpp	Mon Sep 07 14:49:45 2015 -0700
@@ -29,7 +29,6 @@
 
 #include <avr/interrupt.h>
 #include <avr/io.h>
-#include <avr/sleep.h>
 
 #include "project_defs.h"
 
@@ -168,10 +167,6 @@
         odrimu_fatal_error( odrimu_fatal_error_can_init );
     }
 
-    //--    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();
@@ -250,14 +245,35 @@
 
         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();
-
         //--    Process data coming from the IMU module.
 
-        imu_process_pending();
+        if ( imu_process_pending() )
+        {
+            if ( imu_did_receive_imu_angles() )
+            {
+                if ( ! canmsg_send_imu_roll( imu_angle_roll() ) )
+                {
+                    ++can_comm_errors;
+                }
+
+                if ( ! canmsg_send_imu_pitch( imu_angle_pitch() ) )
+                {
+                    ++can_comm_errors;
+                }
+
+                if ( ! canmsg_send_imu_yaw( imu_angle_yaw() ) )
+                {
+                    ++can_comm_errors;
+                }
+            }
+            else if ( imu_did_receive_imu_health() )
+            {
+                if ( ! canmsg_send_imu_health( imu_health_info() ) )
+                {
+                    ++can_comm_errors;
+                }
+            }
+        }
         
         if ( status_is_imu_active() )
         {
--- a/main/robots/odr-imu/project_defs.h	Sun Sep 06 10:53:50 2015 -0700
+++ b/main/robots/odr-imu/project_defs.h	Mon Sep 07 14:49:45 2015 -0700
@@ -41,7 +41,6 @@
 //  packages/avr/device
 
 #define PRJ_UART0_USE_INTERRUPT_MODE
-#define PRJ_UART0_INITIAL_BAUCRATE    115200
 #define PRJ_UART0_BUFFER_SIZE         32
 #define PRJ_UART0_EXTENDED_READ_WRITE
 
--- a/main/robots/odr-imu/status.cpp	Sun Sep 06 10:53:50 2015 -0700
+++ b/main/robots/odr-imu/status.cpp	Mon Sep 07 14:49:45 2015 -0700
@@ -30,9 +30,6 @@
 // ----------------------------------------------------------------------------------------
 
 #include "func.h"
-#include "leds.h"
-
-#include <avr/io.h>
 
 #include "packages/common/can/can_nodes.h"