Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 08:15:26 +03:00
Code Issues Wiki Activity

Merge pull request #17 from iNavFlight/master

Browse source 
Sync with master
This commit is contained in:
breadoven 2020-12-31 12:45:52 +00:00 • committed by GitHub
commit febd8afec1
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
32 changed files with 570 additions and 33 deletions
Download patch file Download diff file Expand all files Collapse all files

29
docs/Programming Framework.md
View file

@ -9,10 +9,13 @@ INAV Programming Framework coinsists of:
* Logic Conditions - each Logic Condition can be understood as a single command, a single line of code
* Global Variables - variables that can store values from and for LogiC Conditions and servo mixer
* Programming PID - general purpose, user configurable PID controllers
IPF can be edited using INAV Configurator user interface, of via CLI
## CLI
## Logic Conditions
### CLI
`logic <rule> <enabled> <activatorId> <operation> <operand A type> <operand A value> <operand B type> <operand B value> <flags>`
@ -80,6 +83,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
@ -120,7 +124,7 @@ IPF can be edited using INAV Configurator user interface, of via CLI
| 32 | CROSSFIRE LQ | Crossfire Link quality as returned by the CRSF protocol |
| 33 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol |
##### ACTIVE_WAYPOINT_ACTION
#### ACTIVE_WAYPOINT_ACTION
| Action | Value |
|---- |---- |
@ -158,6 +162,27 @@ All flags are reseted on ARM and DISARM event.
|---- |---- |---- |
| 0 | 1 | Latch - after activation LC will stay active until LATCH flag is reseted |
## Global variables
### CLI
`gvar <index> <default value> <min> <max>`
## Programming PID
`pid <index> <enabled> <setpoint type> <setpoint value> <measurement type> <measurement value> <P gain> <I gain> <D gain> <FF gain>`
* `<index>` - ID of PID Controller, starting from `0`
* `<enabled>` - `0` evaluates as disabled, `1` evaluates as enabled
* `<setpoint type>` - See `Operands` paragraph
* `<setpoint value>` - See `Operands` paragraph
* `<measurement type>` - See `Operands` paragraph
* `<measurement value>` - See `Operands` paragraph
* `<P gain>` - P-gain, scaled to `1/1000`
* `<I gain>` - I-gain, scaled to `1/1000`
* `<D gain>` - D-gain, scaled to `1/1000`
* `<FF gain>` - FF-gain, scaled to `1/1000`
## Examples
### Dynamic THROTTLE scale

6
docs/Settings.md
View file

@ -137,6 +137,9 @@
| gps_provider | UBLOX | Which GPS protocol to be used, note that UBLOX is 5Hz and UBLOX7 is 10Hz (M8N). |
| gps_sbas_mode | NONE | Which SBAS mode to be used |
| gps_ublox_use_galileo | OFF | Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON]. |
| gyro_dyn_lpf_curve_expo | 5 | Expo value for the throttle-to-frequency mapping for Dynamic LPF |
| gyro_dyn_lpf_max_hz | 500 | Maximum frequency of the gyro Dynamic LPF |
| gyro_dyn_lpf_min_hz | 200 | Minimum frequency of the gyro Dynamic LPF |
| gyro_hardware_lpf | 42HZ | Hardware lowpass filter for gyro. Allowed values depend on the driver - For example MPU6050 allows 10HZ,20HZ,42HZ,98HZ,188HZ,256Hz (8khz mode). If you have to set gyro lpf below 42Hz generally means the frame is vibrating too much, and that should be fixed first. |
| gyro_lpf_hz | 60 | Software-based filter to remove mechanical vibrations from the gyro signal. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. |
| gyro_lpf_type | BIQUAD | Specifies the type of the software LPF of the gyro signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. |
@ -146,6 +149,7 @@
| gyro_stage2_lowpass_type | `BIQUAD` | Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. |
| gyro_sync | OFF | This option enables gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Maximum gyro refresh rate is determined by gyro_hardware_lpf |
| gyro_to_use | | |
| gyro_use_dyn_lpf | OFF | Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position. |
| has_flaps | OFF | Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot |
| heading_hold_rate_limit | 90 | This setting limits yaw rotation rate that HEADING_HOLD controller can request from PID inner loop controller. It is independent from manual yaw rate and used only when HEADING_HOLD flight mode is enabled by pilot, RTH or WAYPOINT modes. |
| hott_alarm_sound_interval | 5 | Battery alarm delay in seconds for Hott telemetry |
@ -324,7 +328,7 @@
| nav_mc_vel_z_i | 50 | I gain of velocity PID controller |
| nav_mc_vel_z_p | 100 | P gain of velocity PID controller |
| nav_min_rth_distance | 500 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. |
| nav_overrides_motor_stop | ALL_NAV | When set OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF always the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV |
| nav_overrides_motor_stop | ALL_NAV | When set to OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV |
| nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) |
| nav_rth_abort_threshold | 50000 | RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm] |
| nav_rth_allow_landing | ALWAYS | If set to ON drone will land as a last phase of RTH. |

4
src/main/CMakeLists.txt
View file

@ -321,6 +321,8 @@ main_sources(COMMON_SRC
flight/rpm_filter.h
flight/dynamic_gyro_notch.c
flight/dynamic_gyro_notch.h
flight/dynamic_lpf.c
flight/dynamic_lpf.h
io/beeper.c
io/beeper.h
@ -362,6 +364,8 @@ main_sources(COMMON_SRC
programming/global_variables.h
programming/programming_task.c
programming/programming_task.h
programming/pid.c
programming/pid.h
rx/crsf.c
rx/crsf.h

1
src/main/build/debug.h
View file

@ -79,5 +79,6 @@ typedef enum {
DEBUG_SPM_VS600, // Smartport master VS600 VTX
DEBUG_SPM_VARIO, // Smartport master variometer
DEBUG_PCF8574,
DEBUG_DYNAMIC_GYRO_LPF,
DEBUG_COUNT
} debugType_e;

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)

24
src/main/common/filter.c
View file

@ -28,9 +28,6 @@ FILE_COMPILE_FOR_SPEED
#include "common/maths.h"
#include "common/utils.h"
#define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */
#define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - butterworth*/
// NULL filter
float nullFilterApply(void *filter, float input)
{
@ -48,17 +45,23 @@ float nullFilterApply4(void *filter, float input, float f_cut, float dt)
// PT1 Low Pass filter
static float pt1ComputeRC(const float f_cut)
{
return 1.0f / (2.0f * M_PIf * f_cut);
}
// f_cut = cutoff frequency
void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT)
{
filter->state = 0.0f;
filter->RC = tau;
filter->dT = dT;
filter->alpha = filter->dT / (filter->RC + filter->dT);
}
void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT)
{
pt1FilterInitRC(filter, 1.0f / (2.0f * M_PIf * f_cut), dT);
pt1FilterInitRC(filter, pt1ComputeRC(f_cut), dT);
}
void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau) {
@ -69,9 +72,15 @@ float pt1FilterGetLastOutput(pt1Filter_t *filter) {
return filter->state;
}
void pt1FilterUpdateCutoff(pt1Filter_t *filter, float f_cut)
{
filter->RC = pt1ComputeRC(f_cut);
filter->alpha = filter->dT / (filter->RC + filter->dT);
}
float FAST_CODE NOINLINE pt1FilterApply(pt1Filter_t *filter, float input)
{
filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state);
filter->state = filter->state + filter->alpha * (input - filter->state);
return filter->state;
}
@ -86,11 +95,12 @@ float FAST_CODE NOINLINE pt1FilterApply4(pt1Filter_t *filter, float input, float
{
// Pre calculate and store RC
if (!filter->RC) {
filter->RC = 1.0f / ( 2.0f * M_PIf * f_cut );
filter->RC = pt1ComputeRC(f_cut);
}
filter->dT = dT; // cache latest dT for possible use in pt1FilterApply
filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
filter->alpha = filter->dT / (filter->RC + filter->dT);
filter->state = filter->state + filter->alpha * (input - filter->state);
return filter->state;
}

5
src/main/common/filter.h
View file

@ -25,6 +25,7 @@ typedef struct pt1Filter_s {
float state;
float RC;
float dT;
float alpha;
} pt1Filter_t;
/* this holds the data required to update samples thru a filter */
@ -58,12 +59,16 @@ typedef struct firFilter_s {
typedef float (*filterApplyFnPtr)(void *filter, float input);
typedef float (*filterApply4FnPtr)(void *filter, float input, float f_cut, float dt);
#define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */
#define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - butterworth*/
float nullFilterApply(void *filter, float input);
float nullFilterApply4(void *filter, float input, float f_cut, float dt);
void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT);
void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT);
void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau);
void pt1FilterUpdateCutoff(pt1Filter_t *filter, float f_cut);
float pt1FilterGetLastOutput(pt1Filter_t *filter);
float pt1FilterApply(pt1Filter_t *filter, float input);
float pt1FilterApply3(pt1Filter_t *filter, float input, float dT);

3
src/main/config/parameter_group_ids.h
View file

@ -115,7 +115,8 @@
#define PG_OSD_LAYOUTS_CONFIG 1025
#define PG_SAFE_HOME_CONFIG 1026
#define PG_DJI_OSD_CONFIG 1027
#define PG_INAV_END 1027
#define PG_PROGRAMMING_PID 1028
#define PG_INAV_END 1028
// OSD configuration (subject to change)
//#define PG_OSD_FONT_CONFIG 2047

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

@ -42,6 +42,7 @@ uint8_t cliMode = 0;
#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"
@ -1998,6 +1999,118 @@ static void cliGvar(char *cmdline) {
}
}
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_PROGRAMMING_PID_COUNT; i++) {
const programmingPid_t pid = programmingPids[i];
bool equalsDefault = false;
if (defaultProgrammingPids) {
programmingPid_t defaultValue = defaultProgrammingPids[i];
equalsDefault =
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,
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,
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
);
}
}
static void cliPid(char *cmdline) {
char * saveptr;
int args[10], check = 0;
uint8_t len = strlen(cmdline);
if (len == 0) {
printPid(DUMP_MASTER, programmingPids(0), NULL);
} else if (sl_strncasecmp(cmdline, "reset", 5) == 0) {
pgResetCopy(programmingPidsMutable(0), PG_LOGIC_CONDITIONS);
} else {
enum {
INDEX = 0,
ENABLED,
SETPOINT_TYPE,
SETPOINT_VALUE,
MEASUREMENT_TYPE,
MEASUREMENT_VALUE,
P_GAIN,
I_GAIN,
D_GAIN,
FF_GAIN,
ARGS_COUNT
};
char *ptr = strtok_r(cmdline, " ", &saveptr);
while (ptr != NULL && check < ARGS_COUNT) {
args[check++] = fastA2I(ptr);
ptr = strtok_r(NULL, " ", &saveptr);
}
if (ptr != NULL || check != ARGS_COUNT) {
cliShowParseError();
return;
}
int32_t i = args[INDEX];
if (
i >= 0 && i < MAX_PROGRAMMING_PID_COUNT &&
args[ENABLED] >= 0 && args[ENABLED] <= 1 &&
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];
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];
cliPid("");
} else {
cliShowParseError();
}
}
}
#endif
#ifdef USE_SDCARD
@ -3312,6 +3425,9 @@ static void printConfig(const char *cmdline, bool doDiff)
cliPrintHashLine("gvar");
printGvar(dumpMask, globalVariableConfigs_CopyArray, globalVariableConfigs(0));
cliPrintHashLine("pid");
printPid(dumpMask, programmingPids_CopyArray, programmingPids(0));
#endif
cliPrintHashLine("feature");
@ -3566,6 +3682,10 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("gvar", "configure global variables",
"<gvar> <default> <min> <max>\r\n"
"\treset\r\n", cliGvar),
CLI_COMMAND_DEF("pid", "configurable PID controllers",
"<#> <enabled> <setpoint type> <setpoint value> <measurement type> <measurement value> <P gain> <I gain> <D gain> <FF gain>\r\n"
"\treset\r\n", cliPid),
#endif
CLI_COMMAND_DEF("set", "change setting", "[<name>=<value>]", cliSet),
CLI_COMMAND_DEF("smix", "servo mixer",

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));

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

@ -36,6 +36,7 @@
#include "common/time.h"
#include "common/utils.h"
#include "programming/global_variables.h"
#include "programming/pid.h"
#include "config/parameter_group_ids.h"
@ -553,6 +554,19 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU32(dst, gvGet(i));
}
break;
case MSP2_INAV_PROGRAMMING_PID:
for (int i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
sbufWriteU8(dst, programmingPids(i)->enabled);
sbufWriteU8(dst, programmingPids(i)->setpoint.type);
sbufWriteU32(dst, programmingPids(i)->setpoint.value);
sbufWriteU8(dst, programmingPids(i)->measurement.type);
sbufWriteU32(dst, programmingPids(i)->measurement.value);
sbufWriteU16(dst, programmingPids(i)->gains.P);
sbufWriteU16(dst, programmingPids(i)->gains.I);
sbufWriteU16(dst, programmingPids(i)->gains.D);
sbufWriteU16(dst, programmingPids(i)->gains.FF);
}
break;
#endif
case MSP2_COMMON_MOTOR_MIXER:
for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
@ -686,18 +700,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;
@ -1969,6 +1983,22 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
} else
return MSP_RESULT_ERROR;
break;
case MSP2_INAV_SET_PROGRAMMING_PID:
sbufReadU8Safe(&tmp_u8, src);
if ((dataSize == 20) && (tmp_u8 < MAX_PROGRAMMING_PID_COUNT)) {
programmingPidsMutable(tmp_u8)->enabled = sbufReadU8(src);
programmingPidsMutable(tmp_u8)->setpoint.type = sbufReadU8(src);
programmingPidsMutable(tmp_u8)->setpoint.value = sbufReadU32(src);
programmingPidsMutable(tmp_u8)->measurement.type = sbufReadU8(src);
programmingPidsMutable(tmp_u8)->measurement.value = sbufReadU32(src);
programmingPidsMutable(tmp_u8)->gains.P = sbufReadU16(src);
programmingPidsMutable(tmp_u8)->gains.I = sbufReadU16(src);
programmingPidsMutable(tmp_u8)->gains.D = sbufReadU16(src);
programmingPidsMutable(tmp_u8)->gains.FF = sbufReadU16(src);
} else
return MSP_RESULT_ERROR;
break;
#endif
case MSP2_COMMON_SET_MOTOR_MIXER:
sbufReadU8Safe(&tmp_u8, src);

4
src/main/fc/fc_tasks.c
View file

@ -49,6 +49,7 @@
#include "flight/wind_estimator.h"
#include "flight/rpm_filter.h"
#include "flight/servos.h"
#include "flight/dynamic_lpf.h"
#include "navigation/navigation.h"
@ -294,6 +295,7 @@ void taskUpdateAux(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
updatePIDCoefficients();
dynamicLpfGyroTask();
}
void fcTasksInit(void)
@ -620,7 +622,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
[TASK_AUX] = {
.taskName = "AUX",
.taskFunc = taskUpdateAux,
.desiredPeriod = TASK_PERIOD_HZ(TASK_AUX_RATE_HZ), // 300Hz @3,33ms
.desiredPeriod = TASK_PERIOD_HZ(TASK_AUX_RATE_HZ), // 100Hz @10ms
.staticPriority = TASK_PRIORITY_HIGH,
},
};

4
src/main/fc/rc_adjustments.c
View file

@ -360,9 +360,9 @@ static void applyAdjustmentManualRate(adjustmentFunction_e adjustmentFunction, u
return applyAdjustmentU8(adjustmentFunction, val, delta, RC_ADJUSTMENT_MANUAL_RATE_MIN, RC_ADJUSTMENT_MANUAL_RATE_MAX);
}
static void applyAdjustmentPID(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta)
static void applyAdjustmentPID(adjustmentFunction_e adjustmentFunction, uint16_t *val, int delta)
{
applyAdjustmentU8(adjustmentFunction, val, delta, RC_ADJUSTMENT_PID_MIN, RC_ADJUSTMENT_PID_MAX);
applyAdjustmentU16(adjustmentFunction, val, delta, RC_ADJUSTMENT_PID_MIN, RC_ADJUSTMENT_PID_MAX);
}
static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta)

25
src/main/fc/settings.yaml
View file

@ -84,7 +84,7 @@ tables:
"FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
"VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
"ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
"IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574"]
"IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF"]
- name: async_mode
values: ["NONE", "GYRO", "ALL"]
- name: aux_operator
@ -214,6 +214,29 @@ groups:
default_value: "`BIQUAD`"
field: gyro_stage2_lowpass_type
table: filter_type
- name: gyro_use_dyn_lpf
description: "Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position."
default_value: "OFF"
field: useDynamicLpf
type: bool
- name: gyro_dyn_lpf_min_hz
description: "Minimum frequency of the gyro Dynamic LPF"
default_value: "200"
field: gyroDynamicLpfMinHz
min: 40
max: 400
- name: gyro_dyn_lpf_max_hz
description: "Maximum frequency of the gyro Dynamic LPF"
default_value: "500"
field: gyroDynamicLpfMaxHz
min: 40
max: 1000
- name: gyro_dyn_lpf_curve_expo
description: "Expo value for the throttle-to-frequency mapping for Dynamic LPF"
default_value: "5"
field: gyroDynamicLpfCurveExpo
min: 1
max: 10
- name: dynamic_gyro_notch_enabled
description: "Enable/disable dynamic gyro notch also known as Matrix Filter"
default_value: "`OFF`"

50
src/main/flight/dynamic_lpf.c Normal file
View file

@ -0,0 +1,50 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#include "flight/dynamic_lpf.h"
#include "sensors/gyro.h"
#include "flight/mixer.h"
#include "fc/rc_controls.h"
#include "build/debug.h"
static float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo) {
const float expof = expo / 10.0f;
static float curve;
curve = throttle * (1 - throttle) * expof + throttle;
return (dynLpfMax - dynLpfMin) * curve + dynLpfMin;
}
void dynamicLpfGyroTask(void) {
if (!gyroConfig()->useDynamicLpf) {
return;
}
const float throttle = scaleRangef((float) rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle, 0.0f, 1.0f);
const float cutoffFreq = dynLpfCutoffFreq(throttle, gyroConfig()->gyroDynamicLpfMinHz, gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpfCurveExpo);
DEBUG_SET(DEBUG_DYNAMIC_GYRO_LPF, 0, cutoffFreq);
gyroUpdateDynamicLpf(cutoffFreq);
}

29
src/main/flight/dynamic_lpf.h Normal file
View file

@ -0,0 +1,29 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#pragma once
#include <stdint.h>
void dynamicLpfGyroTask(void);

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

@ -1116,7 +1116,7 @@ pidBank_t * pidBankMutable(void) {
return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc;
}
uint8_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex)
uint16_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex)
{
if (pidIndexGetType(pidIndex) == PID_TYPE_PIFF) {
return &pidBank->pid[pidIndex].FF;

10
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 {
@ -199,4 +199,4 @@ void autotuneUpdateState(void);
void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput);
pidType_e pidIndexGetType(pidIndex_e pidIndex);
uint8_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex);
uint16_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex);

11
src/main/io/gps_ublox.c
View file

@ -651,7 +651,16 @@ static bool gpsParceFrameUBLOX(void)
// EXT CORE 3.01 (107900)
// 01234567890123456789012
gpsState.hwVersion = fastA2I(_buffer.ver.hwVersion);
capGalileo = ((gpsState.hwVersion >= 80000) && (_buffer.ver.swVersion[9] > '2')); // M8N and SW major 3 or later
if ((gpsState.hwVersion >= 80000) && (_buffer.ver.swVersion[9] > '2')) {
// check extensions;
// after hw + sw vers; each is 30 bytes
for(int j = 40; j < _payload_length; j += 30) {
if (strnstr((const char *)(_buffer.bytes+j), "GAL", 30)) {
capGalileo = true;
break;
}
}
}
}
break;
case MSG_ACK_ACK:

2
src/main/msp/msp_protocol_v2_inav.h
View file

@ -61,6 +61,8 @@
#define MSP2_INAV_SET_GLOBAL_FUNCTIONS 0x2025
#define MSP2_INAV_LOGIC_CONDITIONS_STATUS 0x2026
#define MSP2_INAV_GVAR_STATUS 0x2027
#define MSP2_INAV_PROGRAMMING_PID 0x2028
#define MSP2_INAV_SET_PROGRAMMING_PID 0x2029
#define MSP2_PID 0x2030
#define MSP2_SET_PID 0x2031

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

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

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

@ -25,7 +25,9 @@
#include "common/maths.h"
#include "common/filter.h"
#include "common/time.h"
#include "common/vector.h"
#include "fc/runtime_config.h"
#include "navigation/navigation.h"
#define MIN_POSITION_UPDATE_RATE_HZ 5 // Minimum position update rate at which XYZ controllers would be applied
#define NAV_THROTTLE_CUTOFF_FREQENCY_HZ 4 // low-pass filter on throttle output
@ -93,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;

124
src/main/programming/pid.c Normal file
View file

@ -0,0 +1,124 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#include "platform.h"
FILE_COMPILE_FOR_SIZE
#ifdef USE_PROGRAMMING_FRAMEWORK
#include "common/utils.h"
#include "config/config_reset.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#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;
PG_REGISTER_ARRAY_WITH_RESET_FN(programmingPid_t, MAX_PROGRAMMING_PID_COUNT, programmingPids, PG_PROGRAMMING_PID, 1);
void pgResetFn_programmingPids(programmingPid_t *instance)
{
for (int i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
RESET_CONFIG(programmingPid_t, &instance[i],
.enabled = 0,
.setpoint = {
.type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
.value = 0
},
.measurement = {
.type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
.value = 0
},
.gains = {
.P = 0,
.I = 0,
.D = 0,
.FF = 0,
}
);
}
}
void programmingPidUpdateTask(timeUs_t 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
);
}
}
previousUpdateTimeUs = currentTimeUs;
}
void programmingPidInit(void)
{
for (uint8_t i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
navPidInit(
&programmingPidState[i].controller,
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

54
src/main/programming/pid.h Normal file
View file

@ -0,0 +1,54 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#pragma once
#include "config/parameter_group.h"
#include "common/time.h"
#include "programming/logic_condition.h"
#include "common/axis.h"
#include "flight/pid.h"
#include "navigation/navigation.h"
#define MAX_PROGRAMMING_PID_COUNT 4
typedef struct programmingPid_s {
uint8_t enabled;
logicOperand_t setpoint;
logicOperand_t measurement;
pid8_t gains;
} programmingPid_t;
PG_DECLARE_ARRAY(programmingPid_t, MAX_PROGRAMMING_PID_COUNT, programmingPids);
typedef struct programmingPidState_s {
pidController_t controller;
float output;
} programmingPidState_t;
void programmingPidUpdateTask(timeUs_t currentTimeUs);
void programmingPidInit(void);
void programmingPidReset(void);
int programmingPidGetOutput(uint8_t i);

2
src/main/programming/programming_task.c
View file

@ -27,7 +27,9 @@
FILE_COMPILE_FOR_SIZE
#include "programming/logic_condition.h"
#include "programming/pid.h"
void programmingFrameworkUpdateTask(timeUs_t currentTimeUs) {
programmingPidUpdateTask(currentTimeUs);
logicConditionUpdateTask(currentTimeUs);
}

2
src/main/rx/ghst.c
View file

@ -241,7 +241,7 @@ uint8_t ghstFrameStatus(rxRuntimeConfig_t *rxRuntimeState)
{
UNUSED(rxRuntimeState);
if(serialIsIdle(serialPort)) {
if (serialIsIdle(serialPort)) {
ghstIdle();
}

19
src/main/sensors/gyro.c
View file

@ -59,6 +59,7 @@ FILE_COMPILE_FOR_SPEED
#include "fc/config.h"
#include "fc/runtime_config.h"
#include "fc/rc_controls.h"
#include "io/beeper.h"
#include "io/statusindicator.h"
@ -101,7 +102,7 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState;
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 10);
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 11);
PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_lpf = GYRO_LPF_256HZ,
@ -116,6 +117,10 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_notch_cutoff = 1,
.gyro_stage2_lowpass_hz = 0,
.gyro_stage2_lowpass_type = FILTER_BIQUAD,
.useDynamicLpf = 0,
.gyroDynamicLpfMinHz = 200,
.gyroDynamicLpfMaxHz = 500,
.gyroDynamicLpfCurveExpo = 5,
.dynamicGyroNotchRange = DYN_NOTCH_RANGE_MEDIUM,
.dynamicGyroNotchQ = 120,
.dynamicGyroNotchMinHz = 150,
@ -510,3 +515,15 @@ bool gyroSyncCheckUpdate(void)
return gyroDev[0].intStatusFn(&gyroDev[0]);
}
void gyroUpdateDynamicLpf(float cutoffFreq) {
if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt1FilterUpdateCutoff(&gyroLpfState[axis].pt1, cutoffFreq);
}
} else if (gyroConfig()->gyro_soft_lpf_type == FILTER_BIQUAD) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterUpdate(&gyroLpfState[axis].biquad, cutoffFreq, getLooptime(), BIQUAD_Q, FILTER_LPF);
}
}
}

5
src/main/sensors/gyro.h
View file

@ -70,6 +70,10 @@ typedef struct gyroConfig_s {
uint16_t gyro_notch_cutoff;
uint16_t gyro_stage2_lowpass_hz;
uint8_t gyro_stage2_lowpass_type;
uint8_t useDynamicLpf;
uint16_t gyroDynamicLpfMinHz;
uint16_t gyroDynamicLpfMaxHz;
uint8_t gyroDynamicLpfCurveExpo;
uint8_t dynamicGyroNotchRange;
uint16_t dynamicGyroNotchQ;
uint16_t dynamicGyroNotchMinHz;
@ -87,3 +91,4 @@ bool gyroReadTemperature(void);
int16_t gyroGetTemperature(void);
int16_t gyroRateDps(int axis);
bool gyroSyncCheckUpdate(void);
void gyroUpdateDynamicLpf(float cutoffFreq);

2
src/main/telemetry/ghst.c
View file

@ -163,7 +163,7 @@ bool checkGhstTelemetryState(void)
// Called periodically by the scheduler
void handleGhstTelemetry(timeUs_t currentTimeUs)
{
static uint32_t ghstLastCycleTime;
static timeUs_t ghstLastCycleTime;
if (!ghstTelemetryEnabled) {
return;

2
src/test/unit/sensor_gyro_unittest.cc
View file

@ -37,6 +37,8 @@ extern "C" {
#include "sensors/gyro.h"
#include "sensors/acceleration.h"
#include "sensors/sensors.h"
#include "fc/rc_controls.h"
#include "flight/mixer.h"
extern zeroCalibrationVector_t gyroCalibration;
extern gyroDev_t gyroDev[];