Skip to content

Commit

Permalink
XSens gps-imu arm setting + declination + pulsepersecond + IMU telemetry
Browse files Browse the repository at this point in the history
  • Loading branch information
@dewagter
dewagter committed May 13, 2011
1 parent 76ffcf4 commit afba2ad
Show file tree
Hide file tree
Showing 7 changed files with 153 additions and 38 deletions.
35 changes: 24 additions & 11 deletions conf/airframes/TU_Delft/skywalker.xml
View file
Expand Up @@ -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>
Expand All @@ -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) ) )"/>

Expand All @@ -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)"/>
Expand All @@ -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;"/>
Expand Down Expand Up @@ -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_END" value="10"/>
<define name="BLEND_START" value="30"/>
<define name="BLEND_END" value="15"/>
<define name="CLIMB_THROTTLE" value="1.00"/>
<define name="CLIMB_PITCH" value="0.3"/>
<define name="DESCENT_THROTTLE" value="0.1"/>
<define name="DESCENT_PITCH" value="-0.25"/>
<define name="CLIMB_PITCH" value="RadOfDeg(30)"/>
<define name="DESCENT_THROTTLE" value="0.0"/>
<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="%"/>
Expand Down
9 changes: 8 additions & 1 deletion conf/messages.xml
View file
Expand Up @@ -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 -->

Expand Down
1 change: 1 addition & 0 deletions conf/modules/ins_xsens_MTiG_fixedwing.xml
View file
Expand Up @@ -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"/>
Expand Down
12 changes: 12 additions & 0 deletions conf/xsens_MTi-G.xml
View file
Expand Up @@ -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"/>
Expand Down
36 changes: 23 additions & 13 deletions sw/airborne/ap_downlink.h
View file
Expand Up @@ -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
Expand Down
97 changes: 85 additions & 12 deletions sw/airborne/modules/ins/ins_xsens.c
View file
Expand Up @@ -27,6 +27,7 @@
*/

#include "ins_module.h"
#include "ins_xsens.h"

#include <inttypes.h>

Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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());
}

Expand Down Expand Up @@ -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 ) {
Expand All @@ -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);
}
Expand Down
1 change: 0 additions & 1 deletion sw/airborne/modules/ins/ins_xsens.h
View file
Expand Up @@ -42,5 +42,4 @@ extern uint8_t xsens_msg_status;
extern uint16_t xsens_time_stamp;



#endif

0 comments on commit afba2ad

Please sign in to comment.