Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 20:38:27 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[fixedwing] added InsEvent for xsens

Browse Source
This commit is contained in:
Felix Ruess
2012-06-04 23:20:49 +02:00
parent 93a8dbeae3
commit 62cb9fd34e
5 changed files with 81 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/subsystems/fixedwing/ins_xsens.makefile
+9 -24
View File
@@ -2,9 +2,10 @@
# XSens Mti-G
# <load name="ins_xsens_MTiG_fixedwing.xml">
# <subsystem name="ins" type="xsens">
# <configure name="XSENS_UART_NR" value="0"/>
# </load>
# <configure name="XSENS_UART_BAUD" value="B115200"/>
# </subsystem>
@@ -13,35 +14,19 @@
ifeq ($(TARGET), ap)
# <init fun="ins_init()"/>
# <periodic fun="ins_periodic_task()" freq="60"/>
# <event fun="InsEventCheckAndHandle(handle_ins_msg())"/>
# <makefile target="ap">
# <define name="AHRS_TYPE_H" value="\\\"modules/ins/ins_xsens.h\\\"" />
# <define name="INS_MODULE_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="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>
# ImuEvent -> XSensEvent
ap.CFLAGS += -DUSE_AHRS
ap.CFLAGS += -DIMU_TYPE_H=\"modules/ins/ins_xsens.h\"
ap.CFLAGS += -DUSE_INS
# AHRS Results
ap.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ins_xsens.h\"
ap.CFLAGS += -DINS_MODULE_H=\"modules/ins/ins_xsens.h\"
ap.CFLAGS += -DGPS_TYPE_H=\"modules/ins/ins_xsens.h\"
ifndef XSENS_UART_BAUD
XSENS_UART_BAUD = B115200
endif
ap.CFLAGS += -DUSE_UART$(XSENS_UART_NR)
ap.CFLAGS += -DINS_LINK=Uart$(XSENS_UART_NR)
ap.CFLAGS += -DUART$(XSENS_UART_NR)_BAUD=B230400
ap.CFLAGS += -DUART$(XSENS_UART_NR)_BAUD=$(XSENS_UART_BAUD)
ap.CFLAGS += -DUSE_GPS_XSENS
ap.CFLAGS += -DUSE_GPS_XSENS_RAW_DATA
ap.CFLAGS += -DGPS_NB_CHANNELS=16
sw/airborne/firmwares/fixedwing/main_ap.c
+14 -2
View File
@@ -188,6 +188,10 @@ void init_ap( void ) {
ahrs_init();
#endif
#if USE_INS
ins_init();
#endif
/************* Links initialization ***************/
#if defined MCU_SPI_LINK
link_mcu_init();
@@ -493,11 +497,9 @@ void navigation_task( void ) {
}
#if USE_AHRS
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
volatile uint8_t new_ins_attitude = 0;
#endif
#endif
void attitude_loop( void ) {
@@ -532,6 +534,10 @@ void sensors_task( void ) {
ahrs_timeout_counter ++;
#endif // USE_AHRS
#endif // USE_IMU
#if USE_INS
ins_periodic_task();
#endif
}
@@ -589,6 +595,10 @@ void event_task_ap( void ) {
ImuEvent(on_gyro_event, on_accel_event, on_mag_event);
#endif
#if USE_INS
InsEvent(NULL);
#endif
#if USE_GPS
GpsEvent(on_gps_solution);
#endif /** USE_GPS */
@@ -624,7 +634,9 @@ void event_task_ap( void ) {
#if USE_GPS
static inline void on_gps_solution( void ) {
estimator_update_state_gps();
#if USE_AHRS
ahrs_update_gps();
#endif
#ifdef GPS_TRIGGERED_FUNCTION
GPS_TRIGGERED_FUNCTION();
#endif
sw/airborne/modules/ins/ins_module.h
-2
View File
@@ -32,7 +32,6 @@
#define INS_H
#include "std.h"
#include "led.h"
#ifndef INS_FORMAT
#define INS_FORMAT float
@@ -97,7 +96,6 @@ void parse_ins_buffer( uint8_t );
ReadInsBuffer(); \
} \
if (ins_msg_received) { \
LED_TOGGLE(2); \
parse_ins_msg(); \
handler; \
ins_msg_received = FALSE; \
sw/airborne/modules/ins/ins_xsens.c
+57 -73
View File
@@ -1,6 +1,4 @@
/*
* Paparazzi mcu0 $Id$
*
* Copyright (C) 2003 Pascal Brisset, Antoine Drouin
*
* This file is part of paparazzi.
@@ -22,8 +20,8 @@
*
*/
/** \file xsens.c
* \brief Parser for the Xsens protocol
/** @file xsens.c
* Parser for the Xsens protocol.
*/
#include "ins_module.h"
@@ -72,25 +70,6 @@ float ins_pitch_neutral;
float ins_roll_neutral;
void ahrs_init(void)
{
ins_init();
}
#include "subsystems/imu.h"
void imu_init(void)
{
ins_init();
}
void imu_periodic(void)
{
ins_periodic_task();
}
//////////////////////////////////////////////////////////////////////////////////////////
//
// XSens Specific
@@ -242,65 +221,68 @@ void ins_init( void ) {
void ins_periodic_task( void ) {
if (xsens_configured > 0)
{
switch (xsens_configured)
{
case 20:
/* send mode and settings to MT */
XSENS_GoToConfig();
XSENS_SetOutputMode(xsens_output_mode);
XSENS_SetOutputSettings(xsens_output_settings);
break;
case 18:
// Give pulses on SyncOut
XSENS_SetSyncOutSettings(0,0x0002);
break;
case 17:
// 1 pulse every 100 samples
XSENS_SetSyncOutSettings(1,100);
break;
case 2:
XSENS_ReqLeverArmGps();
break;
{
switch (xsens_configured)
{
case 20:
/* send mode and settings to MT */
XSENS_GoToConfig();
XSENS_SetOutputMode(xsens_output_mode);
XSENS_SetOutputSettings(xsens_output_settings);
break;
case 18:
// Give pulses on SyncOut
XSENS_SetSyncOutSettings(0,0x0002);
break;
case 17:
// 1 pulse every 100 samples
XSENS_SetSyncOutSettings(1,100);
break;
case 2:
XSENS_ReqLeverArmGps();
break;
case 6:
XSENS_ReqMagneticDeclination();
break;
case 6:
XSENS_ReqMagneticDeclination();
break;
case 13:
#ifdef AHRS_H_X
#pragma message "Sending XSens Magnetic Declination."
xsens_declination = atan2(AHRS_H_Y, AHRS_H_X);
XSENS_SetMagneticDeclination(xsens_declination);
#endif
break;
case 12:
#ifdef GPS_IMU_LEVER_ARM_X
#pragma message "Sending XSens GPS Arm."
XSENS_SetLeverArmGps(GPS_IMU_LEVER_ARM_X,GPS_IMU_LEVER_ARM_Y,GPS_IMU_LEVER_ARM_Z);
#endif
break;
case 10:
{
uint8_t baud = 1;
XSENS_SetBaudrate(baud);
}
break;
case 13:
#ifdef AHRS_H_X
#pragma message "Sending XSens Magnetic Declination."
xsens_declination = atan2(AHRS_H_Y, AHRS_H_X);
XSENS_SetMagneticDeclination(xsens_declination);
#endif
break;
case 12:
#ifdef GPS_IMU_LEVER_ARM_X
#pragma message "Sending XSens GPS Arm."
XSENS_SetLeverArmGps(GPS_IMU_LEVER_ARM_X,GPS_IMU_LEVER_ARM_Y,GPS_IMU_LEVER_ARM_Z);
#endif
break;
case 10:
{
uint8_t baud = 1;
XSENS_SetBaudrate(baud);
}
break;
case 1:
XSENS_GoToMeasurment();
break;
}
xsens_configured--;
return;
}
case 1:
XSENS_GoToMeasurment();
break;
default:
break;
}
xsens_configured--;
return;
}
RunOnceEvery(100,XSENS_ReqGPSStatus());
}
#include "estimator.h"
void handle_ins_msg( void) {
void handle_ins_msg(void) {
// Send to Estimator (Control)
@@ -611,6 +593,8 @@ void parse_ins_buffer( uint8_t c ) {
ins_msg_received = TRUE;
goto restart;
break;
default:
break;
}
return;
error:
sw/airborne/modules/ins/ins_xsens.h
+1 -5
View File
@@ -33,9 +33,6 @@
#include "ins_module.h"
#include "subsystems/ahrs.h"
extern int8_t xsens_hour;
extern int8_t xsens_min;
extern int8_t xsens_sec;
@@ -46,8 +43,7 @@ extern int8_t xsens_day;
extern uint8_t xsens_msg_status;
extern uint16_t xsens_time_stamp;
#define AhrsEvent(_ahrs_handler) { \
LED_TOGGLE(3); \
#define InsEvent(_ins_handler) { \
InsEventCheckAndHandle(handle_ins_msg()) \
}