Merge pull request #3036 from cleanflight/spracingf7dual-pidaudio (#5586)

PID-Audio feature

make/mcu/STM32F7.mk

@@ -145,6 +145,7 @@ MCU_COMMON_SRC = \
             target/system_stm32f7xx.c \
             drivers/accgyro/accgyro_mpu.c \
             drivers/adc_stm32f7xx.c \
+            drivers/audio_stm32f7xx.c \
             drivers/bus_i2c_hal.c \
             drivers/dma_stm32f7xx.c \
             drivers/light_ws2811strip_hal.c \

make/source.mk

@@ -177,6 +177,7 @@ FC_SRC = \
             io/gps.c \
             io/ledstrip.c \
             io/osd.c \
+            io/pidaudio.c \
             sensors/barometer.c \
             sensors/rangefinder.c \
             telemetry/telemetry.c \

src/main/drivers/audio.h

@@ -0,0 +1,27 @@
+/*
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+void audioSetupIO(void);
+void audioGenerateWhiteNoise(void);
+
+#define TONE_COUNT 64
+#define TONE_MIN 0
+#define TONE_MAX (TONE_COUNT - 1)
+#define TONE_MID ((TONE_COUNT / 2) + TONE_MIN)
+void audioPlayTone(uint8_t tone); // TONE_MIN to TONE_MAX
+
+void audioSilence(void);

src/main/drivers/audio_stm32f7xx.c

@@ -0,0 +1,88 @@
+/*
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <math.h>
+
+#include <platform.h>
+
+#include <common/maths.h>
+
+#include <drivers/audio.h>
+
+static DAC_HandleTypeDef hdac;
+static TIM_HandleTypeDef handle;
+static DAC_ChannelConfTypeDef sConfig;
+
+void audioSetupIO(void)
+{
+    __HAL_RCC_DAC_CLK_ENABLE();
+    __HAL_RCC_TIM6_CLK_ENABLE();
+
+    hdac.Instance = DAC;
+    HAL_DAC_Init(&hdac);
+
+    GPIO_InitTypeDef GPIO_InitStruct;
+    GPIO_InitStruct.Pin = GPIO_PIN_4;
+    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+}
+
+void audioGenerateWhiteNoise(void)
+{
+    // TIM6 runs on APB2 at 42Mhz
+    handle.Instance = TIM6;
+    handle.Init.Period = 0xFF;
+    handle.Init.Prescaler = 0;
+
+    handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+    handle.Init.CounterMode = TIM_COUNTERMODE_UP;
+    handle.Init.RepetitionCounter = 0;
+    HAL_TIM_Base_Init(&handle);
+
+    TIM_MasterConfigTypeDef sMasterConfig;
+    sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_ENABLE;
+    sMasterConfig.MasterOutputTrigger = TIM_TRGO_UPDATE;
+    HAL_TIMEx_MasterConfigSynchronization(&handle, &sMasterConfig);
+
+    HAL_TIM_Base_Start(&handle);
+
+    sConfig.DAC_Trigger = DAC_TRIGGER_T6_TRGO;
+    sConfig.DAC_OutputBuffer = DAC_OUTPUTBUFFER_ENABLE;
+    HAL_DAC_ConfigChannel(&hdac, &sConfig, DAC_CHANNEL_1);
+
+    HAL_DACEx_NoiseWaveGenerate(&hdac, DAC_CHANNEL_1, DAC_LFSRUNMASK_BITS10_0);
+    HAL_DAC_SetValue(&hdac, DAC_CHANNEL_1, DAC_ALIGN_12B_L, 0xcd00);
+
+    HAL_DAC_Start(&hdac, DAC_CHANNEL_1);
+}
+
+#define TONE_SPREAD 8
+
+void audioPlayTone(uint8_t tone)
+{
+    handle.Init.Period = 64 + (MAX(TONE_MIN,MIN(tone, TONE_MAX)) * TONE_SPREAD);
+    TIM_Base_SetConfig(handle.Instance, &handle.Init);
+}
+
+void audioSilence(void)
+{
+    HAL_DAC_Stop(&hdac, DAC_CHANNEL_1);
+    HAL_TIM_Base_Stop(&handle);
+}

src/main/fc/fc_core.c

@@ -64,6 +64,7 @@
 #include "io/beeper.h"
 #include "io/gps.h"
 #include "io/motors.h"
+#include "io/pidaudio.h"
 #include "io/servos.h"
 #include "io/serial.h"
 #include "io/statusindicator.h"
@@ -825,6 +826,11 @@ static void subTaskPidController(timeUs_t currentTimeUs)
         DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE);
     }
 #endif
+
+
+#ifdef USE_PID_AUDIO
+    pidAudioUpdate();
+#endif
 }
 
 static void subTaskMainSubprocesses(timeUs_t currentTimeUs)

src/main/fc/fc_init.c

@@ -118,6 +118,7 @@
 #include "io/transponder_ir.h"
 #include "io/osd.h"
 #include "io/osd_slave.h"
+#include "io/pidaudio.h"
 #include "io/piniobox.h"
 #include "io/displayport_msp.h"
 #include "io/vtx.h"
@@ -539,6 +540,10 @@ void init(void)
     pidInit(currentPidProfile);
     accInitFilters();
 
+#ifdef USE_PID_AUDIO
+    pidAudioInit();
+#endif
+
 #ifdef USE_SERVOS
     servosInit();
     servoConfigureOutput();

src/main/fc/rc_adjustments.c

@@ -42,6 +42,7 @@
 
 #include "io/beeper.h"
 #include "io/motors.h"
+#include "io/pidaudio.h"
 
 #include "fc/config.h"
 #include "fc/controlrate_profile.h"
@@ -53,6 +54,21 @@
 
 PG_REGISTER_ARRAY(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges, PG_ADJUSTMENT_RANGE_CONFIG, 0);
 
+uint8_t pidAudioPositionToModeMap[7] = {
+    // on a pot with a center detent, it's easy to have center area for off/default, then three positions to the left and three to the right.
+    // current implementation yields RC values as below.
+
+    PID_AUDIO_PIDSUM_X,     //   900 - ~1071 - Min
+    PID_AUDIO_PIDSUM_Y,     // ~1071 - ~1242
+    PID_AUDIO_PIDSUM_XY,    // ~1242 - ~1414
+    PID_AUDIO_OFF,          // ~1414 - ~1585 - Center
+    PID_AUDIO_OFF,          // ~1585 - ~1757
+    PID_AUDIO_OFF,          // ~1757 - ~1928
+    PID_AUDIO_OFF,          // ~1928 -  2100 - Max
+
+    // Note: Last 3 positions are currently pending implementations and use PID_AUDIO_OFF for now.
+};
+
 static pidProfile_t *pidProfile;
 
 static void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue)
@@ -194,7 +210,10 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
         .adjustmentFunction = ADJUSTMENT_HORIZON_STRENGTH,
         .mode = ADJUSTMENT_MODE_SELECT,
         .data = { .switchPositions = 255 }
-
+    }, {
+        .adjustmentFunction = ADJUSTMENT_PID_AUDIO,
+        .mode = ADJUSTMENT_MODE_SELECT,
+        .data = { .switchPositions = ARRAYLEN(pidAudioPositionToModeMap) }
     }
 };
 
@@ -224,6 +243,7 @@ static const char * const adjustmentLabels[] = {
     "D SETPOINT",
     "D SETPOINT TRANSITION",
     "HORIZON STRENGTH",
+    "PID AUDIO",
 };
 
 const char *adjustmentRangeName;
@@ -420,6 +440,16 @@ static uint8_t applySelectAdjustment(uint8_t adjustmentFunction, uint8_t positio
             }
             break;
         }
+    case ADJUSTMENT_PID_AUDIO:
+        {
+#ifdef USE_PID_AUDIO
+            uint8_t newMode = pidAudioPositionToModeMap[position];
+            if (newMode != pidAudioGetMode()) {
+                pidAudioSetMode(newMode);
+            }
+#endif
+            break;
+        }
     }
 
     if (beeps) {

src/main/fc/rc_adjustments.h

@@ -51,6 +51,7 @@ typedef enum {
     ADJUSTMENT_PITCH_RC_RATE,
     ADJUSTMENT_ROLL_RC_EXPO,
     ADJUSTMENT_PITCH_RC_EXPO,
+    ADJUSTMENT_PID_AUDIO,
     ADJUSTMENT_FUNCTION_COUNT
 } adjustmentFunction_e;
 

src/main/fc/rc_modes.h

@@ -72,6 +72,7 @@ typedef enum {
     BOXUSER2,
     BOXUSER3,
     BOXUSER4,
+    BOXPIDAUDIO,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 

src/main/interface/msp_box.c

@@ -88,6 +88,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
     { BOXUSER2, "USER2", 41 },
     { BOXUSER3, "USER3", 42 },
     { BOXUSER4, "USER4", 43 },
+    { BOXPIDAUDIO, "PID AUDIO", 44 },
 };
 
 // mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index
@@ -287,6 +288,10 @@ void initActiveBoxIds(void)
     }
 #endif
 
+#if defined(USE_PID_AUDIO)
+    BME(BOXPIDAUDIO);
+#endif
+
 #undef BME
     // check that all enabled IDs are in boxes array (check may be skipped when using findBoxById() functions)
     for (boxId_e boxId = 0;  boxId < CHECKBOX_ITEM_COUNT; boxId++)

src/main/io/pidaudio.c

@@ -0,0 +1,107 @@
+/*
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <math.h>
+
+#include <platform.h>
+
+#include "common/axis.h"
+#include "common/maths.h"
+
+#include "drivers/audio.h"
+
+#include "fc/rc_modes.h"
+
+#include "flight/pid.h"
+
+#include "io/pidaudio.h"
+
+static bool pidAudioEnabled = false;
+
+static pidAudioModes_e pidAudioMode = PID_AUDIO_PIDSUM_XY;
+
+void pidAudioInit(void)
+{
+    audioSetupIO();
+}
+
+void pidAudioStart(void)
+{
+    audioGenerateWhiteNoise();
+}
+
+void pidAudioStop(void)
+{
+    audioSilence();
+}
+
+pidAudioModes_e pidAudioGetMode(void)
+{
+    return pidAudioMode;
+}
+
+void pidAudioSetMode(pidAudioModes_e mode)
+{
+    pidAudioMode = mode;
+}
+
+void pidAudioUpdate(void)
+{
+    bool newState = IS_RC_MODE_ACTIVE(BOXPIDAUDIO);
+
+    if (pidAudioEnabled != newState) {
+        if (newState) {
+            pidAudioStart();
+        } else {
+            pidAudioStop();
+        }
+        pidAudioEnabled = newState;
+    }
+
+    if (!pidAudioEnabled) {
+        return;
+    }
+
+    uint8_t tone = TONE_MID;
+
+    switch (pidAudioMode) {
+    case PID_AUDIO_PIDSUM_X:
+        {
+            const uint32_t pidSumX = MIN(ABS(axisPIDSum[FD_ROLL]), PIDSUM_LIMIT);
+            tone = scaleRange(pidSumX, 0, PIDSUM_LIMIT, TONE_MAX, TONE_MIN);
+            break;
+        }
+    case PID_AUDIO_PIDSUM_Y:
+        {
+            const uint32_t pidSumY = MIN(ABS(axisPIDSum[FD_PITCH]), PIDSUM_LIMIT);
+            tone = scaleRange(pidSumY, 0, PIDSUM_LIMIT, TONE_MAX, TONE_MIN);
+            break;
+        }
+    case PID_AUDIO_PIDSUM_XY:
+        {
+            const uint32_t pidSumXY = MIN((ABS(axisPIDSum[FD_ROLL]) + ABS(axisPIDSum[FD_PITCH])) / 2, PIDSUM_LIMIT);
+            tone = scaleRange(pidSumXY, 0, PIDSUM_LIMIT, TONE_MAX, TONE_MIN);
+            break;
+        }
+    default:
+        break;
+    }
+
+    audioPlayTone(tone);
+}

src/main/io/pidaudio.h

@@ -0,0 +1,29 @@
+/*
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+void pidAudioUpdate(void);
+void pidAudioInit(void);
+
+typedef enum {
+    PID_AUDIO_OFF = 0,
+    PID_AUDIO_PIDSUM_X,
+    PID_AUDIO_PIDSUM_Y,
+    PID_AUDIO_PIDSUM_XY,
+} pidAudioModes_e;
+pidAudioModes_e pidAudioGetMode(void);
+void pidAudioSetMode(pidAudioModes_e mode);
+

src/main/target/SPRACINGF7DUAL/target.h

@@ -195,6 +195,8 @@
 #define USE_OSD_OVER_MSP_DISPLAYPORT
 #define USE_MSP_CURRENT_METER
 
+#define USE_PID_AUDIO
+
 #define USE_LED_STRIP
 
 //TODO Implement transponder on F7