Exemplo n.º 1
0
// 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;
}
Exemplo n.º 2
0
// 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;
}
Exemplo n.º 3
0
// 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;
}
Exemplo n.º 4
0
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;
}