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