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

NAV: Surface tracking

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2016-03-28 12:33:35 +10:00
parent 517c958d17
commit 37bd01a186
6 changed files with 25 additions and 9 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -616,7 +616,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navi
setupAltitudeController();
// If low enough and surface offset valid - enter surface tracking
if (posControl.flags.hasValidSurfaceSensor) {
if (posControl.flags.hasValidSurfaceSensor && posControl.flags.isTerrainFollowEnabled) {
setDesiredSurfaceOffset(posControl.actualState.surface);
}
else {
@ -673,7 +673,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n
if (((prevFlags & NAV_CTL_ALT) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0)) {
// If low enough and surface offset valid - enter surface tracking
if (posControl.flags.hasValidSurfaceSensor) {
if (posControl.flags.hasValidSurfaceSensor && posControl.flags.isTerrainFollowEnabled) {
setDesiredSurfaceOffset(posControl.actualState.surface);
}
else {
@ -1604,8 +1604,13 @@ void updateAltitudeTargetFromClimbRate(float climbRate)
// Calculate new altitude target
/* Move surface tracking setpoint if it is set */
if (posControl.desiredState.surface > 0.0f && posControl.actualState.surface > 0.0f && posControl.flags.hasValidSurfaceSensor) {
posControl.desiredState.surface = constrainf(posControl.actualState.surface + (climbRate / posControl.pids.pos[Z].param.kP), 1.0f, 200.0f);
if (posControl.flags.isTerrainFollowEnabled) {
if (posControl.desiredState.surface > 0.0f && posControl.actualState.surface > 0.0f && posControl.flags.hasValidSurfaceSensor) {
posControl.desiredState.surface = constrainf(posControl.actualState.surface + (climbRate / posControl.pids.pos[Z].param.kP), 1.0f, 200.0f);
}
}
else {
posControl.desiredState.surface = -1;
}
posControl.desiredState.pos.V.Z = posControl.actualState.pos.V.Z + (climbRate / posControl.pids.pos[Z].param.kP);
@ -2074,6 +2079,7 @@ static void updateReadyStatus(void)
void updateFlightBehaviorModifiers(void)
{
posControl.flags.isGCSAssistedNavigationEnabled = IS_RC_MODE_ACTIVE(BOXGCSNAV);
posControl.flags.isTerrainFollowEnabled = IS_RC_MODE_ACTIVE(BOXTERRAIN);
}
/**

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

@ -62,13 +62,18 @@ static int16_t rcCommandAdjustedThrottle;
static int16_t altHoldThrottleRCZero = 1500;
static filterStatePt1_t altholdThrottleFilterState;
#define SURFACE_TRACKING_GAIN 5.0f
/* Calculate global altitude setpoint based on surface setpoint */
static void updateSurfaceTrackingAltitudeSetpoint_MC(void)
static void updateSurfaceTrackingAltitudeSetpoint_MC(uint32_t deltaMicros)
{
/* If we have a surface offset target and a valid surface offset reading - recalculate altitude target */
if (posControl.desiredState.surface > 0 && (posControl.actualState.surface > 0 && posControl.flags.hasValidSurfaceSensor)) {
if (posControl.desiredState.surface > 0 && (posControl.actualState.surface > 0 && posControl.flags.hasValidSurfaceSensor) && posControl.flags.isTerrainFollowEnabled) {
float surfaceOffsetError = posControl.desiredState.surface - posControl.actualState.surface;
posControl.desiredState.pos.V.Z = posControl.actualState.pos.V.Z + surfaceOffsetError;
float altitudeError = posControl.desiredState.pos.V.Z - posControl.actualState.pos.V.Z;
float trackingWeight = SURFACE_TRACKING_GAIN * US2S(deltaMicros);
posControl.desiredState.pos.V.Z = posControl.actualState.pos.V.Z + surfaceOffsetError * trackingWeight + altitudeError * (1 - trackingWeight);
}
}
@ -187,7 +192,7 @@ static void applyMulticopterAltitudeController(uint32_t currentTime)
// Check if last correction was too log ago - ignore this update
if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
updateSurfaceTrackingAltitudeSetpoint_MC();
updateSurfaceTrackingAltitudeSetpoint_MC(deltaMicrosPositionUpdate);
updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
}

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

@ -62,7 +62,7 @@ typedef struct navigationFlags_s {
// Behaviour modifiers
bool isGCSAssistedNavigationEnabled; // Does iNav accept WP#255 - follow-me etc.
//bool isTerrainFollowEnabled; // Does iNav use sonar for terrain following (adjusting baro altitude target according to sonar readings)
bool isTerrainFollowEnabled; // Does iNav use sonar for terrain following (adjusting baro altitude target according to sonar readings)
bool forcedRTHActivated;
} navigationFlags_t;

1
src/main/io/rc_controls.h
View file

@ -49,6 +49,7 @@ typedef enum {
BOXAIRMODE,
BOXHOMERESET,
BOXGCSNAV,
BOXTERRAIN,
CHECKBOX_ITEM_COUNT
} boxId_e;

3
src/main/io/serial_msp.c
View file

@ -368,6 +368,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXAIRMODE, "AIR MODE;", 29 },
{ BOXHOMERESET, "HOME RESET;", 30 },
{ BOXGCSNAV, "GCS NAV;", 31 },
{ BOXTERRAIN, "TERRAIN;", 33 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
@ -678,6 +679,7 @@ void mspInit(serialConfig_t *serialConfig)
activeBoxIds[activeBoxIdCount++] = BOXNAVWP;
activeBoxIds[activeBoxIdCount++] = BOXHOMERESET;
activeBoxIds[activeBoxIdCount++] = BOXGCSNAV;
activeBoxIds[activeBoxIdCount++] = BOXTERRAIN;
}
#endif
@ -758,6 +760,7 @@ static uint32_t packFlightModeFlags(void)
IS_ENABLED(FLIGHT_MODE(NAV_WP_MODE)) << BOXNAVWP |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGCSNAV)) << BOXGCSNAV |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTERRAIN)) << BOXTERRAIN |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHOMERESET)) << BOXHOMERESET;
for (i = 0; i < activeBoxIdCount; i++) {

1
src/main/target/NAZE/target.h
View file

@ -124,6 +124,7 @@
#define LED1
#define INVERTER
#define DISPLAY
#define DISPLAY_ARMED_BITMAP
#define USE_USART1
#define USE_USART2