Esempi in C++ (Cpp) per get_alt_frame

Esempio n. 1

File: Location.cpp Progetto: hiro2233/URUS8

/// get altitude in desired frame
bool Location_Class::get_alt_cm(ALT_FRAME desired_frame, int32_t &ret_alt_cm) const
{
    Location_Class::ALT_FRAME frame = get_alt_frame();

    // shortcut if desired and underlying frame are the same
    if (desired_frame == frame) {
        ret_alt_cm = alt;
        return true;
    }

    // check for terrain altitude
    float alt_terr_cm = 0;
    if (frame == ALT_FRAME_ABOVE_TERRAIN || desired_frame == ALT_FRAME_ABOVE_TERRAIN) {
#if AP_TERRAIN_AVAILABLE
        if (_ahrs == NULL || _terrain == NULL || !_terrain->height_amsl(*(Location *)this, alt_terr_cm, true)) {
            return false;
        }
        // convert terrain alt to cm
        alt_terr_cm *= 100.0f;
#else
        return false;
#endif
    }

    // convert alt to absolute
    int32_t alt_abs;
    switch (frame) {
    case ALT_FRAME_ABSOLUTE:
        alt_abs = alt;
        break;
    case ALT_FRAME_ABOVE_HOME:
        alt_abs = alt + _ahrs->get_home().alt;
        break;
    case ALT_FRAME_ABOVE_ORIGIN:
    {
        // fail if we cannot get ekf origin
        Location ekf_origin;
        if (_ahrs == NULL || !_ahrs->get_origin(ekf_origin)) {
            return false;
        }
        alt_abs = alt + ekf_origin.alt;
    }
    break;
    case ALT_FRAME_ABOVE_TERRAIN:
        alt_abs = alt + alt_terr_cm;
        break;
    default:
        // unknown conversion to absolute alt, this should never happen
        return false;
    }

    // convert absolute to desired frame
    switch (desired_frame) {
    case ALT_FRAME_ABSOLUTE:
        ret_alt_cm = alt_abs;
        return true;
    case ALT_FRAME_ABOVE_HOME:
        ret_alt_cm = alt_abs - _ahrs->get_home().alt;
        return true;
    case ALT_FRAME_ABOVE_ORIGIN:
    {
        // fail if we cannot get ekf origin
        Location ekf_origin;
        if (_ahrs == NULL || !_ahrs->get_origin(ekf_origin)) {
            return false;
        }
        ret_alt_cm = alt_abs - ekf_origin.alt;
        return true;
    }
    case ALT_FRAME_ABOVE_TERRAIN:
        ret_alt_cm = alt_abs - alt_terr_cm;
        return true;
    default:
        // should never happen
        return false;
    }
}

Esempio n. 2

File: Location.cpp Progetto: KIrill-ka/ardupilot

/// get altitude in desired frame
bool Location::get_alt_cm(AltFrame desired_frame, int32_t &ret_alt_cm) const
{
    Location::AltFrame frame = get_alt_frame();

    // shortcut if desired and underlying frame are the same
    if (desired_frame == frame) {
        ret_alt_cm = alt;
        return true;
    }

    // check for terrain altitude
    float alt_terr_cm = 0;
    if (frame == AltFrame::ABOVE_TERRAIN || desired_frame == AltFrame::ABOVE_TERRAIN) {
#if AP_TERRAIN_AVAILABLE
        if (_terrain == nullptr || !_terrain->height_amsl(*(Location *)this, alt_terr_cm, true)) {
            return false;
        }
        // convert terrain alt to cm
        alt_terr_cm *= 100.0f;
#else
        return false;
#endif
    }

    // convert alt to absolute
    int32_t alt_abs = 0;
    switch (frame) {
        case AltFrame::ABSOLUTE:
            alt_abs = alt;
            break;
        case AltFrame::ABOVE_HOME:
            if (!AP::ahrs().home_is_set()) {
                return false;
            }
            alt_abs = alt + AP::ahrs().get_home().alt;
            break;
        case AltFrame::ABOVE_ORIGIN:
            {
                // fail if we cannot get ekf origin
                Location ekf_origin;
                if (!AP::ahrs().get_origin(ekf_origin)) {
                    return false;
                }
                alt_abs = alt + ekf_origin.alt;
            }
            break;
        case AltFrame::ABOVE_TERRAIN:
            alt_abs = alt + alt_terr_cm;
            break;
    }

    // convert absolute to desired frame
    switch (desired_frame) {
        case AltFrame::ABSOLUTE:
            ret_alt_cm = alt_abs;
            return true;
        case AltFrame::ABOVE_HOME:
            if (!AP::ahrs().home_is_set()) {
                return false;
            }
            ret_alt_cm = alt_abs - AP::ahrs().get_home().alt;
            return true;
        case AltFrame::ABOVE_ORIGIN:
            {
                // fail if we cannot get ekf origin
                Location ekf_origin;
                if (!AP::ahrs().get_origin(ekf_origin)) {
                    return false;
                }
                ret_alt_cm = alt_abs - ekf_origin.alt;
                return true;
            }
        case AltFrame::ABOVE_TERRAIN:
            ret_alt_cm = alt_abs - alt_terr_cm;
            return true;
    }
    return false;
}