diff --git a/conf/airframes/TU_Delft/skywalker.xml b/conf/airframes/TU_Delft/skywalker.xml
index c5b9395843e..226be3e4272 100644
--- a/conf/airframes/TU_Delft/skywalker.xml
+++ b/conf/airframes/TU_Delft/skywalker.xml
@@ -19,7 +19,7 @@
-
+
@@ -43,7 +43,9 @@
-
+
+
+
@@ -67,13 +69,26 @@
-
+
+
+
+
+
+
-
+
@@ -148,18 +163,16 @@
-
diff --git a/conf/messages.xml b/conf/messages.xml
index a25d2c6ebbb..fdd74237358 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -1567,7 +1567,14 @@
-
+
+
+
+
+
+
+
+
diff --git a/conf/modules/ins_xsens_MTiG_fixedwing.xml b/conf/modules/ins_xsens_MTiG_fixedwing.xml
index dcd3c87cb44..6ff09118062 100644
--- a/conf/modules/ins_xsens_MTiG_fixedwing.xml
+++ b/conf/modules/ins_xsens_MTiG_fixedwing.xml
@@ -11,6 +11,7 @@
+
diff --git a/conf/xsens_MTi-G.xml b/conf/xsens_MTi-G.xml
index 721c801aef9..63bf030e13a 100644
--- a/conf/xsens_MTi-G.xml
+++ b/conf/xsens_MTi-G.xml
@@ -123,6 +123,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h
index 8947b97a6f5..34bedf56790 100644
--- a/sw/airborne/ap_downlink.h
+++ b/sw/airborne/ap_downlink.h
@@ -139,24 +139,34 @@
#define PERIODIC_SEND_SEGMENT(_chan) if (nav_in_segment) { DOWNLINK_SEND_SEGMENT(_chan, &nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2); }
#ifdef IMU_TYPE_H
-#include "subsystems/imu.h"
-#define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) { DOWNLINK_SEND_IMU_ACCEL_RAW(_chan, &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z)}
-#define PERIODIC_SEND_IMU_GYRO_RAW(_chan) { DOWNLINK_SEND_IMU_GYRO_RAW(_chan, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r)}
-#define PERIODIC_SEND_IMU_MAG_RAW(_chan) { DOWNLINK_SEND_IMU_MAG_RAW(_chan, &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z)}
-#define PERIODIC_SEND_IMU_ACCEL(_chan) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); DOWNLINK_SEND_IMU_ACCEL(_chan, &accel_float.x, &accel_float.y, &accel_float.z)}
-#define PERIODIC_SEND_IMU_GYRO(_chan) { struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); DOWNLINK_SEND_IMU_GYRO(_chan, &gyro_float.p, &gyro_float.q, &gyro_float.r)}
+# include "subsystems/imu.h"
+# define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) { DOWNLINK_SEND_IMU_ACCEL_RAW(_chan, &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z)}
+# define PERIODIC_SEND_IMU_GYRO_RAW(_chan) { DOWNLINK_SEND_IMU_GYRO_RAW(_chan, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r)}
+# define PERIODIC_SEND_IMU_MAG_RAW(_chan) { DOWNLINK_SEND_IMU_MAG_RAW(_chan, &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z)}
+# define PERIODIC_SEND_IMU_ACCEL(_chan) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); DOWNLINK_SEND_IMU_ACCEL(_chan, &accel_float.x, &accel_float.y, &accel_float.z)}
+# define PERIODIC_SEND_IMU_GYRO(_chan) { struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); DOWNLINK_SEND_IMU_GYRO(_chan, &gyro_float.p, &gyro_float.q, &gyro_float.r)}
# ifdef USE_MAGNETOMETER
# define PERIODIC_SEND_IMU_MAG(_chan) { struct FloatVect3 mag_float; MAGS_FLOAT_OF_BFP(mag_float, imu.mag); DOWNLINK_SEND_IMU_MAG(_chan, &mag_float.x, &mag_float.y, &mag_float.z)}
# else
-# define PERIODIC_SEND_IMU_MAG_RAW(_chan) {}
+# define PERIODIC_SEND_IMU_MAG(_chan) {}
# endif
#else
-#define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {}
-#define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {}
-#define PERIODIC_SEND_IMU_MAG_RAW(_chan) {}
-#define PERIODIC_SEND_IMU_ACCEL(_chan) {}
-#define PERIODIC_SEND_IMU_GYRO(_chan) {}
-#define PERIODIC_SEND_IMU_MAG(_chan) {}
+# ifdef INS_MODULE_H
+# include "modules/ins/ins_module.h"
+# define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {}
+# define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {}
+# define PERIODIC_SEND_IMU_MAG_RAW(_chan) {}
+# define PERIODIC_SEND_IMU_GYRO(_chan) { DOWNLINK_SEND_IMU_GYRO(_chan, &ins_p, &ins_q, &ins_r)}
+# define PERIODIC_SEND_IMU_ACCEL(_chan) { DOWNLINK_SEND_IMU_ACCEL(_chan, &ins_ax, &ins_ay, &ins_az)}
+# define PERIODIC_SEND_IMU_MAG(_chan) { DOWNLINK_SEND_IMU_MAG(_chan, &ins_mx, &ins_my, &ins_mz)}
+# else
+# define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {}
+# define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {}
+# define PERIODIC_SEND_IMU_MAG_RAW(_chan) {}
+# define PERIODIC_SEND_IMU_ACCEL(_chan) {}
+# define PERIODIC_SEND_IMU_GYRO(_chan) {}
+# define PERIODIC_SEND_IMU_MAG(_chan) {}
+# endif
#endif
#ifdef IMU_ANALOG
diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
index 4e1e12fcccf..c8be89d68c8 100644
--- a/sw/airborne/modules/ins/ins_xsens.c
+++ b/sw/airborne/modules/ins/ins_xsens.c
@@ -27,6 +27,7 @@
*/
#include "ins_module.h"
+#include "ins_xsens.h"
#include
@@ -165,11 +166,25 @@ uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD];
#define GOT_DATA 5
#define GOT_CHECKSUM 6
+// FIXME Debugging Only
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "downlink.h"
+
+
uint8_t xsens_errorcode;
uint8_t xsens_msg_status;
uint16_t xsens_time_stamp;
uint16_t xsens_output_mode;
uint32_t xsens_output_settings;
+float xsens_declination = 0;
+float xsens_gps_arm_x = 0;
+float xsens_gps_arm_y = 0;
+float xsens_gps_arm_z = 0;
+
int8_t xsens_hour;
int8_t xsens_min;
@@ -189,9 +204,12 @@ uint8_t send_ck;
struct LlaCoor_f lla_f;
struct UtmCoor_f utm_f;
+volatile int xsens_configured = 0;
+
void ins_init( void ) {
xsens_status = UNINIT;
+ xsens_configured = 20;
ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
@@ -200,23 +218,65 @@ void ins_init( void ) {
xsens_time_stamp = 0;
xsens_output_mode = XSENS_OUTPUT_MODE;
xsens_output_settings = XSENS_OUTPUT_SETTINGS;
- /* send mode and settings to MT */
- 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 ) {
+ 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;
+
+ case 6:
+ XSENS_ReqMagneticDeclination();
+ break;
+
+ case 13:
+ #ifdef AHRS_H_X
+ #warning 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
+ #warning 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;
+ }
+
RunOnceEvery(100,XSENS_ReqGPSStatus());
}
@@ -256,6 +316,7 @@ void handle_ins_msg( void) {
gps.gspeed = fspeed * 100.;
gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100);
gps.course = fcourse * 1e7;
+
}
void parse_ins_msg( void ) {
@@ -266,6 +327,18 @@ void parse_ins_msg( void ) {
else if (xsens_id == XSENS_ReqOutputSettings_ID) {
xsens_output_settings = XSENS_ReqOutputSettingsAck_settings(xsens_msg_buf);
}
+ else if (xsens_id == XSENS_ReqMagneticDeclinationAck_ID) {
+ xsens_declination = DegOfRad (XSENS_ReqMagneticDeclinationAck_declination(xsens_msg_buf) );
+
+ DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel,&xsens_declination,&xsens_declination,&xsens_gps_arm_x,&xsens_gps_arm_y,&xsens_gps_arm_z);
+ }
+ else if (xsens_id == XSENS_ReqLeverArmGpsAck_ID) {
+ xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens_msg_buf);
+ xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens_msg_buf);
+ xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens_msg_buf);
+
+ DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel,&xsens_declination,&xsens_declination,&xsens_gps_arm_x,&xsens_gps_arm_y,&xsens_gps_arm_z);
+ }
else if (xsens_id == XSENS_Error_ID) {
xsens_errorcode = XSENS_Error_errorcode(xsens_msg_buf);
}
diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h
index c40df9336fe..790e65bf5a5 100644
--- a/sw/airborne/modules/ins/ins_xsens.h
+++ b/sw/airborne/modules/ins/ins_xsens.h
@@ -42,5 +42,4 @@ extern uint8_t xsens_msg_status;
extern uint16_t xsens_time_stamp;
-
#endif