diff --git a/conf/modules/ins_vn100.xml b/conf/modules/ins_vn100.xml new file mode 100644 index 0000000000..913c394448 --- /dev/null +++ b/conf/modules/ins_vn100.xml @@ -0,0 +1,17 @@ + + + + +
+ +
+ + + + + + + + +
+ diff --git a/sw/airborne/modules/ins/VN100.h b/sw/airborne/modules/ins/VN100.h new file mode 100644 index 0000000000..fd39aa1b34 --- /dev/null +++ b/sw/airborne/modules/ins/VN100.h @@ -0,0 +1,204 @@ +/* + * Paparazzi $Id: $ + * + * 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 Library for the VectorNav VN100 AHRS + * based on VN_FWLIB from vector nav + */ + +#ifndef VN100_H +#define VN100_H + +#include "std.h" + +/* VN-100 Registers */ +#define VN100_REG_MODEL 1 +#define VN100_REG_HWREV 2 +#define VN100_REG_SN 3 +#define VN100_REG_FWVER 4 +#define VN100_REG_SBAUD 5 +#define VN100_REG_ADOR 6 +#define VN100_REG_ADOF 7 +#define VN100_REG_YPR 8 +#define VN100_REG_QTN 9 +#define VN100_REG_QTM 10 +#define VN100_REG_QTA 11 +#define VN100_REG_QTR 12 +#define VN100_REG_QMA 13 +#define VN100_REG_QAR 14 +#define VN100_REG_QMR 15 +#define VN100_REG_DCM 16 +#define VN100_REG_MAG 17 +#define VN100_REG_ACC 18 +#define VN100_REG_GYR 19 +#define VN100_REG_MAR 20 +#define VN100_REG_REF 21 +#define VN100_REG_SIG 22 +#define VN100_REG_HSI 23 +#define VN100_REG_ATP 24 +#define VN100_REG_ACT 25 +#define VN100_REG_RFR 26 +#define VN100_REG_YMR 27 +#define VN100_REG_ACG 28 + +/* Data Size */ +#define VN100_REG_MODEL_SIZE 12 +#define VN100_REG_HWREV_SIZE 4 +#define VN100_REG_SN_SIZE 12 +#define VN100_REG_FWVER_SIZE 4 +#define VN100_REG_SBAUD_SIZE 4 +#define VN100_REG_ADOR_SIZE 4 +#define VN100_REG_ADOF_SIZE 4 +#define VN100_REG_YPR_SIZE (3*4) +#define VN100_REG_QTN_SIZE (4*4) +#define VN100_REG_QTM_SIZE (7*4) +#define VN100_REG_QTA_SIZE (7*4) +#define VN100_REG_QTR_SIZE (7*4) +#define VN100_REG_QMA_SIZE (10*4) +#define VN100_REG_QAR_SIZE (10*4) +#define VN100_REG_QMR_SIZE (13*4) +#define VN100_REG_DCM_SIZE (9*4) +#define VN100_REG_MAG_SIZE (3*4) +#define VN100_REG_ACC_SIZE (3*4) +#define VN100_REG_GYR_SIZE (3*4) +#define VN100_REG_MAR_SIZE (9*4) +#define VN100_REG_REF_SIZE (6*4) +#define VN100_REG_SIG_SIZE (10*4) +#define VN100_REG_HSI_SIZE (12*4) +#define VN100_REG_ATP_SIZE (4*4) +#define VN100_REG_ACT_SIZE (12*4) +#define VN100_REG_RFR_SIZE (9*4) +#define VN100_REG_YMR_SIZE (12*4) +#define VN100_REG_ACG_SIZE 4 + +#define VN100_DataSizeOfReg(_r) (_r##_SIZE) + +/* Command IDs */ +#define VN100_CmdID_ReadRegister 0x01 +#define VN100_CmdID_WriteRegister 0x02 +#define VN100_CmdID_WriteSettings 0x03 +#define VN100_CmdID_RestoreFactorySettings 0x04 +#define VN100_CmdID_Tare 0x05 +#define VN100_CmdID_Reset 0x06 + +/* System Error */ +#define VN100_Error_None 0 +#define VN100_Error_HardFaultException 1 +#define VN100_Error_InputBufferOverflow 2 +#define VN100_Error_InvalidChecksum 3 +#define VN100_Error_InvalidCommand 4 +#define VN100_Error_NotEnoughParameters 5 +#define VN100_Error_TooManyParameters 6 +#define VN100_Error_InvalidParameter 7 +#define VN100_Error_InvalidRegister 8 +#define VN100_Error_UnauthorizedAccess 9 +#define VN100_Error_WatchdogReset 10 +#define VN100_Error_OutputBufferOverflow 11 +#define VN100_Error_InsufficientBandwidth 12 + +/* Asynchronous Data Output Register */ +#define VN100_ADOR_OFF 0 +#define VN100_ADOR_YPR 1 +#define VN100_ADOR_QTN 2 +#define VN100_ADOR_QTM 3 +#define VN100_ADOR_QTA 4 +#define VN100_ADOR_QTR 5 +#define VN100_ADOR_QMA 6 +#define VN100_ADOR_QAR 7 +#define VN100_ADOR_QMR 8 +#define VN100_ADOR_DCM 9 +#define VN100_ADOR_MAG 10 +#define VN100_ADOR_ACC 11 +#define VN100_ADOR_GYR 12 +#define VN100_ADOR_MAR 13 +#define VN100_ADOR_YMR 14 +#define VN100_ADOR_RAB 251 +#define VN100_ADOR_RAW 252 +#define VN100_ADOR_CMV 253 +#define VN100_ADOR_STV 254 +#define VN100_ADOR_COV 255 + +/* Asynchronous Data Ouput Rate Register */ +#define VN100_ADOF_1HZ 1 +#define VN100_ADOF_2HZ 2 +#define VN100_ADOF_4HZ 4 +#define VN100_ADOF_5HZ 5 +#define VN100_ADOF_10HZ 10 +#define VN100_ADOF_20HZ 20 +#define VN100_ADOF_25HZ 25 +#define VN100_ADOF_40HZ 40 +#define VN100_ADOF_50HZ 50 +#define VN100_ADOF_100HZ 100 +#define VN100_ADOF_200HZ 200 + +/* Serial Baud Rate Register */ +#define VN100_Baud_9600 9600 +#define VN100_Baud_19200 19200 +#define VN100_Baud_38400 38400 +#define VN100_Baud_57600 57600 +#define VN100_Baud_115200 115200 +#define VN100_Baud_128000 128000 +#define VN100_Baud_230400 230400 +#define VN100_Baud_460800 460800 +#define VN100_Baud_921600 921600 + +/* Accelerometer Gain Type */ +#define VN100_AccGain_2G 0 +#define VN100_AccGain_6G 1 + +/* 32-bit Parameter Type */ +typedef union { + uint32_t UInt; + float Float; +} VN100_Param; + +/* SPI Buffer size */ +#define VN100_SPI_BUFFER_SIZE 48 + +/* SPI Request Packet */ +typedef struct { + uint8_t CmdID; + uint8_t RegID; + uint8_t ZeroByte1; + uint8_t ZeroByte2; + VN100_Param Data[VN100_SPI_BUFFER_SIZE]; +} VN100_Req_Packet; + +/* SPI Response Packet */ +typedef struct { + uint8_t ZeroByte; + uint8_t CmdID; + uint8_t RegID; + uint8_t ErrID; + VN100_Param Data[VN100_SPI_BUFFER_SIZE]; +} VN100_Res_Packet; + +#define VN100_Packet_SetBaud(_b) { VN100_CmdID_WriteRegister, VN100_REG_SBAUD, 0, 0, { _b } } +#define VN100_Packet_SetADOR(_r) { VN100_CmdID_WriteRegister, VN100_REG_ADOR, 0, 0, { _r } } +#define VN100_Packet_SetaDOF(_f) { VN100_CmdID_WriteRegister, VN100_REG_ADOF, 0, 0, { _f } } + +#define VN100_BytesOfWord(_w) { (uint8_t)(_w & 0xFF), (uint8_t)((_w & (0xFF<<8))>>8), (uint8_t)((_w & (0xFF<<16))>>16), (uint8_t)((_w & (0xFF<<24))>>24) } +#define VN100_WordOfBytes(_b) (((uint32_t)(_b[3])<<24)|((uint32_t)(_b[2])<<16)|((uint16_t)(_b[1])<<8)|(uint16_t)(_b[0])) + +#endif diff --git a/sw/airborne/modules/ins/fw_ins_vn100.c b/sw/airborne/modules/ins/fw_ins_vn100.c new file mode 100644 index 0000000000..bb1147c591 --- /dev/null +++ b/sw/airborne/modules/ins/fw_ins_vn100.c @@ -0,0 +1,107 @@ +/* + * Paparazzi $Id: ins_xsens.c 3872 2009-08-05 14:42:41Z mmm $ + * + * 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 "ins_vn100.h" +#include "spi.h" + +void ins_init( void ) { + + /* 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_task( 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 "uart.h" +#include "messages.h" +#include "downlink.h" + +void ins_event_task( void ) { + if (spi_message_received) { + spi_message_received = FALSE; + parse_ins_msg(); + //uint8_t s = 4+VN100_REG_QMR_SIZE; + //DOWNLINK_SEND_DEBUG(DefaultChannel,s,spi_buffer_input); + } +} + diff --git a/sw/airborne/modules/ins/ins_vn100.c b/sw/airborne/modules/ins/ins_vn100.c new file mode 100644 index 0000000000..016c9ad4d8 --- /dev/null +++ b/sw/airborne/modules/ins/ins_vn100.c @@ -0,0 +1,190 @@ +/* + * Paparazzi $Id: ins_xsens.c 3872 2009-08-05 14:42:41Z mmm $ + * + * 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 + */ + +#include "ins_vn100.h" + +#include "airframe.h" +#include "led.h" + +#include "downlink.h" +#include "messages.h" + +struct FloatEulers ins_eulers; +struct FloatQuat ins_quat; +struct FloatRates ins_rates; +struct FloatRMat ins_rmat; +struct FloatVect3 ins_accel; +struct FloatVect3 ins_mag; + +volatile uint8_t ins_msg_received; + +/* last received SPI packet */ +VN100_Res_Packet last_received_packet; +/* last send packet */ +VN100_Req_Packet last_send_packet; + +/* output mode */ +uint32_t ins_ador; +uint32_t ins_adof; +uint32_t ins_baud; + +uint8_t ins_init_status; + +/* ins_init and ins_periodic_task to be implemented according to the airframe type : FW or BOOZ */ + +void parse_ins_msg( void ) { + if (last_received_packet.ErrID != VN100_Error_None) { + //TODO send error + return; + } + + // parse message (will work only with read and write register) + switch (last_received_packet.RegID) { + case VN100_REG_ADOR : + ins_ador = last_received_packet.Data[0].UInt; + break; + case VN100_REG_ADOF : + ins_adof = last_received_packet.Data[0].UInt; + break; + case VN100_REG_SBAUD : + ins_baud = last_received_packet.Data[0].UInt; + break; + case VN100_REG_YPR : + ins_eulers.phi = RadOfDeg(last_received_packet.Data[2].Float); + ins_eulers.theta = RadOfDeg(last_received_packet.Data[1].Float); + ins_eulers.psi = RadOfDeg(last_received_packet.Data[0].Float); + break; + case VN100_REG_QTN : + ins_quat.qi = last_received_packet.Data[0].Float; + ins_quat.qx = last_received_packet.Data[1].Float; + ins_quat.qy = last_received_packet.Data[2].Float; + ins_quat.qz = last_received_packet.Data[3].Float; + FLOAT_EULERS_OF_QUAT(ins_eulers, ins_quat); + break; + case VN100_REG_QTM : + ins_quat.qi = last_received_packet.Data[0].Float; + ins_quat.qx = last_received_packet.Data[1].Float; + ins_quat.qy = last_received_packet.Data[2].Float; + ins_quat.qz = last_received_packet.Data[3].Float; + FLOAT_EULERS_OF_QUAT(ins_eulers, ins_quat); + ins_mag.x = last_received_packet.Data[4].Float; + ins_mag.y = last_received_packet.Data[5].Float; + ins_mag.z = last_received_packet.Data[6].Float; + break; + case VN100_REG_QTA : + ins_quat.qi = last_received_packet.Data[0].Float; + ins_quat.qx = last_received_packet.Data[1].Float; + ins_quat.qy = last_received_packet.Data[2].Float; + ins_quat.qz = last_received_packet.Data[3].Float; + FLOAT_EULERS_OF_QUAT(ins_eulers, ins_quat); + ins_accel.x = last_received_packet.Data[4].Float; + ins_accel.y = last_received_packet.Data[5].Float; + ins_accel.z = last_received_packet.Data[6].Float; + break; + case VN100_REG_QTR : + ins_quat.qi = last_received_packet.Data[0].Float; + ins_quat.qx = last_received_packet.Data[1].Float; + ins_quat.qy = last_received_packet.Data[2].Float; + ins_quat.qz = last_received_packet.Data[3].Float; + FLOAT_EULERS_OF_QUAT(ins_eulers, ins_quat); + ins_rates.p = last_received_packet.Data[4].Float; + ins_rates.q = last_received_packet.Data[5].Float; + ins_rates.r = last_received_packet.Data[6].Float; + break; + case VN100_REG_QMA : + ins_quat.qi = last_received_packet.Data[0].Float; + ins_quat.qx = last_received_packet.Data[1].Float; + ins_quat.qy = last_received_packet.Data[2].Float; + ins_quat.qz = last_received_packet.Data[3].Float; + FLOAT_EULERS_OF_QUAT(ins_eulers, ins_quat); + ins_mag.x = last_received_packet.Data[4].Float; + ins_mag.y = last_received_packet.Data[5].Float; + ins_mag.z = last_received_packet.Data[6].Float; + ins_accel.x = last_received_packet.Data[7].Float; + ins_accel.y = last_received_packet.Data[8].Float; + ins_accel.z = last_received_packet.Data[9].Float; + break; + case VN100_REG_QAR : + ins_quat.qi = last_received_packet.Data[0].Float; + ins_quat.qx = last_received_packet.Data[1].Float; + ins_quat.qy = last_received_packet.Data[2].Float; + ins_quat.qz = last_received_packet.Data[3].Float; + FLOAT_EULERS_OF_QUAT(ins_eulers, ins_quat); + ins_accel.x = last_received_packet.Data[4].Float; + ins_accel.y = last_received_packet.Data[5].Float; + ins_accel.z = last_received_packet.Data[6].Float; + ins_rates.p = last_received_packet.Data[7].Float; + ins_rates.q = last_received_packet.Data[8].Float; + ins_rates.r = last_received_packet.Data[9].Float; + break; + case VN100_REG_QMR : + ins_quat.qi = last_received_packet.Data[0].Float; + ins_quat.qx = last_received_packet.Data[1].Float; + ins_quat.qy = last_received_packet.Data[2].Float; + ins_quat.qz = last_received_packet.Data[3].Float; + FLOAT_EULERS_OF_QUAT(ins_eulers, ins_quat); + ins_mag.x = last_received_packet.Data[4].Float; + ins_mag.y = last_received_packet.Data[5].Float; + ins_mag.z = last_received_packet.Data[6].Float; + ins_accel.x = last_received_packet.Data[7].Float; + ins_accel.y = last_received_packet.Data[8].Float; + ins_accel.z = last_received_packet.Data[9].Float; + ins_rates.p = last_received_packet.Data[10].Float; + ins_rates.q = last_received_packet.Data[11].Float; + ins_rates.r = last_received_packet.Data[12].Float; + break; + case VN100_REG_YMR : + ins_eulers.phi = RadOfDeg(last_received_packet.Data[2].Float); + ins_eulers.theta = RadOfDeg(last_received_packet.Data[1].Float); + ins_eulers.psi = RadOfDeg(last_received_packet.Data[0].Float); + ins_mag.x = last_received_packet.Data[3].Float; + ins_mag.y = last_received_packet.Data[4].Float; + ins_mag.z = last_received_packet.Data[5].Float; + ins_accel.x = last_received_packet.Data[6].Float; + ins_accel.y = last_received_packet.Data[7].Float; + ins_accel.z = last_received_packet.Data[8].Float; + ins_rates.p = last_received_packet.Data[9].Float; + ins_rates.q = last_received_packet.Data[10].Float; + ins_rates.r = last_received_packet.Data[11].Float; + break; + } + +} + +#include "uart.h" +#include "messages.h" +#include "downlink.h" + +extern void ins_report_task( void ) { + DOWNLINK_SEND_BOOZ_AHRS_LKF(DefaultChannel, + &ins_eulers.phi, &ins_eulers.theta, &ins_eulers.psi, + &ins_quat.qi, &ins_quat.qx, &ins_quat.qy, &ins_quat.qz, + &ins_rates.p, &ins_rates.q, &ins_rates.r, + &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 new file mode 100644 index 0000000000..b1274be958 --- /dev/null +++ b/sw/airborne/modules/ins/ins_vn100.h @@ -0,0 +1,89 @@ +/* + * $Id: $ + * + * 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. + * + */ + +/** \file ins_vn100.h + * \brief Interface for the VectorNav VN100 AHRS + * use the binary protocal on the SPI link +*/ + + +#ifndef INS_VN100_H +#define INS_VN100_H + +#include "std.h" +#include "VN100.h" +#include "math/pprz_algebra_float.h" + +/* state */ +extern struct FloatEulers ins_eulers; +extern struct FloatQuat ins_quat; +extern struct FloatRates ins_rates; +extern struct FloatRMat ins_rmat; +extern struct FloatVect3 ins_accel; +extern struct FloatVect3 ins_mag; + +extern volatile uint8_t ins_msg_received; + +extern void ins_init( void ); +extern void ins_periodic_task( void ); +extern void ins_event_task( void ); +extern void ins_report_task( void ); +extern void parse_ins_msg( void ); + +/* last received SPI packet */ +extern VN100_Res_Packet last_received_packet; +/* last send packet */ +extern VN100_Req_Packet last_send_packet; + +/* output mode */ +extern uint32_t ins_ador; +extern uint32_t ins_adof; +extern uint32_t ins_baud; + +#ifndef VN100_ADOR +#define VN100_ADOR VN100_ADOR_OFF +#endif +#ifndef VN100_ADOF +#define VN100_ADOF VN100_ADOF_5HZ +#endif +#ifndef VN100_BAUD +#define VN100_BAUD VN100_Baud_57600 +#endif + +/* Init sequence */ +extern uint8_t ins_init_status; +#define INS_VN100_SET_BAUD 0 +#define INS_VN100_SET_ADOR 1 +#define INS_VN100_SET_ADOF 2 +#define INS_VN100_READY 3 + +/* Telemetry */ +#define PERIODIC_SEND_AHRS(_chan) DOWNLINK_SEND_BOOZ_AHRS_LKF(_chan, \ + &ins_eulers.phi, &ins_eulers.theta, &ins_eulers.psi, \ + &ins_quat.qi, &ins_quat.qx, &ins_quat.qy, &ins_quat.qz, \ + &ins_rates.p, &ins_rates.q, &ins_rates.r, \ + &ins_accel.x, &ins_accel.y, &ins_accel.z, \ + &ins_mag.x, &ins_mag.y, &ins_mag.z) + +#endif /* INS_VN100_H */