void stateCalcPositionUtm_f(void)
{
if (bit_is_set(state.pos_status, POS_UTM_F)) {
return;
}
if (bit_is_set(state.pos_status, POS_LLA_F)) {
utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_I)) {
/* transform lla_i -> lla_f -> utm_f, set status bits */
LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
SetBit(state.pos_status, POS_LLA_F);
utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
} else if (state.utm_initialized_f) {
if (bit_is_set(state.pos_status, POS_ENU_F)) {
UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
} else if (bit_is_set(state.pos_status, POS_ENU_I)) {
ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
SetBit(state.pos_status, POS_ENU_F);
UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
} else if (bit_is_set(state.pos_status, POS_NED_F)) {
UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
} else if (bit_is_set(state.pos_status, POS_NED_I)) {
NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
SetBit(state.pos_status, POS_NED_F);
UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
}
} else {
/* could not get this representation, set errno */
//struct EcefCoor_f _ecef_zero = {0.0f};
//return _ecef_zero;
}
/* set bit to indicate this representation is computed */
SetBit(state.pos_status, POS_UTM_F);
}
void ins_reset_local_origin( void ) {
#if INS_UPDATE_FW_ESTIMATOR
struct UtmCoor_f utm;
#ifdef GPS_USE_LATLONG
/* Recompute UTM coordinates in this zone */
struct LlaCoor_f lla;
lla.lat = gps.lla_pos.lat / 1e7;
lla.lon = gps.lla_pos.lon / 1e7;
utm.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
utm_of_lla_f(&utm, &lla);
#else
utm.zone = gps.utm_pos.zone;
utm.east = gps.utm_pos.east / 100.0f;
utm.north = gps.utm_pos.north / 100.0f;
#endif
// ground_alt
utm.alt = gps.hmsl / 1000.0f;
// reset state UTM ref
stateSetLocalUtmOrigin_f(&utm);
#else
struct LtpDef_i ltp_def;
ltp_def_from_ecef_i(<p_def, &gps.ecef_pos);
ltp_def.hmsl = gps.hmsl;
stateSetLocalOrigin_i(<p_def);
#endif
}
/** Reset the UTM zone to current GPS fix */
unit_t nav_reset_utm_zone(void) {
struct UtmCoor_f utm0_old;
utm0_old.zone = nav_utm_zone0;
utm0_old.north = nav_utm_north0;
utm0_old.east = nav_utm_east0;
utm0_old.alt = ground_alt;
struct LlaCoor_f lla0;
lla_of_utm_f(&lla0, &utm0_old);
#ifdef GPS_USE_LATLONG
/* Set the real UTM zone */
nav_utm_zone0 = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
#else
nav_utm_zone0 = gps.utm_pos.zone;
#endif
struct UtmCoor_f utm0;
utm0.zone = nav_utm_zone0;
utm_of_lla_f(&utm0, &lla0);
nav_utm_east0 = utm0.east;
nav_utm_north0 = utm0.north;
stateSetLocalUtmOrigin_f(&utm0);
return 0;
}
/** Reset the geographic reference to the current GPS fix */
unit_t nav_reset_reference( void ) {
#ifdef GPS_USE_LATLONG
/* Set the real UTM zone */
nav_utm_zone0 = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
/* Recompute UTM coordinates in this zone */
struct LlaCoor_f lla;
lla.lat = gps.lla_pos.lat/1e7;
lla.lon = gps.lla_pos.lon/1e7;
struct UtmCoor_f utm;
utm.zone = nav_utm_zone0;
utm_of_lla_f(&utm, &lla);
nav_utm_east0 = utm.east;
nav_utm_north0 = utm.north;
#else
nav_utm_zone0 = gps.utm_pos.zone;
nav_utm_east0 = gps.utm_pos.east/100;
nav_utm_north0 = gps.utm_pos.north/100;
#endif
// reset state UTM ref
struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 };
stateSetLocalUtmOrigin_f(&utm0);
previous_ground_alt = ground_alt;
ground_alt = gps.hmsl/1000.;
return 0;
}
struct UtmCoor_i utm_int_from_gps(struct GpsState *gps_s, uint8_t zone)
{
struct UtmCoor_i utm;
utm.alt = 0;
if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_UTM_BIT)) {
// A real UTM position is available, use the correct zone
utm.zone = gps_s->utm_pos.zone;
utm.east = gps_s->utm_pos.east;
utm.north = gps_s->utm_pos.north;
}
else {
struct LlaCoor_f lla;
LLA_FLOAT_OF_BFP(lla, gps_s->lla_pos);
// Check if zone should be computed
if (zone > 0) {
utm.zone = zone;
} else {
utm.zone = (gps_s->lla_pos.lon / 1e7 + 180) / 6 + 1;
}
/* Recompute UTM coordinates in this zone */
struct UtmCoor_f utm_f;
utm_f.zone = utm.zone;
utm_of_lla_f(&utm_f, &lla);
/* convert to fixed point in cm */
utm.east = utm_f.east * 100;
utm.north = utm_f.north * 100;
}
return utm;
}
/// Utility function: converts lla (int) to local point (float)
bool mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla)
{
/// TODO: don't convert to float, either use double or do completely in fixed point
struct LlaCoor_f lla_f;
LLA_FLOAT_OF_BFP(lla_f, *lla);
/* Computes from (lat, long) in the referenced UTM zone */
struct UtmCoor_f utm;
utm.zone = nav_utm_zone0;
utm_of_lla_f(&utm, &lla_f);
/* Computes relative position to HOME waypoint
* and bound the distance to max_dist_from_home
*/
float dx, dy;
dx = utm.east - nav_utm_east0 - waypoints[WP_HOME].x;
dy = utm.north - nav_utm_north0 - waypoints[WP_HOME].y;
BoundAbs(dx, max_dist_from_home);
BoundAbs(dy, max_dist_from_home);
/* Update point */
point->x = waypoints[WP_HOME].x + dx;
point->y = waypoints[WP_HOME].y + dy;
point->z = lla_f.alt;
return true;
}
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;
}
void gps_parse(void)
{
if (gps_network == NULL) { return; }
//Read from the network
int size = network_read(gps_network, &gps_udp_read_buffer[0], 256);
if (size > 0) {
// Correct MSG
if ((size == GPS_UDP_MSG_LEN) && (gps_udp_read_buffer[0] == STX)) {
gps.lla_pos.lat = UDP_GPS_INT(gps_udp_read_buffer + 4);
gps.lla_pos.lon = UDP_GPS_INT(gps_udp_read_buffer + 8);
gps.lla_pos.alt = UDP_GPS_INT(gps_udp_read_buffer + 12);
gps.hmsl = UDP_GPS_INT(gps_udp_read_buffer + 16);
gps.ecef_pos.x = UDP_GPS_INT(gps_udp_read_buffer + 20);
gps.ecef_pos.y = UDP_GPS_INT(gps_udp_read_buffer + 24);
gps.ecef_pos.z = UDP_GPS_INT(gps_udp_read_buffer + 28);
gps.ecef_vel.x = UDP_GPS_INT(gps_udp_read_buffer + 32);
gps.ecef_vel.y = UDP_GPS_INT(gps_udp_read_buffer + 36);
gps.ecef_vel.z = UDP_GPS_INT(gps_udp_read_buffer + 40);
gps.fix = GPS_FIX_3D;
#if GPS_USE_LATLONG
// Computes from (lat, long) in the referenced UTM zone
struct LlaCoor_f lla_f;
LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos);
struct UtmCoor_f utm_f;
utm_f.zone = nav_utm_zone0;
// convert to utm
utm_of_lla_f(&utm_f, &lla_f);
// copy results of utm conversion
gps.utm_pos.east = utm_f.east * 100;
gps.utm_pos.north = utm_f.north * 100;
gps.utm_pos.alt = gps.lla_pos.alt;
gps.utm_pos.zone = nav_utm_zone0;
#endif
// publish new GPS data
uint32_t now_ts = get_sys_time_usec();
gps.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec;
if (gps.fix == GPS_FIX_3D) {
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
}
AbiSendMsgGPS(GPS_UDP_ID, now_ts, &gps);
} else {
printf("gps_udp error: msg len invalid %d bytes\n", size);
}
memset(&gps_udp_read_buffer[0], 0, sizeof(gps_udp_read_buffer));
}
}
void WEAK ins_reset_utm_zone(struct UtmCoor_f * utm) {
struct LlaCoor_f lla0;
lla_of_utm_f(&lla0, utm);
#ifdef GPS_USE_LATLONG
utm->zone = (gps.lla_pos.lon/1e7 + 180) / 6 + 1;
#else
utm->zone = gps.utm_pos.zone;
#endif
utm_of_lla_f(utm, &lla0);
stateSetLocalUtmOrigin_f(utm);
}
void gps_parse(void) {
if (gps_network == NULL) return;
//Read from the network
int size = network_read( gps_network, &gps_udp_read_buffer[0], 256);
if(size > 0)
{
// Correct MSG
if ((size == GPS_UDP_MSG_LEN) && (gps_udp_read_buffer[0] == STX))
{
gps.lla_pos.lat = RadOfDeg(UDP_GPS_INT(gps_udp_read_buffer+4));
gps.lla_pos.lon = RadOfDeg(UDP_GPS_INT(gps_udp_read_buffer+8));
gps.lla_pos.alt = UDP_GPS_INT(gps_udp_read_buffer+12);
gps.hmsl = UDP_GPS_INT(gps_udp_read_buffer+16);
gps.ecef_pos.x = UDP_GPS_INT(gps_udp_read_buffer+20);
gps.ecef_pos.y = UDP_GPS_INT(gps_udp_read_buffer+24);
gps.ecef_pos.z = UDP_GPS_INT(gps_udp_read_buffer+28);
gps.ecef_vel.x = UDP_GPS_INT(gps_udp_read_buffer+32);
gps.ecef_vel.y = UDP_GPS_INT(gps_udp_read_buffer+36);
gps.ecef_vel.z = UDP_GPS_INT(gps_udp_read_buffer+40);
gps.fix = GPS_FIX_3D;
gps_available = TRUE;
#if GPS_USE_LATLONG
// Computes from (lat, long) in the referenced UTM zone
struct LlaCoor_f lla_f;
lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
struct UtmCoor_f utm_f;
utm_f.zone = nav_utm_zone0;
// convert to utm
utm_of_lla_f(&utm_f, &lla_f);
// copy results of utm conversion
gps.utm_pos.east = utm_f.east*100;
gps.utm_pos.north = utm_f.north*100;
gps.utm_pos.alt = gps.lla_pos.alt;
gps.utm_pos.zone = nav_utm_zone0;
#endif
}
else
{
printf("gps_udp error: msg len invalid %d bytes\n",size);
}
memset(&gps_udp_read_buffer[0], 0, sizeof(gps_udp_read_buffer));
}
}
void WEAK ins_reset_utm_zone(struct UtmCoor_f *utm)
{
struct LlaCoor_f lla0;
lla_of_utm_f(&lla0, utm);
if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) {
utm->zone = gps.utm_pos.zone;
}
else {
utm->zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
}
utm_of_lla_f(utm, &lla0);
stateSetLocalUtmOrigin_f(utm);
}
/** Parse the REMOTE_GPS datalink packet */
void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t ecef_z, int32_t lat, int32_t lon,
int32_t alt,
int32_t hmsl, int32_t ecef_xd, int32_t ecef_yd, int32_t ecef_zd, uint32_t tow, int32_t course)
{
gps.lla_pos.lat = lat;
gps.lla_pos.lon = lon;
gps.lla_pos.alt = alt;
gps.hmsl = hmsl;
gps.ecef_pos.x = ecef_x;
gps.ecef_pos.y = ecef_y;
gps.ecef_pos.z = ecef_z;
gps.ecef_vel.x = ecef_xd;
gps.ecef_vel.y = ecef_yd;
gps.ecef_vel.z = ecef_zd;
gps.course = course;
gps.num_sv = numsv;
gps.tow = tow;
gps.fix = GPS_FIX_3D;
#if GPS_USE_LATLONG
// Computes from (lat, long) in the referenced UTM zone
struct LlaCoor_f lla_f;
LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos);
struct UtmCoor_f utm_f;
utm_f.zone = nav_utm_zone0;
// convert to utm
utm_of_lla_f(&utm_f, &lla_f);
// copy results of utm conversion
gps.utm_pos.east = utm_f.east * 100;
gps.utm_pos.north = utm_f.north * 100;
gps.utm_pos.alt = gps.lla_pos.alt;
gps.utm_pos.zone = nav_utm_zone0;
#endif
// publish new GPS data
uint32_t now_ts = get_sys_time_usec();
gps.last_msg_ticks = sys_time.nb_sec_rem;
gps.last_msg_time = sys_time.nb_sec;
if (gps.fix == GPS_FIX_3D) {
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
}
AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps);
}
void ins_reset_local_origin(void)
{
struct UtmCoor_f utm;
#ifdef GPS_USE_LATLONG
/* Recompute UTM coordinates in this zone */
struct LlaCoor_f lla;
LLA_FLOAT_OF_BFP(lla, gps.lla_pos);
utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
utm_of_lla_f(&utm, &lla);
#else
utm.zone = gps.utm_pos.zone;
utm.east = gps.utm_pos.east / 100.0f;
utm.north = gps.utm_pos.north / 100.0f;
#endif
// ground_alt
utm.alt = gps.hmsl / 1000.0f;
// reset state UTM ref
stateSetLocalUtmOrigin_f(&utm);
}
/** Reset the geographic reference to the current GPS fix */
void ins_reset_local_origin(void) {
struct UtmCoor_f utm;
#ifdef GPS_USE_LATLONG
/* Recompute UTM coordinates in this zone */
struct LlaCoor_f lla;
lla.lat = gps.lla_pos.lat / 1e7;
lla.lon = gps.lla_pos.lon / 1e7;
utm.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
utm_of_lla_f(&utm, &lla);
#else
utm.zone = gps.utm_pos.zone;
utm.east = gps.utm_pos.east / 100.0f;
utm.north = gps.utm_pos.north / 100.0f;
#endif
// ground_alt
utm.alt = gps.hmsl /1000.0f;
// reset state UTM ref
stateSetLocalUtmOrigin_f(&utm);
// reset filter flag
ins_impl.reset_alt_ref = TRUE;
}
/** Convert a LLA to UTM.
* @param[out] out UTM in cm and mm hmsl alt
* @param[in] in LLA in degrees*1e7 and mm above ellipsoid
*/
void utm_of_lla_i(struct UtmCoor_i *utm, struct LlaCoor_i *lla)
{
#if USE_SINGLE_PRECISION_LLA_UTM
/* convert our input to floating point */
struct LlaCoor_f lla_f;
LLA_FLOAT_OF_BFP(lla_f, *lla);
/* calls the floating point transformation */
struct UtmCoor_f utm_f;
utm_f.zone = utm->zone;
utm_of_lla_f(&utm_f, &lla_f);
/* convert the output to fixed point */
UTM_BFP_OF_REAL(*utm, utm_f);
#else // use double precision by default
/* convert our input to floating point */
struct LlaCoor_d lla_d;
LLA_DOUBLE_OF_BFP(lla_d, *lla);
/* calls the floating point transformation */
struct UtmCoor_d utm_d;
utm_d.zone = utm->zone;
utm_of_lla_d(&utm_d, &lla_d);
/* convert the output to fixed point */
UTM_BFP_OF_REAL(*utm, utm_d);
#endif
}
void stateCalcPositionEnu_f(void)
{
if (bit_is_set(state.pos_status, POS_ENU_F)) {
return;
}
int errno = 0;
if (state.ned_initialized_f) {
if (bit_is_set(state.pos_status, POS_NED_F)) {
VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_ENU_I)) {
ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
} else if (bit_is_set(state.pos_status, POS_NED_I)) {
NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
SetBit(state.pos_status, POS_NED_F);
VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
} else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
/* transform ecef_i -> enu_i -> enu_f, set status bits */
enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
SetBit(state.pos_status, POS_ENU_I);
ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
} else if (bit_is_set(state.pos_status, POS_LLA_F)) {
enu_of_lla_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.lla_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_I)) {
/* transform lla_i -> ecef_i -> enu_i -> enu_f, set status bits */
ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */
SetBit(state.pos_status, POS_ECEF_I);
enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
SetBit(state.pos_status, POS_ENU_I);
ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
} else { /* could not get this representation, set errno */
errno = 1;
}
} else if (state.utm_initialized_f) {
if (bit_is_set(state.pos_status, POS_ENU_I)) {
ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
} else if (bit_is_set(state.pos_status, POS_NED_F)) {
VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_NED_I)) {
NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
SetBit(state.pos_status, POS_NED_F);
VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_UTM_F)) {
ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
} else if (bit_is_set(state.pos_status, POS_LLA_F)) {
/* transform lla_f -> utm_f -> enu, set status bits */
utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
SetBit(state.pos_status, POS_UTM_F);
ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
} else if (bit_is_set(state.pos_status, POS_LLA_I)) {
/* transform lla_i -> lla_f -> utm_f -> enu, set status bits */
LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
SetBit(state.pos_status, POS_LLA_F);
utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
SetBit(state.pos_status, POS_UTM_F);
ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f);
} else { /* could not get this representation, set errno */
errno = 2;
}
} else { /* ned coordinate system not initialized, set errno */
errno = 3;
}
if (errno) {
//struct EnuCoor_f _enu_zero = {0.0f};
//return _enu_zero;
}
/* set bit to indicate this representation is computed */
SetBit(state.pos_status, POS_ENU_F);
}
/**
* 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);
}
/**
* Parse GGA NMEA messages.
* GGA has essential fix data providing 3D location and HDOP.
* Msg stored in gps_nmea.msg_buf.
*/
static void nmea_parse_GGA(void)
{
int i = 6; // current position in the message, start after: GPGGA,
double degrees, minutesfrac;
struct LlaCoor_f lla_f;
// attempt to reject empty packets right away
if (gps_nmea.msg_buf[i] == ',' && gps_nmea.msg_buf[i + 1] == ',') {
NMEA_PRINT("p_GGA() - skipping empty message\n\r");
return;
}
// get UTC time [hhmmss.sss]
// ignored GpsInfo.PosLLA.TimeOfFix.f = strtod(&packet[i], NULL);
// FIXME: parse UTC time correctly
double time = strtod(&gps_nmea.msg_buf[i], NULL);
gps.tow = (uint32_t)((time + 1) * 1000);
// get latitude [ddmm.mmmmm]
nmea_read_until(&i);
double lat = strtod(&gps_nmea.msg_buf[i], NULL);
// convert to pure degrees [dd.dddd] format
minutesfrac = modf(lat / 100, °rees);
lat = degrees + (minutesfrac * 100) / 60;
// get latitute N/S
nmea_read_until(&i);
if (gps_nmea.msg_buf[i] == 'S') {
lat = -lat;
}
// convert to radians
lla_f.lat = RadOfDeg(lat);
gps.lla_pos.lat = lat * 1e7; // convert to fixed-point
NMEA_PRINT("p_GGA() - lat=%f gps_lat=%f\n\r", (lat * 1000), lla_f.lat);
// get longitude [ddmm.mmmmm]
nmea_read_until(&i);
double lon = strtod(&gps_nmea.msg_buf[i], NULL);
// convert to pure degrees [dd.dddd] format
minutesfrac = modf(lon / 100, °rees);
lon = degrees + (minutesfrac * 100) / 60;
// get longitude E/W
nmea_read_until(&i);
if (gps_nmea.msg_buf[i] == 'W') {
lon = -lon;
}
// convert to radians
lla_f.lon = RadOfDeg(lon);
gps.lla_pos.lon = lon * 1e7; // convert to fixed-point
NMEA_PRINT("p_GGA() - lon=%f gps_lon=%f time=%u\n\r", (lon * 1000), lla_f.lon, gps.tow);
// get position fix status
nmea_read_until(&i);
// 0 = Invalid, 1 = Valid SPS, 2 = Valid DGPS, 3 = Valid PPS
// check for good position fix
if ((gps_nmea.msg_buf[i] != '0') && (gps_nmea.msg_buf[i] != ',')) {
gps_nmea.pos_available = TRUE;
NMEA_PRINT("p_GGA() - POS_AVAILABLE == TRUE\n\r");
} else {
gps_nmea.pos_available = FALSE;
NMEA_PRINT("p_GGA() - gps_pos_available == false\n\r");
}
// get number of satellites used in GPS solution
nmea_read_until(&i);
gps.num_sv = atoi(&gps_nmea.msg_buf[i]);
NMEA_PRINT("p_GGA() - gps_numSatlitesUsed=%i\n\r", gps.num_sv);
// get HDOP, but we use PDOP from GSA message
nmea_read_until(&i);
//float hdop = strtof(&gps_nmea.msg_buf[i], NULL);
//gps.pdop = hdop * 100;
// get altitude (in meters) above geoid (MSL)
nmea_read_until(&i);
float hmsl = strtof(&gps_nmea.msg_buf[i], NULL);
gps.hmsl = hmsl * 1000;
NMEA_PRINT("p_GGA() - gps.hmsl=%i\n\r", gps.hmsl);
// get altitude units (always M)
nmea_read_until(&i);
// get geoid seperation
nmea_read_until(&i);
float geoid = strtof(&gps_nmea.msg_buf[i], NULL);
NMEA_PRINT("p_GGA() - geoid alt=%f\n\r", geoid);
// height above ellipsoid
lla_f.alt = hmsl + geoid;
gps.lla_pos.alt = lla_f.alt * 1000;
NMEA_PRINT("p_GGA() - gps.alt=%i\n\r", gps.lla_pos.alt);
// get seperations units
nmea_read_until(&i);
// get DGPS age
nmea_read_until(&i);
// get DGPS station ID
#if GPS_USE_LATLONG
/* convert to utm */
struct UtmCoor_f utm_f;
utm_f.zone = nav_utm_zone0;
utm_of_lla_f(&utm_f, &lla_f);
/* copy results of utm conversion */
gps.utm_pos.east = utm_f.east * 100;
gps.utm_pos.north = utm_f.north * 100;
gps.utm_pos.alt = gps.lla_pos.alt;
gps.utm_pos.zone = nav_utm_zone0;
#endif
/* convert to ECEF */
struct EcefCoor_f ecef_f;
ecef_of_lla_f(&ecef_f, &lla_f);
gps.ecef_pos.x = ecef_f.x * 100;
gps.ecef_pos.y = ecef_f.y * 100;
gps.ecef_pos.z = ecef_f.z * 100;
}
void gps_mtk_read_message(void) {
if (gps_mtk.msg_class == MTK_DIY14_ID) {
if (gps_mtk.msg_id == MTK_DIY14_NAV_ID) {
#ifdef GPS_TIMESTAMP
/* get hardware clock ticks */
SysTimeTimerStart(gps.t0);
gps.t0_tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);
gps.t0_tow_frac = 0;
#endif
gps.lla_pos.lat = RadOfDeg(MTK_DIY14_NAV_LAT(gps_mtk.msg_buf))*10;
gps.lla_pos.lon = RadOfDeg(MTK_DIY14_NAV_LON(gps_mtk.msg_buf))*10;
// FIXME: with MTK you do not receive vertical speed
if (cpu_time_sec - gps.last_fix_time < 2) {
gps.ned_vel.z = ((gps.hmsl -
MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf)*10)*OUTPUT_RATE)/10;
} else gps.ned_vel.z = 0;
gps.hmsl = MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf)*10;
// FIXME: with MTK you do not receive ellipsoid altitude
gps.lla_pos.alt = gps.hmsl;
gps.gspeed = MTK_DIY14_NAV_GSpeed(gps_mtk.msg_buf);
// FIXME: with MTK you do not receive speed 3D
gps.speed_3d = gps.gspeed;
gps.course = (RadOfDeg(MTK_DIY14_NAV_Heading(gps_mtk.msg_buf)))*10;
gps.num_sv = MTK_DIY14_NAV_numSV(gps_mtk.msg_buf);
switch (MTK_DIY14_NAV_GPSfix(gps_mtk.msg_buf)) {
case MTK_DIY_FIX_3D:
gps.fix = GPS_FIX_3D;
break;
case MTK_DIY_FIX_2D:
gps.fix = GPS_FIX_2D;
break;
default:
gps.fix = GPS_FIX_NONE;
}
gps.tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);;
// FIXME: with MTK DIY 1.4 you do not receive GPS week
gps.week = 0;
/* Computes from (lat, long) in the referenced UTM zone */
struct LlaCoor_f lla_f;
lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
struct UtmCoor_f utm_f;
utm_f.zone = nav_utm_zone0;
/* convert to utm */
utm_of_lla_f(&utm_f, &lla_f);
/* copy results of utm conversion */
gps.utm_pos.east = utm_f.east*100;
gps.utm_pos.north = utm_f.north*100;
gps.utm_pos.alt = utm_f.alt*1000;
gps.utm_pos.zone = nav_utm_zone0;
#ifdef GPS_LED
if (gps.fix == GPS_FIX_3D) {
LED_ON(GPS_LED);
}
else {
LED_TOGGLE(GPS_LED);
}
#endif
}
}
if (gps_mtk.msg_class == MTK_DIY16_ID) {
if (gps_mtk.msg_id == MTK_DIY16_NAV_ID) {
uint32_t gps_date, gps_time;
gps_date = MTK_DIY16_NAV_UTC_DATE(gps_mtk.msg_buf);
gps_time = MTK_DIY16_NAV_UTC_TIME(gps_mtk.msg_buf);
gps_mtk_time2itow(gps_date, gps_time, &gps.week, &gps.tow);
#ifdef GPS_TIMESTAMP
/* get hardware clock ticks */
SysTimeTimerStart(gps.t0);
gps.t0_tow = gps.tow;
gps.t0_tow_frac = 0;
#endif
gps.lla_pos.lat = RadOfDeg(MTK_DIY16_NAV_LAT(gps_mtk.msg_buf))*10;
gps.lla_pos.lon = RadOfDeg(MTK_DIY16_NAV_LON(gps_mtk.msg_buf))*10;
// FIXME: with MTK you do not receive vertical speed
if (cpu_time_sec - gps.last_fix_time < 2) {
gps.ned_vel.z = ((gps.hmsl -
MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf)*10)*OUTPUT_RATE)/10;
} else gps.ned_vel.z = 0;
gps.hmsl = MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf)*10;
// FIXME: with MTK you do not receive ellipsoid altitude
gps.lla_pos.alt = gps.hmsl;
gps.gspeed = MTK_DIY16_NAV_GSpeed(gps_mtk.msg_buf);
// FIXME: with MTK you do not receive speed 3D
gps.speed_3d = gps.gspeed;
gps.course = (RadOfDeg(MTK_DIY16_NAV_Heading(gps_mtk.msg_buf)*10000)) * 10;
gps.num_sv = MTK_DIY16_NAV_numSV(gps_mtk.msg_buf);
switch (MTK_DIY16_NAV_GPSfix(gps_mtk.msg_buf)) {
case MTK_DIY_FIX_3D:
gps.fix = GPS_FIX_3D;
break;
case MTK_DIY_FIX_2D:
gps.fix = GPS_FIX_2D;
break;
default:
gps.fix = GPS_FIX_NONE;
}
/* HDOP? */
/* Computes from (lat, long) in the referenced UTM zone */
struct LlaCoor_f lla_f;
lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
struct UtmCoor_f utm_f;
utm_f.zone = nav_utm_zone0;
/* convert to utm */
utm_of_lla_f(&utm_f, &lla_f);
/* copy results of utm conversion */
gps.utm_pos.east = utm_f.east*100;
gps.utm_pos.north = utm_f.north*100;
gps.utm_pos.alt = utm_f.alt*1000;
gps.utm_pos.zone = nav_utm_zone0;
#ifdef GPS_LED
if (gps.fix == GPS_FIX_3D) {
LED_ON(GPS_LED);
}
else {
LED_TOGGLE(GPS_LED);
}
#endif
}
}
}
void gps_skytraq_read_message(void) {
//DEBUG_S1_ON();
if (gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) {
gps.ecef_pos.x = SKYTRAQ_NAVIGATION_DATA_ECEFX(gps_skytraq.msg_buf);
gps.ecef_pos.y = SKYTRAQ_NAVIGATION_DATA_ECEFY(gps_skytraq.msg_buf);
gps.ecef_pos.z = SKYTRAQ_NAVIGATION_DATA_ECEFZ(gps_skytraq.msg_buf);
gps.ecef_vel.x = SKYTRAQ_NAVIGATION_DATA_ECEFVX(gps_skytraq.msg_buf);
gps.ecef_vel.y = SKYTRAQ_NAVIGATION_DATA_ECEFVY(gps_skytraq.msg_buf);
gps.ecef_vel.z = SKYTRAQ_NAVIGATION_DATA_ECEFVZ(gps_skytraq.msg_buf);
gps.lla_pos.lat = RadOfDeg(SKYTRAQ_NAVIGATION_DATA_LAT(gps_skytraq.msg_buf));
gps.lla_pos.lon = RadOfDeg(SKYTRAQ_NAVIGATION_DATA_LON(gps_skytraq.msg_buf));
gps.lla_pos.alt = SKYTRAQ_NAVIGATION_DATA_AEL(gps_skytraq.msg_buf)/10;
gps.hmsl = SKYTRAQ_NAVIGATION_DATA_ASL(gps_skytraq.msg_buf)/10;
// pacc;
// sacc;
// gps.pdop = SKYTRAQ_NAVIGATION_DATA_PDOP(gps_skytraq.msg_buf);
gps.num_sv = SKYTRAQ_NAVIGATION_DATA_NumSV(gps_skytraq.msg_buf);
gps.tow = SKYTRAQ_NAVIGATION_DATA_TOW(gps_skytraq.msg_buf)/10;
switch (SKYTRAQ_NAVIGATION_DATA_FixMode(gps_skytraq.msg_buf)) {
case SKYTRAQ_FIX_3D_DGPS:
case SKYTRAQ_FIX_3D:
gps.fix = GPS_FIX_3D;
break;
case SKYTRAQ_FIX_2D:
gps.fix = GPS_FIX_2D;
break;
default:
gps.fix = GPS_FIX_NONE;
}
#if GPS_USE_LATLONG
/* Computes from (lat, long) in the referenced UTM zone */
struct LlaCoor_f lla_f;
lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
struct UtmCoor_f utm_f;
utm_f.zone = nav_utm_zone0;
/* convert to utm */
utm_of_lla_f(&utm_f, &lla_f);
/* copy results of utm conversion */
gps.utm_pos.east = utm_f.east*100;
gps.utm_pos.north = utm_f.north*100;
gps.utm_pos.alt = gps.lla_pos.alt;
gps.utm_pos.zone = nav_utm_zone0;
#endif
if ( gps.fix == GPS_FIX_3D ) {
if ( distance_too_great( &ref_ltp.ecef, &gps.ecef_pos ) ) {
// just grab current ecef_pos as reference.
ltp_def_from_ecef_i( &ref_ltp, &gps.ecef_pos );
}
// convert ecef velocity vector to NED vector.
ned_of_ecef_vect_i( &gps.ned_vel, &ref_ltp, &gps.ecef_vel );
// ground course in radians
gps.course = ( M_PI_4 + atan2( -gps.ned_vel.y, gps.ned_vel.x )) * 1e7;
// GT: gps.cacc = ... ? what should course accuracy be?
// ground speed
gps.gspeed = sqrt( gps.ned_vel.x * gps.ned_vel.x + gps.ned_vel.y * gps.ned_vel.y );
gps.speed_3d = sqrt( gps.ned_vel.x * gps.ned_vel.x + gps.ned_vel.y * gps.ned_vel.y + gps.ned_vel.z * gps.ned_vel.z );
// vertical speed (climb)
// solved by gps.ned.z?
}
//DEBUG_S2_TOGGLE();
#ifdef GPS_LED
if (gps.fix == GPS_FIX_3D) {
LED_ON(GPS_LED);
}
else {
LED_TOGGLE(GPS_LED);
}
#endif
}
//DEBUG_S1_OFF();
}
void gps_ubx_read_message(void) {
if (gps_ubx.msg_class == UBX_NAV_ID) {
if (gps_ubx.msg_id == UBX_NAV_SOL_ID) {
/* get hardware clock ticks */
gps_time_sync.t0_ticks = sys_time.nb_tick;
gps_time_sync.t0_tow = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf);
gps_time_sync.t0_tow_frac = UBX_NAV_SOL_Frac(gps_ubx.msg_buf);
gps.tow = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf);
gps.week = UBX_NAV_SOL_week(gps_ubx.msg_buf);
gps.fix = UBX_NAV_SOL_GPSfix(gps_ubx.msg_buf);
gps.ecef_pos.x = UBX_NAV_SOL_ECEF_X(gps_ubx.msg_buf);
gps.ecef_pos.y = UBX_NAV_SOL_ECEF_Y(gps_ubx.msg_buf);
gps.ecef_pos.z = UBX_NAV_SOL_ECEF_Z(gps_ubx.msg_buf);
gps.pacc = UBX_NAV_SOL_Pacc(gps_ubx.msg_buf);
gps.ecef_vel.x = UBX_NAV_SOL_ECEFVX(gps_ubx.msg_buf);
gps.ecef_vel.y = UBX_NAV_SOL_ECEFVY(gps_ubx.msg_buf);
gps.ecef_vel.z = UBX_NAV_SOL_ECEFVZ(gps_ubx.msg_buf);
gps.sacc = UBX_NAV_SOL_Sacc(gps_ubx.msg_buf);
gps.pdop = UBX_NAV_SOL_PDOP(gps_ubx.msg_buf);
gps.num_sv = UBX_NAV_SOL_numSV(gps_ubx.msg_buf);
#ifdef GPS_LED
if (gps.fix == GPS_FIX_3D) {
LED_ON(GPS_LED);
}
else {
LED_TOGGLE(GPS_LED);
}
#endif
} else if (gps_ubx.msg_id == UBX_NAV_POSLLH_ID) {
gps.lla_pos.lat = RadOfDeg(UBX_NAV_POSLLH_LAT(gps_ubx.msg_buf));
gps.lla_pos.lon = RadOfDeg(UBX_NAV_POSLLH_LON(gps_ubx.msg_buf));
gps.lla_pos.alt = UBX_NAV_POSLLH_HEIGHT(gps_ubx.msg_buf);
gps.hmsl = UBX_NAV_POSLLH_HMSL(gps_ubx.msg_buf);
#if GPS_USE_LATLONG
/* Computes from (lat, long) in the referenced UTM zone */
struct LlaCoor_f lla_f;
lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
struct UtmCoor_f utm_f;
utm_f.zone = nav_utm_zone0;
/* convert to utm */
utm_of_lla_f(&utm_f, &lla_f);
/* copy results of utm conversion */
gps.utm_pos.east = utm_f.east*100;
gps.utm_pos.north = utm_f.north*100;
gps.utm_pos.alt = gps.lla_pos.alt;
gps.utm_pos.zone = nav_utm_zone0;
#else
}
else if (gps_ubx.msg_id == UBX_NAV_POSUTM_ID) {
gps.utm_pos.east = UBX_NAV_POSUTM_EAST(gps_ubx.msg_buf);
gps.utm_pos.north = UBX_NAV_POSUTM_NORTH(gps_ubx.msg_buf);
uint8_t hem = UBX_NAV_POSUTM_HEM(gps_ubx.msg_buf);
if (hem == UTM_HEM_SOUTH)
gps.utm_pos.north -= 1000000000; /* Subtract false northing: -10000km */
gps.utm_pos.alt = UBX_NAV_POSUTM_ALT(gps_ubx.msg_buf)*10;
gps.hmsl = gps.utm_pos.alt;
gps.lla_pos.alt = gps.utm_pos.alt; // FIXME: with UTM only you do not receive ellipsoid altitude
gps.utm_pos.zone = UBX_NAV_POSUTM_ZONE(gps_ubx.msg_buf);
#endif
}
else if (gps_ubx.msg_id == UBX_NAV_VELNED_ID) {
gps.speed_3d = UBX_NAV_VELNED_Speed(gps_ubx.msg_buf);
gps.gspeed = UBX_NAV_VELNED_GSpeed(gps_ubx.msg_buf);
gps.ned_vel.x = UBX_NAV_VELNED_VEL_N(gps_ubx.msg_buf);
gps.ned_vel.y = UBX_NAV_VELNED_VEL_E(gps_ubx.msg_buf);
gps.ned_vel.z = UBX_NAV_VELNED_VEL_D(gps_ubx.msg_buf);
// Ublox gives I4 heading in 1e-5 degrees, apparenty from 0 to 360 degrees (not -180 to 180)
// I4 max = 2^31 = 214 * 1e5 * 100 < 360 * 1e7: overflow on angles over 214 deg -> casted to -214 deg
// solution: First to radians, and then scale to 1e-7 radians
// First x 10 for loosing less resolution, then to radians, then multiply x 10 again
gps.course = (RadOfDeg(UBX_NAV_VELNED_Heading(gps_ubx.msg_buf)*10)) * 10;
gps.cacc = (RadOfDeg(UBX_NAV_VELNED_CAcc(gps_ubx.msg_buf)*10)) * 10;
gps.tow = UBX_NAV_VELNED_ITOW(gps_ubx.msg_buf);
gps_ubx.have_velned = 1;
}
else if (gps_ubx.msg_id == UBX_NAV_SVINFO_ID) {
gps.nb_channels = Min(UBX_NAV_SVINFO_NCH(gps_ubx.msg_buf), GPS_NB_CHANNELS);
uint8_t i;
for(i = 0; i < gps.nb_channels; i++) {
gps.svinfos[i].svid = UBX_NAV_SVINFO_SVID(gps_ubx.msg_buf, i);
gps.svinfos[i].flags = UBX_NAV_SVINFO_Flags(gps_ubx.msg_buf, i);
gps.svinfos[i].qi = UBX_NAV_SVINFO_QI(gps_ubx.msg_buf, i);
gps.svinfos[i].cno = UBX_NAV_SVINFO_CNO(gps_ubx.msg_buf, i);
gps.svinfos[i].elev = UBX_NAV_SVINFO_Elev(gps_ubx.msg_buf, i);
gps.svinfos[i].azim = UBX_NAV_SVINFO_Azim(gps_ubx.msg_buf, i);
}
}
else if (gps_ubx.msg_id == UBX_NAV_STATUS_ID) {
gps.fix = UBX_NAV_STATUS_GPSfix(gps_ubx.msg_buf);
gps_ubx.status_flags = UBX_NAV_STATUS_Flags(gps_ubx.msg_buf);
gps_ubx.sol_flags = UBX_NAV_SOL_Flags(gps_ubx.msg_buf);
}
}
#if USE_GPS_UBX_RXM_RAW
else if (gps_ubx.msg_class == UBX_RXM_ID) {
if (gps_ubx.msg_id == UBX_RXM_RAW_ID) {
gps_ubx_raw.iTOW = UBX_RXM_RAW_iTOW(gps_ubx.msg_buf);
gps_ubx_raw.week = UBX_RXM_RAW_week(gps_ubx.msg_buf);
gps_ubx_raw.numSV = UBX_RXM_RAW_numSV(gps_ubx.msg_buf);
uint8_t i;
for (i = 0; i < gps_ubx_raw.numSV; i++) {
gps_ubx_raw.measures[i].cpMes = UBX_RXM_RAW_cpMes(gps_ubx.msg_buf, i);
gps_ubx_raw.measures[i].prMes = UBX_RXM_RAW_prMes(gps_ubx.msg_buf, i);
gps_ubx_raw.measures[i].doMes = UBX_RXM_RAW_doMes(gps_ubx.msg_buf, i);
gps_ubx_raw.measures[i].sv = UBX_RXM_RAW_sv(gps_ubx.msg_buf, i);
gps_ubx_raw.measures[i].mesQI = UBX_RXM_RAW_mesQI(gps_ubx.msg_buf, i);
gps_ubx_raw.measures[i].cno = UBX_RXM_RAW_cno(gps_ubx.msg_buf, i);
gps_ubx_raw.measures[i].lli = UBX_RXM_RAW_lli(gps_ubx.msg_buf, i);
}
}
}
#endif
}
void parse_ins_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,&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);
}
#ifdef USE_GPS_XSENS
else if (xsens_id == XSENS_GPSStatus_ID) {
gps.nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
gps.num_sv = 0;
gps.last_fix_time = cpu_time_sec;
uint8_t i;
// Do not write outside buffer
for(i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) {
uint8_t ch = XSENS_GPSStatus_chn(xsens_msg_buf,i);
if (ch > gps.nb_channels) continue;
gps.svinfos[ch].svid = XSENS_GPSStatus_svid(xsens_msg_buf, i);
gps.svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens_msg_buf, i);
gps.svinfos[ch].qi = XSENS_GPSStatus_qi(xsens_msg_buf, i);
gps.svinfos[ch].cno = XSENS_GPSStatus_cnr(xsens_msg_buf, i);
if (gps.svinfos[ch].flags > 0)
{
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)) {
ins_p = XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf,offset);
ins_q = XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf,offset);
ins_r = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf,offset);
offset += XSENS_DATA_RAWInertial_LENGTH;
}
if (XSENS_MASK_RAWGPS(xsens_output_mode)) {
#if defined(USE_GPS_XSENS_RAW_DATA) && defined(USE_GPS_XSENS)
#ifdef GPS_LED
LED_TOGGLE(GPS_LED);
#endif
gps.last_fix_time = cpu_time_sec;
gps.week = 0; // FIXME
gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10;
gps.lla_pos.lat = RadOfDeg(XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset));
gps.lla_pos.lon = RadOfDeg(XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset));
gps.lla_pos.alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset);
/* Set the real UTM zone */
gps.utm_pos.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
utm_f.zone = nav_utm_zone0;
/* convert to utm */
utm_of_lla_f(&utm_f, &lla_f);
/* copy results of utm conversion */
gps.utm_pos.east = utm_f.east*100;
gps.utm_pos.north = utm_f.north*100;
gps.utm_pos.alt = gps.lla_pos.alt;
ins_x = utm_f.east;
ins_y = utm_f.north;
// Altitude: Xsens LLH gives ellipsoid height
ins_z = -(INS_FORMAT)XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 1000.;
// Compute geoid (MSL) height
float hmsl;
WGS84_ELLIPSOID_TO_GEOID(lla_f.lat,lla_f.lon,hmsl);
gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) - (hmsl * 1000.0f);
ins_vx = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset)) / 100.;
ins_vy = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset)) / 100.;
ins_vz = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset)) / 100.;
gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset);
gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset);
gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset);
gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset) / 100;
gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset) / 100;
gps.pdop = 5; // FIXME Not output by XSens
#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)) {
ins_ax = XSENS_DATA_Calibrated_accX(xsens_msg_buf,offset);
ins_ay = XSENS_DATA_Calibrated_accY(xsens_msg_buf,offset);
ins_az = XSENS_DATA_Calibrated_accZ(xsens_msg_buf,offset);
l++;
}
if (!XSENS_MASK_GyrOut(xsens_output_settings)) {
ins_p = XSENS_DATA_Calibrated_gyrX(xsens_msg_buf,offset);
ins_q = XSENS_DATA_Calibrated_gyrY(xsens_msg_buf,offset);
ins_r = XSENS_DATA_Calibrated_gyrZ(xsens_msg_buf,offset);
l++;
}
if (!XSENS_MASK_MagOut(xsens_output_settings)) {
ins_mx = XSENS_DATA_Calibrated_magX(xsens_msg_buf,offset);
ins_my = XSENS_DATA_Calibrated_magY(xsens_msg_buf,offset);
ins_mz = XSENS_DATA_Calibrated_magZ(xsens_msg_buf,offset);
l++;
}
offset += l * XSENS_DATA_Calibrated_LENGTH / 3;
}
if (XSENS_MASK_Orientation(xsens_output_mode)) {
if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x00) {
float q0 = XSENS_DATA_Quaternion_q0(xsens_msg_buf,offset);
float q1 = XSENS_DATA_Quaternion_q1(xsens_msg_buf,offset);
float q2 = XSENS_DATA_Quaternion_q2(xsens_msg_buf,offset);
float q3 = XSENS_DATA_Quaternion_q3(xsens_msg_buf,offset);
float dcm00 = 1.0 - 2 * (q2*q2 + q3*q3);
float dcm01 = 2 * (q1*q2 + q0*q3);
float dcm02 = 2 * (q1*q3 - q0*q2);
float dcm12 = 2 * (q2*q3 + q0*q1);
float dcm22 = 1.0 - 2 * (q1*q1 + q2*q2);
ins_phi = atan2(dcm12, dcm22);
ins_theta = -asin(dcm02);
ins_psi = atan2(dcm01, dcm00);
offset += XSENS_DATA_Quaternion_LENGTH;
}
if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x01) {
ins_phi = XSENS_DATA_Euler_roll(xsens_msg_buf,offset) * M_PI / 180;
ins_theta = XSENS_DATA_Euler_pitch(xsens_msg_buf,offset) * M_PI / 180;
ins_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;
}
new_ins_attitude = 1;
}
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)) {
#if (!defined(USE_GPS_XSENS_RAW_DATA)) && defined(USE_GPS_XSENS)
gps.last_fix_time = cpu_time_sec;
lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens_msg_buf,offset));
lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens_msg_buf,offset));
gps.lla_pos.lat = (int32_t)(lla_f.lat * 1e7);
gps.lla_pos.lon = (int32_t)(lla_f.lon * 1e7);
gps.utm_pos.zone = (DegOfRad(lla_f.lon)+180) / 6 + 1;
/* convert to utm */
utm_of_lla_f(&utm_f, &lla_f);
/* copy results of utm conversion */
gps.utm_pos.east = utm_f.east*100;
gps.utm_pos.north = utm_f.north*100;
ins_x = utm_f.east;
ins_y = utm_f.north;
ins_z = XSENS_DATA_Position_alt(xsens_msg_buf,offset);//TODO is this hms or above ellipsoid?
gps.hmsl = ins_z * 1000;
#endif
offset += XSENS_DATA_Position_LENGTH;
}
if (XSENS_MASK_Velocity(xsens_output_mode)) {
#if (!defined(USE_GPS_XSENS_RAW_DATA)) && defined(USE_GPS_XSENS)
ins_vx = XSENS_DATA_Velocity_vx(xsens_msg_buf,offset);
ins_vy = XSENS_DATA_Velocity_vy(xsens_msg_buf,offset);
ins_vz = XSENS_DATA_Velocity_vz(xsens_msg_buf,offset);
#endif
offset += XSENS_DATA_Velocity_LENGTH;
}
if (XSENS_MASK_Status(xsens_output_mode)) {
xsens_msg_status = XSENS_DATA_Status_status(xsens_msg_buf,offset);
#ifdef USE_GPS_XSENS
if (bit_is_set(xsens_msg_status,2)) gps.fix = GPS_FIX_3D; // gps fix
else if (bit_is_set(xsens_msg_status,1)) gps.fix = 0x01; // efk valid
else gps.fix = GPS_FIX_NONE;
#endif
offset += XSENS_DATA_Status_LENGTH;
}
if (XSENS_MASK_TimeStamp(xsens_output_settings)) {
xsens_time_stamp = XSENS_DATA_TimeStamp_ts(xsens_msg_buf,offset);
#ifdef USE_GPS_XSENS
gps.tow = xsens_time_stamp;
#endif
offset += XSENS_DATA_TimeStamp_LENGTH;
}
if (XSENS_MASK_UTC(xsens_output_settings)) {
xsens_hour = XSENS_DATA_UTC_hour(xsens_msg_buf,offset);
xsens_min = XSENS_DATA_UTC_min(xsens_msg_buf,offset);
xsens_sec = XSENS_DATA_UTC_sec(xsens_msg_buf,offset);
xsens_nanosec = XSENS_DATA_UTC_nanosec(xsens_msg_buf,offset);
xsens_year = XSENS_DATA_UTC_year(xsens_msg_buf,offset);
xsens_month = XSENS_DATA_UTC_month(xsens_msg_buf,offset);
xsens_day = XSENS_DATA_UTC_day(xsens_msg_buf,offset);
offset += XSENS_DATA_UTC_LENGTH;
}
//}
}
}
void parse_ins_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);
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_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];
offset += 3;
if (code1 == 0x10) {
if (code2 == 0x10) {
// UTC
} else if (code2 == 0x20) {
// Packet Counter
}
if (code2 == 0x30) {
// ITOW
}
} else if (code1 == 0x20) {
if (code2 == 0x30) {
// Attitude Euler
ins_phi = XSENS_DATA_Euler_roll(xsens_msg_buf, offset) * M_PI / 180;
ins_theta = XSENS_DATA_Euler_pitch(xsens_msg_buf, offset) * M_PI / 180;
ins_psi = XSENS_DATA_Euler_yaw(xsens_msg_buf, offset) * M_PI / 180;
new_ins_attitude = 1;
}
} else if (code1 == 0x40) {
if (code2 == 0x10) {
// Delta-V
ins_ax = XSENS_DATA_Acceleration_x(xsens_msg_buf, offset) * 100.0f;
ins_ay = XSENS_DATA_Acceleration_y(xsens_msg_buf, offset) * 100.0f;
ins_az = XSENS_DATA_Acceleration_z(xsens_msg_buf, offset) * 100.0f;
}
} else if (code1 == 0x80) {
if (code2 == 0x20) {
// Rate Of Turn
ins_p = XSENS_DATA_RateOfTurn_x(xsens_msg_buf, offset) * M_PI / 180;
ins_q = XSENS_DATA_RateOfTurn_y(xsens_msg_buf, offset) * M_PI / 180;
ins_r = XSENS_DATA_RateOfTurn_z(xsens_msg_buf, offset) * M_PI / 180;
}
} else if (code1 == 0x30) {
if (code2 == 0x10) {
// Baro Pressure
}
} else if (code1 == 0xE0) {
if (code2 == 0x20) {
// Status Word
xsens_msg_statusword = XSENS_DATA_StatusWord_status(xsens_msg_buf, offset);
//gps.tow = xsens_msg_statusword;
#if USE_GPS_XSENS
if (bit_is_set(xsens_msg_statusword, 2) && bit_is_set(xsens_msg_statusword, 1)) {
if (bit_is_set(xsens_msg_statusword, 23) && bit_is_set(xsens_msg_statusword, 24)) {
gps.fix = GPS_FIX_3D;
} else {
gps.fix = GPS_FIX_2D;
}
} else { gps.fix = GPS_FIX_NONE; }
#endif
}
} else if (code1 == 0x88) {
if (code2 == 0x40) {
gps.week = XSENS_DATA_GpsSol_week(xsens_msg_buf, offset);
gps.num_sv = XSENS_DATA_GpsSol_numSv(xsens_msg_buf, offset);
gps.pacc = XSENS_DATA_GpsSol_pAcc(xsens_msg_buf, offset);
gps.sacc = XSENS_DATA_GpsSol_sAcc(xsens_msg_buf, offset);
gps.pdop = XSENS_DATA_GpsSol_pDop(xsens_msg_buf, offset);
} else if (code2 == 0xA0) {
// SVINFO
gps.tow = XSENS_XDI_GpsSvInfo_iTOW(xsens_msg_buf + offset);
#if USE_GPS_XSENS
gps.nb_channels = XSENS_XDI_GpsSvInfo_nch(xsens_msg_buf + offset);
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
uint8_t i;
// Do not write outside buffer
for (i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) {
uint8_t ch = XSENS_XDI_GpsSvInfo_chn(xsens_msg_buf + offset, i);
if (ch > gps.nb_channels) { continue; }
gps.svinfos[ch].svid = XSENS_XDI_GpsSvInfo_svid(xsens_msg_buf + offset, i);
gps.svinfos[ch].flags = XSENS_XDI_GpsSvInfo_bitmask(xsens_msg_buf + offset, i);
gps.svinfos[ch].qi = XSENS_XDI_GpsSvInfo_qi(xsens_msg_buf + offset, i);
gps.svinfos[ch].cno = XSENS_XDI_GpsSvInfo_cnr(xsens_msg_buf + offset, i);
}
#endif
}
} else if (code1 == 0x50) {
if (code2 == 0x10) {
//gps.hmsl = XSENS_DATA_Altitude_h(xsens_msg_buf,offset)* 1000.0f;
} else if (code2 == 0x20) {
// Altitude Elipsoid
gps.utm_pos.alt = XSENS_DATA_Altitude_h(xsens_msg_buf, offset) * 1000.0f;
// Compute geoid (MSL) height
float geoid_h = wgs84_ellipsoid_to_geoid(lla_f.lat, lla_f.lon);
gps.hmsl = gps.utm_pos.alt - (geoid_h * 1000.0f);
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
//gps.tow = geoid_h * 1000.0f; //gps.utm_pos.alt;
} else if (code2 == 0x40) {
// LatLong
#ifdef GPS_LED
LED_TOGGLE(GPS_LED);
#endif
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
gps.week = 0; // FIXME
lla_f.lat = RadOfDeg(XSENS_DATA_LatLon_lat(xsens_msg_buf, offset));
lla_f.lon = RadOfDeg(XSENS_DATA_LatLon_lon(xsens_msg_buf, offset));
// Set the real UTM zone
gps.utm_pos.zone = (DegOfRad(lla_f.lon) + 180) / 6 + 1;
utm_f.zone = nav_utm_zone0;
// convert to utm
utm_of_lla_f(&utm_f, &lla_f);
// copy results of utm conversion
gps.utm_pos.east = utm_f.east * 100;
gps.utm_pos.north = utm_f.north * 100;
SetBit(gps.valid_fields, GPS_VALID_POS_UTM_BIT);
gps_xsens_publish();
}
} else if (code1 == 0xD0) {
if (code2 == 0x10) {
// Velocity
ins_vx = ((INS_FORMAT)XSENS_DATA_VelocityXYZ_x(xsens_msg_buf, offset));
ins_vy = ((INS_FORMAT)XSENS_DATA_VelocityXYZ_y(xsens_msg_buf, offset));
ins_vz = ((INS_FORMAT)XSENS_DATA_VelocityXYZ_z(xsens_msg_buf, offset));
gps.ned_vel.x = ins_vx;
gps.ned_vel.y = ins_vy;
gps.ned_vel.z = ins_vx;
SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
}
}
if (subpacklen < 0) {
subpacklen = 0;
}
offset += subpacklen;
}
/*
gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset) / 100;
gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset) / 100;
gps.pdop = 5; // FIXME Not output by XSens
*/
/*
*/
/*
ins_mx = XSENS_DATA_Calibrated_magX(xsens_msg_buf,offset);
ins_my = XSENS_DATA_Calibrated_magY(xsens_msg_buf,offset);
ins_mz = XSENS_DATA_Calibrated_magZ(xsens_msg_buf,offset);
*/
/*
xsens_time_stamp = XSENS_DATA_TimeStamp_ts(xsens_msg_buf,offset);
#if USE_GPS_XSENS
gps.tow = xsens_time_stamp;
#endif
*/
}
}
void stateCalcPositionNed_i(void)
{
if (bit_is_set(state.pos_status, POS_NED_I)) {
return;
}
int errno = 0;
if (state.ned_initialized_i) {
if (bit_is_set(state.pos_status, POS_NED_F)) {
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_ENU_I)) {
INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
} else if (bit_is_set(state.pos_status, POS_ENU_F)) {
ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
SetBit(state.pos_status, POS_ENU_I);
INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
} else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i);
} else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
/* transform ecef_f -> ned_f, set status bit, then convert to int */
ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
SetBit(state.pos_status, POS_NED_F);
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_F)) {
/* transform lla_f -> ecef_f -> ned_f, set status bits, then convert to int */
ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
SetBit(state.pos_status, POS_ECEF_F);
ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f);
SetBit(state.pos_status, POS_NED_F);
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_I)) {
ned_of_lla_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.lla_pos_i);
} else { /* could not get this representation, set errno */
errno = 1;
}
} else if (state.utm_initialized_f) {
if (bit_is_set(state.pos_status, POS_NED_F)) {
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_ENU_I)) {
INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
} else if (bit_is_set(state.pos_status, POS_ENU_F)) {
ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
SetBit(state.pos_status, POS_ENU_I);
INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i);
} else if (bit_is_set(state.pos_status, POS_UTM_F)) {
/* transform utm_f -> ned_f -> ned_i, set status bits */
NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
SetBit(state.pos_status, POS_NED_F);
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_F)) {
/* transform lla_f -> utm_f -> ned_f -> ned_i, set status bits */
utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
SetBit(state.pos_status, POS_UTM_F);
NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
SetBit(state.pos_status, POS_NED_F);
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_I)) {
/* transform lla_i -> lla_f -> utm_f -> ned_f -> ned_i, set status bits */
LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
SetBit(state.pos_status, POS_LLA_F);
utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
SetBit(state.pos_status, POS_UTM_F);
NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f);
SetBit(state.pos_status, POS_NED_F);
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
} else { /* could not get this representation, set errno */
errno = 2;
}
} else { /* ned coordinate system not initialized, set errno */
errno = 3;
}
if (errno) {
//struct NedCoor_i _ned_zero = {0};
//return _ned_zero;
}
/* set bit to indicate this representation is computed */
SetBit(state.pos_status, POS_NED_I);
}
void dl_parse_msg(void) {
datalink_time = 0;
uint8_t msg_id = IdOfMsg(dl_buffer);
#if 0 // not ready yet
uint8_t sender_id = SenderIdOfMsg(dl_buffer);
/* parse telemetry messages coming from other AC */
if (sender_id != 0) {
switch (msg_id) {
#ifdef TCAS
case DL_TCAS_RA:
{
if (DL_TCAS_RESOLVE_ac_id(dl_buffer) == AC_ID && SenderIdOfMsg(dl_buffer) != AC_ID) {
uint8_t ac_id_conflict = SenderIdOfMsg(dl_buffer);
tcas_acs_status[the_acs_id[ac_id_conflict]].resolve = DL_TCAS_RA_resolve(dl_buffer);
}
}
#endif
}
return;
}
#endif
if (msg_id == DL_PING) {
DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice)
} else
#ifdef TRAFFIC_INFO
if (msg_id == DL_ACINFO && DL_ACINFO_ac_id(dl_buffer) != AC_ID) {
uint8_t id = DL_ACINFO_ac_id(dl_buffer);
float ux = MOfCm(DL_ACINFO_utm_east(dl_buffer));
float uy = MOfCm(DL_ACINFO_utm_north(dl_buffer));
float a = MOfCm(DL_ACINFO_alt(dl_buffer));
float c = RadOfDeg(((float)DL_ACINFO_course(dl_buffer))/ 10.);
float s = MOfCm(DL_ACINFO_speed(dl_buffer));
float cl = MOfCm(DL_ACINFO_climb(dl_buffer));
uint32_t t = DL_ACINFO_itow(dl_buffer);
SetAcInfo(id, ux, uy, c, a, s, cl, t);
} else
#endif
#ifdef NAV
if (msg_id == DL_MOVE_WP && DL_MOVE_WP_ac_id(dl_buffer) == AC_ID) {
uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
float a = MOfCm(DL_MOVE_WP_alt(dl_buffer));
/* Computes from (lat, long) in the referenced UTM zone */
struct LlaCoor_f lla;
lla.lat = RadOfDeg((float)(DL_MOVE_WP_lat(dl_buffer) / 1e7));
lla.lon = RadOfDeg((float)(DL_MOVE_WP_lon(dl_buffer) / 1e7));
struct UtmCoor_f utm;
utm.zone = nav_utm_zone0;
utm_of_lla_f(&utm, &lla);
nav_move_waypoint(wp_id, utm.east, utm.north, a);
/* Waypoint range is limited. Computes the UTM pos back from the relative
coordinates */
utm.east = waypoints[wp_id].x + nav_utm_east0;
utm.north = waypoints[wp_id].y + nav_utm_north0;
DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0);
} else if (msg_id == DL_BLOCK && DL_BLOCK_ac_id(dl_buffer) == AC_ID) {
nav_goto_block(DL_BLOCK_block_id(dl_buffer));
SEND_NAVIGATION(DefaultChannel, DefaultDevice);
} else
#endif /** NAV */
#ifdef WIND_INFO
if (msg_id == DL_WIND_INFO && DL_WIND_INFO_ac_id(dl_buffer) == AC_ID) {
struct FloatVect2 wind;
wind.x = DL_WIND_INFO_north(dl_buffer);
wind.y = DL_WIND_INFO_east(dl_buffer);
stateSetHorizontalWindspeed_f(&wind);
#if !USE_AIRSPEED
float airspeed = DL_WIND_INFO_airspeed(dl_buffer);
stateSetAirspeed_f(&airspeed);
#endif
#ifdef WIND_INFO_RET
DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, DefaultDevice, &wind.y, &wind.x, stateGetAirspeed_f());
#endif
} else
#endif /** WIND_INFO */
#ifdef HITL
/** Infrared and GPS sensors are replaced by messages on the datalink */
if (msg_id == DL_HITL_INFRARED) {
/** This code simulates infrared.c:ir_update() */
infrared.roll = DL_HITL_INFRARED_roll(dl_buffer);
infrared.pitch = DL_HITL_INFRARED_pitch(dl_buffer);
infrared.top = DL_HITL_INFRARED_top(dl_buffer);
} else if (msg_id == DL_HITL_UBX) {
/** This code simulates gps_ubx.c:parse_ubx() */
if (gps_msg_received) {
gps_nb_ovrn++;
} else {
ubx_class = DL_HITL_UBX_class(dl_buffer);
ubx_id = DL_HITL_UBX_id(dl_buffer);
uint8_t l = DL_HITL_UBX_ubx_payload_length(dl_buffer);
uint8_t *ubx_payload = DL_HITL_UBX_ubx_payload(dl_buffer);
memcpy(ubx_msg_buf, ubx_payload, l);
gps_msg_received = TRUE;
}
} else
#endif
#ifdef DlSetting
if (msg_id == DL_SETTING && DL_SETTING_ac_id(dl_buffer) == AC_ID) {
uint8_t i = DL_SETTING_index(dl_buffer);
float val = DL_SETTING_value(dl_buffer);
DlSetting(i, val);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
} else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(dl_buffer) == AC_ID) {
uint8_t i = DL_GET_SETTING_index(dl_buffer);
float val = settings_get_value(i);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
} else
#endif /** Else there is no dl_settings section in the flight plan */
#if USE_JOYSTICK
if (msg_id == DL_JOYSTICK_RAW && DL_JOYSTICK_RAW_ac_id(dl_buffer) == AC_ID) {
JoystickHandeDatalink(DL_JOYSTICK_RAW_roll(dl_buffer),
DL_JOYSTICK_RAW_pitch(dl_buffer),
DL_JOYSTICK_RAW_throttle(dl_buffer));
} else
#endif // USE_JOYSTICK
#if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
if (msg_id == DL_RC_3CH /*&& DL_RC_3CH_ac_id(dl_buffer) == TX_ID*/) {
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_3ch_datalink(
DL_RC_3CH_throttle_mode(dl_buffer),
DL_RC_3CH_roll(dl_buffer),
DL_RC_3CH_pitch(dl_buffer));
} else if (msg_id == DL_RC_4CH && DL_RC_4CH_ac_id(dl_buffer) == AC_ID) {
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_4ch_datalink(
DL_RC_4CH_mode(dl_buffer),
DL_RC_4CH_throttle(dl_buffer),
DL_RC_4CH_roll(dl_buffer),
DL_RC_4CH_pitch(dl_buffer),
DL_RC_4CH_yaw(dl_buffer));
} else
#endif // RC_DATALINK
{ /* Last else */
/* Parse modules datalink */
modules_parse_datalink(msg_id);
}
}