33 #include "generated/airframe.h"
40 #error "USE_GPS needs to be 1 to use the Xsens GPS!"
47 bool_t gps_xsens_msg_available;
93 #define XsensInitCheksum() { send_ck = 0; }
94 #define XsensUpdateChecksum(c) { send_ck += c; }
96 #define XsensSend1(c) { uint8_t i8=c; InsUartSend1(i8); XsensUpdateChecksum(i8); }
97 #define XsensSend1ByAddr(x) { XsensSend1(*x); }
98 #define XsensSend2ByAddr(x) { XsensSend1(*(x+1)); XsensSend1(*x); }
99 #define XsensSend4ByAddr(x) { XsensSend1(*(x+3)); XsensSend1(*(x+2)); XsensSend1(*(x+1)); XsensSend1(*x); }
101 #define XsensHeader(msg_id, len) { \
102 InsUartSend1(XSENS_START); \
103 XsensInitCheksum(); \
104 XsensSend1(XSENS_BID); \
105 XsensSend1(msg_id); \
108 #define XsensTrailer() { uint8_t i8=0x100-send_ck; InsUartSend1(i8); }
111 #include "xsens_protocol.h"
114 #define XSENS_MAX_PAYLOAD 254
138 #ifndef XSENS_OUTPUT_MODE
139 #define XSENS_OUTPUT_MODE 0x1836
167 #ifndef XSENS_OUTPUT_SETTINGS
168 #define XSENS_OUTPUT_SETTINGS 0x80000C05
177 #define GOT_CHECKSUM 6
181 #include "messages.h"
250 void ins_xsens_update_gps(
struct GpsState *gps_s);
252 void ins_xsens_init(
void)
263 static void gps_cb(
uint8_t sender_id __attribute__((unused)),
264 uint32_t stamp __attribute__((unused)),
267 ins_xsens_update_gps(gps_s);
270 void ins_xsens_register(
void)
276 void ins_xsens_update_gps(
struct GpsState *gps_s)
282 utm.alt = gps_s->
hmsl / 1000.;
303 static void gps_xsens_publish(
void)
329 XSENS_SetSyncOutSettings(0, 0x0002);
333 XSENS_SetSyncOutSettings(1, 100);
336 XSENS_ReqLeverArmGps();
340 XSENS_ReqMagneticDeclination();
345 #pragma message "Sending XSens Magnetic Declination."
351 #ifdef GPS_IMU_LEVER_ARM_X
352 #pragma message "Sending XSens GPS Arm."
353 XSENS_SetLeverArmGps(GPS_IMU_LEVER_ARM_X, GPS_IMU_LEVER_ARM_Y, GPS_IMU_LEVER_ARM_Z);
358 XSENS_SetBaudrate(baud);
363 XSENS_GoToMeasurment();
373 RunOnceEvery(100, XSENS_ReqGPSStatus());
379 static inline void update_state_interface(
void)
382 #ifdef XSENS_BACKWARDS
414 update_state_interface();
419 #ifdef XSENS_BACKWARDS
470 float fcourse = atan2f((
float)ins_vy, (
float)
ins_vx);
472 #endif // USE_GPS_XSENS
478 if (
xsens_id == XSENS_ReqOutputModeAck_ID) {
480 }
else if (
xsens_id == XSENS_ReqOutputSettings_ID) {
482 }
else if (
xsens_id == XSENS_ReqMagneticDeclinationAck_ID) {
487 }
else if (
xsens_id == XSENS_ReqLeverArmGpsAck_ID) {
494 }
else if (
xsens_id == XSENS_Error_ID) {
498 else if (
xsens_id == XSENS_GPSStatus_ID) {
517 else if (
xsens_id == XSENS_MTData_ID) {
527 offset += XSENS_DATA_RAWInertial_LENGTH;
530 #if USE_GPS_XSENS_RAW_DATA && USE_GPS_XSENS
570 offset += XSENS_DATA_RAWGPS_LENGTH;
574 offset += XSENS_DATA_Temp_LENGTH;
605 offset += l * XSENS_DATA_Calibrated_LENGTH / 3;
613 float dcm00 = 1.0 - 2 * (q2 * q2 + q3 * q3);
614 float dcm01 = 2 * (q1 * q2 + q0 * q3);
615 float dcm02 = 2 * (q1 * q3 - q0 * q2);
616 float dcm12 = 2 * (q2 * q3 + q0 * q1);
617 float dcm22 = 1.0 - 2 * (q1 * q1 + q2 * q2);
621 offset += XSENS_DATA_Quaternion_LENGTH;
627 offset += XSENS_DATA_Euler_LENGTH;
630 offset += XSENS_DATA_Matrix_LENGTH;
642 offset += l * XSENS_DATA_Auxiliary_LENGTH / 2;
645 #if (! USE_GPS_XSENS_RAW_DATA) && USE_GPS_XSENS
664 offset += XSENS_DATA_Position_LENGTH;
667 #if (! USE_GPS_XSENS_RAW_DATA) && USE_GPS_XSENS
672 offset += XSENS_DATA_Velocity_LENGTH;
687 #endif // USE_GPS_XSENS
688 offset += XSENS_DATA_Status_LENGTH;
695 offset += XSENS_DATA_TimeStamp_LENGTH;
706 offset += XSENS_DATA_UTC_LENGTH;
719 if (c != XSENS_START) {
726 if (c != XSENS_BID) {
uint8_t qi
quality bitfield (GPS receiver specific)
Event structure to store callbacks in a linked list.
int32_t north
in centimeters
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
struct SVinfo svinfos[GPS_NB_CHANNELS]
holds information from the Space Vehicles (Satellites)
#define XSENS_OUTPUT_SETTINGS
uint32_t pacc
position accuracy in cm
void imu_scale_gyro(struct Imu *_imu)
uint8_t nb_channels
Number of scanned satellites.
void gps_impl_init(void)
GPS initialization.
void imu_scale_accel(struct Imu *_imu)
uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD]
static uint8_t xsens_status
#define VECT3_ASSIGN(_a, _x, _y, _z)
#define RATES_ASSIGN(_ra, _p, _q, _r)
Main include for ABI (AirBorneInterface).
Integrated Navigation System interface.
position in UTM coordinates Units: meters
int32_t east
in centimeters
struct ImuXsens imu_xsens
uint8_t svid
Satellite ID.
uint16_t speed_3d
norm of 3d speed in cm/s
#define GPS_FIX_3D
3D GPS fix
#define INS_ROLL_NEUTRAL_DEFAULT
volatile int xsens_configured
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
uint32_t xsens_output_settings
volatile uint8_t ins_msg_received
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
int32_t alt
in millimeters above WGS84 reference ellipsoid
static uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
uint32_t sacc
speed accuracy in cm/s
uint32_t last_msg_time
cpu time in sec at last received GPS message
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
static void stateSetPositionUtm_f(struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
uint8_t zone
UTM zone number.
Paparazzi floating point math for geodetic calculations.
vector in Latitude, Longitude and Altitude
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
void parse_ins_buffer(uint8_t c)
int32_t hmsl
height above mean sea level in mm
uint8_t cno
Carrier to Noise Ratio (Signal Strength) in dbHz.
vector in North East Down coordinates Units: meters
#define MAG_BFP_OF_REAL(_af)
Architecture independent timing functions.
struct XsensTime xsens_time
data structure for GPS information
uint32_t tow
GPS time of week in ms.
#define GPS_FIX_NONE
No GPS fix.
Device independent GPS code (interface)
uint16_t pdop
position dilution of precision scaled by 100
struct Imu imu
global IMU state
int32_t lon
in degrees*1e7
uint16_t xsens_time_stamp
#define INS_PITCH_NEUTRAL_DEFAULT
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
uint8_t zone
UTM zone number.
Library for the XSENS AHRS.
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
void handle_ins_msg(void)
#define DefaultChannel
SITL.
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static float wgs84_ellipsoid_to_geoid(float lat, float lon)
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
int32_t alt
in millimeters above WGS84 reference ellipsoid
void imu_scale_mag(struct Imu *_imu)
API to get/set the generic vehicle states.
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
volatile uint8_t new_ins_attitude
volatile uint32_t nb_sec
full seconds since startup
uint16_t xsens_output_mode
Common code for AP and FBW telemetry.
uint8_t flags
bitfield with GPS receiver specific flags
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
uint8_t num_sv
number of sat in fix
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
#define ABI_BROADCAST
Broadcast address.
void imu_periodic(void)
optional.
uint16_t gspeed
norm of 2d ground speed in cm/s
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
#define ACCEL_BFP_OF_REAL(_af)
void imu_impl_init(void)
must be defined by underlying hardware
void xsens_periodic(void)
int32_t lat
in degrees*1e7
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
static uint8_t xsens_msg_idx
void ins_register_impl(InsInit init)
#define XSENS_OUTPUT_MODE
struct NedCoor_i ned_vel
speed NED in cm/s
static void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
struct GpsState gps
global GPS state
Device independent INS code.
#define LLA_FLOAT_OF_BFP(_o, _i)
#define XSENS_MAX_PAYLOAD
Includes macros generated from xsens_MTi-G.xml.
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
#define RATE_BFP_OF_REAL(_af)