// return a relative ground position to the home in meters
// North/East/Down order.
bool AP_AHRS_NavEKF::get_relative_position_NED_home(Vector3f &vec) const
{
Location originLLH;
Vector3f originNED;
if (!get_relative_position_NED_origin(originNED) ||
!get_origin(originLLH)) {
return false;
}
const Vector3f offset = location_3d_diff_NED(originLLH, _home);
vec.x = originNED.x - offset.x;
vec.y = originNED.y - offset.y;
vec.z = originNED.z - offset.z;
return true;
}
// get distance vector to target (in meters) and target's velocity all in NED frame
bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
{
// get our location
Location current_loc;
if (!AP::ahrs().get_position(current_loc)) {
return false;
}
// get target location and velocity
Location target_loc;
Vector3f veh_vel;
if (!get_target_location_and_velocity(target_loc, veh_vel)) {
return false;
}
// change to altitude above home if relative altitude is being used
if (target_loc.flags.relative_alt == 1) {
current_loc.alt -= AP::ahrs().get_home().alt;
}
// calculate difference
const Vector3f dist_vec = location_3d_diff_NED(current_loc, target_loc);
// fail if too far
if (is_positive(_dist_max.get()) && (dist_vec.length() > _dist_max)) {
return false;
}
// initialise offsets from distance vector if required
init_offsets_if_required(dist_vec);
// get offsets
Vector3f offsets;
if (!get_offsets_ned(offsets)) {
return false;
}
// return results
dist_ned = dist_vec;
dist_with_offs = dist_vec + offsets;
vel_ned = veh_vel;
return true;
}
// these are basically the same checks as in AP_Arming:
bool Mode::enter_gps_checks() const
{
const AP_GPS &gps = AP::gps();
if (gps.status() < AP_GPS::GPS_OK_FIX_3D) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Bad GPS Position");
return false;
}
//GPS update rate acceptable
if (!gps.is_healthy()) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "GPS is not healthy");
}
// check GPSs are within 50m of each other and that blending is healthy
float distance_m;
if (!gps.all_consistent(distance_m)) {
gcs().send_text(MAV_SEVERITY_CRITICAL,
"GPS positions differ by %4.1fm",
(double)distance_m);
return false;
}
if (!gps.blend_health_check()) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "GPS blending unhealthy");
return false;
}
// check AHRS and GPS are within 10m of each other
const Location gps_loc = gps.location();
Location ahrs_loc;
if (ahrs.get_position(ahrs_loc)) {
float distance = location_3d_diff_NED(gps_loc, ahrs_loc).length();
if (distance > MODE_AHRS_GPS_ERROR_MAX) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "GPS and AHRS differ by %4.1fm", (double)distance);
return false;
}
}
return true;
}
bool AP_Arming::gps_checks(bool report)
{
const AP_GPS &gps = AP::gps();
if ((checks_to_perform & ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_GPS)) {
//GPS OK?
if (!AP::ahrs().home_is_set() ||
gps.status() < AP_GPS::GPS_OK_FIX_3D) {
if (report) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Bad GPS Position");
}
return false;
}
//GPS update rate acceptable
if (!gps.is_healthy()) {
if (report) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: GPS is not healthy");
}
return false;
}
// check GPSs are within 50m of each other and that blending is healthy
float distance_m;
if (!gps.all_consistent(distance_m)) {
if (report) {
gcs().send_text(MAV_SEVERITY_CRITICAL,
"PreArm: GPS positions differ by %4.1fm",
(double)distance_m);
}
return false;
}
if (!gps.blend_health_check()) {
if (report) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: GPS blending unhealthy");
}
return false;
}
// check AHRS and GPS are within 10m of each other
Location gps_loc = gps.location();
Location ahrs_loc;
if (ahrs.get_position(ahrs_loc)) {
float distance = location_3d_diff_NED(gps_loc, ahrs_loc).length();
if (distance > AP_ARMING_AHRS_GPS_ERROR_MAX) {
if (report) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: GPS and AHRS differ by %4.1fm", (double)distance);
}
return false;
}
}
}
if ((checks_to_perform & ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_GPS_CONFIG)) {
uint8_t first_unconfigured = gps.first_unconfigured_gps();
if (first_unconfigured != AP_GPS::GPS_ALL_CONFIGURED) {
if (report) {
gcs().send_text(MAV_SEVERITY_CRITICAL,
"PreArm: GPS %d failing configuration checks",
first_unconfigured + 1);
gps.broadcast_first_configuration_failure_reason();
}
return false;
}
}
return true;
}
