mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-10 06:59:54 +08:00
driver module for VN100
This commit is contained in:
Gautier Hattenberger
@@ -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>
|
||||
|
||||
@@ -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
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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 */
|
||||
Reference in New Issue
Block a user