[paparazzi-commits] [5210] refactored beth code

[antoine drouin]

Revision: 5210
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5210
Author:   poine
Date:     2010-08-04 13:51:40 +0000 (Wed, 04 Aug 2010)
Log Message:
-----------
refactored beth code

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

Added Paths:
-----------
    paparazzi3/trunk/conf/settings/settings_beth.xml
    paparazzi3/trunk/sw/airborne/beth/overo_controller.c
    paparazzi3/trunk/sw/airborne/beth/overo_controller.h
    paparazzi3/trunk/sw/airborne/beth/overo_estimator.c
    paparazzi3/trunk/sw/airborne/beth/overo_estimator.h
    paparazzi3/trunk/sw/airborne/beth/overo_file_logger.c
    paparazzi3/trunk/sw/airborne/beth/overo_file_logger.h
    paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.c
    paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.h

Added: paparazzi3/trunk/conf/settings/settings_beth.xml
===================================================================
--- paparazzi3/trunk/conf/settings/settings_beth.xml                            
(rev 0)
+++ paparazzi3/trunk/conf/settings/settings_beth.xml    2010-08-04 13:51:40 UTC 
(rev 5210)
@@ -0,0 +1,12 @@
+<!DOCTYPE settings SYSTEM "settings.dtd">
+
+<settings>
+  <dl_settings>
+
+    <dl_settings NAME="Controller">
+      <dl_setting var="controller.kp" min="0" step="0.01" max="1" 
module="beth/overo_controller"/>
+      <dl_setting var="controller.kd" min="0" step="0.01" max="1" 
module="beth/overo_controller"/>
+    </dl_settings>
+
+    </dl_settings>
+</settings>

Modified: paparazzi3/trunk/sw/airborne/beth/main_overo.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-08-04 11:48:05 UTC 
(rev 5209)
+++ paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-08-04 13:51:40 UTC 
(rev 5210)
@@ -28,78 +28,108 @@
 #include <string.h>
 #include <signal.h>
 
-#include "downlink_transport.h"
+#include <event.h>
+
 #include "messages2.h"
-#include "udp_transport2.h"
-#include "dl_protocol.h"
 
-#include "fms_network.h"
 #include "fms_periodic.h"
 #include "fms_debug.h"
 #include "fms_spi_link.h"
 #include "fms_autopilot_msg.h"
 #include "booz/booz_imu.h"
 
-#include <event.h>
+#include "overo_file_logger.h"
+#include "overo_gcs_com.h"
+#include "overo_estimator.h"
+#include "overo_controller.h"
 
+
+
 #define GCS_HOST "10.31.4.104"
 #define GCS_PORT 4242
 #define DATALINK_PORT 4243
 
 
 static void main_periodic(int);
+static void main_parse_cmd_line(int argc, char *argv[]);
+static void main_exit(int sig);
+static void main_talk_with_stm32(void);
 
-static void on_datalink_event(int fd, short event __attribute__((unused)), 
void *arg);
 
-static struct FmsNetwork* network;
-static struct DownlinkTransport *udp_transport;
-
 static struct AutopilotMessageBethUp   msg_in;
 static struct AutopilotMessageBethDown msg_out;
-static void send_message(void);
-static void PID(void);
-void ex_program(int sig);
 
-
 struct BoozImu booz_imu;
 struct BoozImuFloat booz_imu_float;
 
-uint16_t az,elev,tilt;
-//FILE *outfile;
 
 static uint32_t foo = 0;
 
-static void main_periodic(int my_sig_num) {
 
-  RunOnceEvery(50, {DOWNLINK_SEND_ALIVE(udp_transport, 16, MD5SUM);});
+int main(int argc, char *argv[]) {
 
-  PID();
-  send_message();
+  
+  (void) signal(SIGINT, main_exit);
+  
+  RATES_ASSIGN(booz_imu.gyro_neutral,  IMU_GYRO_P_NEUTRAL,  
IMU_GYRO_Q_NEUTRAL,  IMU_GYRO_R_NEUTRAL);
+  VECT3_ASSIGN(booz_imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, 
IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL);
+  VECT3_ASSIGN(booz_imu.mag_neutral,   IMU_MAG_X_NEUTRAL,   IMU_MAG_Y_NEUTRAL, 
  IMU_MAG_Z_NEUTRAL);
+  
+  if (spi_link_init()) {
+    TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n");
+    return -1;
+  }
+  
+  /* Initalize the event library */
+  event_init();
+  
+  if (fms_periodic_init(main_periodic)) {
+    TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n");
+    return -1; 
+  }
 
-  RunOnceEvery(50, 
{DOWNLINK_SEND_BETH(udp_transport,&msg_in.bench_sensor.x,&msg_in.bench_sensor.y,
-    &msg_in.bench_sensor.z,&foo);});
+  //  file_logger_init("my_log.data");
 
-  booz_imu.gyro_unscaled.p = (msg_in.gyro.p&0xFFFF);
-  booz_imu.gyro_unscaled.q = (msg_in.gyro.q&0xFFFF);
-  booz_imu.gyro_unscaled.r = (msg_in.gyro.r&0xFFFF);
-  booz_imu.accel_unscaled.x = (msg_in.accel.x&0xFFFF);
-  booz_imu.accel_unscaled.y = (msg_in.accel.y&0xFFFF);
-  booz_imu.accel_unscaled.z = (msg_in.accel.z&0xFFFF);
+  gcs_com_init(GCS_HOST, GCS_PORT, DATALINK_PORT, FALSE);
 
-  //fprintf(outfile,"%f %d IMU_ACCEL_RAW %d %d 
%d\n",foo/500.,42,booz_imu.accel_unscaled.x,booz_imu.accel_unscaled.y,booz_imu.accel_unscaled.z);
-  //fprintf(outfile,"%f %d IMU_GYRO_RAW %d %d 
%d\n",foo/500.,42,booz_imu.gyro_unscaled.p,booz_imu.gyro_unscaled.q,booz_imu.gyro_unscaled.r);
+  main_parse_cmd_line(argc, argv);
+  
+  event_dispatch();
+ 
+  printf("goodbye! (%d)\n",foo);
 
-  RunOnceEvery(10, {DOWNLINK_SEND_IMU_GYRO_RAW(udp_transport,
+  return 0;
+}
+
+
+
+static void main_periodic(int my_sig_num) {
+
+  RunOnceEvery(50, {DOWNLINK_SEND_ALIVE(gcs_com.udp_transport, 16, MD5SUM);});
+  
+  main_talk_with_stm32();
+  
+  RunOnceEvery(50, 
{DOWNLINK_SEND_BETH(gcs_com.udp_transport,&msg_in.bench_sensor.x,&msg_in.bench_sensor.y,
+                                      &msg_in.bench_sensor.z,&foo);});
+  
+  estimator_run(msg_in.bench_sensor.z);
+ 
+  control_run();
+
+  //  file_logger_periodic();
+
+
+  RunOnceEvery(10, {DOWNLINK_SEND_IMU_GYRO_RAW(gcs_com.udp_transport,
                             //&msg_in.gyro.p,&msg_in.gyro.q,&msg_in.gyro.r)
                                
&booz_imu.gyro_unscaled.p,&booz_imu.gyro_unscaled.q,&booz_imu.gyro_unscaled.r);});
     
 
-  RunOnceEvery(10, {DOWNLINK_SEND_IMU_ACCEL_RAW(udp_transport,
+  RunOnceEvery(10, {DOWNLINK_SEND_IMU_ACCEL_RAW(gcs_com.udp_transport,
                             //&msg_in.accel.x,&msg_in.accel.y,&msg_in.accel.z
                                
&booz_imu.accel_unscaled.x,&booz_imu.accel_unscaled.y,&booz_imu.accel_unscaled.z);});
 
   BoozImuScaleGyro();
-  RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_GYRO(udp_transport,
+  RunOnceEvery(50, {DOWNLINK_SEND_BOOZ2_GYRO(gcs_com.udp_transport,
                             //&msg_in.gyro.p,&msg_in.gyro.q,&msg_in.gyro.r)
                                
&booz_imu.gyro.p,&booz_imu.gyro.q,&booz_imu.gyro.r);});
     
@@ -109,102 +139,42 @@
                                
&booz_imu.accel.x,&booz_imu.accel.y,&booz_imu.accel.z);});*/
 
   //RunOnceEvery(33, {UdpTransportPeriodic();});
-  RunOnceEvery(33, {if (udp_transport->Periodic) 
{udp_transport->Periodic(udp_transport->impl);} });
+  RunOnceEvery(33, gcs_com_periodic());
 
 
 }
 
-static int8_t pitchval = 0;
-static float kp, ki, kd;
-int8_t presp,dresp;
-static uint16_t tilt_sp=2770;
-static float piderror,piderrorold;
 
-static void PID(){
-  piderror = tilt_sp-msg_in.bench_sensor.z;
+static void main_parse_cmd_line(int argc, char *argv[]) {
 
-  presp = (int8_t)(kp * piderror);
-  //Generate derivative error :
-  //dresp = (int8_t)(kd * (piderror - piderrorold) );
-  //Now using gyro measurement
-  dresp = (int8_t)(kd * booz_imu.gyro.q);  
-
-  pitchval =  presp + dresp;   
-
-  piderrorold = piderror;
-
-  if (!(foo%100)) {
-    printf("%d %d\n",presp,dresp);
-  }
-}
-
-void ex_program(int sig) {
-  //fprintf(outfile,"%d\n",foo);
-  printf("Closing down\n");
-  //fclose(outfile);
-  exit(EXIT_SUCCESS);
-}
-
-int main(int argc, char *argv[]) {
-
   if (argc>1){
-    kp = atof(argv[1]);
-    printf("kp set to %f\n",kp);
+    controller.kp = atof(argv[1]);
+    //    printf("kp set to %f\n",kp);
     if (argc>2) {
-      kd = atof(argv[2]);
-      printf("kd set to %f\n",kd);
+      controller.kd = atof(argv[2]);
+      //      printf("kd set to %f\n",kd);
     } else {
-      kd=1.0;
-      printf("using default value of kd %f\n",kd);
+      controller.kd=1.0;
+      //      printf("using default value of kd %f\n",kd);
     }
   } else {
-    kp = 0.05;
-    printf("using default value of kp %f\n",kp);
+    controller.kp = 0.05;
+    //    printf("using default value of kp %f\n",kp);
   }
-  ki=0.0;
 
-  //outfile = fopen("output.data","w+");
+}
 
-  (void) signal(SIGINT, ex_program);
 
-  RATES_ASSIGN(booz_imu.gyro_neutral,  IMU_GYRO_P_NEUTRAL,  
IMU_GYRO_Q_NEUTRAL,  IMU_GYRO_R_NEUTRAL);
-  VECT3_ASSIGN(booz_imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, 
IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL);
-  VECT3_ASSIGN(booz_imu.mag_neutral,   IMU_MAG_X_NEUTRAL,   IMU_MAG_Y_NEUTRAL, 
  IMU_MAG_Z_NEUTRAL);
-
-  if (spi_link_init()) {
-    TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n");
-    return -1;
-  }
-
-  /* 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);
-  udp_transport = udp_transport_new(network);
-
-  struct event datalink_event;
-  //event_set(&datalink_event, network->socket_in, EV_READ, on_datalink_event, 
&datalink_event);
-  event_set(&datalink_event, network->socket_in, EV_READ| EV_PERSIST, 
on_datalink_event, udp_transport);
-  event_add(&datalink_event, NULL);
-
-  event_dispatch();
- 
-  printf("goodbye! (%d)\n",foo);
-
-  return 0;
+static void main_exit(int sig) {
+  printf("Closing down\n");
+  //file_logger_exit()
+  exit(EXIT_SUCCESS);
 }
 
 
+static void main_talk_with_stm32() {
 
-//static int8_t pitchval = 0;
-static int8_t adder = 1;
-
-static void send_message() {
+  static int8_t adder = 1;
   //uint8_t *fooptr;
 
   msg_out.thrust = 10;
@@ -213,9 +183,9 @@
     /*if (pitchval == 15 ) adder=-1;
     if (pitchval == -15 ) adder=1;
     pitchval = pitchval + adder;*/
-    printf("pitchval now %d\n",pitchval);
+    printf("cmd now %f\n",controller.cmd);
   }
-  msg_out.pitch = pitchval;
+  msg_out.pitch = (int8_t)controller.cmd;
   //msg_out.cksum = msg_out.thrust + msg_out.pitch;
 
   spi_link_send(&msg_out, sizeof(union AutopilotMessage) , &msg_in);
@@ -232,72 +202,16 @@
     msg_in.bench_sensor.z,msg_in.bench_sensor.x);   
   }*/
   foo++;
-}
 
-static inline int checked_read(int fd, char *buf, size_t len)
-{
-  int bytes = read(fd, buf, len);
+  booz_imu.gyro_unscaled.p = (msg_in.gyro.p&0xFFFF);
+  booz_imu.gyro_unscaled.q = (msg_in.gyro.q&0xFFFF);
+  booz_imu.gyro_unscaled.r = (msg_in.gyro.r&0xFFFF);
+  booz_imu.accel_unscaled.x = (msg_in.accel.x&0xFFFF);
+  booz_imu.accel_unscaled.y = (msg_in.accel.y&0xFFFF);
+  booz_imu.accel_unscaled.z = (msg_in.accel.z&0xFFFF);
 
-  if (bytes == 0) {
-    fprintf(stderr, "Connection closed\n");
-  } else if (bytes == -1) {
-    perror("read");
-  }
-
-  return bytes;
 }
 
-#define DL_MSG_SIZE 128
 
-bool_t my_dl_msg_available;
-uint8_t my_dl_buffer[DL_MSG_SIZE]  __attribute__ ((aligned));
 
-#define IdOfMsg(x) (x[1])
 
-static void dl_handle_msg(struct DownlinkTransport *tp) {
-  uint8_t msg_id = IdOfMsg(my_dl_buffer);
-  switch (msg_id) {
-  
-  case  DL_PING:
-    {
-      DOWNLINK_SEND_PONG(tp);
-printf("sent pong\n");
-    }
-    break;
-    
-  case DL_SETTING :
-    {
-      uint8_t i = DL_SETTING_index(my_dl_buffer);
-      float var = DL_SETTING_value(my_dl_buffer);
-      // DlSetting(i, var);
-      DOWNLINK_SEND_DL_VALUE(tp, &i, &var);
-printf("sent back value\n");
-    }
-    break;
-   default :
-printf("did nothing\n");
-     break;
-  }
-
-}
-
-static void on_datalink_event(int fd, short event __attribute__((unused)), 
void *arg)
-{
-  char buf[255];
-  int bytes_read;
-  bytes_read = checked_read(fd, buf, sizeof(buf) - 1);
-  struct DownlinkTransport *tp = (struct DownlinkTransport *) arg;
-  struct udp_transport *udp_impl = tp->impl;
-  printf("on datalink event: %d bytes\n",bytes_read);
-  int i = 0;
-  while (i<bytes_read) {
-    parse_udp_dl(udp_impl, buf[i]);
-    i++;
-    if (udp_impl->udp_dl_msg_received) {
-      memcpy(my_dl_buffer, udp_impl->udp_dl_payload, 
udp_impl->udp_dl_payload_len);
-      dl_handle_msg(tp);
-      udp_impl->udp_dl_msg_received = FALSE;
-    }
-  }
- 
-}

Added: paparazzi3/trunk/sw/airborne/beth/overo_controller.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_controller.c                        
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/beth/overo_controller.c        2010-08-04 
13:51:40 UTC (rev 5210)
@@ -0,0 +1,25 @@
+#include "overo_controller.h"
+
+#include "overo_estimator.h"
+
+struct OveroController controller;
+
+void control_init(void) {
+  controller.kp = 0.05;
+  controller.kd = 0.01;
+
+  controller.tilt_sp = 0.;
+
+  controller.cmd = 0.;
+}
+
+
+
+void control_run(void) {
+  
+  float track_err = estimator.tilt - controller.tilt_sp;
+  
+  controller.cmd = controller.kp*track_err + controller.kd*estimator.tilt_dot;
+  
+}
+

Added: paparazzi3/trunk/sw/airborne/beth/overo_controller.h
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_controller.h                        
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/beth/overo_controller.h        2010-08-04 
13:51:40 UTC (rev 5210)
@@ -0,0 +1,19 @@
+#ifndef OVERO_CONTROLLER_H
+#define OVERO_CONTROLLER_H
+
+struct OveroController {
+  float kp;
+  float kd;
+  
+  float tilt_sp;
+
+  float cmd;
+};
+
+
+extern struct OveroController controller;
+
+extern void control_init(void);
+extern void control_run(void);
+
+#endif /* OVERO_CONTROLLER_H */

Added: paparazzi3/trunk/sw/airborne/beth/overo_estimator.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_estimator.c                         
(rev 0)
+++ paparazzi3/trunk/sw/airborne/beth/overo_estimator.c 2010-08-04 13:51:40 UTC 
(rev 5210)
@@ -0,0 +1,22 @@
+#include "overo_estimator.h"
+
+#include "booz/booz_imu.h"
+
+struct OveroEstimator estimator;
+
+void estimator_init(void) {
+
+}
+
+
+void estimator_run(uint16_t tilt_measure) {
+  
+  const int32_t tilt_neutral = 2770;
+  const float   tilt_scale = 1./580.;
+
+  estimator.tilt = ((int32_t)tilt_measure - tilt_neutral) * tilt_scale;
+  estimator.tilt_dot = booz_imu.gyro.q;
+
+
+}
+

Added: paparazzi3/trunk/sw/airborne/beth/overo_estimator.h
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_estimator.h                         
(rev 0)
+++ paparazzi3/trunk/sw/airborne/beth/overo_estimator.h 2010-08-04 13:51:40 UTC 
(rev 5210)
@@ -0,0 +1,22 @@
+#ifndef OVERO_ESTIMATOR_H
+#define OVERO_ESTIMATOR_H
+
+#include <inttypes.h>
+
+struct OveroEstimator {
+
+  float azimuth;
+  float elevation;
+  float tilt;
+  
+  float tilt_dot;
+
+};
+
+
+extern struct OveroEstimator estimator;
+
+extern void estimator_init(void);
+extern void estimator_run(uint16_t tilt_measure);
+
+#endif /* OVERO_CONTROLLER_H */

Added: paparazzi3/trunk/sw/airborne/beth/overo_file_logger.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_file_logger.c                       
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/beth/overo_file_logger.c       2010-08-04 
13:51:40 UTC (rev 5210)
@@ -0,0 +1,31 @@
+#include "overo_file_logger.h"
+
+#include "booz/booz_imu.h"
+
+struct FileLogger file_logger;
+
+void file_logger_init(char* filename) {
+
+  file_logger.outfile = fopen(filename,"w+");
+
+}
+
+
+
+void file_logger_periodic(void) {
+  static uint32_t foo = 0;
+  foo++;
+  fprintf(file_logger.outfile,"%f %d IMU_ACCEL_RAW %d %d 
%d\n",foo/512.,42,booz_imu.accel_unscaled.x,booz_imu.accel_unscaled.y,booz_imu.accel_unscaled.z);
+  fprintf(file_logger.outfile,"%f %d IMU_GYRO_RAW %d %d 
%d\n",foo/512.,42,booz_imu.gyro_unscaled.p,booz_imu.gyro_unscaled.q,booz_imu.gyro_unscaled.r);
+
+}
+
+void file_logger_exit(void) {
+
+  fclose(file_logger.outfile);
+
+}
+
+
+
+

Added: paparazzi3/trunk/sw/airborne/beth/overo_file_logger.h
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_file_logger.h                       
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/beth/overo_file_logger.h       2010-08-04 
13:51:40 UTC (rev 5210)
@@ -0,0 +1,17 @@
+#ifndef OVERO_FILE_LOGGER_H
+#define OVERO_FILE_LOGGER_H
+
+#include <stdio.h>
+
+struct FileLogger {
+  FILE* outfile;
+};
+
+extern struct FileLogger file_logger;
+
+extern void file_logger_init(char* filename);
+extern void file_logger_periodic(void);
+extern void file_logger_exit(void);
+
+#endif /* OVERO_FILE_LOGGER_H */
+

Added: paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.c                           
(rev 0)
+++ paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.c   2010-08-04 13:51:40 UTC 
(rev 5210)
@@ -0,0 +1,98 @@
+#include "overo_gcs_com.h"
+
+#include <event.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "downlink_transport.h"
+#include "messages2.h"
+#include "udp_transport2.h"
+#include "dl_protocol.h"
+#include "settings.h"
+//#include "downlink.h"
+
+
+static void dl_handle_msg(struct DownlinkTransport *tp);
+static inline int checked_read(int fd, char *buf, size_t len);
+static void on_datalink_event(int fd, short event __attribute__((unused)), 
void *arg);
+
+struct OveroGcsCom gcs_com;
+
+
+void gcs_com_init(char* gcs_host, uint32_t gcs_port, uint32_t datalink_port, 
uint8_t broadcast) {
+
+  gcs_com.network = network_new(gcs_host, gcs_port, datalink_port, broadcast);
+  gcs_com.udp_transport = udp_transport_new(gcs_com.network);
+  struct event datalink_event;
+  event_set(&datalink_event, gcs_com.network->socket_in, EV_READ| EV_PERSIST, 
on_datalink_event, gcs_com.udp_transport);
+  event_add(&datalink_event, NULL);
+  
+}
+
+void gcs_com_periodic(void) {
+  if (gcs_com.udp_transport->Periodic) 
+    gcs_com.udp_transport->Periodic(gcs_com.udp_transport->impl);
+}
+
+
+static void on_datalink_event(int fd, short event __attribute__((unused)), 
void *arg) {
+
+  char buf[255];
+  int bytes_read;
+  bytes_read = checked_read(fd, buf, sizeof(buf) - 1);
+  struct DownlinkTransport *tp = (struct DownlinkTransport *) arg;
+  struct udp_transport *udp_impl = tp->impl;
+  printf("on datalink event: %d bytes\n",bytes_read);
+  int i = 0;
+  while (i<bytes_read) {
+    parse_udp_dl(udp_impl, buf[i]);
+    i++;
+    if (udp_impl->udp_dl_msg_received) {
+      memcpy(gcs_com.my_dl_buffer, udp_impl->udp_dl_payload, 
udp_impl->udp_dl_payload_len);
+      dl_handle_msg(tp);
+      udp_impl->udp_dl_msg_received = FALSE;
+    }
+  }
+ 
+}
+
+#define IdOfMsg(x) (x[1])
+
+static void dl_handle_msg(struct DownlinkTransport *tp) {
+  uint8_t msg_id = IdOfMsg(gcs_com.my_dl_buffer);
+  switch (msg_id) {
+  
+  case  DL_PING:
+    DOWNLINK_SEND_PONG(tp);
+    break;
+    
+  case DL_SETTING :
+    {
+      uint8_t i = DL_SETTING_index(gcs_com.my_dl_buffer);
+      float var = DL_SETTING_value(gcs_com.my_dl_buffer);
+      DlSetting(i, var);
+      DOWNLINK_SEND_DL_VALUE(tp, &i, &var);
+    }
+    break;
+
+  default :
+    printf("did nothing\n");
+    break;
+  }
+
+}
+
+
+static inline int checked_read(int fd, char *buf, size_t len)
+{
+  int bytes = read(fd, buf, len);
+
+  if (bytes == 0) {
+    fprintf(stderr, "Connection closed\n");
+  } else if (bytes == -1) {
+    perror("read");
+  }
+  return bytes;
+}
+
+

Added: paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.h
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.h                           
(rev 0)
+++ paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.h   2010-08-04 13:51:40 UTC 
(rev 5210)
@@ -0,0 +1,28 @@
+#ifndef OVERO_GCS_COM_H
+#define OVERO_GCS_COM_H
+
+#include "fms_network.h"
+#include "downlink_transport.h"
+
+#include "std.h"
+
+#define GCS_COM_DL_BUF_SIZE 128
+
+struct OveroGcsCom {
+
+  struct FmsNetwork* network;
+  struct DownlinkTransport *udp_transport;
+  /*  bool_t  my_dl_msg_available; */
+  uint8_t my_dl_buffer[GCS_COM_DL_BUF_SIZE]  __attribute__ ((aligned));
+
+};
+
+
+extern struct OveroGcsCom gcs_com;
+
+
+extern void gcs_com_init(char* gcs_host, uint32_t gcs_port, uint32_t 
datalink_port, uint8_t broadcast);
+extern void gcs_com_periodic(void);
+
+
+#endif /* OVERO_GCS_COM_H */