[paparazzi-commits] [6117] Convert/fix booz_hmc5843 to use drdy interrupt, put arch specific in stm32 arch file, works for lisa_l_test_progs.makefile

[Allen Ibara]

Revision: 6117
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6117
Author:   aibara
Date:     2010-10-08 22:18:44 +0000 (Fri, 08 Oct 2010)
Log Message:
-----------
Convert/fix booz_hmc5843 to use drdy interrupt, put arch specific in stm32 arch 
file, works for lisa_l_test_progs.makefile

Modified Paths:
--------------
    paparazzi3/trunk/conf/autopilot/lisa_l_test_progs.makefile
    paparazzi3/trunk/sw/airborne/booz/peripherals/booz_hmc5843.c
    paparazzi3/trunk/sw/airborne/booz/peripherals/booz_hmc5843.h
    paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/imu_b2.h

Added Paths:
-----------
    paparazzi3/trunk/sw/airborne/booz/arch/stm32/peripherals/hmc5843_arch.c
    paparazzi3/trunk/sw/airborne/booz/arch/stm32/peripherals/hmc5843_arch.h

Modified: paparazzi3/trunk/conf/autopilot/lisa_l_test_progs.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/lisa_l_test_progs.makefile  2010-10-08 
22:11:33 UTC (rev 6116)
+++ paparazzi3/trunk/conf/autopilot/lisa_l_test_progs.makefile  2010-10-08 
22:18:44 UTC (rev 6117)
@@ -362,10 +362,10 @@
 test_imu_b2_2.srcs += $(SRC_BOOZ)/peripherals/booz_max1168.c 
$(SRC_BOOZ_ARCH)/peripherals/booz_max1168_arch.c
 test_imu_b2_2.CFLAGS += -DUSE_I2C2
 test_imu_b2_2.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-test_imu_b2_2.srcs += $(SRC_BOOZ)/peripherals/booz_hmc5843.c
+test_imu_b2_2.srcs += $(SRC_BOOZ)/peripherals/booz_hmc5843.c 
$(SRC_BOOZ_ARCH)/peripherals/hmc5843_arch.c
+test_imu_b2_2.CFLAGS += -DUSE_EXTI9_5_IRQ    # Mag Int on PB5
 
 
-
 #
 # test IMU aspirin
 #

Added: paparazzi3/trunk/sw/airborne/booz/arch/stm32/peripherals/hmc5843_arch.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/arch/stm32/peripherals/hmc5843_arch.c     
                        (rev 0)
+++ paparazzi3/trunk/sw/airborne/booz/arch/stm32/peripherals/hmc5843_arch.c     
2010-10-08 22:18:44 UTC (rev 6117)
@@ -0,0 +1,63 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2010 Antoine Drouin <address@hidden>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA. 
+ */
+#include "peripherals/booz_hmc5843.h"
+
+#include <stm32/gpio.h>
+#include <stm32/rcc.h>
+#include <stm32/spi.h>
+#include <stm32/exti.h>
+#include <stm32/misc.h>
+
+void hmc5843_arch_init( void ) {
+  /* configure external interrupt exti5 on PB5( mag int ) */
+       GPIO_InitTypeDef GPIO_InitStructure;
+  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO, ENABLE);
+  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+  GPIO_EXTILineConfig(GPIO_PortSourceGPIOB, GPIO_PinSource5);
+  EXTI_InitTypeDef EXTI_InitStructure;
+  EXTI_InitStructure.EXTI_Line = EXTI_Line5;
+  EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
+  EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
+  EXTI_InitStructure.EXTI_LineCmd = ENABLE;
+  EXTI_Init(&EXTI_InitStructure);
+
+  NVIC_InitTypeDef NVIC_InitStructure;
+  NVIC_InitStructure.NVIC_IRQChannel = EXTI9_5_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+  NVIC_Init(&NVIC_InitStructure); 
+}
+
+void exti9_5_irq_handler(void) {
+  
+  /* clear EXTI */
+  if(EXTI_GetITStatus(EXTI_Line5) != RESET)
+    EXTI_ClearITPendingBit(EXTI_Line5);
+
+       hmc5843.ready_for_read = TRUE;
+}

Added: paparazzi3/trunk/sw/airborne/booz/arch/stm32/peripherals/hmc5843_arch.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/arch/stm32/peripherals/hmc5843_arch.h     
                        (rev 0)
+++ paparazzi3/trunk/sw/airborne/booz/arch/stm32/peripherals/hmc5843_arch.h     
2010-10-08 22:18:44 UTC (rev 6117)
@@ -0,0 +1,27 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2010 Antoine Drouin <address@hidden>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA. 
+ */
+
+#ifndef HMC5843_ARCH_H
+#define HMC5843_ARCH_H
+
+#endif /* HMC5843_ARCH_H */

Modified: paparazzi3/trunk/sw/airborne/booz/peripherals/booz_hmc5843.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/peripherals/booz_hmc5843.c        
2010-10-08 22:11:33 UTC (rev 6116)
+++ paparazzi3/trunk/sw/airborne/booz/peripherals/booz_hmc5843.c        
2010-10-08 22:18:44 UTC (rev 6117)
@@ -1,58 +1,69 @@
 #include "peripherals/booz_hmc5843.h"
 
-#include "i2c.h"
+#define bswap_16(x)   ((((x) & 0xFF00) >> 8) | (((x) & 0x00FF) << 8))
 
 struct Hmc5843 hmc5843;
-struct i2c_transaction hmc5843_i2c_trans;
+void exti9_5_irq_handler(void);
 
-void hmc5843_init(void) {
-  hmc5843.status = HMC5843_UNINITIALIZED1;
-  hmc5843.i2c_done = TRUE;
+void hmc5843_init(void)
+{
+       hmc5843.i2c_trans.status = I2CTransSuccess;
+       hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
+       hmc5843.i2c_trans.stop_after_transmit = TRUE;
 
-       hmc5843_i2c_trans.status = I2CTransSuccess;
-       hmc5843_i2c_trans.slave_addr = HMC5843_ADDR;
-       hmc5843_i2c_trans.stop_after_transmit = TRUE;
+       hmc5843_arch_init();
 }
 
-void hmc5843_periodic(void) {
-  
-  if (hmc5843_i2c_trans.status == I2CTransPending) return;
+// blocking, only intended to be called for initialization
+static void send_config(void)
+{
+  hmc5843.i2c_trans.type = I2CTransTx;
+  hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGA;  // set to rate to 50Hz
+  hmc5843.i2c_trans.buf[1] = 0x00 | (0x06 << 2);
+  hmc5843.i2c_trans.len_w = 2;
+  i2c_submit(&i2c2,&hmc5843.i2c_trans);
+       while(hmc5843.i2c_trans.status == I2CTransPending);
+    
+  hmc5843.i2c_trans.type = I2CTransTx;
+  hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGB;  // set to gain to 1 Gauss
+  hmc5843.i2c_trans.buf[1] = 0x01<<5;
+  hmc5843.i2c_trans.len_w = 2;
+  i2c_submit(&i2c2,&hmc5843.i2c_trans);
+       while(hmc5843.i2c_trans.status == I2CTransPending);
 
-  switch (hmc5843.status) {
-  case HMC5843_UNINITIALIZED1:
-    hmc5843_i2c_trans.buf[0] = HMC5843_REG_CFGA;  // set to rate to 50Hz
-    hmc5843_i2c_trans.buf[1] = 0x00 | (0x06 << 2);
-    hmc5843_i2c_trans.type = I2CTransTx;
-    hmc5843_i2c_trans.len_w = 2;
-    i2c_submit(&i2c2, &hmc5843_i2c_trans);
-    hmc5843.status = HMC5843_UNINITIALIZED2;
-    break;
-  case HMC5843_UNINITIALIZED2:
-    hmc5843_i2c_trans.buf[0] = HMC5843_REG_CFGB;  // set to gain to 1 Gauss
-    hmc5843_i2c_trans.buf[1] = 0x01<<5;
-    hmc5843_i2c_trans.type = I2CTransTx;
-    hmc5843_i2c_trans.len_w = 2;
-    i2c_submit(&i2c2, &hmc5843_i2c_trans);
-    hmc5843.status = HMC5843_UNINITIALIZED3;
-    break;
-  case HMC5843_UNINITIALIZED3:
-    hmc5843_i2c_trans.buf[0] = HMC5843_REG_MODE;  // set to continuous mode
-    hmc5843_i2c_trans.buf[1] = 0x00;
-    hmc5843_i2c_trans.type = I2CTransTx;
-    hmc5843_i2c_trans.len_w = 2;
-    i2c_submit(&i2c2, &hmc5843_i2c_trans);
-    hmc5843.status = HMC5843_IDLE;
-    break;
-  case HMC5843_IDLE:
-    hmc5843_i2c_trans.type = I2CTransRx;
-    hmc5843_i2c_trans.len_r = 7;
-    i2c_submit(&i2c2, &hmc5843_i2c_trans);
-    hmc5843.status = HMC5843_READING;
-    break;
-  default:
-    /* FIXME : report error */
-    break;
-  }
+  hmc5843.i2c_trans.type = I2CTransTx;
+  hmc5843.i2c_trans.buf[0] = HMC5843_REG_MODE;  // set to continuous mode
+  hmc5843.i2c_trans.buf[1] = 0x00;
+  hmc5843.i2c_trans.len_w = 2;
+  i2c_submit(&i2c2,&hmc5843.i2c_trans);
+       while(hmc5843.i2c_trans.status == I2CTransPending);
 
 }
 
+void hmc5843_idle_task(void)
+{
+       if (hmc5843.initialized && hmc5843.ready_for_read && 
(hmc5843.i2c_trans.status == I2CTransSuccess || hmc5843.i2c_trans.status == 
I2CTransFailed)) {
+               hmc5843.i2c_trans.type = I2CTransRx;
+               hmc5843.i2c_trans.len_r = 7;
+               i2c_submit(&i2c2, &hmc5843.i2c_trans);
+               hmc5843.reading = TRUE;
+               hmc5843.ready_for_read = FALSE;
+       }
+
+       if (hmc5843.reading && hmc5843.i2c_trans.status == I2CTransSuccess) {
+               hmc5843.data_available = TRUE;
+               hmc5843.reading = FALSE;
+               memcpy(hmc5843.data.buf, (const void *) hmc5843.i2c_trans.buf, 
6);
+               for (int i = 0; i < 3; i++) {
+                       hmc5843.data.value[i] = bswap_16(hmc5843.data.value[i]);
+               }
+       }
+}
+
+void hmc5843_periodic(void)
+{
+       if (!hmc5843.initialized) {
+               send_config();
+               hmc5843.initialized = TRUE;
+       }
+}

Modified: paparazzi3/trunk/sw/airborne/booz/peripherals/booz_hmc5843.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/peripherals/booz_hmc5843.h        
2010-10-08 22:11:33 UTC (rev 6116)
+++ paparazzi3/trunk/sw/airborne/booz/peripherals/booz_hmc5843.h        
2010-10-08 22:18:44 UTC (rev 6117)
@@ -25,30 +25,29 @@
 #define BOOZ_HMC5843_H
 
 #include "std.h"
+#include "i2c.h"
 
-enum Hmc5843Status {
-  HMC5843_UNINITIALIZED1,
-  HMC5843_UNINITIALIZED2,
-  HMC5843_UNINITIALIZED3,
-  HMC5843_IDLE,
-  HMC5843_DATA_AVAILABLE,
-  HMC5843_READING
-};
+#include "peripherals/hmc5843_arch.h"
 
 struct Hmc5843 {
-  volatile enum Hmc5843Status status;
-  volatile uint8_t i2c_done;
+       struct i2c_transaction i2c_trans;
   union {
     uint8_t buf[7];
     int16_t value[3];
   } data;
+       uint8_t initialized;
+       uint8_t reading;
+       uint8_t ready_for_read;
+       uint8_t data_available;
 };
 
 extern struct Hmc5843 hmc5843;
-extern struct i2c_transaction hmc5843_i2c_trans;
 
+extern void hmc5843_arch_init( void );
+
 extern void hmc5843_init(void);
 extern void hmc5843_periodic(void);
+extern void hmc5843_idle_task(void);
 
 /* default I2C address */
 #define HMC5843_ADDR 0x3C
@@ -71,12 +70,9 @@
 #include <string.h>
 
 #define MagEvent(_m_handler) {                                         \
-    if (hmc5843.status == HMC5843_READING && hmc5843_i2c_trans.status == 
I2CTransSuccess) {    \
-      memcpy(hmc5843.data.buf, (const void*)hmc5843_i2c_trans.buf, 6);         
\
-      hmc5843.status = HMC5843_DATA_AVAILABLE;                                 
\
-      _m_handler();                                                    \
-    }  else if (hmc5843_i2c_trans.status != I2CTransPending) { \
-      hmc5843.status = HMC5843_IDLE;                                   \
+         hmc5843_idle_task(); \
+    if (hmc5843.data_available) { \
+                       _m_handler(); \
     } \
   }
 

Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/imu_b2.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/imu_b2.h      
2010-10-08 22:11:33 UTC (rev 6116)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/imu_b2.h      
2010-10-08 22:18:44 UTC (rev 6117)
@@ -171,12 +171,12 @@
 #define foo_handler() {}
 #define ImuMagEvent(_mag_handler) {                                    \
          MagEvent(foo_handler); \
-    if (hmc5843.status == HMC5843_DATA_AVAILABLE) {                    \
+    if (hmc5843.data_available) {                      \
       imu.mag_unscaled.x = hmc5843.data.value[IMU_MAG_X_CHAN];         \
       imu.mag_unscaled.y = hmc5843.data.value[IMU_MAG_Y_CHAN];         \
       imu.mag_unscaled.z = hmc5843.data.value[IMU_MAG_Z_CHAN];         \
       _mag_handler();                                                  \
-      hmc5843.status == HMC5843_IDLE;          \
+      hmc5843.data_available = FALSE;          \
     }                                                                  \
   }
 #else