Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-20 23:05:17 +03:00
Code Issues Wiki Activity

SURFACE: Improve surface tracking

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2016-04-01 14:37:01 +10:00
parent 14a777dcaf
commit 5e45c692f7
5 changed files with 45 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files

12
src/main/blackbox/blackbox.c
View file

@ -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];
}

21
src/main/flight/navigation_rewrite.c
View file

@ -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,

2
src/main/flight/navigation_rewrite.h
View file

@ -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)

16
src/main/flight/navigation_rewrite_multicopter.c
View file

@ -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;

4
src/main/flight/navigation_rewrite_private.h
View file

@ -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;