[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [5037] Add fms_spi_autopilot_msg.[ch] for parsing da
From: |
Allen Ibara |
Subject: |
[paparazzi-commits] [5037] Add fms_spi_autopilot_msg.[ch] for parsing data from stm32 via SPI |
Date: |
Fri, 09 Jul 2010 05:56:44 +0000 |
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
[Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [5037] Add fms_spi_autopilot_msg.[ch] for parsing data from stm32 via SPI,
Allen Ibara <=