void navRecallHome(void) {
navUkfSetGlobalPositionTarget(navData.homeLeg.targetLat, navData.homeLeg.targetLon);
navSetHoldAlt(navData.homeLeg.targetAlt, navData.homeLeg.relativeAlt);
navData.holdMaxHorizSpeed = navData.homeLeg.maxHorizSpeed;
navData.holdMaxVertSpeed = navData.homeLeg.maxVertSpeed;
navSetHoldHeading(navData.homeLeg.poiHeading);
if (navData.holdMaxHorizSpeed == 0.0f)
navData.holdMaxHorizSpeed = p[NAV_MAX_SPEED];
if (navData.holdMaxVertSpeed == 0.0f)
navData.holdMaxVertSpeed = p[NAV_ALT_POS_OM];
}
void navRecallHome(void) {
if (navData.homeLeg.targetLat == (double)0.0 || navData.homeLeg.targetLon == (double)0.0)
return;
navUkfSetGlobalPositionTarget(navData.homeLeg.targetLat, navData.homeLeg.targetLon);
navSetHoldAlt(navData.homeLeg.targetAlt, navData.homeLeg.relativeAlt);
navData.holdMaxHorizSpeed = navData.homeLeg.maxHorizSpeed;
navData.holdMaxVertSpeed = navData.homeLeg.maxVertSpeed;
navSetHoldHeading(navData.homeLeg.poiHeading);
if (navData.holdMaxHorizSpeed == 0.0f)
navData.holdMaxHorizSpeed = NAV_DFLT_HOR_SPEED;
if (navData.holdMaxVertSpeed == 0.0f)
navData.holdMaxVertSpeed = NAV_DFLT_VRT_SPEED;
}
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;
}
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;
}
void navUkfGpsPosUpdate(uint32_t gpsMicros, double lat, double lon, float alt, float hAcc, float vAcc) {
float y[3];
float noise[3];
float posDelta[3];
int histIndex;
if (navUkfData.holdLat == (double)0.0) {
navUkfData.holdLat = lat;
navUkfData.holdLon = lon;
navUkfCalcEarthRadius(lat);
navUkfSetGlobalPositionTarget(lat, lon);
navUkfResetPosition(-UKF_POSN, -UKF_POSE, alt - UKF_POSD);
}
else {
navUkfCalcGlobalDistance(lat, lon, &y[0], &y[1]);
y[2] = alt;
// determine how far back this GPS position update came from
histIndex = (timerMicros() - (gpsMicros + UKF_POS_DELAY)) / (int)(1e6f * AQ_OUTER_TIMESTEP);
histIndex = navUkfData.navHistIndex - histIndex;
if (histIndex < 0)
histIndex += UKF_HIST;
if (histIndex < 0 || histIndex >= UKF_HIST)
histIndex = 0;
// calculate delta from current position
posDelta[0] = UKF_POSN - navUkfData.posN[histIndex];
posDelta[1] = UKF_POSE - navUkfData.posE[histIndex];
posDelta[2] = UKF_POSD - navUkfData.posD[histIndex];
// set current position state to historic data
UKF_POSN = navUkfData.posN[histIndex];
UKF_POSE = navUkfData.posE[histIndex];
UKF_POSD = navUkfData.posD[histIndex];
noise[0] = UKF_GPS_POS_N + hAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.nDOP*gpsData.nDOP) * UKF_GPS_POS_M_N;
noise[1] = UKF_GPS_POS_N + hAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.eDOP*gpsData.eDOP) * UKF_GPS_POS_M_N;
noise[2] = UKF_GPS_ALT_N + vAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.vDOP*gpsData.vDOP) * UKF_GPS_ALT_M_N;
srcdkfMeasurementUpdate(navUkfData.kf, 0, y, 3, 3, noise, navUkfPosUpdate);
// add the historic position delta back to the current state
UKF_POSN += posDelta[0];
UKF_POSE += posDelta[1];
UKF_POSD += posDelta[2];
#ifdef UKF_LOG_FNAME
{
float *log = (float *)&ukfLog[navUkfData.logPointer];
int i = 0;
*(uint32_t *)&log[i++] = 0xffffffff;
log[i++] = y[0];
log[i++] = y[1];
log[i++] = y[2];
log[i++] = noise[0];
log[i++] = noise[1];
log[i++] = noise[2];
log[i++] = posDelta[0];
log[i++] = posDelta[1];
log[i++] = posDelta[2];
navUkfData.logPointer = (navUkfData.logPointer + UKF_LOG_SIZE) % UKF_LOG_BUF_SIZE;
filerSetHead(navUkfData.logHandle, navUkfData.logPointer);
}
#endif
}
}
void navUkfSetHereAsPositionTarget(void) {
if (navUkfData.flowPosN != 0.0f && navUkfData.flowPosE != 0.0f)
navUkfSetLocalPositionTarget(navUkfData.flowPosN, navUkfData.flowPosE);
else
navUkfSetGlobalPositionTarget(gpsData.lat, gpsData.lon);
}
