__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat,
double *lon)
{
if (!map_projection_initialized(ref)) {
return -1;
}
double x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
double y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
double c = sqrtf(x_rad * x_rad + y_rad * y_rad);
double sin_c = sin(c);
double cos_c = cos(c);
double lat_rad;
double lon_rad;
if (fabs(c) > DBL_EPSILON) {
lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
} else {
lat_rad = ref->lat_rad;
lon_rad = ref->lon_rad;
}
*lat = lat_rad * 180.0 / M_PI;
*lon = lon_rad * 180.0 / M_PI;
return 0;
}
int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x,
float *y)
{
if (!map_projection_initialized(ref)) {
return -1;
}
double lat_rad = lat * M_DEG_TO_RAD;
double lon_rad = lon * M_DEG_TO_RAD;
double sin_lat = sin(lat_rad);
double cos_lat = cos(lat_rad);
double cos_d_lon = cos(lon_rad - ref->lon_rad);
double arg = ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon;
if (arg > 1.0) {
arg = 1.0;
} else if (arg < -1.0) {
arg = -1.0;
}
double c = acos(arg);
double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c));
*x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH;
*y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH;
return 0;
}
__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad)
{
if (!map_projection_initialized(ref)) {
return -1;
}
*ref_lat_rad = ref->lat_rad;
*ref_lon_rad = ref->lon_rad;
return 0;
}
float OutputBase::_calculate_pitch(double lon, double lat, float altitude,
const vehicle_global_position_s &global_position)
{
if (!map_projection_initialized(&_projection_reference)) {
map_projection_init(&_projection_reference, global_position.lat, global_position.lon);
}
float x1, y1, x2, y2;
map_projection_project(&_projection_reference, lat, lon, &x1, &y1);
map_projection_project(&_projection_reference, global_position.lat, global_position.lon, &x2, &y2);
float dx = x1 - x2, dy = y1 - y2;
float target_distance = sqrtf(dx * dx + dy * dy);
float z = altitude - global_position.alt;
return atan2f(z, target_distance);
}
void
PrecLand::on_activation()
{
// We need to subscribe here and not in the constructor because constructor is called before the navigator task is spawned
if (!_targetPoseSub) {
_targetPoseSub = orb_subscribe(ORB_ID(landing_target_pose));
}
_state = PrecLandState::Start;
_search_cnt = 0;
_last_slewrate_time = 0;
vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position();
if (!map_projection_initialized(&_map_ref)) {
map_projection_init(&_map_ref, vehicle_local_position->ref_lat, vehicle_local_position->ref_lon);
}
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->next.valid = false;
// Check that the current position setpoint is valid, otherwise land at current position
if (!pos_sp_triplet->current.valid) {
PX4_WARN("Resetting landing position to current position");
pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
pos_sp_triplet->current.valid = true;
}
_sp_pev = matrix::Vector2f(0, 0);
_sp_pev_prev = matrix::Vector2f(0, 0);
_last_slewrate_time = 0;
switch_to_state_start();
}
__EXPORT bool map_projection_global_initialized()
{
return map_projection_initialized(&mp_ref);
}
