Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 01:05:21 +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; posControl.desiredState.pos.z = navGetCurrentActualPositionAndVelocity()->pos.z;
} else if (mode == ROC_TO_ALT_TARGET) { } else if (mode == ROC_TO_ALT_TARGET) {
posControl.desiredState.pos.z = targetAltitude; posControl.desiredState.pos.z = targetAltitude;
} else { // ROC_TO_ALT_CONSTANT
posControl.desiredState.climbRateDemand = desiredClimbRate;
} }
posControl.flags.rocToAltMode = mode; 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.desiredState.pos.z = MIN(posControl.desiredState.pos.z, navConfig()->general.max_altitude);
posControl.flags.rocToAltMode = ROC_TO_ALT_TARGET; posControl.flags.rocToAltMode = ROC_TO_ALT_TARGET;
} }
posControl.desiredState.vel.z = desiredClimbRate; // only used for ROC_TO_ALT_CONSTANT
} }
static void resetAltitudeController(bool useTerrainFollowing) 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; static pt1Filter_t velzFilterState;
float desiredClimbRate = posControl.desiredState.vel.z; float desiredClimbRate = posControl.desiredState.climbRateDemand;
if (posControl.flags.rocToAltMode != ROC_TO_ALT_CONSTANT) { if (posControl.flags.rocToAltMode != ROC_TO_ALT_CONSTANT) {
desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros); 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 // Position to velocity controller for Z axis
static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) 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) { if (posControl.flags.rocToAltMode != ROC_TO_ALT_CONSTANT) {
targetVel = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros); targetVel = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
@ -249,6 +249,7 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
if (prepareForTakeoffOnReset) { if (prepareForTakeoffOnReset) {
const navEstimatedPosVel_t *posToUse = navGetCurrentActualPositionAndVelocity(); 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.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; posControl.pids.vel[Z].integrator = -500.0f;
pt1FilterReset(&altholdThrottleFilterState, -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 pos;
fpVector3_t vel; fpVector3_t vel;
int32_t yaw; int32_t yaw;
int16_t climbRateDemand;
} navigationDesiredState_t; } navigationDesiredState_t;
typedef enum { typedef enum {