Merge pull request #17 from iNavFlight/master

Sync with master

docs/Programming Framework.md

@@ -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

docs/Settings.md

@@ -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. |

src/main/CMakeLists.txt

@@ -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

src/main/build/debug.h

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

src/main/cms/cms_menu_imu.c

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

src/main/common/filter.c

@@ -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;
 }
 

src/main/common/filter.h

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

src/main/config/parameter_group_ids.h

@@ -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

src/main/fc/cli.c

@@ -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",

src/main/fc/fc_core.c

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