Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-21 20:04:09 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[XSens] Move parser variables to struct, and call the decoder after parsing. (#2277)

Browse Source
This commit is contained in:
Christophe De Wagter
2018-06-27 00:18:18 +02:00
committed by GitHub
parent e1735db5fd
commit a419195ca2
12 changed files with 181 additions and 186 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/imu_xsens.xml
+1 -1
View File
@@ -19,7 +19,7 @@
<event fun="imu_xsens_event()"/>
<makefile target="ap">
<file name="xsens.c"/>
<file name="xsens_common.c"/>
<file name="xsens_parser.c"/>
<file name="imu_xsens.c"/>
<configure name="XSENS_PORT" default="uart1" case="upper|lower"/>
conf/modules/ins_xsens.xml
+1 -1
View File
@@ -20,7 +20,7 @@
<event fun="ins_xsens_event()"/>
<makefile target="ap">
<file name="xsens.c"/>
<file name="xsens_common.c"/>
<file name="xsens_parser.c"/>
<file name="ins_xsens.c"/>
<file name="ins.c" dir="subsystems"/>
<configure name="XSENS_PORT" default="uart1" case="upper|lower"/>
conf/modules/ins_xsens700.xml
+1 -1
View File
@@ -20,7 +20,7 @@
<event fun="ins_xsens700_event()"/>
<makefile target="ap">
<file name="xsens700.c"/>
<file name="xsens_common.c"/>
<file name="xsens_parser.c"/>
<file name="ins_xsens700.c"/>
<file name="ins.c" dir="subsystems"/>
<configure name="XSENS_PORT" default="uart1" case="upper|lower"/>
sw/airborne/modules/ins/imu_xsens.c
+3 -4
View File
@@ -27,7 +27,6 @@
#include "imu_xsens.h"
#include "xsens.h"
#include "xsens_common.h"
#include "generated/airframe.h"
@@ -43,11 +42,11 @@ void imu_xsens_init(void)
void imu_xsens_event(void)
{
xsens_event();
if (xsens.msg_received) {
xsens_parser_event(&(xsens.parser));
if (xsens.parser.msg_received) {
parse_xsens_msg();
handle_ins_msg();
xsens.msg_received = FALSE;
xsens.parser.msg_received = FALSE;
}
}
sw/airborne/modules/ins/ins_xsens.c
+3 -4
View File
@@ -25,7 +25,6 @@
*/
#include "ins_xsens.h"
#include "xsens_common.h"
#include "subsystems/ins.h"
#include "generated/airframe.h"
@@ -77,11 +76,11 @@ void ins_xsens_init(void)
void ins_xsens_event(void)
{
xsens_event();
if (xsens.msg_received) {
xsens_parser_event(&(xsens.parser));
if (xsens.parser.msg_received) {
parse_xsens_msg();
handle_ins_msg();
xsens.msg_received = false;
xsens.parser.msg_received = FALSE;
}
}
sw/airborne/modules/ins/ins_xsens700.c
+3 -4
View File
@@ -26,7 +26,6 @@
*/
#include "ins_xsens700.h"
#include "xsens_common.h"
#include "subsystems/ins.h"
#include "generated/airframe.h"
@@ -80,11 +79,11 @@ void ins_xsens700_init(void)
void ins_xsens700_event(void)
{
xsens_event();
if (xsens700.msg_received) {
xsens_parser_event(&(xsens700.parser));
if (xsens700.parser.msg_received) {
parse_xsens700_msg();
handle_ins_msg();
xsens700.msg_received = false;
xsens700.parser.msg_received = false;
}
}
sw/airborne/modules/ins/xsens.c
+65 -66
View File
@@ -24,7 +24,6 @@
*/
#include "xsens.h"
#include "xsens_common.h"
#include "generated/airframe.h"
#include "led.h"
@@ -113,7 +112,7 @@ void parse_xsens_buffer(uint8_t c);
void xsens_init(void)
{
xsens_status = UNINIT;
xsens.parser.status = UNINIT;
xsens_configured = 20;
xsens_msg_status = 0;
@@ -185,76 +184,76 @@ void xsens_periodic(void)
void parse_xsens_msg(void)
{
uint8_t offset = 0;
if (xsens_id == XSENS_ReqOutputModeAck_ID) {
xsens_output_mode = XSENS_ReqOutputModeAck_mode(xsens_msg_buf);
} 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));
if (xsens.parser.id == XSENS_ReqOutputModeAck_ID) {
xsens_output_mode = XSENS_ReqOutputModeAck_mode(xsens.parser.msg_buf);
} else if (xsens.parser.id == XSENS_ReqOutputSettings_ID) {
xsens_output_settings = XSENS_ReqOutputSettingsAck_settings(xsens.parser.msg_buf);
} else if (xsens.parser.id == XSENS_ReqMagneticDeclinationAck_ID) {
xsens_declination = DegOfRad(XSENS_ReqMagneticDeclinationAck_declination(xsens.parser.msg_buf));
DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel, DefaultDevice, &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);
} else if (xsens.parser.id == XSENS_ReqLeverArmGpsAck_ID) {
xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens.parser.msg_buf);
xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens.parser.msg_buf);
xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens.parser.msg_buf);
DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel, DefaultDevice, &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);
} else if (xsens.parser.id == XSENS_Error_ID) {
xsens_errorcode = XSENS_Error_errorcode(xsens.parser.msg_buf);
}
#if USE_GPS_XSENS
else if (xsens_id == XSENS_GPSStatus_ID) {
xsens.gps.nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
else if (xsens.parser.id == XSENS_GPSStatus_ID) {
xsens.gps.nb_channels = XSENS_GPSStatus_nch(xsens.parser.msg_buf);
xsens.gps.num_sv = 0;
uint8_t i;
// Do not write outside buffer
for (i = 0; i < Min(xsens.gps.nb_channels, GPS_NB_CHANNELS); i++) {
uint8_t ch = XSENS_GPSStatus_chn(xsens_msg_buf, i);
uint8_t ch = XSENS_GPSStatus_chn(xsens.parser.msg_buf, i);
if (ch > xsens.gps.nb_channels) { continue; }
xsens.gps.svinfos[ch].svid = XSENS_GPSStatus_svid(xsens_msg_buf, i);
xsens.gps.svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens_msg_buf, i);
xsens.gps.svinfos[ch].qi = XSENS_GPSStatus_qi(xsens_msg_buf, i);
xsens.gps.svinfos[ch].cno = XSENS_GPSStatus_cnr(xsens_msg_buf, i);
xsens.gps.svinfos[ch].svid = XSENS_GPSStatus_svid(xsens.parser.msg_buf, i);
xsens.gps.svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens.parser.msg_buf, i);
xsens.gps.svinfos[ch].qi = XSENS_GPSStatus_qi(xsens.parser.msg_buf, i);
xsens.gps.svinfos[ch].cno = XSENS_GPSStatus_cnr(xsens.parser.msg_buf, i);
if (xsens.gps.svinfos[ch].flags > 0) {
xsens.gps.num_sv++;
}
}
}
#endif
else if (xsens_id == XSENS_MTData_ID) {
else if (xsens.parser.id == XSENS_MTData_ID) {
/* test RAW modes else calibrated modes */
//if ((XSENS_MASK_RAWInertial(xsens_output_mode)) || (XSENS_MASK_RAWGPS(xsens_output_mode))) {
if (XSENS_MASK_RAWInertial(xsens_output_mode)) {
/* should we write raw data to separate struct? */
xsens.gyro.p = XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf, offset);
xsens.gyro.q = XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf, offset);
xsens.gyro.r = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf, offset);
xsens.gyro.p = XSENS_DATA_RAWInertial_gyrX(xsens.parser.msg_buf, offset);
xsens.gyro.q = XSENS_DATA_RAWInertial_gyrY(xsens.parser.msg_buf, offset);
xsens.gyro.r = XSENS_DATA_RAWInertial_gyrZ(xsens.parser.msg_buf, offset);
xsens.gyro_available = TRUE;
offset += XSENS_DATA_RAWInertial_LENGTH;
}
if (XSENS_MASK_RAWGPS(xsens_output_mode)) {
#if USE_GPS_XSENS_RAW_DATA && USE_GPS_XSENS
xsens.gps.week = 0; // FIXME
xsens.gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf, offset) * 10;
xsens.gps.lla_pos.lat = XSENS_DATA_RAWGPS_lat(xsens_msg_buf, offset);
xsens.gps.lla_pos.lon = XSENS_DATA_RAWGPS_lon(xsens_msg_buf, offset);
xsens.gps.lla_pos.alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf, offset);
xsens.gps.tow = XSENS_DATA_RAWGPS_itow(xsens.parser.msg_buf, offset) * 10;
xsens.gps.lla_pos.lat = XSENS_DATA_RAWGPS_lat(xsens.parser.msg_buf, offset);
xsens.gps.lla_pos.lon = XSENS_DATA_RAWGPS_lon(xsens.parser.msg_buf, offset);
xsens.gps.lla_pos.alt = XSENS_DATA_RAWGPS_alt(xsens.parser.msg_buf, offset);
SetBit(xsens.gps.valid_fields, GPS_VALID_POS_LLA_BIT);
// Compute geoid (MSL) height
uint32_t hmsl = wgs84_ellipsoid_to_geoid_i(xsens.gps.lla_pos.lat, xsens.gps.lla_pos.lon);
xsens.gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens_msg_buf, offset) - hmsl;
xsens.gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens.parser.msg_buf, offset) - hmsl;
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
xsens.gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf, offset);
xsens.gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf, offset);
xsens.gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf, offset);
xsens.gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens.parser.msg_buf, offset);
xsens.gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens.parser.msg_buf, offset);
xsens.gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens.parser.msg_buf, offset);
SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
xsens.gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf, offset) / 100;
xsens.gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf, offset) / 100;
xsens.gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens.parser.msg_buf, offset) / 100;
xsens.gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens.parser.msg_buf, offset) / 100;
xsens.gps.pdop = 5; // FIXME Not output by XSens
xsens.gps_available = TRUE;
@@ -268,23 +267,23 @@ void parse_xsens_msg(void)
if (XSENS_MASK_Calibrated(xsens_output_mode)) {
uint8_t l = 0;
if (!XSENS_MASK_AccOut(xsens_output_settings)) {
xsens.accel.x = XSENS_DATA_Calibrated_accX(xsens_msg_buf, offset);
xsens.accel.y = XSENS_DATA_Calibrated_accY(xsens_msg_buf, offset);
xsens.accel.z = XSENS_DATA_Calibrated_accZ(xsens_msg_buf, offset);
xsens.accel.x = XSENS_DATA_Calibrated_accX(xsens.parser.msg_buf, offset);
xsens.accel.y = XSENS_DATA_Calibrated_accY(xsens.parser.msg_buf, offset);
xsens.accel.z = XSENS_DATA_Calibrated_accZ(xsens.parser.msg_buf, offset);
xsens.accel_available = TRUE;
l++;
}
if (!XSENS_MASK_GyrOut(xsens_output_settings)) {
xsens.gyro.p = XSENS_DATA_Calibrated_gyrX(xsens_msg_buf, offset);
xsens.gyro.q = XSENS_DATA_Calibrated_gyrY(xsens_msg_buf, offset);
xsens.gyro.r = XSENS_DATA_Calibrated_gyrZ(xsens_msg_buf, offset);
xsens.gyro.p = XSENS_DATA_Calibrated_gyrX(xsens.parser.msg_buf, offset);
xsens.gyro.q = XSENS_DATA_Calibrated_gyrY(xsens.parser.msg_buf, offset);
xsens.gyro.r = XSENS_DATA_Calibrated_gyrZ(xsens.parser.msg_buf, offset);
xsens.gyro_available = TRUE;
l++;
}
if (!XSENS_MASK_MagOut(xsens_output_settings)) {
xsens.mag.x = XSENS_DATA_Calibrated_magX(xsens_msg_buf, offset);
xsens.mag.y = XSENS_DATA_Calibrated_magY(xsens_msg_buf, offset);
xsens.mag.z = XSENS_DATA_Calibrated_magZ(xsens_msg_buf, offset);
xsens.mag.x = XSENS_DATA_Calibrated_magX(xsens.parser.msg_buf, offset);
xsens.mag.y = XSENS_DATA_Calibrated_magY(xsens.parser.msg_buf, offset);
xsens.mag.z = XSENS_DATA_Calibrated_magZ(xsens.parser.msg_buf, offset);
xsens.mag_available = TRUE;
l++;
}
@@ -292,17 +291,17 @@ void parse_xsens_msg(void)
}
if (XSENS_MASK_Orientation(xsens_output_mode)) {
if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x00) {
xsens.quat.qi = XSENS_DATA_Quaternion_q0(xsens_msg_buf, offset);
xsens.quat.qx = XSENS_DATA_Quaternion_q1(xsens_msg_buf, offset);
xsens.quat.qy = XSENS_DATA_Quaternion_q2(xsens_msg_buf, offset);
xsens.quat.qz = XSENS_DATA_Quaternion_q3(xsens_msg_buf, offset);
xsens.quat.qi = XSENS_DATA_Quaternion_q0(xsens.parser.msg_buf, offset);
xsens.quat.qx = XSENS_DATA_Quaternion_q1(xsens.parser.msg_buf, offset);
xsens.quat.qy = XSENS_DATA_Quaternion_q2(xsens.parser.msg_buf, offset);
xsens.quat.qz = XSENS_DATA_Quaternion_q3(xsens.parser.msg_buf, offset);
//float_eulers_of_quat(&xsens.euler, &xsens.quat);
offset += XSENS_DATA_Quaternion_LENGTH;
}
if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x01) {
xsens.euler.phi = XSENS_DATA_Euler_roll(xsens_msg_buf, offset) * M_PI / 180;
xsens.euler.theta = XSENS_DATA_Euler_pitch(xsens_msg_buf, offset) * M_PI / 180;
xsens.euler.psi = XSENS_DATA_Euler_yaw(xsens_msg_buf, offset) * M_PI / 180;
xsens.euler.phi = XSENS_DATA_Euler_roll(xsens.parser.msg_buf, offset) * M_PI / 180;
xsens.euler.theta = XSENS_DATA_Euler_pitch(xsens.parser.msg_buf, offset) * M_PI / 180;
xsens.euler.psi = XSENS_DATA_Euler_yaw(xsens.parser.msg_buf, offset) * M_PI / 180;
offset += XSENS_DATA_Euler_LENGTH;
}
if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x10) {
@@ -321,9 +320,9 @@ void parse_xsens_msg(void)
offset += l * XSENS_DATA_Auxiliary_LENGTH / 2;
}
if (XSENS_MASK_Position(xsens_output_mode)) {
xsens.lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens_msg_buf, offset));
xsens.lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens_msg_buf, offset));
xsens.lla_f.alt = XSENS_DATA_Position_alt(xsens_msg_buf, offset);
xsens.lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens.parser.msg_buf, offset));
xsens.lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens.parser.msg_buf, offset));
xsens.lla_f.alt = XSENS_DATA_Position_alt(xsens.parser.msg_buf, offset);
offset += XSENS_DATA_Position_LENGTH;
#if (! USE_GPS_XSENS_RAW_DATA) && USE_GPS_XSENS
@@ -333,13 +332,13 @@ void parse_xsens_msg(void)
#endif
}
if (XSENS_MASK_Velocity(xsens_output_mode)) {
xsens.vel.x = XSENS_DATA_Velocity_vx(xsens_msg_buf, offset);
xsens.vel.y = XSENS_DATA_Velocity_vy(xsens_msg_buf, offset);
xsens.vel.z = XSENS_DATA_Velocity_vz(xsens_msg_buf, offset);
xsens.vel.x = XSENS_DATA_Velocity_vx(xsens.parser.msg_buf, offset);
xsens.vel.y = XSENS_DATA_Velocity_vy(xsens.parser.msg_buf, offset);
xsens.vel.z = XSENS_DATA_Velocity_vz(xsens.parser.msg_buf, offset);
offset += XSENS_DATA_Velocity_LENGTH;
}
if (XSENS_MASK_Status(xsens_output_mode)) {
xsens_msg_status = XSENS_DATA_Status_status(xsens_msg_buf, offset);
xsens_msg_status = XSENS_DATA_Status_status(xsens.parser.msg_buf, offset);
#if USE_GPS_XSENS
if (bit_is_set(xsens_msg_status, 2)) { xsens.gps.fix = GPS_FIX_3D; } // gps fix
else if (bit_is_set(xsens_msg_status, 1)) { xsens.gps.fix = 0x01; } // efk valid
@@ -355,17 +354,17 @@ void parse_xsens_msg(void)
offset += XSENS_DATA_Status_LENGTH;
}
if (XSENS_MASK_TimeStamp(xsens_output_settings)) {
xsens.time_stamp = XSENS_DATA_TimeStamp_ts(xsens_msg_buf, offset);
xsens.time_stamp = XSENS_DATA_TimeStamp_ts(xsens.parser.msg_buf, offset);
offset += XSENS_DATA_TimeStamp_LENGTH;
}
if (XSENS_MASK_UTC(xsens_output_settings)) {
xsens.time.hour = XSENS_DATA_UTC_hour(xsens_msg_buf, offset);
xsens.time.min = XSENS_DATA_UTC_min(xsens_msg_buf, offset);
xsens.time.sec = XSENS_DATA_UTC_sec(xsens_msg_buf, offset);
xsens.time.nanosec = XSENS_DATA_UTC_nanosec(xsens_msg_buf, offset);
xsens.time.year = XSENS_DATA_UTC_year(xsens_msg_buf, offset);
xsens.time.month = XSENS_DATA_UTC_month(xsens_msg_buf, offset);
xsens.time.day = XSENS_DATA_UTC_day(xsens_msg_buf, offset);
xsens.time.hour = XSENS_DATA_UTC_hour(xsens.parser.msg_buf, offset);
xsens.time.min = XSENS_DATA_UTC_min(xsens.parser.msg_buf, offset);
xsens.time.sec = XSENS_DATA_UTC_sec(xsens.parser.msg_buf, offset);
xsens.time.nanosec = XSENS_DATA_UTC_nanosec(xsens.parser.msg_buf, offset);
xsens.time.year = XSENS_DATA_UTC_year(xsens.parser.msg_buf, offset);
xsens.time.month = XSENS_DATA_UTC_month(xsens.parser.msg_buf, offset);
xsens.time.day = XSENS_DATA_UTC_day(xsens.parser.msg_buf, offset);
offset += XSENS_DATA_UTC_LENGTH;
}
sw/airborne/modules/ins/xsens.h
+4 -1
View File
@@ -32,6 +32,9 @@
#include "math/pprz_geodetic_float.h"
#include "math/pprz_geodetic_int.h"
#include "xsens_parser.h"
#if USE_GPS_XSENS
#include "subsystems/gps.h"
#endif
@@ -60,7 +63,7 @@ struct Xsens {
struct FloatQuat quat;
struct FloatEulers euler;
volatile bool msg_received;
struct XsensParser parser;
volatile bool new_attitude;
bool gyro_available;
sw/airborne/modules/ins/xsens700.c
+41 -48
View File
@@ -26,20 +26,13 @@
*/
#include "xsens700.h"
#include "xsens_common.h"
#include "generated/airframe.h"
#include "led.h"
#if USE_GPS_XSENS
#include "math/pprz_geodetic_wgs84.h"
#endif
// FIXME Debugging Only
#include "mcu_periph/uart.h"
#include "pprzlink/messages.h"
#include "subsystems/datalink/downlink.h"
uint8_t xsens_errorcode;
uint32_t xsens_msg_statusword;
@@ -57,7 +50,7 @@ volatile int xsens_configured = 0;
void xsens700_init(void)
{
xsens_status = UNINIT;
xsens.parser.status = UNINIT;
xsens_configured = 30;
xsens_msg_statusword = 0;
@@ -146,17 +139,17 @@ void xsens700_periodic(void)
void parse_xsens700_msg(void)
{
uint8_t offset = 0;
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);
} else if (xsens_id == XSENS_Error_ID) {
xsens_errorcode = XSENS_Error_errorcode(xsens_msg_buf);
} else if (xsens_id == XSENS_MTData2_ID) {
for (offset = 0; offset < xsens_len;) {
uint8_t code1 = xsens_msg_buf[offset];
uint8_t code2 = xsens_msg_buf[offset + 1];
int subpacklen = (int)xsens_msg_buf[offset + 2];
if (xsens.parser.id == XSENS_ReqLeverArmGpsAck_ID) {
xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens.parser.msg_buf);
xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens.parser.msg_buf);
xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens.parser.msg_buf);
} else if (xsens.parser.id == XSENS_Error_ID) {
xsens_errorcode = XSENS_Error_errorcode(xsens.parser.msg_buf);
} else if (xsens.parser.id == XSENS_MTData2_ID) {
for (offset = 0; offset < xsens.parser.len;) {
uint8_t code1 = xsens.parser.msg_buf[offset];
uint8_t code2 = xsens.parser.msg_buf[offset + 1];
int subpacklen = (int)xsens.parser.msg_buf[offset + 2];
offset += 3;
if (code1 == 0x10) {
@@ -171,25 +164,25 @@ void parse_xsens700_msg(void)
} else if (code1 == 0x20) {
if (code2 == 0x30) {
// Attitude Euler
xsens700.euler.phi = XSENS_DATA_Euler_roll(xsens_msg_buf, offset) * M_PI / 180;
xsens700.euler.theta = XSENS_DATA_Euler_pitch(xsens_msg_buf, offset) * M_PI / 180;
xsens700.euler.psi = XSENS_DATA_Euler_yaw(xsens_msg_buf, offset) * M_PI / 180;
xsens700.euler.phi = XSENS_DATA_Euler_roll(xsens.parser.msg_buf, offset) * M_PI / 180;
xsens700.euler.theta = XSENS_DATA_Euler_pitch(xsens.parser.msg_buf, offset) * M_PI / 180;
xsens700.euler.psi = XSENS_DATA_Euler_yaw(xsens.parser.msg_buf, offset) * M_PI / 180;
xsens700.new_attitude = TRUE;
}
} else if (code1 == 0x40) {
if (code2 == 0x10) {
// Delta-V
xsens700.accel.x = XSENS_DATA_Acceleration_x(xsens_msg_buf, offset) * 100.0f;
xsens700.accel.y = XSENS_DATA_Acceleration_y(xsens_msg_buf, offset) * 100.0f;
xsens700.accel.z = XSENS_DATA_Acceleration_z(xsens_msg_buf, offset) * 100.0f;
xsens700.accel.x = XSENS_DATA_Acceleration_x(xsens.parser.msg_buf, offset) * 100.0f;
xsens700.accel.y = XSENS_DATA_Acceleration_y(xsens.parser.msg_buf, offset) * 100.0f;
xsens700.accel.z = XSENS_DATA_Acceleration_z(xsens.parser.msg_buf, offset) * 100.0f;
}
} else if (code1 == 0x80) {
if (code2 == 0x20) {
// Rate Of Turn
xsens700.gyro.p = XSENS_DATA_RateOfTurn_x(xsens_msg_buf, offset) * M_PI / 180;
xsens700.gyro.q = XSENS_DATA_RateOfTurn_y(xsens_msg_buf, offset) * M_PI / 180;
xsens700.gyro.r = XSENS_DATA_RateOfTurn_z(xsens_msg_buf, offset) * M_PI / 180;
xsens700.gyro.p = XSENS_DATA_RateOfTurn_x(xsens.parser.msg_buf, offset) * M_PI / 180;
xsens700.gyro.q = XSENS_DATA_RateOfTurn_y(xsens.parser.msg_buf, offset) * M_PI / 180;
xsens700.gyro.r = XSENS_DATA_RateOfTurn_z(xsens.parser.msg_buf, offset) * M_PI / 180;
}
} else if (code1 == 0x30) {
if (code2 == 0x10) {
@@ -198,7 +191,7 @@ void parse_xsens700_msg(void)
} else if (code1 == 0xE0) {
if (code2 == 0x20) {
// Status Word
xsens_msg_statusword = XSENS_DATA_StatusWord_status(xsens_msg_buf, offset);
xsens_msg_statusword = XSENS_DATA_StatusWord_status(xsens.parser.msg_buf, offset);
//xsens700.gps.tow = xsens_msg_statusword;
#if USE_GPS_XSENS
if (bit_is_set(xsens_msg_statusword, 2) && bit_is_set(xsens_msg_statusword, 1)) {
@@ -212,17 +205,17 @@ void parse_xsens700_msg(void)
}
} else if (code1 == 0x88) {
if (code2 == 0x40) {
xsens700.gps.week = XSENS_DATA_GpsSol_week(xsens_msg_buf, offset);
xsens700.gps.num_sv = XSENS_DATA_GpsSol_numSv(xsens_msg_buf, offset);
xsens700.gps.pacc = XSENS_DATA_GpsSol_pAcc(xsens_msg_buf, offset);
xsens700.gps.sacc = XSENS_DATA_GpsSol_sAcc(xsens_msg_buf, offset);
xsens700.gps.pdop = XSENS_DATA_GpsSol_pDop(xsens_msg_buf, offset);
xsens700.gps.week = XSENS_DATA_GpsSol_week(xsens.parser.msg_buf, offset);
xsens700.gps.num_sv = XSENS_DATA_GpsSol_numSv(xsens.parser.msg_buf, offset);
xsens700.gps.pacc = XSENS_DATA_GpsSol_pAcc(xsens.parser.msg_buf, offset);
xsens700.gps.sacc = XSENS_DATA_GpsSol_sAcc(xsens.parser.msg_buf, offset);
xsens700.gps.pdop = XSENS_DATA_GpsSol_pDop(xsens.parser.msg_buf, offset);
} else if (code2 == 0xA0) {
// SVINFO
xsens700.gps.tow = XSENS_XDI_GpsSvInfo_iTOW(xsens_msg_buf + offset);
xsens700.gps.tow = XSENS_XDI_GpsSvInfo_iTOW(xsens.parser.msg_buf + offset);
#if USE_GPS_XSENS
xsens700.gps.nb_channels = XSENS_XDI_GpsSvInfo_nch(xsens_msg_buf + offset);
xsens700.gps.nb_channels = XSENS_XDI_GpsSvInfo_nch(xsens.parser.msg_buf + offset);
xsens700.gps.last_3dfix_ticks = sys_time.nb_sec_rem;
xsens700.gps.last_3dfix_time = sys_time.nb_sec;
@@ -230,21 +223,21 @@ void parse_xsens700_msg(void)
uint8_t i;
// Do not write outside buffer
for (i = 0; i < Min(xsens700.gps.nb_channels, GPS_NB_CHANNELS); i++) {
uint8_t ch = XSENS_XDI_GpsSvInfo_chn(xsens_msg_buf + offset, i);
uint8_t ch = XSENS_XDI_GpsSvInfo_chn(xsens.parser.msg_buf + offset, i);
if (ch > xsens700.gps.nb_channels) { continue; }
xsens700.gps.svinfos[ch].svid = XSENS_XDI_GpsSvInfo_svid(xsens_msg_buf + offset, i);
xsens700.gps.svinfos[ch].flags = XSENS_XDI_GpsSvInfo_bitmask(xsens_msg_buf + offset, i);
xsens700.gps.svinfos[ch].qi = XSENS_XDI_GpsSvInfo_qi(xsens_msg_buf + offset, i);
xsens700.gps.svinfos[ch].cno = XSENS_XDI_GpsSvInfo_cnr(xsens_msg_buf + offset, i);
xsens700.gps.svinfos[ch].svid = XSENS_XDI_GpsSvInfo_svid(xsens.parser.msg_buf + offset, i);
xsens700.gps.svinfos[ch].flags = XSENS_XDI_GpsSvInfo_bitmask(xsens.parser.msg_buf + offset, i);
xsens700.gps.svinfos[ch].qi = XSENS_XDI_GpsSvInfo_qi(xsens.parser.msg_buf + offset, i);
xsens700.gps.svinfos[ch].cno = XSENS_XDI_GpsSvInfo_cnr(xsens.parser.msg_buf + offset, i);
}
#endif
}
} else if (code1 == 0x50) {
if (code2 == 0x10) {
//xsens700.gps.hmsl = XSENS_DATA_Altitude_h(xsens_msg_buf,offset)* 1000.0f;
//xsens700.gps.hmsl = XSENS_DATA_Altitude_h(xsens.parser.msg_buf,offset)* 1000.0f;
} else if (code2 == 0x20) {
// Altitude Elipsoid
xsens700.gps.lla_pos.alt = XSENS_DATA_Altitude_h(xsens_msg_buf, offset) * 1000.0f;
xsens700.gps.lla_pos.alt = XSENS_DATA_Altitude_h(xsens.parser.msg_buf, offset) * 1000.0f;
// Compute geoid (MSL) height
float geoid_h = wgs84_ellipsoid_to_geoid_f(xsens700.lla_f.lat, xsens700.lla_f.lon);
@@ -261,15 +254,15 @@ void parse_xsens700_msg(void)
xsens700.gps.last_3dfix_time = sys_time.nb_sec;
xsens700.gps.week = 0; // FIXME
xsens700.lla_f.lat = RadOfDeg(XSENS_DATA_LatLon_lat(xsens_msg_buf, offset));
xsens700.lla_f.lon = RadOfDeg(XSENS_DATA_LatLon_lon(xsens_msg_buf, offset));
xsens700.lla_f.lat = RadOfDeg(XSENS_DATA_LatLon_lat(xsens.parser.msg_buf, offset));
xsens700.lla_f.lon = RadOfDeg(XSENS_DATA_LatLon_lon(xsens.parser.msg_buf, offset));
}
} else if (code1 == 0xD0) {
if (code2 == 0x10) {
// Velocity
xsens700.vel.x = XSENS_DATA_VelocityXYZ_x(xsens_msg_buf, offset);
xsens700.vel.y = XSENS_DATA_VelocityXYZ_y(xsens_msg_buf, offset);
xsens700.vel.z = XSENS_DATA_VelocityXYZ_z(xsens_msg_buf, offset);
xsens700.vel.x = XSENS_DATA_VelocityXYZ_x(xsens.parser.msg_buf, offset);
xsens700.vel.y = XSENS_DATA_VelocityXYZ_y(xsens.parser.msg_buf, offset);
xsens700.vel.z = XSENS_DATA_VelocityXYZ_z(xsens.parser.msg_buf, offset);
xsens700.gps.ned_vel.x = xsens700.vel.x;
xsens700.gps.ned_vel.y = xsens700.vel.y;
xsens700.gps.ned_vel.z = xsens700.vel.x;
sw/airborne/modules/ins/xsens700.h
+4 -1
View File
@@ -32,6 +32,9 @@
#include "math/pprz_geodetic_float.h"
#include "math/pprz_geodetic_int.h"
#include "xsens_parser.h"
#if USE_GPS_XSENS
#include "subsystems/gps.h"
#endif
@@ -60,7 +63,7 @@ struct Xsens {
struct FloatQuat quat;
struct FloatEulers euler;
volatile bool msg_received;
struct XsensParser parser;
volatile bool new_attitude;
bool gyro_available;
sw/airborne/modules/ins/xsens_common.c → sw/airborne/modules/ins/xsens_parser.c
+25 -33
View File
@@ -23,75 +23,67 @@
* Parser for the Xsens protocol.
*/
#include "xsens_common.h"
#include "xsens_parser.h"
#include "pprzlink/pprzlink_device.h"
#include "mcu_periph/uart.h"
volatile uint8_t xsens_msg_received;
uint8_t xsens_id;
uint8_t xsens_status;
uint8_t xsens_len;
uint8_t xsens_msg_idx;
uint8_t ck;
uint8_t send_ck;
uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD];
void xsens_parser_func(struct XsensParser *xsens, uint8_t c);
void parse_xsens_buffer(uint8_t c);
void xsens_event(void)
void xsens_parser_event(struct XsensParser *xsensparser)
{
struct link_device *dev = &((XSENS_LINK).device);
if (dev->char_available(dev->periph)) {
while (dev->char_available(dev->periph) && !xsens_msg_received) {
parse_xsens_buffer(dev->get_byte(dev->periph));
while (dev->char_available(dev->periph) && !xsensparser->msg_received) {
xsens_parser_func(xsensparser, dev->get_byte(dev->periph));
}
}
}
void parse_xsens_buffer(uint8_t c)
void xsens_parser_func(struct XsensParser *xsens, uint8_t c)
{
ck += c;
switch (xsens_status) {
xsens->ck += c;
switch (xsens->status) {
case UNINIT:
if (c != XSENS_START) {
goto error;
}
xsens_status++;
ck = 0;
xsens->status++;
xsens->ck = 0;
break;
case GOT_START:
if (c != XSENS_BID) {
goto error;
}
xsens_status++;
xsens->status++;
break;
case GOT_BID:
xsens_id = c;
xsens_status++;
xsens->id = c;
xsens->status++;
break;
case GOT_MID:
xsens_len = c;
if (xsens_len > XSENS_MAX_PAYLOAD) {
xsens->len = c;
if (xsens->len > XSENS_MAX_PAYLOAD) {
goto error;
}
xsens_msg_idx = 0;
xsens_status++;
xsens->msg_idx = 0;
xsens->status++;
break;
case GOT_LEN:
xsens_msg_buf[xsens_msg_idx] = c;
xsens_msg_idx++;
if (xsens_msg_idx >= xsens_len) {
xsens_status++;
xsens->msg_buf[xsens->msg_idx] = c;
xsens->msg_idx++;
if (xsens->msg_idx >= xsens->len) {
xsens->status++;
}
break;
case GOT_DATA:
if (ck != 0) {
if (xsens->ck != 0) {
goto error;
}
xsens_msg_received = TRUE;
xsens->msg_received = TRUE;
goto restart;
break;
default:
@@ -100,6 +92,6 @@ void parse_xsens_buffer(uint8_t c)
return;
error:
restart:
xsens_status = UNINIT;
xsens->status = UNINIT;
return;
}
sw/airborne/modules/ins/xsens_common.h → sw/airborne/modules/ins/xsens_parser.h
+30 -22
View File
@@ -19,23 +19,42 @@
* Boston, MA 02111-1307, USA.
*/
/** @file modules/ins/xsens_common.h
* Parser for the Xsens protocol.
/** @file modules/ins/xsens_parser.h
* Parser for the XSens protocol.
* Transmit XSens protocol.
*/
#ifndef XSENS_COMMON_H
#define XSENS_COMMON_H
#ifndef XSENS_PARSER_H
#define XSENS_PARSER_H
#include "std.h"
/** Includes macros generated from xsens_MTi-G.xml */
#include "xsens_protocol.h"
extern uint8_t xsens_id;
extern uint8_t xsens_status;
extern uint8_t xsens_len;
extern uint8_t xsens_msg_idx;
extern uint8_t ck;
#define XSENS_MAX_PAYLOAD 254
// Status
#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
struct XsensParser {
volatile uint8_t msg_received;
uint8_t id;
uint8_t status;
uint8_t len;
uint8_t msg_idx;
uint8_t ck;
uint8_t msg_buf[XSENS_MAX_PAYLOAD];
};
// Only 1 send allowed at the same time:
extern uint8_t send_ck;
#define XsensLinkDevice (&((XSENS_LINK).device))
@@ -59,17 +78,6 @@ extern uint8_t send_ck;
#define XsensTrailer() { uint8_t i8=0x100-send_ck; XsensUartSend1(i8); }
#define XSENS_MAX_PAYLOAD 254
extern uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD];
extern void xsens_parser_event(struct XsensParser *xsensparser);
#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
extern void xsens_event(void);
#endif /* XSENS_COMMON_H */
#endif /* XSENS_PARSER_H */