C++ (Cpp) mavlinkWpAnnounceCurrent 예제들

예제 #1

파일: nav.c 프로젝트: ChrelleP/autoquad

navMission_t *navLoadLeg(uint8_t leg) {
    navMission_t *curLeg = &navData.missionLegs[leg];

    // invalid type?
    if (!curLeg->type || curLeg->type >= NAV_NUM_LEG_TYPES)
        return &navData.missionLegs[0];

    // common
    if (curLeg->relativeAlt)
        navSetHoldAlt(curLeg->targetAlt, curLeg->relativeAlt);
    else
        navSetHoldAlt(curLeg->targetAlt - navData.presAltOffset, curLeg->relativeAlt);

    navData.holdMaxHorizSpeed = curLeg->maxHorizSpeed;
    navData.holdMaxVertSpeed = curLeg->maxVertSpeed;

    // type specific
    if (curLeg->type == NAV_LEG_HOME) {
        navSetHoldAlt(navData.homeLeg.targetAlt, navData.homeLeg.relativeAlt);
        navUkfSetGlobalPositionTarget(navData.homeLeg.targetLat, navData.homeLeg.targetLon);
        navData.targetHeading = navData.homeLeg.poiHeading;

        navData.holdMaxHorizSpeed = navData.homeLeg.maxHorizSpeed;
        navData.holdMaxVertSpeed = navData.homeLeg.maxVertSpeed;
    }
    else if (curLeg->type == NAV_LEG_GOTO) {
        if (curLeg->targetLat != (double)0.0 && curLeg->targetLon != (double)0.0)
            navUkfSetGlobalPositionTarget(curLeg->targetLat, curLeg->targetLon);
        navData.targetHeading = curLeg->poiHeading;
    }
    else if (curLeg->type == NAV_LEG_ORBIT) {
        if (curLeg->targetLat != (double)0.0 && curLeg->targetLon != (double)0.0)
            navUkfSetGlobalPositionTarget(curLeg->targetLat, curLeg->targetLon);
        navData.targetHeading = curLeg->poiHeading;
        navData.holdMaxHorizSpeed = p[NAV_MAX_SPEED];
    }
    else if (curLeg->type == NAV_LEG_TAKEOFF) {
        // store this position as the takeoff position
        navUkfSetHereAsPositionTarget();
        navData.targetHeading = AQ_YAW;

        if (curLeg->maxVertSpeed == 0.0f)
            navData.holdMaxVertSpeed = p[NAV_LANDING_VEL];
        else
            navData.holdMaxVertSpeed = curLeg->maxVertSpeed;

        // set the launch location as home
        navSetHomeCurrent();
        navData.homeLeg.targetAlt = navData.holdAlt;
        navData.homeLeg.poiHeading = -0.0f;                // relative
    }
    else if (curLeg->type == NAV_LEG_LAND) {
        if (curLeg->maxVertSpeed == 0.0f)
            navData.holdMaxVertSpeed = p[NAV_LANDING_VEL];
        else
            navData.holdMaxVertSpeed = curLeg->maxVertSpeed;
    }

    if (navData.holdMaxHorizSpeed == 0.0f)
        navData.holdMaxHorizSpeed = p[NAV_MAX_SPEED];
    if (navData.holdMaxVertSpeed == 0.0f)
        navData.holdMaxVertSpeed = p[NAV_ALT_POS_OM];

    navData.loiterCompleteTime = 0;

    navData.missionLeg = leg;

#ifdef USE_MAVLINK
    // notify ground
    mavlinkWpAnnounceCurrent(leg);
#endif

    return curLeg;
}

예제 #2

파일: nav.c 프로젝트: mpaperno/aq_flight_control

navMission_t *navLoadLeg(uint8_t leg) {
    navMission_t *curLeg = &navData.missionLegs[leg];

    // invalid type?
    if (!curLeg->type || curLeg->type >= NAV_NUM_LEG_TYPES) {
        navData.hasMissionLeg = 0;
        return &navData.missionLegs[0];
    }

    // common
    if (curLeg->relativeAlt)
        navSetHoldAlt(curLeg->targetAlt, curLeg->relativeAlt);
    else
        navSetHoldAlt(curLeg->targetAlt - navData.presAltOffset, curLeg->relativeAlt);

    navData.holdMaxHorizSpeed = curLeg->maxHorizSpeed;
    navData.holdMaxVertSpeed = curLeg->maxVertSpeed;

    // type specific
    if (curLeg->type == NAV_LEG_HOME) {
        navSetHoldAlt(navData.homeLeg.targetAlt, navData.homeLeg.relativeAlt);
        navUkfSetGlobalPositionTarget(navData.homeLeg.targetLat, navData.homeLeg.targetLon);
        navData.targetHeading = navData.homeLeg.poiHeading;
        navData.holdMaxHorizSpeed = navData.homeLeg.maxHorizSpeed;
        navData.holdMaxVertSpeed = navData.homeLeg.maxVertSpeed;
    }
    else if (curLeg->type == NAV_LEG_GOTO) {
        if (curLeg->targetLat != (double)0.0 && curLeg->targetLon != (double)0.0)
            navUkfSetGlobalPositionTarget(curLeg->targetLat, curLeg->targetLon);
        navData.targetHeading = curLeg->poiHeading;
    }
    else if (curLeg->type == NAV_LEG_ORBIT) {
        if (curLeg->targetLat != (double)0.0 && curLeg->targetLon != (double)0.0)
            navUkfSetGlobalPositionTarget(curLeg->targetLat, curLeg->targetLon);
        navData.targetHeading = -0.0f;  // must point to target
    }
    else if (curLeg->type == NAV_LEG_TAKEOFF) {
        // store this position as the takeoff position
        navUkfSetHereAsPositionTarget();
        navData.targetHeading = AQ_YAW;
        navData.holdMaxVertSpeed = curLeg->maxVertSpeed;

        // set the launch location as home
        navSetHomeCurrent();
        navData.homeLeg.targetAlt = navData.holdAlt;
        navData.homeLeg.poiHeading = -0.0f;                // relative
    }
    else if (curLeg->type == NAV_LEG_LAND) {
        navUkfSetHereAsPositionTarget();  // hold current position
        navData.targetHeading = fabsf(curLeg->poiHeading);  // always fixed
        navData.holdMaxVertSpeed = curLeg->maxVertSpeed;
    }

    if (navData.holdMaxHorizSpeed == 0.0f)
        navData.holdMaxHorizSpeed = NAV_DFLT_HOR_SPEED;
    if (navData.holdMaxVertSpeed == 0.0f)
        navData.holdMaxVertSpeed = NAV_DFLT_VRT_SPEED;

    navData.loiterCompleteTime = 0;
    navData.hasMissionLeg = 1;
    navData.missionLeg = leg;

    AQ_PRINTF("NAV: Loaded waypoint %d.\n", leg);

#ifdef USE_MAVLINK
    // notify ground
    mavlinkWpAnnounceCurrent(leg);
#endif

    return curLeg;
}