[paparazzi-commits] [5119] changes to allow bench sensors to use i2c or CAN.

[Paul Cox]

Revision: 5119
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5119
Author:   paulcox
Date:     2010-07-22 12:38:40 +0000 (Thu, 22 Jul 2010)
Log Message:
-----------
changes to allow bench sensors to use i2c or CAN. tested with CAN only.

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

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

Modified: paparazzi3/trunk/sw/airborne/beth/bench_sensors.h
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/bench_sensors.h   2010-07-22 12:35:37 UTC 
(rev 5118)
+++ paparazzi3/trunk/sw/airborne/beth/bench_sensors.h   2010-07-22 12:38:40 UTC 
(rev 5119)
@@ -2,8 +2,21 @@
 #define BENCH_SENSORS_H
 
 #include "std.h"
+
+#ifdef USE_I2C2
 #include "i2c.h"
+#endif
 
+#ifdef USE_CAN1
+#include "can.h"
+#include "can_hw.h"
+#include <stm32/can.h>
+
+extern volatile uint8_t CAN_RX_FLAG;
+extern CanRxMsg can_rx_msg;
+extern uint16_t halfw1,halfw2,halfw3,halfw4,tempid;
+#endif
+
 extern void bench_sensors_init(void);
 extern void read_bench_sensors(void);
 
@@ -15,20 +28,48 @@
   uint16_t angle_2;
   uint16_t angle_3;
   uint16_t current;
-  bool_t   i2c_done;
+  bool_t   ready;
 };
 
+
 extern struct BenchSensors bench_sensors;
 
+#ifdef USE_I2C2
 #define BenchSensorsEvent( _handler) {         \
-    if (bench_sensors.status ==  BS_BUSY && bench_sensors.i2c_done) {  \
+    if (bench_sensors.status ==  BS_BUSY && bench_sensors.ready) {     \
       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 = BS_IDLE;                                  \
       _handler();                                                      \
     }                                                                  \
   }
+#endif
 
 
+#ifdef USE_CAN1
+#define BenchSensorsEvent( _handler) {         \
+    if (CAN_RX_FLAG == 1) {    \
+       tempid = (uint16_t)(can_rx_msg.ExtId>>7); \
+       if (tempid == 2) { \
+               bench_sensors.current = 2;\
+               halfw2 = can_rx_msg.Data[3]; \
+               halfw2 = (halfw2<<8) + can_rx_msg.Data[2]; \
+               halfw1 = can_rx_msg.Data[1]; \
+               halfw1 = (halfw1<<8) + can_rx_msg.Data[0]; \
+               bench_sensors.angle_2 = halfw1;         \
+               bench_sensors.angle_3 = halfw2;         \
+       } else { \
+               bench_sensors.current = 1;\
+               halfw4 = can_rx_msg.Data[3]; \
+               halfw4 = (halfw4<<8) + can_rx_msg.Data[2]; \
+               halfw3 = can_rx_msg.Data[1]; \
+               halfw3 = (halfw3<<8) + can_rx_msg.Data[0]; \
+               bench_sensors.angle_1 = halfw4;         \
+       } \
+      CAN_RX_FLAG = 0;                                 \
+      _handler();                                                      \
+    }                                                                  \
+  }
+#endif
 
 #endif /* BENCH_SENSORS_H  */

Added: paparazzi3/trunk/sw/airborne/beth/bench_sensors_can.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/bench_sensors_can.c                       
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/beth/bench_sensors_can.c       2010-07-22 
12:38:40 UTC (rev 5119)
@@ -0,0 +1,15 @@
+#include "bench_sensors.h"
+#include "can.h"
+
+uint16_t halfw1,halfw2,halfw3,halfw4,tempid;
+
+struct BenchSensors bench_sensors;
+
+void bench_sensors_init(void) {
+  can_init();
+}
+
+void read_bench_sensors(void) {
+  
+}
+

Added: paparazzi3/trunk/sw/airborne/beth/bench_sensors_i2c.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/bench_sensors_i2c.c                       
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/beth/bench_sensors_i2c.c       2010-07-22 
12:38:40 UTC (rev 5119)
@@ -0,0 +1,34 @@
+#include "bench_sensors.h"
+
+struct BenchSensors bench_sensors,bench_sensors2;
+
+
+void bench_sensors_init(void) {
+  bench_sensors.status = BS_IDLE;
+  bench_sensors.i2c_done = TRUE;
+}
+
+void bench_sensors2_init(void) {
+  bench_sensors2.status = BS_IDLE;
+  bench_sensors2.i2c_done = TRUE;
+}
+
+
+void read_bench_sensors(void) {
+
+  const uint8_t bench_addr = 0x40;
+  bench_sensors.status = BS_BUSY;
+  bench_sensors.i2c_done = FALSE;
+  i2c2_receive(bench_addr, 4, &bench_sensors.i2c_done);
+  
+}
+
+
+void read_bench_sensors2(void) {
+
+  const uint8_t bench_addr2 = 0x30;
+  bench_sensors2.status = BS_BUSY;
+  bench_sensors2.i2c_done = FALSE;
+  i2c2_receive(bench_addr2, 4, &bench_sensors2.i2c_done);
+  
+}

Modified: paparazzi3/trunk/sw/airborne/beth/main_coders.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_coders.c     2010-07-22 12:35:37 UTC 
(rev 5118)
+++ paparazzi3/trunk/sw/airborne/beth/main_coders.c     2010-07-22 12:38:40 UTC 
(rev 5119)
@@ -37,7 +37,7 @@
 //void i2c2_er_irq_handler(void);
 
 #define ADC1_DR_Address    ((uint32_t)0x4001244C)
-static uint16_t coder_values[2];
+static uint16_t coder_values[3];
 
 //azimuth potentiometer board address is 0x30
 //#define I2C2_SLAVE_ADDRESS7     0x30
@@ -49,10 +49,15 @@
 static uint8_t i2c2_idx;
 static uint8_t i2c2_buf[MY_I2C2_BUF_LEN];
 
+uint16_t servos[4];
 
 int main(void) {
   main_init();
 
+  servos[0] = 1;
+  servos[1] = 2;
+  servos[2] = 3;
+  servos[3] = 4;
 
   while (1) {
     if (sys_time_periodic())
@@ -81,9 +86,9 @@
   //RunOnceEvery(5, {DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, 
&coder_values[0], &coder_values[1]);});
   //RunOnceEvery(5, {DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, &can1_status, 
&can1_pending);});
   
-  //ID=2 for tilt/elevation  board, ID=1 for azimuth board
-  //the CAN receiver (lisa) determines which board sent it values based on 
this ID
-  can_transmit(2, 0, (uint8_t *)coder_values, 8);
+  servos[0]=coder_values[0];
+  servos[1]=coder_values[1];
+  can_transmit(2, 0, (uint8_t *)servos, 8);
 
 }
 
@@ -92,7 +97,62 @@
 
 }
 
+/*
+ *
+ *  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))
   {
@@ -121,6 +181,16 @@
 }
 
 
+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

Modified: paparazzi3/trunk/sw/airborne/beth/main_overo.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-07-22 12:35:37 UTC 
(rev 5118)
+++ paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-07-22 12:38:40 UTC 
(rev 5119)
@@ -34,8 +34,10 @@
 
 
 //static union AutopilotMessageBeth my_buffers[2];
-static struct AutopilotMessageBethUp   msg_in;
-static struct AutopilotMessageBethDown msg_out;
+//static struct AutopilotMessageBethUp   msg_in;
+//static struct AutopilotMessageBethDown msg_out;
+static struct AutopilotMessageFoo   msg_in;
+static struct AutopilotMessageFoo   msg_out;
 static void send_message(void);
 
 int main(int argc, char *argv[]) {
@@ -46,27 +48,30 @@
   while (1) {
     send_message();
     usleep(1953);
-    //usleep(50000);
+    //usleep(500000);
   }
 
   return 0;
 }
 
+uint16_t az,elev,tilt;
 
-
+static uint32_t foo = 0;
 static void send_message() {
-  static uint32_t foo = 0;
 
-  spi_link_send(&msg_out, sizeof(struct AutopilotMessageBethUp), &msg_in);
-  //  if (!foo%100) {
-#if 0
-  if (0) {
-  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);
+  msg_out.foo = 0x0123;
+  msg_out.bar = 0x4567;
+
+  spi_link_send(&msg_out, sizeof(struct AutopilotMessageFoo) , &msg_in);
+//  if (msg_in.bli == "0xdead") {
+    az = msg_in.foo;
+    elev = msg_in.bar;
+    tilt = msg_in.blaa;
+//  }
+  if (!(foo%100)) { 
+    printf("%d %d %d %x\r\n",az,elev,tilt,msg_in.bli);
+    
   }
-#endif
   foo++;
 }
 

Modified: paparazzi3/trunk/sw/airborne/beth/main_stm32.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_stm32.c      2010-07-22 12:35:37 UTC 
(rev 5118)
+++ paparazzi3/trunk/sw/airborne/beth/main_stm32.c      2010-07-22 12:38:40 UTC 
(rev 5119)
@@ -32,7 +32,7 @@
 //#include "booz/booz_radio_control.h"
 #include "booz/booz_imu.h"
 #include "lisa/lisa_overo_link.h"
-//#include "beth/bench_sensors.h"
+#include "beth/bench_sensors.h"
 
 static inline void main_init( void );
 static inline void main_periodic( void );
@@ -43,20 +43,13 @@
 
 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 inline void main_on_bench_sensors( void );
 
 static int16_t my_cnt;
 
-//hack to get beth angle values from CAN receive buffer
-//see airborne/stm32/can_hw.c
-#ifdef BETH_HACK
-extern uint16_t halfw1,halfw2,halfw3,halfw4;
-#endif
-
 int main(void) {
   main_init();
 
-
   while (1) {
     if (sys_time_periodic())
       main_periodic();
@@ -68,54 +61,58 @@
 static inline void main_init( void ) {
   hw_init();
   sys_time_init();
-  //actuators_init();
+  actuators_init();
   //radio_control_init();
   //booz_imu_init();
   overo_link_init();
-  //bench_sensors_init();
-  can_init();
+  bench_sensors_init();
 }
 
 
 static inline void main_periodic( void ) {
   //booz_imu_periodic();
-  //actuators_set(FALSE);
+
   OveroLinkPeriodic(main_on_overo_link_lost)
 
   RunOnceEvery(10, {LED_PERIODIC(); DOWNLINK_SEND_ALIVE(DefaultChannel, 16, 
MD5SUM);});
-#ifdef BETH_HACK
-  RunOnceEvery(5, {DOWNLINK_SEND_BETH(DefaultChannel, &halfw4, 
&halfw1,&halfw2, &halfw3);});
-#endif
 
-  //No longer needed as we switched from I2C to CAN
-  //read_bench_sensors();
- 
+  RunOnceEvery(5, {DOWNLINK_SEND_BETH(DefaultChannel, &bench_sensors.angle_1,
+    &bench_sensors.angle_2,&bench_sensors.angle_3, &bench_sensors.current);});
+
+  RunOnceEvery(5, {DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, 
&overo_link.down.msg.foo,&overo_link.down.msg.bar);});
+
+  /*Request reception of values from coder boards :
+    When configured for I2C, lisa stm32 is master and requests data from the
+    beth board slaves.
+    When configured for CAN, data is automatically available as CAN reception 
is
+    always ongoing, and new data generates a flag by the IST. */
+  read_bench_sensors();
+
+  booz2_commands[COMMAND_PITCH] = 0;
+  booz2_commands[COMMAND_ROLL] = 0;
+  booz2_commands[COMMAND_YAW] = 0;
+  booz2_commands[COMMAND_THRUST] = 4;
 }
 
 static inline void main_event( void ) {
   //BoozImuEvent(on_gyro_accel_event, on_mag_event);
-  //OveroLinkEvent(main_on_overo_msg_received);
+  OveroLinkEvent(main_on_overo_msg_received);
 
-  //No longer needed as we switched from I2C to CAN
-  //BenchSensorsEvent(main_on_bench_sensors);
+  BenchSensorsEvent(main_on_bench_sensors);
 
 }
 
 static inline void main_on_overo_msg_received(void) {
-  // struct AutopilotMessageBethUp* msg_out = (struct 
AutopilotMessageBethUp*)overo_link.msg_out;
-  overo_link.up.msg.gyro.x = booz_imu.gyro.p;
-  /*  msg_out->gyro.y = booz_imu.gyro.q;
-  msg_out->gyro.z = booz_imu.gyro.r;
-  msg_out->accel.x = booz_imu.accel.x;
-  msg_out->accel.y = booz_imu.accel.y;
-  msg_out->accel.z = booz_imu.accel.z;
-  msg_out->bench_sensor.x = my_cnt;
-  msg_out->bench_sensor.y = my_cnt;
-  msg_out->bench_sensor.z = my_cnt; */
+  overo_link.up.msg.foo = bench_sensors.angle_1;
+  overo_link.up.msg.bar = bench_sensors.angle_2;
+  overo_link.up.msg.blaa = bench_sensors.angle_3;
+  overo_link.up.msg.bli = 0xdead;
   my_cnt++;
+  actuators_set(TRUE);
 }
 
 static inline void main_on_overo_link_lost(void) {
+  actuators_set(FALSE);
   my_cnt = 0;
 }
 
@@ -175,13 +172,5 @@
 
 
 static inline void main_on_bench_sensors( void ) {
- /* 
-  DOWNLINK_SEND_BETH(DefaultChannel, &halfw4, &halfw1,&halfw2, &halfw3);
-                           
&bench_sensors.angle_1,&bench_sensors.angle_2,&bench_sensors.angle_1,
-                            &bench_sensors.angle_2);*/
-
-  //DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, 
&bench_sensors.angle_1,&bench_sensors.angle_2);
-  //DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, &can1_status, &can1_pending);
-  //DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, &halfw1, &halfw2);
-  
+ 
 }