/*
* TURN ASSISTANT mode is an assisted mode to do a Yaw rotation on a ground plane, allowing one-stick turn in RATE more
* and keeping ROLL and PITCH attitude though the turn.
*/
static void pidTurnAssistant(pidState_t *pidState)
{
t_fp_vector targetRates;
targetRates.V.X = 0.0f;
targetRates.V.Y = 0.0f;
targetRates.V.Z = pidState[YAW].rateTarget;
imuTransformVectorEarthToBody(&targetRates);
// Add in roll and pitch, replace yaw completery
pidState[ROLL].rateTarget += targetRates.V.X;
pidState[PITCH].rateTarget += targetRates.V.Y;
pidState[YAW].rateTarget = targetRates.V.Z;
}
/**
* Calculate next estimate using IMU and apply corrections from reference sensors (GPS, BARO etc)
* Function is called at main loop rate
*/
static void updateEstimatedTopic(uint32_t currentTime)
{
t_fp_vector accelBiasCorr;
float dt = US2S(currentTime - posEstimator.est.lastUpdateTime);
posEstimator.est.lastUpdateTime = currentTime;
/* If IMU is not ready we can't estimate anything */
if (!isImuReady()) {
posEstimator.est.eph = posControl.navConfig->inav.max_eph_epv + 0.001f;
posEstimator.est.epv = posControl.navConfig->inav.max_eph_epv + 0.001f;
return;
}
/* increase EPH/EPV on each iteration */
if (posEstimator.est.eph <= posControl.navConfig->inav.max_eph_epv) {
posEstimator.est.eph *= 1.0f + dt;
}
if (posEstimator.est.epv <= posControl.navConfig->inav.max_eph_epv) {
posEstimator.est.epv *= 1.0f + dt;
}
/* Figure out if we have valid position data from our data sources */
bool isGPSValid = sensors(SENSOR_GPS) && posControl.gpsOrigin.valid && ((currentTime - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS));
bool isBaroValid = sensors(SENSOR_BARO) && ((currentTime - posEstimator.baro.lastUpdateTime) <= MS2US(INAV_BARO_TIMEOUT_MS));
bool isSonarValid = sensors(SENSOR_SONAR) && ((currentTime - posEstimator.sonar.lastUpdateTime) <= MS2US(INAV_SONAR_TIMEOUT_MS));
/* Do some preparations to data */
if (isBaroValid) {
if (!ARMING_FLAG(ARMED)) {
posEstimator.state.baroGroundAlt = posEstimator.est.pos.V.Z;
posEstimator.state.isBaroGroundValid = true;
posEstimator.state.baroGroundTimeout = currentTime + 250000; // 0.25 sec
}
else {
if (posEstimator.est.vel.V.Z > 15) {
if (currentTime > posEstimator.state.baroGroundTimeout) {
posEstimator.state.isBaroGroundValid = false;
}
}
else {
posEstimator.state.baroGroundTimeout = currentTime + 250000; // 0.25 sec
}
}
}
else {
posEstimator.state.isBaroGroundValid = false;
}
/* We might be experiencing air cushion effect - use sonar or baro groung altitude to detect it */
bool isAirCushionEffectDetected = ARMING_FLAG(ARMED) &&
((isSonarValid && posEstimator.sonar.alt < 20.0f && posEstimator.state.isBaroGroundValid) ||
(isBaroValid && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt));
#if defined(NAV_GPS_GLITCH_DETECTION)
//isGPSValid = isGPSValid && !posEstimator.gps.glitchDetected;
#endif
/* Apply GPS altitude corrections only on fixed wing aircrafts */
bool useGpsZ = STATE(FIXED_WING) && isGPSValid;
/* Pre-calculate history index for GPS delay compensation */
int gpsHistoryIndex = (posEstimator.history.index - 1) - constrain(((int)posControl.navConfig->inav.gps_delay_ms / (1000 / INAV_POSITION_PUBLISH_RATE_HZ)), 0, INAV_HISTORY_BUF_SIZE - 1);
if (gpsHistoryIndex < 0) {
gpsHistoryIndex += INAV_HISTORY_BUF_SIZE;
}
/* Correct accelerometer bias */
if (posControl.navConfig->inav.w_acc_bias > 0) {
accelBiasCorr.V.X = 0;
accelBiasCorr.V.Y = 0;
accelBiasCorr.V.Z = 0;
/* accelerometer bias correction for GPS */
if (isGPSValid) {
accelBiasCorr.V.X -= (posEstimator.gps.pos.V.X - posEstimator.history.pos[gpsHistoryIndex].V.X) * sq(posControl.navConfig->inav.w_xy_gps_p);
accelBiasCorr.V.X -= (posEstimator.gps.vel.V.X - posEstimator.history.vel[gpsHistoryIndex].V.X) * posControl.navConfig->inav.w_xy_gps_v;
accelBiasCorr.V.Y -= (posEstimator.gps.pos.V.Y - posEstimator.history.pos[gpsHistoryIndex].V.Y) * sq(posControl.navConfig->inav.w_xy_gps_p);
accelBiasCorr.V.Y -= (posEstimator.gps.vel.V.Y - posEstimator.history.vel[gpsHistoryIndex].V.Y) * posControl.navConfig->inav.w_xy_gps_v;
if (useGpsZ) {
accelBiasCorr.V.Z -= (posEstimator.gps.pos.V.Z - posEstimator.history.pos[gpsHistoryIndex].V.Z) * sq(posControl.navConfig->inav.w_z_gps_p);
accelBiasCorr.V.Z -= (posEstimator.gps.vel.V.Z - posEstimator.history.vel[gpsHistoryIndex].V.Z) * posControl.navConfig->inav.w_z_gps_v;
}
}
/* accelerometer bias correction for baro */
if (isBaroValid && !isAirCushionEffectDetected) {
accelBiasCorr.V.Z -= (posEstimator.baro.alt - posEstimator.est.pos.V.Z) * sq(posControl.navConfig->inav.w_z_baro_p);
}
/* transform error vector from NEU frame to body frame */
imuTransformVectorEarthToBody(&accelBiasCorr);
/* Correct accel bias */
posEstimator.imu.accelBias.V.X += accelBiasCorr.V.X * posControl.navConfig->inav.w_acc_bias * dt;
posEstimator.imu.accelBias.V.Y += accelBiasCorr.V.Y * posControl.navConfig->inav.w_acc_bias * dt;
posEstimator.imu.accelBias.V.Z += accelBiasCorr.V.Z * posControl.navConfig->inav.w_acc_bias * dt;
}
/* Estimate Z-axis */
if ((posEstimator.est.epv < posControl.navConfig->inav.max_eph_epv) || useGpsZ || isBaroValid) {
/* Predict position/velocity based on acceleration */
inavFilterPredict(Z, dt, posEstimator.imu.accelNEU.V.Z);
#if defined(BARO)
if (isBaroValid) {
float baroError = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.V.Z;
/* Apply only baro correction, no sonar */
inavFilterCorrectPos(Z, dt, baroError, posControl.navConfig->inav.w_z_baro_p);
/* Adjust EPV */
posEstimator.est.epv = MIN(posEstimator.est.epv, posEstimator.baro.epv);
}
#endif
/* Apply GPS correction to altitude */
if (useGpsZ) {
inavFilterCorrectPos(Z, dt, posEstimator.gps.pos.V.Z - posEstimator.history.pos[gpsHistoryIndex].V.Z, posControl.navConfig->inav.w_z_gps_p);
inavFilterCorrectVel(Z, dt, posEstimator.gps.vel.V.Z - posEstimator.history.vel[gpsHistoryIndex].V.Z, posControl.navConfig->inav.w_z_gps_v);
/* Adjust EPV */
posEstimator.est.epv = MIN(posEstimator.est.epv, posEstimator.gps.epv);
}
}
else {
inavFilterCorrectVel(Z, dt, 0.0f - posEstimator.est.vel.V.Z, posControl.navConfig->inav.w_z_res_v);
}
/* Estimate XY-axis only if heading is valid (X-Y acceleration is North-East)*/
if ((posEstimator.est.eph < posControl.navConfig->inav.max_eph_epv) || isGPSValid) {
if (isImuHeadingValid()) {
inavFilterPredict(X, dt, posEstimator.imu.accelNEU.V.X);
inavFilterPredict(Y, dt, posEstimator.imu.accelNEU.V.Y);
}
/* Correct position from GPS - always if GPS is valid */
if (isGPSValid) {
inavFilterCorrectPos(X, dt, posEstimator.gps.pos.V.X - posEstimator.history.pos[gpsHistoryIndex].V.X, posControl.navConfig->inav.w_xy_gps_p);
inavFilterCorrectPos(Y, dt, posEstimator.gps.pos.V.Y - posEstimator.history.pos[gpsHistoryIndex].V.Y, posControl.navConfig->inav.w_xy_gps_p);
inavFilterCorrectVel(X, dt, posEstimator.gps.vel.V.X - posEstimator.history.vel[gpsHistoryIndex].V.X, posControl.navConfig->inav.w_xy_gps_v);
inavFilterCorrectVel(Y, dt, posEstimator.gps.vel.V.Y - posEstimator.history.vel[gpsHistoryIndex].V.Y, posControl.navConfig->inav.w_xy_gps_v);
/* Adjust EPH */
posEstimator.est.eph = MIN(posEstimator.est.eph, posEstimator.gps.eph);
}
}
else {
inavFilterCorrectVel(X, dt, 0.0f - posEstimator.est.vel.V.X, posControl.navConfig->inav.w_xy_res_v);
inavFilterCorrectVel(Y, dt, 0.0f - posEstimator.est.vel.V.Y, posControl.navConfig->inav.w_xy_res_v);
}
/* Surface offset */
#if defined(SONAR)
if (isSonarValid) {
posEstimator.est.surface = posEstimator.sonar.alt;
posEstimator.est.surfaceVel = posEstimator.sonar.vel;
}
else {
posEstimator.est.surface = -1;
posEstimator.est.surfaceVel = 0;
}
#else
posEstimator.est.surface = -1;
posEstimator.est.surfaceVel = 0;
#endif
}