/**
* Calculate LLA (int) from any other available representation.
* Note that since LLA in float has bad precision this is the last choice.
* So we mostly first convert to ECEF and then use lla_of_ecef_i
* which provides higher precision but is currently using the double function internally.
*/
void stateCalcPositionLla_i(void)
{
if (bit_is_set(state.pos_status, POS_LLA_I)) {
return;
}
if (bit_is_set(state.pos_status, POS_ECEF_I)) {
lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
} else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
/* transform ecef_f -> ecef_i -> lla_i, set status bits */
ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
SetBit(state.pos_status, POS_ECEF_I);
lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
} else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_i) {
/* transform ned_i -> ecef_i -> lla_i, set status bits */
ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
SetBit(state.pos_status, POS_ECEF_I);
lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
} else if (bit_is_set(state.pos_status, POS_ENU_I) && state.ned_initialized_i) {
/* transform enu_i -> ecef_i -> lla_i, set status bits */
ecef_of_enu_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.enu_pos_i);
SetBit(state.pos_status, POS_ECEF_I);
lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
} else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_i) {
/* transform ned_f -> ned_i -> ecef_i -> lla_i, set status bits */
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
SetBit(state.pos_status, POS_NED_I);
ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
SetBit(state.pos_status, POS_ECEF_I);
lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
} else if (bit_is_set(state.pos_status, POS_ENU_F) && state.ned_initialized_i) {
/* transform enu_f -> enu_i -> ecef_i -> lla_i, set status bits */
ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
SetBit(state.pos_status, POS_ENU_I);
ecef_of_enu_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.enu_pos_i);
SetBit(state.pos_status, POS_ECEF_I);
lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
} else if (bit_is_set(state.pos_status, POS_LLA_F)) {
LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
} else if (bit_is_set(state.pos_status, POS_UTM_F)) {
/* transform utm_f -> lla_f -> lla_i, set status bits */
lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
SetBit(state.pos_status, POS_LLA_F);
LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
} else {
/* could not get this representation, set errno */
//struct LlaCoor_i _lla_zero = {0};
//return _lla_zero;
}
/* set bit to indicate this representation is computed */
SetBit(state.pos_status, POS_LLA_I);
}
void stateCalcPositionLla_i(void) {
if (bit_is_set(state.pos_status, POS_LLA_I))
return;
if (bit_is_set(state.pos_status, POS_LLA_F)) {
LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
}
else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
}
else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
/* transform ecef_f -> lla_f, set status bit, then convert to int */
lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
SetBit(state.pos_status, POS_LLA_F);
LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
}
else if (bit_is_set(state.pos_status, POS_NED_F)) {
/* transform ned_f -> ecef_f -> lla_f -> lla_i, set status bits */
ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
SetBit(state.pos_status, POS_ECEF_F);
lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
SetBit(state.pos_status, POS_LLA_F);
LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
}
else if (bit_is_set(state.pos_status, POS_NED_I)) {
/* transform ned_i -> ecef_i -> lla_i, set status bits */
/// @todo check if resolution is enough
ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
SetBit(state.pos_status, POS_ECEF_I);
lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); /* uses double version internally */
}
else if (bit_is_set(state.pos_status, POS_UTM_F)) {
/* transform utm_f -> lla_f -> lla_i, set status bits */
lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
SetBit(state.pos_status, POS_LLA_F);
LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
}
else {
/* could not get this representation, set errno */
//struct LlaCoor_i _lla_zero = {0};
//return _lla_zero;
}
/* set bit to indicate this representation is computed */
SetBit(state.pos_status, POS_LLA_I);
}
value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, value cl, value t, value m, value lat, value lon) {
gps.fix = (Bool_val(m) ? 3 : 0);
gps.course = Double_val(c) * 1e7;
gps.hmsl = Double_val(a) * 1000.;
gps.gspeed = Double_val(s) * 100.;
gps.ned_vel.z = -Double_val(cl) * 100.;
gps.week = 0; // FIXME
gps.tow = Double_val(t) * 1000.;
//TODO set alt above ellipsoid and hmsl
#ifdef GPS_USE_LATLONG
struct LlaCoor_f lla_f;
struct UtmCoor_f utm_f;
lla_f.lat = Double_val(lat);
lla_f.lon = Double_val(lon);
utm_f.zone = nav_utm_zone0;
utm_of_lla_f(&utm_f, &lla_f);
LLA_BFP_OF_REAL(gps.lla_pos, lla_f);
gps.utm_pos.east = utm_f.east*100;
gps.utm_pos.north = utm_f.north*100;
gps.utm_pos.zone = nav_utm_zone0;
x = y = z; /* Just to get rid of the "unused arg" warning */
y = x; /* Just to get rid of the "unused arg" warning */
#else // GPS_USE_LATLONG
gps.utm_pos.east = Int_val(x);
gps.utm_pos.north = Int_val(y);
gps.utm_pos.zone = Int_val(z);
lat = lon; /* Just to get rid of the "unused arg" warning */
lon = lat; /* Just to get rid of the "unused arg" warning */
#endif // GPS_USE_LATLONG
/** Space vehicle info simulation */
gps.nb_channels=7;
int i;
static int time;
time++;
for(i = 0; i < gps.nb_channels; i++) {
gps.svinfos[i].svid = 7 + i;
gps.svinfos[i].elev = (cos(((100*i)+time)/100.) + 1) * 45;
gps.svinfos[i].azim = (time/gps.nb_channels + 50 * i) % 360;
gps.svinfos[i].cno = 40 + sin((time+i*10)/100.) * 10.;
gps.svinfos[i].flags = ((time/10) % (i+1) == 0 ? 0x00 : 0x01);
gps.svinfos[i].qi = (int)((time / 1000.) + i) % 8;
}
gps.pdop = gps.sacc = gps.pacc = 500+200*sin(time/100.);
gps.num_sv = 7;
//gps_verbose_downlink = !launch;
//gps_downlink();
gps_available = TRUE;
return Val_unit;
}
/** Convert a local NED position to ECEF.
* @param[out] ecef ECEF position in cm
* @param[in] def local coordinate system definition
* @param[in] ned NED position in meter << #INT32_POS_FRAC
*/
void lla_of_utm_i(struct LlaCoor_i *lla, struct UtmCoor_i *utm)
{
#if USE_SINGLE_PRECISION_LLA_UTM
/* convert our input to floating point */
struct UtmCoor_f utm_f;
UTM_FLOAT_OF_BFP(utm_f, *utm);
/* calls the floating point transformation */
struct LlaCoor_f lla_f;
lla_of_utm_f(&lla_f, &utm_f);
/* convert the output to fixed point */
LLA_BFP_OF_REAL(*lla, lla_f);
#else // use double precision by default
/* convert our input to floating point */
struct UtmCoor_d utm_d;
UTM_DOUBLE_OF_BFP(utm_d, *utm);
/* calls the floating point transformation */
struct LlaCoor_d lla_d;
lla_of_utm_d(&lla_d, &utm_d);
/* convert the output to fixed point */
LLA_BFP_OF_REAL(*lla, lla_d);
#endif
}
/** Convert a ECEF to LLA.
* @param[out] out LLA in degrees*1e7 and mm above ellipsoid
* @param[in] in ECEF in cm
*/
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
{
#if USE_SINGLE_PRECISION_LLA_ECEF
/* convert our input to floating point */
struct EcefCoor_f in_f;
ECEF_FLOAT_OF_BFP(in_f, *in);
/* calls the floating point transformation */
struct LlaCoor_f out_f;
lla_of_ecef_f(&out_f, &in_f);
/* convert the output to fixed point */
LLA_BFP_OF_REAL(*out, out_f);
#else // use double precision by default
/* convert our input to floating point */
struct EcefCoor_d in_d;
ECEF_DOUBLE_OF_BFP(in_d, *in);
/* calls the floating point transformation */
struct LlaCoor_d out_d;
lla_of_ecef_d(&out_d, &in_d);
/* convert the output to fixed point */
LLA_BFP_OF_REAL(*out, out_d);
#endif
}
/**
* Propagate the received states into the vehicle
* state machine
*/
void ins_vectornav_propagate()
{
// Acceleration [m/s^2]
// in fixed point for sending as ABI and telemetry msgs
ACCELS_BFP_OF_REAL(ins_vn.accel_i, ins_vn.accel);
// Rates [rad/s]
static struct FloatRates body_rate;
// in fixed point for sending as ABI and telemetry msgs
RATES_BFP_OF_REAL(ins_vn.gyro_i, ins_vn.gyro);
float_rmat_ratemult(&body_rate, orientationGetRMat_f(&ins_vn.body_to_imu), &ins_vn.gyro); // compute body rates
stateSetBodyRates_f(&body_rate); // Set state [rad/s]
// Attitude [deg]
ins_vectornav_yaw_pitch_roll_to_attitude(&ins_vn.attitude); // convert to correct units and axis [rad]
static struct FloatQuat imu_quat; // convert from euler to quat
float_quat_of_eulers(&imu_quat, &ins_vn.attitude);
static struct FloatRMat imu_rmat; // convert from quat to rmat
float_rmat_of_quat(&imu_rmat, &imu_quat);
static struct FloatRMat ltp_to_body_rmat; // rotate to body frame
float_rmat_comp(<p_to_body_rmat, &imu_rmat, orientationGetRMat_f(&ins_vn.body_to_imu));
stateSetNedToBodyRMat_f(<p_to_body_rmat); // set body states [rad]
// NED (LTP) velocity [m/s]
// North east down (NED), also known as local tangent plane (LTP),
// is a geographical coordinate system for representing state vectors that is commonly used in aviation.
// It consists of three numbers: one represents the position along the northern axis,
// one along the eastern axis, and one represents vertical position. Down is chosen as opposed to
// up in order to comply with the right-hand rule.
// The origin of this coordinate system is usually chosen to be the aircraft's center of gravity.
// x = North
// y = East
// z = Down
stateSetSpeedNed_f(&ins_vn.vel_ned); // set state
// NED (LTP) acceleration [m/s^2]
static struct FloatVect3 accel_meas_ltp;// first we need to rotate linear acceleration from imu-frame to body-frame
float_rmat_transp_vmult(&accel_meas_ltp, orientationGetRMat_f(&ins_vn.body_to_imu), &(ins_vn.lin_accel));
static struct NedCoor_f ltp_accel; // assign to NedCoord_f struct
VECT3_ASSIGN(ltp_accel, accel_meas_ltp.x, accel_meas_ltp.y, accel_meas_ltp.z);
stateSetAccelNed_f(<p_accel); // then set the states
ins_vn.ltp_accel_f = ltp_accel;
// LLA position [rad, rad, m]
//static struct LlaCoor_f lla_pos; // convert from deg to rad, and from double to float
ins_vn.lla_pos.lat = RadOfDeg((float)ins_vn.pos_lla[0]); // ins_impl.pos_lla[0] = lat
ins_vn.lla_pos.lon = RadOfDeg((float)ins_vn.pos_lla[1]); // ins_impl.pos_lla[1] = lon
ins_vn.lla_pos.alt = ((float)ins_vn.pos_lla[2]); // ins_impl.pos_lla[2] = alt
LLA_BFP_OF_REAL(gps.lla_pos, ins_vn.lla_pos);
stateSetPositionLla_i(&gps.lla_pos);
// ECEF position
struct LtpDef_f def;
ltp_def_from_lla_f(&def, &ins_vn.lla_pos);
struct EcefCoor_f ecef_vel;
ecef_of_ned_point_f(&ecef_vel, &def, &ins_vn.vel_ned);
ECEF_BFP_OF_REAL(gps.ecef_vel, ecef_vel);
// ECEF velocity
gps.ecef_pos.x = stateGetPositionEcef_i()->x;
gps.ecef_pos.y = stateGetPositionEcef_i()->y;
gps.ecef_pos.z = stateGetPositionEcef_i()->z;
#if GPS_USE_LATLONG
// GPS UTM
/* Computes from (lat, long) in the referenced UTM zone */
struct UtmCoor_f utm_f;
utm_f.zone = nav_utm_zone0;
/* convert to utm */
//utm_of_lla_f(&utm_f, &lla_f);
utm_of_lla_f(&utm_f, &ins_vn.lla_pos);
/* copy results of utm conversion */
gps.utm_pos.east = (int32_t)(utm_f.east * 100);
gps.utm_pos.north = (int32_t)(utm_f.north * 100);
gps.utm_pos.alt = (int32_t)(utm_f.alt * 1000);
gps.utm_pos.zone = (uint8_t)nav_utm_zone0;
#endif
// GPS Ground speed
float speed = sqrt(ins_vn.vel_ned.x * ins_vn.vel_ned.x + ins_vn.vel_ned.y * ins_vn.vel_ned.y);
gps.gspeed = ((uint16_t)(speed * 100));
// GPS course
gps.course = (int32_t)(1e7 * (atan2(ins_vn.vel_ned.y, ins_vn.vel_ned.x)));
// Because we have not HMSL data from Vectornav, we are using LLA-Altitude
// as a workaround
gps.hmsl = (uint32_t)(gps.lla_pos.alt);
// set position uncertainty
ins_vectornav_set_pacc();
// set velocity uncertainty
ins_vectornav_set_sacc();
// check GPS status
gps.last_msg_time = sys_time.nb_sec;
gps.last_msg_ticks = sys_time.nb_sec_rem;
if (gps.fix == GPS_FIX_3D) {
gps.last_3dfix_time = sys_time.nb_sec;
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
}
// read INS status
ins_vectornav_check_status();
// update internal states for telemetry purposes
// TODO: directly convert vectornav output instead of using state interface
// to support multiple INS running at the same time
ins_vn.ltp_pos_i = *stateGetPositionNed_i();
ins_vn.ltp_speed_i = *stateGetSpeedNed_i();
ins_vn.ltp_accel_i = *stateGetAccelNed_i();
// send ABI messages
uint32_t now_ts = get_sys_time_usec();
AbiSendMsgGPS(GPS_UBX_ID, now_ts, &gps);
AbiSendMsgIMU_GYRO_INT32(IMU_ASPIRIN_ID, now_ts, &ins_vn.gyro_i);
AbiSendMsgIMU_ACCEL_INT32(IMU_ASPIRIN_ID, now_ts, &ins_vn.accel_i);
}
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));
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);
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);
}
#if USE_GPS_XSENS
else if (xsens_id == XSENS_GPSStatus_ID) {
xsens.gps.nb_channels = XSENS_GPSStatus_nch(xsens_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);
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);
if (xsens.gps.svinfos[ch].flags > 0) {
xsens.gps.num_sv++;
}
}
}
#endif
else if (xsens_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_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);
SetBit(xsens.gps.valid_fields, GPS_VALID_POS_LLA_BIT);
// Compute geoid (MSL) height
float 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 * 1000.0f);
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);
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.pdop = 5; // FIXME Not output by XSens
xsens.gps_available = TRUE;
#endif
offset += XSENS_DATA_RAWGPS_LENGTH;
}
//} else {
if (XSENS_MASK_Temp(xsens_output_mode)) {
offset += XSENS_DATA_Temp_LENGTH;
}
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_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_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_available = TRUE;
l++;
}
offset += l * XSENS_DATA_Calibrated_LENGTH / 3;
}
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);
//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;
offset += XSENS_DATA_Euler_LENGTH;
}
if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x10) {
offset += XSENS_DATA_Matrix_LENGTH;
}
xsens.new_attitude = TRUE;
}
if (XSENS_MASK_Auxiliary(xsens_output_mode)) {
uint8_t l = 0;
if (!XSENS_MASK_Aux1Out(xsens_output_settings)) {
l++;
}
if (!XSENS_MASK_Aux2Out(xsens_output_settings)) {
l++;
}
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);
offset += XSENS_DATA_Position_LENGTH;
#if (! USE_GPS_XSENS_RAW_DATA) && USE_GPS_XSENS
LLA_BFP_OF_REAL(xsens.gps.lla_pos, xsens.lla_f);
SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
xsens.gps_available = TRUE;
#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);
offset += XSENS_DATA_Velocity_LENGTH;
}
if (XSENS_MASK_Status(xsens_output_mode)) {
xsens_msg_status = XSENS_DATA_Status_status(xsens_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
else { xsens.gps.fix = GPS_FIX_NONE; }
#ifdef GPS_LED
if (xsens.gps.fix == GPS_FIX_3D) {
LED_ON(GPS_LED);
} else {
LED_TOGGLE(GPS_LED);
}
#endif // GPS_LED
#endif // USE_GPS_XSENS
offset += XSENS_DATA_Status_LENGTH;
}
if (XSENS_MASK_TimeStamp(xsens_output_settings)) {
xsens.time_stamp = XSENS_DATA_TimeStamp_ts(xsens_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);
offset += XSENS_DATA_UTC_LENGTH;
}
}
}
