[paparazzi-commits] [5037] Add fms_spi_autopilot_msg.[ch] for parsing data from stm32 via SPI

[Allen Ibara]

Revision: 5037
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5037
Author:   aibara
Date:     2010-07-09 05:56:44 +0000 (Fri, 09 Jul 2010)
Log Message:
-----------
Add fms_spi_autopilot_msg.[ch] for parsing data from stm32 via SPI

Added Paths:
-----------
    paparazzi3/trunk/sw/airborne/fms/fms_spi_autopilot_msg.c
    paparazzi3/trunk/sw/airborne/fms/fms_spi_autopilot_msg.h

Added: paparazzi3/trunk/sw/airborne/fms/fms_spi_autopilot_msg.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/fms_spi_autopilot_msg.c                    
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/fms_spi_autopilot_msg.c    2010-07-09 
05:56:44 UTC (rev 5037)
@@ -0,0 +1,157 @@
+/*
+ * $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"
+#include "fms_spi_autopilot_msg.h"
+
+/* all these for telemetry */
+#include "messages.h"
+#include "downlink.h"
+#include "udp_transport.h"
+#include "fms/fms_network.h"
+#include "ready_main.h"
+
+#include "airframe.h"
+#include "actuators.h"
+#include "rdyb_booz_imu.h"
+#include "booz_radio_control.h"
+#include "rdyb_mahrs.h"
+
+static struct BoozImuFloat imu;
+
+static void (* vane_callback)(uint8_t vane_id, float alpha, float beta) = NULL;
+static void (* pressure_callback)(uint8_t pressure_id, uint32_t pressure1, 
uint32_t pressure2) = NULL;
+static void (* radio_control_callback)(void) = NULL;
+
+void spi_ap_link_downlink_send()
+{
+  DOWNLINK_SEND_EKF7_Y(DefaultChannel, &imu.accel.x, &imu.accel.y, 
&imu.accel.z,
+                   &imu.mag.x, &imu.mag.y, &imu.mag.z,
+                   &imu.gyro.p, &imu.gyro.q, &imu.gyro.r);
+}
+
+void spi_ap_link_set_vane_callback(void (* vane_cb)(uint8_t vane_id, float 
alpha, float beta))
+{
+  vane_callback = vane_cb;
+}
+
+void spi_ap_link_set_pressure_callback(void (* pressure_cb)(uint8_t 
pressure_id, uint32_t pressure1, uint32_t pressure2))
+{
+  pressure_callback = pressure_cb;
+}
+
+void spi_ap_link_set_radio_control_callback(void (* radio_control_cb)(void))
+{
+  radio_control_callback = radio_control_cb;
+}
+
+int spi_ap_link_init()
+{
+  if (spi_link_init()) {
+    TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n");
+    return -1;
+  }
+
+  // Initialize IMU->Body orientation
+  imu.body_to_imu_eulers.phi = 0;
+  imu.body_to_imu_eulers.theta = 0;
+  imu.body_to_imu_eulers.psi = 0;
+
+  FLOAT_QUAT_OF_EULERS(imu.body_to_imu_quat, imu.body_to_imu_eulers);
+  FLOAT_QUAT_NORMALISE(imu.body_to_imu_quat);
+  FLOAT_RMAT_OF_QUAT(imu.body_to_imu_rmat, imu.body_to_imu_quat);
+
+  struct FloatRates bias0 = { 0., 0., 0.};
+  rdyb_mahrs_init(imu.body_to_imu_quat, bias0);
+
+  return 0;
+}
+
+static void passthrough_up_parse(struct AutopilotMessagePTUp *msg_up)
+{
+  // FIXME: Should only callback when there is something new
+  // FIXME: placeholders since the vane and pressure data fields don't exist 
yet
+  if (vane_callback)
+    vane_callback(0, 0., 0.);
+  if (pressure_callback)
+    pressure_callback(0, 0, 0);
+
+  // Fill radio data
+  radio_control.values[RADIO_CONTROL_ROLL] = msg_up->rc_roll;
+  radio_control.values[RADIO_CONTROL_PITCH] = msg_up->rc_pitch;
+  radio_control.values[RADIO_CONTROL_YAW] = msg_up->rc_yaw;
+  radio_control.values[RADIO_CONTROL_THROTTLE] = msg_up->rc_thrust;
+  radio_control.values[RADIO_CONTROL_MODE] = msg_up->rc_mode;
+  radio_control.values[RADIO_CONTROL_KILL] = msg_up->rc_kill;
+  radio_control.values[RADIO_CONTROL_GEAR] = msg_up->rc_gear;
+  radio_control.values[RADIO_CONTROL_AUX3] = msg_up->rc_aux3;
+  radio_control.values[RADIO_CONTROL_AUX4] = msg_up->rc_aux4;
+  radio_control.status = msg_up->rc_status;
+  
+  if (radio_control_callback)
+    radio_control_callback();
+
+  // Fill IMU data
+  imu.gyro.p = RATE_FLOAT_OF_BFP(msg_up->gyro.x);
+  imu.gyro.q = RATE_FLOAT_OF_BFP(msg_up->gyro.y);
+  imu.gyro.r = RATE_FLOAT_OF_BFP(msg_up->gyro.z);
+
+  imu.accel.x = ACCEL_FLOAT_OF_BFP(msg_up->accel.x);
+  imu.accel.y = ACCEL_FLOAT_OF_BFP(msg_up->accel.y);
+  imu.accel.z = ACCEL_FLOAT_OF_BFP(msg_up->accel.z);
+
+  imu.mag.x = MAG_FLOAT_OF_BFP(msg_up->mag.x);
+  imu.mag.y = MAG_FLOAT_OF_BFP(msg_up->mag.y);
+  imu.mag.z = MAG_FLOAT_OF_BFP(msg_up->mag.z);
+
+  rdyb_booz_imu_update(&imu);
+}
+
+static void passthrough_down_fill(struct AutopilotMessagePTDown *msg_out)
+{
+  for (int i = 0; i < SERVOS_NB; i++) {
+    msg_out->actuators[i] = actuators[i];
+  }
+}
+
+void spi_ap_link_periodic()
+{
+  struct AutopilotMessagePTUp msg_in;
+  struct AutopilotMessagePTDown msg_out;
+
+  passthrough_down_fill(&msg_out);
+
+  // SPI transcieve
+  spi_link_send(&msg_out, sizeof(union AutopilotMessagePT), &msg_in);
+
+  passthrough_up_parse(&msg_in);
+}

Added: paparazzi3/trunk/sw/airborne/fms/fms_spi_autopilot_msg.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/fms_spi_autopilot_msg.h                    
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/fms_spi_autopilot_msg.h    2010-07-09 
05:56:44 UTC (rev 5037)
@@ -0,0 +1,33 @@
+/*
+ * $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.
+ */
+
+int spi_ap_link_init(void);
+void spi_ap_link_set_vane_callback(void (* vane_cb)(uint8_t vane_id, float 
alpha, float beta));
+void spi_ap_link_set_pressure_callback(void (* pressure_cb)(uint8_t 
pressure_id, uint32_t pressure1, uint32_t pressure2));
+void spi_ap_link_set_radio_control_callback(void (* radio_control_cb)(void));
+void spi_ap_link_periodic(void);
+void spi_ap_link_downlink_send(void);
+
+#ifdef USE_SPI_LINK
+#define PERIODIC_SEND_EKF7_Y(_chan) spi_ap_link_downlink_send();
+#endif