[paparazzi-commits] [5184] updates to beth : tested at overo periodic running at 500Hz.

[Paul Cox]

Revision: 5184
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5184
Author:   paulcox
Date:     2010-07-28 15:18:09 +0000 (Wed, 28 Jul 2010)
Log Message:
-----------
updates to beth : tested at overo periodic running at 500Hz. motors enabled 
only if overo com active and allowable thrust for now < 6. 

Modified Paths:
--------------
    paparazzi3/trunk/conf/airframes/Poine/beth.xml
    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 08:12:27 UTC 
(rev 5183)
+++ paparazzi3/trunk/conf/airframes/Poine/beth.xml      2010-07-28 15:18:09 UTC 
(rev 5184)
@@ -208,7 +208,7 @@
 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.CFLAGS  += -DFMS_PERIODIC_FREQ=500
 main_overo.srcs    += $(SRC_FMS)/fms_periodic.c
 main_overo.srcs    += $(SRC_FMS)/fms_serial_port.c
 main_overo.LDFLAGS += -lrt

Modified: paparazzi3/trunk/sw/airborne/beth/main_overo.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-07-28 08:12:27 UTC 
(rev 5183)
+++ paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-07-28 15:18:09 UTC 
(rev 5184)
@@ -58,25 +58,25 @@
 
 static void main_periodic(int my_sig_num) {
 
-  DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
+  RunOnceEvery(10, {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);
+  RunOnceEvery(10, 
{DOWNLINK_SEND_BETH(DefaultChannel,&msg_in.bench_sensor.x,&msg_in.bench_sensor.y,
+    &msg_in.bench_sensor.z,&foo);});
 
-  DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel,
+  RunOnceEvery(10, {DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel,
                             &msg_in.gyro.p,
                             &msg_in.gyro.q,
-                            &msg_in.gyro.r);
+                            &msg_in.gyro.r);});
     
 
-  DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel,
+  RunOnceEvery(10, {DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel,
                              &msg_in.accel.x,
                              &msg_in.accel.y,
-                             &msg_in.accel.z);
+                             &msg_in.accel.z);});
 
-  UdpTransportPeriodic();
+  RunOnceEvery(10, {UdpTransportPeriodic();});
 
 }
 
@@ -107,14 +107,21 @@
   return 0;
 }
 
-static uint16_t foo1 = 0x12;
+static int8_t pitchval = 0;
+static int8_t adder = 1;
 
 static void send_message() {
   //uint8_t *fooptr;
 
-  msg_out.thrust = foo1+foo1;
-  msg_out.pitch = foo1;
-  foo1 = foo1+2;
+  msg_out.thrust = 7;
+
+  if (!(foo%100)) { 
+    if (pitchval == 10 ) adder=-1;
+    if (pitchval == -10 ) adder=1;
+    pitchval = pitchval + adder;
+    printf("pitchval now %d\n",pitchval);
+  }
+  msg_out.pitch = pitchval;
   //msg_out.cksum = msg_out.thrust + msg_out.pitch;
 
   spi_link_send(&msg_out, sizeof(union AutopilotMessage) , &msg_in);

Modified: paparazzi3/trunk/sw/airborne/beth/main_stm32.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_stm32.c      2010-07-28 08:12:27 UTC 
(rev 5183)
+++ paparazzi3/trunk/sw/airborne/beth/main_stm32.c      2010-07-28 15:18:09 UTC 
(rev 5184)
@@ -89,11 +89,19 @@
     always ongoing, and new data generates a flag by the IST. */
   read_bench_sensors();
 
-  booz2_commands[COMMAND_PITCH] = 0;
+  booz2_commands[COMMAND_PITCH] = (int8_t)((0xFF) & overo_link.down.msg.pitch);
   booz2_commands[COMMAND_ROLL] = 0;
   booz2_commands[COMMAND_YAW] = 0;
-  booz2_commands[COMMAND_THRUST] = 4;
-  //actuators_set(TRUE);
+  if ( overo_link.down.msg.thrust < 6) {
+    booz2_commands[COMMAND_THRUST] = overo_link.down.msg.thrust;
+  } else { 
+    booz2_commands[COMMAND_THRUST] = 5;
+  }
+  if (my_cnt == 0) {
+    actuators_set(FALSE);
+  } else {
+    actuators_set(TRUE);
+  }
 }
 
 static inline void main_event( void ) {
@@ -117,8 +125,8 @@
   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);
+  my_cnt=1;
+  //actuators_set(TRUE);
 }
 
 static inline void main_on_overo_link_lost(void) {