END_TEST
START_TEST(test_wgsecef2llh2ecef)
{
double llh[3];
double ecef[3];
wgsecef2llh(ecefs[_i], llh);
wgsllh2ecef(llh, ecef);
for (int n=0; n<3; n++) {
fail_unless(!isnan(llh[n]), "NaN in LLH output from conversion.");
fail_unless(!isnan(ecef[n]), "NaN in ECEF output from conversion.");
}
for (int n=0; n<3; n++) {
double err = fabs(ecef[n] - ecefs[_i][n]);
fail_unless(err < MAX_DIST_ERROR_M,
"Converting WGS84 ECEF to LLH and back again does not return the "
"original values.\n"
"Initial ECEF: %f, %f, %f\n"
"LLH: %f, %f, %f\n"
"Final ECEF: %f, %f, %f\n"
"X error (mm): %g\nY error (mm): %g\nZ error (mm): %g",
ecefs[_i][0], ecefs[_i][1], ecefs[_i][2],
R2D*llh[0], R2D*llh[1], llh[2],
ecef[0], ecef[1], ecef[2],
(ecef[0] - ecefs[_i][0])*1e3,
(ecef[1] - ecefs[_i][1])*1e3,
(ecef[2] - ecefs[_i][2])*1e3
);
}
}
void initialize_datum(double datum_ecef[3],
const sensor_msgs::NavSatFixConstPtr fix_ptr,
const ros::Publisher& pub_datum)
{
ros::NodeHandle n("~");
sensor_msgs::NavSatFix datum_msg(*fix_ptr);
// Local ENU coordinates are with respect to a plane which is
// perpendicular to a particular lat/lon. This logic decides
// whether to use a specific passed-in point (typical for
// repeated tests in a locality) or just an arbitrary starting
// point (more ad-hoc type scenarios).
if (n.hasParam("datum_latitude") &&
n.hasParam("datum_longitude") &&
n.hasParam("datum_altitude")) {
n.getParam("datum_latitude", datum_msg.latitude);
n.getParam("datum_longitude", datum_msg.longitude);
n.getParam("datum_altitude", datum_msg.altitude);
ROS_INFO("Using datum provided by node parameters.");
} else {
ROS_INFO("Using initial position fix as datum.");
}
pub_datum.publish(datum_msg);
// The datum point is stored as an ECEF, for mathematical reasons.
// We convert it here, using the appropriate function from
// libswiftnav.
double llh[3] = { datum_msg.latitude * TO_RADIANS,
datum_msg.longitude * TO_RADIANS,
datum_msg.altitude };
wgsllh2ecef(llh, datum_ecef);
}
END_TEST
START_TEST(test_wgsllh2ecef2llh)
{
double ecef[3];
double llh[3];
wgsllh2ecef(llhs[_i], ecef);
wgsecef2llh(ecef, llh);
for (int n=0; n<3; n++) {
fail_unless(!isnan(llh[n]), "NaN in LLH output from conversion.");
fail_unless(!isnan(ecef[n]), "NaN in ECEF output from conversion.");
}
double lat_err = fabs(llh[0] - llhs[_i][0]);
double lon_err = fabs(llh[1] - llhs[_i][1]);
double hgt_err = fabs(llh[2] - llhs[_i][2]);
fail_unless((lat_err < MAX_ANGLE_ERROR_RAD) &&
(lon_err < MAX_ANGLE_ERROR_RAD) &&
(hgt_err < MAX_DIST_ERROR_M),
"Converting WGS84 LLH to ECEF and back again does not return the "
"original values.\n"
"Initial LLH: %f, %f, %f\n"
"ECEF: %f, %f, %f\n"
"Final LLH: %f, %f, %f\n"
"Lat error (arc sec): %g\nLon error (arc sec): %g\nH error (mm): %g",
R2D*llhs[_i][0], R2D*llhs[_i][1], llhs[_i][2],
ecef[0], ecef[1], ecef[2],
R2D*llh[0], R2D*llh[1], llh[2],
(llh[0] - llhs[_i][0])*(R2D*3600),
(llh[1] - llhs[_i][1])*(R2D*3600),
(llh[2] - llhs[_i][2])*1e3
);
}
static void system_monitor_thread(void *arg)
{
(void)arg;
chRegSetThreadName("system monitor");
systime_t time = chVTGetSystemTime();
bool ant_status = 0;
while (TRUE) {
if (ant_status != frontend_ant_status()) {
ant_status = frontend_ant_status();
if (ant_status && frontend_ant_setting() == AUTO) {
log_info("Now using external antenna.");
}
else if (frontend_ant_setting() == AUTO) {
log_info("Now using patch antenna.");
}
}
u32 status_flags = ant_status << 31 | SBP_MAJOR_VERSION << 16 | SBP_MINOR_VERSION << 8;
sbp_send_msg(SBP_MSG_HEARTBEAT, sizeof(status_flags), (u8 *)&status_flags);
/* If we are in base station mode then broadcast our known location. */
if (broadcast_surveyed_position && position_quality == POSITION_FIX) {
double tmp[3];
double base_ecef[3];
double base_distance;
llhdeg2rad(base_llh, tmp);
wgsllh2ecef(tmp, base_ecef);
vector_subtract(3, base_ecef, position_solution.pos_ecef, tmp);
base_distance = vector_norm(3, tmp);
if (base_distance > BASE_STATION_DISTANCE_THRESHOLD) {
log_warn("Invalid surveyed position coordinates\n");
} else {
sbp_send_msg(SBP_MSG_BASE_POS_ECEF, sizeof(msg_base_pos_ecef_t), (u8 *)&base_ecef);
}
}
msg_iar_state_t iar_state;
if (simulation_enabled_for(SIMULATION_MODE_RTK)) {
iar_state.num_hyps = 1;
} else {
iar_state.num_hyps = dgnss_iar_num_hyps();
}
sbp_send_msg(SBP_MSG_IAR_STATE, sizeof(msg_iar_state_t), (u8 *)&iar_state);
DO_EVERY(2,
send_thread_states();
);
sleep_until(&time, MS2ST(heartbeat_period_milliseconds));
}
static void handle_fix(const sensor_msgs::NavSatFixConstPtr fix_ptr,
const ros::Publisher& pub_enu,
const ros::Publisher& pub_datum)
{
static double ecef_datum[3];
static bool have_datum = false;
if (!have_datum) {
initialize_datum(ecef_datum, fix_ptr, pub_datum);
have_datum = true;
}
// Prepare the appropriate input vector to convert the input latlon
// to an ECEF triplet.
double llh[3] = { fix_ptr->latitude * TO_RADIANS,
fix_ptr->longitude * TO_RADIANS,
fix_ptr->altitude };
double ecef[3];
wgsllh2ecef(llh, ecef);
// ECEF triplet is converted to north-east-down (NED), by combining it
// with the ECEF-formatted datum point.
double ned[3];
wgsecef2ned_d(ecef, ecef_datum, ned);
nav_msgs::Odometry odom_msg;
odom_msg.header.stamp = fix_ptr->header.stamp;
odom_msg.pose.pose.position.x = ned[1];
odom_msg.pose.pose.position.y = ned[0];
odom_msg.pose.pose.position.z = -ned[2];
// We only need to populate the diagonals of the covariance matrix; the
// rest initialize to zero automatically, which is correct as the
// dimensions of the state are independent.
odom_msg.pose.covariance[0] = fix_ptr->position_covariance[0];
odom_msg.pose.covariance[7] = fix_ptr->position_covariance[4];
odom_msg.pose.covariance[14] = fix_ptr->position_covariance[8];
// Do not use/trust orientation dimensions from GPS.
odom_msg.pose.covariance[21] = 1e6;
odom_msg.pose.covariance[28] = 1e6;
odom_msg.pose.covariance[35] = 1e6;
// Orientation of GPS is pointing upward (as far as we know).
odom_msg.pose.pose.orientation.w = 1;
pub_enu.publish(odom_msg);
}
/** SBP callback for when the base station sends us a message containing its
* known location.
* Stores the base station position in the global #base_pos_ecef variable and
* sets #base_pos_known to `true`.
*/
static void base_pos_callback(u16 sender_id, u8 len, u8 msg[], void* context)
{
(void) context; (void) len; (void) msg; (void) sender_id;
double llh_degrees[3];
double llh[3];
memcpy(llh_degrees, msg, 3*sizeof(double));
llh[0] = llh_degrees[0] * D2R;
llh[1] = llh_degrees[1] * D2R;
llh[2] = llh_degrees[2];
chMtxLock(&base_pos_lock);
wgsllh2ecef(llh, base_pos_ecef);
base_pos_known = true;
chMtxUnlock();
}
END_TEST
START_TEST(test_wgsllh2ecef)
{
double ecef[3];
wgsllh2ecef(llhs[_i], ecef);
for (int n=0; n<3; n++) {
fail_unless(!isnan(ecef[n]), "NaN in output from wgsllh2ecef.");
double err = fabs(ecef[n] - ecefs[_i][n]);
fail_unless(err < MAX_DIST_ERROR_M,
"Conversion from WGS84 LLH to ECEF has >1e-6m error:\n"
"LLH: %f, %f, %f\n"
"X error (mm): %g\nY error (mm): %g\nZ error (mm): %g",
llhs[_i][0]*R2D, llhs[_i][1]*R2D, llhs[_i][2],
(ecef[0] - ecefs[_i][0])*1e3,
(ecef[1] - ecefs[_i][1])*1e3,
(ecef[2] - ecefs[_i][2])*1e3
);
}
}
END_TEST
START_TEST(test_random_wgsecef2llh2ecef)
{
double ecef_init[3];
double llh[3];
double ecef[3];
seed_rng();
ecef_init[0] = frand(-4*EARTH_A, 4*EARTH_A);
ecef_init[1] = frand(-4*EARTH_A, 4*EARTH_A);
ecef_init[2] = frand(-4*EARTH_A, 4*EARTH_A);
wgsecef2llh(ecef_init, llh);
wgsllh2ecef(llh, ecef);
for (int n=0; n<3; n++) {
fail_unless(!isnan(llh[n]), "NaN in LLH output from conversion.");
fail_unless(!isnan(ecef[n]), "NaN in ECEF output from conversion.");
}
for (int n=0; n<3; n++) {
double err = fabs(ecef[n] - ecef_init[n]);
fail_unless(err < MAX_DIST_ERROR_M,
"Converting random WGS84 ECEF to LLH and back again does not return the "
"original values.\n"
"Initial ECEF: %f, %f, %f\n"
"LLH: %f, %f, %f\n"
"Final ECEF: %f, %f, %f\n"
"X error (mm): %g\nY error (mm): %g\nZ error (mm): %g",
ecef_init[0], ecef_init[1], ecef_init[2],
R2D*llh[0], R2D*llh[1], llh[2],
ecef[0], ecef[1], ecef[2],
(ecef[0] - ecef_init[0])*1e3,
(ecef[1] - ecef_init[1])*1e3,
(ecef[2] - ecef_init[2])*1e3
);
}
fail_unless((R2D*llh[0] >= -90) && (R2D*llh[0] <= 90),
"Converting random WGS84 ECEF gives latitude out of bounds.\n"
"Initial ECEF: %f, %f, %f\n"
"LLH: %f, %f, %f\n",
ecef_init[0], ecef_init[1], ecef_init[2],
R2D*llh[0], R2D*llh[1], llh[2]
);
fail_unless((R2D*llh[1] >= -180) && (R2D*llh[1] <= 180),
"Converting random WGS84 ECEF gives longitude out of bounds.\n"
"Initial ECEF: %f, %f, %f\n"
"LLH: %f, %f, %f\n",
ecef_init[0], ecef_init[1], ecef_init[2],
R2D*llh[0], R2D*llh[1], llh[2]
);
fail_unless(llh[2] > -EARTH_A,
"Converting random WGS84 ECEF gives height out of bounds.\n"
"Initial ECEF: %f, %f, %f\n"
"LLH: %f, %f, %f\n",
ecef_init[0], ecef_init[1], ecef_init[2],
R2D*llh[0], R2D*llh[1], llh[2]
);
}
END_TEST
START_TEST(test_random_wgsllh2ecef2llh)
{
double ecef[3];
double llh_init[3];
double llh[3];
seed_rng();
llh_init[0] = D2R*frand(-90, 90);
llh_init[1] = D2R*frand(-180, 180);
llh_init[2] = frand(-0.5 * EARTH_A, 4 * EARTH_A);
wgsllh2ecef(llh_init, ecef);
wgsecef2llh(ecef, llh);
for (int n=0; n<3; n++) {
fail_unless(!isnan(llh[n]), "NaN in LLH output from conversion.");
fail_unless(!isnan(ecef[n]), "NaN in ECEF output from conversion.");
}
double lat_err = fabs(llh[0] - llh_init[0]);
double lon_err = fabs(llh[1] - llh_init[1]);
double hgt_err = fabs(llh[2] - llh_init[2]);
fail_unless((lat_err < MAX_ANGLE_ERROR_RAD) &&
(lon_err < MAX_ANGLE_ERROR_RAD) &&
(hgt_err < MAX_DIST_ERROR_M),
"Converting random WGS84 LLH to ECEF and back again does not return the "
"original values.\n"
"Initial LLH: %f, %f, %f\n"
"ECEF: %f, %f, %f\n"
"Final LLH: %f, %f, %f\n"
"Lat error (arc sec): %g\nLon error (arc sec): %g\nH error (mm): %g",
R2D*llh_init[0], R2D*llh_init[1], llh_init[2],
ecef[0], ecef[1], ecef[2],
R2D*llh[0], R2D*llh[1], llh[2],
(llh[0] - llh_init[0])*(R2D*3600),
(llh[1] - llh_init[1])*(R2D*3600),
(llh[2] - llh_init[2])*1e3
);
fail_unless((R2D*llh[0] >= -90) && (R2D*llh[0] <= 90),
"Converting random WGS84 ECEF gives latitude out of bounds.\n"
"ECEF: %f, %f, %f\n"
"LLH: %f, %f, %f\n",
ecef[0], ecef[1], ecef[2],
R2D*llh[0], R2D*llh[1], llh[2]
);
fail_unless((R2D*llh[1] >= -180) && (R2D*llh[1] <= 180),
"Converting random WGS84 ECEF gives longitude out of bounds.\n"
"ECEF: %f, %f, %f\n"
"LLH: %f, %f, %f\n",
ecef[0], ecef[1], ecef[2],
R2D*llh[0], R2D*llh[1], llh[2]
);
fail_unless(llh[2] > -EARTH_A,
"Converting random WGS84 ECEF gives height out of bounds.\n"
"ECEF: %f, %f, %f\n"
"LLH: %f, %f, %f\n",
ecef[0], ecef[1], ecef[2],
R2D*llh[0], R2D*llh[1], llh[2]
);
}
static void test_wgs_conversion_functions(void)
{
#define D2R DEG_TO_RAD_DOUBLE
/* Maximum allowable error in quantities with units of length (in meters). */
#define MAX_DIST_ERROR_M 1e-6
/* Maximum allowable error in quantities with units of angle (in sec of arc).
* 1 second of arc on the equator is ~31 meters. */
#define MAX_ANGLE_ERROR_SEC 1e-7
#define MAX_ANGLE_ERROR_RAD (MAX_ANGLE_ERROR_SEC*(D2R/3600.0))
/* Semi-major axis. */
#define EARTH_A 6378137.0
/* Semi-minor axis. */
#define EARTH_B 6356752.31424517929553985595703125
#define NUM_COORDS 10
Vector3d llhs[NUM_COORDS];
llhs[0] = Vector3d(0, 0, 0); /* On the Equator and Prime Meridian. */
llhs[1] = Vector3d(0, 180*D2R, 0); /* On the Equator. */
llhs[2] = Vector3d(0, 90*D2R, 0); /* On the Equator. */
llhs[3] = Vector3d(0, -90*D2R, 0); /* On the Equator. */
llhs[4] = Vector3d(90*D2R, 0, 0); /* North pole. */
llhs[5] = Vector3d(-90*D2R, 0, 0); /* South pole. */
llhs[6] = Vector3d(90*D2R, 0, 22); /* 22m above the north pole. */
llhs[7] = Vector3d(-90*D2R, 0, 22); /* 22m above the south pole. */
llhs[8] = Vector3d(0, 0, 22); /* 22m above the Equator and Prime Meridian. */
llhs[9] = Vector3d(0, 180*D2R, 22); /* 22m above the Equator. */
Vector3d ecefs[NUM_COORDS];
ecefs[0] = Vector3d(EARTH_A, 0, 0);
ecefs[1] = Vector3d(-EARTH_A, 0, 0);
ecefs[2] = Vector3d(0, EARTH_A, 0);
ecefs[3] = Vector3d(0, -EARTH_A, 0);
ecefs[4] = Vector3d(0, 0, EARTH_B);
ecefs[5] = Vector3d(0, 0, -EARTH_B);
ecefs[6] = Vector3d(0, 0, (EARTH_B+22));
ecefs[7] = Vector3d(0, 0, -(EARTH_B+22));
ecefs[8] = Vector3d((22+EARTH_A), 0, 0);
ecefs[9] = Vector3d(-(22+EARTH_A), 0, 0);
hal.console->printf("TESTING wgsllh2ecef\n");
for (int i = 0; i < NUM_COORDS; i++) {
Vector3d ecef;
wgsllh2ecef(llhs[i], ecef);
double x_err = fabs(ecef[0] - ecefs[i][0]);
double y_err = fabs(ecef[1] - ecefs[i][1]);
double z_err = fabs(ecef[2] - ecefs[i][2]);
if ((x_err < MAX_DIST_ERROR_M) &&
(y_err < MAX_DIST_ERROR_M) &&
(z_err < MAX_DIST_ERROR_M)) {
hal.console->printf("passing llh to ecef test %d\n", i);
} else {
hal.console->printf("failed llh to ecef test %d: ", i);
hal.console->printf("(%f - %f) (%f - %f) (%f - %f) => %.10f %.10f %.10f\n", ecef[0], ecefs[i][0], ecef[1], ecefs[i][1], ecef[2], ecefs[i][2], x_err, y_err, z_err);
}
}
hal.console->printf("TESTING wgsecef2llh\n");
for (int i = 0; i < NUM_COORDS; i++) {
Vector3d llh;
wgsecef2llh(ecefs[i], llh);
double lat_err = fabs(llh[0] - llhs[i][0]);
double lon_err = fabs(llh[1] - llhs[i][1]);
double hgt_err = fabs(llh[2] - llhs[i][2]);
if ((lat_err < MAX_ANGLE_ERROR_RAD) &&
(lon_err < MAX_ANGLE_ERROR_RAD) &&
(hgt_err < MAX_DIST_ERROR_M)) {
hal.console->printf("passing exef to llh test %d\n", i);
} else {
hal.console->printf("failed ecef to llh test %d: ", i);
hal.console->printf("%.10f %.10f %.10f\n", lat_err, lon_err, hgt_err);
}
}
}
