From 7d886a29bc577fa2cd6d8b1535870fcfefbedbd5 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 27 Nov 2012 23:21:16 +0100 Subject: [PATCH] [vn100] adapt vn100 to new spi driver (untested) --- conf/airframes/ENAC/fixed-wing/merlin.xml | 6 + conf/modules/ins_vn100.xml | 12 +- sw/airborne/modules/ins/fw_ins_vn100.c | 126 --------------------- sw/airborne/modules/ins/ins_vn100.c | 131 +++++++++++++++++++++- sw/airborne/modules/ins/ins_vn100.h | 8 +- 5 files changed, 143 insertions(+), 140 deletions(-) delete mode 100644 sw/airborne/modules/ins/fw_ins_vn100.c 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;