[paparazzi-commits] [4948] a little progress on beth

[antoine drouin]

Revision: 4948
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4948
Author:   poine
Date:     2010-06-14 16:52:14 +0000 (Mon, 14 Jun 2010)
Log Message:
-----------
a little progress on beth

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/beth/bench_sensors.c
    paparazzi3/trunk/sw/airborne/beth/bench_sensors.h
    paparazzi3/trunk/sw/airborne/beth/main_overo.c
    paparazzi3/trunk/sw/airborne/beth/main_stm32.c

Added Paths:
-----------
    paparazzi3/trunk/sw/airborne/beth/main_coders.c

Modified: paparazzi3/trunk/sw/airborne/beth/bench_sensors.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/bench_sensors.c   2010-06-14 16:50:56 UTC 
(rev 4947)
+++ paparazzi3/trunk/sw/airborne/beth/bench_sensors.c   2010-06-14 16:52:14 UTC 
(rev 4948)
@@ -1,22 +1,19 @@
 #include "bench_sensors.h"
 
-#include "i2c.h"
+struct BenchSensors bench_sensors;
 
-bool_t   bench_sensors_available;
-uint16_t bench_sensors_angle_1;
-uint16_t bench_sensors_angle_2;
-uint16_t bench_sensors_angle_3;
-uint16_t bench_sensors_current;
 
 void bench_sensors_init(void) {
-  bench_sensors_available = FALSE;
+  bench_sensors.status = BS_IDLE;
+  bench_sensors.i2c_done = TRUE;
 }
 
 
 void read_bench_sensors(void) {
 
-  const uint8_t bench_addr = 0x52;
-
-  i2c1_receive(bench_addr, 4, &bench_sensors_available);
+  const uint8_t bench_addr = 0x30;
+  bench_sensors.status = BS_BUSY;
+  bench_sensors.i2c_done = FALSE;
+  i2c2_receive(bench_addr, 4, &bench_sensors.i2c_done);
   
 }

Modified: paparazzi3/trunk/sw/airborne/beth/bench_sensors.h
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/bench_sensors.h   2010-06-14 16:50:56 UTC 
(rev 4947)
+++ paparazzi3/trunk/sw/airborne/beth/bench_sensors.h   2010-06-14 16:52:14 UTC 
(rev 4948)
@@ -2,24 +2,31 @@
 #define BENCH_SENSORS_H
 
 #include "std.h"
+#include "i2c.h"
 
 extern void bench_sensors_init(void);
 extern void read_bench_sensors(void);
-extern bool_t   bench_sensors_available;
-extern uint16_t bench_sensors_angle_1;
-extern uint16_t bench_sensors_angle_2;
-extern uint16_t bench_sensors_angle_3;
-extern uint16_t bench_sensors_current;
 
+enum BenchSensorsStatus { BS_IDLE, BS_BUSY, BS_AVAILABLE};
+
+struct BenchSensors {
+  enum BenchSensorsStatus status;
+  uint16_t angle_1;
+  uint16_t angle_2;
+  uint16_t angle_3;
+  uint16_t current;
+  bool_t   i2c_done;
+};
+
+extern struct BenchSensors bench_sensors;
+
 #define BenchSensorsEvent( _handler) {         \
-    if (bench_sensors_available) {             \
-      bench_sensors_angle_1 = i2c1_buf[0];     \
-      bench_sensors_angle_2 = i2c1_buf[1];     \
-      bench_sensors_angle_3 = i2c1_buf[2];     \
-      bench_sensors_current = i2c1_buf[3];     \
-      _handler();                              \
-      bench_sensors_available = FALSE;         \
-    }                                          \
+    if (bench_sensors.status ==  BS_BUSY && bench_sensors.i2c_done) {  \
+      bench_sensors.angle_1 = i2c2.buf[0] + (i2c2.buf[1] << 8);                
\
+      bench_sensors.angle_2 = i2c2.buf[2] + (i2c2.buf[3] << 8);                
\
+      bench_sensors.status = IDLE;                                     \
+      _handler();                                                      \
+    }                                                                  \
   }
 
 

Added: paparazzi3/trunk/sw/airborne/beth/main_coders.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_coders.c                             
(rev 0)
+++ paparazzi3/trunk/sw/airborne/beth/main_coders.c     2010-06-14 16:52:14 UTC 
(rev 4948)
@@ -0,0 +1,283 @@
+#include BOARD_CONFIG
+#include "init_hw.h"
+#include "sys_time.h"
+#include "downlink.h"
+
+#include <stm32/rcc.h>
+#include <stm32/gpio.h>
+#include <stm32/flash.h>
+#include <stm32/misc.h>
+#include <stm32/dma.h>
+#include <stm32/adc.h>
+#include <stm32/i2c.h>
+#include "interrupt_hw.h"
+
+#include <string.h>
+
+/*
+ *
+ *  PC.01 (ADC Channel11) ext1-20 coder_values[1]
+ *  PC.04 (ADC Channel14) ext2-12 coder_values[0]
+ *
+ *  PB.10 I2C2 SCL        ext2-14
+ *  PB.11 I2C2 SDA        ext2-15
+ *
+ */
+
+
+static inline void main_init( void );
+static inline void main_periodic( void );
+static inline void main_event( void );
+
+static inline void main_init_adc(void);
+//static inline void main_init_i2c2(void);
+//void i2c2_ev_irq_handler(void);
+//void i2c2_er_irq_handler(void);
+
+#define ADC1_DR_Address    ((uint32_t)0x4001244C)
+static uint16_t coder_values[3];
+
+#define I2C2_SLAVE_ADDRESS7     0x30
+#define I2C2_ClockSpeed       200000
+
+#define MY_I2C2_BUF_LEN 4
+static uint8_t i2c2_idx;
+static uint8_t i2c2_buf[MY_I2C2_BUF_LEN];
+
+int main(void) {
+
+  main_init();
+
+  while (1) {
+    if (sys_time_periodic())
+      main_periodic();
+    main_event();
+  }
+  return 0;
+}
+
+
+static inline void main_init( void ) {
+  hw_init();
+  sys_time_init();
+  main_init_adc();
+  //  main_init_i2c2();
+  int_enable();
+ 
+}
+
+
+static inline void main_periodic( void ) {
+
+  RunOnceEvery(10, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});
+  
+  RunOnceEvery(5, {DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, &coder_values[0], 
&coder_values[1]);});
+
+
+}
+
+
+static inline void main_event( void ) {
+
+}
+
+/*
+ *
+ *  I2C2 : autopilot link
+ *
+ */
+void i2c2_init(void) {
+  //static inline void main_init_i2c2(void) {
+
+  /* System clocks configuration 
---------------------------------------------*/
+  /* Enable I2C2 clock */
+  RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
+  /* Enable GPIOB clock */
+  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
+  
+  /* NVIC configuration 
------------------------------------------------------*/
+  NVIC_InitTypeDef  NVIC_InitStructure;
+  NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);
+  /* Configure and enable I2C2 event interrupt 
-------------------------------*/
+  NVIC_InitStructure.NVIC_IRQChannel = I2C2_EV_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
+  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+  NVIC_Init(&NVIC_InitStructure);
+  /* Configure and enable I2C2 error interrupt 
-------------------------------*/  
+  NVIC_InitStructure.NVIC_IRQChannel = I2C2_ER_IRQn;
+  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
+  NVIC_Init(&NVIC_InitStructure);
+
+  
+  /* GPIO configuration 
------------------------------------------------------*/
+  GPIO_InitTypeDef GPIO_InitStructure;
+  /* Configure I2C2 pins: SCL and SDA 
----------------------------------------*/
+  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+ 
+  /* Enable I2C2 
-------------------------------------------------------------*/
+  I2C_Cmd(I2C2, ENABLE);
+  /* I2C2 configuration 
------------------------------------------------------*/
+  I2C_InitTypeDef   I2C_InitStructure;
+  I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
+  I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
+  I2C_InitStructure.I2C_OwnAddress1 = I2C2_SLAVE_ADDRESS7;
+  I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
+  I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
+  I2C_InitStructure.I2C_ClockSpeed = I2C2_ClockSpeed;
+  I2C_Init(I2C2, &I2C_InitStructure);
+
+  /* Enable I2C1 event and buffer interrupts */
+  //  I2C_ITConfig(I2C2, I2C_IT_EVT | I2C_IT_BUF, ENABLE);
+  I2C_ITConfig(I2C2, I2C_IT_EVT | I2C_IT_ERR, ENABLE);
+
+}
+
+
+void i2c2_ev_irq_handler(void) {
+  switch (I2C_GetLastEvent(I2C2))
+  {
+    /* Slave Transmitter ---------------------------------------------------*/
+  case I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED:  /* EV1 */
+    memcpy(i2c2_buf, coder_values, MY_I2C2_BUF_LEN); 
+    i2c2_idx = 0;
+    
+  case I2C_EVENT_SLAVE_BYTE_TRANSMITTED:             /* EV3 */
+    /* Transmit I2C2 data */
+    if (i2c2_idx < MY_I2C2_BUF_LEN) {
+      I2C_SendData(I2C2, i2c2_buf[i2c2_idx]);
+      i2c2_idx++;
+    }
+    break;
+    
+
+  case I2C_EVENT_SLAVE_STOP_DETECTED:                /* EV4 */
+    LED_ON(1);
+    /* Clear I2C2 STOPF flag: read of I2C_SR1 followed by a write on I2C_CR1 */
+    (void)(I2C_GetITStatus(I2C2, I2C_IT_STOPF));
+    I2C_Cmd(I2C2, ENABLE);
+    break;
+
+  }
+}
+
+
+void i2c2_er_irq_handler(void) {
+  /* Check on I2C2 AF flag and clear it */
+  if (I2C_GetITStatus(I2C2, I2C_IT_AF))  {
+    I2C_ClearITPendingBit(I2C2, I2C_IT_AF);
+  }
+}
+
+
+
+
+/*
+ *
+ *  ADC : coders
+ *
+ */
+
+
+
+
+static inline void main_init_adc(void) {
+
+  /* Enable DMA1 clock */
+  RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
+  
+  /* Enable ADC1 and GPIOC clock */
+  RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2 | 
+                        RCC_APB2Periph_GPIOC, ENABLE);
+  
+  /* Configure PC.01 (ADC Channel11) and PC.04 (ADC Channel14) as analog 
input-*/
+  GPIO_InitTypeDef GPIO_InitStructure;
+  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
+  GPIO_Init(GPIOC, &GPIO_InitStructure);
+  
+ /* DMA1 channel1 configuration 
----------------------------------------------*/
+  DMA_InitTypeDef DMA_InitStructure;
+  DMA_DeInit(DMA1_Channel1);
+  DMA_InitStructure.DMA_PeripheralBaseAddr = ADC1_DR_Address;
+  DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&coder_values;
+  DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
+  DMA_InitStructure.DMA_BufferSize = 1;
+  DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+  DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Disable;
+  DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;
+  DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word;
+  DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
+  DMA_InitStructure.DMA_Priority = DMA_Priority_High;
+  DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
+  DMA_Init(DMA1_Channel1, &DMA_InitStructure);
+  
+  /* Enable DMA1 channel1 */
+  DMA_Cmd(DMA1_Channel1, ENABLE);
+     
+  /* ADC1 configuration 
------------------------------------------------------*/
+  ADC_InitTypeDef ADC_InitStructure;
+  ADC_InitStructure.ADC_Mode = ADC_Mode_RegSimult;
+  ADC_InitStructure.ADC_ScanConvMode = ENABLE;
+  ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;
+  ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
+  ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
+  ADC_InitStructure.ADC_NbrOfChannel = 1;
+  ADC_Init(ADC1, &ADC_InitStructure);
+
+  /* ADC1 regular channel14 configuration */ 
+  ADC_RegularChannelConfig(ADC1, ADC_Channel_14, 1, ADC_SampleTime_239Cycles5);
+
+  /* Enable ADC1 DMA */
+  ADC_DMACmd(ADC1, ENABLE);
+  
+  
+  /* ADC2 configuration 
------------------------------------------------------*/
+  ADC_InitStructure.ADC_Mode = ADC_Mode_RegSimult;
+  ADC_InitStructure.ADC_ScanConvMode = ENABLE;
+  ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;
+  ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
+  ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
+  ADC_InitStructure.ADC_NbrOfChannel = 1;
+  ADC_Init(ADC2, &ADC_InitStructure);
+  /* ADC2 regular channels configuration */ 
+  ADC_RegularChannelConfig(ADC2, ADC_Channel_11, 1, ADC_SampleTime_239Cycles5);
+  /* Enable ADC2 external trigger conversion */
+  ADC_ExternalTrigConvCmd(ADC2, ENABLE);
+
+ 
+  /* Enable ADC1 */
+  ADC_Cmd(ADC1, ENABLE);
+
+  /* Enable ADC1 reset calibaration register */   
+  ADC_ResetCalibration(ADC1);
+  /* Check the end of ADC1 reset calibration register */
+  while(ADC_GetResetCalibrationStatus(ADC1));
+
+  /* Start ADC1 calibaration */
+  ADC_StartCalibration(ADC1);
+  /* Check the end of ADC1 calibration */
+  while(ADC_GetCalibrationStatus(ADC1));
+
+  /* Enable ADC2 */
+  ADC_Cmd(ADC2, ENABLE);
+
+  /* Enable ADC2 reset calibaration register */   
+  ADC_ResetCalibration(ADC2);
+  /* Check the end of ADC2 reset calibration register */
+  while(ADC_GetResetCalibrationStatus(ADC2));
+
+  /* Start ADC2 calibaration */
+  ADC_StartCalibration(ADC2);
+  /* Check the end of ADC2 calibration */
+  while(ADC_GetCalibrationStatus(ADC2));
+
+
+  /* Start ADC1 Software Conversion */ 
+  ADC_SoftwareStartConvCmd(ADC1, ENABLE); 
+
+}
+

Modified: paparazzi3/trunk/sw/airborne/beth/main_overo.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-06-14 16:50:56 UTC 
(rev 4947)
+++ paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-06-14 16:52:14 UTC 
(rev 4948)
@@ -59,12 +59,12 @@
   static uint32_t foo = 0;
 
   spi_link_send(msg_out, sizeof(union AutopilotMessageBeth), msg_in);
-  if (!foo%100) {
-    printf("%d -> %d %d %d %d %d %d %d %d %d\n", foo, 
-          msg_in->bench_sensor.x, msg_in->bench_sensor.y, 
msg_in->bench_sensor.z,
-          msg_in->gyro.x, msg_in->gyro.y, msg_in->gyro.z,
-          msg_in->accel.x, msg_in->accel.y, msg_in->accel.z);
-  }
+  //  if (!foo%100) {
+  printf("%d -> %d %d %d %d %d %d %d %d %d\n", foo, 
+        msg_in->bench_sensor.x, msg_in->bench_sensor.y, msg_in->bench_sensor.z,
+        msg_in->gyro.x, msg_in->gyro.y, msg_in->gyro.z,
+        msg_in->accel.x, msg_in->accel.y, msg_in->accel.z);
+  //  }
   foo++;
 }
 

Modified: paparazzi3/trunk/sw/airborne/beth/main_stm32.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_stm32.c      2010-06-14 16:50:56 UTC 
(rev 4947)
+++ paparazzi3/trunk/sw/airborne/beth/main_stm32.c      2010-06-14 16:52:14 UTC 
(rev 4948)
@@ -21,20 +21,18 @@
  * Boston, MA 02111-1307, USA. 
  */
 
-#include <stm32/rcc.h>
-#include <stm32/gpio.h>
 
-#include <stm32/flash.h>
-#include <stm32/misc.h>
-
 #include BOARD_CONFIG
 #include "init_hw.h"
 #include "sys_time.h"
 #include "downlink.h"
+#include "booz/booz2_commands.h"
+#include "booz/booz_actuators.h"
+//#include "booz/booz_radio_control.h"
 #include "booz/booz_imu.h"
 #include "lisa/lisa_overo_link.h"
+#include "beth/bench_sensors.h"
 
-
 static inline void main_init( void );
 static inline void main_periodic( void );
 static inline void main_event( void );
@@ -44,6 +42,7 @@
 
 static inline void main_on_overo_msg_received(void);
 static inline void main_on_overo_link_lost(void);
+static inline void main_on_bench_sensors( void );
 
 static int16_t my_cnt;
 
@@ -62,20 +61,29 @@
 static inline void main_init( void ) {
   hw_init();
   sys_time_init();
-  booz_imu_init();
-  overo_link_init();
+  actuators_init();
+  //  radio_control_init();
+  //  booz_imu_init();
+  //  overo_link_init();
+  bench_sensors_init();
 }
 
 static inline void main_periodic( void ) {
-  booz_imu_periodic();
-  OveroLinkPperiodic(main_on_overo_link_lost)
+  //  booz_imu_periodic();
+  actuators_set(FALSE);
+  //  OveroLinkPeriodic(main_on_overo_link_lost)
   RunOnceEvery(10, {LED_PERIODIC(); DOWNLINK_SEND_ALIVE(DefaultChannel, 16, 
MD5SUM);});
-  
+
+  read_bench_sensors();
+ 
 }
 
 static inline void main_event( void ) {
-    BoozImuEvent(on_gyro_accel_event, on_mag_event);
-    OveroLinkEvent(main_on_overo_msg_received);
+  //    BoozImuEvent(on_gyro_accel_event, on_mag_event);
+  //    OveroLinkEvent(main_on_overo_msg_received);
+
+  BenchSensorsEvent(main_on_bench_sensors);
+
 }
 
 static inline void main_on_overo_msg_received(void) {
@@ -151,3 +159,12 @@
                              &booz_imu.mag_unscaled.z);
   }
 }
+
+
+static inline void main_on_bench_sensors( void ) {
+  
+  DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, 
+                           &bench_sensors.angle_1,
+                            &bench_sensors.angle_2);
+  
+}