void navUkfGpsVelUpdate(uint32_t gpsMicros, float velN, float velE, float velD, float sAcc) {
float y[3];
float noise[3];
float velDelta[3];
int histIndex;
y[0] = velN;
y[1] = velE;
y[2] = velD;
// determine how far back this GPS velocity update came from
histIndex = (timerMicros() - (gpsMicros + UKF_VEL_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
velDelta[0] = UKF_VELN - navUkfData.velN[histIndex];
velDelta[1] = UKF_VELE - navUkfData.velE[histIndex];
velDelta[2] = UKF_VELD - navUkfData.velD[histIndex];
// set current position state to historic data
UKF_VELN = navUkfData.velN[histIndex];
UKF_VELE = navUkfData.velE[histIndex];
UKF_VELD = navUkfData.velD[histIndex];
noise[0] = UKF_GPS_VEL_N + sAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.nDOP*gpsData.nDOP) * UKF_GPS_VEL_M_N;
noise[1] = UKF_GPS_VEL_N + sAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.eDOP*gpsData.eDOP) * UKF_GPS_VEL_M_N;
noise[2] = UKF_GPS_VD_N + sAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.vDOP*gpsData.vDOP) * UKF_GPS_VD_M_N;
srcdkfMeasurementUpdate(navUkfData.kf, 0, y, 3, 3, noise, navUkfVelUpdate);
// add the historic position delta back to the current state
UKF_VELN += velDelta[0];
UKF_VELE += velDelta[1];
UKF_VELD += velDelta[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++] = velDelta[0];
log[i++] = velDelta[1];
log[i++] = velDelta[2];
navUkfData.logPointer = (navUkfData.logPointer + UKF_LOG_SIZE) % UKF_LOG_BUF_SIZE;
filerSetHead(navUkfData.logHandle, navUkfData.logPointer);
}
#endif
}
// performs Cholesky factorization of 3x3 matrix
int cholF(float32_t *U) {
float32_t a11 = U[0];
float32_t a12 = U[1];
float32_t a13 = U[2];
float32_t a22 = U[4];
float32_t a23 = U[5];
float32_t a33 = U[8];
float32_t t0 = 1.0f / __sqrtf(a11);
float32_t t1 = a12 * t0;
float32_t t2 = a13 * t0;
float32_t t3 = t2 * t1;
float32_t t4 = __sqrtf(a22 - t1*t1);
float32_t t5 = (a23 - t3) / t4;
U[0] = a11 * t0;
U[1] = t1;
U[2] = t2;
U[3] = 0.0f;
U[4] = t4;
U[5] = t5;
U[6] = 0.0f;
U[7] = 0.0f;
U[8] = __sqrtf(a33 - t3 - t5 * t5);
// is positive definite?
return (isnan(t4) || isnan(U[8])) ? 0 : 1;
}
void simDoAccUpdate(float accX, float accY, float accZ) {
float noise[3]; // measurement variance
float y[3]; // measurement(s)
float norm;
// remove bias
accX += UKF_ACC_BIAS_X;
accY += UKF_ACC_BIAS_Y;
accZ += UKF_ACC_BIAS_Z;
// normalize vector
norm = __sqrtf(accX*accX + accY*accY + accZ*accZ);
y[0] = accX / norm;
y[1] = accY / norm;
y[2] = accZ / norm;
noise[0] = UKF_ACC_N + fabsf(GRAVITY - norm) * UKF_DIST_N;
if (!(supervisorData.state & STATE_FLYING)) {
accX -= UKF_ACC_BIAS_X;
accY -= UKF_ACC_BIAS_Y;
noise[0] *= 0.001f;
}
noise[1] = noise[0];
noise[2] = noise[0];
srcdkfMeasurementUpdate(navUkfData.kf, 0, y, 3, 3, noise, navUkfAccUpdate);
}
void navUkfNormalizeVec3(float *vr, float *v) {
float norm;
norm = __sqrtf(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
vr[0] = v[0] / norm;
vr[1] = v[1] / norm;
vr[2] = v[2] / norm;
}
void navUkfNormalizeQuat(float *qr, float *q) {
float norm;
norm = __sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
qr[0] = q[0] / norm;
qr[1] = q[1] / norm;
qr[2] = q[2] / norm;
qr[3] = q[3] / norm;
}
void vectorNormalize(float32_t *v, int n) {
float32_t t;
int i;
t = 0.0f;
for (i = 0; i < n; i++)
t += v[i] * v[i];
t = __sqrtf(t);
if (t > 1e-6f) {
t = 1.0f / t;
for (i = 0; i < n; i++)
v[i] *= t;
}
else {
for (i = 0; i < n; i++)
v[i] *= 0.0f;
}
}
void simDoMagUpdate(float magX, float magY, float magZ) {
float noise[3]; // measurement variance
float y[3]; // measurement(s)
float norm;
noise[0] = UKF_MAG_N;
if (!(supervisorData.state & STATE_FLYING))
noise[0] *= 0.001f;
noise[1] = noise[0];
noise[2] = noise[0];
// normalize vector
norm = 1.0f / __sqrtf(magX*magX + magY*magY + magZ*magZ);
y[0] = magX * norm;
y[1] = magY * norm;
y[2] = magZ * norm;
srcdkfMeasurementUpdate(navUkfData.kf, 0, y, 3, 3, noise, navUkfMagUpdate);
}
// Calculates the QR decomposition of the given matrix A Transposed (decomp's A', not A)
// notes: A matrix is modified
// Adapted from Java code originaly written by Joni Salonen
//
// returns 1 for success, 0 for failure
int qrDecompositionT_f32(arm_matrix_instance_f32 *A, arm_matrix_instance_f32 *Q, arm_matrix_instance_f32 *R) {
int minor;
int row, col;
int m = A->numCols;
int n = A->numRows;
int min;
// clear R
arm_fill_f32(0, R->pData, R->numRows*R->numCols);
min = MIN(m, n);
/*
* The QR decomposition of a matrix A is calculated using Householder
* reflectors by repeating the following operations to each minor
* A(minor,minor) of A:
*/
for (minor = 0; minor < min; minor++) {
float xNormSqr = 0.0f;
float a;
/*
* Let x be the first column of the minor, and a^2 = |x|^2.
* x will be in the positions A[minor][minor] through A[m][minor].
* The first column of the transformed minor will be (a,0,0,..)'
* The sign of a is chosen to be opposite to the sign of the first
* component of x. Let's find a:
*/
for (row = minor; row < m; row++)
xNormSqr += A->pData[minor*m + row]*A->pData[minor*m + row];
a = __sqrtf(xNormSqr);
if (A->pData[minor*m + minor] > 0.0f)
a = -a;
if (a != 0.0f) {
R->pData[minor*R->numCols + minor] = a;
/*
* Calculate the normalized reflection vector v and transform
* the first column. We know the norm of v beforehand: v = x-ae
* so |v|^2 = <x-ae,x-ae> = <x,x>-2a<x,e>+a^2<e,e> =
* a^2+a^2-2a<x,e> = 2a*(a - <x,e>).
* Here <x, e> is now A[minor][minor].
* v = x-ae is stored in the column at A:
*/
A->pData[minor*m + minor] -= a; // now |v|^2 = -2a*(A[minor][minor])
/*
* Transform the rest of the columns of the minor:
* They will be transformed by the matrix H = I-2vv'/|v|^2.
* If x is a column vector of the minor, then
* Hx = (I-2vv'/|v|^2)x = x-2vv'x/|v|^2 = x - 2<x,v>/|v|^2 v.
* Therefore the transformation is easily calculated by
* subtracting the column vector (2<x,v>/|v|^2)v from x.
*
* Let 2<x,v>/|v|^2 = alpha. From above we have
* |v|^2 = -2a*(A[minor][minor]), so
* alpha = -<x,v>/(a*A[minor][minor])
*/
for (col = minor+1; col < n; col++) {
float alpha = 0.0f;
for (row = minor; row < m; row++)
alpha -= A->pData[col*m + row]*A->pData[minor*m + row];
alpha /= a*A->pData[minor*m + minor];
// Subtract the column vector alpha*v from x.
for (row = minor; row < m; row++)
A->pData[col*m + row] -= alpha*A->pData[minor*m + row];
}
}
// rank deficient
else
return 0;
}
// Form the matrix R of the QR-decomposition.
// R is supposed to be m x n, but only calculate n x n
// copy the upper triangle of A
for (row = min-1; row >= 0; row--)
for (col = row+1; col < n; col++)
R->pData[row*R->numCols + col] = A->pData[col*m + row];
// Form the matrix Q of the QR-decomposition.
// Q is supposed to be m x m
// only compute Q if requested
if (Q) {
arm_fill_f32(0, Q->pData, Q->numRows*Q->numCols);
/*
* Q = Q1 Q2 ... Q_m, so Q is formed by first constructing Q_m and then
* applying the Householder transformations Q_(m-1),Q_(m-2),...,Q1 in
* succession to the result
*/
for (minor = m-1; minor >= min; minor--)
Q->pData[minor*m + minor] = 1.0f;
for (minor = min-1; minor >= 0; minor--) {
Q->pData[minor * m + minor] = 1.0f;
if (A->pData[minor*m + minor] != 0.0f) {
for (col = minor; col < m; col++) {
float alpha = 0.0f;
for (row = minor; row < m; row++)
alpha -= Q->pData[row*m + col]*A->pData[minor*m + row];
alpha /= R->pData[minor*R->numCols + minor]*A->pData[minor*m + minor];
for (row = minor; row < m; row++)
Q->pData[row*m + col] -= alpha*A->pData[minor*m + row];
}
}
}
}
return 1;
}
static float motorsThrust2Value(float thrust) {
return (-p[MOT_VALUE2T_A1] + __sqrtf(p[MOT_VALUE2T_A1]*p[MOT_VALUE2T_A1] + p[MOT_VALUE2T_A2] * 4.0f * thrust)) * (1.0f / (2.0f * p[MOT_VALUE2T_A2]));
}
// input lat/lon in degrees, returns distance in meters
float navCalcDistance(double lat1, double lon1, double lat2, double lon2) {
float n = (lat1 - lat2) * navUkfData.r1;
float e = (lon1 - lon2) * navUkfData.r2;
return __sqrtf(n*n + e*e);
}
void navNavigate(void) {
unsigned long currentTime;
unsigned char leg = navData.missionLeg;
navMission_t *curLeg = &navData.missionLegs[leg];
float tmp;
currentTime = IMU_LASTUPD;
navSetFixType();
// is there sufficient position accuracy to navigate?
if (navData.fixType == 3)
navData.navCapable = 1;
// do not drop out of mission due to (hopefully) temporary GPS degradation
else if (navData.mode < NAV_STATUS_POSHOLD)
navData.navCapable = 0;
// Can we navigate && do we want to be in mission mode?
if ((supervisorData.state & STATE_ARMED) && navData.navCapable && RADIO_FLAPS > 250) {
// are we currently in position hold mode && do we have a clear mission ahead of us?
if ((navData.mode == NAV_STATUS_POSHOLD || navData.mode == NAV_STATUS_DVH) && leg < NAV_MAX_MISSION_LEGS && curLeg->type > 0) {
curLeg = navLoadLeg(leg);
navData.mode = NAV_STATUS_MISSION;
}
}
// do we want to be in position hold mode?
else if ((supervisorData.state & STATE_ARMED) && RADIO_FLAPS > -250) {
// always allow alt hold
if (navData.mode < NAV_STATUS_ALTHOLD) {
// record this altitude as the hold altitude
navSetHoldAlt(ALTITUDE, 0);
// set integral to current RC throttle setting
pidZeroIntegral(navData.altSpeedPID, -VELOCITYD, motorsData.throttle * RADIO_MID_THROTTLE / MOTORS_SCALE);
pidZeroIntegral(navData.altPosPID, ALTITUDE, 0.0f);
navData.holdSpeedAlt = navData.targetHoldSpeedAlt = -VELOCITYD;
navData.holdMaxVertSpeed = p[NAV_ALT_POS_OM];
navData.mode = NAV_STATUS_ALTHOLD;
// notify ground
AQ_NOTICE("Altitude Hold engaged\n");
}
// are we not in position hold mode now?
if ((navData.navCapable || navUkfData.flowQuality > 0.0f) && navData.mode != NAV_STATUS_POSHOLD && navData.mode != NAV_STATUS_DVH) {
// only zero bias if coming from lower mode
if (navData.mode < NAV_STATUS_POSHOLD) {
navData.holdTiltN = 0.0f;
navData.holdTiltE = 0.0f;
// speed
pidZeroIntegral(navData.speedNPID, UKF_VELN, 0.0f);
pidZeroIntegral(navData.speedEPID, UKF_VELE, 0.0f);
// pos
pidZeroIntegral(navData.distanceNPID, UKF_POSN, 0.0f);
pidZeroIntegral(navData.distanceEPID, UKF_POSE, 0.0f);
}
// store this position as hold position
navUkfSetHereAsPositionTarget();
if (navData.navCapable) {
// set this position as home if we have none
if (navData.homeLeg.targetLat == (double)0.0 || navData.homeLeg.targetLon == (double)0.0)
navSetHomeCurrent();
}
// activate pos hold
navData.mode = NAV_STATUS_POSHOLD;
navData.holdSpeedN = 0.0f;
navData.holdSpeedE = 0.0f;
navData.holdMaxHorizSpeed = p[NAV_MAX_SPEED];
navData.holdMaxVertSpeed = p[NAV_ALT_POS_OM];
// notify ground
AQ_NOTICE("Position Hold engaged\n");
}
// DVH
else if ((navData.navCapable || navUkfData.flowQuality > 0.0f) && (
RADIO_PITCH > p[CTRL_DEAD_BAND] ||
RADIO_PITCH < -p[CTRL_DEAD_BAND] ||
RADIO_ROLL > p[CTRL_DEAD_BAND] ||
RADIO_ROLL < -p[CTRL_DEAD_BAND])) {
navData.mode = NAV_STATUS_DVH;
}
else if (navData.mode == NAV_STATUS_DVH) {
// allow speed to drop before holding position (or if RTH engaged)
if ((UKF_VELN < +0.1f && UKF_VELN > -0.1f && UKF_VELE < +0.1f && UKF_VELE > -0.1f) || RADIO_AUX2 < -250) {
navUkfSetHereAsPositionTarget();
navData.mode = NAV_STATUS_POSHOLD;
}
}
}
else {
// switch to manual mode
navData.mode = NAV_STATUS_MANUAL;
// reset mission legs
navData.missionLeg = leg = 0;
// keep up with changing altitude
navSetHoldAlt(ALTITUDE, 0);
}
// ceiling set ?, 0 is disable
if (navData.ceilingAlt) {
// ceiling reached 1st time
if (ALTITUDE > navData.ceilingAlt && !navData.setCeilingFlag) {
navData.setCeilingFlag = 1;
navData.ceilingTimer = timerMicros();
}
// ceiling still reached for 5 seconds
else if (navData.setCeilingFlag && ALTITUDE > navData.ceilingAlt && (timerMicros() - navData.ceilingTimer) > (1e6 * 5) ) {
navData.ceilingTimer = timerMicros();
if (!navData.setCeilingReached)
AQ_NOTICE("Warning: Altitude ceiling reached.");
navData.setCeilingReached = 1;
}
else if ((navData.setCeilingFlag || navData.setCeilingReached) && ALTITUDE <= navData.ceilingAlt) {
if (navData.setCeilingReached)
AQ_NOTICE("Notice: Altitude returned to within ceiling.");
navData.setCeilingFlag = 0;
navData.setCeilingReached = 0;
}
}
// home set
if ((supervisorData.state & STATE_ARMED) && RADIO_AUX2 > 250) {
if (!navData.homeActionFlag) {
navSetHomeCurrent();
navData.homeActionFlag = 1;
}
}
// recall home
else if ((supervisorData.state & STATE_ARMED) && RADIO_AUX2 < -250) {
if (!navData.homeActionFlag) {
navRecallHome();
AQ_NOTICE("Returning to home position\n");
navData.homeActionFlag = 1;
}
}
// switch to middle, clear action flag
else {
navData.homeActionFlag = 0;
}
// heading-free mode
if ((int)p[NAV_HDFRE_CHAN] > 0 && (int)p[NAV_HDFRE_CHAN] <= RADIO_MAX_CHANNELS) {
navSetHeadFreeMode();
// set/maintain headfree frame reference
if (!navData.homeActionFlag && ( navData.headFreeMode == NAV_HEADFREE_SETTING ||
(navData.headFreeMode == NAV_HEADFREE_DYNAMIC && navData.mode == NAV_STATUS_DVH) )) {
uint8_t dfRefTyp = 0;
if ((supervisorData.state & STATE_FLYING) && navData.homeLeg.targetLat != (double)0.0f && navData.homeLeg.targetLon != (double)0.0f) {
if (NAV_HF_HOME_DIST_D_MIN && NAV_HF_HOME_DIST_FREQ && (currentTime - navData.homeDistanceLastUpdate) > (AQ_US_PER_SEC / NAV_HF_HOME_DIST_FREQ)) {
navData.distanceToHome = navCalcDistance(gpsData.lat, gpsData.lon, navData.homeLeg.targetLat, navData.homeLeg.targetLon);
navData.homeDistanceLastUpdate = currentTime;
}
if (!NAV_HF_HOME_DIST_D_MIN || navData.distanceToHome > NAV_HF_HOME_DIST_D_MIN)
dfRefTyp = 1;
}
navSetHfReference(dfRefTyp);
}
}
if (UKF_POSN != 0.0f || UKF_POSE != 0.0f) {
navData.holdCourse = compassNormalize(atan2f(-UKF_POSE, -UKF_POSN) * RAD_TO_DEG);
navData.holdDistance = __sqrtf(UKF_POSN*UKF_POSN + UKF_POSE*UKF_POSE);
}
else {
navData.holdCourse = 0.0f;
navData.holdDistance = 0.0f;
}
if (navData.mode == NAV_STATUS_MISSION) {
// have we arrived yet?
if (navData.loiterCompleteTime == 0) {
// are we close enough (distance and altitude)?
// goto/home test
if (((curLeg->type == NAV_LEG_GOTO || curLeg->type == NAV_LEG_HOME) &&
navData.holdDistance < curLeg->targetRadius &&
fabsf(navData.holdAlt - ALTITUDE) < curLeg->targetRadius) ||
// orbit test
(curLeg->type == NAV_LEG_ORBIT &&
fabsf(navData.holdDistance - curLeg->targetRadius) +
fabsf(navData.holdAlt - ALTITUDE) < 2.0f) ||
// takeoff test
(curLeg->type == NAV_LEG_TAKEOFF &&
navData.holdDistance < curLeg->targetRadius &&
fabsf(navData.holdAlt - ALTITUDE) < curLeg->targetRadius)
) {
// freeze heading unless orbiting
if (curLeg->type != NAV_LEG_ORBIT)
navSetHoldHeading(AQ_YAW);
// start the loiter clock
navData.loiterCompleteTime = currentTime + curLeg->loiterTime;
#ifdef USE_MAVLINK
// notify ground
mavlinkWpReached(leg);
#endif
}
}
// have we loitered long enough?
else if (currentTime > navData.loiterCompleteTime && curLeg->type != NAV_LEG_LAND) {
// next leg
if (++leg < NAV_MAX_MISSION_LEGS && navData.missionLegs[leg].type > 0) {
curLeg = navLoadLeg(leg);
}
else {
navData.mode = NAV_STATUS_POSHOLD;
}
}
}
// DVH
if (navData.mode == NAV_STATUS_DVH) {
float factor = (1.0f / 500.0f) * navData.holdMaxHorizSpeed;
float x = 0.0f;
float y = 0.0f;
if (RADIO_PITCH > p[CTRL_DEAD_BAND])
x = -(RADIO_PITCH - p[CTRL_DEAD_BAND]) * factor;
if (RADIO_PITCH < -p[CTRL_DEAD_BAND])
x = -(RADIO_PITCH + p[CTRL_DEAD_BAND]) * factor;
if (RADIO_ROLL > p[CTRL_DEAD_BAND])
y = +(RADIO_ROLL - p[CTRL_DEAD_BAND]) * factor;
if (RADIO_ROLL < -p[CTRL_DEAD_BAND])
y = +(RADIO_ROLL + p[CTRL_DEAD_BAND]) * factor;
// do we want to ignore rotation of craft (headfree/carefree mode)?
if (navData.headFreeMode > NAV_HEADFREE_OFF) {
if (navData.hfUseStoredReference) {
// rotate to stored frame
navData.holdSpeedN = x * navData.hfReferenceCos - y * navData.hfReferenceSin;
navData.holdSpeedE = y * navData.hfReferenceCos + x * navData.hfReferenceSin;
}
else {
// don't rotate to any frame (pitch/roll move to N/S/E/W)
navData.holdSpeedN = x;
navData.holdSpeedE = y;
}
}
else {
// rotate to earth frame
navData.holdSpeedN = x * navUkfData.yawCos - y * navUkfData.yawSin;
navData.holdSpeedE = y * navUkfData.yawCos + x * navUkfData.yawSin;
}
}
// orbit POI
else if (navData.mode == NAV_STATUS_MISSION && curLeg->type == NAV_LEG_ORBIT) {
float velX, velY;
// maintain orbital radius
velX = -pidUpdate(navData.distanceNPID, curLeg->targetRadius, navData.holdDistance);
// maintain orbital velocity (clock wise)
velY = -curLeg->maxHorizSpeed;
// rotate to earth frame
navData.holdSpeedN = velX * navUkfData.yawCos - velY * navUkfData.yawSin;
navData.holdSpeedE = velY * navUkfData.yawCos + velX * navUkfData.yawSin;
}
else {
// distance => velocity
navData.holdSpeedN = pidUpdate(navData.distanceNPID, 0.0f, UKF_POSN);
navData.holdSpeedE = pidUpdate(navData.distanceEPID, 0.0f, UKF_POSE);
}
// normalize N/E speed requests to fit below max nav speed
tmp = __sqrtf(navData.holdSpeedN*navData.holdSpeedN + navData.holdSpeedE*navData.holdSpeedE);
if (tmp > navData.holdMaxHorizSpeed) {
navData.holdSpeedN = (navData.holdSpeedN / tmp) * navData.holdMaxHorizSpeed;
navData.holdSpeedE = (navData.holdSpeedE / tmp) * navData.holdMaxHorizSpeed;
}
// velocity => tilt
navData.holdTiltN = -pidUpdate(navData.speedNPID, navData.holdSpeedN, UKF_VELN);
navData.holdTiltE = +pidUpdate(navData.speedEPID, navData.holdSpeedE, UKF_VELE);
if (navData.mode > NAV_STATUS_MANUAL) {
float vertStick;
// Throttle controls vertical speed
vertStick = RADIO_THROT - RADIO_MID_THROTTLE;
if ((vertStick > p[CTRL_DBAND_THRO] && !navData.setCeilingReached) || vertStick < -p[CTRL_DBAND_THRO]) {
// altitude velocity proportional to throttle stick
navData.targetHoldSpeedAlt = (vertStick - p[CTRL_DBAND_THRO] * (vertStick > 0.0f ? 1.0f : -1.0f)) * p[NAV_ALT_POS_OM] * (1.0f / RADIO_MID_THROTTLE);
navData.verticalOverride = 1;
}
// are we trying to land?
else if (navData.mode == NAV_STATUS_MISSION && curLeg->type == NAV_LEG_LAND) {
navData.targetHoldSpeedAlt = -navData.holdMaxVertSpeed;
}
// coming out of vertical override?
else if (navData.verticalOverride) {
navData.targetHoldSpeedAlt = 0.0f;
// slow down before trying to hold altitude
if (fabsf(VELOCITYD) < 0.025f)
navData.verticalOverride = 0;
// set new hold altitude to wherever we are while still in override
if (navData.mode != NAV_STATUS_MISSION)
navSetHoldAlt(ALTITUDE, 0);
}
// PID has the throttle
else {
navData.targetHoldSpeedAlt = pidUpdate(navData.altPosPID, navData.holdAlt, ALTITUDE);
}
// constrain vertical velocity
navData.targetHoldSpeedAlt = constrainFloat(navData.targetHoldSpeedAlt, (navData.holdMaxVertSpeed < p[NAV_MAX_DECENT]) ? -navData.holdMaxVertSpeed : -p[NAV_MAX_DECENT], navData.holdMaxVertSpeed);
// smooth vertical velocity changes
navData.holdSpeedAlt += (navData.targetHoldSpeedAlt - navData.holdSpeedAlt) * 0.05f;
}
else {
navData.verticalOverride = 0;
}
// calculate POI angle (used for tilt in gimbal function)
if (navData.mode == NAV_STATUS_MISSION && curLeg->poiAltitude != 0.0f) {
float a, b, c;
a = navData.holdDistance;
b = ALTITUDE - curLeg->poiAltitude;
c = __sqrtf(a*a + b*b);
navData.poiAngle = asinf(a/c) * RAD_TO_DEG - 90.0f;
}
else {
navData.poiAngle = 0.0f;
}
if (navData.mode == NAV_STATUS_MISSION) {
// recalculate autonomous heading
navSetHoldHeading(navData.targetHeading);
// wait for low throttle if landing
if (curLeg->type == NAV_LEG_LAND && motorsData.throttle <= 1)
// shut everything down (sure hope we are really on the ground :)
supervisorDisarm();
}
navData.lastUpdate = currentTime;
}
void navNavigate(void) {
unsigned long currentTime = IMU_LASTUPD;
unsigned char leg = navData.missionLeg;
navMission_t *curLeg = &navData.missionLegs[leg];
int reqFlightMode; // requested flight mode based on user controls or other factors
float tmp;
navSetFixType();
// is there sufficient position accuracy to navigate?
if (navData.fixType == 3)
navData.navCapable = 1;
// do not drop out of mission due to (hopefully) temporary GPS degradation
else if (navData.mode < NAV_STATUS_POSHOLD)
navData.navCapable = 0;
bool hasViableMission = (navData.navCapable && ((navData.mode != NAV_STATUS_MISSION && leg < NAV_MAX_MISSION_LEGS && curLeg->type) || (navData.mode == NAV_STATUS_MISSION && navData.hasMissionLeg)));
// this defines the hierarchy of available flight modes in case of failsafe override or conflicting controls being active
if (navData.spvrModeOverride)
reqFlightMode = navData.spvrModeOverride;
else if (rcIsSwitchActive(NAV_CTRL_MISN))
if (hasViableMission)
reqFlightMode = NAV_STATUS_MISSION;
else
reqFlightMode = NAV_STATUS_POSHOLD;
else if (rcIsSwitchActive(NAV_CTRL_PH)) {
reqFlightMode = NAV_STATUS_POSHOLD;
}
else if (rcIsSwitchActive(NAV_CTRL_AH))
reqFlightMode = NAV_STATUS_ALTHOLD;
// always default to manual
else
reqFlightMode = NAV_STATUS_MANUAL;
// Can we navigate && do we want to be in mission mode?
if ((supervisorData.state & STATE_ARMED) && reqFlightMode == NAV_STATUS_MISSION && hasViableMission) {
// are we currently in position hold mode && do we have a clear mission ahead of us?
if (navData.mode == NAV_STATUS_POSHOLD || navData.mode == NAV_STATUS_DVH) {
curLeg = navLoadLeg(leg);
navData.mode = NAV_STATUS_MISSION;
AQ_NOTICE("Mission mode active\n");
}
}
// do we want to be in position hold mode?
else if ((supervisorData.state & STATE_ARMED) && reqFlightMode > NAV_STATUS_MANUAL) {
// check for DVH
if (reqFlightMode == NAV_STATUS_POSHOLD && (RADIO_PITCH > p[CTRL_DEAD_BAND] || RADIO_PITCH < -p[CTRL_DEAD_BAND] || RADIO_ROLL > p[CTRL_DEAD_BAND] || RADIO_ROLL < -p[CTRL_DEAD_BAND]))
reqFlightMode = NAV_STATUS_DVH;
// allow alt hold regardless of GPS or flow quality
if (navData.mode < NAV_STATUS_ALTHOLD || (navData.mode != NAV_STATUS_ALTHOLD && reqFlightMode == NAV_STATUS_ALTHOLD)) {
// record this altitude as the hold altitude
navSetHoldAlt(ALTITUDE, 0);
// set integral to current RC throttle setting
pidZeroIntegral(navData.altSpeedPID, -VELOCITYD, motorsData.throttle * RADIO_MID_THROTTLE / MOTORS_SCALE);
pidZeroIntegral(navData.altPosPID, ALTITUDE, 0.0f);
navData.holdSpeedAlt = navData.targetHoldSpeedAlt = -VELOCITYD;
navData.holdMaxVertSpeed = NAV_DFLT_VRT_SPEED;
navData.mode = NAV_STATUS_ALTHOLD;
// notify ground
AQ_NOTICE("Altitude Hold engaged\n");
}
// are we not in position hold mode now?
if ((navData.navCapable || navUkfData.flowQuality > 0.0f) && reqFlightMode > NAV_STATUS_ALTHOLD && navData.mode != NAV_STATUS_POSHOLD && navData.mode != NAV_STATUS_DVH) {
// only zero bias if coming from lower mode
if (navData.mode < NAV_STATUS_POSHOLD) {
navData.holdTiltN = 0.0f;
navData.holdTiltE = 0.0f;
// speed
pidZeroIntegral(navData.speedNPID, UKF_VELN, 0.0f);
pidZeroIntegral(navData.speedEPID, UKF_VELE, 0.0f);
// pos
pidZeroIntegral(navData.distanceNPID, UKF_POSN, 0.0f);
pidZeroIntegral(navData.distanceEPID, UKF_POSE, 0.0f);
}
// store this position as hold position
navUkfSetHereAsPositionTarget();
if (navData.navCapable) {
// set this position as home if we have none
if (navData.homeLeg.targetLat == (double)0.0 || navData.homeLeg.targetLon == (double)0.0)
navSetHomeCurrent();
}
// activate pos hold
navData.mode = NAV_STATUS_POSHOLD;
navData.holdSpeedN = 0.0f;
navData.holdSpeedE = 0.0f;
navData.holdMaxHorizSpeed = NAV_DFLT_HOR_SPEED;
navData.holdMaxVertSpeed = NAV_DFLT_VRT_SPEED;
// notify ground
AQ_NOTICE("Position Hold engaged\n");
}
// DVH
else if ((navData.navCapable || navUkfData.flowQuality > 0.0f) && reqFlightMode == NAV_STATUS_DVH) {
navData.mode = NAV_STATUS_DVH;
}
// coming out of DVH mode?
else if (navData.mode == NAV_STATUS_DVH) {
// allow speed to drop before holding position (or if RTH engaged)
// FIXME: RTH switch may no longer be engaged but craft is still returning to home?
if ((UKF_VELN < +0.1f && UKF_VELN > -0.1f && UKF_VELE < +0.1f && UKF_VELE > -0.1f) || rcIsSwitchActive(NAV_CTRL_HOM_GO)) {
navUkfSetHereAsPositionTarget();
navData.mode = NAV_STATUS_POSHOLD;
}
}
}
// default to manual mode
else {
if (navData.mode != NAV_STATUS_MANUAL)
AQ_NOTICE("Manual mode active.\n");
navData.mode = NAV_STATUS_MANUAL;
// reset mission legs
navData.missionLeg = leg = 0;
// keep up with changing altitude
navSetHoldAlt(ALTITUDE, 0);
}
// ceiling set ?, 0 is disable
if (navData.ceilingAlt) {
// ceiling reached 1st time
if (ALTITUDE > navData.ceilingAlt && !navData.setCeilingFlag) {
navData.setCeilingFlag = 1;
navData.ceilingTimer = timerMicros();
}
// ceiling still reached for 5 seconds
else if (navData.setCeilingFlag && ALTITUDE > navData.ceilingAlt && (timerMicros() - navData.ceilingTimer) > (1e6 * 5) ) {
navData.ceilingTimer = timerMicros();
if (!navData.setCeilingReached)
AQ_NOTICE("Warning: Altitude ceiling reached.");
navData.setCeilingReached = 1;
}
else if ((navData.setCeilingFlag || navData.setCeilingReached) && ALTITUDE <= navData.ceilingAlt) {
if (navData.setCeilingReached)
AQ_NOTICE("Notice: Altitude returned to within ceiling.");
navData.setCeilingFlag = 0;
navData.setCeilingReached = 0;
}
}
if (!(supervisorData.state & STATE_RADIO_LOSS1))
navDoUserCommands(&leg, curLeg);
if (UKF_POSN != 0.0f || UKF_POSE != 0.0f) {
navData.holdCourse = compassNormalize(atan2f(-UKF_POSE, -UKF_POSN) * RAD_TO_DEG);
navData.holdDistance = __sqrtf(UKF_POSN*UKF_POSN + UKF_POSE*UKF_POSE);
}
else {
navData.holdCourse = 0.0f;
navData.holdDistance = 0.0f;
}
if (navData.mode == NAV_STATUS_MISSION) {
// have we arrived yet?
if (navData.loiterCompleteTime == 0) {
// are we close enough (distance and altitude)?
// goto/home test
if (((curLeg->type == NAV_LEG_GOTO || curLeg->type == NAV_LEG_HOME) &&
navData.holdDistance < curLeg->targetRadius &&
fabsf(navData.holdAlt - ALTITUDE) < curLeg->targetRadius) ||
// orbit test
(curLeg->type == NAV_LEG_ORBIT &&
fabsf(navData.holdDistance - curLeg->targetRadius) <= 2.0f &&
fabsf(navData.holdAlt - ALTITUDE) <= 1.0f) ||
// takeoff test
(curLeg->type == NAV_LEG_TAKEOFF &&
navData.holdDistance < curLeg->targetRadius &&
fabsf(navData.holdAlt - ALTITUDE) < curLeg->targetRadius)
) {
// freeze heading if relative, unless orbiting
if (curLeg->type != NAV_LEG_ORBIT && signbit(navData.targetHeading))
navData.targetHeading = AQ_YAW;
// start the loiter clock
navData.loiterCompleteTime = currentTime + curLeg->loiterTime;
AQ_PRINTF("NAV: Reached waypoint %d.\n", leg);
#ifdef USE_SIGNALING
signalingOnetimeEvent(SIG_EVENT_OT_WP_REACHED);
#endif
#ifdef USE_MAVLINK
// notify ground
mavlinkWpReached(leg);
#endif
}
}
// have we loitered long enough?
else if (currentTime > navData.loiterCompleteTime && curLeg->type != NAV_LEG_LAND) {
// next leg
leg = ++navData.missionLeg;
curLeg = navLoadLeg(leg);
if (!navData.hasMissionLeg)
navData.mode = NAV_STATUS_POSHOLD;
}
}
// DVH
if (navData.mode == NAV_STATUS_DVH) {
float factor = (1.0f / 500.0f) * navData.holdMaxHorizSpeed;
float x = 0.0f;
float y = 0.0f;
if (RADIO_PITCH > p[CTRL_DEAD_BAND])
x = -(RADIO_PITCH - p[CTRL_DEAD_BAND]) * factor;
if (RADIO_PITCH < -p[CTRL_DEAD_BAND])
x = -(RADIO_PITCH + p[CTRL_DEAD_BAND]) * factor;
if (RADIO_ROLL > p[CTRL_DEAD_BAND])
y = +(RADIO_ROLL - p[CTRL_DEAD_BAND]) * factor;
if (RADIO_ROLL < -p[CTRL_DEAD_BAND])
y = +(RADIO_ROLL + p[CTRL_DEAD_BAND]) * factor;
// do we want to ignore rotation of craft (headfree/carefree mode)?
if (navData.headFreeMode > NAV_HEADFREE_OFF) {
if (navData.hfUseStoredReference) {
// rotate to stored frame
navData.holdSpeedN = x * navData.hfReferenceCos - y * navData.hfReferenceSin;
navData.holdSpeedE = y * navData.hfReferenceCos + x * navData.hfReferenceSin;
}
else {
// don't rotate to any frame (pitch/roll move to N/S/E/W)
navData.holdSpeedN = x;
navData.holdSpeedE = y;
}
}
else {
// rotate to earth frame
navData.holdSpeedN = x * navUkfData.yawCos - y * navUkfData.yawSin;
navData.holdSpeedE = y * navUkfData.yawCos + x * navUkfData.yawSin;
}
}
// orbit POI
else if (navData.mode == NAV_STATUS_MISSION && curLeg->type == NAV_LEG_ORBIT) {
float velX, velY;
// maintain orbital radius
velX = -pidUpdate(navData.distanceNPID, curLeg->targetRadius, navData.holdDistance);
// maintain orbital velocity (clock wise)
velY = -curLeg->maxHorizSpeed;
// rotate to earth frame
navData.holdSpeedN = velX * navUkfData.yawCos - velY * navUkfData.yawSin;
navData.holdSpeedE = velY * navUkfData.yawCos + velX * navUkfData.yawSin;
}
else {
// distance => velocity
navData.holdSpeedN = pidUpdate(navData.distanceNPID, 0.0f, UKF_POSN);
navData.holdSpeedE = pidUpdate(navData.distanceEPID, 0.0f, UKF_POSE);
}
// normalize N/E speed requests to fit below max nav speed
tmp = __sqrtf(navData.holdSpeedN*navData.holdSpeedN + navData.holdSpeedE*navData.holdSpeedE);
if (tmp > navData.holdMaxHorizSpeed) {
navData.holdSpeedN = (navData.holdSpeedN / tmp) * navData.holdMaxHorizSpeed;
navData.holdSpeedE = (navData.holdSpeedE / tmp) * navData.holdMaxHorizSpeed;
}
// velocity => tilt
navData.holdTiltN = -pidUpdate(navData.speedNPID, navData.holdSpeedN, UKF_VELN);
navData.holdTiltE = +pidUpdate(navData.speedEPID, navData.holdSpeedE, UKF_VELE);
if (navData.mode > NAV_STATUS_MANUAL) {
float vertStick;
// Throttle controls vertical speed
vertStick = RADIO_THROT - RADIO_MID_THROTTLE;
if ((vertStick > p[CTRL_DBAND_THRO] && !navData.setCeilingReached) || vertStick < -p[CTRL_DBAND_THRO]) {
// altitude velocity proportional to throttle stick
navData.targetHoldSpeedAlt = (vertStick - p[CTRL_DBAND_THRO] * (vertStick > 0.0f ? 1.0f : -1.0f)) * NAV_DFLT_VRT_SPEED * (1.0f / RADIO_MID_THROTTLE);
navData.verticalOverride = 1;
}
// are we trying to land?
else if (navData.mode == NAV_STATUS_MISSION && curLeg->type == NAV_LEG_LAND) {
navData.targetHoldSpeedAlt = -navData.holdMaxVertSpeed;
}
// coming out of vertical override?
else if (navData.verticalOverride) {
navData.targetHoldSpeedAlt = 0.0f;
// slow down before trying to hold altitude
if (fabsf(VELOCITYD) < 0.025f)
navData.verticalOverride = 0;
// set new hold altitude to wherever we are while still in override
if (navData.mode != NAV_STATUS_MISSION)
navSetHoldAlt(ALTITUDE, 0);
}
// PID has the throttle
else {
navData.targetHoldSpeedAlt = pidUpdate(navData.altPosPID, navData.holdAlt, ALTITUDE);
}
// constrain vertical velocity
tmp = configGetParamValue(NAV_MAX_DECENT);
tmp = (navData.holdMaxVertSpeed < tmp) ? -navData.holdMaxVertSpeed : -tmp;
navData.targetHoldSpeedAlt = constrainFloat(navData.targetHoldSpeedAlt, tmp, navData.holdMaxVertSpeed);
// smooth vertical velocity changes
navData.holdSpeedAlt += (navData.targetHoldSpeedAlt - navData.holdSpeedAlt) * 0.05f;
}
else {
navData.verticalOverride = 0;
}
// calculate POI angle (used for tilt in gimbal function)
if (navData.mode == NAV_STATUS_MISSION && curLeg->poiAltitude != 0.0f) {
float a, b, c;
a = navData.holdDistance;
b = ALTITUDE - curLeg->poiAltitude;
c = __sqrtf(a*a + b*b);
navData.poiAngle = asinf(a/c) * RAD_TO_DEG - 90.0f;
}
else {
navData.poiAngle = 0.0f;
}
if (navData.mode == NAV_STATUS_MISSION) {
// recalculate autonomous heading
navSetHoldHeading(navData.targetHeading);
// wait for low throttle if landing
if (curLeg->type == NAV_LEG_LAND && motorsData.throttle <= 1)
// shut everything down (sure hope we are really on the ground :)
supervisorDisarm();
}
navData.lastUpdate = currentTime;
}
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 svd(float32_t *A, float32_t *S2, int n)
{
int i, j, k, EstColRank = n, RotCount = n, SweepCount = 0,
slimit = (n<120) ? 30 : n/4;
float32_t eps = 1e-7, e2 = 10.0*n*eps*eps, tol = 0.1*eps, vt, p, x0,
y0, q, r, c0, s0, d1, d2;
for (i=0; i<n; i++) {
for (j=0; j<n; j++) A[(n+i)*n + j] = 0.0;
A[(n+i)*n + i] = 1.0;
}
while (RotCount != 0 && SweepCount++ <= slimit) {
RotCount = EstColRank*(EstColRank-1)/2;
for (j=0; j<EstColRank-1; j++)
for (k=j+1; k<EstColRank; k++) {
p = q = r = 0.0;
for (i=0; i<n; i++) {
x0 = A[i*n + j];
y0 = A[i*n + k];
p += x0*y0;
q += x0*x0;
r += y0*y0;
}
S2[j] = q;
S2[k] = r;
if (q >= r) {
if (q<=e2*S2[0] || fabsf(p)<=tol*q)
RotCount--;
else {
p /= q;
r = 1.0-r/q;
vt = __sqrtf(4.0*p*p+r*r);
c0 = __sqrtf(0.5*(1.0+r/vt));
s0 = p/(vt*c0);
for (i=0; i<2*n; i++) {
d1 = A[i*n + j];
d2 = A[i*n + k];
A[i*n + j] = d1*c0+d2*s0;
A[i*n + k] = -d1*s0+d2*c0;
}
}
} else {
p /= r;
q = q/r-1.0;
vt = __sqrtf(4.0*p*p+q*q);
s0 = __sqrtf(0.5*(1.0-q/vt));
if (p<0.0) s0 = -s0;
c0 = p/(vt*s0);
for (i=0; i<2*n; i++) {
d1 = A[i*n + j];
d2 = A[i*n + k];
A[i*n + j] = d1*c0+d2*s0;
A[i*n + k] = -d1*s0+d2*c0;
}
}
}
while (EstColRank>2 && S2[EstColRank-1]<=S2[0]*tol+tol*tol) EstColRank--;
}
// if (SweepCount > slimit)
// printf("Warning: Reached maximum number of sweeps (%d) in SVD routine...\n"
// ,slimit);
}
