/*-----------------------------------------------------------
* Multicopter emergency landing
*-----------------------------------------------------------*/
static void applyMulticopterEmergencyLandingController(uint32_t currentTime)
{
static uint32_t previousTimeUpdate;
static uint32_t previousTimePositionUpdate;
uint32_t deltaMicros = currentTime - previousTimeUpdate;
previousTimeUpdate = currentTime;
/* Attempt to stabilise */
rcCommand[ROLL] = 0;
rcCommand[PITCH] = 0;
rcCommand[YAW] = 0;
if (posControl.flags.hasValidAltitudeSensor) {
/* We have an altitude reference, apply AH-based landing controller */
// If last position update was too long in the past - ignore it (likely restarting altitude controller)
if (deltaMicros > HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
previousTimeUpdate = currentTime;
previousTimePositionUpdate = currentTime;
resetMulticopterAltitudeController();
return;
}
if (posControl.flags.verticalPositionDataNew) {
uint32_t deltaMicrosPositionUpdate = currentTime - previousTimePositionUpdate;
previousTimePositionUpdate = currentTime;
// Check if last correction was too log ago - ignore this update
if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
updateAltitudeTargetFromClimbRate(-1.0f * posControl.navConfig->emerg_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET);
updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
}
else {
// due to some glitch position update has not occurred in time, reset altitude controller
resetMulticopterAltitudeController();
}
// Indicate that information is no longer usable
posControl.flags.verticalPositionDataConsumed = 1;
}
// Update throttle controller
rcCommand[THROTTLE] = constrain((int16_t)posControl.navConfig->mc_hover_throttle + posControl.rcAdjustment[THROTTLE], posControl.escAndServoConfig->minthrottle, posControl.escAndServoConfig->maxthrottle);
}
else {
/* Sensors has gone haywire, attempt to land regardless */
failsafeConfig_t * failsafeConfig = getActiveFailsafeConfig();
if (failsafeConfig) {
rcCommand[THROTTLE] = failsafeConfig->failsafe_throttle;
}
else {
rcCommand[THROTTLE] = posControl.escAndServoConfig->minthrottle;
}
}
}
static void applyMulticopterPositionController(uint32_t currentTime)
{
static uint32_t previousTimePositionUpdate; // Occurs @ GPS update rate
static uint32_t previousTimeUpdate; // Occurs @ looptime rate
uint32_t deltaMicros = currentTime - previousTimeUpdate;
previousTimeUpdate = currentTime;
bool bypassPositionController;
// We should passthrough rcCommand is adjusting position in GPS_ATTI mode
bypassPositionController = (posControl.navConfig->flags.user_control_mode == NAV_GPS_ATTI) && posControl.flags.isAdjustingPosition;
// If last call to controller was too long in the past - ignore it (likely restarting position controller)
if (deltaMicros > HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
previousTimeUpdate = currentTime;
previousTimePositionUpdate = currentTime;
resetMulticopterPositionController();
return;
}
// Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode
// and pilots input would be passed thru to PID controller
if (posControl.flags.hasValidPositionSensor) {
// If we have new position - update velocity and acceleration controllers
if (posControl.flags.horizontalPositionNewData) {
uint32_t deltaMicrosPositionUpdate = currentTime - previousTimePositionUpdate;
previousTimePositionUpdate = currentTime;
if (!bypassPositionController) {
// Update position controller
if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
updatePositionVelocityController_MC();
updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX);
}
else {
resetMulticopterPositionController();
}
}
// Indicate that information is no longer usable
posControl.flags.horizontalPositionNewData = 0;
}
}
else {
/* No position data, disable automatic adjustment, rcCommand passthrough */
posControl.rcAdjustment[PITCH] = 0;
posControl.rcAdjustment[ROLL] = 0;
bypassPositionController = true;
}
if (!bypassPositionController) {
rcCommand[PITCH] = leanAngleToRcCommand(posControl.rcAdjustment[PITCH]);
rcCommand[ROLL] = leanAngleToRcCommand(posControl.rcAdjustment[ROLL]);
}
}
int16_t applyFixedWingMinSpeedController(uint32_t currentTime)
{
static uint32_t previousTimePositionUpdate; // Occurs @ GPS update rate
static uint32_t previousTimeUpdate; // Occurs @ looptime rate
uint32_t deltaMicros = currentTime - previousTimeUpdate;
previousTimeUpdate = currentTime;
// If last position update was too long in the past - ignore it (likely restarting altitude controller)
if (deltaMicros > HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
previousTimeUpdate = currentTime;
previousTimePositionUpdate = currentTime;
throttleSpeedAdjustment = 0;
return 0;
}
// Apply controller only if position source is valid
if (posControl.flags.hasValidPositionSensor) {
// If we have new position - update velocity and acceleration controllers
if (posControl.flags.horizontalPositionDataNew) {
uint32_t deltaMicrosPositionUpdate = currentTime - previousTimePositionUpdate;
previousTimePositionUpdate = currentTime;
if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
float velThrottleBoost = (NAV_FW_MIN_VEL_SPEED_BOOST - posControl.actualState.velXY) * NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate);
// If we are in the deadband of 50cm/s - don't update speed boost
if (ABS(posControl.actualState.velXY - NAV_FW_MIN_VEL_SPEED_BOOST) > 50) {
throttleSpeedAdjustment += velThrottleBoost;
}
throttleSpeedAdjustment = constrainf(throttleSpeedAdjustment, 0.0f, 500.0f);
}
else {
throttleSpeedAdjustment = 0;
}
// Indicate that information is no longer usable
posControl.flags.horizontalPositionDataConsumed = 1;
}
}
else {
// No valid pos sensor data, we can't calculate speed
throttleSpeedAdjustment = 0;
}
return throttleSpeedAdjustment;
}
/**
* 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;
}
}
}
void applyFixedWingPositionController(uint32_t currentTime)
{
static uint32_t previousTimePositionUpdate; // Occurs @ GPS update rate
static uint32_t previousTimeUpdate; // Occurs @ looptime rate
uint32_t deltaMicros = currentTime - previousTimeUpdate;
previousTimeUpdate = currentTime;
// If last position update was too long in the past - ignore it (likely restarting altitude controller)
if (deltaMicros > HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
previousTimeUpdate = currentTime;
previousTimePositionUpdate = currentTime;
resetFixedWingPositionController();
return;
}
// Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode
if (posControl.flags.hasValidPositionSensor) {
// If we have new position - update velocity and acceleration controllers
if (posControl.flags.horizontalPositionDataNew) {
uint32_t deltaMicrosPositionUpdate = currentTime - previousTimePositionUpdate;
previousTimePositionUpdate = currentTime;
if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
// Calculate virtual position target at a distance of forwardVelocity * HZ2S(POSITION_TARGET_UPDATE_RATE_HZ)
// Account for pilot's roll input (move position target left/right at max of max_manual_speed)
// POSITION_TARGET_UPDATE_RATE_HZ should be chosen keeping in mind that position target shouldn't be reached until next pos update occurs
// FIXME: verify the above
calculateVirtualPositionTarget_FW(HZ2S(MIN_POSITION_UPDATE_RATE_HZ) * 2);
updatePositionHeadingController_FW(deltaMicrosPositionUpdate);
}
else {
resetFixedWingPositionController();
}
// Indicate that information is no longer usable
posControl.flags.horizontalPositionDataConsumed = 1;
}
isRollAdjustmentValid = true;
}
else {
// No valid pos sensor data, don't adjust pitch automatically, rcCommand[ROLL] is passed through to PID controller
isRollAdjustmentValid = false;
}
}
void applyFixedWingAltitudeController(uint32_t currentTime)
{
static uint32_t previousTimePositionUpdate; // Occurs @ altitude sensor update rate (max MAX_ALTITUDE_UPDATE_RATE_HZ)
static uint32_t previousTimeUpdate; // Occurs @ looptime rate
uint32_t deltaMicros = currentTime - previousTimeUpdate;
previousTimeUpdate = currentTime;
// If last time Z-controller was called is too far in the past - ignore it (likely restarting altitude controller)
if (deltaMicros > HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
previousTimeUpdate = currentTime;
previousTimePositionUpdate = currentTime;
resetFixedWingAltitudeController();
return;
}
if (posControl.flags.hasValidPositionSensor) {
// If we have an update on vertical position data - update velocity and accel targets
if (posControl.flags.verticalPositionDataNew) {
uint32_t deltaMicrosPositionUpdate = currentTime - previousTimePositionUpdate;
previousTimePositionUpdate = currentTime;
// Check if last correction was too log ago - ignore this update
if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
updateAltitudeVelocityAndPitchController_FW(deltaMicrosPositionUpdate);
}
else {
// due to some glitch position update has not occurred in time, reset altitude controller
resetFixedWingAltitudeController();
}
// Indicate that information is no longer usable
posControl.flags.verticalPositionDataConsumed = 1;
}
isPitchAdjustmentValid = true;
}
else {
// No valid altitude sensor data, don't adjust pitch automatically, rcCommand[PITCH] is passed through to PID controller
isPitchAdjustmentValid = false;
}
}
static void applyMulticopterAltitudeController(uint32_t currentTime)
{
static uint32_t previousTimePositionUpdate; // Occurs @ altitude sensor update rate (max MAX_ALTITUDE_UPDATE_RATE_HZ)
static uint32_t previousTimeUpdate; // Occurs @ looptime rate
uint32_t deltaMicros = currentTime - previousTimeUpdate;
previousTimeUpdate = currentTime;
// If last position update was too long in the past - ignore it (likely restarting altitude controller)
if (deltaMicros > HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
previousTimeUpdate = currentTime;
previousTimePositionUpdate = currentTime;
resetMulticopterAltitudeController();
return;
}
// If we have an update on vertical position data - update velocity and accel targets
if (posControl.flags.verticalPositionNewData) {
uint32_t deltaMicrosPositionUpdate = currentTime - previousTimePositionUpdate;
previousTimePositionUpdate = currentTime;
// Check if last correction was too log ago - ignore this update
if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
updateSurfaceTrackingAltitudeSetpoint_MC();
updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
}
else {
// due to some glitch position update has not occurred in time, reset altitude controller
resetMulticopterAltitudeController();
}
// Indicate that information is no longer usable
posControl.flags.verticalPositionNewData = 0;
}
// Update throttle controller
rcCommand[THROTTLE] = constrain((int16_t)posControl.navConfig->mc_hover_throttle + posControl.rcAdjustment[THROTTLE], posControl.escAndServoConfig->minthrottle, posControl.escAndServoConfig->maxthrottle);
// Save processed throttle for future use
rcCommandAdjustedThrottle = rcCommand[THROTTLE];
}
/* Why is this here: Because GPS will be sending at quiet a nailed rate (if not overloaded by junk tasks at the brink of its specs)
* but we might read out with timejitter because Irq might be off by a few us so we do a +-10% margin around the time between GPS
* datasets representing the most common Hz-rates today. You might want to extend the list or find a smarter way.
* Don't overload your GPS in its config with trash, choose a Hz rate that it can deliver at a sustained rate.
* (c) CrashPilot1000
*/
static uint32_t getGPSDeltaTimeFilter(uint32_t dTus)
{
if (dTus >= 225000 && dTus <= 275000) return HZ2US(4); // 4Hz Data 250ms
if (dTus >= 180000 && dTus <= 220000) return HZ2US(5); // 5Hz Data 200ms
if (dTus >= 90000 && dTus <= 110000) return HZ2US(10); // 10Hz Data 100ms
if (dTus >= 45000 && dTus <= 55000) return HZ2US(20); // 20Hz Data 50ms
if (dTus >= 30000 && dTus <= 36000) return HZ2US(30); // 30Hz Data 33ms
if (dTus >= 23000 && dTus <= 27000) return HZ2US(40); // 40Hz Data 25ms
if (dTus >= 18000 && dTus <= 22000) return HZ2US(50); // 50Hz Data 20ms
return dTus; // Filter failed. Set GPS Hz by measurement
}
/**
* Read BARO and update alt/vel topic
* Function is called at main loop rate, updates happen at reduced rate
*/
static void updateBaroTopic(uint32_t currentTime)
{
static navigationTimer_t baroUpdateTimer;
if (updateTimer(&baroUpdateTimer, HZ2US(INAV_BARO_UPDATE_RATE), currentTime)) {
float newBaroAlt = baroCalculateAltitude();
if (sensors(SENSOR_BARO) && isBaroCalibrationComplete()) {
posEstimator.baro.alt = newBaroAlt;
posEstimator.baro.epv = posControl.navConfig->inav.baro_epv;
posEstimator.baro.lastUpdateTime = currentTime;
}
else {
posEstimator.baro.alt = 0;
posEstimator.baro.lastUpdateTime = 0;
}
}
}
/**
* Examine estimation error and update navigation system if estimate is good enough
* Function is called at main loop rate, but updates happen less frequently - at a fixed rate
*/
static void publishEstimatedTopic(uint32_t currentTime)
{
static navigationTimer_t posPublishTimer;
/* IMU operates in decidegrees while INAV operates in deg*100 */
updateActualHeading(DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw));
/* Position and velocity are published with INAV_POSITION_PUBLISH_RATE_HZ */
if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTime)) {
/* Publish position update */
if (posEstimator.est.eph < posControl.navConfig->inav.max_eph_epv) {
updateActualHorizontalPositionAndVelocity(true, posEstimator.est.pos.V.X, posEstimator.est.pos.V.Y, posEstimator.est.vel.V.X, posEstimator.est.vel.V.Y);
}
else {
updateActualHorizontalPositionAndVelocity(false, posEstimator.est.pos.V.X, posEstimator.est.pos.V.Y, 0, 0);
}
/* Publish altitude update and set altitude validity */
if (posEstimator.est.epv < posControl.navConfig->inav.max_eph_epv) {
updateActualAltitudeAndClimbRate(true, posEstimator.est.pos.V.Z, posEstimator.est.vel.V.Z);
}
else {
updateActualAltitudeAndClimbRate(false, posEstimator.est.pos.V.Z, 0);
}
/* Publish surface distance */
if (posEstimator.est.surface > 0) {
updateActualSurfaceDistance(true, posEstimator.est.surface, posEstimator.est.surfaceVel);
}
else {
updateActualSurfaceDistance(false, -1, 0);
}
/* Store history data */
posEstimator.history.pos[posEstimator.history.index] = posEstimator.est.pos;
posEstimator.history.vel[posEstimator.history.index] = posEstimator.est.vel;
posEstimator.history.index++;
if (posEstimator.history.index >= INAV_HISTORY_BUF_SIZE) {
posEstimator.history.index = 0;
}
}
}
