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)

conf/airframes/Poine/beth.xml

@@ -122,8 +122,7 @@ main_stm32.srcs += downlink.c pprz_transport.c
 
 
 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=AutopilotMessageBethUp -DOVERO_LINK_MSG_DOWN=AutopilotMessageBethDown
-main_stm32.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageFoo -DOVERO_LINK_MSG_DOWN=AutopilotMessageFoo
 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 @@ main_stm32.srcs += $(SRC_BOOZ)/actuators/booz_actuators_asctec.c
 #\
 #                             $(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.CFLAGS += -DUSE_LED
 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 @@ SRC_FMS=fms
 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=AutopilotMessageBethUp -DOVERO_LINK_MSG_DOWN=AutopilotMessageBethDown
-main_overo.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageFoo -DOVERO_LINK_MSG_DOWN=AutopilotMessageFoo
 
+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
 
 #
 #

sw/airborne/beth/main_coders.c

@@ -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_periodic( void );
 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 @@ static uint16_t coder_values[3];
 #define I2C2_ClockSpeed       200000
 
 #define MY_I2C2_BUF_LEN 4
-static uint8_t i2c2_idx;
+//static uint8_t i2c2_idx;
-static uint8_t i2c2_buf[MY_I2C2_BUF_LEN];
+//static uint8_t i2c2_buf[MY_I2C2_BUF_LEN];
 
 uint16_t servos[4];
 
@@ -85,18 +89,28 @@ static inline void main_periodic( void ) {
   
   //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,7 +203,7 @@ void i2c2_er_irq_handler(void) {
 }
 
 
-
+#endif
 
 /*
  *

sw/airborne/beth/main_overo.c

@@ -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];
+#include "downlink.h"
-//static struct AutopilotMessageBethUp   msg_in;
+#include "udp_transport.h"
-//static struct AutopilotMessageBethDown msg_out;
+
-static struct AutopilotMessageFoo   msg_in;
+#define GCS_HOST "10.31.4.104"
-static struct AutopilotMessageFoo   msg_out;
+#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();
+  network = network_new(GCS_HOST, GCS_PORT, DATALINK_PORT, FALSE);
-    usleep(1953);
+
-    //usleep(500000);
+  /* 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.thrust = foo1+foo1;
-  msg_out.bar = 0x4567;
+  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);
+  spi_link_send(&msg_out, sizeof(union AutopilotMessage) , &msg_in);
-//  if (msg_in.bli == "0xdead") {
+
-    az = msg_in.foo;
+  /*if (msg_in.bench_sensor.x == 0){
-    elev = msg_in.bar;
+    fooptr = &msg_in;
-    tilt = msg_in.blaa;
+    for (int i=0; i<sizeof(msg_in); i++)
-//  }
+      printf("%02x ",  fooptr[i]);
-  if (!(foo%100)) { 
+    printf("\n");
-    printf("%d %d %d %x\r\n",az,elev,tilt,msg_in.bli);
+  }*/
-    
+
-  }
+  /*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);
+}

sw/airborne/beth/main_stm32.c

@@ -63,9 +63,10 @@ static inline void main_init( void ) {
   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 @@ static inline void main_periodic( void ) {
 
   RunOnceEvery(10, {LED_PERIODIC(); DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});
 
-  RunOnceEvery(5, {DOWNLINK_SEND_BETH(DefaultChannel, &bench_sensors.angle_1,
+  //RunOnceEvery(5, {DOWNLINK_SEND_BETH(DefaultChannel, &bench_sensors.angle_1,
-    &bench_sensors.angle_2,&bench_sensors.angle_3, &bench_sensors.current);});
+  //  &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 @@ static inline void main_periodic( void ) {
   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 @@ static inline void main_event( void ) {
   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.bench_sensor.x = bench_sensors.angle_1;
-  overo_link.up.msg.blaa = bench_sensors.angle_3;
+  overo_link.up.msg.bench_sensor.y = bench_sensors.angle_2;
-  overo_link.up.msg.bli = 0xdead;
+  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();