diff --git a/conf/airframes/ENAC/fixed-wing/merlin.xml b/conf/airframes/ENAC/fixed-wing/merlin.xml
index 11d9c086df..f42d074242 100644
--- a/conf/airframes/ENAC/fixed-wing/merlin.xml
+++ b/conf/airframes/ENAC/fixed-wing/merlin.xml
@@ -23,6 +23,11 @@
+
+
@@ -32,6 +37,7 @@
+
diff --git a/conf/modules/ins_vn100.xml b/conf/modules/ins_vn100.xml
index 0c6fbe3df1..69722d3524 100644
--- a/conf/modules/ins_vn100.xml
+++ b/conf/modules/ins_vn100.xml
@@ -3,18 +3,20 @@
VectorNav VN100 (SPI)
+
+
-
-
-
-
+
+
+
+
-
+
diff --git a/sw/airborne/modules/ins/fw_ins_vn100.c b/sw/airborne/modules/ins/fw_ins_vn100.c
deleted file mode 100644
index 4ad331fc1a..0000000000
--- a/sw/airborne/modules/ins/fw_ins_vn100.c
+++ /dev/null
@@ -1,126 +0,0 @@
-/*
- * Copyright (C) 2010 ENAC
- *
- * 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.
- *
- */
-
-/**
- * \brief driver for the VectorNav VN100 (Fixed-Wing part)
- */
-
-#include "modules/ins/ins_vn100.h"
-#include "mcu_periph/spi.h"
-#include "state.h"
-#include "generated/airframe.h"
-
-#ifndef INS_YAW_NEUTRAL_DEFAULT
-#define INS_YAW_NEUTRAL_DEFAULT 0.
-#endif
-
-void ins_init( void ) {
-
- ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
- ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
- ins_yaw_neutral = INS_YAW_NEUTRAL_DEFAULT;
-
- /* SPI polarity = 1 - data sampled on rising edge */
- SpiSetCPOL();
- /* SPI phase = 1 - SCK idle high */
- SpiSetCPHA();
-
- ins_ador = VN100_ADOR;
- ins_adof = VN100_ADOF;
- ins_baud = VN100_BAUD;
-
- ins_init_status = INS_VN100_SET_BAUD;
-
-}
-
-static inline bool_t ins_configure( void ) {
- switch (ins_init_status) {
- case INS_VN100_SET_BAUD :
- last_send_packet.RegID = VN100_REG_SBAUD;
- spi_buffer_length = 4+VN100_REG_SBAUD_SIZE;
- ins_init_status++;
- break;
- case INS_VN100_SET_ADOR :
- last_send_packet.RegID = VN100_REG_ADOR;
- spi_buffer_length = 4+VN100_REG_ADOR_SIZE;
- ins_init_status++;
- break;
- case INS_VN100_SET_ADOF :
- last_send_packet.RegID = VN100_REG_ADOF;
- spi_buffer_length = 4+VN100_REG_ADOF_SIZE;
- ins_init_status++;
- break;
- case INS_VN100_READY :
- return TRUE;
- }
- last_send_packet.CmdID = VN100_CmdID_WriteRegister;
- spi_buffer_input = (uint8_t*)&last_received_packet;
- spi_buffer_output = (uint8_t*)&last_send_packet;
- SpiSelectSlave0();
- SpiStart();
- return FALSE;
-}
-
-void ins_periodic( void ) {
- if (!SpiCheckAvailable()) {
- SpiOverRun();
- return;
- }
-
- if (!ins_configure()) return;
-
- // Fill request for QMR
- last_send_packet.CmdID = VN100_CmdID_ReadRegister;
- last_send_packet.RegID = VN100_REG_YMR;
-
- spi_buffer_input = (uint8_t*)&last_received_packet;
- spi_buffer_output = (uint8_t*)&last_send_packet;
- spi_buffer_length = 4+VN100_REG_YMR_SIZE;
- SpiSelectSlave0();
- SpiStart();
-
-}
-
-#include "mcu_periph/uart.h"
-#include "messages.h"
-#include "subsystems/datalink/downlink.h"
-
-void ins_event_task( void ) {
- if (spi_message_received) {
- spi_message_received = FALSE;
- parse_ins_msg();
-#ifndef INS_VN100_READ_ONLY
- // Update estimator
- // FIXME Use a proper rotation matrix here
- struct FloatEulers att = {
- ins_eulers.phi - ins_roll_neutral,
- ins_eulers.theta - ins_pitch_neutral,
- ins_eulers.psi
- };
- stateSetNedToBodyEulers_f(&att);
- stateSetBodyRates(&ins_rates);
-#endif
- //uint8_t s = 4+VN100_REG_QMR_SIZE;
- //DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,s,spi_buffer_input);
- }
-}
-
diff --git a/sw/airborne/modules/ins/ins_vn100.c b/sw/airborne/modules/ins/ins_vn100.c
index 2b8c556df6..45ed7059af 100644
--- a/sw/airborne/modules/ins/ins_vn100.c
+++ b/sw/airborne/modules/ins/ins_vn100.c
@@ -24,14 +24,31 @@
* \brief driver for the VectorNav VN100
*/
-#include "ins_vn100.h"
+#include "modules/ins/ins_vn100.h"
#include "generated/airframe.h"
-#include "led.h"
+#include "mcu_periph/spi.h"
+#include "state.h"
+// for telemetry report
+#include "mcu_periph/uart.h"
#include "subsystems/datalink/downlink.h"
#include "messages.h"
+#ifndef INS_YAW_NEUTRAL_DEFAULT
+#define INS_YAW_NEUTRAL_DEFAULT 0.
+#endif
+
+// default spi device
+#ifndef VN100_SPI_DEV
+#define VN100_SPI_DEV spi1
+#endif
+
+// default slave number
+#ifndef VN100_SLAVE_IDX
+#define VN100_SLAVE_IDX 0
+#endif
+
/* neutrals */
float ins_roll_neutral;
float ins_pitch_neutral;
@@ -58,9 +75,110 @@ uint32_t ins_baud;
uint8_t ins_init_status;
-/* ins_init and ins_periodic to be implemented according to the airframe type : FW or BOOZ */
+// parsing function
+static inline void parse_ins_msg( void );
-void parse_ins_msg( void ) {
+/* spi transaction */
+struct spi_transaction vn100_trans;
+
+/* init vn100 */
+void vn100_init( void ) {
+
+ //ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
+ //ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
+ ins_yaw_neutral = INS_YAW_NEUTRAL_DEFAULT;
+
+ vn100_trans.slave_idx = VN100_SLAVE_IDX;
+ vn100_trans.cpol = SPICpolIdleHigh;
+ vn100_trans.cpha = SPICphaEdge2;
+ vn100_trans.dss = SPIDss8bit;
+ vn100_trans.select = SPISelectUnselect;
+ vn100_trans.output_buf = (uint8_t*)&last_send_packet;
+ vn100_trans.input_buf = (uint8_t*)&last_received_packet;
+ vn100_trans.status = SPITransDone;
+
+ ins_ador = VN100_ADOR;
+ ins_adof = VN100_ADOF;
+ ins_baud = VN100_BAUD;
+
+ ins_init_status = INS_VN100_SET_BAUD;
+
+}
+
+static inline bool_t ins_configure( void ) {
+ // nothing to receive during conf
+ vn100_trans.input_length = 0;
+
+ switch (ins_init_status) {
+ case INS_VN100_SET_BAUD :
+ last_send_packet.RegID = VN100_REG_SBAUD;
+ vn100_trans.output_length = 4+VN100_REG_SBAUD_SIZE;
+ ins_init_status++;
+ break;
+ case INS_VN100_SET_ADOR :
+ last_send_packet.RegID = VN100_REG_ADOR;
+ vn100_trans.output_length = 4+VN100_REG_ADOR_SIZE;
+ ins_init_status++;
+ break;
+ case INS_VN100_SET_ADOF :
+ last_send_packet.RegID = VN100_REG_ADOF;
+ vn100_trans.output_length = 4+VN100_REG_ADOF_SIZE;
+ ins_init_status++;
+ break;
+ case INS_VN100_READY :
+ return TRUE;
+ }
+ last_send_packet.CmdID = VN100_CmdID_WriteRegister;
+
+ spi_submit(&(VN100_SPI_DEV),&vn100_trans);
+
+ return FALSE;
+}
+
+void vn100_periodic_task( void ) {
+
+ // only send config or request when last transaction is done
+ if (vn100_trans.status != SPITransDone) { return; }
+
+ // send request when configuration is done
+ if (ins_configure() == TRUE) {
+ // Fill request for QMR
+ last_send_packet.CmdID = VN100_CmdID_ReadRegister;
+ last_send_packet.RegID = VN100_REG_YMR;
+ // Set IO length
+ vn100_trans.output_length = 2; // Only 2 ?
+ vn100_trans.input_length = 4+VN100_REG_YMR_SIZE;
+ // submit
+ spi_submit(&(VN100_SPI_DEV),&vn100_trans);
+ }
+
+}
+
+void vn100_event_task( void ) {
+ if (vn100_trans.status == SPITransSuccess) {
+ parse_ins_msg();
+#ifndef INS_VN100_READ_ONLY
+ // Update estimator
+ // FIXME Use a proper rotation matrix here
+ struct FloatEulers att = {
+ ins_eulers.phi - ins_roll_neutral,
+ ins_eulers.theta - ins_pitch_neutral,
+ ins_eulers.psi
+ };
+ stateSetNedToBodyEulers_f(&att);
+ stateSetBodyRates_f(&ins_rates);
+#endif
+ //uint8_t s = 4+VN100_REG_QMR_SIZE;
+ //DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,s,spi_buffer_input);
+ vn100_trans.status = SPITransDone;
+ }
+ if (vn100_trans.status == SPITransFailed) {
+ vn100_trans.status = SPITransDone;
+ // FIXME retry config if not done ?
+ }
+}
+
+static inline void parse_ins_msg( void ) {
if (last_received_packet.ErrID != VN100_Error_None) {
//TODO send error
return;
@@ -175,6 +293,8 @@ void parse_ins_msg( void ) {
ins_rates.q = last_received_packet.Data[10].Float;
ins_rates.r = last_received_packet.Data[11].Float;
break;
+ default:
+ break;
}
}
@@ -186,7 +306,7 @@ void parse_ins_msg( void ) {
#include "messages.h"
#include "subsystems/datalink/downlink.h"
-extern void ins_report_task( void ) {
+extern void vn100_report_task( void ) {
DOWNLINK_SEND_AHRS_LKF(DefaultChannel, DefaultDevice,
&ins_eulers.phi, &ins_eulers.theta, &ins_eulers.psi,
&ins_quat.qi, &ins_quat.qx, &ins_quat.qy, &ins_quat.qz,
@@ -194,3 +314,4 @@ extern void ins_report_task( void ) {
&ins_accel.x, &ins_accel.y, &ins_accel.z,
&ins_mag.x, &ins_mag.y, &ins_mag.z);
}
+
diff --git a/sw/airborne/modules/ins/ins_vn100.h b/sw/airborne/modules/ins/ins_vn100.h
index 5e99dc4c6a..c11a4d2351 100644
--- a/sw/airborne/modules/ins/ins_vn100.h
+++ b/sw/airborne/modules/ins/ins_vn100.h
@@ -48,10 +48,10 @@ extern struct FloatVect3 ins_mag;
extern volatile uint8_t ins_msg_received;
-extern void ins_init( void );
-extern void ins_event_task( void );
-extern void ins_report_task( void );
-extern void parse_ins_msg( void );
+extern void vn100_init( void );
+extern void vn100_periodic_task( void );
+extern void vn100_event_task( void );
+extern void vn100_report_task( void );
/* last received SPI packet */
extern VN100_Res_Packet last_received_packet;