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

Allow Angle target overrides

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2022-04-12 08:55:36 +02:00
parent 8be0247dec
commit 68c47e7c09
3 changed files with 52 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files

20
src/main/flight/pid.c
View file

@ -591,8 +591,7 @@ int16_t angleFreefloatDeadband(int16_t deadband, flight_dynamics_index_t axis)
}
}
static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float horizonRateMagnitude, float dT)
{
static float computePidLevelTarget(flight_dynamics_index_t axis) {
// This is ROLL/PITCH, run ANGLE/HORIZON controllers
float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]);
@ -620,6 +619,12 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
DEBUG_SET(DEBUG_AUTOLEVEL, 3, angleTarget * 10);
}
return angleTarget;
}
static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynamics_index_t axis, float horizonRateMagnitude, float dT)
{
#ifdef USE_SECONDARY_IMU
float actual;
if (secondaryImuState.active && secondaryImuConfig()->useForStabilized) {
@ -1126,14 +1131,15 @@ void FAST_CODE pidController(float dT)
}
// Step 3: Run control for ANGLE_MODE, HORIZON_MODE, and HEADING_LOCK
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
const float horizonRateMagnitude = calcHorizonRateMagnitude();
pidLevel(&pidState[FD_ROLL], FD_ROLL, horizonRateMagnitude, dT);
pidLevel(&pidState[FD_PITCH], FD_PITCH, horizonRateMagnitude, dT);
levelingEnabled = false;
for (uint8_t axis = FD_ROLL; axis <= FD_PITCH; axis++) {
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || isFlightAxisAngleOverrideActive(axis)) {
float angleTarget = getFlightAxisAngleOverride(axis, computePidLevelTarget(axis));
pidLevel(angleTarget, &pidState[axis], axis, horizonRateMagnitude, dT);
canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with ANGLE/HORIZON
levelingEnabled = true;
} else {
levelingEnabled = false;
}
}
if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || navigationRequiresTurnAssistance())) {

32
src/main/programming/logic_condition.c
View file

@ -817,3 +817,35 @@ uint32_t getLoiterRadius(uint32_t loiterRadius) {
return loiterRadius;
#endif
}
float getFlightAxisAngleOverride(uint8_t axis, float angle) {
if (flightAxisOverride[axis].angleTargetActive) {
return flightAxisOverride[axis].angleTarget;
} else {
return angle;
}
}
float getFlightAxisRateOverride(uint8_t axis, float rate) {
if (flightAxisOverride[axis].rateTargetActive) {
return flightAxisOverride[axis].rateTarget;
} else {
return rate;
}
}
bool isFlightAxisAngleOverrideActive(uint8_t axis) {
if (flightAxisOverride[axis].angleTargetActive) {
return true;
} else {
return false;
}
}
bool isFlightAxisRateOverrideActive(uint8_t axis) {
if (flightAxisOverride[axis].rateTargetActive) {
return true;
} else {
return false;
}
}

4
src/main/programming/logic_condition.h
View file

@ -217,3 +217,7 @@ float getThrottleScale(float globalThrottleScale);
int16_t getRcCommandOverride(int16_t command[], uint8_t axis);
int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue);
uint32_t getLoiterRadius(uint32_t loiterRadius);
float getFlightAxisAngleOverride(uint8_t axis, float angle);
float getFlightAxisRateOverride(uint8_t axis, float rate);
bool isFlightAxisAngleOverrideActive(uint8_t axis);
bool isFlightAxisRateOverrideActive(uint8_t axis);