Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Multicopter emergency landing improvement/fix #9169

Merged
merged 5 commits into from
Aug 11, 2023
Merged

Multicopter emergency landing improvement/fix #9169

Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
a24828b
Update navigation_multicopter.c
breadoven Jul 11, 2023
74635e0
Update navigation_multicopter.c
breadoven Jul 11, 2023
4fc2625
Update navigation_multicopter.c
breadoven Jul 24, 2023
ad9d912
Update navigation_multicopter.c
breadoven Aug 10, 2023
fcc4ad0
clarify naming logic
breadoven Aug 10, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
33 changes: 14 additions & 19 deletions src/main/navigation/navigation_multicopter.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -113,16 +113,15 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros)
{
// Calculate min and max throttle boundaries (to compensate for integral windup)
const int16_t thrAdjustmentMin = (int16_t)getThrottleIdleValue() - (int16_t)currentBatteryProfile->nav.mc.hover_throttle;
const int16_t thrAdjustmentMax = (int16_t)motorConfig()->maxthrottle - (int16_t)currentBatteryProfile->nav.mc.hover_throttle;
const int16_t thrCorrectionMin = getThrottleIdleValue() - currentBatteryProfile->nav.mc.hover_throttle;
const int16_t thrCorrectionMax = motorConfig()->maxthrottle - currentBatteryProfile->nav.mc.hover_throttle;

float velocity_controller = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0);
float velocity_controller = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrCorrectionMin, thrCorrectionMax, 0);

posControl.rcAdjustment[THROTTLE] = pt1FilterApply4(&altholdThrottleFilterState, velocity_controller, NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
int16_t rcThrottleCorrection = pt1FilterApply4(&altholdThrottleFilterState, velocity_controller, NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
rcThrottleCorrection = constrain(rcThrottleCorrection, thrCorrectionMin, thrCorrectionMax);

posControl.rcAdjustment[THROTTLE] = constrain(posControl.rcAdjustment[THROTTLE], thrAdjustmentMin, thrAdjustmentMax);

posControl.rcAdjustment[THROTTLE] = constrain((int16_t)currentBatteryProfile->nav.mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle);
posControl.rcAdjustment[THROTTLE] = constrain(currentBatteryProfile->nav.mc.hover_throttle + rcThrottleCorrection, getThrottleIdleValue(), motorConfig()->maxthrottle);
}

bool adjustMulticopterAltitudeFromRCInput(void)
Expand Down Expand Up @@ -213,7 +212,7 @@ void resetMulticopterAltitudeController(void)
navPidReset(&posControl.pids.vel[Z]);
navPidReset(&posControl.pids.surface);

posControl.rcAdjustment[THROTTLE] = 0;
posControl.rcAdjustment[THROTTLE] = currentBatteryProfile->nav.mc.hover_throttle;

posControl.desiredState.vel.z = posToUse->vel.z; // Gradually transition from current climb

Expand Down Expand Up @@ -869,20 +868,19 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)

/* Attempt to stabilise */
rcCommand[YAW] = 0;
rcCommand[ROLL] = 0;
rcCommand[PITCH] = 0;
rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;

if ((posControl.flags.estAltStatus < EST_USABLE)) {
/* Sensors has gone haywire, attempt to land regardless */
/* Altitude sensors gone haywire, attempt to land regardless */
if (posControl.flags.estAltStatus < EST_USABLE) {
if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_DROP_IT) {
rcCommand[THROTTLE] = getThrottleIdleValue();
}
else {
rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
}

return;
}

// Normal sensor data
// Normal sensor data available, use controlled landing descent
if (posControl.flags.verticalPositionDataNew) {
const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
previousTimePositionUpdate = currentTimeUs;
Expand All @@ -902,15 +900,12 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
posControl.flags.verticalPositionDataConsumed = true;
}

// Update throttle controller
// Update throttle
rcCommand[THROTTLE] = posControl.rcAdjustment[THROTTLE];

// Hold position if possible
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
applyMulticopterPositionController(currentTimeUs);
} else {
rcCommand[ROLL] = 0;
rcCommand[PITCH] = 0;
}
}

Expand Down