__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); }