diff --git a/make/mcu/STM32F7.mk b/make/mcu/STM32F7.mk
index cc578eea08..ade245f038 100644
--- a/make/mcu/STM32F7.mk
+++ b/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 \
diff --git a/make/source.mk b/make/source.mk
index 185dd53390..94859cc404 100644
--- a/make/source.mk
+++ b/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 \
diff --git a/src/main/drivers/audio.h b/src/main/drivers/audio.h
new file mode 100644
index 0000000000..c63a198695
--- /dev/null
+++ b/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 .
+ */
+
+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);
diff --git a/src/main/drivers/audio_stm32f7xx.c b/src/main/drivers/audio_stm32f7xx.c
new file mode 100644
index 0000000000..43bd447a9a
--- /dev/null
+++ b/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 .
+ */
+
+#include
+#include
+#include
+
+#include
+
+#include
+
+#include
+
+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);
+}
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 4e44b668ee..7b656dc13e 100644
--- a/src/main/fc/fc_core.c
+++ b/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)
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 3d60371fb1..9e204e68db 100644
--- a/src/main/fc/fc_init.c
+++ b/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();
diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c
index d1342b2444..87b192e294 100644
--- a/src/main/fc/rc_adjustments.c
+++ b/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) {
diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h
index c8d9ae5e5f..d127c78217 100644
--- a/src/main/fc/rc_adjustments.h
+++ b/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;
diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h
index b82d91ea1a..35b6c392fa 100644
--- a/src/main/fc/rc_modes.h
+++ b/src/main/fc/rc_modes.h
@@ -72,6 +72,7 @@ typedef enum {
BOXUSER2,
BOXUSER3,
BOXUSER4,
+ BOXPIDAUDIO,
CHECKBOX_ITEM_COUNT
} boxId_e;
diff --git a/src/main/interface/msp_box.c b/src/main/interface/msp_box.c
index 7e6dcd68e9..8851af2c43 100644
--- a/src/main/interface/msp_box.c
+++ b/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++)
diff --git a/src/main/io/pidaudio.c b/src/main/io/pidaudio.c
new file mode 100644
index 0000000000..9f3dfb0b65
--- /dev/null
+++ b/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 .
+ */
+
+#include
+#include
+#include
+
+#include
+
+#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);
+}
diff --git a/src/main/io/pidaudio.h b/src/main/io/pidaudio.h
new file mode 100644
index 0000000000..8d3821d261
--- /dev/null
+++ b/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 .
+ */
+
+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);
+
diff --git a/src/main/target/SPRACINGF7DUAL/target.h b/src/main/target/SPRACINGF7DUAL/target.h
index d6d1166b01..e532c6bb7c 100644
--- a/src/main/target/SPRACINGF7DUAL/target.h
+++ b/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