diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 9fc428d114..d672bc1112 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -238,6 +238,8 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = { {"navTgtPos", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"navTgtPos", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"navTgtPos", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"navSurf", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, + {"navTgtSurf", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"navDebug", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"navDebug", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, {"navDebug", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, @@ -329,6 +331,8 @@ typedef struct blackboxMainState_s { int16_t navTargetPos[XYZ_AXIS_COUNT]; int16_t navHeading; int16_t navTargetHeading; + int16_t navSurface; + int16_t navTargetSurface; int16_t navDebug[4]; #endif } blackboxMainState_t; @@ -625,6 +629,9 @@ static void writeIntraframe(void) blackboxWriteSignedVB(blackboxCurrent->navTargetPos[x]); } + blackboxWriteSignedVB(blackboxCurrent->navSurface); + blackboxWriteSignedVB(blackboxCurrent->navTargetSurface); + for (x = 0; x < 4; x++) { blackboxWriteSignedVB(blackboxCurrent->navDebug[x]); } @@ -770,6 +777,9 @@ static void writeInterframe(void) blackboxWriteSignedVB(blackboxHistory[0]->navTargetPos[x] - (blackboxHistory[1]->navTargetPos[x] + blackboxHistory[2]->navTargetPos[x]) / 2); } + blackboxWriteSignedVB(blackboxCurrent->navSurface - blackboxLast->navSurface); + blackboxWriteSignedVB(blackboxCurrent->navTargetSurface - blackboxLast->navTargetSurface); + for (x = 0; x < 4; x++) { blackboxWriteSignedVB(blackboxCurrent->navDebug[x] - blackboxLast->navDebug[x]); } @@ -1063,6 +1073,8 @@ static void loadMainState(void) blackboxCurrent->navTargetVel[i] = navDesiredVelocity[i]; blackboxCurrent->navTargetPos[i] = navTargetPosition[i]; } + blackboxCurrent->navSurface = navActualSurface; + blackboxCurrent->navTargetSurface = navTargetSurface; for (i = 0; i < 4; i++) { blackboxCurrent->navDebug[i] = navDebug[i]; } diff --git a/src/main/flight/navigation_rewrite.c b/src/main/flight/navigation_rewrite.c index f23383a719..f06af97b54 100755 --- a/src/main/flight/navigation_rewrite.c +++ b/src/main/flight/navigation_rewrite.c @@ -66,6 +66,8 @@ int16_t navDesiredHeading; int16_t navTargetPosition[3]; int32_t navLatestActualPosition[3]; int16_t navDebug[4]; +int16_t navTargetSurface; +int16_t navActualSurface; uint16_t navFlags; #endif @@ -1359,6 +1361,10 @@ void updateActualSurfaceDistance(bool hasValidSensor, float surfaceDistance, flo else { posControl.flags.surfaceDistanceNewData = 0; } + +#if defined(NAV_BLACKBOX) + navActualSurface = surfaceDistance; +#endif } /*----------------------------------------------------------- @@ -1503,11 +1509,15 @@ void updateHomePosition(void) void setDesiredSurfaceOffset(float surfaceOffset) { if (surfaceOffset > 0) { - posControl.desiredState.surface = constrainf(surfaceOffset, 1.0f, INAV_SONAR_MAX_DISTANCE); + posControl.desiredState.surface = constrainf(surfaceOffset, 1.0f, INAV_SURFACE_MAX_DISTANCE); } else { posControl.desiredState.surface = -1; } + +#if defined(NAV_BLACKBOX) + navTargetSurface = constrain(lrintf(posControl.desiredState.surface), -32678, 32767); +#endif } /*----------------------------------------------------------- @@ -1556,6 +1566,7 @@ void setDesiredPosition(t_fp_vector * pos, int32_t yaw, navSetWaypointFlags_t us navTargetPosition[X] = constrain(lrintf(posControl.desiredState.pos.V.X), -32678, 32767); navTargetPosition[Y] = constrain(lrintf(posControl.desiredState.pos.V.Y), -32678, 32767); navTargetPosition[Z] = constrain(lrintf(posControl.desiredState.pos.V.Z), -32678, 32767); + navTargetSurface = constrain(lrintf(posControl.desiredState.surface), -32678, 32767); #endif } @@ -1605,7 +1616,7 @@ void updateAltitudeTargetFromClimbRate(float climbRate, navUpdateAltitudeFromRat else { if (posControl.flags.isTerrainFollowEnabled) { if (posControl.actualState.surface >= 0.0f && posControl.flags.hasValidSurfaceSensor && (mode == CLIMB_RATE_UPDATE_SURFACE_TARGET)) { - posControl.desiredState.surface = constrainf(posControl.actualState.surface + (climbRate / posControl.pids.pos[Z].param.kP), 1.0f, INAV_SONAR_MAX_DISTANCE); + posControl.desiredState.surface = constrainf(posControl.actualState.surface + (climbRate / posControl.pids.pos[Z].param.kP), 1.0f, INAV_SURFACE_MAX_DISTANCE); } } else { @@ -1617,6 +1628,7 @@ void updateAltitudeTargetFromClimbRate(float climbRate, navUpdateAltitudeFromRat #if defined(NAV_BLACKBOX) navTargetPosition[Z] = constrain(lrintf(posControl.desiredState.pos.V.Z), -32678, 32767); + navTargetSurface = constrain(lrintf(posControl.desiredState.surface), -32678, 32767); #endif } @@ -2167,6 +2179,11 @@ void navigationUsePIDs(pidProfile_t *initialPidProfile) (float)posControl.pidProfile->I8[PIDVEL] / 100.0f, (float)posControl.pidProfile->D8[PIDVEL] / 100.0f); + // Initialize surface tracking PID + navPidInit(&posControl.pids.surface, 1.0f, + 0.25f, + 0.0f); + // Initialize fixed wing PID controllers navPidInit(&posControl.pids.fw_nav, (float)posControl.pidProfile->P8[PIDNAVR] / 100.0f, (float)posControl.pidProfile->I8[PIDNAVR] / 100.0f, diff --git a/src/main/flight/navigation_rewrite.h b/src/main/flight/navigation_rewrite.h index 0a4d02eb22..385cae4a35 100755 --- a/src/main/flight/navigation_rewrite.h +++ b/src/main/flight/navigation_rewrite.h @@ -273,6 +273,8 @@ extern int16_t navActualVelocity[3]; extern int16_t navDesiredVelocity[3]; extern int16_t navTargetPosition[3]; extern int32_t navLatestActualPosition[3]; +extern int16_t navTargetSurface; +extern int16_t navActualSurface; extern int16_t navDebug[4]; extern uint16_t navFlags; #define NAV_BLACKBOX_DEBUG(x,y) navDebug[x] = constrain((y), -32678, 32767) diff --git a/src/main/flight/navigation_rewrite_multicopter.c b/src/main/flight/navigation_rewrite_multicopter.c index ef64a9f44f..598bdd45ae 100755 --- a/src/main/flight/navigation_rewrite_multicopter.c +++ b/src/main/flight/navigation_rewrite_multicopter.c @@ -62,24 +62,25 @@ static int16_t rcCommandAdjustedThrottle; static int16_t altHoldThrottleRCZero = 1500; static filterStatePt1_t altholdThrottleFilterState; -#define SURFACE_TRACKING_GAIN 40.0f - /* Calculate global altitude setpoint based on surface setpoint */ static void updateSurfaceTrackingAltitudeSetpoint(uint32_t deltaMicros) { /* If we have a surface offset target and a valid surface offset reading - recalculate altitude target */ if (posControl.flags.isTerrainFollowEnabled && posControl.desiredState.surface >= 0) { if (posControl.actualState.surface >= 0 && posControl.flags.hasValidSurfaceSensor) { - float surfaceOffsetError = posControl.desiredState.surface - posControl.actualState.surface; - float trackingWeight = SURFACE_TRACKING_GAIN * US2S(deltaMicros); - - posControl.desiredState.pos.V.Z = posControl.actualState.pos.V.Z + surfaceOffsetError * trackingWeight; + float targetAltitudeError = navPidApply2(posControl.desiredState.surface, posControl.actualState.surface, US2S(deltaMicros), &posControl.pids.surface, -25.0f, +25.0f, false); + posControl.desiredState.pos.V.Z = posControl.actualState.pos.V.Z + targetAltitudeError; } else { // TODO: We are possible above valid range, we now descend down to attempt to get back within range - updateAltitudeTargetFromClimbRate(-0.5f * posControl.navConfig->emerg_descent_rate, CLIMB_RATE_KEEP_SURFACE_TARGET); + //updateAltitudeTargetFromClimbRate(-0.10f * posControl.navConfig->emerg_descent_rate, CLIMB_RATE_KEEP_SURFACE_TARGET); + updateAltitudeTargetFromClimbRate(-20.0f, CLIMB_RATE_KEEP_SURFACE_TARGET); } } + +#if defined(NAV_BLACKBOX) + navTargetPosition[Z] = constrain(lrintf(posControl.desiredState.pos.V.Z), -32678, 32767); +#endif } // Position to velocity controller for Z axis @@ -169,6 +170,7 @@ void setupMulticopterAltitudeController(void) void resetMulticopterAltitudeController() { navPidReset(&posControl.pids.vel[Z]); + navPidReset(&posControl.pids.surface); filterResetPt1(&altholdThrottleFilterState, 0.0f); posControl.desiredState.vel.V.Z = posControl.actualState.vel.V.Z; // Gradually transition from current climb posControl.rcAdjustment[THROTTLE] = 0; diff --git a/src/main/flight/navigation_rewrite_private.h b/src/main/flight/navigation_rewrite_private.h index 85582aa802..757365be72 100755 --- a/src/main/flight/navigation_rewrite_private.h +++ b/src/main/flight/navigation_rewrite_private.h @@ -31,7 +31,8 @@ #define NAV_DTERM_CUT_HZ 10 #define NAV_ACCELERATION_XY_MAX 980.0f // cm/s/s // approx 45 deg lean angle -#define INAV_SONAR_MAX_DISTANCE 50 // Sonar is unreliable above 50cm due to noise from propellers +#define INAV_SONAR_MAX_DISTANCE 55 // Sonar is unreliable above 50cm due to noise from propellers +#define INAV_SURFACE_MAX_DISTANCE 40 #define HZ2US(hz) (1000000 / (hz)) #define US2S(us) ((us) * 1e-6f) @@ -101,6 +102,7 @@ typedef struct navigationPIDControllers_s { /* Multicopter PIDs */ pController_t pos[XYZ_AXIS_COUNT]; pidController_t vel[XYZ_AXIS_COUNT]; + pidController_t surface; /* Fixed-wing PIDs */ pidController_t fw_alt;