/**
* Read sonar and update alt/vel topic
* Function is called at main loop rate, updates happen at reduced rate
*/
static void updateSonarTopic(uint32_t currentTime)
{
static navigationTimer_t sonarUpdateTimer;
if (updateTimer(&sonarUpdateTimer, HZ2US(INAV_SONAR_UPDATE_RATE), currentTime)) {
if (sensors(SENSOR_SONAR)) {
/* Read sonar */
float newSonarAlt = rangefinderRead();
newSonarAlt = rangefinderCalculateAltitude(newSonarAlt, calculateCosTiltAngle());
/* Apply predictive filter to sonar readings (inspired by PX4Flow) */
if (newSonarAlt > 0 && newSonarAlt <= INAV_SONAR_MAX_DISTANCE) {
float sonarPredVel, sonarPredAlt;
float sonarDt = (currentTime - posEstimator.sonar.lastUpdateTime) * 1e-6;
posEstimator.sonar.lastUpdateTime = currentTime;
sonarPredVel = (sonarDt < 0.25f) ? posEstimator.sonar.vel : 0.0f;
sonarPredAlt = posEstimator.sonar.alt + sonarPredVel * sonarDt;
posEstimator.sonar.alt = sonarPredAlt + INAV_SONAR_W1 * (newSonarAlt - sonarPredAlt);
posEstimator.sonar.vel = sonarPredVel + INAV_SONAR_W2 * (newSonarAlt - sonarPredAlt);
}
}
else {
/* No sonar */
posEstimator.sonar.alt = 0;
posEstimator.sonar.vel = 0;
posEstimator.sonar.lastUpdateTime = 0;
}
}
}
static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs)
{
const float swingVelocity = (ABS(imuMeasuredRotationBF.A[Z]) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.A[Y] / imuMeasuredRotationBF.A[Z]) : 0;
const bool isForwardAccelerationHigh = (imuMeasuredAccelBF.A[X] > navConfig()->fw.launch_accel_thresh);
const bool isAircraftAlmostLevel = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle)));
const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel;
const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.A[X] > 0);
if (isBungeeLaunched || isSwingLaunched) {
launchState.launchDetectionTimeAccum += (currentTimeUs - launchState.launchDetectorPreviosUpdate);
launchState.launchDetectorPreviosUpdate = currentTimeUs;
if (launchState.launchDetectionTimeAccum >= MS2US((uint32_t)navConfig()->fw.launch_time_thresh)) {
launchState.launchDetected = true;
}
}
else {
launchState.launchDetectorPreviosUpdate = currentTimeUs;
launchState.launchDetectionTimeAccum = 0;
}
}
