Q-Tune

.gitignore

@@ -27,6 +27,8 @@ cov-int*
 /debug/
 /release/
 /*_SITL/
+.conda/
+.venv/
 
 # script-generated files
 docs/Manual.pdf

cmake/at32.cmake

@@ -18,6 +18,7 @@ set(CMSIS_DSP_SRC
     BasicMathFunctions/arm_scale_f32.c
     BasicMathFunctions/arm_sub_f32.c
     BasicMathFunctions/arm_mult_f32.c
+    BasicMathFunctions/arm_scale_f32.c
     BasicMathFunctions/arm_offset_f32.c
     TransformFunctions/arm_rfft_fast_f32.c
     TransformFunctions/arm_cfft_f32.c
@@ -27,9 +28,10 @@ set(CMSIS_DSP_SRC
     CommonTables/arm_common_tables.c
     ComplexMathFunctions/arm_cmplx_mag_f32.c
     StatisticsFunctions/arm_max_f32.c
+    StatisticsFunctions/arm_var_f32.c
     StatisticsFunctions/arm_rms_f32.c
-    StatisticsFunctions/arm_std_f32.c
     StatisticsFunctions/arm_mean_f32.c
+    StatisticsFunctions/arm_std_f32.c
 )
 list(TRANSFORM CMSIS_DSP_SRC PREPEND "${CMSIS_DSP_DIR}/Source/")
 

cmake/stm32.cmake

@@ -19,6 +19,7 @@ set(CMSIS_DSP_SRC
     BasicMathFunctions/arm_scale_f32.c
     BasicMathFunctions/arm_sub_f32.c
     BasicMathFunctions/arm_mult_f32.c
+    BasicMathFunctions/arm_scale_f32.c
     BasicMathFunctions/arm_offset_f32.c
     TransformFunctions/arm_rfft_fast_f32.c
     TransformFunctions/arm_cfft_f32.c
@@ -28,9 +29,10 @@ set(CMSIS_DSP_SRC
     CommonTables/arm_common_tables.c
     ComplexMathFunctions/arm_cmplx_mag_f32.c
     StatisticsFunctions/arm_max_f32.c
+    StatisticsFunctions/arm_var_f32.c
     StatisticsFunctions/arm_rms_f32.c
-    StatisticsFunctions/arm_std_f32.c
     StatisticsFunctions/arm_mean_f32.c
+    StatisticsFunctions/arm_std_f32.c
 )
 list(TRANSFORM CMSIS_DSP_SRC PREPEND "${CMSIS_DSP_DIR}/Source/")
 

dev/scratch.c

@@ -0,0 +1,50 @@
+...
+#include "arm_math.h"
+#define SAMPLES 128
+const int samplingFreq = 1000;
+...
+
+static void pdAdcTask(void* parameters) {
+
+  DEBUG_PRINT("FFT test function is running!");
+
+  //Configure delay
+  float delay = 500 / portTICK_PERIOD_MS;
+  DEBUG_PRINT("Delay in ticks is %f \n", delay);
+  const TickType_t xDelay =  delay;
+
+  //Some parameters
+  int peakFrequency;
+  int f = 150;
+  int A = 58;
+
+  while (true) {
+    	//Force Delay
+   	 vTaskDelay(xDelay);
+
+    	//Create input signal
+   	for(int i=0; i<SAMPLES; i++){
+   		mySine[i] = A*arm_sin_f32(2*PI*f*i/samplingFreq);
+   	}
+
+   	//Create RFFT instance
+   	arm_rfft_fast_instance_f32 S;
+    	arm_rfft_fast_init_f32(&S, SAMPLES);
+
+    	//RFFT transform
+    	arm_rfft_fast_f32(&S, mySine,rfft_output,0);
+    	//Calculate magnitude of imaginary coefficients
+    	arm_cmplx_mag_f32(rfft_output,test_output, SAMPLES/2);
+    	//Set DC component to 0
+    	test_output[0] = 0;
+    	//Obtain peak frequency
+    	arm_max_f32(test_output, SAMPLES/2, &maxvalue, &maxindex);
+
+    	peakFrequency = maxindex * samplingFreq / SAMPLES;
+
+    	DEBUG_PRINT("Peak frequency %d \n\r", peakFrequency);
+
+    	DEBUG_PRINT("Max Value:[%ld]:%f Output=[",maxindex,2*maxvalue/SAMPLES);
+    	DEBUG_PRINT("]\r\n");
+  }
+}

lib/main/CMSIS/DSP/Source/TransformFunctions/arm_rfft_fast_init_f32.c

@@ -50,7 +50,7 @@
 * \par
 * This Function also initializes Twiddle factor table pointer and Bit reversal table pointer.
 */
-arm_status arm_rfft_fast_init_f32(
+arm_status __attribute__ ( (noinline) ) arm_rfft_fast_init_f32(
   arm_rfft_fast_instance_f32 * S,
   uint16_t fftLen)
 {

src/main/CMakeLists.txt

@@ -327,6 +327,8 @@ main_sources(COMMON_SRC
     flight/smith_predictor.h
     flight/rate_dynamics.c
     flight/rate_dynamics.h
+    flight/q_tune.c
+    flight/q_tune.h
     flight/mixer.c
     flight/mixer.h
     flight/pid.c

src/main/build/debug.h

@@ -79,6 +79,7 @@ typedef enum {
     DEBUG_GPS,
     DEBUG_LULU,
     DEBUG_SBUS2,
+    DEBUG_Q_TUNE,
     DEBUG_COUNT // also update debugModeNames in cli.c
 } debugType_e;
 

src/main/fc/cli.c

@@ -219,7 +219,8 @@ static const char *debugModeNames[DEBUG_COUNT] = {
     "HEADTRACKER",
     "GPS",
     "LULU",
-    "SBUS2"
+    "SBUS2",
+    "Q_TUNE"
 };
 
 /* Sensor names (used in lookup tables for *_hardware settings and in status

src/main/fc/fc_tasks.c

@@ -53,6 +53,7 @@
 #include "flight/rpm_filter.h"
 #include "flight/servos.h"
 #include "flight/wind_estimator.h"
+#include "flight/q_tune.h"
 #include "flight/adaptive_filter.h"
 
 #include "navigation/navigation.h"
@@ -435,6 +436,9 @@ void fcTasksInit(void)
 #if defined(USE_SMARTPORT_MASTER)
     setTaskEnabled(TASK_SMARTPORT_MASTER, true);
 #endif
+#ifdef USE_Q_TUNE
+    setTaskEnabled(TASK_Q_TUNE, true);
+#endif
 
 #ifdef USE_SERIAL_GIMBAL
     setTaskEnabled(TASK_GIMBAL, true);
@@ -708,6 +712,14 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .desiredPeriod = TASK_PERIOD_HZ(RPM_FILTER_UPDATE_RATE_HZ),          // 300Hz @3,33ms
         .staticPriority = TASK_PRIORITY_LOW,
     },
+#endif
+#ifdef USE_Q_TUNE
+    [TASK_Q_TUNE] = {
+        .taskName = "QTUNE",
+        .taskFunc = qTuneProcessTask,
+        .desiredPeriod = TASK_PERIOD_HZ(Q_TUNE_UPDATE_RATE_HZ),          // 20Hz @50ms
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
 #endif
     [TASK_AUX] = {
         .taskName = "AUX",

src/main/fc/settings.yaml

@@ -84,7 +84,7 @@ tables:
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
       "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE",
       "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST",
-      "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2"]
+      "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2", "Q_TUNE"]
   - name: aux_operator
     values: ["OR", "AND"]
     enum: modeActivationOperator_e

src/main/flight/pid.c

@@ -47,6 +47,7 @@
 #include "flight/rpm_filter.h"
 #include "flight/kalman.h"
 #include "flight/smith_predictor.h"
+#include "flight/q_tune.h"
 #include "flight/adaptive_filter.h"
 
 #include "io/gps.h"
@@ -947,10 +948,13 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
         pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit);
     }
 
-
     // Don't grow I-term if motors are at their limit
     applyItermLimiting(pidState);
 
+#ifdef USE_Q_TUNE
+    qTunePushSample(pidState->axis, pidState->rateTarget, pidState->gyroRate, pidState->errorGyroIf);
+#endif
+
     axisPID[pidState->axis] = newOutputLimited;
 
 #ifdef USE_BLACKBOX