[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [4779] driver module for VN100
|
From: |
Gautier Hattenberger |
|
Subject: |
[paparazzi-commits] [4779] driver module for VN100 |
|
Date: |
Fri, 02 Apr 2010 07:53:25 +0000 |
Revision: 4779
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4779
Author: gautier
Date: 2010-04-02 07:53:23 +0000 (Fri, 02 Apr 2010)
Log Message:
-----------
driver module for VN100
Added Paths:
-----------
paparazzi3/trunk/conf/modules/ins_vn100.xml
paparazzi3/trunk/sw/airborne/modules/ins/VN100.h
paparazzi3/trunk/sw/airborne/modules/ins/fw_ins_vn100.c
paparazzi3/trunk/sw/airborne/modules/ins/ins_vn100.c
paparazzi3/trunk/sw/airborne/modules/ins/ins_vn100.h
Added: paparazzi3/trunk/conf/modules/ins_vn100.xml
===================================================================
--- paparazzi3/trunk/conf/modules/ins_vn100.xml (rev 0)
+++ paparazzi3/trunk/conf/modules/ins_vn100.xml 2010-04-02 07:53:23 UTC (rev
4779)
@@ -0,0 +1,17 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="ins">
+ <!-- <depend conflict="ins" -->
+ <header>
+ <file name="ins_vn100.h"/>
+ </header>
+ <init fun="ins_init()"/>
+ <periodic fun="ins_periodic_task()" freq="4"/>
+ <periodic fun="ins_report_task()" freq="4"/>
+ <event fun="ins_event_task()"/>
+ <makefile>
+ <file name="ins_vn100.c"/>
+ <file name="fw_ins_vn100.c"/>
+ </makefile>
+</module>
+
Added: paparazzi3/trunk/sw/airborne/modules/ins/VN100.h
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/ins/VN100.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/ins/VN100.h 2010-04-02 07:53:23 UTC
(rev 4779)
@@ -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
Added: paparazzi3/trunk/sw/airborne/modules/ins/fw_ins_vn100.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/ins/fw_ins_vn100.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/ins/fw_ins_vn100.c 2010-04-02
07:53:23 UTC (rev 4779)
@@ -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);
+ }
+}
+
Added: paparazzi3/trunk/sw/airborne/modules/ins/ins_vn100.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/ins/ins_vn100.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/ins/ins_vn100.c 2010-04-02
07:53:23 UTC (rev 4779)
@@ -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);
+}
Added: paparazzi3/trunk/sw/airborne/modules/ins/ins_vn100.h
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/ins/ins_vn100.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/ins/ins_vn100.h 2010-04-02
07:53:23 UTC (rev 4779)
@@ -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 */
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [4779] driver module for VN100,
Gautier Hattenberger <=