Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-10 06:59:54 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

[vn100] adapt vn100 to new spi driver (untested)

Browse Source
This commit is contained in:
Gautier Hattenberger
2012-11-27 23:21:16 +01:00
parent 9423ab57ce
commit 7d886a29bc
5 changed files with 143 additions and 140 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/ENAC/fixed-wing/merlin.xml
+6
View File
@@ -23,6 +23,11 @@
<subsystem name="ins" type="alt_float"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="navigation"/>
<!--subsystem name="spi">
<define name="USE_SPI_SLAVE0"/>
<define name="USE_SPI1"/>
</subsystem-->
</firmware>
<firmware name="setup">
@@ -32,6 +37,7 @@
<modules>
<load name="infrared_adc.xml"/>
<!--load name="ins_vn100.xml"/-->
</modules>
<!-- commands section -->
conf/modules/ins_vn100.xml
+7 -5
View File
@@ -3,18 +3,20 @@
<module name="vn100" dir="ins">
<doc>
<description>VectorNav VN100 (SPI)</description>
<define name="VN100_SPI_DEV" value="spiX" description="select spi peripherals (default spi1)"/>
<define name="VN100_SLAVE_IDX" value="X" description="spi slave index"/>
</doc>
<!-- <depend conflict="ins" -->
<header>
<file name="ins_vn100.h"/>
</header>
<init fun="ins_init()"/>
<periodic fun="ins_periodic()" freq="60"/>
<periodic fun="ins_report_task()" freq="4"/>
<event fun="ins_event_task()"/>
<init fun="vn100_init()"/>
<periodic fun="vn100_periodic_task()" freq="60"/>
<periodic fun="vn100_report_task()" freq="4"/>
<event fun="vn100_event_task()"/>
<makefile>
<file name="ins_vn100.c"/>
<file name="fw_ins_vn100.c"/>
<define name="SPI_MASTER"/>
</makefile>
</module>
sw/airborne/modules/ins/fw_ins_vn100.c
-126
View File
@@ -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);
}
}
sw/airborne/modules/ins/ins_vn100.c
+126 -5
View File
@@ -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);
}
sw/airborne/modules/ins/ins_vn100.h
+4 -4
View File
@@ -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;