[paparazzi-commits] [5027] First basic lisa passthrough and testprogram.

[Piotr Esden-Tempski]

Revision: 5027
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5027
Author:   esden
Date:     2010-07-08 07:52:29 +0000 (Thu, 08 Jul 2010)
Log Message:
-----------
First basic lisa passthrough and testprogram.

Changed fms structs to packed format.
Added first very basic stm32 passthrough code.
Added a small testprogram for overo picking up fms messages from the
stm32 acting as an IO processor.
Fixed fixed aliasing warnings triggered by struct casting. Now using
unions instead.

Use pt target on the stm32 and overo_test_passthrough on the overo.

Modified Paths:
--------------
    paparazzi3/trunk/conf/autopilot/lisa_test_progs.makefile
    paparazzi3/trunk/sw/airborne/fms/fms_autopilot_msg.h
    paparazzi3/trunk/sw/airborne/lisa/arch/stm32/lisa_overo_link_arch.c
    paparazzi3/trunk/sw/airborne/lisa/lisa_overo_link.h

Added Paths:
-----------
    paparazzi3/trunk/sw/airborne/fms/overo_test_passthrough.c
    paparazzi3/trunk/sw/airborne/lisa/lisa_stm_passthrough_main.c

Modified: paparazzi3/trunk/conf/autopilot/lisa_test_progs.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/lisa_test_progs.makefile    2010-07-08 
04:19:16 UTC (rev 5026)
+++ paparazzi3/trunk/conf/autopilot/lisa_test_progs.makefile    2010-07-08 
07:52:29 UTC (rev 5027)
@@ -40,7 +40,13 @@
 overo_test_spi_link.srcs  = $(SRC_FMS)/overo_test_spi_link.c
 overo_test_spi_link.srcs += $(SRC_FMS)/fms_spi_link.c
 
+# test passthrough spi link between overo and stm32
+overo_test_passthrough.ARCHDIR = omap
+overo_test_passthrough.CFLAGS  += -I$(ACINCLUDE) -I. 
-I$(PAPARAZZI_HOME)/var/include
+overo_test_passthrough.srcs  = $(SRC_FMS)/overo_test_passthrough.c
+overo_test_passthrough.srcs += $(SRC_FMS)/fms_spi_link.c
 
+# test network based telemetry on overo
 overo_test_telemetry.ARCHDIR  = omap
 overo_test_telemetry.CFLAGS  += -I$(ACINCLUDE) -I. 
-I$(PAPARAZZI_HOME)/var/include
 overo_test_telemetry.srcs     = $(SRC_FMS)/overo_test_telemetry.c
@@ -49,7 +55,7 @@
 overo_test_telemetry.srcs    += $(SRC_FMS)/fms_network.c
 overo_test_telemetry.LDFLAGS += -levent
 
-
+# test periodic tasks on the overo
 overo_test_periodic.ARCHDIR  = omap
 overo_test_periodic.CFLAGS  += -I$(ACINCLUDE) -I. 
-I$(PAPARAZZI_HOME)/var/include
 overo_test_periodic.srcs     = $(SRC_FMS)/overo_test_periodic.c
@@ -755,8 +761,6 @@
 stm_test_spi_link.CFLAGS += -DOVERO_LINK_LED_OK=3 -DOVERO_LINK_LED_KO=4 
-DUSE_DMA1_C2_IRQ
 stm_test_spi_link.srcs += lisa/lisa_overo_link.c 
lisa/arch/stm32/lisa_overo_link_arch.c
 
-
-
 #
 # test static
 #

Modified: paparazzi3/trunk/sw/airborne/fms/fms_autopilot_msg.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/fms_autopilot_msg.h        2010-07-08 
04:19:16 UTC (rev 5026)
+++ paparazzi3/trunk/sw/airborne/fms/fms_autopilot_msg.h        2010-07-08 
07:52:29 UTC (rev 5027)
@@ -8,14 +8,16 @@
  * Testing
  */
 
-struct AutopilotMessageFoo {
+struct __attribute__ ((packed)) AutopilotMessageFoo
+{
   uint8_t foo;
   uint8_t bar;
   uint8_t blaa;
   uint8_t bli;
 };
 
-union AutopilotMessageFoo1 {
+union AutopilotMessageFoo1
+{
   struct AutopilotMessageFoo up;
   struct AutopilotMessageFoo down;
 };
@@ -24,19 +26,22 @@
 /*
  * BETH
  */
-struct AutopilotMessageBethUp {
+struct __attribute__ ((packed)) AutopilotMessageBethUp
+{
   struct Int16Vect3 gyro;
   struct Int16Vect3 accel;
   struct Int16Vect3 bench_sensor;
 
 };
 
-struct AutopilotMessageBethDown {
+struct __attribute__ ((packed)) AutopilotMessageBethDown
+{
   uint8_t motor_front;
   uint8_t motor_back;
 };
 
-union AutopilotMessageBeth {
+union AutopilotMessageBeth
+{
   struct AutopilotMessageBethUp up;
   struct AutopilotMessageBethDown down;
 };
@@ -46,17 +51,20 @@
  *  STM Telemetry through wifi
  */
 #define TW_BUF_LEN 63
-struct AutopilotMessageTWUp {
+struct __attribute__ ((packed)) AutopilotMessageTWUp
+{
   uint8_t tw_len;
   uint8_t data[TW_BUF_LEN];
 };
 
-struct AutopilotMessageTWDown {
+struct __attribute__ ((packed)) AutopilotMessageTWDown
+{
   uint8_t tw_len;
   uint8_t data[TW_BUF_LEN];
 };
 
-union AutopilotMessageTW {
+union AutopilotMessageTW
+{
   struct AutopilotMessageTWUp up;
   struct AutopilotMessageTWDown down;
 };
@@ -64,7 +72,8 @@
 /*
  * Passthrough, aka biplan
  */
-struct AutopilotMessagePTUp {
+struct __attribute__ ((packed)) AutopilotMessagePTUp
+{
   struct Int16Vect3 gyro;
   struct Int16Vect3 accel;
   struct Int16Vect3 mag;
@@ -73,24 +82,21 @@
   int16_t rc_yaw;
   int16_t rc_thrust;
   int16_t rc_mode;
-  uint8_t  rc_status;
+  uint8_t rc_status;
 };
 
-struct AutopilotMessagePTDown {
+struct __attribute__ ((packed)) AutopilotMessagePTDown
+{
   int16_t command_pitch;
   int16_t command_roll;
   int16_t command_yaw;
   int16_t command_thrust;
 };
 
-union AutopilotMessagePT {
-  struct AutopilotMessageBethUp up;
-  struct AutopilotMessageBethDown down;
+union AutopilotMessagePT
+{
+  struct AutopilotMessagePTUp up;
+  struct AutopilotMessagePTDown down;
 };
 
-
-
-
-
-
 #endif /* FMS_AUTOPILOT_H */

Added: paparazzi3/trunk/sw/airborne/fms/overo_test_passthrough.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/overo_test_passthrough.c                   
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/overo_test_passthrough.c   2010-07-08 
07:52:29 UTC (rev 5027)
@@ -0,0 +1,72 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2010 Antoine Drouin <address@hidden>
+ * Copyright (C) 2010 Piotr Esden-Tempski <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 AutopilotMessagePT my_buffers[2];
+static struct AutopilotMessagePTUp*   msg_in  = &my_buffers[0].up;
+static struct AutopilotMessagePTDown* 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 AutopilotMessagePT), msg_in);
+       if (!(foo % 100)) {
+               printf("0x%08x -> gx%+06d gy%+06d gz%+06d ax%+06d ay%+06d 
az%+06d rs%02x\n",
+                       foo,
+                       msg_in->gyro.x, msg_in->gyro.y, msg_in->gyro.z,
+                       msg_in->accel.x, msg_in->accel.y, msg_in->accel.z,
+                       msg_in->rc_status);
+       }
+       foo++;
+}
+
+

Modified: paparazzi3/trunk/sw/airborne/lisa/arch/stm32/lisa_overo_link_arch.c
===================================================================
--- paparazzi3/trunk/sw/airborne/lisa/arch/stm32/lisa_overo_link_arch.c 
2010-07-08 04:19:16 UTC (rev 5026)
+++ paparazzi3/trunk/sw/airborne/lisa/arch/stm32/lisa_overo_link_arch.c 
2010-07-08 07:52:29 UTC (rev 5027)
@@ -58,7 +58,7 @@
   DMA_InitTypeDef  DMA_InitStructure;
   DMA_DeInit(DMA1_Channel2);
   DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)(SPI1_BASE+0x0C);
-  DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&(overo_link.msg_in);
+  DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)overo_link.msg_in.array;
   DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
   DMA_InitStructure.DMA_BufferSize = sizeof(overo_link.msg_in);
   DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
@@ -73,7 +73,7 @@
   /* SPI_SLAVE_Tx_DMA_Channel configuration 
------------------------------------*/
   DMA_DeInit(DMA1_Channel3);
   DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)(SPI1_BASE+0x0C);
-  DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&(overo_link.msg_out);
+  DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)overo_link.msg_out.array;
   DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;
   DMA_InitStructure.DMA_Priority = DMA_Priority_Medium;
   DMA_Init(DMA1_Channel3, &DMA_InitStructure);

Modified: paparazzi3/trunk/sw/airborne/lisa/lisa_overo_link.h
===================================================================
--- paparazzi3/trunk/sw/airborne/lisa/lisa_overo_link.h 2010-07-08 04:19:16 UTC 
(rev 5026)
+++ paparazzi3/trunk/sw/airborne/lisa/lisa_overo_link.h 2010-07-08 07:52:29 UTC 
(rev 5027)
@@ -10,10 +10,16 @@
 #define OVERO_LINK_TIMEOUT 10
 
 struct LisaOveroLink {
-  volatile uint8_t status;
-  uint8_t msg_in[sizeof(union OVERO_LINK_MSG_UNION)];
-  uint8_t msg_out[sizeof(union OVERO_LINK_MSG_UNION)];
-  uint8_t timeout;
+       volatile uint8_t status;
+       union {
+               union OVERO_LINK_MSG_UNION uni;
+               uint8_t array[sizeof(union OVERO_LINK_MSG_UNION)];
+       } msg_in;
+       union {
+               union OVERO_LINK_MSG_UNION uni;
+               uint8_t array[sizeof(union OVERO_LINK_MSG_UNION)];
+       } msg_out;
+       uint8_t timeout;
 };
 
 extern struct LisaOveroLink overo_link;

Added: paparazzi3/trunk/sw/airborne/lisa/lisa_stm_passthrough_main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/lisa/lisa_stm_passthrough_main.c               
                (rev 0)
+++ paparazzi3/trunk/sw/airborne/lisa/lisa_stm_passthrough_main.c       
2010-07-08 07:52:29 UTC (rev 5027)
@@ -0,0 +1,147 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010 Piotr Esden-Tempski <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 "init_hw.h"
+#include "sys_time.h"
+#include "downlink.h"
+#include "booz/booz2_commands.h"
+#include "booz/booz_actuators.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);
+
+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();
+       actuators_init();
+       booz_imu_init();
+       overo_link_init();
+}
+
+static inline void main_periodic(void) {
+
+       booz_imu_periodic();
+       actuators_set(FALSE);
+       OveroLinkPeriodic(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 AutopilotMessagePTUp *msg_out = &overo_link.msg_out.uni.up;
+
+       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->rc_status++;
+}
+
+static inline void main_on_overo_link_lost(void) {
+
+}
+
+static inline void on_gyro_accel_event(void) {
+       static uint8_t cnt;
+
+       BoozImuScaleGyro();
+       BoozImuScaleAccel();
+
+       LED_TOGGLE(2);
+       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) {
+       static uint8_t cnt;
+
+       BoozImuScaleMag();
+
+       cnt++;
+       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);
+       }
+}
+