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

Move PID gains to UINT16 instead of UINT8. Keep it compatible on the MSP level

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-12-04 20:38:34 +01:00
parent ebf581c871
commit d32fe6dea5
12 changed files with 122 additions and 65 deletions
Download patch file Download diff file Expand all files Collapse all files

1
docs/Programming Framework.md
View file

@ -80,6 +80,7 @@ IPF can be edited using INAV Configurator user interface, of via CLI
| 3 | FLIGHT_MODE | `value` points to flight modes table |
| 4 | LC | `value` points to other logic condition ID |
| 5 | GVAR | Value stored in Global Variable indexed by `value`. `GVAR 1` means: value in GVAR 1 |
| 5 | PID | Output of a Programming PID indexed by `value`. `PID 1` means: value in PID 1 |
#### FLIGHT

2
src/main/cms/cms_menu_imu.c
View file

@ -55,7 +55,7 @@
#define RPY_PIDFF_MAX 200
#define OTHER_PIDDF_MAX 255
#define PIDFF_ENTRY(label, ptr, max) OSD_UINT8_ENTRY(label, (&(const OSD_UINT8_t){ ptr, PIDFF_MIN, max, PIDFF_STEP }))
#define PIDFF_ENTRY(label, ptr, max) OSD_UINT8_ENTRY(label, (&(const OSD_UINT16_t){ ptr, PIDFF_MIN, max, PIDFF_STEP }))
#define RPY_PIDFF_ENTRY(label, ptr) PIDFF_ENTRY(label, ptr, RPY_PIDFF_MAX)
#define OTHER_PIDFF_ENTRY(label, ptr) PIDFF_ENTRY(label, ptr, OTHER_PIDDF_MAX)

98
src/main/fc/cli.c
View file

@ -44,6 +44,7 @@ extern uint8_t __config_end;
#include "common/time.h"
#include "common/typeconversion.h"
#include "programming/global_variables.h"
#include "programming/pid.h"
#include "config/config_eeprom.h"
#include "config/feature.h"
@ -2001,47 +2002,50 @@ static void cliGvar(char *cmdline) {
}
}
static void printPid(uint8_t dumpMask, const logicCondition_t *logicConditions, const logicCondition_t *defaultLogicConditions)
static void printPid(uint8_t dumpMask, const programmingPid_t *programmingPids, const programmingPid_t *defaultProgrammingPids)
{
const char *format = "pid %d %d %d %d %d %d %d %d %d %d";
for (uint32_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
const logicCondition_t logic = logicConditions[i];
for (uint32_t i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
const programmingPid_t pid = programmingPids[i];
bool equalsDefault = false;
if (defaultLogicConditions) {
logicCondition_t defaultValue = defaultLogicConditions[i];
if (defaultProgrammingPids) {
programmingPid_t defaultValue = defaultProgrammingPids[i];
equalsDefault =
logic.enabled == defaultValue.enabled &&
logic.activatorId == defaultValue.activatorId &&
logic.operation == defaultValue.operation &&
logic.operandA.type == defaultValue.operandA.type &&
logic.operandA.value == defaultValue.operandA.value &&
logic.operandB.type == defaultValue.operandB.type &&
logic.operandB.value == defaultValue.operandB.value &&
logic.flags == defaultValue.flags;
pid.enabled == defaultValue.enabled &&
pid.setpoint.type == defaultValue.setpoint.type &&
pid.setpoint.value == defaultValue.setpoint.value &&
pid.measurement.type == defaultValue.measurement.type &&
pid.measurement.value == defaultValue.measurement.value &&
pid.gains.P == defaultValue.gains.P &&
pid.gains.I == defaultValue.gains.I &&
pid.gains.D == defaultValue.gains.D &&
pid.gains.FF == defaultValue.gains.FF;
cliDefaultPrintLinef(dumpMask, equalsDefault, format,
i,
logic.enabled,
logic.activatorId,
logic.operation,
logic.operandA.type,
logic.operandA.value,
logic.operandB.type,
logic.operandB.value,
logic.flags
pid.enabled,
pid.setpoint.type,
pid.setpoint.value,
pid.measurement.type,
pid.measurement.value,
pid.gains.P,
pid.gains.I,
pid.gains.D,
pid.gains.FF
);
}
cliDumpPrintLinef(dumpMask, equalsDefault, format,
i,
logic.enabled,
logic.activatorId,
logic.operation,
logic.operandA.type,
logic.operandA.value,
logic.operandB.type,
logic.operandB.value,
logic.flags
pid.enabled,
pid.setpoint.type,
pid.setpoint.value,
pid.measurement.type,
pid.measurement.value,
pid.gains.P,
pid.gains.I,
pid.gains.D,
pid.gains.FF
);
}
}
@ -2052,7 +2056,7 @@ static void cliPid(char *cmdline) {
uint8_t len = strlen(cmdline);
if (len == 0) {
printLogic(DUMP_MASTER, programmingPids(0), NULL);
printPid(DUMP_MASTER, programmingPids(0), NULL);
} else if (sl_strncasecmp(cmdline, "reset", 5) == 0) {
pgResetCopy(programmingPidsMutable(0), PG_LOGIC_CONDITIONS);
} else {
@ -2082,24 +2086,28 @@ static void cliPid(char *cmdline) {
int32_t i = args[INDEX];
if (
i >= 0 && i < MAX_LOGIC_CONDITIONS &&
i >= 0 && i < MAX_PROGRAMMING_PID_COUNT &&
args[ENABLED] >= 0 && args[ENABLED] <= 1 &&
args[ACTIVATOR_ID] >= -1 && args[ACTIVATOR_ID] < MAX_LOGIC_CONDITIONS &&
args[OPERATION] >= 0 && args[OPERATION] < LOGIC_CONDITION_LAST &&
args[OPERAND_A_TYPE] >= 0 && args[OPERAND_A_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
args[OPERAND_A_VALUE] >= -1000000 && args[OPERAND_A_VALUE] <= 1000000 &&
args[OPERAND_B_TYPE] >= 0 && args[OPERAND_B_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
args[OPERAND_B_VALUE] >= -1000000 && args[OPERAND_B_VALUE] <= 1000000 &&
args[FLAGS] >= 0 && args[FLAGS] <= 255
args[SETPOINT_TYPE] >= 0 && args[SETPOINT_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
args[SETPOINT_VALUE] >= -1000000 && args[SETPOINT_VALUE] <= 1000000 &&
args[MEASUREMENT_TYPE] >= 0 && args[MEASUREMENT_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
args[MEASUREMENT_VALUE] >= -1000000 && args[MEASUREMENT_VALUE] <= 1000000 &&
args[P_GAIN] >= 0 && args[P_GAIN] <= INT16_MAX &&
args[I_GAIN] >= 0 && args[I_GAIN] <= INT16_MAX &&
args[D_GAIN] >= 0 && args[D_GAIN] <= INT16_MAX &&
args[FF_GAIN] >= 0 && args[FF_GAIN] <= INT16_MAX
) {
programmingPidsMutable(i)->enabled = args[ENABLED];
logicConditionsMutable(i)->setpoint.type = args[OPERAND_A_TYPE];
logicConditionsMutable(i)->setpoint.value = args[OPERAND_A_VALUE];
logicConditionsMutable(i)->measurement.type = args[OPERAND_B_TYPE];
logicConditionsMutable(i)->measurement.value = args[OPERAND_B_VALUE];
programmingPidsMutable(i)->setpoint.type = args[SETPOINT_TYPE];
programmingPidsMutable(i)->setpoint.value = args[SETPOINT_VALUE];
programmingPidsMutable(i)->measurement.type = args[MEASUREMENT_TYPE];
programmingPidsMutable(i)->measurement.value = args[MEASUREMENT_VALUE];
programmingPidsMutable(i)->gains.P = args[P_GAIN];
programmingPidsMutable(i)->gains.I = args[I_GAIN];
programmingPidsMutable(i)->gains.D = args[D_GAIN];
programmingPidsMutable(i)->gains.FF = args[FF_GAIN];
cliLogic("");
cliPid("");
} else {
cliShowParseError();
}
@ -3422,7 +3430,7 @@ static void printConfig(const char *cmdline, bool doDiff)
printGvar(dumpMask, globalVariableConfigs_CopyArray, globalVariableConfigs(0));
cliPrintHashLine("pid");
printLogic(dumpMask, programmingPids_CopyArray, programmingPids(0));
printPid(dumpMask, programmingPids_CopyArray, programmingPids(0));
#endif
cliPrintHashLine("feature");

3
src/main/fc/fc_core.c
View file

@ -89,6 +89,7 @@ FILE_COMPILE_FOR_SPEED
#include "flight/failsafe.h"
#include "config/feature.h"
#include "programming/pid.h"
// June 2013 V2.2-dev
@ -390,6 +391,7 @@ void disarm(disarmReason_t disarmReason)
statsOnDisarm();
logicConditionReset();
programmingPidReset();
beeper(BEEPER_DISARMING); // emit disarm tone
}
}
@ -480,6 +482,7 @@ void tryArm(void)
//It is required to inform the mixer that arming was executed and it has to switch to the FORWARD direction
ENABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD);
logicConditionReset();
programmingPidReset();
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));

14
src/main/fc/fc_msp.c
View file

@ -686,18 +686,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
case MSP_PID:
for (int i = 0; i < PID_ITEM_COUNT; i++) {
sbufWriteU8(dst, pidBank()->pid[i].P);
sbufWriteU8(dst, pidBank()->pid[i].I);
sbufWriteU8(dst, pidBank()->pid[i].D);
sbufWriteU8(dst, constrain(pidBank()->pid[i].P, 0, 255));
sbufWriteU8(dst, constrain(pidBank()->pid[i].I, 0, 255));
sbufWriteU8(dst, constrain(pidBank()->pid[i].D, 0, 255));
}
break;
case MSP2_PID:
for (int i = 0; i < PID_ITEM_COUNT; i++) {
sbufWriteU8(dst, pidBank()->pid[i].P);
sbufWriteU8(dst, pidBank()->pid[i].I);
sbufWriteU8(dst, pidBank()->pid[i].D);
sbufWriteU8(dst, pidBank()->pid[i].FF);
sbufWriteU8(dst, constrain(pidBank()->pid[i].P, 0, 255));
sbufWriteU8(dst, constrain(pidBank()->pid[i].I, 0, 255));
sbufWriteU8(dst, constrain(pidBank()->pid[i].D, 0, 255));
sbufWriteU8(dst, constrain(pidBank()->pid[i].FF, 0, 255));
}
break;

8
src/main/flight/pid.h
View file

@ -81,10 +81,10 @@ typedef enum {
} pidType_e;
typedef struct pid8_s {
uint8_t P;
uint8_t I;
uint8_t D;
uint8_t FF;
uint16_t P;
uint16_t I;
uint16_t D;
uint16_t FF;
} pid8_t;
typedef struct pidBank_s {

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

@ -1962,6 +1962,10 @@ float navPidApply3(
}
}
if (pidFlags & PID_LIMIT_INTEGRATOR) {
pid->integrator = constrainf(pid->integrator, outMin, outMax);
}
return outValConstrained;
}

1
src/main/navigation/navigation_private.h
View file

@ -95,6 +95,7 @@ typedef enum {
PID_DTERM_FROM_ERROR = 1 << 0,
PID_ZERO_INTEGRATOR = 1 << 1,
PID_SHRINK_INTEGRATOR = 1 << 2,
PID_LIMIT_INTEGRATOR = 1 << 3,
} pidControllerFlags_e;
typedef struct {

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

@ -30,6 +30,7 @@
#include "programming/logic_condition.h"
#include "programming/global_variables.h"
#include "programming/pid.h"
#include "common/utils.h"
#include "rx/rx.h"
#include "common/maths.h"
@ -595,6 +596,12 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
}
break;
case LOGIC_CONDITION_OPERAND_TYPE_PID:
if (operand >= 0 && operand < MAX_PROGRAMMING_PID_COUNT) {
retVal = programmingPidGetOutput(operand);
}
break;
default:
break;
}

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

@ -77,6 +77,7 @@ typedef enum logicOperandType_s {
LOGIC_CONDITION_OPERAND_TYPE_FLIGHT_MODE,
LOGIC_CONDITION_OPERAND_TYPE_LC, // Result of different LC and LC operand
LOGIC_CONDITION_OPERAND_TYPE_GVAR, // Value from a global variable
LOGIC_CONDITION_OPERAND_TYPE_PID, // Value from a Programming PID
LOGIC_CONDITION_OPERAND_TYPE_LAST
} logicOperandType_e;

40
src/main/programming/pid.c
View file

@ -35,6 +35,7 @@ FILE_COMPILE_FOR_SIZE
#include "navigation/navigation_private.h"
#include "programming/pid.h"
#include "programming/logic_condition.h"
EXTENDED_FASTRAM programmingPidState_t programmingPidState[MAX_PROGRAMMING_PID_COUNT];
static bool pidsInitiated = false;
@ -66,13 +67,31 @@ void pgResetFn_programmingPids(programmingPid_t *instance)
void programmingPidUpdateTask(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
static timeUs_t previousUpdateTimeUs;
const float dT = US2S(currentTimeUs - previousUpdateTimeUs);
if (!pidsInitiated) {
programmingPidInit();
pidsInitiated = true;
}
for (uint8_t i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
if (programmingPids(i)->enabled) {
const int setpoint = logicConditionGetOperandValue(programmingPids(i)->setpoint.type, programmingPids(i)->setpoint.value);
const int measurement = logicConditionGetOperandValue(programmingPids(i)->measurement.type, programmingPids(i)->measurement.value);
programmingPidState[i].output = navPidApply2(
&programmingPidState[i].controller,
setpoint,
measurement,
dT,
-1000,
1000,
PID_LIMIT_INTEGRATOR
);
}
}
}
void programmingPidInit(void)
@ -80,13 +99,24 @@ void programmingPidInit(void)
for (uint8_t i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
navPidInit(
&programmingPidState[i].controller,
programmingPids(i)->gains.P / 100.0f,
programmingPids(i)->gains.I / 100.0f,
programmingPids(i)->gains.D / 100.0f,
programmingPids(i)->gains.FF / 100.0f,
programmingPids(i)->gains.P / 1000.0f,
programmingPids(i)->gains.I / 1000.0f,
programmingPids(i)->gains.D / 1000.0f,
programmingPids(i)->gains.FF / 1000.0f,
5.0f
);
}
}
int programmingPidGetOutput(uint8_t i) {
return programmingPidState[constrain(i, 0, MAX_PROGRAMMING_PID_COUNT)].output;
}
void programmingPidReset(void)
{
for (uint8_t i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
navPidReset(&programmingPidState[i].controller);
}
}
#endif

6
src/main/programming/pid.h
View file

@ -32,7 +32,7 @@
#include "flight/pid.h"
#include "navigation/navigation.h"
#define MAX_PROGRAMMING_PID_COUNT 2
#define MAX_PROGRAMMING_PID_COUNT 4
typedef struct programmingPid_s {
uint8_t enabled;
@ -45,8 +45,10 @@ PG_DECLARE_ARRAY(programmingPid_t, MAX_PROGRAMMING_PID_COUNT, programmingPids);
typedef struct programmingPidState_s {
pidController_t controller;
logicOperand_t setpoint;
float output;
} programmingPidState_t;
void programmingPidUpdateTask(timeUs_t currentTimeUs);
void programmingPidInit(void);
void programmingPidReset(void);
int programmingPidGetOutput(uint8_t i);