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

Surface sensor (SONAR) assisted landing detection

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-11-25 16:50:11 +10:00
parent 6850fe324f
commit 6afd3741b2
3 changed files with 28 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1010,6 +1010,15 @@ void updateActualSurfaceDistance(bool hasValidSensor, float surfaceDistance, flo
posControl.actualState.surface = surfaceDistance;
posControl.actualState.surfaceVel = surfaceVelocity;
if (surfaceDistance > 0) {
if (posControl.actualState.surfaceMin > 0) {
posControl.actualState.surfaceMin = MIN(posControl.actualState.surfaceMin, surfaceDistance);
}
else {
posControl.actualState.surfaceMin = surfaceDistance;
}
}
posControl.flags.hasValidSurfaceSensor = hasValidSensor;
if (hasValidSensor) {
@ -1785,6 +1794,11 @@ void navigationInit(navConfig_t *initialnavConfig,
posControl.waypointCount = 0;
posControl.activeWaypointIndex = 0;
/* Set initial surface invalid */
posControl.actualState.surface = -1.0f;
posControl.actualState.surfaceVel = 0.0f;
posControl.actualState.surfaceMin = -1.0f;
/* Use system config */
navigationUseConfig(initialnavConfig);
navigationUsePIDs(initialPidProfile);

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

@ -424,7 +424,17 @@ bool isMulticopterLandingDetected(uint32_t * landingTimer)
// from processRx() and rcCommand at that moment holds rc input, not adjusted values from NAV core)
bool minimalThrust = rcCommandAdjustedThrottle <= (posControl.escAndServoConfig->minthrottle + (posControl.escAndServoConfig->maxthrottle - posControl.escAndServoConfig->minthrottle) * 0.25f);
if (!minimalThrust || verticalMovement || horizontalMovement) {
bool possibleLandingDetected = false;
if (posControl.flags.hasValidSurfaceSensor) {
/* surfaceMin is our ground reference. If we are less than 5cm above the ground - we are likely landed */
possibleLandingDetected = (posControl.actualState.surface <= (posControl.actualState.surfaceMin + 5.0f));
}
else {
possibleLandingDetected = (minimalThrust && !verticalMovement && !horizontalMovement);
}
if (!possibleLandingDetected) {
*landingTimer = currentTime;
return false;
}

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

@ -21,7 +21,7 @@
#define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR 1.113195f // MagicEarthNumber from APM
#define LAND_DETECTOR_TRIGGER_TIME_MS 2000
#define LAND_DETECTOR_TRIGGER_TIME_MS 2000 // 2 seconds
#define NAV_ROLL_PITCH_MAX_DECIDEGREES (30 * 10) // Max control input from NAV (30 deg)
#define POSITION_TARGET_UPDATE_RATE_HZ 5 // Rate manual position target update (minumum possible speed in cms will be this value)
@ -108,6 +108,7 @@ typedef struct {
float cosYaw;
float surface;
float surfaceVel;
float surfaceMin;
} navigationEstimatedState_t;
typedef struct {