Skip to content
Permalink
Browse files

This might be surface mode. Or not...

  • Loading branch information...
DzikuVx committed Jul 30, 2017
1 parent c901a44 commit 6eb4fd4be8a3028f3a9e39241f261c83a8670018
Showing with 71 additions and 5 deletions.
  1. +63 −5 src/main/navigation/navigation_multicopter.c
  2. +8 −0 src/main/navigation/navigation_private.h
@@ -62,10 +62,64 @@ static bool prepareForTakeoffOnReset = false;
// Position to velocity controller for Z axis
static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
{
static bool previousIsTerrainFollowingEnabled = false;
static bool previousIsSurfaceAltitudeValid = false;
bool surfaceValid = isSurfaceAltitudeValid();

/*
* In this case we switch surface mode ON, store current surface altitude as target
* It is possible only is surface tracking is valid
* Only when we switched ON when we had valid AGL surface can be really enabled
*/
if (
surfaceValid &&
posControl.surfaceState.previousIsTerrainFollowingEnabled == false &&
posControl.flags.isTerrainFollowEnabled == true
) {
posControl.desiredState.pos.V.Z = rangefinderGetLatestAltitude();
posControl.surfaceState.surfaceAllowed = true;
}

/*
* We swithced surface OFF
*/
if (
posControl.surfaceState.previousIsTerrainFollowingEnabled == true &&
posControl.flags.isTerrainFollowEnabled == false
) {
posControl.desiredState.pos.V.Z = posControl.actualState.pos.V.Z;
posControl.surfaceState.surfaceAllowed = false;
}

if (
posControl.flags.isTerrainFollowEnabled == true &&
posControl.surfaceState.surfaceAllowed == true
) {

/*
* We regained valid AGL
*/
if (posControl.surfaceState.previousIsSurfaceAltitudeValid == false && surfaceValid == true) {
posControl.desiredState.pos.V.Z = rangefinderGetLatestAltitude();
}

/*
* We lost AGL
*/
if (posControl.surfaceState.previousIsSurfaceAltitudeValid == true && surfaceValid == false) {
posControl.desiredState.pos.V.Z = posControl.actualState.pos.V.Z;
}
}

const float altitudeError = posControl.desiredState.pos.V.Z - posControl.actualState.pos.V.Z;
float altitudeError;
if (
posControl.flags.isTerrainFollowEnabled == true &&
surfaceValid == true &&
posControl.surfaceState.surfaceAllowed == true
) {
altitudeError = posControl.desiredState.pos.V.Z - rangefinderGetLatestAltitude();
} else {
altitudeError = posControl.desiredState.pos.V.Z - posControl.actualState.pos.V.Z;
}

float targetVel = altitudeError * posControl.pids.pos[Z].param.kP;

// hard limit desired target velocity to max_climb_rate
@@ -86,8 +140,8 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
posControl.desiredState.vel.V.Z = targetVel;
}

previousIsSurfaceAltitudeValid = isSurfaceAltitudeValid();
previousIsTerrainFollowingEnabled = posControl.flags.isTerrainFollowEnabled;
posControl.surfaceState.previousIsSurfaceAltitudeValid = surfaceValid;
posControl.surfaceState.previousIsTerrainFollowingEnabled = posControl.flags.isTerrainFollowEnabled;

#if defined(NAV_BLACKBOX)
navDesiredVelocity[Z] = constrain(lrintf(posControl.desiredState.vel.V.Z), -32678, 32767);
@@ -171,6 +225,10 @@ void resetMulticopterAltitudeController(void)
navPidReset(&posControl.pids.surface);
posControl.rcAdjustment[THROTTLE] = 0;

posControl.surfaceState.surfaceAllowed = false;
posControl.surfaceState.previousIsSurfaceAltitudeValid = false;
posControl.surfaceState.previousIsTerrainFollowingEnabled = false;

if (prepareForTakeoffOnReset) {
/* If we are preparing for takeoff - start with lowset possible climb rate, adjust alt target and make sure throttle doesn't jump */
posControl.desiredState.vel.V.Z = -navConfig()->general.max_manual_climb_rate;
@@ -250,6 +250,12 @@ typedef struct {
float minimalDistanceToHome;
} rthSanityChecker_t;

typedef struct {
bool surfaceAllowed;
bool previousIsSurfaceAltitudeValid;
bool previousIsTerrainFollowingEnabled;
} surfacePosControl_t;

typedef struct {
/* Flags and navigation system state */
navigationFSMState_t navState;
@@ -265,6 +271,8 @@ typedef struct {
navigationEstimatedState_t actualState;
navigationDesiredState_t desiredState; // waypoint coordinates + velocity

surfacePosControl_t surfaceState;

uint32_t lastValidPositionTimeMs;
uint32_t lastValidAltitudeTimeMs;

0 comments on commit 6eb4fd4

Please sign in to comment.
You can’t perform that action at this time.