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_mode = (Bool_val(m) ? 3 : 0);
gps_course = DegOfRad(Double_val(c)) * 10.;
gps_alt = Double_val(a) * 100.;
gps_gspeed = Double_val(s) * 100.;
gps_climb = Double_val(cl) * 100.;
gps_week = 0; // FIXME
gps_itow = Double_val(t) * 1000.;
#ifdef GPS_USE_LATLONG
gps_lat = DegOfRad(Double_val(lat))*1e7;
gps_lon = DegOfRad(Double_val(lon))*1e7;
latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), nav_utm_zone0);
gps_utm_east = latlong_utm_x * 100;
gps_utm_north = latlong_utm_y * 100;
gps_utm_zone = nav_utm_zone0;
x = y = z; /* Just to get rid of the "unused arg" warning */
#else // GPS_USE_LATLONG
gps_utm_east = Int_val(x);
gps_utm_north = Int_val(y);
gps_utm_zone = Int_val(z);
lat = lon; /* 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_numSV = 7;
gps_verbose_downlink = !launch;
UseGpsPosNoSend(estimator_update_state_gps);
gps_downlink();
return Val_unit;
}
void parse_gps_msg( void ) {
if (ubx_class == UBX_NAV_ID) {
if (ubx_id == UBX_NAV_STATUS_ID) {
gps_mode = UBX_NAV_STATUS_GPSfix(ubx_msg_buf);
gps_status_flags = UBX_NAV_STATUS_Flags(ubx_msg_buf);
gps_sol_flags = UBX_NAV_SOL_Flags(ubx_msg_buf);
#ifdef GPS_USE_LATLONG
/* Computes from (lat, long) in the referenced UTM zone */
} else if (ubx_id == UBX_NAV_POSLLH_ID) {
gps_lat = UBX_NAV_POSLLH_LAT(ubx_msg_buf);
gps_lon = UBX_NAV_POSLLH_LON(ubx_msg_buf);
gps_hmsl = UBX_NAV_POSLLH_HMSL(ubx_msg_buf);
latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), nav_utm_zone0);
gps_utm_east = latlong_utm_x * 100;
gps_utm_north = latlong_utm_y * 100;
gps_alt = UBX_NAV_POSLLH_HMSL(ubx_msg_buf) / 10;
gps_utm_zone = nav_utm_zone0;
#else
} else if (ubx_id == UBX_NAV_POSUTM_ID) {
gps_utm_east = UBX_NAV_POSUTM_EAST(ubx_msg_buf);
gps_utm_north = UBX_NAV_POSUTM_NORTH(ubx_msg_buf);
uint8_t hem = UBX_NAV_POSUTM_HEM(ubx_msg_buf);
if (hem == UTM_HEM_SOUTH)
gps_utm_north -= 1000000000; /* Subtract false northing: -10000km */
gps_alt = UBX_NAV_POSUTM_ALT(ubx_msg_buf);
gps_utm_zone = UBX_NAV_POSUTM_ZONE(ubx_msg_buf);
#endif
} else if (ubx_id == UBX_NAV_VELNED_ID) {
gps_speed_3d = UBX_NAV_VELNED_Speed(ubx_msg_buf);
gps_gspeed = UBX_NAV_VELNED_GSpeed(ubx_msg_buf);
gps_climb = - UBX_NAV_VELNED_VEL_D(ubx_msg_buf);
gps_course = UBX_NAV_VELNED_Heading(ubx_msg_buf) / 10000;
gps_itow = UBX_NAV_VELNED_ITOW(ubx_msg_buf);
gps_pos_available = TRUE; /* The 3 UBX messages are sent in one rafale */
} else if (ubx_id == UBX_NAV_SOL_ID) {
#ifdef GPS_TIMESTAMP
/* get hardware clock ticks */
gps_t0 = T0TC;
/* set receive time */
gps_t0_itow = UBX_NAV_SOL_ITOW(ubx_msg_buf);
gps_t0_frac = UBX_NAV_SOL_Frac(ubx_msg_buf);
#endif
gps_mode = UBX_NAV_SOL_GPSfix(ubx_msg_buf);
gps_PDOP = UBX_NAV_SOL_PDOP(ubx_msg_buf);
gps_Pacc = UBX_NAV_SOL_Pacc(ubx_msg_buf);
gps_ecefVZ = UBX_NAV_SOL_ECEFVZ(ubx_msg_buf);
gps_Sacc = UBX_NAV_SOL_Sacc(ubx_msg_buf);
gps_numSV = UBX_NAV_SOL_numSV(ubx_msg_buf);
gps_week = UBX_NAV_SOL_week(ubx_msg_buf);
} else if (ubx_id == UBX_NAV_SVINFO_ID) {
gps_nb_channels = Min(UBX_NAV_SVINFO_NCH(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(ubx_msg_buf, i);
gps_svinfos[i].flags = UBX_NAV_SVINFO_Flags(ubx_msg_buf, i);
gps_svinfos[i].qi = UBX_NAV_SVINFO_QI(ubx_msg_buf, i);
gps_svinfos[i].cno = UBX_NAV_SVINFO_CNO(ubx_msg_buf, i);
gps_svinfos[i].elev = UBX_NAV_SVINFO_Elev(ubx_msg_buf, i);
gps_svinfos[i].azim = UBX_NAV_SVINFO_Azim(ubx_msg_buf, 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);
} 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 */
float lat = RadOfDeg((float)(DL_MOVE_WP_lat(dl_buffer) / 1e7));
float lon = RadOfDeg((float)(DL_MOVE_WP_lon(dl_buffer) / 1e7));
latlong_utm_of(lat, lon, nav_utm_zone0);
nav_move_waypoint(wp_id, latlong_utm_x, latlong_utm_y, a);
/* Waypoint range is limited. Computes the UTM pos back from the relative
coordinates */
latlong_utm_x = waypoints[wp_id].x + nav_utm_east0;
latlong_utm_y = waypoints[wp_id].y + nav_utm_north0;
DOWNLINK_SEND_WP_MOVED(DefaultChannel, &wp_id, &latlong_utm_x, &latlong_utm_y, &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);
} else
#endif /** NAV */
#ifdef WIND_INFO
if (msg_id == DL_WIND_INFO && DL_WIND_INFO_ac_id(dl_buffer) == AC_ID) {
wind_east = DL_WIND_INFO_east(dl_buffer);
wind_north = DL_WIND_INFO_north(dl_buffer);
#ifndef USE_AIRSPEED
estimator_airspeed = DL_WIND_INFO_airspeed(dl_buffer);
#endif
#ifdef WIND_INFO_RET
DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, &wind_east, &wind_north, &estimator_airspeed);
#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() */
ir_roll = DL_HITL_INFRARED_roll(dl_buffer);
ir_pitch = DL_HITL_INFRARED_pitch(dl_buffer);
ir_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, &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, &i, &val);
} else
#endif /** Else there is no dl_settings section in the flight plan */
#ifdef 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*/) {
LED_TOGGLE(3);
parse_rc_datalink(
DL_RC_3CH_throttle_mode(dl_buffer),
DL_RC_3CH_roll(dl_buffer),
DL_RC_3CH_pitch(dl_buffer));
} else
#endif // RC_DATALINK
{ /* Last else */
/* Parse modules datalink */
modules_parse_datalink(msg_id);
}
}
/*
void decode_imupacket( struct imu *data, uint8_t* buffer){
data->phi = (double)((((signed char)buffer[25])<<8)|buffer[26]);
data->theta = (double)((((signed char)buffer[27])<<8)|buffer[28]);
data->psi = (double)((((signed char)buffer[29])<<8)|buffer[30]);
}
*/
void parse_ugear_msg( void ){
float ins_phi, ins_psi, ins_theta;
switch (ugear_type){
case 0: /*gps*/
ugear_debug1 = ugear_debug1+1;
gps_lat = UGEAR_NAV_POSLLH_LAT(ugear_msg_buf);
gps_lon = UGEAR_NAV_POSLLH_LON(ugear_msg_buf);
nav_utm_zone0 = (gps_lon/10000000+180) / 6 + 1;
latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), nav_utm_zone0);
gps_utm_east = latlong_utm_x * 100;
gps_utm_north = latlong_utm_y * 100;
gps_alt = UGEAR_NAV_POSLLH_HEIGHT(ugear_msg_buf);
gps_utm_zone = nav_utm_zone0;
gps_gspeed = UGEAR_NAV_VELNED_GSpeed(ugear_msg_buf);
gps_climb = - UGEAR_NAV_POSLLH_VD(ugear_msg_buf);
gps_course = UGEAR_NAV_VELNED_Heading(ugear_msg_buf)/10000; /*in decdegree */
gps_PDOP = UGEAR_NAV_SOL_PDOP(ugear_msg_buf);
gps_Pacc = UGEAR_NAV_SOL_Pacc(ugear_msg_buf);
gps_Sacc = UGEAR_NAV_SOL_Sacc(ugear_msg_buf);
gps_numSV = UGEAR_NAV_SOL_numSV(ugear_msg_buf);
gps_week = 0; // FIXME
gps_itow = UGEAR_NAV_VELNED_ITOW(ugear_msg_buf);
//ugear_debug2 = gps_climb;
//ugear_debug4 = (int32_t)(UGEAR_NAV_VELNED_GSpeed(ugear_msg_buf));
//ugear_debug5 = UGEAR_NAV_VELNED_GSpeed(ugear_msg_buf);
//ugear_debug6 = (int16_t)estimator_phi*100;
gps_mode = 3; /*force GPSfix to be valided*/
gps_pos_available = TRUE; /* The 3 UBX messages are sent in one rafale */
break;
case 1: /*IMU*/
ugear_debug2 = ugear_debug2+1;
ugear_phi = UGEAR_IMU_PHI(ugear_msg_buf);
ugear_psi = UGEAR_IMU_PSI(ugear_msg_buf);
ugear_theta = UGEAR_IMU_THE(ugear_msg_buf);
ugear_debug4 = (int32_t)ugear_phi;
ugear_debug5 = (int32_t)ugear_theta;
ugear_debug6 = (int32_t)ugear_psi;
ugear_debug3 = 333;
ins_phi = (float)ugear_phi/10000 - ins_roll_neutral;
ins_psi = 0;
ins_theta = (float)ugear_theta/10000 - ins_pitch_neutral;
#ifndef INFRARED
EstimatorSetAtt(ins_phi, ins_psi, ins_theta);
#endif
break;
case 2: /*GPS status*/
// ugear_debug1 = 2;
gps_nb_channels = XSENS_GPSStatus_nch(ugear_msg_buf);
uint8_t is;
for(is = 0; is < Min(gps_nb_channels, 16); is++) {
uint8_t ch = XSENS_GPSStatus_chn(ugear_msg_buf,is);
if (ch > 16) continue;
gps_svinfos[ch].svid = XSENS_GPSStatus_svid(ugear_msg_buf, is);
gps_svinfos[ch].flags = XSENS_GPSStatus_bitmask(ugear_msg_buf, is);
gps_svinfos[ch].qi = XSENS_GPSStatus_qi(ugear_msg_buf, is);
gps_svinfos[ch].cno = XSENS_GPSStatus_cnr(ugear_msg_buf, is);
gps_svinfos[ch].elev = 0;
gps_svinfos[ch].azim = 0;
}
break;
case 3: /*servo*/
break;
case 4: /*health*/
break;
}
}
static gboolean read_data(GIOChannel *chan, GIOCondition cond, gpointer data) {
int count;
char buf[BUFSIZE];
/* receive data packet containing formatted data */
count = recv(sock, buf, sizeof(buf), 0);
if (count > 0) {
if (count == 23) {
// FillBufWith32bit(com_trans.buf, 1, gps_lat);
gps_lat = buf2uint(&buf[0]);
// FillBufWith32bit(com_trans.buf, 5, gps_lon);
gps_lon = buf2uint(&buf[4]);
// FillBufWith16bit(com_trans.buf, 9, (int16_t)(gps_alt/100)); // meters
gps_alt = buf2ushort(&buf[8]) * 100;
// FillBufWith16bit(com_trans.buf, 11, gps_gspeed); // ground speed
gps_gspeed = buf2ushort(&buf[10]);
// FillBufWith16bit(com_trans.buf, 13, gps_course); // course
gps_course = buf2ushort(&buf[12]);
// FillBufWith16bit(com_trans.buf, 15, (uint16_t)(estimator_airspeed*100)); // TAS (cm/s)
estimator_airspeed = buf2ushort(&buf[14]);
// com_trans.buf[16] = electrical.vsupply;
// should be (estimator_airspeed is two bytes):
// com_trans.buf[17] = electrical.vsupply;
electrical_vsupply = buf[16];
// com_trans.buf[17] = (uint8_t)(energy*10);
energy = buf[17] / 10;
// com_trans.buf[18] = (uint8_t)(ap_state->commands[COMMAND_THROTTLE]*100/MAX_PPRZ);
throttle = buf[18];
// com_trans.buf[19] = pprz_mode;
pprz_mode = buf[19];
// com_trans.buf[20] = nav_block;
nav_block = buf[20];
// FillBufWith16bit(com_trans.buf, 21, estimator_flight_time);
estimator_flight_time = buf2ushort(&buf[21]);
//gps_lat = 52.2648312 * 1e7;
//gps_lon = 9.9939456 * 1e7;
//gps_alt = 169 * 1000;
//gps_gspeed = 13 * 100;
//gps_course = 60 * 10;
//estimator_airspeed = 15 * 100;
//electrical_vsupply = 126;
//energy = 9;
//throttle = 51;
//pprz_mode = 2;
//nav_block = 1;
//estimator_flight_time = 123;
nav_utm_zone0 = (gps_lon/10000000+180) / 6 + 1;
latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), nav_utm_zone0);
gps_utm_east = latlong_utm_x * 100;
gps_utm_north = latlong_utm_y * 100;
gps_utm_zone = nav_utm_zone0;
printf("gps_lat %f\n", gps_lat/1e7);
printf("gps_lon %f\n", gps_lon/1e7);
printf("gps_alt %d\n", gps_alt);
printf("gps_gspeed %d\n", gps_gspeed);
printf("gps_course %d\n", gps_course);
printf("estimator_airspeed %d\n", estimator_airspeed);
printf("electrical_vsupply %d\n", electrical_vsupply);
printf("energy %d\n", energy);
printf("throttle %d\n", throttle);
printf("pprz_mode %d\n", pprz_mode);
printf("nav_block %d\n", nav_block);
printf("estimator_flight_time %d\n", estimator_flight_time);
printf("gps_utm_east %d\n", gps_utm_east);
printf("gps_utm_north %d\n", gps_utm_north);
printf("gps_utm_zone %d\n", gps_utm_zone);
/*
<message name="GPS" id="8">
<field name="mode" type="uint8" unit="byte_mask"/>
<field name="utm_east" type="int32" unit="cm" alt_unit="m"/>
<field name="utm_north" type="int32" unit="cm" alt_unit="m"/>
<field name="course" type="int16" unit="decideg" alt_unit="deg"/>
<field name="alt" type="int32" unit="mm" alt_unit="m"/>
<field name="speed" type="uint16" unit="cm/s" alt_unit="m/s"/>
<field name="climb" type="int16" unit="cm/s" alt_unit="m/s"/>
<field name="week" type="uint16" unit="weeks"/>
<field name="itow" type="uint32" unit="ms"/>
<field name="utm_zone" type="uint8"/>
<field name="gps_nb_err" type="uint8"/>
</message>
*/
IvySendMsg("%d GPS %d %d %d %d %d %d %d %d %d %d %d",
AC_ID,
3, // mode = 3D
gps_utm_east,
gps_utm_north,
gps_course,
gps_alt,
gps_gspeed,
0, // climb
0, // week
0, //itow
gps_utm_zone,
0); // gps_nb_err
/*
<message name="PPRZ_MODE" id="11">
<field name="ap_mode" type="uint8" values="MANUAL|AUTO1|AUTO2|HOME|NOGPS|FAILSAFE"/>
<field name="ap_gaz" type="uint8" values="MANUAL|AUTO_THROTTLE|AUTO_CLIMB|AUTO_ALT"/>
<field name="ap_lateral" type="uint8" values="MANUAL|ROLL_RATE|ROLL|COURSE"/>
<field name="ap_horizontal" type="uint8" values="WAYPOINT|ROUTE|CIRCLE"/>
<field name="if_calib_mode" type="uint8" values="NONE|DOWN|UP"/>
<field name="mcu1_status" type="uint8" values="LOST|OK|REALLY_LOST"/>
</message>
*/
IvySendMsg("%d PPRZ_MODE %d %d %d %d %d %d",
AC_ID,
pprz_mode,
0, // ap_gaz
0, // ap_lateral
0, // ap_horizontal
0, // if_calib_mode
0); // mcu1_status
/*
<message name="AIRSPEED" id="54">
<field name="airspeed" type="float" unit="m/s"/>
<field name="airspeed_sp" type="float" unit="m/s"/>
<field name="airspeed_cnt" type="float" unit="m/s"/>
<field name="groundspeed_sp" type="float" unit="m/s"/>
</message>
*/
IvySendMsg("%d AIRSPEED %f %d %d %d",
AC_ID,
(float)(estimator_airspeed / 100.),
0, // airspeed_sp
0, // airspeed_cnt
0); // groundspeed_sp
/*
<message name="BAT" id="12">
<field name="throttle" type="int16" unit="pprz"/>
<field name="voltage" type="uint8" unit="1e-1V" alt_unit="V" alt_unit_coef="0.1"/>
<field name="amps" type="int16" unit="A" alt_unit="A" />
<field name="flight_time" type="uint16" unit="s"/>
<field name="kill_auto_throttle" type="uint8" unit="bool"/>
<field name="block_time" type="uint16" unit="s"/>
<field name="stage_time" type="uint16" unit="s"/>
<field name="energy" type="int16" unit="mAh"/>
</message>
*/
IvySendMsg("%d BAT %d %d %d %d %d %d %d %d",
AC_ID,
throttle * MAX_PPRZ / 100,
electrical_vsupply,
0, // amps
estimator_flight_time,
0, // kill_auto_throttle
0, // block_time
0, // stage_time
energy);
/*
<message name="NAVIGATION" id="10">
<field name="cur_block" type="uint8"/>
<field name="cur_stage" type="uint8"/>
<field name="pos_x" type="float" unit="m" format="%.1f"/>
<field name="pos_y" type="float" unit="m" format="%.1f"/>
<field name="dist2_wp" type="float" format="%.1f" unit="m^2"/>
<field name="dist2_home" type="float" format="%.1f" unit="m^2"/>
<field name="circle_count" type="uint8"/>
<field name="oval_count" type="uint8"/>
</message>
*/
IvySendMsg("%d NAVIGATION %d %d %d %d %d %d %d %d",
AC_ID,
nav_block,
0, // cur_stage
0, // pos_x
0, // pos_y
0, // dist2_wp
0, // dist2_home
0, // circle_count
0); // oval_count
/*
<message name="ESTIMATOR" id="42">
<field name="z" type="float" unit="m"/>
<field name="z_dot" type="float" unit="m/s"/>
</message>
*/
IvySendMsg("%d ESTIMATOR %f %d",
AC_ID,
gps_alt / 1000.,
0); // z_dot
/*
<message name="ATTITUDE" id="6">
<field name="phi" type="float" unit="rad" alt_unit="deg"/>
<field name="psi" type="float" unit="rad" alt_unit="deg"/>
<field name="theta" type="float" unit="rad" alt_unit="deg"/>
</message>
*/
IvySendMsg("%d ATTITUDE %f %f %f",
AC_ID,
0., // phi
RadOfDeg(gps_course / 10.),
0.); // theta
}
}
else {
printf("disconnect\n");
close(sock);
g_main_loop_quit(ml);
return 0;
}
return 1;
}
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_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_numSV = gps_nb_channels;
uint8_t i;
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);
}
}
#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)
gps_week = 0; // FIXME
gps_itow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10;
gps_lat = XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset);
gps_lon = XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset);
/* Set the real UTM zone */
gps_utm_zone = (gps_lon/1e7+180) / 6 + 1;
latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), gps_utm_zone);
/* utm */
gps_utm_east = latlong_utm_x * 100;
gps_utm_north = latlong_utm_y * 100;
ins_x = latlong_utm_x;
ins_y = latlong_utm_y;
gps_alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 10;
ins_z = -(INS_FORMAT)gps_alt / 100.;
ins_vx = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset) / 100.;
ins_vy = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset) / 100.;
ins_vz = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 100.;
gps_climb = -XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 10;
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;
#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;
}
}
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)
xsens_lat = XSENS_DATA_Position_lat(xsens_msg_buf,offset);
xsens_lon = XSENS_DATA_Position_lon(xsens_msg_buf,offset);
gps_lat = (int32_t)(xsens_lat * 1e7);
gps_lon = (int32_t)(xsens_lon * 1e7);
gps_utm_zone = (xsens_lon+180) / 6 + 1;
latlong_utm_of(RadOfDeg(xsens_lat), RadOfDeg(xsens_lon), gps_utm_zone);
ins_x = latlong_utm_x;
ins_y = latlong_utm_y;
gps_utm_east = ins_x * 100;
gps_utm_north = ins_y * 100;
ins_z = XSENS_DATA_Position_alt(xsens_msg_buf,offset);
gps_alt = ins_z * 100;
#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_mode = 0x03; // gps fix
else if (bit_is_set(xsens_msg_status,1)) gps_mode = 0x01; // efk valid
else gps_mode = 0;
#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_itow = 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;
}
//}
}
}