Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

XSens -> 230k4: otherwise half the data is lost. Fixed GPS_NB_CHANNELS issue with new GPS

Browse Source
This commit is contained in:
Christophe De Wagter
2011-05-04 13:06:12 +02:00
parent a43abc9297
commit d9f78faeca
10 changed files with 58 additions and 151 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/TU_Delft/yapa_xsens.xml
+9 -5
View File
@@ -1,6 +1,6 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!--
<!--
YAPA + XSens + XBee
-->
@@ -51,7 +51,7 @@
<!-- Brake Rate Limiter -->
<let var="brake_value" value="Chop(-@BRAKE, 0, MAX_PPRZ)"/>
<!--<let var="brake_value" value="RATELIMIT( $brake_value , MAX_BRAKE_RATE )"/>
<let var="test; \
static int16_t _var_brake_value = 0; \
_var_brake_value += LIMIT(_var_brake_value_nofilt - _var_brake_value,-MAX_BRAKE_RATE,MAX_BRAKE_RATE); \
@@ -180,14 +180,16 @@
<load name="ins_xsens_MTiG_fixedwing.xml">
<configure name="XSENS_UART_NR" value="0"/>
</load>
<!--
<load name="light.xml">
<define name="LIGHT_LED_STROBE" value="3"/>
<define name="LIGHT_LED_NAV" value="2"/>
<define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
<define name="NAV_LIGHT_MODE_DEFAULT" value="4"/>
</load>
<!-- <load name="digital_cam_i2c.xml"/> -->
-->
<!-- <load name="digital_cam_i2c.xml"/> -->
<load name="mag_hmc5843.xml" />
<load name="digital_cam.xml" >
<define name="DC_SHUTTER_LED" value="3"/>
</load>
@@ -218,6 +220,8 @@
<subsystem name="navigation"/>
<subsystem name="gps" type="xsens"/>
</firmware>
<subsystem name="i2c"/>
</firmware>
</airframe>
conf/autopilot/subsystems/fixedwing/imu_booz.makefile
-1
View File
@@ -52,7 +52,6 @@ imu_srcs += $(SRC_ARCH)/peripherals/max1168_arch.c
#ifeq ($(ARCH), lpc21)
imu_CFLAGS += -DSSP_VIC_SLOT=9
imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8
#FIXME ms2100 not used on this imu
#else ifeq ($(ARCH), stm32)
#imu_CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ
#imu_CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT)
conf/modules/ins_xsens_MTiG_fixedwing.xml
+4 -1
View File
@@ -13,10 +13,13 @@
<define name="AHRS_TYPE_H" value="\\\"modules/ins/ins_xsens.h\\\"" />
<define name="USE_UART$(XSENS_UART_NR)"/>
<define name="INS_LINK" value="Uart$(XSENS_UART_NR)"/>
<define name="UART$(XSENS_UART_NR)_BAUD" value="B115200"/>
<define name="UART$(XSENS_UART_NR)_BAUD" value="B230400"/>
<define name="USE_GPS_XSENS"/>
<define name="USE_GPS_XSENS_RAW_DATA" />
<define name="GPS_NB_CHANNELS" value="16" />
<define name="XSENS_OUTPUT_MODE" value="0x1836" />
<file name="ins_xsens.c"/>
<define name="AHRS_TRIGGERED_ATTITUDE_LOOP" />
</makefile>
</module>
conf/xsens_MTi-G.xml
+5
View File
@@ -40,6 +40,11 @@
</message>
<message name="SetBaudrateAck" ID="0x19" to="host"/>
<message name="SetSyncOutSettings" ID="0xD8" to="MT" length="3">
<field name="param" format="U1"/>
<field name="value" format="U2"/>
</message>
<message name="RestoreFactoryDef" ID="0x0E" to="MT"/>
<message name="RestoreFactoryDefAck" ID="0x0F" to="host"/>
sw/airborne/ap_downlink.h
+1 -1
View File
@@ -191,7 +191,7 @@
int16_t climb = -gps.ned_vel.z; \
int16_t course = DegOfRad(gps.course / 10); \
DOWNLINK_SEND_GPS(DefaultChannel, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &course, &gps.lla_pos.alt, &gps.gspeed, &climb, &gps.week, &gps.tow, &gps.utm_pos.zone, &i); \
if (i == gps.nb_channels) i = 0; \
if (i >= gps.nb_channels) i = 0; \
if (i < gps.nb_channels && gps.svinfos[i].cno > 0 && gps.svinfos[i].cno != last_cnos[i]) { \
DOWNLINK_SEND_SVINFO(DefaultChannel, &i, &gps.svinfos[i].svid, &gps.svinfos[i].flags, &gps.svinfos[i].qi, &gps.svinfos[i].cno, &gps.svinfos[i].elev, &gps.svinfos[i].azim); \
last_cnos[i] = gps.svinfos[i].cno; \
sw/airborne/gps_xsens.c
-74
View File
@@ -1,74 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2005 Pascal Brisset, Antoine Drouin
*
* 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 gps_xsens.c
* \brief GPS hardware for Xsens MTi-G
*
*/
#include <stdlib.h>
#include "gps.h"
#include "sys_time.h"
#include "generated/airframe.h"
#include "mcu_periph/uart.h"
#include "messages.h"
#include "downlink.h"
uint8_t gps_mode;
uint16_t gps_week;
uint32_t gps_itow;
int32_t gps_alt;
uint16_t gps_gspeed;
int16_t gps_climb;
int16_t gps_course;
int32_t gps_utm_east, gps_utm_north;
uint8_t gps_utm_zone;
int32_t gps_lat, gps_lon;
uint16_t gps_PDOP;
uint32_t gps_Pacc, gps_Sacc;
uint8_t gps_numSV;
uint8_t gps_configuring;
uint16_t gps_reset;
void reset_gps_watchdog(void)
{
last_gps_msg_t = cpu_time_sec;
}
volatile bool_t gps_msg_received;
bool_t gps_pos_available;
uint8_t gps_nb_ovrn;
uint8_t gps_nb_channels;
struct svinfo gps_svinfos[GPS_NB_CHANNELS];
void gps_init( void ) {}
void gps_configure( void ) {}
void parse_gps_msg( void ) {}
void gps_configure_uart( void ) {}
sw/airborne/gps_xsens.h
-48
View File
@@ -1,48 +0,0 @@
/*
* Paparazzi autopilot $Id: gps_ubx.h 4659 2010-03-10 16:55:04Z mmm $
*
* Copyright (C) 2004-2006 Pascal Brisset, Antoine Drouin
*
* 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 gps_xsens.h
* \brief XSens GPS
*
*/
#ifndef XSENS_GPS_H
#define XSENS_GPS_H
extern uint16_t gps_reset;
#define GPS_NB_CHANNELS 16
extern void reset_gps_watchdog(void);
#define GpsFixValid() (gps_mode > 0)
#define gps_xsens_Reset(_val) { \
gps_reset = _val; \
}
#endif
sw/airborne/modules/ins/ins_chimu_uart.c
+11 -11
View File
@@ -34,22 +34,22 @@ INS_FORMAT ins_pitch_neutral;
volatile uint8_t new_ins_attitude;
void ins_init( void )
void ins_init( void )
{
uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI
// uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
// uint8_t euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI
uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI
CHIMU_Checksum(rate,12);
new_ins_attitude = 0;
ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
CHIMU_Init(&CHIMU_DATA);
CHIMU_Init(&CHIMU_DATA);
// Quat Filter
for (int i=0;i<7;i++)
{
@@ -68,7 +68,7 @@ void ins_init( void )
{
InsUartSend1(rate[i]);
}
}
void parse_ins_msg( void )
@@ -76,7 +76,7 @@ void parse_ins_msg( void )
while (InsLink(ChAvailable()))
{
uint8_t ch = InsLink(Getch());
if (CHIMU_Parse(ch, 0, &CHIMU_DATA))
{
if(CHIMU_DATA.m_MsgID==0x03)
@@ -87,10 +87,10 @@ void parse_ins_msg( void )
{
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
}
EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
//EstimatorSetRate(ins_p,ins_q);
DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi);
}
@@ -100,7 +100,7 @@ void parse_ins_msg( void )
//Frequency defined in conf *.xml
void ins_periodic_task( void )
void ins_periodic_task( void )
{
// Downlink Send
}
sw/airborne/modules/ins/ins_xsens.c
+27 -9
View File
@@ -68,6 +68,8 @@ INS_FORMAT ins_mz;
float ins_pitch_neutral;
float ins_roll_neutral;
volatile uint8_t new_ins_attitude;
//////////////////////////////////////////////////////////////////////////////////////////
//
// XSens Specific
@@ -177,9 +179,6 @@ int8_t xsens_day;
float xsens_lat;
float xsens_lon;
int8_t xsens_gps_nr_channels = 16;
static uint8_t xsens_id;
static uint8_t xsens_status;
static uint8_t xsens_len;
@@ -205,7 +204,16 @@ void ins_init( void ) {
XSENS_GoToConfig();
XSENS_SetOutputMode(xsens_output_mode);
XSENS_SetOutputSettings(xsens_output_settings);
uint8_t baud = 1;
XSENS_SetBaudrate(baud);
// Give pulses on SyncOut
XSENS_SetSyncOutSettings(0,0x0002);
// 1 pulse every 100 samples
// XSENS_SetSyncOutSettings(1,100);
//XSENS_GoToMeasurment();
gps.nb_channels = 0;
}
void ins_periodic_task( void ) {
@@ -243,16 +251,22 @@ void parse_ins_msg( void ) {
}
#ifdef USE_GPS_XSENS
else if (xsens_id == XSENS_GPSStatus_ID) {
xsens_gps_nr_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
gps.num_sv = xsens_gps_nr_channels;
gps.nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
gps.num_sv = 0;
uint8_t i;
for(i = 0; i < Min(xsens_gps_nr_channels, xsens_gps_nr_channels); i++) {
// Do not write outside buffer
for(i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) {
uint8_t ch = XSENS_GPSStatus_chn(xsens_msg_buf,i);
if (ch > xsens_gps_nr_channels) continue;
if (ch > gps.nb_channels) continue;
gps.svinfos[ch].svid = XSENS_GPSStatus_svid(xsens_msg_buf, i);
gps.svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens_msg_buf, i);
gps.svinfos[ch].qi = XSENS_GPSStatus_qi(xsens_msg_buf, i);
gps.svinfos[ch].cno = XSENS_GPSStatus_cnr(xsens_msg_buf, i);
if (gps.svinfos[ch].flags > 0)
{
gps.num_sv++;
}
}
}
#endif
@@ -267,15 +281,18 @@ void parse_ins_msg( void ) {
}
if (XSENS_MASK_RAWGPS(xsens_output_mode)) {
#if defined(USE_GPS_XSENS_RAW_DATA) && defined(USE_GPS_XSENS)
LED_TOGGLE(3);
gps.week = 0; // FIXME
gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10;
gps.lla_pos.lat = RadOfDeg(XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset));
gps.lla_pos.lon = RadOfDeg(XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset));
/* Set the real UTM zone */
gps.utm_pos.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
lla_f.lat = (float) gps.lla_pos.lat * 1e7;
lla_f.lon = (float) gps.lla_pos.lat * 1e7;
lla_f.lat = (float) gps.lla_pos.lat / 1e7;
lla_f.lon = (float) gps.lla_pos.lat / 1e7;
utm_f.zone = nav_utm_zone0;
/* convert to utm */
utm_of_lla_f(&utm_f, &lla_f);
@@ -348,6 +365,7 @@ void parse_ins_msg( void ) {
if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x10) {
offset += XSENS_DATA_Matrix_LENGTH;
}
new_ins_attitude = 1;
}
if (XSENS_MASK_Auxiliary(xsens_output_mode)) {
uint8_t l = 0;
sw/airborne/modules/sensors/mag_hmc5843.c
+1 -1
View File
@@ -46,7 +46,7 @@ void hmc5843_module_periodic ( void )
mag_x = hmc5843.data.value[0];
mag_y = hmc5843.data.value[1];
mag_z = hmc5843.data.value[2];
RunOnceEvery(1,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z));
RunOnceEvery(30,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z));
}
void hmc5843_module_event( void )