float ImagePair::location_distance() const {
auto p1 = image_1->get_metadata_position();
auto p2 = image_2->get_metadata_position();
if (p1.x != 0 && p1.y != 0 && p2.x != 0 && p2.y != 0)
return earth_distance(p1, p2);
else
return std::numeric_limits<float>::max();
}
static void calc_home(void *d)
{
home.uav_bearing = (int) get_bearing(&priv.home_coord, &priv.uav_coord);
home.direction = home.uav_bearing + 180;
home.direction -= priv.heading;
if (home.direction < 0)
home.direction += 360;
home.distance = earth_distance(&priv.home_coord, &priv.uav_coord);
home.altitude = priv.altitude - priv.home_altitude;
}
/**
* Return offset in meters of second arg from first
*/
void meter_offset(double *dx, double *dy, double lat1, double lon1, double lat2, double lon2)
{
double _dx = (double)earth_distance(lat1, lon1, lat1, lon2);
double _dy = (double)earth_distance(lat1, lon1, lat2, lon1);
if (lat1 < lat2)
{
_dy *= -1.0;
}
if (lon1 < lon2)
{
_dx *= -1.0;
}
if (isnan(_dx))
{
_dx = 0.0f;
}
if (isnan(_dy))
{
_dy = 0.0f;
}
*dx = _dx;
*dy = _dy;
}
/* return offset in meters */
static void meter_offset(float *dn, float *de, double lat1, double lon1, double lat2, double lon2)
{
double _dx = (double)earth_distance(lat1, lon1, lat1, lon2);
double _dy = (double)earth_distance(lat1, lon1, lat2, lon1);
if (lat1 < lat2)
{
_dy *= -1.0;
}
if (lon1 < lon2)
{
_dx *= -1.0;
}
if (isnan(_dx))
{
_dx = 0.0f;
}
if (isnan(_dy))
{
_dy = 0.0f;
}
*de = _dx;
*dn = _dy;
}
static void conditionally_log_fix(struct gps_data_t *gpsdata)
{
static double int_time, old_int_time;
static double old_lat, old_lon;
static bool first = true;
int_time = gpsdata->fix.time;
if ((int_time == old_int_time) || gpsdata->fix.mode < MODE_2D)
return;
/* may not be worth logging if we've moved only a very short distance */
if (minmove>0 && !first && earth_distance(
gpsdata->fix.latitude,
gpsdata->fix.longitude,
old_lat, old_lon) < minmove)
return;
/*
* Make new track if the jump in time is above
* timeout. Handle jumps both forward and
* backwards in time. The clock sometimes jumps
* backward when gpsd is submitting junk on the
* dbus.
*/
/*@-type@*/
if (fabs(int_time - old_int_time) > timeout && !first) {
print_gpx_trk_end();
intrack = false;
}
/*@+type@*/
if (!intrack) {
print_gpx_trk_start();
intrack = true;
if (first)
first = false;
}
old_int_time = int_time;
if (minmove > 0) {
old_lat = gpsdata->fix.latitude;
old_lon = gpsdata->fix.longitude;
}
print_fix(gpsdata, int_time);
}
bool mainLoop(struct gps_data_t *gps_data, int sockfd)
{
int rc;
while (1) {
/* wait for 2 seconds to receive data */
if (gps_waiting (gps_data, 2000000)) {
/* read data */
if ((rc = gps_read(gps_data)) == -1) {
return false;
} else {
/* Display data from the GPS receiver. */
if ((gps_data->status == STATUS_FIX) &&
(gps_data->fix.mode == MODE_2D || gps_data->fix.mode == MODE_3D) &&
!isnan(gps_data->fix.latitude) &&
!isnan(gps_data->fix.longitude)) {
if (isPrevValid) {
distance += earth_distance(prev.latitude,
prev.longitude,
gps_data->fix.latitude,
gps_data->fix.longitude);
}
logDataOut(&gps_data->fix);
prev = gps_data->fix;
isPrevValid = 1;
} else {
printf("no GPS data available\n");
}
}
}
}
return true;
}
static void calc_home(struct timer *t, void *d)
{
mavlink_heartbeat_t *hb = mavdata_get(MAVLINK_MSG_ID_HEARTBEAT);
mavlink_global_position_int_t *gpi;
mavlink_message_t this_msg;
switch (home.lock) {
case HOME_NONE:
default:
if (mavdata_age(MAVLINK_MSG_ID_HEARTBEAT) > 5000)
return;
/* check arming status */
if (hb->base_mode & MAV_MODE_FLAG_SAFETY_ARMED)
home.lock = HOME_WAIT;
break;
case HOME_WAIT:
if (mavdata_age(MAVLINK_MSG_ID_MISSION_ITEM) > 2000) {
/* when UAV is armed, home is WP0 */
mavlink_msg_mission_request_pack(config.mav.osd_sysid, MAV_COMP_ID_OSD, &this_msg,
config.mav.uav_sysid, MAV_COMP_ID_ALL, 0);
mavlink_send_msg(&this_msg);
} else {
mavlink_mission_item_t *mi = mavdata_get(MAVLINK_MSG_ID_MISSION_ITEM);
priv.home_coord.lat = DEG2RAD(mi->x);
priv.home_coord.lon = DEG2RAD(mi->y);
priv.home_altitude = (unsigned int) mi->z;
home.lock = HOME_GOT;
}
break;
case HOME_GOT:
home.lock = HOME_LOCKED;
set_timer_period(t, 200);
break;
case HOME_LOCKED:
{
gpi = mavdata_get(MAVLINK_MSG_ID_GLOBAL_POSITION_INT);
priv.uav_coord.lat = DEG2RAD(gpi->lat / 10000000.0);
priv.uav_coord.lon = DEG2RAD(gpi->lon / 10000000.0);
priv.altitude = (unsigned int) (gpi->alt / 1000);
priv.heading = (int) (gpi->hdg / 100);
home.uav_bearing = (int) get_bearing(&priv.home_coord, &priv.uav_coord);
home.direction = home.uav_bearing + 180;
home.direction -= priv.heading;
if (home.direction < 0)
home.direction += 360;
home.distance = earth_distance(&priv.home_coord, &priv.uav_coord);
home.altitude = priv.altitude - priv.home_altitude;
break;
case HOME_RESET:
home.lock = HOME_NONE;
set_timer_period(t, 1000);
break;
case HOME_FORCE:
gpi = mavdata_get(MAVLINK_MSG_ID_GLOBAL_POSITION_INT);
priv.home_coord.lat = DEG2RAD(gpi->lat / 10000000.0);
priv.home_coord.lon = DEG2RAD(gpi->lon / 10000000.0);
priv.home_altitude = (unsigned int) (gpi->alt / 1000);
home.lock = HOME_GOT;
break;
}
}
}
/*
* Initialize the trajectory tracing module.
* Return: 1 = ok, 0 = error
*/
int init_traj( Context ctx )
{
int i, j;
float lata, lona, latb, lonb, latc, lonc;
float d, us, vs;
float lat0, lon0, lat1, lon1, lat2, lon2;
float midrow, midcol;
int var = 0;
/* the index of any wind variable */
if (ctx->dpy_ctx->TrajU>=0) var = ctx->dpy_ctx->TrajU;
else if (ctx->dpy_ctx->TrajV>=0) var= ctx->dpy_ctx->TrajV;
else if (ctx->dpy_ctx->TrajW>=0) var= ctx->dpy_ctx->TrajW;
/* Compute initial trajectory step and length values */
switch (ctx->Projection) {
case PROJ_GENERIC:
ctx->TrajStep = 1.0;
ctx->TrajLength = 1.0;
break;
default:
/* TODO: verify this is ok: (seems to work) */
/* This is tricky: compute distance, in meters, from left to */
/* right edge of domain. Do it in two steps to prevent "wrap- */
/* around" when difference in longitudes > 180 degrees. */
midrow = (float) ctx->Nr / 2.0;
midcol = (float) ctx->Nc / 2.0;
rowcol_to_latlon( ctx, -1, -1, midrow, 0.0, &lat0, &lon0 );
rowcol_to_latlon( ctx, -1, -1, midrow, midcol, &lat1, &lon1 );
rowcol_to_latlon( ctx, -1, -1, midrow, (float) (ctx->Nc-1), &lat2, &lon2 );
d = earth_distance( lat0, lon0, lat1, lon1 )
+ earth_distance( lat1, lon1, lat2, lon2 );
ctx->TrajStep = TRUNC( (d / 100.0) / 25.0 );
/* the above was: (float) ctx->Elapsed[1] / 2.0;*/
ctx->TrajLength = 5.0 * ctx->TrajStep;
/* the above was: (float) ctx->Elapsed[1]; */
break;
}
/* These values are set by the user through the trajectory widget */
/* They are just multipliers of the internal values TrajStep and */
/* TrajLength: */
ctx->dpy_ctx->UserTrajStep = ctx->dpy_ctx->UserTrajLength = 1.0;
/*
* Compute m/s to boxes/s scaling factors for U and V components
*/
switch (ctx->Projection) {
case PROJ_GENERIC:
/* for a generic projection, we assume U, and V velocities are in */
/* X per second, where X is the same units used for NorthBound, */
/* WestBound, ctx->RowInc, and ctx->ColInc */
us = 1.0 / ctx->ColInc;
vs = -1.0 / ctx->RowInc;
for (i=0;i<ctx->Nr;i++) {
for (j=0;j<ctx->Nc;j++) {
ctx->Uscale[i][j] = us;
ctx->Vscale[i][j] = vs;
}
}
break;
case PROJ_LINEAR:
case PROJ_LAMBERT:
case PROJ_STEREO:
case PROJ_ROTATED:
case PROJ_CYLINDRICAL:
case PROJ_SPHERICAL:
case PROJ_MERCATOR:
for (i=0;i<ctx->Nr;i++) {
for (j=0;j<ctx->Nc;j++) {
float ii = (float) i;
float jj = (float) j;
/* Compute U scale */
if (j==0) {
rowcol_to_latlon( ctx, 0, var, ii, jj, &lata, &lona );
rowcol_to_latlon( ctx, 0, var, ii, jj+1.0, &latb, &lonb );
/* WLH 3-21-97 */
if (lata > 89.9) lata = 89.9;
if (lata < -89.9) lata = -89.9;
if (latb > 89.9) latb = 89.9;
if (latb < -89.9) latb = -89.9;
d = earth_distance( lata, lona, latb, lonb );
}
else if (j==ctx->Nc-1) {
rowcol_to_latlon( ctx, 0, var, ii, jj-1.0, &lata, &lona );
rowcol_to_latlon( ctx, 0, var, ii, jj, &latb, &lonb );
/* WLH 3-21-97 */
if (lata > 89.9) lata = 89.9;
if (lata < -89.9) lata = -89.9;
if (latb > 89.9) latb = 89.9;
if (latb < -89.9) latb = -89.9;
d = earth_distance( lata, lona, latb, lonb );
}
else {
rowcol_to_latlon( ctx, 0, var, ii, jj-1.0, &lata, &lona );
rowcol_to_latlon( ctx, 0, var, ii, jj, &latb, &lonb );
rowcol_to_latlon( ctx, 0, var, ii, jj+1.0, &latc, &lonc );
/* WLH 3-21-97 */
if (lata > 89.9) lata = 89.9;
if (lata < -89.9) lata = -89.9;
if (latb > 89.9) latb = 89.9;
if (latb < -89.9) latb = -89.9;
if (latc > 89.9) latc = 89.9;
if (latc < -89.9) latc = -89.9;
d = (earth_distance( lata,lona, latb,lonb)
+ earth_distance( latb,lonb, latc,lonc)) / 2.0;
}
ctx->Uscale[i][j] = 1.0 / d;
/* Compute V scale */
if (i==0) {
rowcol_to_latlon( ctx, 0, ctx->dpy_ctx->TrajV, ii, jj, &lata, &lona );
rowcol_to_latlon( ctx, 0, ctx->dpy_ctx->TrajV, ii+1.0, jj, &latb, &lonb );
/* WLH 3-21-97 */
if (lata > 89.9) lata = 89.9;
if (lata < -89.9) lata = -89.9;
if (latb > 89.9) latb = 89.9;
if (latb < -89.9) latb = -89.9;
d = earth_distance( lata, lona, latb, lonb );
}
else if (j==ctx->Nc-1) {
rowcol_to_latlon( ctx, 0, ctx->dpy_ctx->TrajV, ii-1.0, jj, &lata, &lona );
rowcol_to_latlon( ctx, 0, ctx->dpy_ctx->TrajV, ii, jj, &latb, &lonb );
/* WLH 3-21-97 */
if (lata > 89.9) lata = 89.9;
if (lata < -89.9) lata = -89.9;
if (latb > 89.9) latb = 89.9;
if (latb < -89.9) latb = -89.9;
d = earth_distance( lata, lona, latb, lonb );
}
else {
rowcol_to_latlon( ctx, 0, ctx->dpy_ctx->TrajV, ii-1.0, jj, &lata, &lona );
rowcol_to_latlon( ctx, 0, ctx->dpy_ctx->TrajV, ii, jj, &latb, &lonb );
rowcol_to_latlon( ctx, 0, ctx->dpy_ctx->TrajV, ii+1.0, jj, &latc, &lonc );
/* WLH 3-21-97 */
if (lata > 89.9) lata = 89.9;
if (lata < -89.9) lata = -89.9;
if (latb > 89.9) latb = 89.9;
if (latb < -89.9) latb = -89.9;
if (latc > 89.9) latc = 89.9;
if (latc < -89.9) latc = -89.9;
d = (earth_distance( lata,lona, latb,lonb)
+ earth_distance( latb,lonb, latc,lonc)) / 2.0;
}
ctx->Vscale[i][j] = -1.0 / d; /* Note negative!!! */
}
}
break;
default:
printf("Error in init_traj: Projection=%d\n", ctx->Projection);
return 0;
}
/*
* Compute m/s to boxes/s scaling factors for W component
*/
switch (ctx->VerticalSystem) {
case VERT_GENERIC:
for (i=0;i<ctx->MaxNl;i++) {
ctx->Wscale[i] = 1.0 / ctx->LevInc;
}
break;
case VERT_EQUAL_KM:
for (i=0;i<ctx->MaxNl;i++) {
ctx->Wscale[i] = 1.0 / (ctx->LevInc * 1000.0);
}
break;
case VERT_NONEQUAL_MB:
case VERT_NONEQUAL_KM:
for (i=0;i<ctx->MaxNl;i++) {
if (i==0) {
float hgt1 = gridlevel_to_height( ctx, 1.0 );
float hgt0 = gridlevel_to_height( ctx, 0.0 );
float diff = hgt1-hgt0;
if (fabs(diff) < 0.000001) diff = 0.000001;
ctx->Wscale[i] = 1.0 / (diff * 1000.0);
/* ctx->Wscale[i] = 1.0 / ((hgt1-hgt0) * 1000.0); */
}
else if (i==ctx->MaxNl-1) {
float hgt1 = gridlevel_to_height( ctx, (float)(ctx->MaxNl-1) );
float hgt0 = gridlevel_to_height( ctx, (float)(ctx->MaxNl-2) );
float diff = hgt1-hgt0;
if (fabs(diff) < 0.000001) diff = 0.000001;
ctx->Wscale[i] = 1.0 / (diff * 1000.0);
/* ctx->Wscale[i] = 1.0 / ((hgt1-hgt0) * 1000.0); */
}
else {
float a, b;
float hgt2 = gridlevel_to_height( ctx, (float)(i+1) );
float hgt1 = gridlevel_to_height( ctx, (float) i );
float hgt0 = gridlevel_to_height( ctx, (float)(i-1) );
float diffa = hgt1-hgt0;
float diffb = hgt2-hgt1;
if (fabs(diffa) < 0.000001) diffa = 0.000001;
if (fabs(diffb) < 0.000001) diffb = 0.000001;
a = 1.0 / (diffa * 1000.0);
b = 1.0 / (diffb * 1000.0);
/*
float a = 1.0 / ((hgt1-hgt0) * 1000.0);
float b = 1.0 / ((hgt2-hgt1) * 1000.0);
*/
ctx->Wscale[i] = (a+b) / 2.0;
}
}
break;
default:
printf("Error in init_traj: ctx->VerticalSystem=%d\n", ctx->VerticalSystem);
return 0;
}
return 1;
}
