mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-18 22:05:15 +03:00
[NAV] Cleanup pos/vel controllers
This commit is contained in:
Konstantin (DigitalEntity) Sharlaimov
parent
f83b54beaa
commit
d2f05b7e23
2 changed files with 51 additions and 115 deletions
|
|
@ -158,19 +158,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
|
|||
|
||||
void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs)
|
||||
{
|
||||
static timeUs_t previousTimePositionUpdate; // Occurs @ altitude sensor update rate (max MAX_ALTITUDE_UPDATE_RATE_HZ)
|
||||
static timeUs_t previousTimeUpdate; // Occurs @ looptime rate
|
||||
|
||||
const timeDelta_t deltaMicros = currentTimeUs - previousTimeUpdate;
|
||||
previousTimeUpdate = currentTimeUs;
|
||||
|
||||
// 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 = currentTimeUs;
|
||||
previousTimePositionUpdate = currentTimeUs;
|
||||
resetFixedWingAltitudeController();
|
||||
return;
|
||||
}
|
||||
static timeUs_t previousTimePositionUpdate = 0; // Occurs @ altitude sensor update rate (max MAX_ALTITUDE_UPDATE_RATE_HZ)
|
||||
|
||||
if ((posControl.flags.estAltStatus >= EST_USABLE)) {
|
||||
if (posControl.flags.verticalPositionDataNew) {
|
||||
|
|
@ -182,7 +170,7 @@ void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs)
|
|||
updateAltitudeVelocityAndPitchController_FW(deltaMicrosPositionUpdate);
|
||||
}
|
||||
else {
|
||||
// due to some glitch position update has not occurred in time, reset altitude controller
|
||||
// Position update has not occurred in time (first iteration or glitch), reset altitude controller
|
||||
resetFixedWingAltitudeController();
|
||||
}
|
||||
|
||||
|
|
@ -398,19 +386,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
|
|||
|
||||
void applyFixedWingPositionController(timeUs_t currentTimeUs)
|
||||
{
|
||||
static timeUs_t previousTimePositionUpdate; // Occurs @ GPS update rate
|
||||
static timeUs_t previousTimeUpdate; // Occurs @ looptime rate
|
||||
|
||||
const timeDelta_t deltaMicros = currentTimeUs - previousTimeUpdate;
|
||||
previousTimeUpdate = currentTimeUs;
|
||||
|
||||
// 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 = currentTimeUs;
|
||||
previousTimePositionUpdate = currentTimeUs;
|
||||
resetFixedWingPositionController();
|
||||
return;
|
||||
}
|
||||
static timeUs_t previousTimePositionUpdate = 0; // Occurs @ GPS update rate
|
||||
|
||||
// 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.estPosStatus >= EST_USABLE)) {
|
||||
|
|
@ -429,6 +405,7 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs)
|
|||
updatePositionHeadingController_FW(currentTimeUs, deltaMicrosPositionUpdate);
|
||||
}
|
||||
else {
|
||||
// Position update has not occurred in time (first iteration or glitch), reset altitude controller
|
||||
resetFixedWingPositionController();
|
||||
}
|
||||
|
||||
|
|
@ -448,19 +425,7 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs)
|
|||
|
||||
int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
|
||||
{
|
||||
static timeUs_t previousTimePositionUpdate; // Occurs @ GPS update rate
|
||||
static timeUs_t previousTimeUpdate; // Occurs @ looptime rate
|
||||
|
||||
const timeDelta_t deltaMicros = currentTimeUs - previousTimeUpdate;
|
||||
previousTimeUpdate = currentTimeUs;
|
||||
|
||||
// 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 = currentTimeUs;
|
||||
previousTimePositionUpdate = currentTimeUs;
|
||||
throttleSpeedAdjustment = 0;
|
||||
return 0;
|
||||
}
|
||||
static timeUs_t previousTimePositionUpdate = 0; // Occurs @ GPS update rate
|
||||
|
||||
// Apply controller only if position source is valid
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
|
||||
|
|
@ -480,6 +445,7 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
|
|||
throttleSpeedAdjustment = constrainf(throttleSpeedAdjustment, 0.0f, 500.0f);
|
||||
}
|
||||
else {
|
||||
// Position update has not occurred in time (first iteration or glitch), reset altitude controller
|
||||
throttleSpeedAdjustment = 0;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -199,35 +199,13 @@ void resetMulticopterAltitudeController(void)
|
|||
navPidReset(&posControl.pids.surface);
|
||||
posControl.rcAdjustment[THROTTLE] = 0;
|
||||
|
||||
if (prepareForTakeoffOnReset) {
|
||||
/* If we are preparing for takeoff - start with lowset possible climb rate, adjust alt target and make sure throttle doesn't jump */
|
||||
posControl.desiredState.vel.z = -navConfig()->general.max_manual_climb_rate;
|
||||
posControl.desiredState.pos.z = posToUse->pos.z - (navConfig()->general.max_manual_climb_rate / posControl.pids.pos[Z].param.kP);
|
||||
posControl.pids.vel[Z].integrator = -500.0f;
|
||||
pt1FilterReset(&altholdThrottleFilterState, -500.0f);
|
||||
prepareForTakeoffOnReset = false;
|
||||
}
|
||||
else {
|
||||
posControl.desiredState.vel.z = posToUse->vel.z; // Gradually transition from current climb
|
||||
pt1FilterReset(&altholdThrottleFilterState, 0.0f);
|
||||
}
|
||||
}
|
||||
|
||||
static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
|
||||
{
|
||||
static timeUs_t previousTimePositionUpdate; // Occurs @ altitude sensor update rate (max MAX_ALTITUDE_UPDATE_RATE_HZ)
|
||||
static timeUs_t previousTimeUpdate; // Occurs @ looptime rate
|
||||
|
||||
const timeDelta_t deltaMicros = currentTimeUs - previousTimeUpdate;
|
||||
previousTimeUpdate = currentTimeUs;
|
||||
|
||||
// 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 = currentTimeUs;
|
||||
previousTimePositionUpdate = currentTimeUs;
|
||||
resetMulticopterAltitudeController();
|
||||
return;
|
||||
}
|
||||
static timeUs_t previousTimePositionUpdate = 0; // Occurs @ altitude sensor update rate (max MAX_ALTITUDE_UPDATE_RATE_HZ)
|
||||
|
||||
// If we have an update on vertical position data - update velocity and accel targets
|
||||
if (posControl.flags.verticalPositionDataNew) {
|
||||
|
|
@ -236,11 +214,23 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
|
|||
|
||||
// Check if last correction was too log ago - ignore this update
|
||||
if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
|
||||
// If we are preparing for takeoff - start with lowset possible climb rate, adjust alt target and make sure throttle doesn't jump
|
||||
if (prepareForTakeoffOnReset) {
|
||||
const navEstimatedPosVel_t * posToUse = navGetCurrentActualPositionAndVelocity();
|
||||
|
||||
posControl.desiredState.vel.z = -navConfig()->general.max_manual_climb_rate;
|
||||
posControl.desiredState.pos.z = posToUse->pos.z - (navConfig()->general.max_manual_climb_rate / posControl.pids.pos[Z].param.kP);
|
||||
posControl.pids.vel[Z].integrator = -500.0f;
|
||||
pt1FilterReset(&altholdThrottleFilterState, -500.0f);
|
||||
prepareForTakeoffOnReset = false;
|
||||
}
|
||||
|
||||
// Execute actual altitude controllers
|
||||
updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
|
||||
updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
|
||||
}
|
||||
else {
|
||||
// due to some glitch position update has not occurred in time, reset altitude controller
|
||||
// Position update has not occurred in time (first start or glitch), reset altitude controller
|
||||
resetMulticopterAltitudeController();
|
||||
}
|
||||
|
||||
|
|
@ -625,24 +615,12 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
|
|||
|
||||
static void applyMulticopterPositionController(timeUs_t currentTimeUs)
|
||||
{
|
||||
static timeUs_t previousTimePositionUpdate; // Occurs @ GPS update rate
|
||||
static timeUs_t previousTimeUpdate; // Occurs @ looptime rate
|
||||
|
||||
const timeDelta_t deltaMicros = currentTimeUs - previousTimeUpdate;
|
||||
previousTimeUpdate = currentTimeUs;
|
||||
static timeUs_t previousTimePositionUpdate = 0; // Occurs @ GPS update rate
|
||||
bool bypassPositionController;
|
||||
|
||||
// We should passthrough rcCommand is adjusting position in GPS_ATTI mode
|
||||
bypassPositionController = (navConfig()->general.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 = currentTimeUs;
|
||||
previousTimePositionUpdate = currentTimeUs;
|
||||
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.estPosStatus >= EST_USABLE)) {
|
||||
|
|
@ -660,6 +638,7 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs)
|
|||
updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX, maxSpeed);
|
||||
}
|
||||
else {
|
||||
// Position update has not occurred in time (first start or glitch), reset altitude controller
|
||||
resetMulticopterPositionController();
|
||||
}
|
||||
}
|
||||
|
|
@ -749,25 +728,26 @@ bool isMulticopterLandingDetected(void)
|
|||
*-----------------------------------------------------------*/
|
||||
static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
|
||||
{
|
||||
static timeUs_t previousTimeUpdate;
|
||||
static timeUs_t previousTimePositionUpdate;
|
||||
const timeDelta_t deltaMicros = currentTimeUs - previousTimeUpdate;
|
||||
previousTimeUpdate = currentTimeUs;
|
||||
static timeUs_t previousTimePositionUpdate = 0;
|
||||
|
||||
/* Attempt to stabilise */
|
||||
rcCommand[ROLL] = 0;
|
||||
rcCommand[PITCH] = 0;
|
||||
rcCommand[YAW] = 0;
|
||||
|
||||
if ((posControl.flags.estAltStatus >= EST_USABLE)) {
|
||||
// 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 = currentTimeUs;
|
||||
previousTimePositionUpdate = currentTimeUs;
|
||||
resetMulticopterAltitudeController();
|
||||
if ((posControl.flags.estAltStatus < EST_USABLE)) {
|
||||
/* Sensors has gone haywire, attempt to land regardless */
|
||||
if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_DROP_IT) {
|
||||
rcCommand[THROTTLE] = getThrottleIdleValue();
|
||||
}
|
||||
else {
|
||||
rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
// Normal sensor data
|
||||
if (posControl.flags.verticalPositionDataNew) {
|
||||
const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
|
||||
previousTimePositionUpdate = currentTimeUs;
|
||||
|
|
@ -789,16 +769,6 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
|
|||
|
||||
// Update throttle controller
|
||||
rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle);
|
||||
}
|
||||
else {
|
||||
/* Sensors has gone haywire, attempt to land regardless */
|
||||
if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_DROP_IT) {
|
||||
rcCommand[THROTTLE] = getThrottleIdleValue();
|
||||
}
|
||||
else {
|
||||
rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue