Parser for the Xsens protocol. More...

#include "ins_module.h"
#include "ins_xsens.h"
#include "subsystems/ins.h"
#include <inttypes.h>
#include "generated/airframe.h"
#include "mcu_periph/sys_time.h"
#include "messages.h"
#include "xsens_protocol.h"
#include "mcu_periph/uart.h"
#include "subsystems/datalink/downlink.h"

Include dependency graph for ins_xsens.c:

Go to the source code of this file.

Macros
#define	XsensInitCheksum() { send_ck = 0; }

#define	XsensUpdateChecksum(c) { send_ck += c; }

#define	XsensSend1(c) { uint8_t i8=c; InsUartSend1(i8); XsensUpdateChecksum(i8); }

#define	XsensSend1ByAddr(x) { XsensSend1(*x); }

#define	XsensSend2ByAddr(x) { XsensSend1((x+1)); XsensSend1(x); }

#define	XsensSend4ByAddr(x) { XsensSend1((x+3)); XsensSend1((x+2)); XsensSend1((x+1)); XsensSend1(x); }

#define	XsensHeader(msg_id, len)

#define	XsensTrailer() { uint8_t i8=0x100-send_ck; InsUartSend1(i8); }

#define	XSENS_MAX_PAYLOAD 254
	Includes macros generated from xsens_MTi-G.xml. More...

#define	XSENS_OUTPUT_MODE 0x1836

#define	XSENS_OUTPUT_SETTINGS 0x80000C05

#define	UNINIT 0

#define	GOT_START 1

#define	GOT_BID 2

#define	GOT_MID 3

#define	GOT_LEN 4

#define	GOT_DATA 5

#define	GOT_CHECKSUM 6

Functions
void	xsens_init (void)

void	imu_impl_init (void)
	must be defined by underlying hardware More...

void	imu_periodic (void)
	optional. More...

void	xsens_periodic (void)

void	handle_ins_msg (void)

void	parse_ins_msg (void)

void	parse_ins_buffer (uint8_t c)

Variables
INS_FORMAT	ins_x

INS_FORMAT	ins_y

INS_FORMAT	ins_z

INS_FORMAT	ins_vx

INS_FORMAT	ins_vy

INS_FORMAT	ins_vz

INS_FORMAT	ins_phi

INS_FORMAT	ins_theta

INS_FORMAT	ins_psi

INS_FORMAT	ins_p

INS_FORMAT	ins_q

INS_FORMAT	ins_r

INS_FORMAT	ins_ax

INS_FORMAT	ins_ay

INS_FORMAT	ins_az

INS_FORMAT	ins_mx

INS_FORMAT	ins_my

INS_FORMAT	ins_mz

volatile uint8_t	ins_msg_received

uint8_t	xsens_msg_buf [XSENS_MAX_PAYLOAD]

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

struct XsensTime	xsens_time

static uint8_t	xsens_id

static uint8_t	xsens_status

static uint8_t	xsens_len

static uint8_t	xsens_msg_idx

static uint8_t	ck

uint8_t	send_ck

volatile int	xsens_configured = 0

struct ImuXsens	imu_xsens

Detailed Description

Parser for the Xsens protocol.

Definition in file ins_xsens.c.

Macro Definition Documentation

#define GOT_BID 2

Definition at line 173 of file ins_xsens.c.

Referenced by parse_ins_buffer().

#define GOT_CHECKSUM 6

Definition at line 177 of file ins_xsens.c.

#define GOT_DATA 5

Definition at line 176 of file ins_xsens.c.

Referenced by parse_ins_buffer().

#define GOT_LEN 4

Definition at line 175 of file ins_xsens.c.

Referenced by parse_ins_buffer().

#define GOT_MID 3

Definition at line 174 of file ins_xsens.c.

Referenced by parse_ins_buffer().

#define GOT_START 1

Definition at line 172 of file ins_xsens.c.

Referenced by parse_ins_buffer().

#define UNINIT 0

Definition at line 171 of file ins_xsens.c.

Referenced by parse_ins_buffer(), and xsens_init().

#define XSENS_MAX_PAYLOAD 254

Includes macros generated from xsens_MTi-G.xml.

Definition at line 114 of file ins_xsens.c.

Referenced by parse_ins_buffer().

#define XSENS_OUTPUT_MODE 0x1836

Definition at line 139 of file ins_xsens.c.

Referenced by xsens_init().

#define XSENS_OUTPUT_SETTINGS 0x80000C05

Definition at line 168 of file ins_xsens.c.

Referenced by xsens_init().

#define XsensHeader	(	msg_id,
		len
	)

Value:

{ \
    InsUartSend1(XSENS_START); \
    XsensInitCheksum(); \
    XsensSend1(XSENS_BID); \
    XsensSend1(msg_id); \
    XsensSend1(len); \
  }

Definition at line 101 of file ins_xsens.c.

#define XsensInitCheksum ( ) { send_ck = 0; }

Definition at line 93 of file ins_xsens.c.

#define XsensSend1 ( c ) { uint8_t i8=c; InsUartSend1(i8); XsensUpdateChecksum(i8); }

Definition at line 96 of file ins_xsens.c.

#define XsensSend1ByAddr ( x ) { XsensSend1(*x); }

Definition at line 97 of file ins_xsens.c.

#define XsensSend2ByAddr ( x ) { XsensSend1(*(x+1)); XsensSend1(*x); }

Definition at line 98 of file ins_xsens.c.

#define XsensSend4ByAddr ( x ) { XsensSend1(*(x+3)); XsensSend1(*(x+2)); XsensSend1(*(x+1)); XsensSend1(*x); }

Definition at line 99 of file ins_xsens.c.

#define XsensTrailer ( ) { uint8_t i8=0x100-send_ck; InsUartSend1(i8); }

Definition at line 108 of file ins_xsens.c.

#define XsensUpdateChecksum ( c ) { send_ck += c; }

Definition at line 94 of file ins_xsens.c.

Function Documentation

void handle_ins_msg ( void )

Definition at line 410 of file ins_xsens.c.

References Imu::accel, ImuXsens::accel_available, ACCEL_BFP_OF_REAL, Imu::accel_unscaled, GpsState::course, FALSE, GpsState::fix, get_sys_time_usec(), gps, GPS_FIX_3D, GpsState::gspeed, Imu::gyro, ImuXsens::gyro_available, Imu::gyro_unscaled, imu, imu_scale_accel(), imu_scale_gyro(), imu_scale_mag(), imu_xsens, IMU_XSENS_ID, ins_ax, ins_ay, ins_az, ins_mx, ins_my, ins_mz, ins_p, ins_q, ins_r, ins_vx, ins_vy, ins_vz, Imu::mag, ImuXsens::mag_available, MAG_BFP_OF_REAL, Imu::mag_unscaled, RATE_BFP_OF_REAL, RATES_ASSIGN, GpsState::speed_3d, and VECT3_ASSIGN.

Here is the call graph for this function:

void imu_impl_init ( void )

must be defined by underlying hardware

Definition at line 235 of file ins_xsens.c.

References ImuXsens::accel_available, FALSE, ImuXsens::gyro_available, imu_xsens, ImuXsens::mag_available, and xsens_init().

Here is the call graph for this function:

void imu_periodic ( void )

optional.

If not provided by implementation, empty function is used

Definition at line 243 of file ins_xsens.c.

References xsens_periodic().

Here is the call graph for this function:

void parse_ins_buffer ( uint8_t c )

Definition at line 714 of file ins_xsens.c.

References ck, GOT_BID, GOT_DATA, GOT_LEN, GOT_MID, GOT_START, ins_msg_received, TRUE, UNINIT, xsens_id, xsens_len, XSENS_MAX_PAYLOAD, xsens_msg_buf, xsens_msg_idx, and xsens_status.

void parse_ins_msg ( void )

inline

Definition at line 475 of file ins_xsens.c.

References ImuXsens::accel_available, LlaCoor_i::alt, UtmCoor_i::alt, SVinfo::cno, XsensTime::day, DefaultChannel, DefaultDevice, UtmCoor_f::east, UtmCoor_i::east, GpsState::fix, SVinfo::flags, gps, GPS_FIX_3D, GPS_FIX_NONE, GPS_NB_CHANNELS, ImuXsens::gyro_available, GpsState::hmsl, XsensTime::hour, imu_xsens, ins_ax, ins_ay, ins_az, INS_FORMAT, ins_mx, ins_my, ins_mz, ins_p, ins_phi, ins_psi, ins_q, ins_r, ins_theta, ins_vx, ins_vy, ins_vz, ins_x, ins_y, ins_z, LlaCoor_f::lat, LlaCoor_i::lat, LED_ON, LED_TOGGLE, lla_f, LLA_FLOAT_OF_BFP, GpsState::lla_pos, LlaCoor_f::lon, LlaCoor_i::lon, ImuXsens::mag_available, XsensTime::min, Min, XsensTime::month, XsensTime::nanosec, nav_utm_zone0, GpsState::nb_channels, GpsState::ned_vel, new_ins_attitude, UtmCoor_f::north, UtmCoor_i::north, GpsState::num_sv, GpsState::pacc, GpsState::pdop, SVinfo::qi, GpsState::sacc, XsensTime::sec, SVinfo::svid, GpsState::svinfos, GpsState::tow, TRUE, utm_f, utm_of_lla_f(), GpsState::utm_pos, GpsState::week, wgs84_ellipsoid_to_geoid(), NedCoor_i::x, xsens_declination, xsens_errorcode, xsens_gps_arm_x, xsens_gps_arm_y, xsens_gps_arm_z, xsens_id, xsens_msg_buf, xsens_msg_status, xsens_output_mode, xsens_output_settings, xsens_time, xsens_time_stamp, NedCoor_i::y, XsensTime::year, NedCoor_i::z, UtmCoor_f::zone, and UtmCoor_i::zone.

Here is the call graph for this function:

void xsens_init ( void )

Definition at line 215 of file ins_xsens.c.

References ins_pitch_neutral, INS_PITCH_NEUTRAL_DEFAULT, ins_roll_neutral, INS_ROLL_NEUTRAL_DEFAULT, UNINIT, xsens_configured, xsens_msg_status, XSENS_OUTPUT_MODE, xsens_output_mode, XSENS_OUTPUT_SETTINGS, xsens_output_settings, xsens_status, and xsens_time_stamp.

Referenced by imu_impl_init().

Here is the caller graph for this function:

void xsens_periodic ( void )

Definition at line 317 of file ins_xsens.c.

Referenced by imu_periodic().

Here is the caller graph for this function:

Variable Documentation

uint8_t ck

static

Definition at line 208 of file ins_xsens.c.

Referenced by parse_ins_buffer().

struct ImuXsens imu_xsens

Definition at line 233 of file ins_xsens.c.

Referenced by handle_ins_msg(), imu_impl_init(), and parse_ins_msg().

INS_FORMAT ins_ax

Definition at line 71 of file ins_xsens.c.

Referenced by handle_ins_msg(), and parse_ins_msg().

INS_FORMAT ins_ay

Definition at line 72 of file ins_xsens.c.

Referenced by handle_ins_msg(), and parse_ins_msg().

INS_FORMAT ins_az

Definition at line 73 of file ins_xsens.c.

Referenced by handle_ins_msg(), and parse_ins_msg().

volatile uint8_t ins_msg_received

Definition at line 91 of file ins_xsens.c.

Referenced by parse_ins_buffer().

INS_FORMAT ins_mx

Definition at line 76 of file ins_xsens.c.

Referenced by handle_ins_msg(), and parse_ins_msg().

INS_FORMAT ins_my

Definition at line 77 of file ins_xsens.c.

Referenced by handle_ins_msg(), and parse_ins_msg().

INS_FORMAT ins_mz

Definition at line 78 of file ins_xsens.c.

Referenced by handle_ins_msg(), and parse_ins_msg().

INS_FORMAT ins_p

Definition at line 66 of file ins_xsens.c.

Referenced by handle_ins_msg(), and parse_ins_msg().

INS_FORMAT ins_phi

Definition at line 61 of file ins_xsens.c.

Referenced by parse_ins_msg().

INS_FORMAT ins_psi

Definition at line 63 of file ins_xsens.c.

Referenced by parse_ins_msg().

INS_FORMAT ins_q

Definition at line 67 of file ins_xsens.c.

Referenced by handle_ins_msg(), and parse_ins_msg().

INS_FORMAT ins_r

Definition at line 68 of file ins_xsens.c.

Referenced by handle_ins_msg(), and parse_ins_msg().

INS_FORMAT ins_theta

Definition at line 62 of file ins_xsens.c.

Referenced by parse_ins_msg().

INS_FORMAT ins_vx

Definition at line 56 of file ins_xsens.c.

Referenced by handle_ins_msg(), and parse_ins_msg().

INS_FORMAT ins_vy

Definition at line 57 of file ins_xsens.c.

Referenced by handle_ins_msg(), and parse_ins_msg().

INS_FORMAT ins_vz

Definition at line 58 of file ins_xsens.c.

Referenced by handle_ins_msg(), and parse_ins_msg().

INS_FORMAT ins_x

Definition at line 51 of file ins_xsens.c.

Referenced by parse_ins_msg().

INS_FORMAT ins_y

Definition at line 52 of file ins_xsens.c.

Referenced by parse_ins_msg().

INS_FORMAT ins_z

Definition at line 53 of file ins_xsens.c.

Referenced by parse_ins_msg().

uint8_t send_ck

Definition at line 209 of file ins_xsens.c.

volatile int xsens_configured = 0

Definition at line 211 of file ins_xsens.c.

Referenced by xsens_init(), and xsens_periodic().

float xsens_declination = 0

Definition at line 192 of file ins_xsens.c.

Referenced by parse_ins_msg(), and xsens_periodic().

uint8_t xsens_errorcode

Definition at line 185 of file ins_xsens.c.

Referenced by parse_ins_msg().

float xsens_gps_arm_x = 0

Definition at line 193 of file ins_xsens.c.

Referenced by parse_ins_msg().

float xsens_gps_arm_y = 0

Definition at line 194 of file ins_xsens.c.

Referenced by parse_ins_msg().

float xsens_gps_arm_z = 0

Definition at line 195 of file ins_xsens.c.

Referenced by parse_ins_msg().

uint8_t xsens_id

static

Definition at line 204 of file ins_xsens.c.

Referenced by parse_ins_buffer(), and parse_ins_msg().

uint8_t xsens_len

static

Definition at line 206 of file ins_xsens.c.

Referenced by parse_ins_buffer().

uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD]

Definition at line 115 of file ins_xsens.c.

Referenced by parse_ins_buffer(), and parse_ins_msg().

uint8_t xsens_msg_idx

static

Definition at line 207 of file ins_xsens.c.

Referenced by parse_ins_buffer().

uint8_t xsens_msg_status

Definition at line 186 of file ins_xsens.c.

Referenced by parse_ins_msg(), and xsens_init().

uint16_t xsens_output_mode

Definition at line 188 of file ins_xsens.c.

Referenced by parse_ins_msg(), xsens_init(), and xsens_periodic().

uint32_t xsens_output_settings

Definition at line 189 of file ins_xsens.c.

Referenced by parse_ins_msg(), xsens_init(), and xsens_periodic().

uint8_t xsens_status

static

Definition at line 205 of file ins_xsens.c.

Referenced by parse_ins_buffer(), and xsens_init().

struct XsensTime xsens_time

Definition at line 202 of file ins_xsens.c.

Referenced by parse_ins_msg().

uint16_t xsens_time_stamp

Definition at line 187 of file ins_xsens.c.

Referenced by parse_ins_msg(), and xsens_init().

Macros

Functions

Variables

Detailed Description

Macro Definition Documentation

Function Documentation

Variable Documentation