From de21b5ae3e422e31bbe075cec21087e3671ac72e Mon Sep 17 00:00:00 2001 From: IVData Date: Thu, 3 Sep 2020 18:38:10 +1200 Subject: [PATCH 001/103] Added option to output servos on PWM and SBUS --- src/main/drivers/pwm_mapping.c | 3 ++- src/main/drivers/pwm_mapping.h | 3 ++- src/main/drivers/pwm_output.c | 11 +++++++++++ src/main/fc/config.c | 2 +- src/main/fc/fc_tasks.c | 2 +- src/main/fc/settings.yaml | 2 +- 6 files changed, 18 insertions(+), 5 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 8c6eaea463..a7a27b4ede 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -268,7 +268,8 @@ static bool motorsUseHardwareTimers(void) static bool servosUseHardwareTimers(void) { - return servoConfig()->servo_protocol == SERVO_TYPE_PWM; + return servoConfig()->servo_protocol == SERVO_TYPE_PWM || + servoConfig()->servo_protocol == SERVO_TYPE_SBUS_PWM; } static void pwmInitMotors(timMotorServoHardware_t * timOutputs) diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 2c8b8f83a5..7efbf39431 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -52,7 +52,8 @@ typedef enum { typedef enum { SERVO_TYPE_PWM = 0, SERVO_TYPE_SERVO_DRIVER, - SERVO_TYPE_SBUS + SERVO_TYPE_SBUS, + SERVO_TYPE_SBUS_PWM } servoProtocolType_e; typedef enum { diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 6472741a78..0f4e0caf18 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -501,6 +501,12 @@ static void pwmServoWriteStandard(uint8_t index, uint16_t value) } } +static void sbusPwmWriteStandard(uint8_t index, uint16_t value) +{ + pwmServoWriteStandard(index, value); + sbusServoUpdate(index, value); +} + #ifdef USE_PWM_SERVO_DRIVER static void pwmServoWriteExternalDriver(uint8_t index, uint16_t value) { @@ -532,6 +538,11 @@ void pwmServoPreconfigure(void) sbusServoInitialize(); servoWritePtr = sbusServoUpdate; break; + + case SERVO_TYPE_SBUS_PWM: + sbusServoInitialize(); + servoWritePtr = sbusPwmWriteStandard; + break; #endif } } diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 627fd350b5..5fcf8a6d11 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -217,7 +217,7 @@ void validateAndFixConfig(void) #endif #ifndef USE_SERVO_SBUS - if (servoConfig()->servo_protocol == SERVO_TYPE_SBUS) { + if (servoConfig()->servo_protocol == SERVO_TYPE_SBUS || servoConfig()->servo_protocol == SERVO_TYPE_SBUS_PWM) { servoConfigMutable()->servo_protocol = SERVO_TYPE_PWM; } #endif diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 190a299d9d..4ff3e6b9b0 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -346,7 +346,7 @@ void fcTasksInit(void) setTaskEnabled(TASK_STACK_CHECK, true); #endif #if defined(USE_PWM_SERVO_DRIVER) || defined(USE_SERVO_SBUS) - setTaskEnabled(TASK_PWMDRIVER, (servoConfig()->servo_protocol == SERVO_TYPE_SERVO_DRIVER) || (servoConfig()->servo_protocol == SERVO_TYPE_SBUS)); + setTaskEnabled(TASK_PWMDRIVER, (servoConfig()->servo_protocol == SERVO_TYPE_SERVO_DRIVER) || (servoConfig()->servo_protocol == SERVO_TYPE_SBUS) || (servoConfig()->servo_protocol == SERVO_TYPE_SBUS_PWM)); #endif #ifdef USE_CMS #ifdef USE_MSP_DISPLAYPORT diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 65ac689cdd..d4246fc471 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -34,7 +34,7 @@ tables: - name: motor_pwm_protocol values: ["STANDARD", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED", "DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200", "SERIALSHOT"] - name: servo_protocol - values: ["PWM", "SERVO_DRIVER", "SBUS"] + values: ["PWM", "SERVO_DRIVER", "SBUS", "SBUS_PWM"] - name: failsafe_procedure values: ["SET-THR", "DROP", "RTH", "NONE"] - name: current_sensor From 64f3d70002e2890e0f8d8f3c066ff342bb6e3b90 Mon Sep 17 00:00:00 2001 From: IVData Date: Wed, 16 Sep 2020 12:30:17 +1200 Subject: [PATCH 002/103] Fix builds for targets without USE_SERVO_SBUS --- src/main/drivers/pwm_output.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 0f4e0caf18..ba147cc562 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -501,11 +501,13 @@ static void pwmServoWriteStandard(uint8_t index, uint16_t value) } } +#ifdef USE_SERVO_SBUS static void sbusPwmWriteStandard(uint8_t index, uint16_t value) { pwmServoWriteStandard(index, value); sbusServoUpdate(index, value); } +#endif #ifdef USE_PWM_SERVO_DRIVER static void pwmServoWriteExternalDriver(uint8_t index, uint16_t value) From 28448bd7e1259081b3ea5d23de0923b73b140401 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 19 Nov 2020 19:21:12 +0100 Subject: [PATCH 003/103] Precompute k for PT1 filter --- src/main/common/filter.c | 6 ++++-- src/main/common/filter.h | 1 + 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index b3683c3edc..4a3c37aba5 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -54,6 +54,7 @@ void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT) filter->state = 0.0f; filter->RC = tau; filter->dT = dT; + filter->k = filter->dT / (filter->RC + filter->dT); } void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT) @@ -71,7 +72,7 @@ float pt1FilterGetLastOutput(pt1Filter_t *filter) { 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->k * (input - filter->state); return filter->state; } @@ -90,7 +91,8 @@ float FAST_CODE NOINLINE pt1FilterApply4(pt1Filter_t *filter, float input, float } filter->dT = dT; // cache latest dT for possible use in pt1FilterApply - filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state); + filter->k = filter->dT / (filter->RC + filter->dT); + filter->state = filter->state + filter->k * (input - filter->state); return filter->state; } diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 258b13dbe1..e083f9bce7 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -25,6 +25,7 @@ typedef struct pt1Filter_s { float state; float RC; float dT; + float k; } pt1Filter_t; /* this holds the data required to update samples thru a filter */ From 1b4c517de8255e3007192c2f2335a0c9d0c685bf Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 19 Nov 2020 19:33:14 +0100 Subject: [PATCH 004/103] Parameter rename --- src/main/common/filter.c | 8 ++++---- src/main/common/filter.h | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 4a3c37aba5..1cdba06c19 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -54,7 +54,7 @@ void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT) filter->state = 0.0f; filter->RC = tau; filter->dT = dT; - filter->k = filter->dT / (filter->RC + filter->dT); + filter->alpha = filter->dT / (filter->RC + filter->dT); } void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT) @@ -72,7 +72,7 @@ float pt1FilterGetLastOutput(pt1Filter_t *filter) { float FAST_CODE NOINLINE pt1FilterApply(pt1Filter_t *filter, float input) { - filter->state = filter->state + filter->k * (input - filter->state); + filter->state = filter->state + filter->alpha * (input - filter->state); return filter->state; } @@ -91,8 +91,8 @@ float FAST_CODE NOINLINE pt1FilterApply4(pt1Filter_t *filter, float input, float } filter->dT = dT; // cache latest dT for possible use in pt1FilterApply - filter->k = filter->dT / (filter->RC + filter->dT); - filter->state = filter->state + filter->k * (input - filter->state); + filter->alpha = filter->dT / (filter->RC + filter->dT); + filter->state = filter->state + filter->alpha * (input - filter->state); return filter->state; } diff --git a/src/main/common/filter.h b/src/main/common/filter.h index e083f9bce7..b0e782c5c9 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -25,7 +25,7 @@ typedef struct pt1Filter_s { float state; float RC; float dT; - float k; + float alpha; } pt1Filter_t; /* this holds the data required to update samples thru a filter */ From 3e2bfec8c6597addeedce83378d4d9f6a819b54d Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 19 Nov 2020 21:20:41 +0100 Subject: [PATCH 005/103] Settings for Dynamic LPF filter --- src/main/common/filter.c | 15 +++++++++++++-- src/main/common/filter.h | 1 + src/main/fc/settings.yaml | 23 +++++++++++++++++++++++ src/main/sensors/gyro.c | 6 +++++- src/main/sensors/gyro.h | 4 ++++ 5 files changed, 46 insertions(+), 3 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 1cdba06c19..4bc2f88425 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -48,6 +48,11 @@ 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) { @@ -59,7 +64,7 @@ void pt1FilterInitRC(pt1Filter_t *filter, float tau, float 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) { @@ -70,6 +75,12 @@ 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->alpha * (input - filter->state); @@ -87,7 +98,7 @@ 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 diff --git a/src/main/common/filter.h b/src/main/common/filter.h index b0e782c5c9..ed8500ecdd 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -65,6 +65,7 @@ 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); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 62c5496df7..79b9d007a0 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -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." + 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: 0 + max: 10 - name: dynamic_gyro_notch_enabled description: "Enable/disable dynamic gyro notch also known as Matrix Filter" default_value: "`OFF`" diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index d9cd15891d..0518d8f229 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -101,7 +101,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_42HZ, // 42HZ value is defined for Invensense/TDK gyros @@ -116,6 +116,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, diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index b709294b55..57e6c484ff 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -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; From cf1b17181f76a2461d46bd49f7ab81954cdf6ff9 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 20 Nov 2020 15:15:59 +0100 Subject: [PATCH 006/103] Dynamic update of the gyro stage1 LPF --- src/main/common/filter.c | 3 --- src/main/common/filter.h | 3 +++ src/main/fc/fc_tasks.c | 3 ++- src/main/sensors/gyro.c | 39 +++++++++++++++++++++++++++++++++++++++ src/main/sensors/gyro.h | 1 + 5 files changed, 45 insertions(+), 4 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 4bc2f88425..3a4c9797ef 100644 --- a/src/main/common/filter.c +++ b/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) { diff --git a/src/main/common/filter.h b/src/main/common/filter.h index ed8500ecdd..81c76cd0dc 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -59,6 +59,9 @@ 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); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 190a299d9d..3a4517cf56 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -294,6 +294,7 @@ void taskUpdateAux(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); updatePIDCoefficients(); + gyroUpdateDynamicLpf(); } void fcTasksInit(void) @@ -620,7 +621,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, }, }; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 0518d8f229..5006c3509d 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -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" @@ -72,6 +73,7 @@ FILE_COMPILE_FOR_SPEED #include "flight/gyroanalyse.h" #include "flight/rpm_filter.h" #include "flight/dynamic_gyro_notch.h" +#include "flight/mixer.h" #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" @@ -514,3 +516,40 @@ bool gyroSyncCheckUpdate(void) return gyroDev[0].intStatusFn(&gyroDev[0]); } + +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; +} + +static float dynThrottle(float throttle) { + return throttle * (1 - (throttle * throttle) / 3.0f) * 1.5f; +} + +void gyroUpdateDynamicLpf(void) { + if (!gyroConfig()->useDynamicLpf) { + return; + } + + const float throttle = scaleRangef((float) rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle, 0.0f, 1.0f); + + uint16_t cutoffFreq; + if (gyroConfig()->gyroDynamicLpFCurveExpo > 0) { + cutoffFreq = dynLpfCutoffFreq(throttle, gyroConfig()->gyroDynamicLpfMinHz, gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpFCurveExpo); + } else { + cutoffFreq = fmax(dynThrottle(throttle) * gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpfMinHz); + } + + if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) { + const float gyroDt = getLooptime() * 1e-6f; + 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); + } + } +} \ No newline at end of file diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 57e6c484ff..b5bf66a4b2 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -91,3 +91,4 @@ bool gyroReadTemperature(void); int16_t gyroGetTemperature(void); int16_t gyroRateDps(int axis); bool gyroSyncCheckUpdate(void); +void gyroUpdateDynamicLpf(void); From 390987597634831791cd20a1f4b18add53a770a1 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 20 Nov 2020 16:00:58 +0100 Subject: [PATCH 007/103] Remove unused variable --- src/main/sensors/gyro.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 5006c3509d..7adadf2c10 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -543,7 +543,6 @@ void gyroUpdateDynamicLpf(void) { } if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) { - const float gyroDt = getLooptime() * 1e-6f; for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { pt1FilterUpdateCutoff(&gyroLpfState[axis].pt1, cutoffFreq); } From 94f14286d6a65a3094e629e254a6c4f941dc58d8 Mon Sep 17 00:00:00 2001 From: scavanger Date: Sat, 21 Nov 2020 20:52:04 +0100 Subject: [PATCH 008/103] Let only values blink. --- src/main/io/osd.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 4d7e035919..a5c375b613 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2419,21 +2419,21 @@ static bool osdDrawSingleElement(uint8_t item) char buff[4]; textAttributes_t attr; - displayWrite(osdDisplayPort, elemPosX, elemPosY, "TPA BP"); - + displayWrite(osdDisplayPort, elemPosX, elemPosY, "TPA"); attr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "TPA %3d", currentControlRateProfile->throttle.dynPID); + tfp_sprintf(buff, "%3d", currentControlRateProfile->throttle.dynPID); if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA)) { TEXT_ATTRIBUTES_ADD_BLINK(attr); } - displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, attr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 5, elemPosY, buff, attr); + displayWrite(osdDisplayPort, elemPosX, elemPosY + 1, "BP"); attr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "BP %4d", currentControlRateProfile->throttle.pa_breakpoint); + tfp_sprintf(buff, "%4d", currentControlRateProfile->throttle.pa_breakpoint); if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA_BREAKPOINT)) { TEXT_ATTRIBUTES_ADD_BLINK(attr); } - displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY + 1, buff, attr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY + 1, buff, attr); return true; } From 9dea44a4dafd065f5fb9313fbf27e51b8ed42387 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 25 Nov 2020 08:39:31 +0100 Subject: [PATCH 009/103] Allow Logic Conditions to override RC channels --- src/main/programming/logic_condition.c | 22 ++++++++++++++++++++++ src/main/programming/logic_condition.h | 12 ++++++++++-- src/main/rx/rx.c | 8 +++++++- 3 files changed, 39 insertions(+), 3 deletions(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index b9b35d810d..6432dd4140 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -54,6 +54,7 @@ PG_REGISTER_ARRAY_WITH_RESET_FN(logicCondition_t, MAX_LOGIC_CONDITIONS, logicCon EXTENDED_FASTRAM uint64_t logicConditionsGlobalFlags; EXTENDED_FASTRAM int logicConditionValuesByType[LOGIC_CONDITION_LAST]; +EXTENDED_FASTRAM rcChannelOverride_t rcChannelOverrides[MAX_SUPPORTED_RC_CHANNEL_COUNT]; void pgResetFn_logicConditions(logicCondition_t *instance) { @@ -307,6 +308,14 @@ static int logicConditionCompute( return scaleRange(constrain(operandA, 0, 1000), 0, 1000, 0, operandB); break; + case LOGIC_CONDITION_RC_CHANNEL_OVERRIDE: + temporaryValue = constrain(operandA - 1, 0, MAX_SUPPORTED_RC_CHANNEL_COUNT - 1); + rcChannelOverrides[temporaryValue].active = true; + rcChannelOverrides[temporaryValue].value = constrain(operandB, PWM_RANGE_MIN, PWM_RANGE_MAX); + LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL); + return true; + break; + default: return false; break; @@ -622,6 +631,11 @@ void logicConditionUpdateTask(timeUs_t currentTimeUs) { for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) { logicConditionProcess(i); } + + for (uint8_t i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { + rcChannelOverrides[i].active = false; + } + #ifdef USE_I2C_IO_EXPANDER ioPortExpanderSync(); #endif @@ -663,3 +677,11 @@ int16_t getRcCommandOverride(int16_t command[], uint8_t axis) { return outputValue; } + +int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue) { + if (rcChannelOverrides[channel].active) { + return rcChannelOverrides[channel].value; + } else { + return originalValue; + } +} diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 75905c51ca..8c8167e6ac 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -67,7 +67,8 @@ typedef enum { LOGIC_CONDITION_TAN = 35, LOGIC_CONDITION_MAP_INPUT = 36, LOGIC_CONDITION_MAP_OUTPUT = 37, - LOGIC_CONDITION_LAST = 38, + LOGIC_CONDITION_RC_CHANNEL_OVERRIDE = 38, + LOGIC_CONDITION_LAST = 39, } logicOperation_e; typedef enum logicOperandType_s { @@ -141,6 +142,7 @@ typedef enum { LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_YAW = (1 << 5), LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE = (1 << 6), LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT = (1 << 7), + LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL = (1 << 8), } logicConditionsGlobalFlags_t; typedef enum { @@ -168,6 +170,11 @@ typedef struct logicConditionState_s { uint8_t flags; } logicConditionState_t; +typedef struct rcChannelOverride_s { + uint8_t active; + int value; +} rcChannelOverride_t; + extern int logicConditionValuesByType[LOGIC_CONDITION_LAST]; extern uint64_t logicConditionsGlobalFlags; @@ -184,4 +191,5 @@ void logicConditionUpdateTask(timeUs_t currentTimeUs); void logicConditionReset(void); float getThrottleScale(float globalThrottleScale); -int16_t getRcCommandOverride(int16_t command[], uint8_t axis); \ No newline at end of file +int16_t getRcCommandOverride(int16_t command[], uint8_t axis); +int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue); \ No newline at end of file diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index fa24d6ceb9..1df4ccfac6 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -29,6 +29,8 @@ #include "common/maths.h" #include "common/utils.h" +#include "programming/logic_condition.h" + #include "config/feature.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" @@ -713,7 +715,11 @@ uint16_t rxGetRefreshRate(void) int16_t rxGetChannelValue(unsigned channelNumber) { - return rcChannels[channelNumber].data; + if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL)) { + return getRcChannelOverride(channelNumber, rcChannels[channelNumber].data); + } else { + return rcChannels[channelNumber].data; + } } int16_t rxGetRawChannelValue(unsigned channelNumber) From 0436442e49795dfdbacd6f649ed5b6f387030e99 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Wed, 25 Nov 2020 18:07:28 +0000 Subject: [PATCH 010/103] Added version number to OSD and Armed screen. --- src/main/io/osd.c | 16 ++++++++++++++-- src/main/io/osd.h | 1 + 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 4d7e035919..4d21f828ca 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2067,6 +2067,13 @@ static bool osdDrawSingleElement(uint8_t item) break; } + case OSD_VERSION: + { + tfp_sprintf(buff, "INAV %s", FC_VERSION_STRING); + displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); + break; + } + case OSD_MAIN_BATT_CELL_VOLTAGE: { osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawAverageCellVoltage(), 3, 2); @@ -3030,10 +3037,11 @@ static void osdShowArmed(void) dateTime_t dt; char buf[MAX(32, FORMATTED_DATE_TIME_BUFSIZE)]; char craftNameBuf[MAX_NAME_LENGTH]; + char versionBuf[30]; char *date; char *time; - // We need 10 visible rows - uint8_t y = MIN((osdDisplayPort->rows / 2) - 1, osdDisplayPort->rows - 10 - 1); + // We need 12 visible rows + uint8_t y = MIN((osdDisplayPort->rows / 2) - 1, osdDisplayPort->rows - 12 - 1); displayClearScreen(osdDisplayPort); displayWrite(osdDisplayPort, 12, y, "ARMED"); @@ -3080,7 +3088,11 @@ static void osdShowArmed(void) displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(date)) / 2, y, date); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(time)) / 2, y + 1, time); + y += 3; } + + tfp_sprintf(versionBuf, "INAV VERSION: %s", FC_VERSION_STRING); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(versionBuf)) / 2, y, versionBuf); } static void osdFilterData(timeUs_t currentTimeUs) { diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 01670a320e..51fe9b67d4 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -218,6 +218,7 @@ typedef enum { OSD_GVAR_3, OSD_TPA, OSD_NAV_FW_CONTROL_SMOOTHNESS, + OSD_VERSION, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; From 524e66b1600a3b06b17c9e2e63b410a1939ff14b Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 25 Nov 2020 21:23:37 +0100 Subject: [PATCH 011/103] Fix channel override --- src/main/programming/logic_condition.c | 8 ++++---- src/main/rx/rx.c | 1 - 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 6432dd4140..4c069a8266 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -628,14 +628,14 @@ void logicConditionUpdateTask(timeUs_t currentTimeUs) { //Disable all flags logicConditionsGlobalFlags = 0; + for (uint8_t i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { + rcChannelOverrides[i].active = false; + } + for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) { logicConditionProcess(i); } - for (uint8_t i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { - rcChannelOverrides[i].active = false; - } - #ifdef USE_I2C_IO_EXPANDER ioPortExpanderSync(); #endif diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 1df4ccfac6..734edaf3b3 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -69,7 +69,6 @@ #include "rx/uib_rx.h" #include "rx/xbus.h" - //#define DEBUG_RX_SIGNAL_LOSS const char rcChannelLetters[] = "AERT"; From b190ffa6b5c009472229233e5da23b8a8810dd7c Mon Sep 17 00:00:00 2001 From: scavanger Date: Thu, 26 Nov 2020 19:12:32 +0100 Subject: [PATCH 012/103] Enable softserial1 RX on LED pin, disable LED_STRIP. --- src/main/target/MATEKF411SE/CMakeLists.txt | 1 + src/main/target/MATEKF411SE/target.c | 2 +- src/main/target/MATEKF411SE/target.h | 6 ++++++ 3 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/target/MATEKF411SE/CMakeLists.txt b/src/main/target/MATEKF411SE/CMakeLists.txt index df67c6f00b..3f2dd908bf 100644 --- a/src/main/target/MATEKF411SE/CMakeLists.txt +++ b/src/main/target/MATEKF411SE/CMakeLists.txt @@ -1 +1,2 @@ target_stm32f411xe(MATEKF411SE) +target_stm32f411xe(MATEKF411SE_FD_SFTSRL1) \ No newline at end of file diff --git a/src/main/target/MATEKF411SE/target.c b/src/main/target/MATEKF411SE/target.c index be71e58f2d..879acd6131 100755 --- a/src/main/target/MATEKF411SE/target.c +++ b/src/main/target/MATEKF411SE/target.c @@ -35,7 +35,7 @@ const timerHardware_t timerHardware[] = { DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 pad -softserial_tx2 DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2 Pad -PPM - DEF_TIM(TIM2, CH3, PB10, TIM_USE_LED, 0, 0), //LED 2812 D(1,1,3) + DEF_TIM(TIM2, CH3, PB10, TIM_USE_ANY, 0, 0), // softserial_rx1 - LED 2812 D(1,1,3) }; diff --git a/src/main/target/MATEKF411SE/target.h b/src/main/target/MATEKF411SE/target.h index a91f0eb177..7e322e8401 100755 --- a/src/main/target/MATEKF411SE/target.h +++ b/src/main/target/MATEKF411SE/target.h @@ -70,7 +70,11 @@ #define USE_SOFTSERIAL1 #define SOFTSERIAL_1_TX_PIN PB9 // ST1 pad +#ifdef MATEKF411SE_FD_SFTSRL1 +#define SOFTSERIAL_1_RX_PIN PB10 // LED pad +#else #define SOFTSERIAL_1_RX_PIN PB9 +#endif #define USE_SOFTSERIAL2 #define SOFTSERIAL_2_TX_PIN PA2 // TX2 pad @@ -131,8 +135,10 @@ #define AIRSPEED_ADC_CHANNEL ADC_CHN_4 // *************** LED2812 ************************ +#ifndef MATEKF411SE_FD_SFTSRL1 #define USE_LED_STRIP #define WS2811_PIN PB10 +#endif // *************** PINIO *************************** #define USE_PINIO From 565f41cf1c609130d87edbf51ca2b032d21866c6 Mon Sep 17 00:00:00 2001 From: scavanger Date: Thu, 26 Nov 2020 19:14:03 +0100 Subject: [PATCH 013/103] New line --- src/main/target/MATEKF411SE/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/MATEKF411SE/CMakeLists.txt b/src/main/target/MATEKF411SE/CMakeLists.txt index 3f2dd908bf..fa289c23ff 100644 --- a/src/main/target/MATEKF411SE/CMakeLists.txt +++ b/src/main/target/MATEKF411SE/CMakeLists.txt @@ -1,2 +1,2 @@ target_stm32f411xe(MATEKF411SE) -target_stm32f411xe(MATEKF411SE_FD_SFTSRL1) \ No newline at end of file +target_stm32f411xe(MATEKF411SE_FD_SFTSRL1) From fa077fa1868f7e3fa4aa57c61d086a5fe2901884 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 28 Nov 2020 18:28:54 +0100 Subject: [PATCH 014/103] PID Progrsmming Framework pt1 --- src/main/CMakeLists.txt | 2 ++ src/main/programming/pid.c | 38 +++++++++++++++++++++++++ src/main/programming/pid.h | 30 +++++++++++++++++++ src/main/programming/programming_task.c | 2 ++ 4 files changed, 72 insertions(+) create mode 100644 src/main/programming/pid.c create mode 100644 src/main/programming/pid.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 0bd201a9fb..6bf6e2ec60 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -366,6 +366,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 diff --git a/src/main/programming/pid.c b/src/main/programming/pid.c new file mode 100644 index 0000000000..1040980c1c --- /dev/null +++ b/src/main/programming/pid.c @@ -0,0 +1,38 @@ +/* + * 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 "programming/pid.h" + +void programmingPidUpdateTask(timeUs_t currentTimeUs) +{ + //Dummy +} + +#endif \ No newline at end of file diff --git a/src/main/programming/pid.h b/src/main/programming/pid.h new file mode 100644 index 0000000000..8869a0d6c0 --- /dev/null +++ b/src/main/programming/pid.h @@ -0,0 +1,30 @@ +/* + * 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" + +void programmingPidUpdateTask(timeUs_t currentTimeUs); \ No newline at end of file diff --git a/src/main/programming/programming_task.c b/src/main/programming/programming_task.c index ba26217fc3..19257cb736 100644 --- a/src/main/programming/programming_task.c +++ b/src/main/programming/programming_task.c @@ -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); } \ No newline at end of file From 7516617ec004bb9dafacadaba27e7d8c5bed65cf Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Tue, 1 Dec 2020 21:31:02 +0100 Subject: [PATCH 015/103] [MATEKF405] Add the possibility of MC servo on S7 --- src/main/target/MATEKF405/target.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/MATEKF405/target.c b/src/main/target/MATEKF405/target.c index 1b44217756..d0543ad77e 100644 --- a/src/main/target/MATEKF405/target.c +++ b/src/main/target/MATEKF405/target.c @@ -40,7 +40,7 @@ const timerHardware_t timerHardware[] = { #else DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 UP(2,5) #endif - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S7 D(1,7)!S5 UP(2,6) + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S7 D(1,7)!S5 UP(2,6) DEF_TIM(TIM5, CH3, PA2, TIM_USE_PWM, 0, 0), // TX2 From 4f3abed3ef07809f62f4e778d52f2d781d03040d Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 2 Dec 2020 17:26:56 +0100 Subject: [PATCH 016/103] Configuration --- src/main/config/parameter_group_ids.h | 3 +- src/main/programming/pid.c | 41 +++++++++++++++++++++++++++ src/main/programming/pid.h | 24 +++++++++++++++- 3 files changed, 66 insertions(+), 2 deletions(-) diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 47c1faeb05..6e526ea610 100644 --- a/src/main/config/parameter_group_ids.h +++ b/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 diff --git a/src/main/programming/pid.c b/src/main/programming/pid.c index 1040980c1c..9b09e2ea9d 100644 --- a/src/main/programming/pid.c +++ b/src/main/programming/pid.c @@ -28,11 +28,52 @@ FILE_COMPILE_FOR_SIZE #ifdef USE_PROGRAMMING_FRAMEWORK +#include "config/config_reset.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "programming/pid.h" +EXTENDED_FASTRAM programmingPidState_t programmingPidState[MAX_PROGRAMMING_PID_COUNT]; + +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) { + UNUSED(currentTimeUs); //Dummy } +void programmingPidInit(void) +{ + for (uint8_t i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) { + navPidInit( + + ); + } +} + #endif \ No newline at end of file diff --git a/src/main/programming/pid.h b/src/main/programming/pid.h index 8869a0d6c0..810b9cfa3c 100644 --- a/src/main/programming/pid.h +++ b/src/main/programming/pid.h @@ -27,4 +27,26 @@ #include "config/parameter_group.h" #include "common/time.h" -void programmingPidUpdateTask(timeUs_t currentTimeUs); \ No newline at end of file +#include "programming/logic_condition.h" +#include "common/axis.h" +#include "flight/pid.h" +#include "navigation/navigation.h" + +#define MAX_PROGRAMMING_PID_COUNT 2 + +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; + logicOperand_t setpoint; +} programmingPidState_t; + +void programmingPidUpdateTask(timeUs_t currentTimeUs); +void programmingPidInit(void); \ No newline at end of file From 3dd995bd328a07e5bf9fc5ea824f357b6bfe9816 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 3 Dec 2020 20:42:51 +0100 Subject: [PATCH 017/103] Initiate PID controllers --- src/main/navigation/navigation_private.h | 2 ++ src/main/programming/pid.c | 16 ++++++++++++++-- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index fcae1b7ded..f2b348d650 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -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 diff --git a/src/main/programming/pid.c b/src/main/programming/pid.c index 9b09e2ea9d..a6882b8190 100644 --- a/src/main/programming/pid.c +++ b/src/main/programming/pid.c @@ -31,10 +31,12 @@ FILE_COMPILE_FOR_SIZE #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" 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); @@ -64,14 +66,24 @@ void pgResetFn_programmingPids(programmingPid_t *instance) void programmingPidUpdateTask(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); - //Dummy + + if (!pidsInitiated) { + programmingPidInit(); + pidsInitiated = true; + } + } 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, + 5.0f ); } } From ebf581c871e61c50d9d62bb34bf824008405d04a Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 3 Dec 2020 21:22:06 +0100 Subject: [PATCH 018/103] WIP --- src/main/fc/cli.c | 112 +++++++++++++++++++++++++++++++++++++ src/main/programming/pid.c | 1 + 2 files changed, 113 insertions(+) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 53c7fdb58c..172e0fb47f 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2001,6 +2001,111 @@ static void cliGvar(char *cmdline) { } } +static void printPid(uint8_t dumpMask, const logicCondition_t *logicConditions, const logicCondition_t *defaultLogicConditions) +{ + 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]; + + bool equalsDefault = false; + if (defaultLogicConditions) { + logicCondition_t defaultValue = defaultLogicConditions[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; + + 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 + ); + } + 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 + ); + } +} + +static void cliPid(char *cmdline) { + char * saveptr; + int args[10], check = 0; + uint8_t len = strlen(cmdline); + + if (len == 0) { + printLogic(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_LOGIC_CONDITIONS && + 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 + + ) { + 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]; + + cliLogic(""); + } else { + cliShowParseError(); + } + } +} + #endif #ifdef USE_SDCARD @@ -3315,6 +3420,9 @@ static void printConfig(const char *cmdline, bool doDiff) cliPrintHashLine("gvar"); printGvar(dumpMask, globalVariableConfigs_CopyArray, globalVariableConfigs(0)); + + cliPrintHashLine("pid"); + printLogic(dumpMask, programmingPids_CopyArray, programmingPids(0)); #endif cliPrintHashLine("feature"); @@ -3569,6 +3677,10 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("gvar", "configure global variables", " \r\n" "\treset\r\n", cliGvar), + + CLI_COMMAND_DEF("pid", "configurable PID controllers", + "<#>

\r\n" + "\treset\r\n", cliPid), #endif CLI_COMMAND_DEF("set", "change setting", "[=]", cliSet), CLI_COMMAND_DEF("smix", "servo mixer", diff --git a/src/main/programming/pid.c b/src/main/programming/pid.c index a6882b8190..eb680f6687 100644 --- a/src/main/programming/pid.c +++ b/src/main/programming/pid.c @@ -28,6 +28,7 @@ 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" From 687ae188b67e475f9a529682132778dfd1ea8e12 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 3 Dec 2020 21:26:52 +0100 Subject: [PATCH 019/103] Revert "Adds DJI OSD support to 2x F3 boards" This reverts commit ea59b0e5ec94f75047b8a980c79663089891e7e1. --- src/main/target/AIRHEROF3/target.h | 4 ---- src/main/target/SPRACINGF3/target.h | 5 ----- 2 files changed, 9 deletions(-) diff --git a/src/main/target/AIRHEROF3/target.h b/src/main/target/AIRHEROF3/target.h index 74ba97293f..9356bf0cb2 100755 --- a/src/main/target/AIRHEROF3/target.h +++ b/src/main/target/AIRHEROF3/target.h @@ -76,10 +76,6 @@ #define VBAT_ADC_CHANNEL ADC_CHN_1 #define AIRSPEED_ADC_CHANNEL ADC_CHN_2 -#define USE_DJI_HD_OSD -#define USE_OSD -#undef USE_CMS -#undef CMS_MENU_OSD /* #define USE_LED_STRIP diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 3b506c86b4..05727f5d42 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -93,11 +93,6 @@ #define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 #define RSSI_ADC_CHANNEL ADC_CHN_3 -#define USE_DJI_HD_OSD -#define USE_OSD -#undef USE_CMS -#undef CMS_MENU_OSD - #define USE_LED_STRIP #define WS2811_PIN PA8 From d32fe6dea50a512e40a02a913e1d2ac856937333 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 4 Dec 2020 20:38:34 +0100 Subject: [PATCH 020/103] Move PID gains to UINT16 instead of UINT8. Keep it compatible on the MSP level --- docs/Programming Framework.md | 1 + src/main/cms/cms_menu_imu.c | 2 +- src/main/fc/cli.c | 98 +++++++++++++----------- src/main/fc/fc_core.c | 3 + src/main/fc/fc_msp.c | 14 ++-- src/main/flight/pid.h | 8 +- src/main/navigation/navigation.c | 4 + src/main/navigation/navigation_private.h | 1 + src/main/programming/logic_condition.c | 7 ++ src/main/programming/logic_condition.h | 1 + src/main/programming/pid.c | 40 ++++++++-- src/main/programming/pid.h | 8 +- 12 files changed, 122 insertions(+), 65 deletions(-) diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index 2034bc0da1..fc11010f1f 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -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 diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 07e56539b7..68311250c7 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/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) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 172e0fb47f..abffc91bbd 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -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"); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index fb6d610337..71f758dc40 100755 --- a/src/main/fc/fc_core.c +++ b/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)); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 7e070167ee..342077a039 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -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; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index cc4b20ae59..fa8ed01405 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -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 { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index ec8047f35e..0f9567bc0d 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1961,6 +1961,10 @@ float navPidApply3( pid->integrator = newIntegrator; } } + + if (pidFlags & PID_LIMIT_INTEGRATOR) { + pid->integrator = constrainf(pid->integrator, outMin, outMax); + } return outValConstrained; } diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index f2b348d650..ceae9fd56c 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -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 { diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index b9b35d810d..7d309132cd 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -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; } diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 75905c51ca..65e98713ca 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -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; diff --git a/src/main/programming/pid.c b/src/main/programming/pid.c index eb680f6687..91134f4096 100644 --- a/src/main/programming/pid.c +++ b/src/main/programming/pid.c @@ -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 \ No newline at end of file diff --git a/src/main/programming/pid.h b/src/main/programming/pid.h index 810b9cfa3c..f120f9f073 100644 --- a/src/main/programming/pid.h +++ b/src/main/programming/pid.h @@ -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); \ No newline at end of file +void programmingPidInit(void); +void programmingPidReset(void); +int programmingPidGetOutput(uint8_t i); \ No newline at end of file From 97eea57e9e01a754df06d0d16c2e3c26fbb4c2a3 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 4 Dec 2020 21:20:02 +0100 Subject: [PATCH 021/103] Fix PID bank uint16 transition --- src/main/fc/rc_adjustments.c | 4 ++-- src/main/flight/pid.c | 2 +- src/main/flight/pid.h | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index d7ae44f9d8..434815b69c 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -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) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 00cd8ac9c5..5e5f9d3b5a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index fa8ed01405..7f83accab6 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -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); From 159ba94c9fc08ff8bb4535c56a2c016164667de1 Mon Sep 17 00:00:00 2001 From: user Date: Sun, 6 Dec 2020 07:21:36 +0300 Subject: [PATCH 022/103] Enable DSHOT support on F35/WINGFC --- src/main/target/FF_F35_LIGHTNING/target.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/target/FF_F35_LIGHTNING/target.h b/src/main/target/FF_F35_LIGHTNING/target.h index 5d6beaf503..d2285e8a38 100644 --- a/src/main/target/FF_F35_LIGHTNING/target.h +++ b/src/main/target/FF_F35_LIGHTNING/target.h @@ -27,6 +27,8 @@ #define BEEPER PA1 #define BEEPER_INVERTED +#define USE_DSHOT + // MPU interrupt #define USE_EXTI #define GYRO_INT_EXTI PC4 From a952e659395208ba702fd353d1181c9807cff21c Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Sun, 6 Dec 2020 11:44:35 +0100 Subject: [PATCH 023/103] [EEPROM] Ensure we don't read beyond config memory area when loading EEPROM --- src/main/config/config_eeprom.c | 27 ++++++++++++++++++++------- 1 file changed, 20 insertions(+), 7 deletions(-) diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c index a66ea60576..e59c42b700 100755 --- a/src/main/config/config_eeprom.c +++ b/src/main/config/config_eeprom.c @@ -118,8 +118,13 @@ bool isEEPROMContentValid(void) // Found the end. Stop scanning. break; } - if (p + record->size >= &__config_end - || record->size < sizeof(*record)) { + + if (p + sizeof(*record) >= &__config_end) { + // Too big. Further checking for size doesn't make sense + return false; + } + + if (p + record->size >= &__config_end || record->size < sizeof(*record)) { // Too big or too small. return false; } @@ -152,13 +157,21 @@ static const configRecord_t *findEEPROM(const pgRegistry_t *reg, configRecordFla p += sizeof(configHeader_t); // skip header while (true) { const configRecord_t *record = (const configRecord_t *)p; - if (record->size == 0 - || p + record->size >= &__config_end - || record->size < sizeof(*record)) + // Ensure that the record header fits into config memory, otherwise accessing size and flags may cause a hardfault. + if (p + sizeof(*record) >= &__config_end) { break; - if (pgN(reg) == record->pgn - && (record->flags & CR_CLASSIFICATION_MASK) == classification) + } + + // Check that record header makes sense + if (record->size == 0 || p + record->size >= &__config_end || record->size < sizeof(*record)) { + break; + } + + // Check if this is the record we're looking for (check for size) + if (pgN(reg) == record->pgn && (record->flags & CR_CLASSIFICATION_MASK) == classification) { return record; + } + p += record->size; } // record not found From fe55843e20ca0bd9fea229618c3489b3a1f42d3a Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Sun, 6 Dec 2020 11:49:43 +0100 Subject: [PATCH 024/103] [EEPROM] Restore the EEPROM validity check --- src/main/fc/fc_init.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index a18d572fc5..b8cb792156 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -218,7 +218,7 @@ void init(void) #endif initEEPROM(); - //ensureEEPROMContainsValidData(); + ensureEEPROMContainsValidData(); readEEPROM(); // Re-initialize system clock to their final values (if necessary) From 6dd4cf9b7946121582f0a5b59d5f19fd279a98dd Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 7 Dec 2020 09:56:42 +0100 Subject: [PATCH 025/103] Drop legacy curve --- docs/Settings.md | 10 +++++++--- src/main/fc/settings.yaml | 4 ++-- src/main/sensors/gyro.c | 14 ++------------ src/main/sensors/gyro.h | 2 +- 4 files changed, 12 insertions(+), 18 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 00999bf75a..986235b09b 100644 --- a/docs/Settings.md +++ b/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. | | 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 | @@ -277,8 +281,8 @@ | nav_fw_max_thr | 1700 | Maximum throttle for flying wing in GPS assisted modes | | nav_fw_min_thr | 1200 | Minimum throttle for flying wing in GPS assisted modes | | nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) | -| nav_fw_pitch2thr_smoothing | 0 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. | -| nav_fw_pitch2thr_threshold | 0 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] | +| nav_fw_pitch2thr_smoothing | 6 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. | +| nav_fw_pitch2thr_threshold | 50 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] | | nav_fw_pos_hdg_d | 0 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_i | 0 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_p | 60 | P gain of heading PID controller. (Fixedwing, rovers, boats) | @@ -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 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 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_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. | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 4b7d453b26..5ecc66907d 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -234,8 +234,8 @@ groups: - name: gyro_dyn_lpf_curve_expo description: "Expo value for the throttle-to-frequency mapping for Dynamic LPF" default_value: "5" - field: gyroDynamicLpFCurveExpo - min: 0 + field: gyroDynamicLpfCurveExpo + min: 1 max: 10 - name: dynamic_gyro_notch_enabled description: "Enable/disable dynamic gyro notch also known as Matrix Filter" diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 7adadf2c10..62bc58887c 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -121,7 +121,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .useDynamicLpf = 0, .gyroDynamicLpfMinHz = 200, .gyroDynamicLpfMaxHz = 500, - .gyroDynamicLpFCurveExpo = 5, + .gyroDynamicLpfCurveExpo = 5, .dynamicGyroNotchRange = DYN_NOTCH_RANGE_MEDIUM, .dynamicGyroNotchQ = 120, .dynamicGyroNotchMinHz = 150, @@ -524,23 +524,13 @@ static float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLp return (dynLpfMax - dynLpfMin) * curve + dynLpfMin; } -static float dynThrottle(float throttle) { - return throttle * (1 - (throttle * throttle) / 3.0f) * 1.5f; -} - void gyroUpdateDynamicLpf(void) { if (!gyroConfig()->useDynamicLpf) { return; } const float throttle = scaleRangef((float) rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle, 0.0f, 1.0f); - - uint16_t cutoffFreq; - if (gyroConfig()->gyroDynamicLpFCurveExpo > 0) { - cutoffFreq = dynLpfCutoffFreq(throttle, gyroConfig()->gyroDynamicLpfMinHz, gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpFCurveExpo); - } else { - cutoffFreq = fmax(dynThrottle(throttle) * gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpfMinHz); - } + const uint16_t cutoffFreq = dynLpfCutoffFreq(throttle, gyroConfig()->gyroDynamicLpfMinHz, gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpfCurveExpo); if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index b5bf66a4b2..491ff0a106 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -73,7 +73,7 @@ typedef struct gyroConfig_s { uint8_t useDynamicLpf; uint16_t gyroDynamicLpfMinHz; uint16_t gyroDynamicLpfMaxHz; - uint8_t gyroDynamicLpFCurveExpo; + uint8_t gyroDynamicLpfCurveExpo; uint8_t dynamicGyroNotchRange; uint16_t dynamicGyroNotchQ; uint16_t dynamicGyroNotchMinHz; From a160f2a4d3c3cbcedc8670736ac561b2809eaaf6 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 7 Dec 2020 11:16:07 +0100 Subject: [PATCH 026/103] Update tests --- src/test/unit/sensor_gyro_unittest.cc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index 7eb0418397..0433b88939 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -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[]; From 4c32c596f9d4e1a075142e2f36151d911ae9e6ec Mon Sep 17 00:00:00 2001 From: Airwide Date: Mon, 7 Dec 2020 20:00:56 +0100 Subject: [PATCH 027/103] Added pitch2thr smoothing defaults to pg_reset_template --- src/main/navigation/navigation.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 0bcfb50b96..24708ae1d6 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -157,8 +157,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .max_throttle = 1700, .min_throttle = 1200, .pitch_to_throttle = 10, // pwm units per degree of pitch (10pwm units ~ 1% throttle) - .pitch_to_throttle_smooth = 0, - .pitch_to_throttle_thresh = 0, + .pitch_to_throttle_smooth = 6, + .pitch_to_throttle_thresh = 50, .loiter_radius = 5000, // 50m //Fixed wing landing From ed57dd03e30bbb371887fb2c1ab38d65b94a3cb4 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Wed, 9 Dec 2020 08:39:48 +0100 Subject: [PATCH 028/103] Add note about Google Maps --- docs/Safehomes.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Safehomes.md b/docs/Safehomes.md index 414c8482eb..e90d0e7fe9 100644 --- a/docs/Safehomes.md +++ b/docs/Safehomes.md @@ -53,7 +53,7 @@ Parameters: * `` - Latitude (WGS84), in degrees * 1E7 (for example 123456789 means 12.3456789). * `` - Longitude. -Safehomes are saved along with your regular settings and will appear in `diff` and `dump` output. Use `save` to save any changes, as with other settings. +Safehomes are saved along with your regular settings and will appear in `diff` and `dump` output. Use `save` to save any changes, as with other settings. Note that coordinates from Google Maps only have six decimals, so you need to remove the decimal dot and add a zero to the end to get the correct coordinates. ### `safehome` example From ab3c25f5c8f05406568bcdfcd619cb25518920c7 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Wed, 9 Dec 2020 08:44:16 +0100 Subject: [PATCH 029/103] Improve text --- docs/Safehomes.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/docs/Safehomes.md b/docs/Safehomes.md index e90d0e7fe9..bd720f5dfc 100644 --- a/docs/Safehomes.md +++ b/docs/Safehomes.md @@ -53,7 +53,9 @@ Parameters: * `` - Latitude (WGS84), in degrees * 1E7 (for example 123456789 means 12.3456789). * `` - Longitude. -Safehomes are saved along with your regular settings and will appear in `diff` and `dump` output. Use `save` to save any changes, as with other settings. Note that coordinates from Google Maps only have six decimals, so you need to remove the decimal dot and add a zero to the end to get the correct coordinates. +Note that coordinates from Google Maps only have six decimals, so you need to remove the decimal dot and add a zero to the end to set the correct safehome location. + +Safehomes are saved along with your regular settings and will appear in `diff` and `dump` output. Use `save` to save any changes, as with other settings. ### `safehome` example From 828a9e98bf70d436b9a212b8ba50f3c7abdf96bb Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Thu, 10 Dec 2020 10:57:35 +0000 Subject: [PATCH 030/103] add USE_BARO,USE_MAG to non-I2C KAKUTEF4 V1 (for MSP sensors) (#6398) --- src/main/target/KAKUTEF4/target.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h index 0bc68d3e3d..80013075a3 100755 --- a/src/main/target/KAKUTEF4/target.h +++ b/src/main/target/KAKUTEF4/target.h @@ -70,6 +70,9 @@ # define BARO_I2C_BUS BUS_I2C1 # define USE_BARO_MS5611 # define USE_BARO_BMP280 +#else // V1 does not have I2C exposed, common_post.h will pull in USE_*_MSP +# define USE_BARO +# define USE_MAG #endif #define USE_OSD From f56180b69b39da7b89cd41289983633f95cb129d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Niccol=C3=B2=20Maggioni?= Date: Sat, 12 Dec 2020 16:02:50 +0100 Subject: [PATCH 031/103] Update Settings.md --- docs/Settings.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 00999bf75a..37e053c932 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -277,8 +277,8 @@ | nav_fw_max_thr | 1700 | Maximum throttle for flying wing in GPS assisted modes | | nav_fw_min_thr | 1200 | Minimum throttle for flying wing in GPS assisted modes | | nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) | -| nav_fw_pitch2thr_smoothing | 0 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. | -| nav_fw_pitch2thr_threshold | 0 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] | +| nav_fw_pitch2thr_smoothing | 6 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. | +| nav_fw_pitch2thr_threshold | 50 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] | | nav_fw_pos_hdg_d | 0 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_i | 0 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_p | 60 | P gain of heading PID controller. (Fixedwing, rovers, boats) | @@ -324,7 +324,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 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 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_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. | From 2397d4bc97687447109f5da69cf4b5a13815ad91 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Niccol=C3=B2=20Maggioni?= Date: Sat, 12 Dec 2020 16:10:11 +0100 Subject: [PATCH 032/103] Trigger docs CI on PRs --- .github/workflows/docs.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml index 674155dacc..20f5379c3d 100644 --- a/.github/workflows/docs.yml +++ b/.github/workflows/docs.yml @@ -1,5 +1,9 @@ name: Make sure docs are updated on: + pull_request: + paths: + - src/main/fc/settings.yaml + - docs/Settings.md push: paths: - src/main/fc/settings.yaml From f6dd19224fa1d04072b3114b481c24dae59ccc9c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Niccol=C3=B2=20Maggioni?= Date: Sat, 12 Dec 2020 16:29:08 +0100 Subject: [PATCH 033/103] Do not run tests for docs-only PRs --- .github/workflows/ci.yml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 63362e6468..129a38e891 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -2,7 +2,10 @@ name: Build firmware # Don't enable CI on push, just on PR. If you # are working on the main repo and want to trigger # a CI build submit a draft PR. -on: pull_request +on: + pull_request: + paths-ignore: + - 'docs/**' jobs: build: From beb2a4dd7e62ada43c74513962864a7e2f466925 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Sat, 12 Dec 2020 21:58:11 +0100 Subject: [PATCH 034/103] Fix nav_overrides_motor_stop doc (#6405) --- src/main/fc/settings.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 7d9104093c..6ac12bbb41 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2107,7 +2107,7 @@ groups: min: 0 max: 5000 - name: nav_overrides_motor_stop - description: "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" + description: "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" default_value: "ALL_NAV" field: general.flags.nav_overrides_motor_stop table: nav_overrides_motor_stop From 1401a9997dae972d6b7f3d08adc8c4782e100d54 Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Tue, 15 Dec 2020 09:12:36 +0000 Subject: [PATCH 035/103] add SET_POI and SET_HEAD to WP types recognised by CLI wp command (#6418) --- src/main/fc/cli.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 8253ad84d1..34295860f4 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -1334,7 +1334,7 @@ static void cliSafeHomes(char *cmdline) } } -#endif +#endif #if defined(USE_NAV) && defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) && defined(NAV_NON_VOLATILE_WAYPOINT_CLI) static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, const navWaypoint_t *defaultNavWaypoint) { @@ -1457,7 +1457,7 @@ static void cliWaypoints(char *cmdline) if (!(validArgumentCount == 6 || validArgumentCount == 8)) { cliShowParseError(); - } else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH || action == NAV_WP_ACTION_JUMP || action == NAV_WP_ACTION_HOLD_TIME || action == NAV_WP_ACTION_LAND) || (p1 < 0) || !(flag == 0 || flag == NAV_WP_FLAG_LAST)) { + } else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH || action == NAV_WP_ACTION_JUMP || action == NAV_WP_ACTION_HOLD_TIME || action == NAV_WP_ACTION_LAND || action == NAV_WP_ACTION_SET_POI || action == NAV_WP_ACTION_SET_HEAD) || !(flag == 0 || flag == NAV_WP_FLAG_LAST)) { cliShowParseError(); } else { posControl.waypointList[i].action = action; @@ -2591,7 +2591,7 @@ void cliRxBind(char *cmdline){ break; #endif } - } + } #if defined(USE_RX_SPI) else if (rxConfig()->receiverType == RX_TYPE_SPI) { switch (rxConfig()->rx_spi_protocol) { @@ -2599,7 +2599,7 @@ void cliRxBind(char *cmdline){ cliPrint("Not supported."); break; } - + } #endif } @@ -3304,7 +3304,7 @@ static void printConfig(const char *cmdline, bool doDiff) #if defined(USE_SAFE_HOME) cliPrintHashLine("safehome"); - printSafeHomes(dumpMask, safeHomeConfig_CopyArray, safeHomeConfig(0)); + printSafeHomes(dumpMask, safeHomeConfig_CopyArray, safeHomeConfig(0)); #endif #ifdef USE_PROGRAMMING_FRAMEWORK cliPrintHashLine("logic"); From aafb06989ea26388392d43c13963d041b182aa8f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Niccol=C3=B2=20Maggioni?= Date: Tue, 15 Dec 2020 11:37:20 +0100 Subject: [PATCH 036/103] Revert "Do not run tests for docs-only PRs" --- .github/workflows/ci.yml | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 129a38e891..63362e6468 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -2,10 +2,7 @@ name: Build firmware # Don't enable CI on push, just on PR. If you # are working on the main repo and want to trigger # a CI build submit a draft PR. -on: - pull_request: - paths-ignore: - - 'docs/**' +on: pull_request jobs: build: From fbc85d535d5bacb401bd689abc4e95c4d5eff2b7 Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Tue, 15 Dec 2020 11:33:29 +0000 Subject: [PATCH 037/103] add MSP for setting and getting safe homes (#6420) --- src/main/fc/fc_msp.c | 33 ++++++++++++++++++++++++++++- src/main/msp/msp_protocol_v2_inav.h | 3 +++ 2 files changed, 35 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index b52d667454..37d414ef2f 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1515,6 +1515,20 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF return true; } +static mspResult_e mspFcSafeHomeOutCommand(sbuf_t *dst, sbuf_t *src) +{ + const uint8_t safe_home_no = sbufReadU8(src); // get the home number + if(safe_home_no < MAX_SAFE_HOMES) { + sbufWriteU8(dst, safe_home_no); + sbufWriteU8(dst, safeHomeConfig(safe_home_no)->enabled); + sbufWriteU32(dst, safeHomeConfig(safe_home_no)->lat); + sbufWriteU32(dst, safeHomeConfig(safe_home_no)->lon); + return MSP_RESULT_ACK; + } else { + return MSP_RESULT_ERROR; + } +} + #ifdef USE_NAV static void mspFcWaypointOutCommand(sbuf_t *dst, sbuf_t *src) { @@ -2095,7 +2109,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) sbufReadU16(src); //Legacy yaw_jump_prevention_limit gyroConfigMutable()->gyro_lpf = sbufReadU8(src); accelerometerConfigMutable()->acc_lpf_hz = sbufReadU8(src); - sbufReadU8(src); //reserved + sbufReadU8(src); //reserved sbufReadU8(src); //reserved sbufReadU8(src); //reserved sbufReadU8(src); //reserved @@ -2892,6 +2906,19 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) return MSP_RESULT_ERROR; // will only be reached if the rollback is not ready break; #endif + case MSP2_INAV_SET_SAFEHOME: + if (dataSize == 10) { + uint8_t i; + if (!sbufReadU8Safe(&i, src) || i >= MAX_SAFE_HOMES) { + return MSP_RESULT_ERROR; + } + safeHomeConfigMutable(i)->enabled = sbufReadU8(src); + safeHomeConfigMutable(i)->lat = sbufReadU32(src); + safeHomeConfigMutable(i)->lon = sbufReadU32(src); + } else { + return MSP_RESULT_ERROR; + } + break; default: return MSP_RESULT_ERROR; @@ -3195,6 +3222,10 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu break; #endif + case MSP2_INAV_SAFEHOME: + *ret = mspFcSafeHomeOutCommand(dst, src); + break; + default: // Not handled return false; diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index b2d43590f2..3da8499507 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -72,3 +72,6 @@ #define MSP2_INAV_FWUPDT_EXEC 0x2035 #define MSP2_INAV_FWUPDT_ROLLBACK_PREPARE 0x2036 #define MSP2_INAV_FWUPDT_ROLLBACK_EXEC 0x2037 + +#define MSP2_INAV_SAFEHOME 0x2038 +#define MSP2_INAV_SET_SAFEHOME 0x2039 From 30a68365b3114fd13bddb96895ac6f6a053c817f Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Tue, 15 Dec 2020 11:58:10 +0000 Subject: [PATCH 038/103] update `Cmake usage.md` for `src/main/CMakeLists.txt`. (#6419) * update `Cmake usage.md` for `src/main/CMakeLists.txt`. --- docs/development/Cmake usage.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/docs/development/Cmake usage.md b/docs/development/Cmake usage.md index da499a59c0..6a7f74c6a6 100644 --- a/docs/development/Cmake usage.md +++ b/docs/development/Cmake usage.md @@ -67,3 +67,7 @@ target_stm32f405xg(OMNIBUSF4V3_S6_SS) # except for an inverter on UART6. target_stm32f405xg(OMNIBUSF4V3) ``` + +## Adding (or removing) a source file + +In the cmake system, project source files are listed in `src/main/CMakeLists.txt`. New source files must be added to this list to be considered by the build system. From b24d780507f8119433018fcbbb099e8c44ad5b9f Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Wed, 16 Dec 2020 10:33:01 +0100 Subject: [PATCH 039/103] Add example. --- docs/Safehomes.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/Safehomes.md b/docs/Safehomes.md index bd720f5dfc..3575da8f1c 100644 --- a/docs/Safehomes.md +++ b/docs/Safehomes.md @@ -53,7 +53,7 @@ Parameters: * `` - Latitude (WGS84), in degrees * 1E7 (for example 123456789 means 12.3456789). * `` - Longitude. -Note that coordinates from Google Maps only have six decimals, so you need to remove the decimal dot and add a zero to the end to set the correct safehome location. +Note that coordinates from Google Maps only have five or six decimals, so you need to pad zero decimals until you have seven before removing the decimal period to set the correct safehome location. For example, coordinates 54.353319 -4.517927 obtained from Google Maps need to be entered as 543533190 -45179270 and coordiniates 43.54648 -7.86545 as 435464800 -78654500. Safehomes are saved along with your regular settings and will appear in `diff` and `dump` output. Use `save` to save any changes, as with other settings. @@ -61,8 +61,8 @@ Safehomes are saved along with your regular settings and will appear in `diff` a ``` # safehome -safehome 0 1 543533193 -45179273 -safehome 1 1 435464846 -78654544 +safehome 0 1 543533190 -45179270 +safehome 1 1 435464800 -78654500 safehome 2 0 0 0 safehome 3 0 0 0 safehome 4 0 0 0 From 7e1e19fb64a8f3c77e2d747c3d1fdb229c57da32 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Wed, 16 Dec 2020 10:41:54 +0100 Subject: [PATCH 040/103] Example to adress recent issue. --- docs/Safehomes.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/Safehomes.md b/docs/Safehomes.md index 3575da8f1c..b3ed76403c 100644 --- a/docs/Safehomes.md +++ b/docs/Safehomes.md @@ -53,7 +53,7 @@ Parameters: * `` - Latitude (WGS84), in degrees * 1E7 (for example 123456789 means 12.3456789). * `` - Longitude. -Note that coordinates from Google Maps only have five or six decimals, so you need to pad zero decimals until you have seven before removing the decimal period to set the correct safehome location. For example, coordinates 54.353319 -4.517927 obtained from Google Maps need to be entered as 543533190 -45179270 and coordiniates 43.54648 -7.86545 as 435464800 -78654500. +Note that coordinates from Google Maps only have five or six decimals, so you need to pad zero decimals until you have seven before removing the decimal period to set the correct safehome location. For example, coordinates 54.353319 -4.517927 obtained from Google Maps need to be entered as 543533190 -45179270, coordiniates 43.54648 -7.86545 as 435464800 -78654500 and 51.309842 -0.095651 as 513098420 -00956510. Safehomes are saved along with your regular settings and will appear in `diff` and `dump` output. Use `save` to save any changes, as with other settings. @@ -63,7 +63,7 @@ Safehomes are saved along with your regular settings and will appear in `diff` a # safehome safehome 0 1 543533190 -45179270 safehome 1 1 435464800 -78654500 -safehome 2 0 0 0 +safehome 2 1 513098420 -00956510 safehome 3 0 0 0 safehome 4 0 0 0 safehome 5 0 0 0 From c25a361252dc6095cbb42cfee2f1eff7cb4381b6 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 18 Dec 2020 13:48:25 +0100 Subject: [PATCH 041/103] Set default gyro LPF to 256Hz --- src/main/sensors/gyro.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index d9cd15891d..afbd0bc8f3 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -104,7 +104,7 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState; PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 10); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, - .gyro_lpf = GYRO_LPF_42HZ, // 42HZ value is defined for Invensense/TDK gyros + .gyro_lpf = GYRO_LPF_256HZ, .gyro_soft_lpf_hz = 60, .gyro_soft_lpf_type = FILTER_BIQUAD, .gyro_align = ALIGN_DEFAULT, From e90c76aef35c59807270ca86918b824bfd3f5f3d Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Fri, 18 Dec 2020 16:55:11 +0000 Subject: [PATCH 042/103] update EGNOS PRN mask to latest EGSA definition, fix SBAS for non-Galileo usage. (#6431) [GPS] update SBAS and Galileo * Update GNSS / SBAS handling * Improve readabily of SBAS scanmask1 / PRN setting * Use current PRNs for all SBAS options * Decouple Galileo dependency for UBX-CFG-GNSS * Only set required GNSS options * Set GNSS options dynamically * Replace magic numbers with #define where appropriate Co-authored-by: Konstantin (DigitalEntity) Sharlaimov Fixes #6412 --- src/main/io/gps_ublox.c | 121 +++++++++++++++++++++++++++++++--------- 1 file changed, 96 insertions(+), 25 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 89a7cac8ec..3a7d3d12c1 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -21,6 +21,7 @@ #include #include #include +#include #include "platform.h" #include "build/build_config.h" @@ -67,8 +68,18 @@ #define UBX_VALID_GPS_DATE_TIME(valid) (UBX_VALID_GPS_DATE(valid) && UBX_VALID_GPS_TIME(valid)) // SBAS_AUTO, SBAS_EGNOS, SBAS_WAAS, SBAS_MSAS, SBAS_GAGAN, SBAS_NONE +// note PRNs last upadted 2020-12-18 + +#define SBASMASK1_BASE 120 +#define SBASMASK1_BITS(prn) (1 << (prn-SBASMASK1_BASE)) + static const uint32_t ubloxScanMode1[] = { - 0x00000000, 0x00000851, 0x0004E004, 0x00020200, 0x00000180, 0x00000000, + 0x00000000, // AUTO + (SBASMASK1_BITS(123) | SBASMASK1_BITS(126) | SBASMASK1_BITS(136)), // SBAS + (SBASMASK1_BITS(131) | SBASMASK1_BITS(133) | SBASMASK1_BITS(138)), // WAAS + (SBASMASK1_BITS(129) | SBASMASK1_BITS(137)), // MAAS + (SBASMASK1_BITS(127) | SBASMASK1_BITS(128)), // GAGAN + 0x00000000, // NONE }; static const char * baudInitDataNMEA[GPS_BAUDRATE_COUNT] = { @@ -101,11 +112,35 @@ typedef struct { uint16_t time; } ubx_rate; +typedef struct { + uint8_t gnssId; + uint8_t resTrkCh; + uint8_t maxTrkCh; + uint8_t reserved1; +// flags + uint8_t enabled; + uint8_t undefined0; + uint8_t sigCfgMask; + uint8_t undefined1; +} ubx_gnss_element_t; + +typedef struct { + uint8_t msgVer; + uint8_t numTrkChHw; + uint8_t numTrkChUse; + uint8_t numConfigBlocks; + ubx_gnss_element_t config[0]; +} ubx_gnss_msg_t; + +#define MAX_GNSS 7 +#define MAX_GNSS_SIZE_BYTES (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)*MAX_GNSS) + typedef union { - uint8_t bytes[60]; // sizeof Galileo config + uint8_t bytes[MAX_GNSS_SIZE_BYTES]; // placeholder ubx_sbas sbas; ubx_msg msg; ubx_rate rate; + ubx_gnss_msg_t gnss; } ubx_payload; // UBX support @@ -409,25 +444,63 @@ static const uint8_t default_payload[] = { 0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; -// Note the organisation of the bytes reflects the structure of the payload -// 4 bytes then 8*number of elements (7) -static const uint8_t galileo_payload[] = { - 0x00, 0x00, 0x20, 0x07, // GNSS / min / max / enable - 0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01, // GPS / 8 / 16 / Y - 0x01, 0x01, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01, // SBAS / 1 / 3 / Y - 0x02, 0x04, 0x08, 0x00, 0x01, 0x00, 0x01, 0x01, // Galileo / 4 / 8 / Y - 0x03, 0x08, 0x10, 0x00, 0x00, 0x00, 0x01, 0x01, // BeiDou / 8 / 16 / N - 0x04, 0x00, 0x08, 0x00, 0x00, 0x00, 0x01, 0x01, // IMES / 0 / 8 / N - 0x05, 0x00, 0x03, 0x00, 0x00, 0x00, 0x01, 0x01, // QZSS / 0 / 3 / N - 0x06, 0x08, 0x0e, 0x00, 0x01, 0x00, 0x01, 0x01 // GLONASS / 8 / 14 / Y -}; +#define GNSSID_SBAS 1 +#define GNSSID_GALILEO 2 -static void configureGalileo(void) +static int configureGNSS_SBAS(ubx_gnss_element_t * gnss_block) { + gnss_block->gnssId = GNSSID_SBAS; + gnss_block->maxTrkCh = 3; + gnss_block->sigCfgMask = 1; + if (gpsState.gpsConfig->sbasMode == SBAS_NONE) { + gnss_block->enabled = 0; + gnss_block->resTrkCh = 0; + } else { + gnss_block->enabled = 1; + gnss_block->resTrkCh = 1; + } + + return 1; +} + +static int configureGNSS_GALILEO(ubx_gnss_element_t * gnss_block) +{ + if (!capGalileo) { + return 0; + } + + gnss_block->gnssId = GNSSID_GALILEO; + gnss_block->maxTrkCh = 8; + gnss_block->sigCfgMask = 1; + if (gpsState.gpsConfig->ubloxUseGalileo) { + gnss_block->enabled = 1; + gnss_block->resTrkCh = 4; + } else { + gnss_block->enabled = 0; + gnss_block->resTrkCh = 0; + } + + return 1; +} + +static void configureGNSS(void) +{ + int blocksUsed = 0; + send_buffer.message.header.msg_class = CLASS_CFG; send_buffer.message.header.msg_id = MSG_CFG_GNSS; - send_buffer.message.header.length = sizeof(galileo_payload); - memcpy(send_buffer.message.payload.bytes, galileo_payload, sizeof(galileo_payload)); + send_buffer.message.payload.gnss.msgVer = 0; + send_buffer.message.payload.gnss.numTrkChHw = 0; // read only, so unset + send_buffer.message.payload.gnss.numTrkChUse = 32; + + /* SBAS, always generated */ + blocksUsed += configureGNSS_SBAS(&send_buffer.message.payload.gnss.config[blocksUsed]); + + /* Galileo */ + blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]); + + send_buffer.message.payload.gnss.numConfigBlocks = blocksUsed; + send_buffer.message.header.length = (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)* blocksUsed); sendConfigMessageUBLOX(); } @@ -741,7 +814,7 @@ STATIC_PROTOTHREAD(gpsConfigure) // Configure UBX binary messages gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); - // u-Blox 9 + // u-Blox 9 // M9N does not support some of the UBX 6/7/8 messages, so we have to configure it using special sequence if (gpsState.hwVersion >= 190000) { configureMSG(MSG_CLASS_UBX, MSG_POSLLH, 0); @@ -840,13 +913,11 @@ STATIC_PROTOTHREAD(gpsConfigure) configureSBAS(); ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); - // Enable GALILEO - if (gpsState.gpsConfig->ubloxUseGalileo && capGalileo) { - // If GALILEO is not supported by the hardware we'll get a NAK, - // however GPS would otherwise be perfectly initialized, so we are just waiting for any response - gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); - configureGalileo(); - ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); + // Configure GNSS for M8N and later + if (gpsState.hwVersion >= 80000) { + gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); + configureGNSS(); + ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); } ptEnd(0); From a15993cf3b6ef975825fda3ca7696f48123c182a Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Mon, 21 Dec 2020 12:41:37 +0000 Subject: [PATCH 043/103] Correct test for Galileo capability (#6434) * Improve test for Galileo capability --- src/main/io/gps_ublox.c | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 3a7d3d12c1..a1d1378e5c 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -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: From 121b6ae098451d1c65693658bfb3785be2d890e2 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 24 Dec 2020 19:57:13 +0100 Subject: [PATCH 044/103] Settings update --- docs/Settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 37e053c932..fd8b9fed9b 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -324,7 +324,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. | From ee5bc7baefe3243c410059c2d041242340d1baad Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 24 Dec 2020 21:06:39 +0100 Subject: [PATCH 045/103] Fix dT component --- src/main/programming/pid.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/programming/pid.c b/src/main/programming/pid.c index 91134f4096..09916fbb1b 100644 --- a/src/main/programming/pid.c +++ b/src/main/programming/pid.c @@ -92,6 +92,8 @@ void programmingPidUpdateTask(timeUs_t currentTimeUs) } } + + previousUpdateTimeUs = currentTimeUs; } void programmingPidInit(void) From 9c11ceddf135302db21fdbc97d90feecb7496621 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 24 Dec 2020 21:49:50 +0100 Subject: [PATCH 046/103] MSP layer for Programming PIDs --- src/main/fc/fc_msp.c | 30 +++++++++++++++++++++++++++++ src/main/msp/msp_protocol_v2_inav.h | 2 ++ 2 files changed, 32 insertions(+) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 1573c0b682..d5ad2f3022 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -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++) { @@ -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); diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 3da8499507..219e770722 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -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 From fa9251f8b00ce3f4ca8619b6ca406a36b2740187 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 25 Dec 2020 14:09:29 +0100 Subject: [PATCH 047/103] Basic doc update --- docs/Programming Framework.md | 28 ++++++++++++++++++++++++++-- 1 file changed, 26 insertions(+), 2 deletions(-) diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index fc11010f1f..6e658058d1 100644 --- a/docs/Programming Framework.md +++ b/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 ` @@ -121,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 | |---- |---- | @@ -159,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 ` + +## Programming PID + +`pid

` + +* `` - ID of PID Controller, starting from `0` +* `` - `0` evaluates as disabled, `1` evaluates as enabled +* `` - See `Operands` paragraph +* `` - See `Operands` paragraph +* `` - See `Operands` paragraph +* `` - See `Operands` paragraph +* `

` - P-gain, scaled to `1/1000` +* `` - I-gain, scaled to `1/1000` +* `` - D-gain, scaled to `1/1000` +* `` - FF-gain, scaled to `1/1000` + ## Examples ### Dynamic THROTTLE scale From 1405fd3fc1587a515666c3f3e94bff1c4be7283e Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 25 Dec 2020 16:15:56 +0100 Subject: [PATCH 048/103] Refactor --- src/main/CMakeLists.txt | 2 ++ src/main/build/debug.h | 1 + src/main/fc/fc_tasks.c | 2 +- src/main/fc/settings.yaml | 2 +- src/main/flight/dynamic_lpf.c | 50 +++++++++++++++++++++++++++++++++++ src/main/flight/dynamic_lpf.h | 29 ++++++++++++++++++++ src/main/sensors/gyro.c | 17 +----------- src/main/sensors/gyro.h | 2 +- 8 files changed, 86 insertions(+), 19 deletions(-) create mode 100644 src/main/flight/dynamic_lpf.c create mode 100644 src/main/flight/dynamic_lpf.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 793c64c3bd..927070dcc4 100644 --- a/src/main/CMakeLists.txt +++ b/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 diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 860289c076..b484fd4d35 100644 --- a/src/main/build/debug.h +++ b/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; diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 3a4517cf56..e4ff976cfc 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -294,7 +294,7 @@ void taskUpdateAux(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); updatePIDCoefficients(); - gyroUpdateDynamicLpf(); + dynamicLpfGyroTask(); } void fcTasksInit(void) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 5ecc66907d..59331e7c7a 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -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 diff --git a/src/main/flight/dynamic_lpf.c b/src/main/flight/dynamic_lpf.c new file mode 100644 index 0000000000..c93cd92146 --- /dev/null +++ b/src/main/flight/dynamic_lpf.c @@ -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); +} \ No newline at end of file diff --git a/src/main/flight/dynamic_lpf.h b/src/main/flight/dynamic_lpf.h new file mode 100644 index 0000000000..f8824fc5b1 --- /dev/null +++ b/src/main/flight/dynamic_lpf.h @@ -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 + +void dynamicLpfGyroTask(void); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 62bc58887c..ced897d8ad 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -73,7 +73,6 @@ FILE_COMPILE_FOR_SPEED #include "flight/gyroanalyse.h" #include "flight/rpm_filter.h" #include "flight/dynamic_gyro_notch.h" -#include "flight/mixer.h" #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" @@ -517,21 +516,7 @@ bool gyroSyncCheckUpdate(void) return gyroDev[0].intStatusFn(&gyroDev[0]); } -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 gyroUpdateDynamicLpf(void) { - if (!gyroConfig()->useDynamicLpf) { - return; - } - - const float throttle = scaleRangef((float) rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle, 0.0f, 1.0f); - const uint16_t cutoffFreq = dynLpfCutoffFreq(throttle, gyroConfig()->gyroDynamicLpfMinHz, gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpfCurveExpo); - +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); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 491ff0a106..737055af1f 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -91,4 +91,4 @@ bool gyroReadTemperature(void); int16_t gyroGetTemperature(void); int16_t gyroRateDps(int axis); bool gyroSyncCheckUpdate(void); -void gyroUpdateDynamicLpf(void); +void gyroUpdateDynamicLpf(float cutoffFreq); From 9cc0d79460ebc36a20dfd45ee0bfba11565e8eff Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 25 Dec 2020 16:26:55 +0100 Subject: [PATCH 049/103] Fix includ --- src/main/fc/fc_tasks.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index e4ff976cfc..b000de7017 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -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" From 4d3fab588130b5929f149cdb8b84bebbc27c43a6 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 25 Dec 2020 16:31:27 +0100 Subject: [PATCH 050/103] Docs update --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 986235b09b..b345407376 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -149,7 +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. | +| 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 | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 59331e7c7a..85e21cc3c7 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -215,7 +215,7 @@ groups: field: gyro_stage2_lowpass_type table: filter_type - name: gyro_use_dyn_lpf - description: "Use Dynamic LPF instead of static gyro stage1 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 From be73b015e59a6b58540f4886d7a0751148936bbb Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 25 Dec 2020 16:52:26 +0100 Subject: [PATCH 051/103] One more time regenerate settings.md --- docs/Settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index b345407376..18cf166e67 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -328,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. | From 750d271ddff50a2f71f27b26d11dda2c100bd8ea Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Tue, 29 Dec 2020 02:21:33 +0200 Subject: [PATCH 052/103] added vtx_smartaudio_early_akk_workaround option --- src/main/fc/settings.yaml | 5 +++++ src/main/io/vtx_control.h | 1 + src/main/io/vtx_smartaudio.c | 3 ++- 3 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 6ac12bbb41..c7d766440b 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3075,6 +3075,11 @@ groups: default_value: "ON" field: halfDuplex type: bool + - name: vtx_smartaudio_early_akk_workaround + description: "Enable workaround for early AKK SAudio-enabled VTX bug." + default_value: "ON" + field: smartAudioEarlyAkkWorkaroundEnable + type: bool - name: PG_VTX_SETTINGS_CONFIG type: vtxSettingsConfig_t diff --git a/src/main/io/vtx_control.h b/src/main/io/vtx_control.h index e39e447a9f..1e0d4c96f1 100644 --- a/src/main/io/vtx_control.h +++ b/src/main/io/vtx_control.h @@ -31,6 +31,7 @@ typedef struct vtxChannelActivationCondition_s { typedef struct vtxConfig_s { vtxChannelActivationCondition_t vtxChannelActivationConditions[MAX_CHANNEL_ACTIVATION_CONDITION_COUNT]; uint8_t halfDuplex; + uint8_t smartAudioEarlyAkkWorkaroundEnable; } vtxConfig_t; PG_DECLARE(vtxConfig_t, vtxConfig); diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index cb60d447ad..cb057f0fc5 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -506,7 +506,8 @@ static void saSendFrame(uint8_t *buf, int len) // XXX: Workaround for early AKK SAudio-enabled VTX bug, // shouldn't cause any problems with VTX with properly // implemented SAudio. - serialWrite(smartAudioSerialPort, 0x00); + //Update: causes problem with new AKK AIO camera connected to SoftUART + if (vtxConfig()->smartAudioEarlyAkkWorkaroundEnable) serialWrite(smartAudioSerialPort, 0x00); sa_lastTransmissionMs = millis(); saStat.pktsent++; From 71a4e32bf1d9cd2471a48a1a9581174a451329e5 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Tue, 29 Dec 2020 02:49:21 +0200 Subject: [PATCH 053/103] fixed: vtxSAIsReady() returns true before vtx is initialized fixed: vtxSAGetPowerIndex() is trying to decode power index from mW --- src/main/io/vtx_smartaudio.c | 6 ++++-- src/main/io/vtx_smartaudio.h | 2 +- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index cb057f0fc5..2f12f3b58b 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -195,6 +195,7 @@ static void saPrintSettings(void) LOG_D(VTX, "BootIntoPitMode: %s", saDevice.willBootIntoPitMode ? "yes" : "no"); } +/* int saDacToPowerIndex(int dac) { for (int idx = saPowerCount - 1 ; idx >= 0 ; idx--) { @@ -204,6 +205,7 @@ int saDacToPowerIndex(int dac) } return 0; } +*/ int saDbiToMw(uint16_t dbi) { @@ -794,7 +796,7 @@ vtxDevType_e vtxSAGetDeviceType(const vtxDevice_t *vtxDevice) static bool vtxSAIsReady(const vtxDevice_t *vtxDevice) { - return vtxDevice != NULL && !(saDevice.power == 0); + return vtxDevice != NULL && (saDevice.power >= 0 ); //wait until power reading exists } @@ -912,7 +914,7 @@ static bool vtxSAGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) return false; } - *pIndex = ((saDevice.version == SA_1_0) ? saDacToPowerIndex(saDevice.power) : saDevice.power); + *pIndex = saDevice.power; return true; } diff --git a/src/main/io/vtx_smartaudio.h b/src/main/io/vtx_smartaudio.h index c817f05a79..914995f1f2 100644 --- a/src/main/io/vtx_smartaudio.h +++ b/src/main/io/vtx_smartaudio.h @@ -72,7 +72,7 @@ typedef enum { typedef struct smartAudioDevice_s { smartAudioVersion_e version; int8_t channel; - int8_t power; + int8_t power; // -1 - not initialized, 0 - power level unknown, or power level index 1..n. For SA_2_1 device you can get power in mW: saPowerTable[index-1].mW int8_t mode; uint16_t freq; uint16_t orfreq; From 895a7625e81bd5e8e51a80f77b752163713cfac3 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Tue, 29 Dec 2020 03:00:31 +0200 Subject: [PATCH 054/103] Revert "fixed: vtxSAIsReady() returns true before vtx is initialized fixed: vtxSAGetPowerIndex() is trying to decode power index from mW" This reverts commit 71a4e32bf1d9cd2471a48a1a9581174a451329e5. --- src/main/io/vtx_smartaudio.c | 6 ++---- src/main/io/vtx_smartaudio.h | 2 +- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 2f12f3b58b..cb057f0fc5 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -195,7 +195,6 @@ static void saPrintSettings(void) LOG_D(VTX, "BootIntoPitMode: %s", saDevice.willBootIntoPitMode ? "yes" : "no"); } -/* int saDacToPowerIndex(int dac) { for (int idx = saPowerCount - 1 ; idx >= 0 ; idx--) { @@ -205,7 +204,6 @@ int saDacToPowerIndex(int dac) } return 0; } -*/ int saDbiToMw(uint16_t dbi) { @@ -796,7 +794,7 @@ vtxDevType_e vtxSAGetDeviceType(const vtxDevice_t *vtxDevice) static bool vtxSAIsReady(const vtxDevice_t *vtxDevice) { - return vtxDevice != NULL && (saDevice.power >= 0 ); + return vtxDevice != NULL && !(saDevice.power == 0); //wait until power reading exists } @@ -914,7 +912,7 @@ static bool vtxSAGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) return false; } - *pIndex = saDevice.power; + *pIndex = ((saDevice.version == SA_1_0) ? saDacToPowerIndex(saDevice.power) : saDevice.power); return true; } diff --git a/src/main/io/vtx_smartaudio.h b/src/main/io/vtx_smartaudio.h index 914995f1f2..c817f05a79 100644 --- a/src/main/io/vtx_smartaudio.h +++ b/src/main/io/vtx_smartaudio.h @@ -72,7 +72,7 @@ typedef enum { typedef struct smartAudioDevice_s { smartAudioVersion_e version; int8_t channel; - int8_t power; // -1 - not initialized, 0 - power level unknown, or power level index 1..n. For SA_2_1 device you can get power in mW: saPowerTable[index-1].mW + int8_t power; int8_t mode; uint16_t freq; uint16_t orfreq; From f8ce88f20631ef501a782e2c62481a0a643804f0 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 30 Dec 2020 22:53:14 +0100 Subject: [PATCH 055/103] dzikuvx-fix-ghst-telemetry --- src/main/rx/ghst.c | 2 +- src/main/telemetry/ghst.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/rx/ghst.c b/src/main/rx/ghst.c index 6f3173804f..54d3b6f0f5 100644 --- a/src/main/rx/ghst.c +++ b/src/main/rx/ghst.c @@ -241,7 +241,7 @@ uint8_t ghstFrameStatus(rxRuntimeConfig_t *rxRuntimeState) { UNUSED(rxRuntimeState); - if(serialIsIdle(serialPort)) { + if (serialIsIdle(serialPort)) { ghstIdle(); } diff --git a/src/main/telemetry/ghst.c b/src/main/telemetry/ghst.c index 121f239446..dc9c02579e 100644 --- a/src/main/telemetry/ghst.c +++ b/src/main/telemetry/ghst.c @@ -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; From 7d4be879fa2401c15726527943f2eac15186e4c3 Mon Sep 17 00:00:00 2001 From: Tim Long Date: Fri, 1 Jan 2021 19:17:18 +0000 Subject: [PATCH 056/103] Modified ZEEZF7 target so that it can use MSP-based compass and barometer --- src/main/target/ZEEZF7/target.h | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/main/target/ZEEZF7/target.h b/src/main/target/ZEEZF7/target.h index f7056fc3c9..839aeeaa1d 100755 --- a/src/main/target/ZEEZF7/target.h +++ b/src/main/target/ZEEZF7/target.h @@ -44,6 +44,17 @@ #define USE_IMU_MPU6000 #define IMU_MPU6000_ALIGN CW180_DEG +// *************** I2C/Baro/Mag ********************* +// Target has no I2C but can use MSP devices, such as those provided on Mateksys MQ8-CAN/MSP +// Which contains: GPS SAM-M8Q, Compass QMC5883L, Barometer DPS310 +// See: http://www.mateksys.com/?portfolio=m8q-can + +#define USE_BARO +#define USE_BARO_DPS310 + +#define USE_MAG 1 +#define USE_MAG_QMC5883 + // *************** Flash **************************** #define USE_SPI_DEVICE_1 From 0f880c6c1d5b9662283ec82c93a650516d75bdbf Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sat, 2 Jan 2021 23:44:44 +0100 Subject: [PATCH 057/103] Replace first 4 digits with ' ' (ugly solution) --- src/main/io/osd.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 4d7e035919..71e2da0b4a 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2277,6 +2277,7 @@ static bool osdDrawSingleElement(uint8_t item) int digits = osdConfig()->plus_code_digits; if (STATE(GPS_FIX)) { olc_encode(gpsSol.llh.lat, gpsSol.llh.lon, digits, buff, sizeof(buff)); + buff[0] = buff[1] = buff[2] = buff[3] = ' '; } else { // +codes with > 8 digits have a + at the 9th digit // and we only support 10 and up. From 6eaefd9588794e46c736488762845d0b279213f2 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 3 Jan 2021 00:03:14 +0100 Subject: [PATCH 058/103] Also overwrite in case of no GPS fix --- src/main/io/osd.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 71e2da0b4a..32ff0156a9 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2282,6 +2282,7 @@ static bool osdDrawSingleElement(uint8_t item) // +codes with > 8 digits have a + at the 9th digit // and we only support 10 and up. memset(buff, '-', digits + 1); + buff[0] = buff[1] = buff[2] = buff[3] = ' '; buff[8] = '+'; buff[digits + 1] = '\0'; } From beb74e51be04269b024dea390b528e689132481c Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 3 Jan 2021 10:48:48 +0100 Subject: [PATCH 059/103] Better implementation --- src/main/io/osd.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 32ff0156a9..9b49065e18 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2277,15 +2277,16 @@ static bool osdDrawSingleElement(uint8_t item) int digits = osdConfig()->plus_code_digits; if (STATE(GPS_FIX)) { olc_encode(gpsSol.llh.lat, gpsSol.llh.lon, digits, buff, sizeof(buff)); - buff[0] = buff[1] = buff[2] = buff[3] = ' '; } else { // +codes with > 8 digits have a + at the 9th digit // and we only support 10 and up. memset(buff, '-', digits + 1); - buff[0] = buff[1] = buff[2] = buff[3] = ' '; buff[8] = '+'; buff[digits + 1] = '\0'; } + // Use local code + memmove(buff, buff+4, strlen(buff)+4); + buff[digits + 1 - 4] = '\0'; break; } From ce5382ddb30fed623a683e63414a7566c8e0355a Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 3 Jan 2021 11:32:55 +0100 Subject: [PATCH 060/103] CLI command --- src/main/fc/settings.yaml | 9 +++++++++ src/main/io/osd.c | 9 ++++++--- src/main/io/osd.h | 6 ++++++ 3 files changed, 21 insertions(+), 3 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 4316e29f7a..905a81a887 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -155,6 +155,9 @@ tables: - name: nav_overrides_motor_stop enum: navOverridesMotorStop_e values: ["OFF_ALWAYS", "OFF", "AUTO_ONLY", "ALL_NAV"] + - name: osd_plus_code_type + enum: osd_plus_code_type_e + values: ["GLOBAL", "LOCAL"] groups: - name: PG_GYRO_CONFIG @@ -2913,6 +2916,12 @@ groups: field: plus_code_digits min: 10 max: 13 + - name: osd_plus_code_type + description: "Select plus code type, `LOCAL` removes first four digits and needs a reference location within ~40 km to recover the original coordinates." + field: plus_code_type + default_value: "GLOBAL" + table: osd_plus_code_type + type: uint8_t - name: osd_ahi_style description: "Sets OSD Artificial Horizon style \"DEFAULT\" or \"LINE\" for the FrSky Graphical OSD." diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 9b49065e18..72d206d28a 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2284,9 +2284,11 @@ static bool osdDrawSingleElement(uint8_t item) buff[8] = '+'; buff[digits + 1] = '\0'; } - // Use local code - memmove(buff, buff+4, strlen(buff)+4); - buff[digits + 1 - 4] = '\0'; + if (osdConfig()->plus_code_type == OSD_PLUS_CODE_LOCAL) { + // Use local code (see https://github.com/google/open-location-code/wiki/Guidance-for-shortening-codes) + memmove(buff, buff+4, strlen(buff)+4); + buff[digits + 1 - 4] = '\0'; + } break; } @@ -2594,6 +2596,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .osd_failsafe_switch_layout = false, .plus_code_digits = 11, + .plus_code_type = OSD_PLUS_CODE_GLOBAL, .ahi_width = OSD_AHI_WIDTH * OSD_CHAR_WIDTH, .ahi_height = OSD_AHI_HEIGHT * OSD_CHAR_HEIGHT, diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 01670a320e..f6549423b2 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -268,6 +268,11 @@ typedef enum { OSD_CRSF_LQ_TYPE2, } osd_crsf_lq_format_e; +typedef enum { + OSD_PLUS_CODE_GLOBAL, + OSD_PLUS_CODE_LOCAL, +} osd_plus_code_type_e; + typedef struct osdLayoutsConfig_s { // Layouts uint16_t item_pos[OSD_LAYOUT_COUNT][OSD_ITEM_COUNT]; @@ -336,6 +341,7 @@ typedef struct osdConfig_s { bool osd_failsafe_switch_layout; uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE + uint8_t plus_code_type; uint8_t osd_ahi_style; uint8_t force_grid; // Force a pixel based OSD to use grid mode. uint8_t ahi_bordered; // Only used by the AHI widget From a802b0d370644195267495cef7f0e268eb8b1948 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 3 Jan 2021 16:09:50 +0100 Subject: [PATCH 061/103] Update docs --- docs/Settings.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/Settings.md b/docs/Settings.md index 7c53b289d4..4a6b4fa31a 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -385,6 +385,7 @@ | osd_main_voltage_decimals | 1 | Number of decimals for the battery voltages displayed in the OSD [1-2]. | | osd_neg_alt_alarm | 5 | Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters) | | osd_plus_code_digits | | | +| osd_plus_code_type | GLOBAL | Select plus code type, `LOCAL` removes first four digits and needs a reference location within ~40 km to recover the original coordinates. | | osd_right_sidebar_scroll | | | | osd_right_sidebar_scroll_step | 0 | Same as left_sidebar_scroll_step, but for the right sidebar | | osd_row_shiftdown | 0 | Number of rows to shift the OSD display (increase if top rows are cut off) | From dfbb91b4196ca40afdc8487da26d8ff0885b927b Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 3 Jan 2021 17:52:29 +0100 Subject: [PATCH 062/103] Add check for ground speed > 1m/s --- src/main/io/osd.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 4d7e035919..1756c62550 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2102,7 +2102,7 @@ static bool osdDrawSingleElement(uint8_t item) { // amperage is in centi amps, speed is in cms/s. We want // mah/km. Values over 999 are considered useless and - // displayed as "---"" + // displayed as "---". Only shows if ground speed > 1m/s. static pt1Filter_t eFilterState; static timeUs_t efficiencyUpdated = 0; int32_t value = 0; @@ -2118,7 +2118,7 @@ static bool osdDrawSingleElement(uint8_t item) value = eFilterState.state; } } - if (value > 0 && value <= 999) { + if (value > 0 && value <= 999 && gpsSol.groundSpeed > 100) { tfp_sprintf(buff, "%3d", (int)value); } else { buff[0] = buff[1] = buff[2] = '-'; @@ -2133,7 +2133,7 @@ static bool osdDrawSingleElement(uint8_t item) { // amperage is in centi amps, speed is in cms/s. We want // mWh/km. Values over 999Wh/km are considered useless and - // displayed as "---"" + // displayed as "---". Only shows if ground speed > 1m/s. static pt1Filter_t eFilterState; static timeUs_t efficiencyUpdated = 0; int32_t value = 0; @@ -2149,7 +2149,7 @@ static bool osdDrawSingleElement(uint8_t item) value = eFilterState.state; } } - if (value > 0 && value <= 999999) { + if (value > 0 && value <= 999999 && gpsSol.groundSpeed > 100) { osdFormatCentiNumber(buff, value / 10, 0, 2, 0, 3); } else { buff[0] = buff[1] = buff[2] = '-'; From ab640daaf38ee06f41765f65f7edabfb5b158536 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 3 Jan 2021 17:56:04 +0100 Subject: [PATCH 063/103] Remove old check --- src/main/io/osd.c | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 1756c62550..7f237c8fcc 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2101,8 +2101,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_EFFICIENCY_MAH_PER_KM: { // amperage is in centi amps, speed is in cms/s. We want - // mah/km. Values over 999 are considered useless and - // displayed as "---". Only shows if ground speed > 1m/s. + // mah/km. Only show when ground speed > 1m/s. static pt1Filter_t eFilterState; static timeUs_t efficiencyUpdated = 0; int32_t value = 0; @@ -2118,7 +2117,7 @@ static bool osdDrawSingleElement(uint8_t item) value = eFilterState.state; } } - if (value > 0 && value <= 999 && gpsSol.groundSpeed > 100) { + if (value > 0 && gpsSol.groundSpeed > 100) { tfp_sprintf(buff, "%3d", (int)value); } else { buff[0] = buff[1] = buff[2] = '-'; @@ -2132,8 +2131,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_EFFICIENCY_WH_PER_KM: { // amperage is in centi amps, speed is in cms/s. We want - // mWh/km. Values over 999Wh/km are considered useless and - // displayed as "---". Only shows if ground speed > 1m/s. + // mWh/km. Only show when ground speed > 1m/s. static pt1Filter_t eFilterState; static timeUs_t efficiencyUpdated = 0; int32_t value = 0; @@ -2149,7 +2147,7 @@ static bool osdDrawSingleElement(uint8_t item) value = eFilterState.state; } } - if (value > 0 && value <= 999999 && gpsSol.groundSpeed > 100) { + if (value > 0 && gpsSol.groundSpeed > 100) { osdFormatCentiNumber(buff, value / 10, 0, 2, 0, 3); } else { buff[0] = buff[1] = buff[2] = '-'; From 0c24b1faf9dfa5bb3d160f0d953fffc4df87a3a3 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 3 Jan 2021 18:27:35 +0100 Subject: [PATCH 064/103] Don't calculate efficiency if traveled distance is less than 100m --- src/main/io/osd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 7f237c8fcc..6792aafdd1 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2980,7 +2980,7 @@ static void osdShowStats(void) displayWrite(osdDisplayPort, statValuesX, top++, buff); int32_t totalDistance = getTotalTravelDistance(); - if (totalDistance > 0) { + if (totalDistance > 10000) { displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY :"); if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) tfp_sprintf(buff, "%d%c%c", (int)(getMAhDrawn() * 100000 / totalDistance), From e79fad582aa811893e17e2c954fae57ca1425677 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 3 Jan 2021 19:54:57 +0100 Subject: [PATCH 065/103] Display --- instead of omitting efficiency from disarm screen --- src/main/io/osd.c | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 6792aafdd1..d610eb0205 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2980,7 +2980,7 @@ static void osdShowStats(void) displayWrite(osdDisplayPort, statValuesX, top++, buff); int32_t totalDistance = getTotalTravelDistance(); - if (totalDistance > 10000) { + if (STATE(GPS_FIX)) { displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY :"); if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) tfp_sprintf(buff, "%d%c%c", (int)(getMAhDrawn() * 100000 / totalDistance), @@ -2991,6 +2991,19 @@ static void osdShowStats(void) buff[4] = SYM_WH_KM_1; buff[5] = '\0'; } + // If traveled distance is less than 100 meters efficiency numbers are useless and unreliable so display --- instead + if (totalDistance < 10000) { + buff[0] = buff[1] = buff[2] = '-'; + if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH){ + buff[3] = SYM_MAH_KM_0; + buff[4] = SYM_MAH_KM_1; + buff[5] = '\0'; + } else { + buff[3] = SYM_WH_KM_0; + buff[4] = SYM_WH_KM_1; + buff[5] = '\0'; + } + } displayWrite(osdDisplayPort, statValuesX, top++, buff); } } From 2e9a2ddd8ec2e0ee4fd4bde250862b7c745120c2 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Sun, 3 Jan 2021 23:01:03 +0100 Subject: [PATCH 066/103] Add section for Omnibus Corner Nano to Omnibus F4 board doc --- docs/Board - Omnibus F4.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/docs/Board - Omnibus F4.md b/docs/Board - Omnibus F4.md index 62bbd76835..d4e09dd5b8 100644 --- a/docs/Board - Omnibus F4.md +++ b/docs/Board - Omnibus F4.md @@ -79,6 +79,11 @@ More target options: * SBUS inverter connected to UART1 * Uses target **FIREWORKSV2** +### Omnibus Corner Nano + +* Configurable inverter on UART6 +* Use target **OMNIBUSF4V3** + ## **NOT** supported * HC-SR04 Rangefinder From 791ed4ee7801a14f3299bc602122ec76d5d97a3f Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Mon, 4 Jan 2021 17:06:17 +0100 Subject: [PATCH 067/103] Change check --- src/main/io/osd.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index d610eb0205..9fce3af101 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2928,7 +2928,7 @@ static void osdShowStats(void) if (osdDisplayIsPAL()) displayWrite(osdDisplayPort, statNameX, top++, " --- STATS ---"); - if (STATE(GPS_FIX)) { + if (feature(FEATURE_GPS)) { displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :"); osdFormatVelocityStr(buff, stats.max_speed, true); osdLeftAlignString(buff); @@ -2980,7 +2980,7 @@ static void osdShowStats(void) displayWrite(osdDisplayPort, statValuesX, top++, buff); int32_t totalDistance = getTotalTravelDistance(); - if (STATE(GPS_FIX)) { + if (feature(FEATURE_GPS)) { displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY :"); if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) tfp_sprintf(buff, "%d%c%c", (int)(getMAhDrawn() * 100000 / totalDistance), From 6e9b1b6a3b1b289a3b84f8623fc9846fcb712d12 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Mon, 4 Jan 2021 21:08:59 +0100 Subject: [PATCH 068/103] Added options for COUNTRY (2 digits trimmed) and FIELD (6 digits trimmed) --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 4 ++-- src/main/io/osd.c | 9 ++++----- src/main/io/osd.h | 2 ++ 4 files changed, 9 insertions(+), 8 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 4a6b4fa31a..185ed4f7e0 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -385,7 +385,7 @@ | osd_main_voltage_decimals | 1 | Number of decimals for the battery voltages displayed in the OSD [1-2]. | | osd_neg_alt_alarm | 5 | Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters) | | osd_plus_code_digits | | | -| osd_plus_code_type | GLOBAL | Select plus code type, `LOCAL` removes first four digits and needs a reference location within ~40 km to recover the original coordinates. | +| osd_plus_code_type | GLOBAL | Select plus code type. Each consecutive setting (`COUNTRY`, `LOCAL`, `FIELD`) removes two extra digits from the beginning of the plus code. This requires a reference location within respectively ~800km, ~40 km and ~2km to recover the original coordinates. | | osd_right_sidebar_scroll | | | | osd_right_sidebar_scroll_step | 0 | Same as left_sidebar_scroll_step, but for the right sidebar | | osd_row_shiftdown | 0 | Number of rows to shift the OSD display (increase if top rows are cut off) | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 905a81a887..a4c83f9ed4 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -157,7 +157,7 @@ tables: values: ["OFF_ALWAYS", "OFF", "AUTO_ONLY", "ALL_NAV"] - name: osd_plus_code_type enum: osd_plus_code_type_e - values: ["GLOBAL", "LOCAL"] + values: ["GLOBAL", "COUNTRY", "LOCAL", "FIELD"] groups: - name: PG_GYRO_CONFIG @@ -2917,7 +2917,7 @@ groups: min: 10 max: 13 - name: osd_plus_code_type - description: "Select plus code type, `LOCAL` removes first four digits and needs a reference location within ~40 km to recover the original coordinates." + description: "Select plus code type. Each consecutive setting (`COUNTRY`, `LOCAL`, `FIELD`) removes two extra digits from the beginning of the plus code. This requires a reference location within respectively ~800km, ~40 km and ~2km to recover the original coordinates." field: plus_code_type default_value: "GLOBAL" table: osd_plus_code_type diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 72d206d28a..863b660f9a 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2275,6 +2275,7 @@ static bool osdDrawSingleElement(uint8_t item) { STATIC_ASSERT(GPS_DEGREES_DIVIDER == OLC_DEG_MULTIPLIER, invalid_olc_deg_multiplier); int digits = osdConfig()->plus_code_digits; + int digitsRemoved = osdConfig()->plus_code_type * 2; if (STATE(GPS_FIX)) { olc_encode(gpsSol.llh.lat, gpsSol.llh.lon, digits, buff, sizeof(buff)); } else { @@ -2284,11 +2285,9 @@ static bool osdDrawSingleElement(uint8_t item) buff[8] = '+'; buff[digits + 1] = '\0'; } - if (osdConfig()->plus_code_type == OSD_PLUS_CODE_LOCAL) { - // Use local code (see https://github.com/google/open-location-code/wiki/Guidance-for-shortening-codes) - memmove(buff, buff+4, strlen(buff)+4); - buff[digits + 1 - 4] = '\0'; - } + // Optionally trim digits from the left + memmove(buff, buff+digitsRemoved, strlen(buff) + digitsRemoved); + buff[digits + 1 - digitsRemoved] = '\0'; break; } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index f6549423b2..d846cd7220 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -270,7 +270,9 @@ typedef enum { typedef enum { OSD_PLUS_CODE_GLOBAL, + OSD_PLUS_CODE_COUNTRY, OSD_PLUS_CODE_LOCAL, + OSD_PLUS_CODE_FIELD, } osd_plus_code_type_e; typedef struct osdLayoutsConfig_s { From f43fd0868952b2c956a82f87051791bb38c5ea06 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Mon, 4 Jan 2021 21:22:53 +0100 Subject: [PATCH 069/103] Rename type to format --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 12 ++++++------ src/main/io/osd.c | 4 ++-- src/main/io/osd.h | 4 ++-- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 185ed4f7e0..1c4be596a8 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -385,7 +385,7 @@ | osd_main_voltage_decimals | 1 | Number of decimals for the battery voltages displayed in the OSD [1-2]. | | osd_neg_alt_alarm | 5 | Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters) | | osd_plus_code_digits | | | -| osd_plus_code_type | GLOBAL | Select plus code type. Each consecutive setting (`COUNTRY`, `LOCAL`, `FIELD`) removes two extra digits from the beginning of the plus code. This requires a reference location within respectively ~800km, ~40 km and ~2km to recover the original coordinates. | +| osd_plus_code_format | GLOBAL | Select plus code format. Each consecutive setting (`COUNTRY`, `LOCAL`, `FIELD`) removes two extra digits from the beginning of the plus code. This requires a reference location within respectively ~800km, ~40 km and ~2km to recover the original coordinates. | | osd_right_sidebar_scroll | | | | osd_right_sidebar_scroll_step | 0 | Same as left_sidebar_scroll_step, but for the right sidebar | | osd_row_shiftdown | 0 | Number of rows to shift the OSD display (increase if top rows are cut off) | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a4c83f9ed4..38eb8a66c1 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -155,8 +155,8 @@ tables: - name: nav_overrides_motor_stop enum: navOverridesMotorStop_e values: ["OFF_ALWAYS", "OFF", "AUTO_ONLY", "ALL_NAV"] - - name: osd_plus_code_type - enum: osd_plus_code_type_e + - name: osd_plus_code_format + enum: osd_plus_code_format_e values: ["GLOBAL", "COUNTRY", "LOCAL", "FIELD"] groups: @@ -2916,11 +2916,11 @@ groups: field: plus_code_digits min: 10 max: 13 - - name: osd_plus_code_type - description: "Select plus code type. Each consecutive setting (`COUNTRY`, `LOCAL`, `FIELD`) removes two extra digits from the beginning of the plus code. This requires a reference location within respectively ~800km, ~40 km and ~2km to recover the original coordinates." - field: plus_code_type + - name: osd_plus_code_format + description: "Select plus code format. Each consecutive setting (`COUNTRY`, `LOCAL`, `FIELD`) removes two extra digits from the beginning of the plus code. This requires a reference location within respectively ~800km, ~40 km and ~2km to recover the original coordinates." + field: plus_code_format default_value: "GLOBAL" - table: osd_plus_code_type + table: osd_plus_code_format type: uint8_t - name: osd_ahi_style diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 863b660f9a..407a8569df 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2275,7 +2275,7 @@ static bool osdDrawSingleElement(uint8_t item) { STATIC_ASSERT(GPS_DEGREES_DIVIDER == OLC_DEG_MULTIPLIER, invalid_olc_deg_multiplier); int digits = osdConfig()->plus_code_digits; - int digitsRemoved = osdConfig()->plus_code_type * 2; + int digitsRemoved = osdConfig()->plus_code_format * 2; if (STATE(GPS_FIX)) { olc_encode(gpsSol.llh.lat, gpsSol.llh.lon, digits, buff, sizeof(buff)); } else { @@ -2595,7 +2595,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .osd_failsafe_switch_layout = false, .plus_code_digits = 11, - .plus_code_type = OSD_PLUS_CODE_GLOBAL, + .plus_code_format = OSD_PLUS_CODE_GLOBAL, .ahi_width = OSD_AHI_WIDTH * OSD_CHAR_WIDTH, .ahi_height = OSD_AHI_HEIGHT * OSD_CHAR_HEIGHT, diff --git a/src/main/io/osd.h b/src/main/io/osd.h index d846cd7220..ef5635a55f 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -273,7 +273,7 @@ typedef enum { OSD_PLUS_CODE_COUNTRY, OSD_PLUS_CODE_LOCAL, OSD_PLUS_CODE_FIELD, -} osd_plus_code_type_e; +} osd_plus_code_format_e; typedef struct osdLayoutsConfig_s { // Layouts @@ -343,7 +343,7 @@ typedef struct osdConfig_s { bool osd_failsafe_switch_layout; uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE - uint8_t plus_code_type; + uint8_t plus_code_format; uint8_t osd_ahi_style; uint8_t force_grid; // Force a pixel based OSD to use grid mode. uint8_t ahi_bordered; // Only used by the AHI widget From 11545a2416119202c1d60f553ff9bb582ca9cfab Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Tue, 5 Jan 2021 19:35:11 +0100 Subject: [PATCH 070/103] Option to remove home position from arm screen --- docs/Settings.md | 1 + src/main/fc/settings.yaml | 4 ++++ src/main/io/osd.c | 17 ++++++++++------- src/main/io/osd.h | 1 + 4 files changed, 16 insertions(+), 7 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 7c53b289d4..555356ce45 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -367,6 +367,7 @@ | osd_gforce_alarm | 5 | Value above which the OSD g force indicator will blink (g) | | osd_gforce_axis_alarm_max | 5 | Value above which the OSD axis g force indicators will blink (g) | | osd_gforce_axis_alarm_min | -5 | Value under which the OSD axis g force indicators will blink (g) | +| osd_home_position_arm_screen | ON | Should home position coordinates be displayed on the arming screen. | | osd_horizon_offset | 0 | To vertically adjust the whole OSD and AHI and scrolling bars | | osd_hud_homepoint | 0 | To 3D-display the home point location in the hud | | osd_hud_homing | 0 | To display little arrows around the crossair showing where the home point is in the hud | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 4316e29f7a..181581edc4 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2969,6 +2969,10 @@ groups: default_value: 0 description: Same as left_sidebar_scroll_step, but for the right sidebar + - name: osd_home_position_arm_screen + type: bool + default_value: "ON" + description: Should home position coordinates be displayed on the arming screen. - name: PG_SYSTEM_CONFIG type: systemConfig_t diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 4d7e035919..9953ff9e44 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2581,6 +2581,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .left_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE, .right_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE, .sidebar_scroll_arrows = 0, + .osd_home_position_arm_screen = true, .units = OSD_UNIT_METRIC, .main_voltage_decimals = 1, @@ -3048,13 +3049,15 @@ static void osdShowArmed(void) #if defined(USE_GPS) if (feature(FEATURE_GPS)) { if (STATE(GPS_FIX_HOME)) { - osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); - osdFormatCoordinate(buf, SYM_LON, GPS_home.lon); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 1, buf); - int digits = osdConfig()->plus_code_digits; - olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 2, buf); + if (osdConfig()->osd_home_position_arm_screen){ + osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); + osdFormatCoordinate(buf, SYM_LON, GPS_home.lon); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 1, buf); + int digits = osdConfig()->plus_code_digits; + olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 2, buf); + } y += 4; #if defined (USE_SAFE_HOME) if (isSafeHomeInUse()) { diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 01670a320e..70f4177cb5 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -345,6 +345,7 @@ typedef struct osdConfig_s { int8_t sidebar_horizontal_offset; // Horizontal offset from default position. Units are grid slots for grid OSDs, pixels for pixel based OSDs. Positive values move sidebars closer to the edges. uint8_t left_sidebar_scroll_step; // How many units each sidebar step represents. 0 means the default value for the scroll type. uint8_t right_sidebar_scroll_step; // Same as left_sidebar_scroll_step, but for the right sidebar. + bool osd_home_position_arm_screen; uint8_t crsf_lq_format; From 9490330efc9712f643861186878896c9da7ce854 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Wed, 6 Jan 2021 12:01:47 +0100 Subject: [PATCH 071/103] Change text settings to numeric values --- docs/Settings.md | 4 ++-- src/main/fc/settings.yaml | 18 ++++++++++-------- src/main/io/osd.c | 4 ++-- src/main/io/osd.h | 9 +-------- 4 files changed, 15 insertions(+), 20 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 1c4be596a8..6a185349a3 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -384,8 +384,8 @@ | osd_link_quality_alarm | 70 | LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50% | | osd_main_voltage_decimals | 1 | Number of decimals for the battery voltages displayed in the OSD [1-2]. | | osd_neg_alt_alarm | 5 | Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters) | -| osd_plus_code_digits | | | -| osd_plus_code_format | GLOBAL | Select plus code format. Each consecutive setting (`COUNTRY`, `LOCAL`, `FIELD`) removes two extra digits from the beginning of the plus code. This requires a reference location within respectively ~800km, ~40 km and ~2km to recover the original coordinates. | +| osd_plus_code_digits | 10 | Numer of plus code digits before shortening with `osd_plus_code_short`. Precision at the equator: 10=13.9x13.9m; 11=2.8x3.5m; 12=56x87cm; 13=11x22cm. | +| osd_plus_code_short | 0 | Number of leading digits removed from plus code. Removing 2, 4 and 6 digits requires a reference location within, respectively, ~800km, ~40 km and ~2km to recover the original coordinates. | | osd_right_sidebar_scroll | | | | osd_right_sidebar_scroll_step | 0 | Same as left_sidebar_scroll_step, but for the right sidebar | | osd_row_shiftdown | 0 | Number of rows to shift the OSD display (increase if top rows are cut off) | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 38eb8a66c1..0e11ac314c 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -155,9 +155,9 @@ tables: - name: nav_overrides_motor_stop enum: navOverridesMotorStop_e values: ["OFF_ALWAYS", "OFF", "AUTO_ONLY", "ALL_NAV"] - - name: osd_plus_code_format - enum: osd_plus_code_format_e - values: ["GLOBAL", "COUNTRY", "LOCAL", "FIELD"] + - name: osd_plus_code_short + # enum: osd_plus_code_short_e + values: ["0", "2", "4", "6"] groups: - name: PG_GYRO_CONFIG @@ -2913,14 +2913,16 @@ groups: type: bool - name: osd_plus_code_digits + description: "Numer of plus code digits before shortening with `osd_plus_code_short`. Precision at the equator: 10=13.9x13.9m; 11=2.8x3.5m; 12=56x87cm; 13=11x22cm." field: plus_code_digits + default_value: 10 min: 10 max: 13 - - name: osd_plus_code_format - description: "Select plus code format. Each consecutive setting (`COUNTRY`, `LOCAL`, `FIELD`) removes two extra digits from the beginning of the plus code. This requires a reference location within respectively ~800km, ~40 km and ~2km to recover the original coordinates." - field: plus_code_format - default_value: "GLOBAL" - table: osd_plus_code_format + - name: osd_plus_code_short + description: "Number of leading digits removed from plus code. Removing 2, 4 and 6 digits requires a reference location within, respectively, ~800km, ~40 km and ~2km to recover the original coordinates." + field: plus_code_short + default_value: "0" + table: osd_plus_code_short type: uint8_t - name: osd_ahi_style diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 407a8569df..bf0cf203fc 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2275,7 +2275,7 @@ static bool osdDrawSingleElement(uint8_t item) { STATIC_ASSERT(GPS_DEGREES_DIVIDER == OLC_DEG_MULTIPLIER, invalid_olc_deg_multiplier); int digits = osdConfig()->plus_code_digits; - int digitsRemoved = osdConfig()->plus_code_format * 2; + int digitsRemoved = osdConfig()->plus_code_short * 2; if (STATE(GPS_FIX)) { olc_encode(gpsSol.llh.lat, gpsSol.llh.lon, digits, buff, sizeof(buff)); } else { @@ -2595,7 +2595,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .osd_failsafe_switch_layout = false, .plus_code_digits = 11, - .plus_code_format = OSD_PLUS_CODE_GLOBAL, + .plus_code_short = 0, .ahi_width = OSD_AHI_WIDTH * OSD_CHAR_WIDTH, .ahi_height = OSD_AHI_HEIGHT * OSD_CHAR_HEIGHT, diff --git a/src/main/io/osd.h b/src/main/io/osd.h index ef5635a55f..4654a6dc3a 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -268,13 +268,6 @@ typedef enum { OSD_CRSF_LQ_TYPE2, } osd_crsf_lq_format_e; -typedef enum { - OSD_PLUS_CODE_GLOBAL, - OSD_PLUS_CODE_COUNTRY, - OSD_PLUS_CODE_LOCAL, - OSD_PLUS_CODE_FIELD, -} osd_plus_code_format_e; - typedef struct osdLayoutsConfig_s { // Layouts uint16_t item_pos[OSD_LAYOUT_COUNT][OSD_ITEM_COUNT]; @@ -343,7 +336,7 @@ typedef struct osdConfig_s { bool osd_failsafe_switch_layout; uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE - uint8_t plus_code_format; + uint8_t plus_code_short; uint8_t osd_ahi_style; uint8_t force_grid; // Force a pixel based OSD to use grid mode. uint8_t ahi_bordered; // Only used by the AHI widget From 96f2a0828b6b8062236ee662adf635e8292f27db Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Thu, 7 Jan 2021 16:19:43 +0100 Subject: [PATCH 072/103] Bump PG and remove forgotten comment --- src/main/fc/settings.yaml | 1 - src/main/io/osd.c | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 0e11ac314c..468f2e5082 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -156,7 +156,6 @@ tables: enum: navOverridesMotorStop_e values: ["OFF_ALWAYS", "OFF", "AUTO_ONLY", "ALL_NAV"] - name: osd_plus_code_short - # enum: osd_plus_code_short_e values: ["0", "2", "4", "6"] groups: diff --git a/src/main/io/osd.c b/src/main/io/osd.c index bf0cf203fc..3545b00fbe 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -185,7 +185,7 @@ static bool osdDisplayHasCanvas; #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees) -PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 13); +PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 14); PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 0); static int digitCount(int32_t value) From d47349166753df162661681c8295e19a8f6cfd32 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Thu, 7 Jan 2021 16:22:40 +0100 Subject: [PATCH 073/103] Bump PG --- src/main/io/osd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 9953ff9e44..3d61783f3c 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -185,7 +185,7 @@ static bool osdDisplayHasCanvas; #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees) -PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 13); +PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 14); PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 0); static int digitCount(int32_t value) From c7508c78a7f3556b524365fced1f4a7009dfda3b Mon Sep 17 00:00:00 2001 From: Tim Long Date: Fri, 8 Jan 2021 02:41:06 +0000 Subject: [PATCH 074/103] Incorporate code review feedback --- src/main/target/ZEEZF7/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/ZEEZF7/target.h b/src/main/target/ZEEZF7/target.h index 839aeeaa1d..558b7c5910 100755 --- a/src/main/target/ZEEZF7/target.h +++ b/src/main/target/ZEEZF7/target.h @@ -52,7 +52,7 @@ #define USE_BARO #define USE_BARO_DPS310 -#define USE_MAG 1 +#define USE_MAG #define USE_MAG_QMC5883 // *************** Flash **************************** From 2fdf53c04b6b48f64dbe6c2fe0af03230307873b Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Fri, 8 Jan 2021 18:51:00 +0000 Subject: [PATCH 075/103] address 10.20 "maybe unintialised" warnings with MinSizeRel builds --- src/main/fc/cli.c | 20 ++++++++++---------- src/main/msc/emfat_file.c | 2 +- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index e3b147d484..3555f463c5 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -1208,10 +1208,10 @@ static void cliTempSensor(char *cmdline) } else { int16_t i; const char *ptr = cmdline, *label; - int16_t type, alarm_min, alarm_max; + int16_t type=0, alarm_min=0, alarm_max=0; bool addressValid = false; uint64_t address; - int8_t osdSymbol; + int8_t osdSymbol=0; uint8_t validArgumentCount = 0; i = fastA2I(ptr); if (i >= 0 && i < MAX_TEMP_SENSORS) { @@ -1300,8 +1300,8 @@ static void cliSafeHomes(char *cmdline) } else if (sl_strcasecmp(cmdline, "reset") == 0) { resetSafeHomes(); } else { - int32_t lat, lon; - bool enabled; + int32_t lat=0, lon=0; + bool enabled=false; uint8_t validArgumentCount = 0; const char *ptr = cmdline; int8_t i = fastA2I(ptr); @@ -1402,9 +1402,9 @@ static void cliWaypoints(char *cmdline) cliShowParseError(); } } else { - int16_t i, p1,p2=0,p3=0,tmp; - uint8_t action, flag; - int32_t lat, lon, alt; + int16_t i, p1=0,p2=0,p3=0,tmp=0; + uint8_t action=0, flag=0; + int32_t lat=0, lon=0, alt=0; uint8_t validArgumentCount = 0; const char *ptr = cmdline; i = fastA2I(ptr); @@ -2014,9 +2014,9 @@ static void printPid(uint8_t dumpMask, const programmingPid_t *programmingPids, 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.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, diff --git a/src/main/msc/emfat_file.c b/src/main/msc/emfat_file.c index 5d3c0d8142..c6bad38a26 100644 --- a/src/main/msc/emfat_file.c +++ b/src/main/msc/emfat_file.c @@ -411,7 +411,7 @@ static int emfat_find_log(emfat_entry_t *entry, int maxCount, int flashfsUsedSpa char *last; char* tok = strtok_r((char *)buffer, "-T:.", &last); int index=0; - int year=0,month,day,hour,min,sec; + int year=0,month=0,day=0,hour=0,min=0,sec=0; while (tok != NULL) { switch(index) { case 0: From eb8a882f3c50a764323ee3a6c56fa2b1eb7d445d Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Fri, 8 Jan 2021 18:58:00 +0000 Subject: [PATCH 076/103] address one more 10.20 "maybe unintialised" warnings with MinSizeRel builds --- src/main/io/osd_canvas.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd_canvas.c b/src/main/io/osd_canvas.c index dedb36db34..a13e66bc47 100644 --- a/src/main/io/osd_canvas.c +++ b/src/main/io/osd_canvas.c @@ -647,7 +647,7 @@ static bool osdCanvasDrawSidebar(uint32_t *configured, displayWidgets_t *widgets .options = options, .divisions = OSD_AH_SIDEBAR_HEIGHT_POS * 2, }; - uint16_t countsPerStep; + uint16_t countsPerStep = 0; osdCanvasSidebarGetUnit(&config.unit, &countsPerStep, scroll); int center = ex * OSD_CHAR_WIDTH; From ef34fc29f2abe9abd0270d4e4f3cc61bc7651849 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Sat, 9 Jan 2021 11:18:30 +0100 Subject: [PATCH 077/103] Remove unused UAV_INTERCONNECT --- src/main/CMakeLists.txt | 6 - src/main/drivers/opflow/opflow_virtual.c | 2 +- .../drivers/rangefinder/rangefinder_virtual.c | 2 +- src/main/fc/fc_init.c | 6 - src/main/fc/fc_tasks.c | 16 +- src/main/fc/settings.yaml | 4 +- src/main/io/serial.h | 2 +- src/main/rx/rx.c | 7 - src/main/rx/rx.h | 2 +- src/main/rx/uib_rx.c | 126 ---- src/main/rx/uib_rx.h | 31 - src/main/scheduler/scheduler.h | 3 - src/main/sensors/rangefinder.c | 11 - src/main/sensors/rangefinder.h | 2 +- src/main/target/common.h | 2 - src/main/uav_interconnect/uav_interconnect.h | 70 -- .../uav_interconnect/uav_interconnect_bus.c | 635 ------------------ .../uav_interconnect_rangefinder.c | 121 ---- 18 files changed, 8 insertions(+), 1040 deletions(-) delete mode 100755 src/main/rx/uib_rx.c delete mode 100755 src/main/rx/uib_rx.h delete mode 100755 src/main/uav_interconnect/uav_interconnect.h delete mode 100755 src/main/uav_interconnect/uav_interconnect_bus.c delete mode 100755 src/main/uav_interconnect/uav_interconnect_rangefinder.c diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index e0c50535eb..364356342f 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -406,8 +406,6 @@ main_sources(COMMON_SRC rx/sumd.h rx/sumh.c rx/sumh.h - rx/uib_rx.c - rx/uib_rx.h rx/xbus.c rx/xbus.h @@ -435,10 +433,6 @@ main_sources(COMMON_SRC sensors/temperature.c sensors/temperature.h - uav_interconnect/uav_interconnect.h - uav_interconnect/uav_interconnect_bus.c - uav_interconnect/uav_interconnect_rangefinder.c - blackbox/blackbox.c blackbox/blackbox.h blackbox/blackbox_encoding.c diff --git a/src/main/drivers/opflow/opflow_virtual.c b/src/main/drivers/opflow/opflow_virtual.c index c6f9857e81..a559d5be19 100755 --- a/src/main/drivers/opflow/opflow_virtual.c +++ b/src/main/drivers/opflow/opflow_virtual.c @@ -23,7 +23,7 @@ */ /* - * This is a bridge driver between a sensor reader that operates at high level (on top of UART or UIB) + * This is a bridge driver between a sensor reader that operates at high level (i.e. on top of UART) */ #include diff --git a/src/main/drivers/rangefinder/rangefinder_virtual.c b/src/main/drivers/rangefinder/rangefinder_virtual.c index 4d535db8bb..c3bf85b89b 100644 --- a/src/main/drivers/rangefinder/rangefinder_virtual.c +++ b/src/main/drivers/rangefinder/rangefinder_virtual.c @@ -23,7 +23,7 @@ */ /* - * This is a bridge driver between a sensor reader that operates at high level (on top of UART or UIB) + * This is a bridge driver between a sensor reader that operates at high level (i.e. on top of UART) */ #include diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index b8cb792156..025861d35e 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -148,8 +148,6 @@ #include "telemetry/telemetry.h" -#include "uav_interconnect/uav_interconnect.h" - #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif @@ -570,10 +568,6 @@ void init(void) } #endif -#ifdef USE_UAV_INTERCONNECT - uavInterconnectBusInit(); -#endif - #if defined(USE_CMS) && defined(USE_SPEKTRUM_CMS_TELEMETRY) && defined(USE_TELEMETRY_SRXL) // Register the srxl Textgen telemetry sensor as a displayport device cmsDisplayPortRegister(displayPortSrxlInit()); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index b000de7017..260d5b2f63 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -91,8 +91,6 @@ #include "config/feature.h" -#include "uav_interconnect/uav_interconnect.h" - void taskHandleSerial(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); @@ -365,9 +363,6 @@ void fcTasksInit(void) setTaskEnabled(TASK_VTXCTRL, true); #endif #endif -#ifdef USE_UAV_INTERCONNECT - setTaskEnabled(TASK_UAV_INTERCONNECT, uavInterconnectBusIsInitialized()); -#endif #ifdef USE_RCDEVICE setTaskEnabled(TASK_RCDEVICE, rcdeviceIsEnabled()); #endif @@ -572,16 +567,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_OPFLOW] = { .taskName = "OPFLOW", .taskFunc = taskUpdateOpticalFlow, - .desiredPeriod = TASK_PERIOD_HZ(100), // I2C/SPI sensor will work at higher rate and accumulate, UIB/UART sensor will work at lower rate w/o accumulation - .staticPriority = TASK_PRIORITY_MEDIUM, - }, -#endif - -#ifdef USE_UAV_INTERCONNECT - [TASK_UAV_INTERCONNECT] = { - .taskName = "UIB", - .taskFunc = uavInterconnectBusTask, - .desiredPeriod = 1000000 / 500, // 500 Hz + .desiredPeriod = TASK_PERIOD_HZ(100), // I2C/SPI sensor will work at higher rate and accumulate, UART sensor will work at lower rate w/o accumulation .staticPriority = TASK_PRIORITY_MEDIUM, }, #endif diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 95199bde63..f5fde879ef 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -7,7 +7,7 @@ tables: values: ["NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "FAKE"] enum: accelerationSensor_e - name: rangefinder_hardware - values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UIB", "BENEWAKE"] + values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE"] enum: rangefinderType_e - name: mag_hardware values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "FAKE"] @@ -22,7 +22,7 @@ tables: values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP"] enum: pitotSensor_e - name: receiver_type - values: ["NONE", "PPM", "SERIAL", "MSP", "SPI", "UIB"] + values: ["NONE", "PPM", "SERIAL", "MSP", "SPI", "UNUSED"] enum: rxReceiverType_e - name: serial_rx values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2", "GHST"] diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 9ea7869ab1..7b4f05c76b 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -44,7 +44,7 @@ typedef enum { FUNCTION_RCDEVICE = (1 << 10), // 1024 FUNCTION_VTX_SMARTAUDIO = (1 << 11), // 2048 FUNCTION_VTX_TRAMP = (1 << 12), // 4096 - FUNCTION_UAV_INTERCONNECT = (1 << 13), // 8192 + FUNCTION_UNUSED_1 = (1 << 13), // 8192: former UAV_INTERCONNECT FUNCTION_OPTICAL_FLOW = (1 << 14), // 16384 FUNCTION_LOG = (1 << 15), // 32768 FUNCTION_RANGEFINDER = (1 << 16), // 65536 diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index c8baee9322..1a9ef844bd 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -64,7 +64,6 @@ #include "rx/srxl2.h" #include "rx/sumd.h" #include "rx/sumh.h" -#include "rx/uib_rx.h" #include "rx/xbus.h" #include "rx/ghst.h" @@ -330,12 +329,6 @@ void rxInit(void) break; #endif -#ifdef USE_RX_UIB - case RX_TYPE_UIB: - rxUIBInit(rxConfig(), &rxRuntimeConfig); - break; -#endif - #ifdef USE_RX_SPI case RX_TYPE_SPI: if (!rxSpiInit(rxConfig(), &rxRuntimeConfig)) { diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 7c7b7367b4..0aee1e8353 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -65,7 +65,7 @@ typedef enum { RX_TYPE_SERIAL = 2, RX_TYPE_MSP = 3, RX_TYPE_SPI = 4, - RX_TYPE_UIB = 5 + RX_TYPE_UNUSED_1 = 5 } rxReceiverType_e; typedef enum { diff --git a/src/main/rx/uib_rx.c b/src/main/rx/uib_rx.c deleted file mode 100755 index e8ab466927..0000000000 --- a/src/main/rx/uib_rx.c +++ /dev/null @@ -1,126 +0,0 @@ -/* - * This file is part of INAV. - * - * 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/. - * - * @author Konstantin Sharlaimov - */ - -#include -#include -#include - -#include "platform.h" - -#if defined(USE_UAV_INTERCONNECT) && defined(USE_RX_UIB) - -#include "build/build_config.h" -#include "build/debug.h" - -#include "common/utils.h" -#include "common/maths.h" - -#include "rx/rx.h" -#include "rx/uib_rx.h" - -#include "uav_interconnect/uav_interconnect.h" - -#define UIB_DEVICE_ADDRESS UIB_DEVICE_ID_RC_RECEIVER - -typedef struct __attribute__((packed)) { - uint8_t flags; // UIB_DATA_VALID (0x01) - link ok - uint8_t rssi; - uint8_t sticks[4]; // Values in range [0;255], center = 127 - uint8_t aux[8]; // Analog AUX channels - values in range [0;255], center = 127 - uint16_t reserved; // Reserved for future use -} rcUibReceiverData_t; - -static rcUibReceiverData_t uibData; - -#define UIB_RX_MAX_CHANNEL_COUNT 16 - -static uint16_t rxUIBReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan) -{ - UNUSED(rxRuntimeConfigPtr); - - switch (chan) { - case 0 ... 3: - return scaleRange(uibData.sticks[chan], 0, 255, 1000, 2000); - - case 4 ... 11: - return scaleRange(uibData.aux[chan - 4], 0, 255, 1000, 2000); - - case 12: - case 13: - case 14: - return 1500; - - case 15: - return scaleRange(uibData.rssi, 0, 255, 1000, 2000); - - default: - return 1500; - } -} - -static uint8_t rxUIBFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) -{ - UNUSED(rxRuntimeConfig); - - if (!uavInterconnectBusIsInitialized()) { - return RX_FRAME_FAILSAFE; - } - - // If bus didn't detect the yet - report failure - if (!uibDetectAndActivateDevice(UIB_DEVICE_ADDRESS)) { - return RX_FRAME_FAILSAFE; - } - - if (uibGetUnansweredRequests(UIB_DEVICE_ADDRESS) > 10) { // Tolerate 200ms loss (10 packet loss) - return RX_FRAME_FAILSAFE; - } - - if (uibDataAvailable(UIB_DEVICE_ADDRESS)) { - rcUibReceiverData_t uibDataTmp; - uibRead(UIB_DEVICE_ADDRESS, (uint8_t*)&uibDataTmp, sizeof(uibDataTmp)); - - if (!(uibDataTmp.flags & UIB_DATA_VALID)) - return RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE; - - memcpy(&uibData, &uibDataTmp, sizeof(rcUibReceiverData_t)); - return RX_FRAME_COMPLETE; - } - else { - return RX_FRAME_PENDING; - } -} - -void rxUIBInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) -{ - UNUSED(rxConfig); - - rxRuntimeConfig->channelCount = UIB_RX_MAX_CHANNEL_COUNT; - rxRuntimeConfig->rxRefreshRate = 20000; - rxRuntimeConfig->rxSignalTimeout = DELAY_5_HZ; - rxRuntimeConfig->rcReadRawFn = rxUIBReadRawRC; - rxRuntimeConfig->rcFrameStatusFn = rxUIBFrameStatus; -} -#endif diff --git a/src/main/rx/uib_rx.h b/src/main/rx/uib_rx.h deleted file mode 100755 index 1db7a10221..0000000000 --- a/src/main/rx/uib_rx.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * This file is part of INAV. - * - * 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/. - * - * @author Konstantin Sharlaimov - */ - -#pragma once - -#include "rx/rx.h" - -void rxUIBInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index b4661d16e0..233eaec99c 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -101,9 +101,6 @@ typedef enum { #ifdef USE_OPFLOW TASK_OPFLOW, #endif -#ifdef USE_UAV_INTERCONNECT - TASK_UAV_INTERCONNECT, -#endif #ifdef USE_RCDEVICE TASK_RCDEVICE, #endif diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index aec1e065aa..29955a42d5 100644 --- a/src/main/sensors/rangefinder.c +++ b/src/main/sensors/rangefinder.c @@ -53,8 +53,6 @@ #include "scheduler/scheduler.h" -#include "uav_interconnect/uav_interconnect.h" - rangefinder_t rangefinder; #define RANGEFINDER_HARDWARE_TIMEOUT_MS 500 // Accept 500ms of non-responsive sensor, report HW failure otherwise @@ -142,15 +140,6 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar #endif break; - case RANGEFINDER_UIB: -#if defined(USE_RANGEFINDER_UIB) - if (uibRangefinderDetect(dev)) { - rangefinderHardware = RANGEFINDER_UIB; - rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_UIB_TASK_PERIOD_MS)); - } -#endif - break; - case RANGEFINDER_BENEWAKE: #if defined(USE_RANGEFINDER_BENEWAKE) if (virtualRangefinderDetect(dev, &rangefinderBenewakeVtable)) { diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h index c80d18ac1e..2cd7d01e1c 100644 --- a/src/main/sensors/rangefinder.h +++ b/src/main/sensors/rangefinder.h @@ -28,7 +28,7 @@ typedef enum { RANGEFINDER_HCSR04I2C = 3, RANGEFINDER_VL53L0X = 4, RANGEFINDER_MSP = 5, - RANGEFINDER_UIB = 6, + RANGEFINDER_UNUSED = 6, // Was UIB RANGEFINDER_BENEWAKE = 7, } rangefinderType_e; diff --git a/src/main/target/common.h b/src/main/target/common.h index dbe28d00a5..9d0ddc3346 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -87,8 +87,6 @@ #define USE_DYNAMIC_FILTERS #define USE_GYRO_KALMAN #define USE_EXTENDED_CMS_MENUS -#define USE_UAV_INTERCONNECT -#define USE_RX_UIB #define USE_HOTT_TEXTMODE // NAZA GPS support for F4+ only diff --git a/src/main/uav_interconnect/uav_interconnect.h b/src/main/uav_interconnect/uav_interconnect.h deleted file mode 100755 index 95e9e46e41..0000000000 --- a/src/main/uav_interconnect/uav_interconnect.h +++ /dev/null @@ -1,70 +0,0 @@ -/* - * This file is part of INAV. - * - * 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/. - * - * @author Konstantin Sharlaimov - */ - -#include -#include -#include -#include - -#include "platform.h" -#include "build/build_config.h" -#include "drivers/rangefinder/rangefinder.h" - -#ifdef USE_UAV_INTERCONNECT - -#define UIB_MAX_PACKET_SIZE 32 - -#define UIB_DEVICE_ID_RANGEFINDER 0x12 -#define UIB_DEVICE_ID_GPS 0x13 -#define UIB_DEVICE_ID_COMPASS 0x14 -#define UIB_DEVICE_ID_RC_RECEIVER 0x80 - -typedef enum { - UIB_DATA_NONE = 0, - UIB_DATA_VALID = (1 << 0), // Data is valid -} uibDataFlags_t; - -/* Bus task */ -bool uavInterconnectBusCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime); -void uavInterconnectBusTask(timeUs_t currentTimeUs); -void uavInterconnectBusInit(void); -bool uavInterconnectBusIsInitialized(void); - -/* Bus device API */ -bool uibRegisterDevice(uint8_t devId); -bool uibDetectAndActivateDevice(uint8_t devId); -bool uibGetDeviceParams(uint8_t devId, uint8_t * params); -timeUs_t uibGetPollRateUs(uint8_t devId); -uint32_t uibGetUnansweredRequests(uint8_t devId); -uint8_t uibDataAvailable(uint8_t devId); -uint8_t uibRead(uint8_t devId, uint8_t * buffer, uint8_t bufSize); -bool uibCanWrite(uint8_t devId); -bool uibWrite(uint8_t devId, const uint8_t * buffer, uint8_t bufSize); - -#define RANGEFINDER_UIB_TASK_PERIOD_MS 100 -bool uibRangefinderDetect(rangefinderDev_t *dev); - -#endif diff --git a/src/main/uav_interconnect/uav_interconnect_bus.c b/src/main/uav_interconnect/uav_interconnect_bus.c deleted file mode 100755 index 9eb0fca674..0000000000 --- a/src/main/uav_interconnect/uav_interconnect_bus.c +++ /dev/null @@ -1,635 +0,0 @@ -/* - * This file is part of INAV. - * - * 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/. - * - * @author Konstantin Sharlaimov - */ - -#include -#include -#include -#include -#include - -#include "platform.h" -#include "build/build_config.h" - -#ifdef USE_UAV_INTERCONNECT - -#include "build/debug.h" - -#include "common/maths.h" -#include "common/axis.h" -#include "common/utils.h" -#include "common/crc.h" - -#include "config/parameter_group.h" -#include "config/parameter_group_ids.h" - -#include "drivers/serial.h" -#include "drivers/time.h" - -#include "io/serial.h" - -#include "uav_interconnect/uav_interconnect.h" - -typedef enum { - UIB_COMMAND_IDENTIFY = (0x00 << 5), // 0x00 - UIB_COMMAND_NOTIFY = (0x01 << 5), // 0x20 - UIB_COMMAND_READ = (0x02 << 5), // 0x40 - UIB_COMMAND_WRITE = (0x03 << 5), // 0x60 - UIB_COMMAND_RES_1 = (0x04 << 5), // 0x80 - UIB_COMMAND_RES_2 = (0x05 << 5), // 0xA0 - UIB_COMMAND_RES_3 = (0x06 << 5), // 0xC0 - UIB_COMMAND_RES_4 = (0x07 << 5), // 0xE0 -} uibCommand_e; - -typedef enum { - UIB_FLAG_HAS_READ = (1 << 0), // Device supports READ command (sensor) - UIB_FLAG_HAS_WRITE = (1 << 1), // Device supports WRITE command (sensor configuration or executive device) -} uibDeviceFlags_e; - -#define UIB_PROTOCOL_VERSION 0x00 -#define UIB_MAX_SLOTS 16 // 32 is design maximum -#define UIB_PORT_OPTIONS (SERIAL_NOT_INVERTED | SERIAL_STOPBITS_1 | SERIAL_PARITY_NO | SERIAL_UNIDIR) - -#define UIB_DISCOVERY_DELAY_US 2000000 // 2 seconds from power-up to allow all devices to boot -#define UIB_GUARD_INTERVAL_US 1000 -#define UIB_REFRESH_INTERVAL_US 200000 - -typedef struct { - bool allocated; - bool activated; - uint8_t deviceAddress; - uint16_t deviceFlags; - uint8_t devParams[4]; - timeUs_t pollIntervalUs; - timeUs_t lastPollTimeUs; - uint32_t unrepliedRequests; // 0 - all answered, 1 - request in progress, 2 or more - device failed to answer one or more requests - - uint8_t rxDataReadySize; - uint8_t txDataReadySize; - uint8_t rxPacket[UIB_MAX_PACKET_SIZE]; - uint8_t txPacket[UIB_MAX_PACKET_SIZE]; -} uavInterconnectSlot_t; - -typedef enum { - STATE_INITIALIZE, - STATE_DISCOVER, - STATE_READY, -} uavInterconnectState_t; - -typedef struct { - int sentCommands; - int discoveredDevices; - int failedCRC; - int commandTimeouts; -} uavInterconnectStats_t; - -static serialPort_t * busPort; -static bool uibInitialized = false; - -static uavInterconnectSlot_t slots[UIB_MAX_SLOTS]; -static timeUs_t slotStartTimeUs; -static uavInterconnectState_t busState = STATE_INITIALIZE; - -static int discoveryAddress; -static int refreshSlot; -static timeUs_t refreshStartTimeUs; - -static uint8_t slotDataBuffer[20]; // Max transaction length is 20 bytes -static unsigned slotDataBufferCount; -static timeUs_t slotLastActivityUs; - -// Statistics -static uavInterconnectStats_t uibStats; - -static void switchState(uavInterconnectState_t newState) -{ - if (busState == newState) - return; - - busState = newState; -} - -static void sendDiscover(timeUs_t currentTimeUs, uint8_t slot, uint8_t devId) -{ - uint8_t buf[4]; - buf[0] = UIB_COMMAND_IDENTIFY | slot; - buf[1] = devId; - buf[2] = UIB_PROTOCOL_VERSION; - buf[3] = crc8_dvb_s2_update(0x00, &buf[0], 3); - slotStartTimeUs = currentTimeUs; - serialWriteBuf(busPort, buf, 4); - uibStats.sentCommands++; -} - -static void sendNotify(timeUs_t currentTimeUs, uint8_t slot, uint8_t devId) -{ - uint8_t buf[4]; - buf[0] = UIB_COMMAND_NOTIFY | slot; - buf[1] = devId; - buf[2] = UIB_PROTOCOL_VERSION; - buf[3] = crc8_dvb_s2_update(0x00, &buf[0], 3); - slotStartTimeUs = currentTimeUs; - serialWriteBuf(busPort, buf, 4); - uibStats.sentCommands++; -} - -static void sendRead(timeUs_t currentTimeUs, uint8_t slot) -{ - uint8_t buf[2]; - buf[0] = UIB_COMMAND_READ | slot; - buf[1] = crc8_dvb_s2_update(0x00, &buf[0], 1); - slotStartTimeUs = currentTimeUs; - serialWriteBuf(busPort, buf, 2); - uibStats.sentCommands++; -} - -static void sendWrite(timeUs_t currentTimeUs, uint8_t slot, uint8_t * data, uint8_t len) -{ - uint8_t buf[UIB_MAX_PACKET_SIZE + 3]; - buf[0] = UIB_COMMAND_WRITE | slot; - buf[1] = len; - memcpy(&buf[2], data, len); - buf[UIB_MAX_PACKET_SIZE + 2] = crc8_dvb_s2_update(0x00, &buf[0], UIB_MAX_PACKET_SIZE + 2); - slotStartTimeUs = currentTimeUs; - serialWriteBuf(busPort, buf, UIB_MAX_PACKET_SIZE + 3); - uibStats.sentCommands++; -} - -static uavInterconnectSlot_t * findDevice(uint8_t devId) -{ - for (int i = 0; i < UIB_MAX_SLOTS; i++) { - if (slots[i].allocated && slots[i].deviceAddress == devId) { - return &slots[i]; - } - } - - return NULL; -} - -static int findEmptySlot(void) -{ - for (int i = 0; i < UIB_MAX_SLOTS; i++) { - if (!slots[i].allocated) - return i; - } - - return -1; -} - -static void registerDeviceSlot(uint8_t slotId, uint8_t devId, uint16_t deviceFlags, uint32_t pollIntervalUs, const uint8_t * devParams) -{ - slots[slotId].allocated = true; - slots[slotId].deviceAddress = devId; - slots[slotId].deviceFlags = deviceFlags; - slots[slotId].pollIntervalUs = pollIntervalUs; - - if (devParams) { - slots[slotId].devParams[0] = devParams[0]; - slots[slotId].devParams[1] = devParams[1]; - slots[slotId].devParams[2] = devParams[2]; - slots[slotId].devParams[3] = devParams[3]; - } - else { - slots[slotId].devParams[0] = 0; - slots[slotId].devParams[1] = 0; - slots[slotId].devParams[2] = 0; - slots[slotId].devParams[3] = 0; - } - - slots[slotId].rxDataReadySize = 0; - slots[slotId].txDataReadySize = 0; - slots[slotId].lastPollTimeUs = 0; - slots[slotId].unrepliedRequests = 0; -} - -typedef struct __attribute__((packed)) { - uint8_t slotAndCmd; - uint8_t devId; - uint8_t protoVersion; - uint8_t crc1; - uint16_t pollIntervalMs; - uint16_t devFlags; - uint8_t devParams[4]; - uint8_t crc2; -} uibPktIdentify_t; - -typedef struct __attribute__((packed)) { - uint8_t slotAndCmd; - uint8_t devId; - uint8_t protoVersion; - uint8_t crc1; -} uibPktNotify_t; - -typedef struct __attribute__((packed)) { - uint8_t slotAndCmd; - uint8_t crc1; - uint8_t size; - uint8_t data[0]; -} uibPktRead_t; - -typedef struct __attribute__((packed)) { - uint8_t slotAndCmd; - uint8_t size; - uint8_t data[0]; -} uibPktWrite_t; - -typedef union { - uibPktIdentify_t ident; - uibPktNotify_t notify; - uibPktRead_t read; - uibPktWrite_t write; -} uibPktData_t; - -static void processSlot(void) -{ - // First byte is command / slot - const uint8_t cmd = slotDataBuffer[0] & 0xE0; - const uint8_t slot = slotDataBuffer[0] & 0x1F; - const uibPktData_t * pkt = (const uibPktData_t *)&slotDataBuffer[0]; - const int lastPacketByteIndex = slotDataBufferCount - 1; - - // CRC is calculated over the whole slot, including command byte(s) sent by FC. This ensures integrity of the transaction as a whole - switch (cmd) { - // Identify command (13 bytes) - // FC: IDENTIFY[1] + DevID[1] + ProtoVersion [1] + CRC1[1] - // DEV: PollInterval[2] + Flags[2] + DevParams[4] + CRC2[1] - case UIB_COMMAND_IDENTIFY: - if (slotDataBufferCount == sizeof(uibPktIdentify_t)) { - if (crc8_dvb_s2_update(0x00, pkt, slotDataBufferCount - 1) == pkt->ident.crc2) { - // CRC valid - process valid IDENTIFY slot - registerDeviceSlot(slot, pkt->ident.devId, pkt->ident.devFlags, pkt->ident.pollIntervalMs * 1000, pkt->ident.devParams); - uibStats.discoveredDevices++; - } - else { - uibStats.failedCRC++; - } - - // Regardless of CRC validity - discard buffer data - slotDataBufferCount = 0; - } - break; - - // NOTIFY command (4 bytes) - // FC: IDENTIFY[1] + DevID[1] + ProtoVersion [1] + CRC1[1] - case UIB_COMMAND_NOTIFY: - if (slotDataBufferCount == sizeof(uibPktNotify_t)) { - // Record failed CRC. Do nothing else - NOTIFY is only sent for devices that already have a slot assigned - if (crc8_dvb_s2_update(0x00, pkt, slotDataBufferCount - 1) != pkt->notify.crc1) { - uibStats.failedCRC++; - } - - slotDataBufferCount = 0; - } - break; - - // Read command (min 4 bytes) - // FC: READ[1] + CRC1[1] - // DEV: DATA_LEN[1] + DATA[variable] + CRC2[1] - case UIB_COMMAND_READ: - if ((slotDataBufferCount >= sizeof(uibPktRead_t)) && (slotDataBufferCount == ((unsigned)pkt->read.size + 4)) && (pkt->read.size <= UIB_MAX_PACKET_SIZE)) { - if (crc8_dvb_s2_update(0x00, pkt, slotDataBufferCount - 1) == slotDataBuffer[lastPacketByteIndex]) { - // CRC valid - process valid READ slot - // Check if this slot has read capability and is allocated - if (slots[slot].allocated && (slots[slot].deviceFlags & UIB_FLAG_HAS_READ)) { - if ((slots[slot].rxDataReadySize == 0) && (pkt->read.size > 0)) { - memcpy(slots[slot].rxPacket, pkt->read.data, pkt->read.size); - slots[slot].rxDataReadySize = pkt->read.size; - } - - slots[slot].unrepliedRequests = 0; - } - } - else { - uibStats.failedCRC++; - } - - // Regardless of CRC validity - discard buffer data - slotDataBufferCount = 0; - } - break; - - // Write command (min 3 bytes) - // FC: WRITE[1] + DATA_LEN[1] + DATA[variable] + CRC1[1] - case UIB_COMMAND_WRITE: - if ((slotDataBufferCount >= sizeof(uibPktWrite_t)) && (slotDataBufferCount == ((unsigned)pkt->write.size + 3)) && (pkt->write.size <= UIB_MAX_PACKET_SIZE)) { - // Keep track of failed CRC events - if (crc8_dvb_s2_update(0x00, pkt, slotDataBufferCount - 1) != slotDataBuffer[lastPacketByteIndex]) { - uibStats.failedCRC++; - } - - // Regardless of CRC validity - discard buffer data - slotDataBufferCount = 0; - } - break; - - default: - break; - } -} - -static void processScheduledTransactions(timeUs_t currentTimeUs) -{ - int slotPrio = 0x100; - int slotId = -1; - - // First - find device with highest priority that has the READ capability and is scheduled for READ - for (int i = 0; i < UIB_MAX_SLOTS; i++) { - // Only do scheduled READs on allocated and active slots - if (!(slots[i].allocated && slots[i].activated)) - continue; - - if ((slots[i].deviceFlags & UIB_FLAG_HAS_READ) && ((currentTimeUs - slots[i].lastPollTimeUs) >= slots[i].pollIntervalUs) && (slotPrio > slots[i].deviceAddress)) { - slotId = i; - slotPrio = slots[i].deviceAddress; - } - } - - // READ command - if (slotId >= 0) { - sendRead(currentTimeUs, slotId); - slots[slotId].unrepliedRequests++; - slots[slotId].lastPollTimeUs = currentTimeUs; - return; - } - - // No READ command executed - check if we have data to WRITE - slotPrio = 0x100; - slotId = -1; - - for (int i = 0; i < UIB_MAX_SLOTS; i++) { - // Only do WRITEs on allocated and active slots - if (!(slots[i].allocated && slots[i].activated)) - continue; - - if (slots[i].txDataReadySize && (slots[i].deviceFlags & UIB_FLAG_HAS_WRITE) && (slotPrio > slots[i].deviceAddress)) { - slotId = i; - slotPrio = slots[i].deviceAddress; - } - } - - // WRITE command - if (slotId >= 0) { - sendWrite(currentTimeUs, slotId, slots[slotId].txPacket, slots[slotId].txDataReadySize); - slots[slotId].unrepliedRequests++; - slots[slotId].txDataReadySize = 0; - return; - } - - // Neither READ nor WRITE command are queued - issue IDENTIFY once in a while - if ((currentTimeUs - refreshStartTimeUs) > UIB_REFRESH_INTERVAL_US) { - // Send notifications for allocated slots - if (slots[refreshSlot].allocated) { - sendNotify(currentTimeUs, refreshSlot, slots[refreshSlot].deviceAddress); - refreshStartTimeUs = currentTimeUs; - } - - if (++refreshSlot >= UIB_MAX_SLOTS) { - refreshSlot = 0; - } - } -} - -static bool canSendNewRequest(timeUs_t currentTimeUs) -{ - return ((currentTimeUs - slotLastActivityUs) >= UIB_GUARD_INTERVAL_US); -} - -void uavInterconnectBusTask(timeUs_t currentTimeUs) -{ - if (!uibInitialized) - return; - - // Receive bytes to the buffer - bool hasNewBytes = false; - while (serialRxBytesWaiting(busPort) > 0) { - uint8_t c = serialRead(busPort); - if (slotDataBufferCount < (int)(sizeof(slotDataBuffer) / sizeof(slotDataBuffer[0]))) { - slotDataBuffer[slotDataBufferCount++] = c; - hasNewBytes = true; - } - - slotLastActivityUs = currentTimeUs; - } - - // Flush receive buffer if guard interval elapsed - if ((currentTimeUs - slotLastActivityUs) >= UIB_GUARD_INTERVAL_US && slotDataBufferCount > 0) { - uibStats.commandTimeouts++; - slotDataBufferCount = 0; - } - - // If we have new bytes - process packet - if (hasNewBytes && slotDataBufferCount >= 4) { // minimum transaction length is 4 bytes - no point in processing something smaller - processSlot(); - } - - // Process request scheduling - we can initiate another slot if guard interval has elapsed and slot interval has elapsed as well - if (canSendNewRequest(currentTimeUs)) { - // We get here only if we can send requests - no timeout checking should be done beyond this point - switch (busState) { - case STATE_INITIALIZE: - if ((currentTimeUs - slotStartTimeUs) > UIB_DISCOVERY_DELAY_US) { - discoveryAddress = 0; - switchState(STATE_DISCOVER); - } - break; - - case STATE_DISCOVER: - { - int discoverySlot = findEmptySlot(); - if (discoverySlot >= 0) { - sendDiscover(currentTimeUs, discoverySlot, discoveryAddress); - if (discoveryAddress == 0xFF) { - // All addresses have been polled - switchState(STATE_READY); - } - else { - // Query next address and stick here - discoveryAddress++; - } - } - else { - // All slots are allocated - can't discover more devices - refreshSlot = 0; - refreshStartTimeUs = currentTimeUs; - switchState(STATE_READY); - } - } - break; - - case STATE_READY: - // Bus ready - process scheduled transfers - processScheduledTransactions(currentTimeUs); - break; - } - } -} - -void uavInterconnectBusInit(void) -{ - for (int i = 0; i < UIB_MAX_SLOTS; i++) { - slots[i].allocated = false; - slots[i].activated = false; - } - - serialPortConfig_t * portConfig = findSerialPortConfig(FUNCTION_UAV_INTERCONNECT); - if (!portConfig) - return; - - baudRate_e baudRateIndex = portConfig->peripheral_baudrateIndex; - busPort = openSerialPort(portConfig->identifier, FUNCTION_UAV_INTERCONNECT, NULL, NULL, baudRates[baudRateIndex], MODE_RXTX, UIB_PORT_OPTIONS); - if (!busPort) - return; - - slotStartTimeUs = 0; - uibInitialized = true; -} - -bool uavInterconnectBusIsInitialized(void) -{ - return uibInitialized; -} - -bool uibDetectAndActivateDevice(uint8_t devId) -{ - uavInterconnectSlot_t * slot = findDevice(devId); - if (slot == NULL) - return false; - - /* We have discovered device in out registry and code is asking to access it - activate device */ - slot->activated = true; - - return slot->allocated; -} - -bool uibRegisterDevice(uint8_t devId) -{ - int slotId = findEmptySlot(); - if (slotId >= 0) { - registerDeviceSlot(slotId, devId, UIB_FLAG_HAS_WRITE, 0, NULL); - return uibDetectAndActivateDevice(devId); - } - - return false; -} - -bool uibGetDeviceParams(uint8_t devId, uint8_t * params) -{ - uavInterconnectSlot_t * slot = findDevice(devId); - if (slot == NULL || params == NULL) - return false; - - params[0] = slot->devParams[0]; - params[1] = slot->devParams[1]; - params[2] = slot->devParams[2]; - params[3] = slot->devParams[3]; - return true; -} - -timeUs_t uibGetPollRateUs(uint8_t devId) -{ - uavInterconnectSlot_t * slot = findDevice(devId); - if (slot == NULL) - return 0; - - return slot->pollIntervalUs; -} - -uint32_t uibGetUnansweredRequests(uint8_t devId) -{ - uavInterconnectSlot_t * slot = findDevice(devId); - if (slot == NULL) - return 0; - - return (slot->unrepliedRequests < 2) ? 0 : slot->unrepliedRequests - 1; -} - -uint8_t uibDataAvailable(uint8_t devId) -{ - uavInterconnectSlot_t * slot = findDevice(devId); - if (slot == NULL) - return 0; - - return slot->rxDataReadySize; -} - -uint8_t uibRead(uint8_t devId, uint8_t * buffer, uint8_t bufSize) -{ - uavInterconnectSlot_t * slot = findDevice(devId); - if (slot == NULL) - return 0; - - // If no READ capability - fail - if (!(slot->deviceFlags & UIB_FLAG_HAS_READ)) - return false; - - // If no data ready - fail - if (!slot->rxDataReadySize) - return 0; - - uint8_t bytes = slot->rxDataReadySize; - memcpy(buffer, slot->rxPacket, MIN(bytes, bufSize)); - slot->rxDataReadySize = 0; - - return bytes; -} - -static bool slotCanWrite(const uavInterconnectSlot_t * slot) -{ - // If no WRITE capability - fail - if (!(slot->deviceFlags & UIB_FLAG_HAS_WRITE)) - return false; - - // If we have unsent data in the buffer - fail - if (slot->txDataReadySize > 0) - return false; - - return true; -} - -bool uibCanWrite(uint8_t devId) -{ - // No device available - uavInterconnectSlot_t * slot = findDevice(devId); - if (slot == NULL) - return false; - - return slotCanWrite(slot); -} - -bool uibWrite(uint8_t devId, const uint8_t * buffer, uint8_t len) -{ - uavInterconnectSlot_t * slot = findDevice(devId); - if (slot == NULL) - return false; - - if (slotCanWrite(slot)) { - memcpy(slot->txPacket, buffer, len); - slot->txDataReadySize = len; - return true; - } - - return false; -} -#endif diff --git a/src/main/uav_interconnect/uav_interconnect_rangefinder.c b/src/main/uav_interconnect/uav_interconnect_rangefinder.c deleted file mode 100755 index 6fd9d825b2..0000000000 --- a/src/main/uav_interconnect/uav_interconnect_rangefinder.c +++ /dev/null @@ -1,121 +0,0 @@ -/* - * This file is part of INAV. - * - * 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/. - * - * @author Konstantin Sharlaimov - */ - -#include -#include -#include -#include -#include - -#include "platform.h" -#include "build/build_config.h" -#include "build/debug.h" - -#include "common/maths.h" -#include "common/axis.h" -#include "common/utils.h" - -#include "config/parameter_group.h" -#include "config/parameter_group_ids.h" - -#ifdef USE_RANGEFINDER_UIB -#include "drivers/rangefinder/rangefinder.h" -#include "uav_interconnect/uav_interconnect.h" - -#define UIB_DEVICE_ADDRESS UIB_DEVICE_ID_RANGEFINDER - -#define RANGEFINDER_MAX_RANGE_CM 1000 -#define RANGEFINDER_DETECTION_CONE_DECIDEGREES 450 - -typedef struct __attribute__((packed)) { - uint8_t flags; - uint16_t distanceCm; -} rangefinderData_t; - -static int32_t lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE; -static rangefinderData_t uibData; - -static void uibRangefinderInit(rangefinderDev_t *dev) -{ - UNUSED(dev); -} - -static void uibRangefinderUpdate(rangefinderDev_t *dev) -{ - UNUSED(dev); - - if (!uavInterconnectBusIsInitialized()) { - lastCalculatedDistance = RANGEFINDER_HARDWARE_FAILURE; - return; - } - - // If bus didn't detect the yet - report failure - if (!uibDetectAndActivateDevice(UIB_DEVICE_ADDRESS)) { - lastCalculatedDistance = RANGEFINDER_HARDWARE_FAILURE; - return; - } - - // If device is not responding - report failure - if (uibGetUnansweredRequests(UIB_DEVICE_ADDRESS) > 0) { - lastCalculatedDistance = RANGEFINDER_HARDWARE_FAILURE; - return; - } - - if (uibDataAvailable(UIB_DEVICE_ADDRESS)) { - uibRead(UIB_DEVICE_ADDRESS, (uint8_t*)&uibData, sizeof(uibData)); - - if (uibData.flags & UIB_DATA_VALID) { - lastCalculatedDistance = uibData.distanceCm; - dev->delayMs = uibGetPollRateUs(UIB_DEVICE_ADDRESS) / 1000; - } - else { - lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE; - } - } -} - -static int32_t uibRangefinderGetDistance(rangefinderDev_t *dev) -{ - UNUSED(dev); - return lastCalculatedDistance; -} - -bool uibRangefinderDetect(rangefinderDev_t *dev) -{ - // This always succeed - dev->delayMs = RANGEFINDER_UIB_TASK_PERIOD_MS; - dev->maxRangeCm = RANGEFINDER_MAX_RANGE_CM; - dev->detectionConeDeciDegrees = RANGEFINDER_DETECTION_CONE_DECIDEGREES; - dev->detectionConeExtendedDeciDegrees = RANGEFINDER_DETECTION_CONE_DECIDEGREES; - - dev->init = &uibRangefinderInit; - dev->update = &uibRangefinderUpdate; - dev->read = &uibRangefinderGetDistance; - - return true; -} - -#endif From 8d5d17fab7a9141d2a03cc1c65f0e05bee2e23d5 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sat, 9 Jan 2021 16:25:31 +0100 Subject: [PATCH 078/103] blink power on current draw alarm --- src/main/io/osd.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 4d7e035919..648f321011 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2037,6 +2037,11 @@ static bool osdDrawSingleElement(uint8_t item) osdFormatCentiNumber(buff, getPower(), 0, 2, 0, 3); buff[3] = SYM_WATT; buff[4] = '\0'; + + uint8_t current_alarm = osdConfig()->current_alarm; + if ((current_alarm > 0) && ((getAmperage() / 100.0f) > current_alarm)) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } break; } From 9311c423bf5c989b1f41dde47310c0c2ee0c546e Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sun, 10 Jan 2021 01:19:01 +0200 Subject: [PATCH 079/103] added vtx_smartaudio_early_akk_workaround options to settings.md --- docs/Settings.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/Settings.md b/docs/Settings.md index 547e03cacd..55bdc14b82 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -494,6 +494,7 @@ | vbat_min_cell_voltage | 330 | Minimum voltage per cell, this triggers battery out alarms, in 0.01V units, default is 330 (3.3V) | | vbat_scale | 1100 | Battery voltage calibration value. 1100 = 11:1 voltage divider (10k:1k) x 100. Adjust this slightly if reported pack voltage is different from multimeter reading. You can get current voltage by typing "status" in cli. | | vbat_warning_cell_voltage | 350 | Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, default is 350 (3.5V) | +| vtx_smartaudio_early_akk_workaround | ON | Enable workaround for early AKK SAudio-enabled VTX bug. | | vtx_band | 4 | Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race. | | vtx_channel | 1 | Channel to use within the configured `vtx_band`. Valid values are [1, 8]. | | vtx_halfduplex | ON | Use half duplex UART to communicate with the VTX, using only a TX pin in the FC. | From a06f59ba828b02730f02f194915909a6d727fd5b Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sun, 10 Jan 2021 01:31:59 +0200 Subject: [PATCH 080/103] added vtx_smartaudio_early_akk_workaround options to settings.md in alphabetical order --- docs/Settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 55bdc14b82..5c66266b76 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -494,7 +494,6 @@ | vbat_min_cell_voltage | 330 | Minimum voltage per cell, this triggers battery out alarms, in 0.01V units, default is 330 (3.3V) | | vbat_scale | 1100 | Battery voltage calibration value. 1100 = 11:1 voltage divider (10k:1k) x 100. Adjust this slightly if reported pack voltage is different from multimeter reading. You can get current voltage by typing "status" in cli. | | vbat_warning_cell_voltage | 350 | Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, default is 350 (3.5V) | -| vtx_smartaudio_early_akk_workaround | ON | Enable workaround for early AKK SAudio-enabled VTX bug. | | vtx_band | 4 | Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race. | | vtx_channel | 1 | Channel to use within the configured `vtx_band`. Valid values are [1, 8]. | | vtx_halfduplex | ON | Use half duplex UART to communicate with the VTX, using only a TX pin in the FC. | @@ -502,6 +501,7 @@ | vtx_max_power_override | 0 | Some VTXes may report max power incorrectly (i.e. 200mW for a 600mW VTX). Use this to override max supported power. 0 to disable and use whatever VTX reports as its capabilities | | vtx_pit_mode_chan | | | | vtx_power | 1 | VTX RF power level to use. The exact number of mw depends on the VTX hardware. | +| vtx_smartaudio_early_akk_workaround | ON | Enable workaround for early AKK SAudio-enabled VTX bug. | | yaw_deadband | 5 | These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. | | yaw_lpf_hz | 30 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) | | yaw_rate | 20 | Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | From 5b0c8535b90cb88d8b8de91e24482ba53dd0a65d Mon Sep 17 00:00:00 2001 From: Tim Long Date: Sun, 10 Jan 2021 00:06:09 +0000 Subject: [PATCH 081/103] Add VTX power switcher via USER1 box on pin PB11 --- src/main/target/ZEEZF7/config.c | 30 ++++++++++++++++++++++++++++++ src/main/target/ZEEZF7/target.h | 2 +- 2 files changed, 31 insertions(+), 1 deletion(-) create mode 100644 src/main/target/ZEEZF7/config.c diff --git a/src/main/target/ZEEZF7/config.c b/src/main/target/ZEEZF7/config.c new file mode 100644 index 0000000000..ffbe3639f0 --- /dev/null +++ b/src/main/target/ZEEZF7/config.c @@ -0,0 +1,30 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" + +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; // VTX power switcher + //pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; +} diff --git a/src/main/target/ZEEZF7/target.h b/src/main/target/ZEEZF7/target.h index 558b7c5910..2d6e4ab51f 100755 --- a/src/main/target/ZEEZF7/target.h +++ b/src/main/target/ZEEZF7/target.h @@ -84,7 +84,7 @@ // *************** PINIO *************************** #define USE_PINIO #define USE_PINIOBOX -#define PINIO1_PIN PB11 +#define PINIO1_PIN PB11 // VTX power switcher // *************** UART ***************************** #define USE_VCP From fcfc1a91b22b89d469af214382b6cfa72c1d11c3 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Tue, 29 Dec 2020 02:21:33 +0200 Subject: [PATCH 082/103] report launch flight mode with higher priority --- src/main/fc/runtime_config.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index cb7af593bb..5ba8c6287e 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -144,6 +144,9 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void) if (FLIGHT_MODE(MANUAL_MODE)) return FLM_MANUAL; + if (FLIGHT_MODE(NAV_LAUNCH_MODE)) + return FLM_LAUNCH; + if (FLIGHT_MODE(NAV_RTH_MODE)) return FLM_RTH; @@ -165,8 +168,6 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void) if (FLIGHT_MODE(HORIZON_MODE)) return FLM_HORIZON; - if (FLIGHT_MODE(NAV_LAUNCH_MODE)) - return FLM_LAUNCH; return STATE(AIRMODE_ACTIVE) ? FLM_ACRO_AIR : FLM_ACRO; } From 686165049ae7465e3359d7c31e85289277c6ccf8 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Wed, 13 Jan 2021 10:39:23 +0100 Subject: [PATCH 083/103] servo middle as first servo command --- src/main/flight/servos.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index f70b0d5327..918a20ce99 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -133,7 +133,7 @@ void servosInit(void) { // give all servos a default command for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - servo[i] = DEFAULT_SERVO_MIDDLE; + servo[i] = servoParams(i)->middle; } /* From 01799179e9b8ac8bf36a3ab1bb69fbe248afd2a5 Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Wed, 13 Jan 2021 11:27:22 +0000 Subject: [PATCH 084/103] FLYWOOF411_V2 based on target files from vendor (#6495) --- src/main/target/FLYWOOF411/CMakeLists.txt | 1 + src/main/target/FLYWOOF411/target.c | 14 +++++++-- src/main/target/FLYWOOF411/target.h | 37 +++++++++++++++++++++-- 3 files changed, 48 insertions(+), 4 deletions(-) diff --git a/src/main/target/FLYWOOF411/CMakeLists.txt b/src/main/target/FLYWOOF411/CMakeLists.txt index ca3e384913..bf329e0d5f 100644 --- a/src/main/target/FLYWOOF411/CMakeLists.txt +++ b/src/main/target/FLYWOOF411/CMakeLists.txt @@ -1 +1,2 @@ target_stm32f411xe(FLYWOOF411) +target_stm32f411xe(FLYWOOF411_V2) diff --git a/src/main/target/FLYWOOF411/target.c b/src/main/target/FLYWOOF411/target.c index 56a44b256d..6354776c73 100644 --- a/src/main/target/FLYWOOF411/target.c +++ b/src/main/target/FLYWOOF411/target.c @@ -26,7 +26,17 @@ const timerHardware_t timerHardware[] = { DEF_TIM(TIM9, CH1, PA2, TIM_USE_PPM, 0, 0), // PPM IN - +#ifdef FLYWOOF411_V2 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S1 - D(2,1) + DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 - D(1,6) + DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 - D(1,1) + DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 - D(1,5) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_ANY, 0, 0), // SOFTSERIAL_1_TX_PIN - D(1,0) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_ANY, 0, 0), // SOFTSERIAL_1_RX_PIN - D(1,3) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_ANY, 0, 0), // RSSI_ADC_CHANNEL 1-7 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_ANY, 0, 0), // 1-4 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_LED, 0, 0), // LED 1,2 +#else DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S1_OUT 2,1 DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S2_OUT 2,2 DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3_OUT 2,6 @@ -35,7 +45,7 @@ const timerHardware_t timerHardware[] = { DEF_TIM(TIM3, CH4, PB1, TIM_USE_ANY, 0, 0), // RSSI 1,2 DEF_TIM(TIM5, CH4, PA3, TIM_USE_ANY, 0, 1), // RX2 1,0 DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED 1,5 - +#endif }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FLYWOOF411/target.h b/src/main/target/FLYWOOF411/target.h index 4c471a044a..57d2347660 100644 --- a/src/main/target/FLYWOOF411/target.h +++ b/src/main/target/FLYWOOF411/target.h @@ -17,8 +17,13 @@ #pragma once +#ifdef FLYWOOF411_V2 +#define TARGET_BOARD_IDENTIFIER "FW42" +#define USBD_PRODUCT_STRING "FLYWOOF411V2" +#else #define TARGET_BOARD_IDENTIFIER "FW41" #define USBD_PRODUCT_STRING "FLYWOOF411" +#endif #define LED0 PC13 @@ -52,7 +57,11 @@ #define IMU_ICM20689_ALIGN CW180_DEG #define USE_EXTI +#ifdef FLYWOOF411_V2 +#define GYRO_INT_EXTI PB5 +#else #define GYRO_INT_EXTI PB3 +#endif #define USE_MPU_DATA_READY_SIGNAL // *************** Baro ***************************** @@ -93,23 +102,40 @@ #define USE_VCP #define USE_UART1 +#ifdef FLYWOOF411_V2 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 +#else #define UART1_TX_PIN PB6 #define UART1_RX_PIN PB7 +#endif #define USE_UART2 +#ifdef FLYWOOF411_V2 +#define UART2_TX_PIN PA2 +#else #define UART2_TX_PIN NONE //PA2 +#endif #define UART2_RX_PIN PA3 #define USE_SOFTSERIAL1 +#ifdef FLYWOOF411_V2 +#define SOFTSERIAL_1_TX_PIN PB6 // Clash with TX2, possible to use as S.Port or VTX control +#define SOFTSERIAL_1_RX_PIN PB7 +#else #define SOFTSERIAL_1_TX_PIN PA2 // Clash with TX2, possible to use as S.Port or VTX control #define SOFTSERIAL_1_RX_PIN PA2 +#endif #define SERIAL_PORT_COUNT 4 // VCP, USART1, USART2, SS1 #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS +#ifdef FLYWOOF411_V2 +#define SERIALRX_UART SERIAL_PORT_USART1 +#else #define SERIALRX_UART SERIAL_PORT_USART2 - +#endif // *************** ADC ***************************** #define USE_ADC #define ADC_INSTANCE ADC1 @@ -123,8 +149,11 @@ // *************** LED2812 ************************ #define USE_LED_STRIP +#ifdef FLYWOOF411_V2 +#define WS2811_PIN PA0 +#else #define WS2811_PIN PA15 - +#endif // *************** OTHERS ************************* #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_OSD | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_SOFTSERIAL) @@ -138,4 +167,8 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) +#ifdef FLYWOOF411_V2 +#define MAX_PWM_OUTPUT_PORTS 6 +#else #define MAX_PWM_OUTPUT_PORTS 4 +#endif From 22772392154e398b4a8d07710e336453f0f03ae6 Mon Sep 17 00:00:00 2001 From: Daniel Arruda Ribeiro Date: Thu, 14 Jan 2021 21:18:11 -0300 Subject: [PATCH 085/103] Add a new MSP2_INAV_MISC2 message type, that allows a peer to fetch the On Time, Flight Time, Throttle percent and Auto Throttle flag. --- src/main/fc/fc_msp.c | 19 +++++++++++++++++++ src/main/msp/msp_protocol_v2_inav.h | 3 +++ 2 files changed, 22 insertions(+) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index d5ad2f3022..854c000d4f 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -824,6 +824,25 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, currentBatteryProfile->capacity.unit); break; + case MSP2_INAV_MISC2: + // Timers + sbufWriteU32(dst, micros() / 1000000); // On time (seconds) + sbufWriteU32(dst, getFlightTime()); // Flight time (seconds) + + // Throttle + int16_t thr = 0; + const int minThrottle = getThrottleIdleValue(); + bool autoThrottle = navigationIsControllingThrottle(); + if(autoThrottle) + thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); + else + thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX ) - minThrottle) * 100 / (motorConfig()->maxthrottle - minThrottle); + + sbufWriteU8(dst, thr); // Throttle Percent + sbufWriteU8(dst, autoThrottle ? 1 : 0); // Auto Throttle Flag (0 or 1) + + break; + case MSP2_INAV_BATTERY_CONFIG: sbufWriteU16(dst, batteryMetersConfig()->voltage.scale); sbufWriteU8(dst, batteryMetersConfig()->voltageSource); diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 219e770722..70e394ccb3 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -77,3 +77,6 @@ #define MSP2_INAV_SAFEHOME 0x2038 #define MSP2_INAV_SET_SAFEHOME 0x2039 + +#define MSP2_INAV_MISC2 0x203A + From 101799952e608afd061cfce44ef6ec96005ee3fb Mon Sep 17 00:00:00 2001 From: Daniel Arruda Ribeiro Date: Fri, 15 Jan 2021 18:12:45 -0300 Subject: [PATCH 086/103] Moved throttle percent calculation from MSP and OSD to Mixer --- src/main/fc/fc_msp.c | 12 ++---------- src/main/flight/mixer.c | 7 +++++++ src/main/flight/mixer.h | 1 + src/main/io/osd.c | 5 +---- 4 files changed, 11 insertions(+), 14 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 854c000d4f..0887564892 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -830,16 +830,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU32(dst, getFlightTime()); // Flight time (seconds) // Throttle - int16_t thr = 0; - const int minThrottle = getThrottleIdleValue(); - bool autoThrottle = navigationIsControllingThrottle(); - if(autoThrottle) - thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); - else - thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX ) - minThrottle) * 100 / (motorConfig()->maxthrottle - minThrottle); - - sbufWriteU8(dst, thr); // Throttle Percent - sbufWriteU8(dst, autoThrottle ? 1 : 0); // Auto Throttle Flag (0 or 1) + sbufWriteU8(dst, throttlePercent); // Throttle Percent + sbufWriteU8(dst, navigationIsControllingThrottle() ? 1 : 0); // Auto Throttle Flag (0 or 1) break; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index c3699ac1b0..9d6ed592b9 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -62,6 +62,7 @@ static float mixerScale = 1.0f; static EXTENDED_FASTRAM motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; static EXTENDED_FASTRAM uint8_t motorCount = 0; EXTENDED_FASTRAM int mixerThrottleCommand; +EXTENDED_FASTRAM int throttlePercent; static EXTENDED_FASTRAM int throttleIdleValue = 0; static EXTENDED_FASTRAM int motorValueWhenStopped = 0; static reversibleMotorsThrottleState_e reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD; @@ -529,6 +530,12 @@ void FAST_CODE mixTable(const float dT) throttleRange = throttleMax - throttleMin; + // Throttle Percent used by OSD and MSP + if(navigationIsControllingThrottle()) + throttlePercent = (constrain(mixerThrottleCommand, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); + else + throttlePercent = (constrain(mixerThrottleCommand, PWM_RANGE_MIN, PWM_RANGE_MAX) - throttleMin) * 100 / throttleRange; + #define THROTTLE_CLIPPING_FACTOR 0.33f motorMixRange = (float)rpyMixRange / (float)throttleRange; if (motorMixRange > 1.0f) { diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 405c876aa9..14d2d6c6d4 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -111,6 +111,7 @@ typedef enum { extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; extern int mixerThrottleCommand; +extern int throttlePercent; int getThrottleIdleValue(void); uint8_t getMotorCount(void); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 15bdb87f12..7c4a7aeac2 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -869,18 +869,15 @@ void osdCrosshairPosition(uint8_t *x, uint8_t *y) **/ static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t *elemAttr) { - const int minThrottle = getThrottleIdleValue(); buff[0] = SYM_BLANK; buff[1] = SYM_THR; - int16_t thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX ) - minThrottle) * 100 / (motorConfig()->maxthrottle - minThrottle); if (autoThr && navigationIsControllingThrottle()) { buff[0] = SYM_AUTO_THR0; buff[1] = SYM_AUTO_THR1; - thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); if (isFixedWingAutoThrottleManuallyIncreased()) TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr); } - tfp_sprintf(buff + 2, "%3d", thr); + tfp_sprintf(buff + 2, "%3d", throttlePercent); } /** From b60e7337a99e592e718935634b7091d272fbdb76 Mon Sep 17 00:00:00 2001 From: Adilson Oliveira Date: Sat, 23 Jan 2021 12:33:54 -0300 Subject: [PATCH 087/103] Update Building in Linux.md (#6526) --- docs/development/Building in Linux.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/development/Building in Linux.md b/docs/development/Building in Linux.md index 0c3a1aa83a..73287a216b 100644 --- a/docs/development/Building in Linux.md +++ b/docs/development/Building in Linux.md @@ -69,7 +69,7 @@ For 2.6 and later, inav uses `cmake` as its primary build tool. `cmake` simplies ## Using `cmake` -The canonanical method of using `cmake` is to create a `build` directory and run the `cmake` and `make` commands from within the `build` directory. So, assuming we've cloned the firmware repository into an `inav` directory, we can issue the following commands to set up the build environment. +The canonical method of using `cmake` is to create a `build` directory and run the `cmake` and `make` commands from within the `build` directory. So, assuming we've cloned the firmware repository into an `inav` directory, we can issue the following commands to set up the build environment. ``` cd inav From 331afab90712e98fbd0279e30624291a0b01e017 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Fri, 29 Jan 2021 20:59:44 +0000 Subject: [PATCH 088/103] Update Buzzer.md Added link to tool that can be used to listen to the buzzer sequences. There is also a custom sequence box at the bottom of the tool, which may aid creating new sequences. --- docs/Buzzer.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docs/Buzzer.md b/docs/Buzzer.md index 580c58ade1..62e2c8527e 100644 --- a/docs/Buzzer.md +++ b/docs/Buzzer.md @@ -43,6 +43,8 @@ Sequences: 14 DISARM_REPEAT 0, 100, 10 Stick held in disarm position (after pause) 15 ARMED 0, 245, 10, 5 Board is armed (after pause ; repeats until board is disarmed or throttle is increased) +You can use [this tool](https://www.mrd-rc.com/tutorials-tools-and-testing/useful-tools/helpful-inav-buzzer-code-checker/) to hear current buzzer sequences or enter custom sequences. + ## Controlling buzzer usage The usage of the buzzer can be controlled by the CLI `beeper` command. From fa660aaacd490e56cda5907e1d4e6319ec064e31 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Sun, 31 Jan 2021 16:45:03 +0000 Subject: [PATCH 089/103] build: Use -Os for F7 targets with flash <= 512K As requested by @dzikuvx ;-) --- cmake/stm32f7.cmake | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/cmake/stm32f7.cmake b/cmake/stm32f7.cmake index 51d6786a26..238920bcf8 100644 --- a/cmake/stm32f7.cmake +++ b/cmake/stm32f7.cmake @@ -97,8 +97,6 @@ function(target_stm32f7xx) VCP_SOURCES ${STM32F7_USB_SRC} ${STM32F7_VCP_SRC} VCP_INCLUDE_DIRECTORIES ${STM32F7_USB_INCLUDE_DIRS} ${STM32F7_VCP_DIR} - OPTIMIZATION -O2 - OPENOCD_TARGET stm32f7x BOOTLOADER @@ -112,6 +110,11 @@ macro(define_target_stm32f7 subfamily size) set(func_ARGV ARGV) string(TOUPPER ${size} upper_size) get_stm32_flash_size(flash_size ${size}) + if(flash_size GREATER 512) + set(opt -O2) + else() + set(opt -Os) + endif() set(definitions STM32F7 STM32F7${subfamily}xx @@ -123,6 +126,8 @@ macro(define_target_stm32f7 subfamily size) STARTUP startup_stm32f7${subfamily}xx.s COMPILE_DEFINITIONS ${definitions} LINKER_SCRIPT stm32_flash_f7${subfamily}x${size} + OPTIMIZATION ${opt} + ${${func_ARGV}} ) endfunction() From 7447beb25dd693c83796c061c1820e430e3e48a7 Mon Sep 17 00:00:00 2001 From: Daniel Arruda Ribeiro Date: Mon, 1 Feb 2021 10:54:06 -0300 Subject: [PATCH 090/103] Fix Throttle percent logic and code cleanup --- src/main/fc/fc_msp.c | 2 +- src/main/flight/mixer.c | 12 ++++++------ src/main/flight/mixer.h | 2 +- src/main/io/osd.c | 2 +- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 0887564892..a32961daba 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -830,7 +830,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU32(dst, getFlightTime()); // Flight time (seconds) // Throttle - sbufWriteU8(dst, throttlePercent); // Throttle Percent + sbufWriteU8(dst, getThrottlePercent()); // Throttle Percent sbufWriteU8(dst, navigationIsControllingThrottle() ? 1 : 0); // Auto Throttle Flag (0 or 1) break; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 9d6ed592b9..a07e4f9aa4 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -530,12 +530,6 @@ void FAST_CODE mixTable(const float dT) throttleRange = throttleMax - throttleMin; - // Throttle Percent used by OSD and MSP - if(navigationIsControllingThrottle()) - throttlePercent = (constrain(mixerThrottleCommand, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); - else - throttlePercent = (constrain(mixerThrottleCommand, PWM_RANGE_MIN, PWM_RANGE_MAX) - throttleMin) * 100 / throttleRange; - #define THROTTLE_CLIPPING_FACTOR 0.33f motorMixRange = (float)rpyMixRange / (float)throttleRange; if (motorMixRange > 1.0f) { @@ -579,6 +573,12 @@ void FAST_CODE mixTable(const float dT) motorRateLimitingApplyFn(dT); } +int16_t getThrottlePercent(void) +{ + int16_t thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX ) - getThrottleIdleValue()) * 100 / (motorConfig()->maxthrottle - getThrottleIdleValue()); + return thr; +} + motorStatus_e getMotorStatus(void) { if (failsafeRequiresMotorStop()) { diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 14d2d6c6d4..dfe1abedb5 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -111,9 +111,9 @@ typedef enum { extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; extern int mixerThrottleCommand; -extern int throttlePercent; int getThrottleIdleValue(void); +int16_t getThrottlePercent(void); uint8_t getMotorCount(void); float getMotorMixRange(void); bool mixerIsOutputSaturated(void); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 7c4a7aeac2..c637c45a1b 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -877,7 +877,7 @@ static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t if (isFixedWingAutoThrottleManuallyIncreased()) TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr); } - tfp_sprintf(buff + 2, "%3d", throttlePercent); + tfp_sprintf(buff + 2, "%3d", getThrottlePercent()); } /** From 8a8efbcbeebd3156a0f4a095c57ce53ad4634744 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 1 Feb 2021 14:58:53 +0100 Subject: [PATCH 091/103] Basic support for Rush Blade F7 --- src/main/target/RUSH_BLADE_F7/CMakeLists.txt | 1 + src/main/target/RUSH_BLADE_F7/target.c | 37 +++++ src/main/target/RUSH_BLADE_F7/target.h | 147 +++++++++++++++++++ 3 files changed, 185 insertions(+) create mode 100644 src/main/target/RUSH_BLADE_F7/CMakeLists.txt create mode 100644 src/main/target/RUSH_BLADE_F7/target.c create mode 100644 src/main/target/RUSH_BLADE_F7/target.h diff --git a/src/main/target/RUSH_BLADE_F7/CMakeLists.txt b/src/main/target/RUSH_BLADE_F7/CMakeLists.txt new file mode 100644 index 0000000000..5dd7f2d2e1 --- /dev/null +++ b/src/main/target/RUSH_BLADE_F7/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(RUSH_BLADE_F7) \ No newline at end of file diff --git a/src/main/target/RUSH_BLADE_F7/target.c b/src/main/target/RUSH_BLADE_F7/target.c new file mode 100644 index 0000000000..c56197a99e --- /dev/null +++ b/src/main/target/RUSH_BLADE_F7/target.c @@ -0,0 +1,37 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include "platform.h" +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +const timerHardware_t timerHardware[] = { + + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S1 UP2-1 D(2, 4, 7) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2 UP2-1 D(2, 7, 7) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S6 UP1-2 D(1, 2, 5) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S5 UP1-2 D(1, 7, 5) + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 D(2, 3, 7) + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 D(1, 4, 5) + + DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/RUSH_BLADE_F7/target.h b/src/main/target/RUSH_BLADE_F7/target.h new file mode 100644 index 0000000000..ca0175f10d --- /dev/null +++ b/src/main/target/RUSH_BLADE_F7/target.h @@ -0,0 +1,147 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it 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. + * + * INAV 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 INAV. If not, see . + */ + + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "RBF7" +#define USBD_PRODUCT_STRING "RUSH_BLADE_F7" + +#define LED0 PB10 //Blue SWCLK +// #define LED1 PA13 //Green SWDIO + +#define BEEPER PB2 +#define BEEPER_INVERTED + +// *************** SPI1 Gyro & ACC ******************* +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_EXTI +#define USE_MPU_DATA_READY_SIGNAL +#define GYRO_INT_EXTI PA4 + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW270_DEG +#define MPU6000_CS_PIN PC4 +#define MPU6000_SPI_BUS BUS_SPI1 + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_AK8975 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define TEMPERATURE_I2C_BUS BUS_I2C1 + +#define PITOT_I2C_BUS BUS_I2C1 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 + +// *************** SPI2 Flash *********************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI2 +#define M25P16_CS_PIN PB12 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** UART ***************************** +#define USE_VCP +#define USB_DETECT_PIN PC14 +#define USE_USB_DETECT + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PC10 +#define UART3_RX_PIN PC11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define SERIAL_PORT_COUNT 6 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 + +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL) +#define CURRENT_METER_SCALE 179 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#define MAX_PWM_OUTPUT_PORTS 10 +#define USE_OSD +#define USE_DSHOT +#define USE_SERIALSHOT +#define USE_ESC_SENSOR From 0586f595b24519ce0afb7269c2c0a6f85427051e Mon Sep 17 00:00:00 2001 From: Daniel Arruda Ribeiro Date: Mon, 1 Feb 2021 11:05:09 -0300 Subject: [PATCH 092/103] Remove unused variable --- src/main/flight/mixer.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a07e4f9aa4..fd0b83a468 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -62,7 +62,6 @@ static float mixerScale = 1.0f; static EXTENDED_FASTRAM motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; static EXTENDED_FASTRAM uint8_t motorCount = 0; EXTENDED_FASTRAM int mixerThrottleCommand; -EXTENDED_FASTRAM int throttlePercent; static EXTENDED_FASTRAM int throttleIdleValue = 0; static EXTENDED_FASTRAM int motorValueWhenStopped = 0; static reversibleMotorsThrottleState_e reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD; From 4c5dc00668b27450723aa31c7b97acc1a1195d93 Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Tue, 2 Feb 2021 14:24:04 +0000 Subject: [PATCH 093/103] add missed V2 ADC pin changes (#6555) --- src/main/target/FLYWOOF411/target.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/target/FLYWOOF411/target.h b/src/main/target/FLYWOOF411/target.h index 57d2347660..675513def2 100644 --- a/src/main/target/FLYWOOF411/target.h +++ b/src/main/target/FLYWOOF411/target.h @@ -140,8 +140,13 @@ #define USE_ADC #define ADC_INSTANCE ADC1 #define ADC_CHANNEL_1_PIN PA1 +#ifdef FLYWOOF411_V2 +#define ADC_CHANNEL_2_PIN PB1 +#define ADC_CHANNEL_3_PIN PB0 +#else #define ADC_CHANNEL_2_PIN PA0 #define ADC_CHANNEL_3_PIN PB1 +#endif #define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 #define VBAT_ADC_CHANNEL ADC_CHN_2 From b322940dd78f542864a0d091b5d9420d35c5dd18 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Tue, 2 Feb 2021 18:17:28 +0100 Subject: [PATCH 094/103] AFATFS: fix freadSync (#6550) --- src/main/io/asyncfatfs/asyncfatfs.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/io/asyncfatfs/asyncfatfs.c b/src/main/io/asyncfatfs/asyncfatfs.c index 0d84a756ca..f4f8af6d33 100644 --- a/src/main/io/asyncfatfs/asyncfatfs.c +++ b/src/main/io/asyncfatfs/asyncfatfs.c @@ -3252,6 +3252,7 @@ uint32_t afatfs_freadSync(afatfsFilePtr_t file, uint8_t *buffer, uint32_t length uint32_t leftToRead = length - bytesRead; uint32_t readNow = afatfs_fread(file, buffer, leftToRead); bytesRead += readNow; + buffer += readNow; if (bytesRead < length) { if (afatfs_feof(file)) { From c22218c701562657321bb07793e450541563321b Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Wed, 3 Feb 2021 20:36:39 +0200 Subject: [PATCH 095/103] Update vtx_control.c added smartAudioEarlyAkkWorkaroundEnable initialization --- src/main/io/vtx_control.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/io/vtx_control.c b/src/main/io/vtx_control.c index 6e213d7557..f80b992c29 100644 --- a/src/main/io/vtx_control.c +++ b/src/main/io/vtx_control.c @@ -39,10 +39,11 @@ #if defined(USE_VTX_CONTROL) -PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 2); +PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 3); PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig, .halfDuplex = true, + .smartAudioEarlyAkkWorkaroundEnable = true, ); static uint8_t locked = 0; From fdc8db6a9165e33f5fb987bec148a194adb4cefd Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 5 Jan 2021 12:23:48 +0100 Subject: [PATCH 096/103] Extract navigation PID controller to separate module --- src/main/CMakeLists.txt | 2 + src/main/common/fp_pid.c | 156 +++++++++++++++++++++++ src/main/common/fp_pid.h | 75 +++++++++++ src/main/fc/cli.c | 1 + src/main/flight/pid.c | 1 + src/main/navigation/navigation.c | 126 ------------------ src/main/navigation/navigation.h | 28 +--- src/main/navigation/navigation_private.h | 22 ---- src/main/programming/pid.h | 2 +- 9 files changed, 237 insertions(+), 176 deletions(-) create mode 100644 src/main/common/fp_pid.c create mode 100644 src/main/common/fp_pid.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 364356342f..a80fdd9feb 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -24,6 +24,8 @@ main_sources(COMMON_SRC common/encoding.h common/filter.c common/filter.h + common/fp_pid.c + common/fp_pid.h common/gps_conversion.c common/gps_conversion.h common/log.c diff --git a/src/main/common/fp_pid.c b/src/main/common/fp_pid.c new file mode 100644 index 0000000000..753789c9dc --- /dev/null +++ b/src/main/common/fp_pid.c @@ -0,0 +1,156 @@ +/* + * 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_SPEED + +#include +#include "common/fp_pid.h" + +/*----------------------------------------------------------- + * 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); +} \ No newline at end of file diff --git a/src/main/common/fp_pid.h b/src/main/common/fp_pid.h new file mode 100644 index 0000000000..0698c94ddb --- /dev/null +++ b/src/main/common/fp_pid.h @@ -0,0 +1,75 @@ +/* + * 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 + +#include "common/filter.h" +#include "common/maths.h" + +typedef struct { + float kP; + float kI; + float kD; + float kT; // Tracking gain (anti-windup) + float kFF; // FeedForward Component +} pidControllerParam_t; + +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 { + bool reset; + pidControllerParam_t param; + pt1Filter_t dterm_filter_state; // last derivative for low-pass filter + float dTermLpfHz; // dTerm low pass filter cutoff frequency + float integrator; // integrator value + float last_input; // last input for derivative + + float integral; // used integral value in output + float proportional; // used proportional value in output + float derivative; // used derivative value in output + float feedForward; // used FeedForward value in output + float output_constrained; // controller output constrained +} pidController_t; + +float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags); +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 +); +void navPidReset(pidController_t *pid); +void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz); \ No newline at end of file diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 3555f463c5..3b04aa4dc6 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -41,6 +41,7 @@ uint8_t cliMode = 0; #include "common/memory.h" #include "common/time.h" #include "common/typeconversion.h" +#include "common/fp_pid.h" #include "programming/global_variables.h" #include "programming/pid.h" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 5e5f9d3b5a..fc1db03370 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -30,6 +30,7 @@ FILE_COMPILE_FOR_SPEED #include "common/filter.h" #include "common/maths.h" #include "common/utils.h" +#include "common/fp_pid.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index b00dbbc050..e61811104c 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1913,132 +1913,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 *-----------------------------------------------------------*/ diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 5dcd8c71ca..50ba6b2632 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -23,6 +23,7 @@ #include "common/filter.h" #include "common/maths.h" #include "common/vector.h" +#include "common/fp_pid.h" #include "config/feature.h" @@ -324,33 +325,6 @@ typedef struct navDestinationPath_s { int32_t bearing; // deg * 100 } navDestinationPath_t; -typedef struct { - float kP; - float kI; - float kD; - float kT; // Tracking gain (anti-windup) - float kFF; // FeedForward Component -} pidControllerParam_t; - -typedef struct { - float kP; -} pControllerParam_t; - -typedef struct { - bool reset; - pidControllerParam_t param; - pt1Filter_t dterm_filter_state; // last derivative for low-pass filter - float dTermLpfHz; // dTerm low pass filter cutoff frequency - float integrator; // integrator value - float last_input; // last input for derivative - - float integral; // used integral value in output - float proportional; // used proportional value in output - float derivative; // used derivative value in output - float feedForward; // used FeedForward value in output - float output_constrained; // controller output constrained -} pidController_t; - typedef struct navigationPIDControllers_s { /* Multicopter PIDs */ pidController_t pos[XYZ_AXIS_COUNT]; diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 95a67e3917..6e7c4d3c0c 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -91,13 +91,6 @@ typedef struct navigationFlags_s { bool forcedRTHActivated; } navigationFlags_t; -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 { fpVector3_t pos; fpVector3_t vel; @@ -392,21 +385,6 @@ extern multicopterPosXyCoefficients_t multicopterPosXyCoefficients; /* Internally used functions */ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void); -float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags); -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 -); -void navPidReset(pidController_t *pid); -void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz); - bool isThrustFacingDownwards(void); uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos); int32_t calculateBearingToDestination(const fpVector3_t * destinationPos); diff --git a/src/main/programming/pid.h b/src/main/programming/pid.h index f120f9f073..8645de8f13 100644 --- a/src/main/programming/pid.h +++ b/src/main/programming/pid.h @@ -26,11 +26,11 @@ #include "config/parameter_group.h" #include "common/time.h" +#include "common/fp_pid.h" #include "programming/logic_condition.h" #include "common/axis.h" #include "flight/pid.h" -#include "navigation/navigation.h" #define MAX_PROGRAMMING_PID_COUNT 4 From 9a579ea4b4fcbc55678e955783d908c685fb6732 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 4 Feb 2021 10:00:03 +0100 Subject: [PATCH 097/103] Ability to trim pitch angle for level flight --- docs/Settings.md | 1 + src/main/fc/settings.yaml | 6 ++++++ src/main/flight/pid.c | 24 +++++++++++++++++++++++- src/main/flight/pid.h | 2 ++ 4 files changed, 32 insertions(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 547e03cacd..3c64f833d3 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -120,6 +120,7 @@ | fw_i_yaw | 10 | Fixed-wing rate stabilisation I-gain for YAW | | fw_iterm_limit_stick_position | 0.5 | Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side | | fw_iterm_throw_limit | 165 | Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely. | +| fw_level_pitch_trim | 0 | Pitch trim for self-leveling flight modes. In degrees. +5 means airplane nose should be raised 5 deg from level | | fw_loiter_direction | RIGHT | Direction of loitering: center point on right wing (clockwise - default), or center point on left wing (counterclockwise). If equal YAW then can be changed in flight using a yaw stick. | | fw_min_throttle_down_pitch | 0 | Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees) | | fw_p_level | 20 | Fixed-wing attitude stabilisation P-gain | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c3e286d37e..0e2b51d4fd 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1874,6 +1874,12 @@ groups: condition: USE_GYRO_KALMAN min: 1 max: 16000 + - name: fw_level_pitch_trim + description: "Pitch trim for self-leveling flight modes. In degrees. +5 means airplane nose should be raised 5 deg from level" + default_value: "0" + field: fixedWingLevelTrim + min: -10 + max: 10 - name: PG_PID_AUTOTUNE_CONFIG type: pidAutotuneConfig_t diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index fc1db03370..be792baa6a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -154,8 +154,9 @@ static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn; static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn; static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn; static EXTENDED_FASTRAM bool levelingEnabled = false; +static EXTENDED_FASTRAM float fixedWingLevelTrim; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 0); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 1); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -280,6 +281,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .kalman_w = 4, .kalman_sharpness = 100, .kalmanEnabled = 0, + .fixedWingLevelTrim = 0, ); bool pidInitFilters(void) @@ -534,6 +536,24 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle); + + //PITCH trim applied by a AutoLevel flight mode and manual pitch trimming + if (axis == FD_PITCH && STATE(AIRPLANE)) { + /* + * fixedWingLevelTrim has opposite sign to rcCommand. + * Positive rcCommand means nose should point downwards + * Negative rcCommand mean nose should point upwards + * This is counter intuitive and a natural way suggests that + should mean UP + * This is why fixedWingLevelTrim has opposite sign to rcCommand + * Positive fixedWingLevelTrim means nose should point upwards + * Negative fixedWingLevelTrim means nose should point downwards + */ + DEBUG_SET(DEBUG_ALWAYS, 0, fixedWingLevelTrim); + DEBUG_SET(DEBUG_ALWAYS, 1, angleTarget); + angleTarget -= DEGREES_TO_DECIDEGREES(fixedWingLevelTrim); + DEBUG_SET(DEBUG_ALWAYS, 2, angleTarget); + } + const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]); float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f); @@ -1108,6 +1128,8 @@ void pidInit(void) gyroKalmanInitialize(pidProfile()->kalman_q, pidProfile()->kalman_w, pidProfile()->kalman_sharpness); } #endif + + fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim; } const pidBank_t * pidBank(void) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 7f83accab6..83b7b6380c 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -150,6 +150,8 @@ typedef struct pidProfile_s { uint16_t kalman_w; uint16_t kalman_sharpness; uint8_t kalmanEnabled; + + float fixedWingLevelTrim; } pidProfile_t; typedef struct pidAutotuneConfig_s { From def19bc8ce4a32ad37d0687970f100569e5a9bb8 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 5 Feb 2021 09:21:33 +0100 Subject: [PATCH 098/103] Remove unused debug --- src/main/flight/pid.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index be792baa6a..23fde837b7 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -548,10 +548,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h * Positive fixedWingLevelTrim means nose should point upwards * Negative fixedWingLevelTrim means nose should point downwards */ - DEBUG_SET(DEBUG_ALWAYS, 0, fixedWingLevelTrim); - DEBUG_SET(DEBUG_ALWAYS, 1, angleTarget); angleTarget -= DEGREES_TO_DECIDEGREES(fixedWingLevelTrim); - DEBUG_SET(DEBUG_ALWAYS, 2, angleTarget); } const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]); From 08c9ed2d6ddeefd81c72ea76d2e8443196eb814f Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sat, 6 Feb 2021 10:34:13 +0000 Subject: [PATCH 099/103] Removed mc_ from airmode CLI variables I have removed mc_ from the airmode types in the CLI. Airmode applies to both fixed wing and multicoptors, so the mc_ prefix makes no sense. This should be mentioned in the release notes. I will now create a PR for the configurator side, to reflect these changes. --- docs/Settings.md | 4 ++-- src/main/fc/settings.yaml | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 57d89f3664..3c95f19859 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -20,6 +20,8 @@ | acczero_x | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. | | acczero_y | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. | | acczero_z | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. | +| airmode_throttle_threshold | 1300 | Defines airmode THROTTLE activation threshold when `airmode_type` **THROTTLE_THRESHOLD** is used | +| airmode_type | STICK_CENTER | Defines the Airmode state handling type. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `airmode_throttle_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER_ONCE** or **STICK_CENTER** modes. | | airspeed_adc_channel | 0 | ADC channel to use for analog pitot tube (airspeed) sensor. If board doesn't have a dedicated connector for analog airspeed sensor will default to 0 | | align_acc | DEFAULT | When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. | | align_board_pitch | 0 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | @@ -217,8 +219,6 @@ | max_angle_inclination_rll | 300 | Maximum inclination in level (angle) mode (ROLL axis). 100=10° | | max_check | 1900 | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. | | max_throttle | 1850 | This is the maximum value (in us) sent to esc when armed. Default of 1850 are OK for everyone (legacy). For modern ESCs, higher values (c. 2000) may be more appropriate. If you have brushed motors, the value should be set to 2000. | -| mc_airmode_threshold | 1300 | Defines airmode THROTTLE activation threshold when `mc_airmode_type` **THROTTLE_THRESHOLD** is used | -| mc_airmode_type | STICK_CENTER | Defines the Airmode state handling type for Multirotors. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `mc_airmode_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER** mode. | | mc_cd_lpf_hz | 30 | Cutoff frequency for Control Derivative. Lower value smoother reaction on fast stick movements. With higher values, response will be more aggressive, jerky | | mc_cd_pitch | 60 | Multicopter Control Derivative gain for PITCH | | mc_cd_roll | 60 | Multicopter Control Derivative gain for ROLL | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index ee5aaa0fe2..8ba40bdac9 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1342,13 +1342,13 @@ groups: field: mid_throttle_deadband min: 0 max: 200 - - name: mc_airmode_type - description: "Defines the Airmode state handling type for Multirotors. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `mc_airmode_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER** mode." + - name: airmode_type + description: "Defines the Airmode state handling type. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `airmode_throttle_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER_ONCE** or **STICK_CENTER** modes." default_value: "STICK_CENTER" field: airmodeHandlingType table: airmodeHandlingType - - name: mc_airmode_threshold - description: "Defines airmode THROTTLE activation threshold when `mc_airmode_type` **THROTTLE_THRESHOLD** is used" + - name: airmode_throttle_threshold + description: "Defines airmode THROTTLE activation threshold when `airmode_type` **THROTTLE_THRESHOLD** is used" default_value: "1300" field: airmodeThrottleThreshold min: 1000 From 9130152fab4e264a58a77dfb1fc91bec993dfabe Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sun, 7 Feb 2021 10:55:31 +0000 Subject: [PATCH 100/103] Increased fw loiter radius default Changed the default loiter radius, as have seen a lot of issues with loiter recently. The larger radius will give more of a buffer for larger models and people who have forgotten to/haven't tuned the model correctly. --- src/main/navigation/navigation.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index e61811104c..01e83e70b7 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -159,7 +159,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 From 02b3cc907dbc63fdac4690d9ffdb41e58c02750d Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Sun, 7 Feb 2021 17:04:41 +0000 Subject: [PATCH 101/103] update settings.yaml / settings.md for #6581 (#6582) --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 3c95f19859..e0a8c52242 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -278,7 +278,7 @@ | nav_fw_launch_thr | 1700 | Launch throttle - throttle to be set during launch sequence (pwm units) | | nav_fw_launch_timeout | 5000 | Maximum time for launch sequence to be executed. After this time LAUNCH mode will be turned off and regular flight mode will take over (ms) | | nav_fw_launch_velocity | 300 | Forward velocity threshold for swing-launch detection [cm/s] | -| nav_fw_loiter_radius | 5000 | PosHold radius. 3000 to 7500 is a good value (30-75m) [cm] | +| nav_fw_loiter_radius | 7500 | PosHold radius. 3000 to 7500 is a good value (30-75m) [cm] | | nav_fw_max_thr | 1700 | Maximum throttle for flying wing in GPS assisted modes | | nav_fw_min_thr | 1200 | Minimum throttle for flying wing in GPS assisted modes | | nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 8ba40bdac9..a49d69ff33 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2335,7 +2335,7 @@ groups: max: 900 - name: nav_fw_loiter_radius description: "PosHold radius. 3000 to 7500 is a good value (30-75m) [cm]" - default_value: "5000" + default_value: "7500" field: fw.loiter_radius min: 0 max: 10000 From c2e9dc9cd861e5d8ae605c6fbf5c4f2e591ee196 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 10 Feb 2021 10:49:38 +0100 Subject: [PATCH 102/103] Skip release for Rush Blade F722 --- src/main/target/RUSH_BLADE_F7/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/RUSH_BLADE_F7/CMakeLists.txt b/src/main/target/RUSH_BLADE_F7/CMakeLists.txt index 5dd7f2d2e1..5d612d6c68 100644 --- a/src/main/target/RUSH_BLADE_F7/CMakeLists.txt +++ b/src/main/target/RUSH_BLADE_F7/CMakeLists.txt @@ -1 +1 @@ -target_stm32f722xe(RUSH_BLADE_F7) \ No newline at end of file +target_stm32f722xe(RUSH_BLADE_F7 SKIP_RELEASES) \ No newline at end of file From 730e1a7354cdda5f2eefad40427e83181df4ff7a Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Wed, 10 Feb 2021 20:01:04 +0100 Subject: [PATCH 103/103] Enable baro median filtering by default again (#6584) It had been disabled by mistake in f0943875 --- src/main/sensors/barometer.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 3506e9361a..bb1325aeeb 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -56,7 +56,7 @@ baro_t baro; // barometer access functions -PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 2); +PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 3); #ifdef USE_BARO #define BARO_HARDWARE_DEFAULT BARO_AUTODETECT @@ -65,7 +65,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER #endif PG_RESET_TEMPLATE(barometerConfig_t, barometerConfig, .baro_hardware = BARO_HARDWARE_DEFAULT, - .use_median_filtering = 0, + .use_median_filtering = 1, .baro_calibration_tolerance = 150 );