Skip to content

Commit

Permalink
initial build
Browse files Browse the repository at this point in the history
  • Loading branch information
@breadoven
breadoven committed Nov 7, 2023
1 parent 9ebbfeb commit 0246892
Show file tree
Hide file tree
Showing 4 changed files with 92 additions and 104 deletions.
109 changes: 55 additions & 54 deletions src/main/navigation/navigation.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2927,8 +2927,7 @@ void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlag

// Z-position
if ((useMask & NAV_POS_UPDATE_Z) != 0) {
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); // Reset RoC/RoD -> altitude controller
posControl.desiredState.pos.z = pos->z;
updateClimbRateToAltitudeController(navConfig()->general.max_auto_climb_rate, pos->z, ROC_TO_ALT_TARGET);
}

// Heading
Expand Down Expand Up @@ -3036,66 +3035,68 @@ bool isProbablyStillFlying(void)
/*-----------------------------------------------------------
* Z-position controller
*-----------------------------------------------------------*/
void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode)
static bool isMaxAltitudeLimitExceeded(void)
{
#define MIN_TARGET_CLIMB_RATE 100.0f // cm/s
return navGetCurrentActualPositionAndVelocity()->pos.z >= navConfig()->general.max_altitude;
}

static timeUs_t lastUpdateTimeUs;
timeUs_t currentTimeUs = micros();
int32_t getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
{
if (navConfig()->general.max_altitude && isMaxAltitudeLimitExceeded()) {
targetAltitude = MIN(targetAltitude, navConfig()->general.max_altitude);
}

// Terrain following uses different altitude measurement
const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z;
const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding();

if (mode != ROC_TO_ALT_RESET && desiredClimbRate) {
/* ROC_TO_ALT_CONSTANT - constant climb rate
* ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to min rate when altitude reached
* Rate reduction starts at distance from target altitude of 5 x climb rate for FW, 1 x climb rate for MC */
float maxClimbRate = navConfig()->general.max_auto_climb_rate;
if (emergLandingIsActive) {
maxClimbRate = navConfig()->general.emerg_descent_rate;
} else if (posControl.flags.isAdjustingAltitude) {
maxClimbRate = navConfig()->general.max_manual_climb_rate;
}
const float targetAltitudeError = targetAltitude - navGetCurrentActualPositionAndVelocity()->pos.z;
float targetVel = 0.0f;

if (mode == ROC_TO_ALT_TARGET && fabsf(desiredClimbRate) > MIN_TARGET_CLIMB_RATE) {
const int8_t direction = desiredClimbRate > 0 ? 1 : -1;
const float absClimbRate = fabsf(desiredClimbRate);
const uint16_t maxRateCutoffAlt = STATE(AIRPLANE) ? absClimbRate * 5 : absClimbRate;
const float verticalVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z - targetAltitude,
0.0f, -maxRateCutoffAlt * direction, MIN_TARGET_CLIMB_RATE, absClimbRate);
if (STATE(MULTIROTOR)) {
targetVel = getSqrtControllerVelocity(targetAltitude, deltaMicros);
} else {
targetVel = scaleRangef(targetAltitudeError, 0.0f, maxClimbRate * 5, 0.0f, maxClimbRate);
}

desiredClimbRate = direction * constrainf(verticalVelScaled, MIN_TARGET_CLIMB_RATE, absClimbRate);
}
if (emergLandingIsActive && targetAltitudeError > -50) {
return -100;
} else {
return constrainf(targetVel, -maxClimbRate, maxClimbRate);
}
}

/*
* If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0
* In other words, when altitude is reached, allow it only to shrink
*/
if (navConfig()->general.max_altitude > 0 && altitudeToUse >= navConfig()->general.max_altitude && desiredClimbRate > 0) {
desiredClimbRate = 0;
}
void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode)
{
/* ROC_TO_ALT_CONSTANT - constant climb rate. desiredClimbRate direction (+ or -) is required
* ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to min rate when altitude reached
* Rate reduction starts at distance from target altitude of 5 x climb rate for FW, 1 x climb rate for MC.
* desiredClimbRate direction isn't required, set by target altitude direction instead
* NAV_IMPOSSIBLE_ALTITUDE_TARGET simply uses posControl.desiredState.pos.z to set a constant climb rate */

if (STATE(FIXED_WING_LEGACY)) {
// Fixed wing climb rate controller is open-loop. We simply move the known altitude target
float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs);
static bool targetHoldActive = false;

if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ) && desiredClimbRate) {
// Update target altitude only if actual altitude moving in same direction and lagging by < 5 m, otherwise hold target
if (navGetCurrentActualPositionAndVelocity()->vel.z * desiredClimbRate >= 0 && fabsf(posControl.desiredState.pos.z - altitudeToUse) < 500) {
posControl.desiredState.pos.z += desiredClimbRate * timeDelta;
targetHoldActive = false;
} else if (!targetHoldActive) { // Reset and hold target to actual + climb rate boost until actual catches up
posControl.desiredState.pos.z = altitudeToUse + desiredClimbRate;
targetHoldActive = true;
}
} else {
targetHoldActive = false;
}
}
else {
// Multicopter climb-rate control is closed-loop, it's possible to directly calculate desired altitude setpoint to yield the required RoC/RoD
posControl.desiredState.pos.z = altitudeToUse + (desiredClimbRate / posControl.pids.pos[Z].param.kP);
}
} else { // ROC_TO_ALT_RESET or zero desired climbrate
// Terrain following uses different altitude measurement
const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z;

if (mode == ROC_TO_ALT_RESET) {
posControl.desiredState.pos.z = altitudeToUse;
} else if (mode == ROC_TO_ALT_TARGET) {
posControl.desiredState.pos.z = targetAltitude;
} else { // ROC_TO_ALT_CONSTANT
posControl.desiredState.pos.z = NAV_IMPOSSIBLE_ALTITUDE_TARGET;
}

/*
* If max altitude is set and altitude limit reached reset altitude target to max altitude unless desired climb rate is -ve
*/
if (navConfig()->general.max_altitude && isMaxAltitudeLimitExceeded() && desiredClimbRate >= 0.0f) {
posControl.desiredState.pos.z = MIN(posControl.desiredState.pos.z, navConfig()->general.max_altitude);
}

lastUpdateTimeUs = currentTimeUs;
posControl.desiredState.vel.z = desiredClimbRate;
}

static void resetAltitudeController(bool useTerrainFollowing)
Expand Down Expand Up @@ -4317,9 +4318,9 @@ void navigationUsePIDs(void)
0.0f
);

navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 10.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 10.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 10.0f,
navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 100.0f,
0.0f,
NAV_DTERM_CUT_HZ,
0.0f
Expand Down
46 changes: 17 additions & 29 deletions src/main/navigation/navigation_fixedwing.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -123,7 +123,7 @@ bool adjustFixedWingAltitudeFromRCInput(void)
else {
// Adjusting finished - reset desired position to stay exactly where pilot released the stick
if (posControl.flags.isAdjustingAltitude) {
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
updateClimbRateToAltitudeController(navConfig()->general.max_auto_climb_rate, 0, ROC_TO_ALT_RESET);
}
return false;
}
Expand All @@ -134,46 +134,34 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
{
static pt1Filter_t velzFilterState;

// On a fixed wing we might not have a reliable climb rate source (if no BARO available), so we can't apply PID controller to
// velocity error. We use PID controller on altitude error and calculate desired pitch angle
float desiredClimbRate = posControl.desiredState.vel.z;

// Update energies
const float demSPE = (posControl.desiredState.pos.z * 0.01f) * GRAVITY_MSS;
const float demSKE = 0.0f;

const float estSPE = (navGetCurrentActualPositionAndVelocity()->pos.z * 0.01f) * GRAVITY_MSS;
const float estSKE = 0.0f;

// speedWeight controls balance between potential and kinetic energy used for pitch controller
// speedWeight = 1.0 : pitch will only control airspeed and won't control altitude
// speedWeight = 0.5 : pitch will be used to control both airspeed and altitude
// speedWeight = 0.0 : pitch will only control altitude
const float speedWeight = 0.0f; // no speed sensing for now

const float demSEB = demSPE * (1.0f - speedWeight) - demSKE * speedWeight;
const float estSEB = estSPE * (1.0f - speedWeight) - estSKE * speedWeight;
if (posControl.desiredState.pos.z != NAV_IMPOSSIBLE_ALTITUDE_TARGET) {
desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
}

// SEB to pitch angle gain to account for airspeed (with respect to specified reference (tuning) speed
const float pitchGainInv = 1.0f / 1.0f;
// Reduce max allowed climb pitch if performing loiter (stall prevention)
if (needToCalculateCircularLoiter && desiredClimbRate > 0.0f) {
desiredClimbRate *= 0.67f;
}

// Here we use negative values for dive for better clarity
float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
const float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
const float minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle);

// Reduce max allowed climb pitch if performing loiter (stall prevention)
if (needToCalculateCircularLoiter) {
maxClimbDeciDeg = maxClimbDeciDeg * 0.67f;
}

// PID controller to translate energy balance error [J] into pitch angle [decideg]
float targetPitchAngle = navPidApply3(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0, pitchGainInv, 1.0f);
// PID controller to translate desired climb rate error into pitch angle [decideg]
float currentClimbRate = navGetCurrentActualPositionAndVelocity()->vel.z;
float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, desiredClimbRate, currentClimbRate, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, PID_DTERM_FROM_ERROR);

// Apply low-pass filter to prevent rapid correction
targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros));

// Reconstrain pitch angle ( >0 - climb, <0 - dive)
// Reconstrain pitch angle (> 0 - climb, < 0 - dive)
targetPitchAngle = constrainf(targetPitchAngle, minDiveDeciDeg, maxClimbDeciDeg);
posControl.rcAdjustment[PITCH] = targetPitchAngle;

posControl.desiredState.vel.z = desiredClimbRate;
navDesiredVelocity[Z] = constrain(lrintf(posControl.desiredState.vel.z), -32678, 32767);
}

void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs)
Expand Down
35 changes: 16 additions & 19 deletions src/main/navigation/navigation_multicopter.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,28 +63,26 @@ static pt1Filter_t altholdThrottleFilterState;
static bool prepareForTakeoffOnReset = false;
static sqrt_controller_t alt_hold_sqrt_controller;

// Position to velocity controller for Z axis
static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros)
{
float targetVel = sqrtControllerApply(
&alt_hold_sqrt_controller,
posControl.desiredState.pos.z,
navGetCurrentActualPositionAndVelocity()->pos.z,
US2S(deltaMicros)
return sqrtControllerApply(
&alt_hold_sqrt_controller,
targetAltitude,
navGetCurrentActualPositionAndVelocity()->pos.z,
US2S(deltaMicros)
);
}

// hard limit desired target velocity to max_climb_rate
float vel_max_z = 0.0f;
// Position to velocity controller for Z axis
static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
{
float targetVel = posControl.desiredState.vel.z;

if (posControl.flags.isAdjustingAltitude) {
vel_max_z = navConfig()->general.max_manual_climb_rate;
} else {
vel_max_z = navConfig()->general.max_auto_climb_rate;
if (posControl.desiredState.pos.z != NAV_IMPOSSIBLE_ALTITUDE_TARGET) {
targetVel = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
}

targetVel = constrainf(targetVel, -vel_max_z, vel_max_z);

posControl.pids.pos[Z].output_constrained = targetVel;
posControl.pids.pos[Z].output_constrained = targetVel; // only used for Blackbox and OSD info

// Limit max up/down acceleration target
const float smallVelChange = US2S(deltaMicros) * (GRAVITY_CMSS * 0.1f);
Expand Down Expand Up @@ -132,8 +130,7 @@ bool adjustMulticopterAltitudeFromRCInput(void)
// In terrain follow mode we apply different logic for terrain control
if (posControl.flags.estAglStatus == EST_TRUSTED && altTarget > 10.0f) {
// We have solid terrain sensor signal - directly map throttle to altitude
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
posControl.desiredState.pos.z = altTarget;
updateClimbRateToAltitudeController(navConfig()->general.max_manual_climb_rate, altTarget, ROC_TO_ALT_TARGET);
}
else {
updateClimbRateToAltitudeController(-50.0f, 0, ROC_TO_ALT_CONSTANT);
Expand Down Expand Up @@ -165,7 +162,7 @@ bool adjustMulticopterAltitudeFromRCInput(void)
else {
// Adjusting finished - reset desired position to stay exactly where pilot released the stick
if (posControl.flags.isAdjustingAltitude) {
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
updateClimbRateToAltitudeController(navConfig()->general.max_auto_climb_rate, 0, ROC_TO_ALT_RESET);
}

return false;
Expand Down
6 changes: 4 additions & 2 deletions src/main/navigation/navigation_private.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,8 @@

#define NAV_RTH_TRACKBACK_POINTS 50 // max number RTH trackback points

#define NAV_IMPOSSIBLE_ALTITUDE_TARGET 10000000 // 100km in cm. Serves as a flag for vertical velocity control

#define MAX_POSITION_UPDATE_INTERVAL_US HZ2US(MIN_POSITION_UPDATE_RATE_HZ) // convenience macro
_Static_assert(MAX_POSITION_UPDATE_INTERVAL_US <= TIMEDELTA_MAX, "deltaMicros can overflow!");

Expand Down Expand Up @@ -481,6 +483,7 @@ bool isWaypointNavTrackingActive(void);
void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse);
void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY);
void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus, float gpsCfEstimatedAltitudeError);
int32_t getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros);

bool checkForPositionSensorTimeout(void);

Expand All @@ -499,10 +502,9 @@ bool adjustMulticopterHeadingFromRCInput(void);
bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcRollAdjustment);

void applyMulticopterNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);

bool isMulticopterLandingDetected(void);

void calculateMulticopterInitialHoldPosition(fpVector3_t * pos);
float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros);

/* Fixed-wing specific functions */
void setupFixedWingAltitudeController(void);
Expand Down

0 comments on commit 0246892

Please sign in to comment.