C++ (Cpp) init_offsets_if_required Beispiele

Beispiel #1

Datei: AP_Follow.cpp Projekt: mblsktxy/ardupilot

// 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)) {
        clear_dist_and_bearing_to_target();
         return false;
    }

    // get target location and velocity
    Location target_loc;
    Vector3f veh_vel;
    if (!get_target_location_and_velocity(target_loc, veh_vel)) {
        clear_dist_and_bearing_to_target();
        return false;
    }

    // change to altitude above home if relative altitude is being used
    if (target_loc.relative_alt == 1) {
        current_loc.alt -= AP::ahrs().get_home().alt;
    }

    // calculate difference
    const Vector3f dist_vec = current_loc.get_distance_NED(target_loc);

    // fail if too far
    if (is_positive(_dist_max.get()) && (dist_vec.length() > _dist_max)) {
        clear_dist_and_bearing_to_target();
        return false;
    }

    // initialise offsets from distance vector if required
    init_offsets_if_required(dist_vec);

    // get offsets
    Vector3f offsets;
    if (!get_offsets_ned(offsets)) {
        clear_dist_and_bearing_to_target();
        return false;
    }

    // calculate results
    dist_ned = dist_vec;
    dist_with_offs = dist_vec + offsets;
    vel_ned = veh_vel;

    // record distance and heading for reporting purposes
    if (is_zero(dist_with_offs.x) && is_zero(dist_with_offs.y)) {
        clear_dist_and_bearing_to_target();
    } else {
        _dist_to_target = safe_sqrt(sq(dist_with_offs.x) + sq(dist_with_offs.y));
        _bearing_to_target = degrees(atan2f(dist_with_offs.y, dist_with_offs.x));
    }

    return true;
}

Beispiel #2

Datei: AP_Follow.cpp Projekt: Benbenbeni/ardupilot-1

// 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;
}