XSens gps-imu arm setting + declination + pulsepersecond + IMU telemetry

conf/airframes/TU_Delft/skywalker.xml

@@ -19,7 +19,7 @@
     <axis name="ROLL" failsafe_value="0"/>
     <axis name="YAW" failsafe_value="0"/>
     <axis name="PITCH" failsafe_value="0"/>
-    <axis name="BRAKE" failsafe_value="0"/>	<!-- both elerons up as butterfly brake ? -->
+    <axis name="BRAKE" failsafe_value="9600"/>	<!-- both elerons up as butterfly brake ? -->
   </commands>
 
   <rc_commands>
@@ -43,7 +43,9 @@
 
     <define name="BRAKE_AILEVON" value="-0.7f"/>
     <define name="BRAKE_PITCH" value="0.1f"/>
-    <define name="MAX_BRAKE_RATE" value="150"/>
+    <define name="MAX_BRAKE_RATE" value="130"/>
+
+    <define name="RUDDER_OF_AILERON" value="0.3"/>
 
     <define name="LIMIT(X,XL,XH)" value="(    ((X)>(XH))   ?    (XH)    :    ( ((X)>(XL)) ? (X) : (XL) )    )"/>
 
@@ -67,13 +69,26 @@
     <set servo="AILERON_LEFT" value="($aileron_up * $leftturn) + ($aileron_down * $rightturn) - $brake_value*(BRAKE_AILEVON) - (MAX_PPRZ * AILERON_NEUTRAL)"/>
     <set servo="AILERON_RIGHT" value="($aileron_up * $rightturn) + ($aileron_down * $leftturn) + $brake_value*(BRAKE_AILEVON) + (MAX_PPRZ *AILERON_NEUTRAL)"/>
 
-    <set servo="RUDDER" value="@YAW + @ROLL * 0.3"/>
+    <set servo="RUDDER" value="@YAW + @ROLL * RUDDER_OF_AILERON"/>
     <set servo="THROTTLE" value="@THROTTLE"/>
 
     <!-- Pitch with Brake-Trim Function -->
     <set servo="ELEVATOR" value="@PITCH * PITCH_GAIN - BRAKE_PITCH * $brake_value"/>
   </command_laws>
 
+  <!-- Local magnetic field -->
+  <section name="AHRS" prefix="AHRS_" >
+    <define name="H_X" value="0.51562740288882" />
+    <define name="H_Y" value="-0.05707735220832" />
+    <define name="H_Z" value="0.85490967783446" />
+  </section>
+
+  <section name="XSENS">
+    <define name="GPS_IMU_LEVER_ARM_X" value="-0.285f" />
+    <define name="GPS_IMU_LEVER_ARM_Y" value="0.0f" />
+    <define name="GPS_IMU_LEVER_ARM_Z" value="0.0f" />
+  </section>
+
   <section name="AUTO1" prefix="AUTO1_">
     <define name="MAX_ROLL" value="RadOfDeg(75)"/>
     <define name="MAX_PITCH" value="RadOfDeg(45)"/>
@@ -93,7 +108,7 @@
   </section>
 
   <section name="MISC">
-    <define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
+    <define name="NOMINAL_AIRSPEED" value="14." unit="m/s"/>
     <define name="CARROT" value="5." unit="s"/>
     <define name="CONTROL_RATE" value="60" unit="Hz"/>
     <define name="XBEE_INIT" value="&quot;ATPL2\rATRN5\rATTT80\r&quot;"/>
@@ -148,18 +163,16 @@
     <define name="ROLL_RATE_GAIN" value="-820.312011719"/>
   </section>
 
-<!--
   <section name="AGGRESSIVE" prefix="AGR_">
-    <define name="BLEND_START" value="20"/>
+    <define name="BLEND_START" value="30"/>
-    <define name="BLEND_END" value="10"/>
+    <define name="BLEND_END" value="15"/>
     <define name="CLIMB_THROTTLE" value="1.00"/>
-    <define name="CLIMB_PITCH" value="0.3"/>
+    <define name="CLIMB_PITCH" value="RadOfDeg(30)"/>
-    <define name="DESCENT_THROTTLE" value="0.1"/>
+    <define name="DESCENT_THROTTLE" value="0.0"/>
-    <define name="DESCENT_PITCH" value="-0.25"/>
+    <define name="DESCENT_PITCH" value="RadOfDeg(-15)"/>
     <define name="CLIMB_NAV_RATIO" value="0.8"/>
     <define name="DESCENT_NAV_RATIO" value="1.0"/>
   </section>
--->
 
   <section name="FAILSAFE" prefix="FAILSAFE_">
     <define name="DEFAULT_THROTTLE" value="0.35" unit="%"/>

conf/messages.xml

@@ -1567,7 +1567,14 @@
     <field name="mz" type="int32" unit="adc"/>
   </message>
 
-  <!--206 is free -->
+  <message name="IMU_MAG_SETTINGS" id="206">
+    <field name="inclination" type="float" />
+    <field name="declination" type="float" />
+    <field name="hardiron_x" type="float" />
+    <field name="hardiron_y" type="float" />
+    <field name="hardiron_z" type="float" />
+  </message>
+
   <!--207 is free -->
   <!--208 is free -->
 

conf/modules/ins_xsens_MTiG_fixedwing.xml

@@ -11,6 +11,7 @@
   <event fun="InsEventCheckAndHandle(handle_ins_msg())"/>
   <makefile>
     <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"/>

conf/xsens_MTi-G.xml

@@ -123,6 +123,18 @@
   </message>
   <message name="SetMagneticDeclinationAck" ID="0x6B" to="host"/>
 
+  <message name="ReqLeverArmGps" ID="0x68" to="MT"/>
+  <message name="SetLeverArmGps" ID="0x68" to="MT" length="12">
+    <field name="x" format="R4" unit="m"/>
+    <field name="y" format="R4" unit="m"/>
+    <field name="z" format="R4" unit="m"/>
+  </message>
+  <message name="ReqLeverArmGpsAck" ID="0x69" to="host" length="12">
+    <field name="x" format="R4" unit="m"/>
+    <field name="y" format="R4" unit="m"/>
+    <field name="z" format="R4" unit="m"/>
+  </message>
+
   <message name="ReqGPSStatus" ID="0xA6" to="MT"/>
   <message name="GPSStatus" ID="0xA7" to="host">
     <field name="nch" format="U1"/>

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"
+#  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_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_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_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_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)}
+#  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) {}
+#  ifdef INS_MODULE_H
-#define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {}
+#  include "modules/ins/ins_module.h"
-#define PERIODIC_SEND_IMU_MAG_RAW(_chan) {}
+#    define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {}
-#define PERIODIC_SEND_IMU_ACCEL(_chan) {}
+#    define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {}
-#define PERIODIC_SEND_IMU_GYRO(_chan) {}
+#    define PERIODIC_SEND_IMU_MAG_RAW(_chan) {}
-#define PERIODIC_SEND_IMU_MAG(_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

sw/airborne/modules/ins/ins_xsens.c

@@ -27,6 +27,7 @@
  */
 
 #include "ins_module.h"
+#include "ins_xsens.h"
 
 #include <inttypes.h>
 
@@ -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);
   }

sw/airborne/modules/ins/ins_xsens.h

@@ -42,5 +42,4 @@ extern uint8_t xsens_msg_status;
 extern uint16_t xsens_time_stamp;
 
 
-
 #endif