Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-01 04:46:51 +08:00
Code Issues Actions 27 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 # XSens Mti-G
# <load name="ins_xsens_MTiG_fixedwing.xml"> # <subsystem name="ins" type="xsens">
# <configure name="XSENS_UART_NR" value="0"/> # <configure name="XSENS_UART_NR" value="0"/>
# </load> # <configure name="XSENS_UART_BAUD" value="B115200"/>
# </subsystem>
@@ -13,35 +14,19 @@
ifeq ($(TARGET), ap) ifeq ($(TARGET), ap)
# <init fun="ins_init()"/> ap.CFLAGS += -DUSE_INS
# <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\"
# AHRS Results # AHRS Results
ap.CFLAGS += -DAHRS_TYPE_H=\"modules/ins/ins_xsens.h\"
ap.CFLAGS += -DINS_MODULE_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\" 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 += -DUSE_UART$(XSENS_UART_NR)
ap.CFLAGS += -DINS_LINK=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
ap.CFLAGS += -DUSE_GPS_XSENS_RAW_DATA ap.CFLAGS += -DUSE_GPS_XSENS_RAW_DATA
ap.CFLAGS += -DGPS_NB_CHANNELS=16 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(); ahrs_init();
#endif #endif
#if USE_INS
ins_init();
#endif
/************* Links initialization ***************/ /************* Links initialization ***************/
#if defined MCU_SPI_LINK #if defined MCU_SPI_LINK
link_mcu_init(); link_mcu_init();
@@ -493,11 +497,9 @@ void navigation_task( void ) {
} }
#if USE_AHRS
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
volatile uint8_t new_ins_attitude = 0; volatile uint8_t new_ins_attitude = 0;
#endif #endif
#endif
void attitude_loop( void ) { void attitude_loop( void ) {
@@ -532,6 +534,10 @@ void sensors_task( void ) {
ahrs_timeout_counter ++; ahrs_timeout_counter ++;
#endif // USE_AHRS #endif // USE_AHRS
#endif // USE_IMU #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); ImuEvent(on_gyro_event, on_accel_event, on_mag_event);
#endif #endif
#if USE_INS
InsEvent(NULL);
#endif
#if USE_GPS #if USE_GPS
GpsEvent(on_gps_solution); GpsEvent(on_gps_solution);
#endif /** USE_GPS */ #endif /** USE_GPS */
@@ -624,7 +634,9 @@ void event_task_ap( void ) {
#if USE_GPS #if USE_GPS
static inline void on_gps_solution( void ) { static inline void on_gps_solution( void ) {
estimator_update_state_gps(); estimator_update_state_gps();
#if USE_AHRS
ahrs_update_gps(); ahrs_update_gps();
#endif
#ifdef GPS_TRIGGERED_FUNCTION #ifdef GPS_TRIGGERED_FUNCTION
GPS_TRIGGERED_FUNCTION(); GPS_TRIGGERED_FUNCTION();
#endif #endif
sw/airborne/modules/ins/ins_module.h
-2
View File
@@ -32,7 +32,6 @@
#define INS_H #define INS_H
#include "std.h" #include "std.h"
#include "led.h"
#ifndef INS_FORMAT #ifndef INS_FORMAT
#define INS_FORMAT float #define INS_FORMAT float
@@ -97,7 +96,6 @@ void parse_ins_buffer( uint8_t );
ReadInsBuffer(); \ ReadInsBuffer(); \
} \ } \
if (ins_msg_received) { \ if (ins_msg_received) { \
LED_TOGGLE(2); \
parse_ins_msg(); \ parse_ins_msg(); \
handler; \ handler; \
ins_msg_received = FALSE; \ 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 * Copyright (C) 2003 Pascal Brisset, Antoine Drouin
* *
* This file is part of paparazzi. * This file is part of paparazzi.
@@ -22,8 +20,8 @@
* *
*/ */
/** \file xsens.c /** @file xsens.c
* \brief Parser for the Xsens protocol * Parser for the Xsens protocol.
*/ */
#include "ins_module.h" #include "ins_module.h"
@@ -72,25 +70,6 @@ float ins_pitch_neutral;
float ins_roll_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 // XSens Specific
@@ -242,65 +221,68 @@ void ins_init( void ) {
void ins_periodic_task( void ) { void ins_periodic_task( void ) {
if (xsens_configured > 0) if (xsens_configured > 0)
{ {
switch (xsens_configured) switch (xsens_configured)
{ {
case 20: case 20:
/* send mode and settings to MT */ /* send mode and settings to MT */
XSENS_GoToConfig(); XSENS_GoToConfig();
XSENS_SetOutputMode(xsens_output_mode); XSENS_SetOutputMode(xsens_output_mode);
XSENS_SetOutputSettings(xsens_output_settings); XSENS_SetOutputSettings(xsens_output_settings);
break; break;
case 18: case 18:
// Give pulses on SyncOut // Give pulses on SyncOut
XSENS_SetSyncOutSettings(0,0x0002); XSENS_SetSyncOutSettings(0,0x0002);
break; break;
case 17: case 17:
// 1 pulse every 100 samples // 1 pulse every 100 samples
XSENS_SetSyncOutSettings(1,100); XSENS_SetSyncOutSettings(1,100);
break; break;
case 2: case 2:
XSENS_ReqLeverArmGps(); XSENS_ReqLeverArmGps();
break; break;
case 6: case 6:
XSENS_ReqMagneticDeclination(); XSENS_ReqMagneticDeclination();
break; break;
case 13: case 13:
#ifdef AHRS_H_X #ifdef AHRS_H_X
#pragma message "Sending XSens Magnetic Declination." #pragma message "Sending XSens Magnetic Declination."
xsens_declination = atan2(AHRS_H_Y, AHRS_H_X); xsens_declination = atan2(AHRS_H_Y, AHRS_H_X);
XSENS_SetMagneticDeclination(xsens_declination); XSENS_SetMagneticDeclination(xsens_declination);
#endif #endif
break; break;
case 12: case 12:
#ifdef GPS_IMU_LEVER_ARM_X #ifdef GPS_IMU_LEVER_ARM_X
#pragma message "Sending XSens GPS Arm." #pragma message "Sending XSens GPS Arm."
XSENS_SetLeverArmGps(GPS_IMU_LEVER_ARM_X,GPS_IMU_LEVER_ARM_Y,GPS_IMU_LEVER_ARM_Z); XSENS_SetLeverArmGps(GPS_IMU_LEVER_ARM_X,GPS_IMU_LEVER_ARM_Y,GPS_IMU_LEVER_ARM_Z);
#endif #endif
break; break;
case 10: case 10:
{ {
uint8_t baud = 1; uint8_t baud = 1;
XSENS_SetBaudrate(baud); XSENS_SetBaudrate(baud);
} }
break; break;
case 1: case 1:
XSENS_GoToMeasurment(); XSENS_GoToMeasurment();
break; break;
}
xsens_configured--; default:
return; break;
} }
xsens_configured--;
return;
}
RunOnceEvery(100,XSENS_ReqGPSStatus()); RunOnceEvery(100,XSENS_ReqGPSStatus());
} }
#include "estimator.h" #include "estimator.h"
void handle_ins_msg( void) { void handle_ins_msg(void) {
// Send to Estimator (Control) // Send to Estimator (Control)
@@ -611,6 +593,8 @@ void parse_ins_buffer( uint8_t c ) {
ins_msg_received = TRUE; ins_msg_received = TRUE;
goto restart; goto restart;
break; break;
default:
break;
} }
return; return;
error: error:
sw/airborne/modules/ins/ins_xsens.h
+1 -5
View File
@@ -33,9 +33,6 @@
#include "ins_module.h" #include "ins_module.h"
#include "subsystems/ahrs.h"
extern int8_t xsens_hour; extern int8_t xsens_hour;
extern int8_t xsens_min; extern int8_t xsens_min;
extern int8_t xsens_sec; extern int8_t xsens_sec;
@@ -46,8 +43,7 @@ extern int8_t xsens_day;
extern uint8_t xsens_msg_status; extern uint8_t xsens_msg_status;
extern uint16_t xsens_time_stamp; extern uint16_t xsens_time_stamp;
#define AhrsEvent(_ahrs_handler) { \ #define InsEvent(_ins_handler) { \
LED_TOGGLE(3); \
InsEventCheckAndHandle(handle_ins_msg()) \ InsEventCheckAndHandle(handle_ins_msg()) \
} }