[paparazzi-commits] [4885] a first job for Lisa

[antoine drouin]

Revision: 4885
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4885
Author:   poine
Date:     2010-04-27 18:54:44 +0000 (Tue, 27 Apr 2010)
Log Message:
-----------
a first job for Lisa

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

Added: paparazzi3/trunk/sw/airborne/beth/main_overo.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_overo.c                              
(rev 0)
+++ paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-04-27 18:54:44 UTC 
(rev 4885)
@@ -0,0 +1,71 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#include <stdint.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+
+#include "fms_debug.h"
+#include "fms_spi_link.h"
+#include "fms_autopilot_msg.h"
+
+
+static union AutopilotMessageBeth my_buffers[2];
+static struct AutopilotMessageBethUp*   msg_in  = &my_buffers[0].up;
+static struct AutopilotMessageBethDown* msg_out = &my_buffers[1].down;
+static void send_message(void);
+
+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(50000);
+  }
+
+  return 0;
+}
+
+
+
+static void send_message() {
+  static uint32_t foo = 0;
+
+  spi_link_send(msg_out, sizeof(union AutopilotMessageBeth), msg_in);
+  if (!foo%100) {
+    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);
+  }
+  foo++;
+}
+
+

Added: paparazzi3/trunk/sw/airborne/beth/main_stm32.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_stm32.c                              
(rev 0)
+++ paparazzi3/trunk/sw/airborne/beth/main_stm32.c      2010-04-27 18:54:44 UTC 
(rev 4885)
@@ -0,0 +1,153 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2010 Antoine Drouin <address@hidden>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA. 
+ */
+
+#include <stm32/rcc.h>
+#include <stm32/gpio.h>
+
+#include <stm32/flash.h>
+#include <stm32/misc.h>
+
+#include BOARD_CONFIG
+#include "init_hw.h"
+#include "sys_time.h"
+#include "downlink.h"
+#include "booz/booz_imu.h"
+#include "lisa/lisa_overo_link.h"
+
+
+static inline void main_init( void );
+static inline void main_periodic( void );
+static inline void main_event( void );
+
+static inline void on_gyro_accel_event(void);
+static inline void on_mag_event(void);
+
+static inline void main_on_overo_msg_received(void);
+static inline void main_on_overo_link_lost(void);
+
+static int16_t my_cnt;
+
+int main(void) {
+
+  main_init();
+
+  while (1) {
+    if (sys_time_periodic())
+      main_periodic();
+    main_event();
+  }
+  return 0;
+}
+
+static inline void main_init( void ) {
+  hw_init();
+  sys_time_init();
+  booz_imu_init();
+  overo_link_init();
+}
+
+static inline void main_periodic( void ) {
+  booz_imu_periodic();
+  OveroLinkPperiodic(main_on_overo_link_lost)
+  RunOnceEvery(10, {LED_PERIODIC(); DOWNLINK_SEND_ALIVE(DefaultChannel, 16, 
MD5SUM);});
+  
+}
+
+static inline void main_event( void ) {
+    BoozImuEvent(on_gyro_accel_event, on_mag_event);
+    OveroLinkEvent(main_on_overo_msg_received);
+}
+
+static inline void main_on_overo_msg_received(void) {
+  struct AutopilotMessageBethUp* msg_out = (struct 
AutopilotMessageBethUp*)overo_link.msg_out;
+  msg_out->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;
+  my_cnt++;
+}
+
+static inline void main_on_overo_link_lost(void) {
+  my_cnt = 0;
+}
+
+
+
+static inline void on_gyro_accel_event(void) {
+  BoozImuScaleGyro();
+  BoozImuScaleAccel();
+
+  LED_TOGGLE(2);
+  static uint8_t cnt;
+  cnt++;
+  if (cnt > 15) cnt = 0;
+
+  if (cnt == 0) {
+    DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,
+                              &booz_imu.gyro_unscaled.p,
+                              &booz_imu.gyro_unscaled.q,
+                              &booz_imu.gyro_unscaled.r);
+    
+    DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,
+                               &booz_imu.accel_unscaled.x,
+                               &booz_imu.accel_unscaled.y,
+                               &booz_imu.accel_unscaled.z);
+  }
+  else if (cnt == 7) {
+    DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel,
+                            &booz_imu.gyro.p,
+                            &booz_imu.gyro.q,
+                            &booz_imu.gyro.r);
+    
+    DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel,
+                             &booz_imu.accel.x,
+                             &booz_imu.accel.y,
+                             &booz_imu.accel.z);
+  }
+}
+
+
+static inline void on_mag_event(void) {
+  BoozImuScaleMag();
+  static uint8_t cnt;
+  cnt++;
+  if (cnt > 1) cnt = 0;
+
+  if (cnt%2) {
+    DOWNLINK_SEND_BOOZ2_MAG(DefaultChannel,
+                           &booz_imu.mag.x,
+                           &booz_imu.mag.y,
+                           &booz_imu.mag.z);
+  }
+  else {
+    DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,
+                             &booz_imu.mag_unscaled.x,
+                             &booz_imu.mag_unscaled.y,
+                             &booz_imu.mag_unscaled.z);
+  }
+}