/** * 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; } }