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

Merge pull request #19 from iNavFlight/master

Browse source 
Update INAV master
This commit is contained in:
breadoven 2021-04-03 17:35:44 +01:00 • committed by GitHub
commit 814dc4924c
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
109 changed files with 4296 additions and 522 deletions
Download patch file Download diff file Expand all files Collapse all files

131
src/main/navigation/navigation.c
View file

@ -91,7 +91,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 9);
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 10);
PG_RESET_TEMPLATE(navConfig_t, navConfig,
.general = {
@ -144,6 +144,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.braking_bank_angle = 40, // Max braking angle
.posDecelerationTime = 120, // posDecelerationTime * 100
.posResponseExpo = 10, // posResponseExpo * 100
.slowDownForTurning = true,
},
// Fixed wing
@ -159,7 +160,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.pitch_to_throttle = 10, // pwm units per degree of pitch (10pwm units ~ 1% throttle)
.pitch_to_throttle_smooth = 6,
.pitch_to_throttle_thresh = 50,
.loiter_radius = 5000, // 50m
.loiter_radius = 7500, // 75m
//Fixed wing landing
.land_dive_angle = 2, // 2 degrees dive by default
@ -1923,132 +1924,6 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
return &posControl.rthState.homeTmpWaypoint;
}
/*-----------------------------------------------------------
* Float point PID-controller implementation
*-----------------------------------------------------------*/
// Implementation of PID with back-calculation I-term anti-windup
// Control System Design, Lecture Notes for ME 155A by Karl Johan Åström (p.228)
// http://www.cds.caltech.edu/~murray/courses/cds101/fa02/caltech/astrom-ch6.pdf
float navPidApply3(
pidController_t *pid,
const float setpoint,
const float measurement,
const float dt,
const float outMin,
const float outMax,
const pidControllerFlags_e pidFlags,
const float gainScaler,
const float dTermScaler
) {
float newProportional, newDerivative, newFeedForward;
float error = setpoint - measurement;
/* P-term */
newProportional = error * pid->param.kP * gainScaler;
/* D-term */
if (pid->reset) {
pid->last_input = (pidFlags & PID_DTERM_FROM_ERROR) ? error : measurement;
pid->reset = false;
}
if (pidFlags & PID_DTERM_FROM_ERROR) {
/* Error-tracking D-term */
newDerivative = (error - pid->last_input) / dt;
pid->last_input = error;
} else {
/* Measurement tracking D-term */
newDerivative = -(measurement - pid->last_input) / dt;
pid->last_input = measurement;
}
if (pid->dTermLpfHz > 0) {
newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, pid->dTermLpfHz, dt);
} else {
newDerivative = pid->param.kD * newDerivative;
}
newDerivative = newDerivative * gainScaler * dTermScaler;
if (pidFlags & PID_ZERO_INTEGRATOR) {
pid->integrator = 0.0f;
}
/*
* Compute FeedForward parameter
*/
newFeedForward = setpoint * pid->param.kFF * gainScaler;
/* Pre-calculate output and limit it if actuator is saturating */
const float outVal = newProportional + (pid->integrator * gainScaler) + newDerivative + newFeedForward;
const float outValConstrained = constrainf(outVal, outMin, outMax);
pid->proportional = newProportional;
pid->integral = pid->integrator;
pid->derivative = newDerivative;
pid->feedForward = newFeedForward;
pid->output_constrained = outValConstrained;
/* Update I-term */
if (!(pidFlags & PID_ZERO_INTEGRATOR)) {
const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + ((outValConstrained - outVal) * pid->param.kT * dt);
if (pidFlags & PID_SHRINK_INTEGRATOR) {
// Only allow integrator to shrink
if (fabsf(newIntegrator) < fabsf(pid->integrator)) {
pid->integrator = newIntegrator;
}
}
else {
pid->integrator = newIntegrator;
}
}
if (pidFlags & PID_LIMIT_INTEGRATOR) {
pid->integrator = constrainf(pid->integrator, outMin, outMax);
}
return outValConstrained;
}
float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags)
{
return navPidApply3(pid, setpoint, measurement, dt, outMin, outMax, pidFlags, 1.0f, 1.0f);
}
void navPidReset(pidController_t *pid)
{
pid->reset = true;
pid->proportional = 0.0f;
pid->integral = 0.0f;
pid->derivative = 0.0f;
pid->integrator = 0.0f;
pid->last_input = 0.0f;
pid->feedForward = 0.0f;
pt1FilterReset(&pid->dterm_filter_state, 0.0f);
pid->output_constrained = 0.0f;
}
void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz)
{
pid->param.kP = _kP;
pid->param.kI = _kI;
pid->param.kD = _kD;
pid->param.kFF = _kFF;
if (_kI > 1e-6f && _kP > 1e-6f) {
float Ti = _kP / _kI;
float Td = _kD / _kP;
pid->param.kT = 2.0f / (Ti + Td);
}
else {
pid->param.kI = 0.0;
pid->param.kT = 0.0;
}
pid->dTermLpfHz = _dTermLpfHz;
navPidReset(pid);
}
/*-----------------------------------------------------------
* Detects if thrust vector is facing downwards
*-----------------------------------------------------------*/