[paparazzi-commits] [5183] changes to beth files support: overo now uses events for periodic function, added udp telemetry and start of datalink ( currently just counts datalink received bytes), stm IMU and motor interface tested, CAN link solid, and overo/ stm spi link solid once OVERO_LINK_TIMEOUT was increased in lisa_overo_link .h ( timeouts due to the overo not sending messages fast enough were causing the stm to think the link was lost and was not preparring the next messages--hence overo received a bunch of zeros )

[Paul Cox]

Revision: 5183
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5183
Author:   paulcox
Date:     2010-07-28 08:12:27 +0000 (Wed, 28 Jul 2010)
Log Message:
-----------
changes to beth files support: overo now uses events for periodic function, 
added udp telemetry and start of datalink (currently just counts datalink 
received bytes), stm IMU and motor interface tested, CAN link solid, and 
overo/stm spi link solid once OVERO_LINK_TIMEOUT was increased in 
lisa_overo_link.h (timeouts due to the overo not sending messages fast enough 
were causing the stm to think the link was lost and was not preparring the next 
messages--hence overo received a bunch of zeros)

Modified Paths:
--------------
    paparazzi3/trunk/conf/airframes/Poine/beth.xml
    paparazzi3/trunk/sw/airborne/beth/main_coders.c
    paparazzi3/trunk/sw/airborne/beth/main_overo.c
    paparazzi3/trunk/sw/airborne/beth/main_stm32.c

Modified: paparazzi3/trunk/conf/airframes/Poine/beth.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/beth.xml      2010-07-28 01:07:51 UTC 
(rev 5182)
+++ paparazzi3/trunk/conf/airframes/Poine/beth.xml      2010-07-28 08:12:27 UTC 
(rev 5183)
@@ -122,8 +122,7 @@
 
 
 main_stm32.CFLAGS += -DUSE_OVERO_LINK
-#main_stm32.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageBethUp 
-DOVERO_LINK_MSG_DOWN=AutopilotMessageBethDown
-main_stm32.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageFoo 
-DOVERO_LINK_MSG_DOWN=AutopilotMessageFoo
+main_stm32.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageBethUp 
-DOVERO_LINK_MSG_DOWN=AutopilotMessageBethDown
 main_stm32.CFLAGS += -DOVERO_LINK_LED_OK=5 -DOVERO_LINK_LED_KO=4 
-DUSE_DMA1_C2_IRQ
 main_stm32.srcs += lisa/lisa_overo_link.c 
lisa/arch/stm32/lisa_overo_link_arch.c
 
@@ -150,7 +149,7 @@
 #\
 #                             
$(SRC_BOOZ_ARCH)/actuators/booz_actuators_asctec_arch.c
 main_stm32.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1 
-#-DBOOZ_START_DELAY=3  -DUSE_TIM2_IRQ
+# -DBOOZ_START_DELAY=3  -DUSE_TIM2_IRQ
 main_stm32.CFLAGS += -DUSE_I2C1
 main_stm32.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
 
@@ -181,7 +180,6 @@
 main_coders.srcs += $(SRC_ARCH)/led_hw.c
 main_coders.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=7
 main_coders.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-#main_coders.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./10.)'
 main_coders.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
 
 main_coders.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
@@ -210,10 +208,17 @@
 main_overo.ARCHDIR = omap
 main_overo.CFLAGS = -I. -I$(SRC_FMS)
 main_overo.srcs  = $(SRC_BETH)/main_overo.c
+main_overo.CFLAGS  += -DFMS_PERIODIC_FREQ=100
+main_overo.srcs    += $(SRC_FMS)/fms_periodic.c
+main_overo.srcs    += $(SRC_FMS)/fms_serial_port.c
+main_overo.LDFLAGS += -lrt
 main_overo.srcs += $(SRC_FMS)/fms_spi_link.c
-#main_overo.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageBethUp 
-DOVERO_LINK_MSG_DOWN=AutopilotMessageBethDown
-main_overo.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageFoo 
-DOVERO_LINK_MSG_DOWN=AutopilotMessageFoo
+main_overo.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageBethUp 
-DOVERO_LINK_MSG_DOWN=AutopilotMessageBethDown
 
+main_overo.CFLAGS  += -DDOWNLINK -DDOWNLINK_TRANSPORT=UdpTransport
+main_overo.srcs    += $(SRC_FMS)/udp_transport.c downlink.c
+main_overo.srcs    += $(SRC_FMS)/fms_network.c
+main_overo.LDFLAGS += -levent
 
 #
 #

Modified: paparazzi3/trunk/sw/airborne/beth/main_coders.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_coders.c     2010-07-28 01:07:51 UTC 
(rev 5182)
+++ paparazzi3/trunk/sw/airborne/beth/main_coders.c     2010-07-28 08:12:27 UTC 
(rev 5183)
@@ -15,6 +15,8 @@
 
 #include <string.h>
 
+#include "beth/bench_sensors.h"
+
 /*
  *
  *  PC.01 (ADC Channel11) ext1-20 coder_values[1]
@@ -32,6 +34,8 @@
 static inline void main_event( void );
 
 static inline void main_init_adc(void);
+static inline void main_on_bench_sensors( void );
+
 //static inline void main_init_i2c2(void);
 //void i2c2_ev_irq_handler(void);
 //void i2c2_er_irq_handler(void);
@@ -46,8 +50,8 @@
 #define I2C2_ClockSpeed       200000
 
 #define MY_I2C2_BUF_LEN 4
-static uint8_t i2c2_idx;
-static uint8_t i2c2_buf[MY_I2C2_BUF_LEN];
+//static uint8_t i2c2_idx;
+//static uint8_t i2c2_buf[MY_I2C2_BUF_LEN];
 
 uint16_t servos[4];
 
@@ -85,18 +89,28 @@
   
   //RunOnceEvery(5, {DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, 
&coder_values[0], &coder_values[1]);});
   //RunOnceEvery(5, {DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, &can1_status, 
&can1_pending);});
+
+  RunOnceEvery(5, {DOWNLINK_SEND_BETH(DefaultChannel, &bench_sensors.angle_1,
+    &bench_sensors.angle_2,&bench_sensors.angle_3, &bench_sensors.current);});
   
   servos[0]=coder_values[0];
   servos[1]=coder_values[1];
-  can_transmit(2, 0, (uint8_t *)servos, 8);
+  //use id=1 for azimuth board
+  can_transmit(1, 0, (uint8_t *)servos, 8);
 
 }
 
 
 static inline void main_event( void ) {
+  BenchSensorsEvent(main_on_bench_sensors);
+}
 
+
+static inline void main_on_bench_sensors( void ) {
+ 
 }
 
+#if 0
 /*
  *
  *  I2C2 : autopilot link
@@ -189,8 +203,8 @@
 }
 
 
+#endif
 
-
 /*
  *
  *  ADC : coders

Modified: paparazzi3/trunk/sw/airborne/beth/main_overo.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-07-28 01:07:51 UTC 
(rev 5182)
+++ paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-07-28 08:12:27 UTC 
(rev 5183)
@@ -27,52 +27,125 @@
 #include <stdlib.h>
 #include <string.h>
 
-
+#include "fms_network.h"
+#include "fms_periodic.h"
 #include "fms_debug.h"
 #include "fms_spi_link.h"
 #include "fms_autopilot_msg.h"
 
+#include <event.h>
 
-//static union AutopilotMessageBeth my_buffers[2];
-//static struct AutopilotMessageBethUp   msg_in;
-//static struct AutopilotMessageBethDown msg_out;
-static struct AutopilotMessageFoo   msg_in;
-static struct AutopilotMessageFoo   msg_out;
+#include "downlink.h"
+#include "udp_transport.h"
+
+#define GCS_HOST "10.31.4.104"
+#define GCS_PORT 4242
+#define DATALINK_PORT 4243
+
+
+static void main_periodic(int);
+static void on_datalink_event(int fd, short event, void *arg);
+
+static struct FmsNetwork* network;
+
+static struct AutopilotMessageBethUp   msg_in;
+static struct AutopilotMessageBethDown msg_out;
 static void send_message(void);
 
+uint16_t az,elev,tilt;
+
+static uint32_t foo = 0;
+
+static void main_periodic(int my_sig_num) {
+
+  DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
+
+  send_message();
+
+  
DOWNLINK_SEND_BETH(DefaultChannel,&msg_in.bench_sensor.x,&msg_in.bench_sensor.y,
+    &msg_in.bench_sensor.z,&msg_in.bench_sensor.x);
+
+  DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel,
+                            &msg_in.gyro.p,
+                            &msg_in.gyro.q,
+                            &msg_in.gyro.r);
+    
+
+  DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel,
+                             &msg_in.accel.x,
+                             &msg_in.accel.y,
+                             &msg_in.accel.z);
+
+  UdpTransportPeriodic();
+
+}
+
+
 int main(int argc, char *argv[]) {
   if (spi_link_init()) {
     TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n");
     return -1;
   }
-  while (1) {
-    send_message();
-    usleep(1953);
-    //usleep(500000);
+
+  network = network_new(GCS_HOST, GCS_PORT, DATALINK_PORT, FALSE);
+
+  /* Initalize the event library */
+  event_init();
+
+  if (fms_periodic_init(main_periodic)) {
+    TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n");
+    return -1; 
   }
 
+  network = network_new(GCS_HOST, GCS_PORT, DATALINK_PORT, FALSE);
+  struct event datalink_event;
+  event_set(&datalink_event, network->socket_in, EV_READ, on_datalink_event, 
&datalink_event);
+  event_add(&datalink_event, NULL);
+
+  event_dispatch();
+
   return 0;
 }
 
-uint16_t az,elev,tilt;
+static uint16_t foo1 = 0x12;
 
-static uint32_t foo = 0;
 static void send_message() {
+  //uint8_t *fooptr;
 
-  msg_out.foo = 0x0123;
-  msg_out.bar = 0x4567;
+  msg_out.thrust = foo1+foo1;
+  msg_out.pitch = foo1;
+  foo1 = foo1+2;
+  //msg_out.cksum = msg_out.thrust + msg_out.pitch;
 
-  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);
-    
-  }
+  spi_link_send(&msg_out, sizeof(union AutopilotMessage) , &msg_in);
+
+  /*if (msg_in.bench_sensor.x == 0){
+    fooptr = &msg_in;
+    for (int i=0; i<sizeof(msg_in); i++)
+      printf("%02x ",  fooptr[i]);
+    printf("\n");
+  }*/
+
+  /*if (!(foo%10)) { 
+    printf("%d %d %d %x\r\n",msg_in.bench_sensor.x,msg_in.bench_sensor.y,
+    msg_in.bench_sensor.z,msg_in.bench_sensor.x);   
+  }*/
   foo++;
 }
 
+static void on_datalink_event(int fd, short event, void *arg) {
+  char buf[255];
+  int bytes_read;
+  bytes_read = read(fd, buf, sizeof(buf) - 1);
+  if (bytes_read == -1) {
+    perror("read");
+    return;
+  } else if (bytes_read == 0) {
+    fprintf(stderr, "Connection closed\n");
+    return;
+  }
+  printf("on_datalink_event, read %d\n", bytes_read);
 
+  struct event *ev = arg;
+  event_add(ev, NULL);
+}

Modified: paparazzi3/trunk/sw/airborne/beth/main_stm32.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_stm32.c      2010-07-28 01:07:51 UTC 
(rev 5182)
+++ paparazzi3/trunk/sw/airborne/beth/main_stm32.c      2010-07-28 08:12:27 UTC 
(rev 5183)
@@ -63,9 +63,10 @@
   sys_time_init();
   actuators_init();
   //radio_control_init();
-  //booz_imu_init();
+  booz_imu_init();
   overo_link_init();
   bench_sensors_init();
+  LED_ON(7);
 }
 
 
@@ -76,10 +77,10 @@
 
   RunOnceEvery(10, {LED_PERIODIC(); DOWNLINK_SEND_ALIVE(DefaultChannel, 16, 
MD5SUM);});
 
-  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_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);});
+  //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
@@ -92,6 +93,7 @@
   booz2_commands[COMMAND_ROLL] = 0;
   booz2_commands[COMMAND_YAW] = 0;
   booz2_commands[COMMAND_THRUST] = 4;
+  //actuators_set(TRUE);
 }
 
 static inline void main_event( void ) {
@@ -99,23 +101,32 @@
   OveroLinkEvent(main_on_overo_msg_received);
 
   BenchSensorsEvent(main_on_bench_sensors);
-
 }
 
 static inline void main_on_overo_msg_received(void) {
-  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;
+
+  overo_link.up.msg.bench_sensor.x = bench_sensors.angle_1;
+  overo_link.up.msg.bench_sensor.y = bench_sensors.angle_2;
+  overo_link.up.msg.bench_sensor.z = bench_sensors.angle_3;
+
+  overo_link.up.msg.accel.x = booz_imu.accel.x;
+  overo_link.up.msg.accel.y = booz_imu.accel.y;
+  overo_link.up.msg.accel.z = booz_imu.accel.z;
+
+  overo_link.up.msg.gyro.p = booz_imu.gyro.p;
+  overo_link.up.msg.gyro.q = booz_imu.gyro.q;
+  overo_link.up.msg.gyro.r = booz_imu.gyro.r;
+
   my_cnt++;
   actuators_set(TRUE);
 }
 
 static inline void main_on_overo_link_lost(void) {
-  actuators_set(FALSE);
+  //actuators_set(FALSE);
   my_cnt = 0;
 }
 
+
 static inline void on_gyro_accel_event(void) {
   BoozImuScaleGyro();
   BoozImuScaleAccel();