Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

fix multirotor velocity z

Browse source
This commit is contained in:
breadoven 2023-11-10 22:29:19 +00:00
parent a8079c2842
commit 1fcac6134a
4 changed files with 6 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/navigation/navigation.c
View file

@ -3086,6 +3086,8 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
posControl.desiredState.pos.z = navGetCurrentActualPositionAndVelocity()->pos.z;
} else if (mode == ROC_TO_ALT_TARGET) {
posControl.desiredState.pos.z = targetAltitude;
} else { // ROC_TO_ALT_CONSTANT
posControl.desiredState.climbRateDemand = desiredClimbRate;
}
posControl.flags.rocToAltMode = mode;
@ -3097,8 +3099,6 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
posControl.desiredState.pos.z = MIN(posControl.desiredState.pos.z, navConfig()->general.max_altitude);
posControl.flags.rocToAltMode = ROC_TO_ALT_TARGET;
}
posControl.desiredState.vel.z = desiredClimbRate; // only used for ROC_TO_ALT_CONSTANT
}
static void resetAltitudeController(bool useTerrainFollowing)

2
src/main/navigation/navigation_fixedwing.c
View file

@ -134,7 +134,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
{
static pt1Filter_t velzFilterState;
float desiredClimbRate = posControl.desiredState.vel.z;
float desiredClimbRate = posControl.desiredState.climbRateDemand;
if (posControl.flags.rocToAltMode != ROC_TO_ALT_CONSTANT) {
desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);

3
src/main/navigation/navigation_multicopter.c
View file

@ -76,7 +76,7 @@ float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros)
// Position to velocity controller for Z axis
static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
{
float targetVel = posControl.desiredState.vel.z;
float targetVel = posControl.desiredState.climbRateDemand;
if (posControl.flags.rocToAltMode != ROC_TO_ALT_CONSTANT) {
targetVel = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
@ -249,6 +249,7 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
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);

1
src/main/navigation/navigation_private.h
View file

@ -140,6 +140,7 @@ typedef struct {
fpVector3_t pos;
fpVector3_t vel;
int32_t yaw;
int16_t climbRateDemand;
} navigationDesiredState_t;
typedef enum {