mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-05 15:30:08 +08:00
[gps_datalink] update small message protocol
This commit is contained in:
kirkscheper
@@ -161,13 +161,6 @@
|
||||
<define name="USE_GPS_ALT" value="1"/>
|
||||
</section>
|
||||
|
||||
<section name="GPS" prefix="GPS_">
|
||||
<define name="USE_DATALINK_SMALL" value="1"/>
|
||||
<define name="LOCAL_ECEF_ORIGIN_X" value="392433200"/>
|
||||
<define name="LOCAL_ECEF_ORIGIN_Y" value="30036200"/>
|
||||
<define name="LOCAL_ECEF_ORIGIN_Z" value="500219700"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||
<define name="HOVER_KP" value="276"/>
|
||||
<define name="HOVER_KD" value="455"/>
|
||||
@@ -183,8 +176,6 @@
|
||||
<define name="H_Z" value="0.92060036"/>
|
||||
<define name="USE_GPS_HEADING" value="1"/>
|
||||
</section>
|
||||
<define name="USE_GPS" value="TRUE"/>
|
||||
<define name="USE_GPS_HEADING" value="TRUE"/>
|
||||
|
||||
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||
<define name="PGAIN" value="290"/>
|
||||
|
||||
@@ -173,13 +173,6 @@
|
||||
<define name="USE_GPS_ALT" value="1"/>
|
||||
</section>
|
||||
|
||||
<section name="GPS" prefix="GPS_">
|
||||
<define name="USE_DATALINK_SMALL" value="1"/>
|
||||
<define name="LOCAL_ECEF_ORIGIN_X" value="392433200"/>
|
||||
<define name="LOCAL_ECEF_ORIGIN_Y" value="30036200"/>
|
||||
<define name="LOCAL_ECEF_ORIGIN_Z" value="500219700"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||
<define name="HOVER_KP" value="276"/>
|
||||
<define name="HOVER_KD" value="455"/>
|
||||
|
||||
@@ -102,13 +102,6 @@
|
||||
<define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
<section name="GPS" prefix="GPS_">
|
||||
<define name="USE_DATALINK_SMALL" value="1"/>
|
||||
<define name="LOCAL_ECEF_ORIGIN_X" value="392433200"/>
|
||||
<define name="LOCAL_ECEF_ORIGIN_Y" value="30036200"/>
|
||||
<define name="LOCAL_ECEF_ORIGIN_Z" value="500219700"/>
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
|
||||
<!-- setpoints -->
|
||||
<define name="SP_MAX_P" value="140" unit="deg/s"/>
|
||||
|
||||
+3
-3
@@ -856,7 +856,6 @@
|
||||
|
||||
<message name="BLUEGIGA" id="106">
|
||||
<field name="data_rate" type="uint32" unit="bytes/s"/>
|
||||
<field name="last_msg" type="uint8[]"/>
|
||||
</message>
|
||||
|
||||
<!--107 is free -->
|
||||
@@ -2479,8 +2478,9 @@
|
||||
<message name="REMOTE_GPS_SMALL" id="54" link="broadcasted">
|
||||
<field name="ac_id" type="uint8"/>
|
||||
<field name="numsv" type="uint8"/>
|
||||
<field name="pos_xyz" type="uint32" unit="cm">bits 31-22 x position in cm : bits 21-12 y position in cm : bits 11-2 z position in cm : bits 1 and 0 are free</field>
|
||||
<field name="speed_xy" type="uint32" unit="cm/s">bits 31-22 speed x in cm/s : bits 21-12 speed y in cm/s : bits 11-2 heading in rad*1e2 : bits 1 and 0 are free</field>
|
||||
<field name="pos_xyz" type="uint32" unit="cm">bits 31-21 x position in cm : bits 20-10 y position in cm : bits 9-0 z position in cm</field>
|
||||
<field name="speed_xyz" type="uint32" unit="cm/s">bits 31-21 speed x in cm/s : bits 20-10 speed y in cm/s : bits 9-0 speed z in cm/s</field>
|
||||
<field name="heading" type="int16" unit="rad*1e4"/>
|
||||
</message>
|
||||
|
||||
<message name="REMOTE_GPS" id="55" link="broadcasted">
|
||||
|
||||
@@ -90,7 +90,7 @@ void dl_parse_msg(void)
|
||||
}
|
||||
break;
|
||||
|
||||
#if defined USE_NAVIGATION
|
||||
#ifdef USE_NAVIGATION
|
||||
case DL_BLOCK : {
|
||||
if (DL_BLOCK_ac_id(dl_buffer) != AC_ID) { break; }
|
||||
nav_goto_block(DL_BLOCK_block_id(dl_buffer));
|
||||
@@ -138,21 +138,21 @@ void dl_parse_msg(void)
|
||||
}
|
||||
break;
|
||||
#endif // RADIO_CONTROL_TYPE_DATALINK
|
||||
#if defined GPS_DATALINK
|
||||
#ifdef GPS_USE_DATALINK_SMALL
|
||||
case DL_REMOTE_GPS_SMALL :
|
||||
#if USE_GPS
|
||||
#ifdef GPS_DATALINK
|
||||
case DL_REMOTE_GPS_SMALL : {
|
||||
// Check if the GPS is for this AC
|
||||
if (DL_REMOTE_GPS_SMALL_ac_id(dl_buffer) != AC_ID) {
|
||||
break;
|
||||
}
|
||||
if (DL_REMOTE_GPS_SMALL_ac_id(dl_buffer) != AC_ID) { break; }
|
||||
|
||||
parse_gps_datalink_small(
|
||||
DL_REMOTE_GPS_SMALL_numsv(dl_buffer),
|
||||
DL_REMOTE_GPS_SMALL_pos_xyz(dl_buffer),
|
||||
DL_REMOTE_GPS_SMALL_speed_xy(dl_buffer));
|
||||
DL_REMOTE_GPS_SMALL_speed_xyz(dl_buffer),
|
||||
DL_REMOTE_GPS_SMALL_heading(dl_buffer));
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
case DL_REMOTE_GPS :
|
||||
|
||||
case DL_REMOTE_GPS : {
|
||||
// Check if the GPS is for this AC
|
||||
if (DL_REMOTE_GPS_ac_id(dl_buffer) != AC_ID) { break; }
|
||||
|
||||
@@ -171,10 +171,11 @@ void dl_parse_msg(void)
|
||||
DL_REMOTE_GPS_ecef_zd(dl_buffer),
|
||||
DL_REMOTE_GPS_tow(dl_buffer),
|
||||
DL_REMOTE_GPS_course(dl_buffer));
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
#if USE_GPS
|
||||
case DL_GPS_INJECT :
|
||||
#endif // GPS_DATALINK
|
||||
|
||||
case DL_GPS_INJECT : {
|
||||
// Check if the GPS is for this AC
|
||||
if (DL_GPS_INJECT_ac_id(dl_buffer) != AC_ID) { break; }
|
||||
|
||||
@@ -184,8 +185,9 @@ void dl_parse_msg(void)
|
||||
DL_GPS_INJECT_data_length(dl_buffer),
|
||||
DL_GPS_INJECT_data(dl_buffer)
|
||||
);
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
#endif // USE_GPS
|
||||
|
||||
case DL_GUIDED_SETPOINT_NED:
|
||||
if (DL_GUIDED_SETPOINT_NED_ac_id(dl_buffer) != AC_ID) { break; }
|
||||
|
||||
@@ -27,35 +27,19 @@
|
||||
* GPS structure to the values received.
|
||||
*/
|
||||
|
||||
#include "messages.h"
|
||||
#include "generated/airframe.h" // AC_ID
|
||||
#include "generated/flight_plan.h" // reference lla NAV_XXX0
|
||||
#include "subsystems/datalink/downlink.h"
|
||||
|
||||
#include "subsystems/gps.h"
|
||||
#include "subsystems/abi.h"
|
||||
|
||||
// #include <stdio.h>
|
||||
|
||||
#if GPS_USE_DATALINK_SMALL
|
||||
#ifndef GPS_LOCAL_ECEF_ORIGIN_X
|
||||
#error Local x coordinate in ECEF of the remote GPS required
|
||||
#endif
|
||||
|
||||
#ifndef GPS_LOCAL_ECEF_ORIGIN_Y
|
||||
#error Local y coordinate in ECEF of the remote GPS required
|
||||
#endif
|
||||
|
||||
#ifndef GPS_LOCAL_ECEF_ORIGIN_Z
|
||||
#error Local z coordinate in ECEF of the remote GPS required
|
||||
#endif
|
||||
|
||||
struct EcefCoor_i tracking_ecef;
|
||||
|
||||
struct LtpDef_i tracking_ltp;
|
||||
|
||||
struct LtpDef_i ltp_def;
|
||||
struct EnuCoor_i enu_pos, enu_speed;
|
||||
|
||||
struct EcefCoor_i ecef_pos, ecef_vel;
|
||||
|
||||
struct LlaCoor_i lla_pos;
|
||||
#endif
|
||||
|
||||
bool_t gps_available; ///< Is set to TRUE when a new REMOTE_GPS packet is received and parsed
|
||||
|
||||
/** GPS initialization */
|
||||
@@ -66,70 +50,68 @@ void gps_impl_init(void)
|
||||
gps.gspeed = 700; // To enable course setting
|
||||
gps.cacc = 0; // To enable course setting
|
||||
|
||||
#if GPS_USE_DATALINK_SMALL
|
||||
tracking_ecef.x = GPS_LOCAL_ECEF_ORIGIN_X;
|
||||
tracking_ecef.y = GPS_LOCAL_ECEF_ORIGIN_Y;
|
||||
tracking_ecef.z = GPS_LOCAL_ECEF_ORIGIN_Z;
|
||||
struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
|
||||
llh_nav0.lat = NAV_LAT0;
|
||||
llh_nav0.lon = NAV_LON0;
|
||||
/* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
|
||||
llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
|
||||
|
||||
ltp_def_from_ecef_i(&tracking_ltp, &tracking_ecef);
|
||||
#endif
|
||||
struct EcefCoor_i ecef_nav0;
|
||||
ecef_of_lla_i(&ecef_nav0, &llh_nav0);
|
||||
ltp_def_from_ecef_i(<p_def, &ecef_nav0);
|
||||
}
|
||||
|
||||
#ifdef GPS_USE_DATALINK_SMALL
|
||||
// Parse the REMOTE_GPS_SMALL datalink packet
|
||||
void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xy)
|
||||
void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xyz, int16_t heading)
|
||||
{
|
||||
|
||||
// Position in ENU coordinates
|
||||
enu_pos.x = (int32_t)((pos_xyz >> 22) & 0x3FF); // bits 31-22 x position in cm
|
||||
if (enu_pos.x & 0x200) {
|
||||
enu_pos.x |= 0xFFFFFC00; // fix for twos complements
|
||||
enu_pos.x = (int32_t)((pos_xyz >> 21) & 0x7FF); // bits 31-21 x position in cm
|
||||
if (enu_pos.x & 0x400) {
|
||||
enu_pos.x |= 0xFFFFF800; // sign extend for twos complements
|
||||
}
|
||||
enu_pos.y = (int32_t)((pos_xyz >> 12) & 0x3FF); // bits 21-12 y position in cm
|
||||
if (enu_pos.y & 0x200) {
|
||||
enu_pos.y |= 0xFFFFFC00; // fix for twos complements
|
||||
enu_pos.y = (int32_t)((pos_xyz >> 10) & 0x7FF); // bits 20-10 y position in cm
|
||||
if (enu_pos.y & 0x400) {
|
||||
enu_pos.y |= 0xFFFFF800; // sign extend for twos complements
|
||||
}
|
||||
enu_pos.z = (int32_t)(pos_xyz >> 2 & 0x3FF); // bits 11-2 z position in cm
|
||||
// bits 1 and 0 are free
|
||||
|
||||
// printf("ENU Pos: %u (%d, %d, %d)\n", pos_xyz, enu_pos.x, enu_pos.y, enu_pos.z);
|
||||
enu_pos.z = (int32_t)(pos_xyz & 0x3FF); // bits 9-0 z position in cm
|
||||
|
||||
// Convert the ENU coordinates to ECEF
|
||||
ecef_of_enu_point_i(&ecef_pos, &tracking_ltp, &enu_pos);
|
||||
gps.ecef_pos = ecef_pos;
|
||||
ecef_of_enu_point_i(&gps.ecef_pos, <p_def, &enu_pos);
|
||||
SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT);
|
||||
|
||||
lla_of_ecef_i(&lla_pos, &ecef_pos);
|
||||
gps.lla_pos = lla_pos;
|
||||
lla_of_ecef_i(&gps.lla_pos, &gps.ecef_pos);
|
||||
SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
|
||||
|
||||
enu_speed.x = (int32_t)((speed_xy >> 22) & 0x3FF); // bits 31-22 speed x in cm/s
|
||||
if (enu_speed.x & 0x200) {
|
||||
enu_speed.x |= 0xFFFFFC00; // fix for twos complements
|
||||
enu_speed.x = (int32_t)((speed_xyz >> 21) & 0x7FF); // bits 31-21 speed x in cm/s
|
||||
if (enu_speed.x & 0x400) {
|
||||
enu_speed.x |= 0xFFFFF800; // sign extend for twos complements
|
||||
}
|
||||
enu_speed.y = (int32_t)((speed_xy >> 12) & 0x3FF); // bits 21-12 speed y in cm/s
|
||||
if (enu_speed.y & 0x200) {
|
||||
enu_speed.y |= 0xFFFFFC00; // fix for twos complements
|
||||
enu_speed.y = (int32_t)((speed_xyz >> 10) & 0x7FF); // bits 20-10 speed y in cm/s
|
||||
if (enu_speed.y & 0x400) {
|
||||
enu_speed.y |= 0xFFFFF800; // sign extend for twos complements
|
||||
}
|
||||
enu_speed.z = (int32_t)((speed_xyz) & 0x3FF); // bits 9-0 speed z in cm/s
|
||||
if (enu_speed.z & 0x200) {
|
||||
enu_speed.z |= 0xFFFFFC00; // sign extend for twos complements
|
||||
}
|
||||
enu_speed.z = 0;
|
||||
|
||||
// printf("ENU Speed: %u (%d, %d, %d)\n", speed_xy, enu_speed.x, enu_speed.y, enu_speed.z);
|
||||
|
||||
ecef_of_enu_vect_i(&gps.ecef_vel , &tracking_ltp , &enu_speed);
|
||||
ecef_of_enu_vect_i(&gps.ecef_vel , <p_def , &enu_speed);
|
||||
SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
|
||||
|
||||
gps.hmsl = tracking_ltp.hmsl + enu_pos.z * 10; // TODO: try to compensate for the loss in accuracy
|
||||
// todo check correct
|
||||
gps.ned_vel.x = enu_speed.x;
|
||||
gps.ned_vel.y = enu_speed.y;
|
||||
gps.ned_vel.z = -enu_speed.z;
|
||||
SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
|
||||
|
||||
gps.hmsl = ltp_def.hmsl + enu_pos.z * 10;
|
||||
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
|
||||
|
||||
gps.course = (int32_t)((speed_xy >> 2) & 0x3FF); // bits 11-2 heading in rad*1e2
|
||||
if (gps.course & 0x200) {
|
||||
gps.course |= 0xFFFFFC00; // fix for twos complements
|
||||
}
|
||||
// printf("Heading: %d\n", gps.course);
|
||||
gps.course *= 1e5;
|
||||
gps.course = ((int32_t)heading) * 1e3;
|
||||
SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
|
||||
|
||||
gps.num_sv = num_sv;
|
||||
gps.tow = 0; // set time-of-weak to 0
|
||||
gps.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); // todo check
|
||||
gps.fix = GPS_FIX_3D; // set 3D fix to true
|
||||
gps_available = TRUE; // set GPS available to true
|
||||
|
||||
@@ -143,7 +125,6 @@ void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_x
|
||||
}
|
||||
AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps);
|
||||
}
|
||||
#endif
|
||||
|
||||
/** 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,
|
||||
@@ -168,10 +149,20 @@ void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t e
|
||||
gps.ecef_vel.z = ecef_zd;
|
||||
SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
|
||||
|
||||
struct LtpDef_i ref_ltp;
|
||||
ltp_def_from_ecef_i(&ref_ltp, &gps.ecef_pos);
|
||||
ned_of_ecef_vect_i(&gps.ned_vel, &ref_ltp, &gps.ecef_vel);
|
||||
SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
|
||||
|
||||
gps.course = course;
|
||||
SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
|
||||
|
||||
gps.num_sv = numsv;
|
||||
gps.tow = tow;
|
||||
if (tow == 0) {
|
||||
gps.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); //tow;
|
||||
} else {
|
||||
gps.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); //tow;
|
||||
}
|
||||
gps.fix = GPS_FIX_3D;
|
||||
gps_available = TRUE;
|
||||
|
||||
|
||||
@@ -35,9 +35,9 @@
|
||||
#define GPS_NB_CHANNELS 0
|
||||
|
||||
extern bool_t gps_available;
|
||||
#ifdef GPS_USE_DATALINK_SMALL
|
||||
void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xy);
|
||||
#endif
|
||||
|
||||
extern void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xyz, int16_t heading);
|
||||
|
||||
extern 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,
|
||||
|
||||
@@ -98,6 +98,10 @@ PRINT_CONFIG_MSG("INS_SONAR_UPDATE_ON_AGL defaulting to FALSE")
|
||||
#define INS_VFF_R_GPS 2.0
|
||||
#endif
|
||||
|
||||
#ifndef INS_VFF_VZ_R_GPS
|
||||
#define INS_VFF_VZ_R_GPS 2.0
|
||||
#endif
|
||||
|
||||
/** maximum number of propagation steps without any updates in between */
|
||||
#ifndef INS_MAX_PROPAGATION_STEPS
|
||||
#define INS_MAX_PROPAGATION_STEPS 200
|
||||
@@ -374,6 +378,7 @@ void ins_int_update_gps(struct GpsState *gps_s)
|
||||
|
||||
#if INS_USE_GPS_ALT
|
||||
vff_update_z_conf(((float)gps_pos_cm_ned.z) / 100.0, INS_VFF_R_GPS);
|
||||
vff_update_vz_conf(((float)gps_speed_cm_s_ned.z) / 100.0, INS_VFF_VZ_R_GPS);
|
||||
#endif
|
||||
|
||||
#if USE_HFF
|
||||
|
||||
@@ -126,7 +126,8 @@ double tracking_offset_angle; ///< The offset from the tracking system to
|
||||
float natnet_latency;
|
||||
|
||||
/** Parse the packet from NatNet */
|
||||
void natnet_parse(unsigned char *in) {
|
||||
void natnet_parse(unsigned char *in)
|
||||
{
|
||||
int i, j, k;
|
||||
|
||||
// Create a pointer to go trough the packet
|
||||
@@ -143,8 +144,7 @@ void natnet_parse(unsigned char *in) {
|
||||
memcpy(&nBytes, ptr, 2); ptr += 2;
|
||||
printf_natnet("Byte count : %d\n", nBytes);
|
||||
|
||||
if(MessageID == NAT_FRAMEOFDATA) // FRAME OF MOCAP DATA packet
|
||||
{
|
||||
if (MessageID == NAT_FRAMEOFDATA) { // FRAME OF MOCAP DATA packet
|
||||
// Frame number
|
||||
int frameNumber = 0; memcpy(&frameNumber, ptr, 4); ptr += 4;
|
||||
printf_natnet("Frame # : %d\n", frameNumber);
|
||||
@@ -154,8 +154,7 @@ void natnet_parse(unsigned char *in) {
|
||||
int nMarkerSets = 0; memcpy(&nMarkerSets, ptr, 4); ptr += 4;
|
||||
printf_natnet("Marker Set Count : %d\n", nMarkerSets);
|
||||
|
||||
for (i=0; i < nMarkerSets; i++)
|
||||
{
|
||||
for (i = 0; i < nMarkerSets; i++) {
|
||||
// Markerset name
|
||||
char szName[256];
|
||||
strcpy(szName, ptr);
|
||||
@@ -167,8 +166,7 @@ void natnet_parse(unsigned char *in) {
|
||||
int nMarkers = 0; memcpy(&nMarkers, ptr, 4); ptr += 4;
|
||||
printf_natnet("Marker Count : %d\n", nMarkers);
|
||||
|
||||
for(j=0; j < nMarkers; j++)
|
||||
{
|
||||
for (j = 0; j < nMarkers; j++) {
|
||||
float x = 0; memcpy(&x, ptr, 4); ptr += 4;
|
||||
float y = 0; memcpy(&y, ptr, 4); ptr += 4;
|
||||
float z = 0; memcpy(&z, ptr, 4); ptr += 4;
|
||||
@@ -179,8 +177,7 @@ void natnet_parse(unsigned char *in) {
|
||||
// Unidentified markers
|
||||
int nOtherMarkers = 0; memcpy(&nOtherMarkers, ptr, 4); ptr += 4;
|
||||
printf_natnet("Unidentified Marker Count : %d\n", nOtherMarkers);
|
||||
for(j=0; j < nOtherMarkers; j++)
|
||||
{
|
||||
for (j = 0; j < nOtherMarkers; j++) {
|
||||
float x = 0.0f; memcpy(&x, ptr, 4); ptr += 4;
|
||||
float y = 0.0f; memcpy(&y, ptr, 4); ptr += 4;
|
||||
float z = 0.0f; memcpy(&z, ptr, 4); ptr += 4;
|
||||
@@ -195,12 +192,13 @@ void natnet_parse(unsigned char *in) {
|
||||
|
||||
// Check if there ie enough space for the rigid bodies
|
||||
if (nRigidBodies > MAX_RIGIDBODIES) {
|
||||
fprintf(stderr, "Could not sample all the rigid bodies because the amount of rigid bodies is bigger then %d (MAX_RIGIDBODIES).\r\n", MAX_RIGIDBODIES);
|
||||
fprintf(stderr,
|
||||
"Could not sample all the rigid bodies because the amount of rigid bodies is bigger then %d (MAX_RIGIDBODIES).\r\n",
|
||||
MAX_RIGIDBODIES);
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
for (j=0; j < nRigidBodies; j++)
|
||||
{
|
||||
for (j = 0; j < nRigidBodies; j++) {
|
||||
// rigid body pos/ori
|
||||
struct RigidBody old_rigid;
|
||||
memcpy(&old_rigid, &rigidBodies[j], sizeof(struct RigidBody));
|
||||
@@ -215,12 +213,14 @@ void natnet_parse(unsigned char *in) {
|
||||
memcpy(&rigidBodies[j].qw, ptr, 4); ptr += 4; //qw --> QW
|
||||
printf_natnet("ID (%d) : %d\n", j, rigidBodies[j].id);
|
||||
printf_natnet("pos: [%3.2f,%3.2f,%3.2f]\n", rigidBodies[j].x, rigidBodies[j].y, rigidBodies[j].z);
|
||||
printf_natnet("ori: [%3.2f,%3.2f,%3.2f,%3.2f]\n", rigidBodies[j].qx,rigidBodies[j].qy,rigidBodies[j].qz,rigidBodies[j].qw);
|
||||
printf_natnet("ori: [%3.2f,%3.2f,%3.2f,%3.2f]\n", rigidBodies[j].qx, rigidBodies[j].qy, rigidBodies[j].qz,
|
||||
rigidBodies[j].qw);
|
||||
|
||||
// Differentiate the position to get the speed (TODO: crossreference with labeled markers for occlussion)
|
||||
rigidBodies[j].totalVelocitySamples++;
|
||||
if (old_rigid.x != rigidBodies[j].x || old_rigid.y != rigidBodies[j].y || old_rigid.z != rigidBodies[j].z
|
||||
|| old_rigid.qx != rigidBodies[j].qx || old_rigid.qy != rigidBodies[j].qy || old_rigid.qz != rigidBodies[j].qz || old_rigid.qw != rigidBodies[j].qw) {
|
||||
|| old_rigid.qx != rigidBodies[j].qx || old_rigid.qy != rigidBodies[j].qy || old_rigid.qz != rigidBodies[j].qz
|
||||
|| old_rigid.qw != rigidBodies[j].qw) {
|
||||
|
||||
if (old_rigid.posSampled) {
|
||||
rigidBodies[j].vel_x += (rigidBodies[j].x - old_rigid.x);
|
||||
@@ -231,9 +231,9 @@ void natnet_parse(unsigned char *in) {
|
||||
|
||||
rigidBodies[j].nSamples++;
|
||||
rigidBodies[j].posSampled = TRUE;
|
||||
}
|
||||
else
|
||||
} else {
|
||||
rigidBodies[j].posSampled = FALSE;
|
||||
}
|
||||
|
||||
// When marker id changed, reset the velocity
|
||||
if (old_rigid.id != rigidBodies[j].id) {
|
||||
@@ -254,8 +254,7 @@ void natnet_parse(unsigned char *in) {
|
||||
memcpy(markerData, ptr, nBytes);
|
||||
ptr += nBytes;
|
||||
|
||||
if(natnet_major >= 2)
|
||||
{
|
||||
if (natnet_major >= 2) {
|
||||
// Associated marker IDs
|
||||
nBytes = rigidBodies[j].nMarkers * sizeof(int);
|
||||
int *markerIDs = (int *)malloc(nBytes);
|
||||
@@ -268,37 +267,36 @@ void natnet_parse(unsigned char *in) {
|
||||
memcpy(markerSizes, ptr, nBytes);
|
||||
ptr += nBytes;
|
||||
|
||||
for(k=0; k < rigidBodies[j].nMarkers; k++)
|
||||
{
|
||||
printf_natnet("\tMarker %d: id=%d\tsize=%3.1f\tpos=[%3.2f,%3.2f,%3.2f]\n", k, markerIDs[k], markerSizes[k], markerData[k*3], markerData[k*3+1],markerData[k*3+2]);
|
||||
for (k = 0; k < rigidBodies[j].nMarkers; k++) {
|
||||
printf_natnet("\tMarker %d: id=%d\tsize=%3.1f\tpos=[%3.2f,%3.2f,%3.2f]\n", k, markerIDs[k], markerSizes[k],
|
||||
markerData[k * 3], markerData[k * 3 + 1], markerData[k * 3 + 2]);
|
||||
}
|
||||
|
||||
if(markerIDs)
|
||||
if (markerIDs) {
|
||||
free(markerIDs);
|
||||
if(markerSizes)
|
||||
}
|
||||
if (markerSizes) {
|
||||
free(markerSizes);
|
||||
}
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
for(k=0; k < rigidBodies[j].nMarkers; k++)
|
||||
{
|
||||
printf_natnet("\tMarker %d: pos = [%3.2f,%3.2f,%3.2f]\n", k, markerData[k*3], markerData[k*3+1],markerData[k*3+2]);
|
||||
} else {
|
||||
for (k = 0; k < rigidBodies[j].nMarkers; k++) {
|
||||
printf_natnet("\tMarker %d: pos = [%3.2f,%3.2f,%3.2f]\n", k, markerData[k * 3], markerData[k * 3 + 1],
|
||||
markerData[k * 3 + 2]);
|
||||
}
|
||||
}
|
||||
if(markerData)
|
||||
if (markerData) {
|
||||
free(markerData);
|
||||
}
|
||||
|
||||
if(natnet_major >= 2)
|
||||
{
|
||||
if (natnet_major >= 2) {
|
||||
// Mean marker error
|
||||
memcpy(&rigidBodies[j].error, ptr, 4); ptr += 4;
|
||||
printf_natnet("Mean marker error: %3.8f\n", rigidBodies[j].error);
|
||||
}
|
||||
|
||||
// 2.6 and later
|
||||
if( ((natnet_major == 2)&&(natnet_minor >= 6)) || (natnet_major > 2) || (natnet_major == 0) )
|
||||
{
|
||||
if (((natnet_major == 2) && (natnet_minor >= 6)) || (natnet_major > 2) || (natnet_major == 0)) {
|
||||
// params
|
||||
short params = 0; memcpy(¶ms, ptr, 2); ptr += 2;
|
||||
// bool bTrackingValid = params & 0x01; // 0x01 : rigid body was successfully tracked in this frame
|
||||
@@ -307,13 +305,11 @@ void natnet_parse(unsigned char *in) {
|
||||
|
||||
// ========== SKELETONS ==========
|
||||
// Skeletons (version 2.1 and later)
|
||||
if(((natnet_major == 2) && (natnet_minor>0)) || (natnet_major>2))
|
||||
{
|
||||
if (((natnet_major == 2) && (natnet_minor > 0)) || (natnet_major > 2)) {
|
||||
int nSkeletons = 0;
|
||||
memcpy(&nSkeletons, ptr, 4); ptr += 4;
|
||||
printf_natnet("Skeleton Count : %d\n", nSkeletons);
|
||||
for (j=0; j < nSkeletons; j++)
|
||||
{
|
||||
for (j = 0; j < nSkeletons; j++) {
|
||||
// Skeleton id
|
||||
int skeletonID = 0;
|
||||
memcpy(&skeletonID, ptr, 4); ptr += 4;
|
||||
@@ -321,8 +317,7 @@ void natnet_parse(unsigned char *in) {
|
||||
int nRigidBodies = 0;
|
||||
memcpy(&nRigidBodies, ptr, 4); ptr += 4;
|
||||
printf_natnet("Rigid Body Count : %d\n", nRigidBodies);
|
||||
for (j=0; j < nRigidBodies; j++)
|
||||
{
|
||||
for (j = 0; j < nRigidBodies; j++) {
|
||||
// Rigid body pos/ori
|
||||
int ID = 0; memcpy(&ID, ptr, 4); ptr += 4;
|
||||
float x = 0.0f; memcpy(&x, ptr, 4); ptr += 4;
|
||||
@@ -356,9 +351,9 @@ void natnet_parse(unsigned char *in) {
|
||||
memcpy(markerSizes, ptr, nBytes);
|
||||
ptr += nBytes;
|
||||
|
||||
for(k=0; k < nRigidMarkers; k++)
|
||||
{
|
||||
printf_natnet("\tMarker %d: id=%d\tsize=%3.1f\tpos=[%3.2f,%3.2f,%3.2f]\n", k, markerIDs[k], markerSizes[k], markerData[k*3], markerData[k*3+1],markerData[k*3+2]);
|
||||
for (k = 0; k < nRigidMarkers; k++) {
|
||||
printf_natnet("\tMarker %d: id=%d\tsize=%3.1f\tpos=[%3.2f,%3.2f,%3.2f]\n", k, markerIDs[k], markerSizes[k],
|
||||
markerData[k * 3], markerData[k * 3 + 1], markerData[k * 3 + 2]);
|
||||
}
|
||||
|
||||
// Mean marker error
|
||||
@@ -366,25 +361,26 @@ void natnet_parse(unsigned char *in) {
|
||||
printf_natnet("Mean marker error: %3.2f\n", fError);
|
||||
|
||||
// Release resources
|
||||
if(markerIDs)
|
||||
if (markerIDs) {
|
||||
free(markerIDs);
|
||||
if(markerSizes)
|
||||
}
|
||||
if (markerSizes) {
|
||||
free(markerSizes);
|
||||
if(markerData)
|
||||
}
|
||||
if (markerData) {
|
||||
free(markerData);
|
||||
}
|
||||
} // next rigid body
|
||||
} // next skeleton
|
||||
}
|
||||
|
||||
// ========== LABELED MARKERS ==========
|
||||
// Labeled markers (version 2.3 and later)
|
||||
if( ((natnet_major == 2)&&(natnet_minor>=3)) || (natnet_major>2))
|
||||
{
|
||||
if (((natnet_major == 2) && (natnet_minor >= 3)) || (natnet_major > 2)) {
|
||||
int nLabeledMarkers = 0;
|
||||
memcpy(&nLabeledMarkers, ptr, 4); ptr += 4;
|
||||
printf_natnet("Labeled Marker Count : %d\n", nLabeledMarkers);
|
||||
for (j=0; j < nLabeledMarkers; j++)
|
||||
{
|
||||
for (j = 0; j < nLabeledMarkers; j++) {
|
||||
int ID = 0; memcpy(&ID, ptr, 4); ptr += 4;
|
||||
float x = 0.0f; memcpy(&x, ptr, 4); ptr += 4;
|
||||
float y = 0.0f; memcpy(&y, ptr, 4); ptr += 4;
|
||||
@@ -409,28 +405,30 @@ void natnet_parse(unsigned char *in) {
|
||||
// End of data tag
|
||||
int eod = 0; memcpy(&eod, ptr, 4); ptr += 4;
|
||||
printf_natnet("End Packet\n-------------\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
} else {
|
||||
printf("Error: Unrecognized packet type from Optitrack NatNet.\n");
|
||||
}
|
||||
}
|
||||
|
||||
/** The transmitter periodic function */
|
||||
gboolean timeout_transmit_callback(gpointer data) {
|
||||
gboolean timeout_transmit_callback(gpointer data)
|
||||
{
|
||||
int i;
|
||||
|
||||
// Loop trough all the available rigidbodies (TODO: optimize)
|
||||
for (i = 0; i < MAX_RIGIDBODIES; i++) {
|
||||
// Check if ID's are correct
|
||||
if (rigidBodies[i].id >= MAX_RIGIDBODIES) {
|
||||
fprintf(stderr, "Could not parse rigid body %d from NatNet, because ID is higher then or equal to %d (MAX_RIGIDBODIES-1).\r\n", rigidBodies[i].id, MAX_RIGIDBODIES-1);
|
||||
fprintf(stderr,
|
||||
"Could not parse rigid body %d from NatNet, because ID is higher then or equal to %d (MAX_RIGIDBODIES-1).\r\n",
|
||||
rigidBodies[i].id, MAX_RIGIDBODIES - 1);
|
||||
exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
// Check if we want to transmit (follow) this rigid
|
||||
if(aircrafts[rigidBodies[i].id].ac_id == 0)
|
||||
if (aircrafts[rigidBodies[i].id].ac_id == 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// When we don track anymore and timeout or start tracking
|
||||
if (rigidBodies[i].nSamples < 1
|
||||
@@ -439,15 +437,15 @@ gboolean timeout_transmit_callback(gpointer data) {
|
||||
aircrafts[rigidBodies[i].id].connected = FALSE;
|
||||
fprintf(stderr, "#error Lost tracking rigid id %d, aircraft id %d.\n",
|
||||
rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id);
|
||||
}
|
||||
else if(rigidBodies[i].nSamples > 0 && !aircrafts[rigidBodies[i].id].connected) {
|
||||
} else if (rigidBodies[i].nSamples > 0 && !aircrafts[rigidBodies[i].id].connected) {
|
||||
fprintf(stderr, "#pragma message: Now tracking rigid id %d, aircraft id %d.\n",
|
||||
rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id);
|
||||
}
|
||||
|
||||
// Check if we still track the rigid
|
||||
if(rigidBodies[i].nSamples < 1)
|
||||
if (rigidBodies[i].nSamples < 1) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Update the last tracked
|
||||
aircrafts[rigidBodies[i].id].connected = TRUE;
|
||||
@@ -496,10 +494,12 @@ gboolean timeout_transmit_callback(gpointer data) {
|
||||
double_eulers_of_quat(&orient_eulers, &orient);
|
||||
|
||||
// Calculate the heading by adding the Natnet offset angle and normalizing it
|
||||
double heading = -orient_eulers.psi+90.0/57.6 - tracking_offset_angle; //the optitrack axes are 90 degrees rotated wrt ENU
|
||||
double heading = -orient_eulers.psi + 90.0 / 57.6 -
|
||||
tracking_offset_angle; //the optitrack axes are 90 degrees rotated wrt ENU
|
||||
NormRadAngle(heading);
|
||||
|
||||
printf_debug("[%d -> %d]Samples: %d\t%d\t\tTiming: %3.3f latency\n", rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id
|
||||
printf_debug("[%d -> %d]Samples: %d\t%d\t\tTiming: %3.3f latency\n", rigidBodies[i].id,
|
||||
aircrafts[rigidBodies[i].id].ac_id
|
||||
, rigidBodies[i].nSamples, rigidBodies[i].nVelocitySamples, natnet_latency);
|
||||
printf_debug(" Heading: %f\t\tPosition: %f\t%f\t%f\t\tVelocity: %f\t%f\t%f\n", DegOfRad(heading),
|
||||
rigidBodies[i].x, rigidBodies[i].y, rigidBodies[i].z,
|
||||
@@ -507,33 +507,72 @@ gboolean timeout_transmit_callback(gpointer data) {
|
||||
|
||||
// Transmit the REMOTE_GPS packet on the ivy bus (either small or big)
|
||||
if (small_packets) {
|
||||
/* The GPS messages are most likely too large to be send over either the datalink
|
||||
* The local position is an int32 and the 10 LSBs of the x and y axis are compressed into
|
||||
/* The local position is an int32 and the 11 LSBs of the (signed) x and y axis are compressed into
|
||||
* a single integer. The z axis is considered unsigned and only the latter 10 LSBs are
|
||||
* used.
|
||||
*/
|
||||
uint32_t pos_xyz = (((uint32_t)(pos.x*100.0)) & 0x3FF) << 22; // bits 31-22 x position in cm
|
||||
pos_xyz |= (((uint32_t)(pos.y*100.0)) & 0x3FF) << 12; // bits 21-12 y position in cm
|
||||
pos_xyz |= (((uint32_t)(pos.z*100.0)) & 0x3FF) << 2; // bits 11-2 z position in cm
|
||||
// bits 1 and 0 are free
|
||||
|
||||
uint32_t pos_xyz;
|
||||
// check if position within limits
|
||||
if (fabs(pos.x * 100) < pow(2, 10)) {
|
||||
pos_xyz = (((uint32_t)(pos.x * 100.0)) & 0x7FF) << 21; // bits 31-21 x position in cm
|
||||
} else {
|
||||
fprintf(stderr, "Warning!! X position out of maximum range of small message (±%.2fm): %.2f", pow(2, 10) / 100, pos.x);
|
||||
pos_xyz = (((uint32_t)(pow(2, 10) * pos.x / fabs(pos.x))) & 0x7FF) << 21; // bits 31-21 x position in cm
|
||||
}
|
||||
|
||||
if (fabs(pos.y * 100) < pow(2, 10)) {
|
||||
pos_xyz |= (((uint32_t)(pos.y * 100.0)) & 0x7FF) << 10; // bits 20-10 y position in cm
|
||||
} else {
|
||||
fprintf(stderr, "Warning!! Y position out of maximum range of small message (±%.2fm): %.2f", pow(2, 10) / 100, pos.y);
|
||||
pos_xyz |= (((uint32_t)(pow(2, 10) * pos.y / fabs(pos.y))) & 0x7FF) << 10; // bits 20-10 y position in cm
|
||||
}
|
||||
|
||||
if (pos.z * 100 < pow(2, 10)) {
|
||||
pos_xyz |= (((uint32_t)(pos.z * 100.0)) & 0x3FF); // bits 9-0 z position in cm
|
||||
} else {
|
||||
fprintf(stderr, "Warning!! Z position out of maximum range of small message (%.2fm): %.2f", pow(2, 10) / 100, pos.z);
|
||||
pos_xyz |= (((uint32_t)(pow(2, 10))) & 0x3FF); // bits 9-0 z position in cm
|
||||
}
|
||||
// printf("ENU Pos: %u (%.2f, %.2f, %.2f)\n", pos_xyz, pos.x, pos.y, pos.z);
|
||||
|
||||
uint32_t speed_xy = (((uint32_t)(speed.x*100.0)) & 0x3FF) << 22; // bits 31-22 speed x in cm/s
|
||||
speed_xy |= (((uint32_t)(speed.y*100.0)) & 0x3FF) << 12; // bits 21-12 speed y in cm/s
|
||||
speed_xy |= (((uint32_t)(heading*100.0)) & 0x3FF) << 2; // bits 11-2 heading in rad*1e2 (The heading is already subsampled)
|
||||
// bits 1 and 0 are free
|
||||
/* The speed is an int32 and the 11 LSBs of the x and y axis and 10 LSBs of z (all signed) are compressed into
|
||||
* a single integer.
|
||||
*/
|
||||
uint32_t speed_xyz;
|
||||
// check if speed within limits
|
||||
if (fabs(speed.x * 100) < pow(2, 10)) {
|
||||
speed_xyz = (((uint32_t)(speed.x * 100.0)) & 0x7FF) << 21; // bits 31-21 speed x in cm/s
|
||||
} else {
|
||||
fprintf(stderr, "Warning!! X Speed out of maximum range of small message (±%.2fm/s): %.2f", pow(2, 10) / 100, speed.x);
|
||||
speed_xyz = (((uint32_t)(pow(2, 10) * speed.x / fabs(speed.x))) & 0x7FF) << 21; // bits 31-21 speed x in cm/s
|
||||
}
|
||||
|
||||
if (fabs(speed.y * 100) < pow(2, 10)) {
|
||||
speed_xyz |= (((uint32_t)(speed.y * 100.0)) & 0x7FF) << 10; // bits 20-10 speed y in cm/s
|
||||
} else {
|
||||
fprintf(stderr, "Warning!! Y Speed out of maximum range of small message (±%.2fm/s): %.2f", pow(2, 10) / 100, speed.y);
|
||||
speed_xyz |= (((uint32_t)(pow(2, 10) * speed.y / fabs(speed.y))) & 0x7FF) << 10; // bits 20-10 speed y in cm/s
|
||||
}
|
||||
|
||||
if (fabs(pos.z * 100) < pow(2, 9)) {
|
||||
speed_xyz |= (((uint32_t)(speed.z * 100.0)) & 0x3FF); // bits 9-0 speed z in cm/s
|
||||
} else {
|
||||
fprintf(stderr, "Warning!! Z Speed out of maximum range of small message (±%.2fm/s): %.2f", pow(2, 9) / 100, speed.z);
|
||||
speed_xyz |= (((uint32_t)(pow(2, 9) * speed.z / fabs(speed.z))) & 0x3FF); // bits 9-0 speed z in cm/s
|
||||
}
|
||||
|
||||
// printf("ENU Vel: %u (%.2f, %.2f, 0.0)\n", speed_xy, speed.x, speed.y);
|
||||
|
||||
// printf("Heading: %.2f\n", heading);
|
||||
|
||||
IvySendMsg("0 REMOTE_GPS_SMALL %d %d %d %d", aircrafts[rigidBodies[i].id].ac_id, // uint8 rigid body ID (1 byte)
|
||||
(uint8_t)rigidBodies[i].nMarkers, // status (1 byte)
|
||||
IvySendMsg("0 REMOTE_GPS_SMALL %d %d %d %d %d", aircrafts[rigidBodies[i].id].ac_id, // uint8 rigid body ID (1 byte)
|
||||
(uint8_t)rigidBodies[i].nMarkers, // uint8 Number of markers (sv_num) (1 byte)
|
||||
pos_xyz, // uint32 ENU X, Y and Z in CM (4 bytes)
|
||||
speed_xy); //uint32 ENU velocity X, Y in cm/s and heading in rad*1e2 (4 bytes)
|
||||
}
|
||||
else {
|
||||
speed_xyz, // uint32 ENU velocity X, Y, Z in cm/s (4 bytes)
|
||||
(int16_t)(heading * 10000)); // int6_t heading in rad*1e4 (2 bytes)
|
||||
|
||||
} else {
|
||||
IvySendMsg("0 REMOTE_GPS %d %d %d %d %d %d %d %d %d %d %d %d %d %d", aircrafts[rigidBodies[i].id].ac_id,
|
||||
rigidBodies[i].nMarkers, //uint8 Number of markers (sv_num)
|
||||
(int)(ecef_pos.x * 100.0), //int32 ECEF X in CM
|
||||
@@ -567,7 +606,8 @@ gboolean timeout_transmit_callback(gpointer data) {
|
||||
}
|
||||
|
||||
/** The NatNet sampler periodic function */
|
||||
static gboolean sample_data(GIOChannel *chan, GIOCondition cond, gpointer data) {
|
||||
static gboolean sample_data(GIOChannel *chan, GIOCondition cond, gpointer data)
|
||||
{
|
||||
static unsigned char buffer_data[MAX_PACKETSIZE];
|
||||
static int bytes_data = 0;
|
||||
|
||||
@@ -585,7 +625,8 @@ static gboolean sample_data(GIOChannel *chan, GIOCondition cond, gpointer data)
|
||||
|
||||
|
||||
/** Print the program help */
|
||||
void print_help(char* filename) {
|
||||
void print_help(char *filename)
|
||||
{
|
||||
static const char *usage =
|
||||
"Usage: %s [options]\n"
|
||||
" Options :\n"
|
||||
@@ -613,7 +654,8 @@ void print_help(char* filename) {
|
||||
}
|
||||
|
||||
/** Check the amount of arguments */
|
||||
void check_argcount(int argc, char** argv, int i, int expected) {
|
||||
void check_argcount(int argc, char **argv, int i, int expected)
|
||||
{
|
||||
if (i + expected >= argc) {
|
||||
fprintf(stderr, "Option %s expected %d arguments\r\n\r\n", argv[i], expected);
|
||||
print_help(argv[0]);
|
||||
@@ -622,7 +664,8 @@ void check_argcount(int argc, char** argv, int i, int expected) {
|
||||
}
|
||||
|
||||
/** Parse the options from the commandline */
|
||||
static void parse_options(int argc, char** argv) {
|
||||
static void parse_options(int argc, char **argv)
|
||||
{
|
||||
int i, count_ac = 0;
|
||||
for (i = 1; i < argc; ++i) {
|
||||
|
||||
@@ -768,10 +811,12 @@ int main(int argc, char** argv)
|
||||
|
||||
// Parse the options from cmdline
|
||||
parse_options(argc, argv);
|
||||
printf_debug("Tracking system Latitude: %f Longitude: %f Offset to North: %f degrees\n", DegOfRad(tracking_ltp.lla.lat), DegOfRad(tracking_ltp.lla.lon), DegOfRad(tracking_offset_angle));
|
||||
printf_debug("Tracking system Latitude: %f Longitude: %f Offset to North: %f degrees\n", DegOfRad(tracking_ltp.lla.lat),
|
||||
DegOfRad(tracking_ltp.lla.lon), DegOfRad(tracking_offset_angle));
|
||||
|
||||
// Create the network connections
|
||||
printf_debug("Starting NatNet listening (multicast address: %s, data port: %d, version: %d.%d)\n", natnet_multicast_addr, natnet_data_port, natnet_major, natnet_minor);
|
||||
printf_debug("Starting NatNet listening (multicast address: %s, data port: %d, version: %d.%d)\n",
|
||||
natnet_multicast_addr, natnet_data_port, natnet_major, natnet_minor);
|
||||
udp_socket_create(&natnet_data, "", -1, natnet_data_port, 0); // Only receiving
|
||||
udp_socket_subscribe_multicast(&natnet_data, natnet_multicast_addr);
|
||||
udp_socket_set_recvbuf(&natnet_data, 0x100000); // 1MB
|
||||
|
||||
Reference in New Issue
Block a user