mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-22 07:45:29 +03:00
SPRacingF3Mini - First cut of transponder system.
Implementation is DMA buffer based, just like the WS2811 LED Strip code, so that there is minimal impact on flight performance. Currently the code is mutually exclusive with the LED_STRIP code and there is no means to set the transponder code.
This commit is contained in:
parent
360ea84a14
commit
6d32aa5d7b
11 changed files with 502 additions and 12 deletions
|
@ -553,6 +553,11 @@ static void resetConf(void)
|
|||
#ifdef SPRACINGF3
|
||||
featureSet(FEATURE_BLACKBOX);
|
||||
masterConfig.blackbox_device = 1;
|
||||
#ifdef TRANSPONDER
|
||||
static const uint8_t defaultTransponderData[6] = { 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC }; // Note, this is NOT a valid transponder code, it's just for testing production hardware
|
||||
|
||||
memcpy(masterConfig.transponderData, &defaultTransponderData, sizeof(defaultTransponderData));
|
||||
#endif
|
||||
|
||||
#if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT)
|
||||
featureSet(FEATURE_BLACKBOX);
|
||||
|
|
|
@ -93,6 +93,10 @@ typedef struct master_t {
|
|||
hsvColor_t colors[CONFIGURABLE_COLOR_COUNT];
|
||||
#endif
|
||||
|
||||
#ifdef TRANSPONDER
|
||||
uint8_t transponderData[6];
|
||||
#endif
|
||||
|
||||
profile_t profile[MAX_PROFILE_COUNT];
|
||||
uint8_t current_profile_index;
|
||||
controlRateConfig_t controlRateProfiles[MAX_CONTROL_RATE_PROFILE_COUNT];
|
||||
|
|
|
@ -8,6 +8,7 @@
|
|||
#define NVIC_PRIO_TIMER NVIC_BUILD_PRIORITY(1, 1)
|
||||
#define NVIC_PRIO_BARO_EXT NVIC_BUILD_PRIORITY(0x0f, 0x0f)
|
||||
#define NVIC_PRIO_WS2811_DMA NVIC_BUILD_PRIORITY(1, 2) // TODO - is there some reason to use high priority? (or to use DMA IRQ at all?)
|
||||
#define NVIC_PRIO_TRANSPONDER_DMA NVIC_BUILD_PRIORITY(3, 0)
|
||||
#define NVIC_PRIO_SERIALUART1_TXDMA NVIC_BUILD_PRIORITY(1, 1)
|
||||
#define NVIC_PRIO_SERIALUART1_RXDMA NVIC_BUILD_PRIORITY(1, 1)
|
||||
#define NVIC_PRIO_SERIALUART1 NVIC_BUILD_PRIORITY(1, 1)
|
||||
|
|
115
src/main/drivers/transponder_ir.c
Normal file
115
src/main/drivers/transponder_ir.c
Normal file
|
@ -0,0 +1,115 @@
|
|||
/*
|
||||
* 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 <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "build_config.h"
|
||||
|
||||
#include "drivers/transponder_ir.h"
|
||||
|
||||
/*
|
||||
* Implementation note:
|
||||
* Using around over 700 bytes for a transponder DMA buffer is a little excessive, likely an alternative implementation that uses a fast
|
||||
* ISR to generate the output signal dynamically based on state would be more memory efficient and would likely be more appropriate for
|
||||
* other targets. However this approach requires very little CPU time and is just fire-and-forget.
|
||||
*
|
||||
* On an STM32F303CC 720 bytes is currently fine and that is the target for which this code was designed for.
|
||||
*/
|
||||
uint8_t transponderIrDMABuffer[TRANSPONDER_DMA_BUFFER_SIZE];
|
||||
|
||||
volatile uint8_t transponderIrDataTransferInProgress = 0;
|
||||
|
||||
void transponderIrInit(void)
|
||||
{
|
||||
memset(&transponderIrDMABuffer, 0, TRANSPONDER_DMA_BUFFER_SIZE);
|
||||
transponderIrHardwareInit();
|
||||
}
|
||||
|
||||
bool isTransponderIrReady(void)
|
||||
{
|
||||
return !transponderIrDataTransferInProgress;
|
||||
}
|
||||
|
||||
static uint16_t dmaBufferOffset;
|
||||
|
||||
void updateTransponderDMABuffer(const uint8_t* transponderData)
|
||||
{
|
||||
uint8_t byteIndex;
|
||||
uint8_t bitIndex;
|
||||
uint8_t toggleIndex;
|
||||
|
||||
for (byteIndex = 0; byteIndex < TRANSPONDER_DATA_LENGTH; byteIndex++) {
|
||||
|
||||
uint8_t byteToSend = *transponderData;
|
||||
transponderData++;
|
||||
for (bitIndex = 0; bitIndex < TRANSPONDER_BITS_PER_BYTE; bitIndex++)
|
||||
{
|
||||
bool doToggles = false;
|
||||
if (bitIndex == 0) {
|
||||
doToggles = true;
|
||||
} else if (bitIndex == TRANSPONDER_BITS_PER_BYTE - 1) {
|
||||
doToggles = false;
|
||||
} else {
|
||||
doToggles = byteToSend & (1 << (bitIndex - 1));
|
||||
}
|
||||
|
||||
for (toggleIndex = 0; toggleIndex < TRANSPONDER_TOGGLES_PER_BIT; toggleIndex++)
|
||||
{
|
||||
if (doToggles) {
|
||||
transponderIrDMABuffer[dmaBufferOffset] = BIT_TOGGLE_1;
|
||||
} else {
|
||||
transponderIrDMABuffer[dmaBufferOffset] = BIT_TOGGLE_0;
|
||||
}
|
||||
dmaBufferOffset++;
|
||||
}
|
||||
transponderIrDMABuffer[dmaBufferOffset] = BIT_TOGGLE_0;
|
||||
dmaBufferOffset++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void transponderIrWaitForTransmitComplete(void)
|
||||
{
|
||||
static uint32_t waitCounter = 0;
|
||||
|
||||
while(transponderIrDataTransferInProgress) {
|
||||
waitCounter++;
|
||||
}
|
||||
}
|
||||
|
||||
void transponderIrUpdateData(const uint8_t* transponderData)
|
||||
{
|
||||
transponderIrWaitForTransmitComplete();
|
||||
|
||||
updateTransponderDMABuffer(transponderData);
|
||||
}
|
||||
|
||||
|
||||
void transponderIrTransmit(void)
|
||||
{
|
||||
transponderIrWaitForTransmitComplete();
|
||||
|
||||
dmaBufferOffset = 0;
|
||||
|
||||
transponderIrDataTransferInProgress = 1;
|
||||
transponderIrDMAEnable();
|
||||
}
|
||||
|
45
src/main/drivers/transponder_ir.h
Normal file
45
src/main/drivers/transponder_ir.h
Normal file
|
@ -0,0 +1,45 @@
|
|||
/*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
#define TRANSPONDER_BITS_PER_BYTE 10 // start + 8 data + stop
|
||||
#define TRANSPONDER_DATA_LENGTH 6
|
||||
#define TRANSPONDER_TOGGLES_PER_BIT 11
|
||||
#define TRANSPONDER_GAP_TOGGLES 1
|
||||
#define TRANSPONDER_TOGGLES (TRANSPONDER_TOGGLES_PER_BIT + TRANSPONDER_GAP_TOGGLES)
|
||||
|
||||
#define TRANSPONDER_DMA_BUFFER_SIZE ((TRANSPONDER_TOGGLES_PER_BIT + 1) * TRANSPONDER_BITS_PER_BYTE * TRANSPONDER_DATA_LENGTH)
|
||||
|
||||
#define BIT_TOGGLE_1 78 // (156 / 2)
|
||||
#define BIT_TOGGLE_0 0
|
||||
|
||||
void transponderIrInit(void);
|
||||
|
||||
void transponderIrHardwareInit(void);
|
||||
void transponderIrDMAEnable(void);
|
||||
|
||||
void transponderIrWaitForTransmitComplete(void);
|
||||
|
||||
void transponderIrUpdateData(const uint8_t* transponderData);
|
||||
void transponderIrTransmit(void);
|
||||
|
||||
bool isTransponderIrReady(void);
|
||||
|
||||
extern uint8_t transponderIrDMABuffer[TRANSPONDER_DMA_BUFFER_SIZE];
|
||||
extern volatile uint8_t transponderIrDataTransferInProgress;
|
160
src/main/drivers/transponder_ir_stm32f30x.c
Normal file
160
src/main/drivers/transponder_ir_stm32f30x.c
Normal file
|
@ -0,0 +1,160 @@
|
|||
/*
|
||||
* 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 "platform.h"
|
||||
|
||||
#include "drivers/transponder_ir.h"
|
||||
#include "nvic.h"
|
||||
|
||||
#ifndef TRANSPONDER_GPIO
|
||||
#define USE_TRANSPONDER_ON_DMA1_CHANNEL3
|
||||
#define TRANSPONDER_GPIO GPIOB
|
||||
#define TRANSPONDER_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOB
|
||||
#define TRANSPONDER_GPIO_AF GPIO_AF_1
|
||||
#define TRANSPONDER_PIN GPIO_Pin_8 // TIM16_CH1
|
||||
#define TRANSPONDER_PIN_SOURCE GPIO_PinSource8
|
||||
#define TRANSPONDER_TIMER TIM16
|
||||
#define TRANSPONDER_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16
|
||||
#define TRANSPONDER_DMA_CHANNEL DMA1_Channel3
|
||||
#define TRANSPONDER_IRQ DMA1_Channel3_IRQn
|
||||
#endif
|
||||
|
||||
void transponderIrHardwareInit(void)
|
||||
{
|
||||
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
|
||||
TIM_OCInitTypeDef TIM_OCInitStructure;
|
||||
GPIO_InitTypeDef GPIO_InitStructure;
|
||||
DMA_InitTypeDef DMA_InitStructure;
|
||||
|
||||
RCC_AHBPeriphClockCmd(TRANSPONDER_GPIO_AHB_PERIPHERAL, ENABLE);
|
||||
|
||||
GPIO_PinAFConfig(TRANSPONDER_GPIO, TRANSPONDER_PIN_SOURCE, TRANSPONDER_GPIO_AF);
|
||||
|
||||
/* Configuration alternate function push-pull */
|
||||
GPIO_StructInit(&GPIO_InitStructure);
|
||||
GPIO_InitStructure.GPIO_Pin = TRANSPONDER_PIN;
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
|
||||
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
|
||||
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
|
||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
||||
GPIO_Init(TRANSPONDER_GPIO, &GPIO_InitStructure);
|
||||
|
||||
RCC_APB2PeriphClockCmd(TRANSPONDER_TIMER_APB2_PERIPHERAL, ENABLE);
|
||||
|
||||
/* Time base configuration */
|
||||
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
|
||||
TIM_TimeBaseStructure.TIM_Period = 156;
|
||||
TIM_TimeBaseStructure.TIM_Prescaler = 0;
|
||||
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
|
||||
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
|
||||
TIM_TimeBaseInit(TRANSPONDER_TIMER, &TIM_TimeBaseStructure);
|
||||
|
||||
/* PWM1 Mode configuration: Channel1 */
|
||||
TIM_OCStructInit(&TIM_OCInitStructure);
|
||||
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
|
||||
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
|
||||
TIM_OCInitStructure.TIM_Pulse = 0;
|
||||
#ifdef TRANSPONDER_INVERTED
|
||||
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
|
||||
#else
|
||||
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
|
||||
#endif
|
||||
TIM_OC1Init(TRANSPONDER_TIMER, &TIM_OCInitStructure);
|
||||
TIM_OC1PreloadConfig(TRANSPONDER_TIMER, TIM_OCPreload_Enable);
|
||||
|
||||
TIM_CtrlPWMOutputs(TRANSPONDER_TIMER, ENABLE);
|
||||
|
||||
/* configure DMA */
|
||||
/* DMA clock enable */
|
||||
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
|
||||
|
||||
/* DMA1 Channel6 Config */
|
||||
DMA_DeInit(TRANSPONDER_DMA_CHANNEL);
|
||||
|
||||
DMA_StructInit(&DMA_InitStructure);
|
||||
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&TRANSPONDER_TIMER->CCR1;
|
||||
DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)transponderIrDMABuffer;
|
||||
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;
|
||||
DMA_InitStructure.DMA_BufferSize = TRANSPONDER_DMA_BUFFER_SIZE;
|
||||
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
|
||||
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
|
||||
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
|
||||
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
|
||||
DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
|
||||
DMA_InitStructure.DMA_Priority = DMA_Priority_High;
|
||||
DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
|
||||
|
||||
DMA_Init(TRANSPONDER_DMA_CHANNEL, &DMA_InitStructure);
|
||||
|
||||
TIM_DMACmd(TRANSPONDER_TIMER, TIM_DMA_CC1, ENABLE);
|
||||
|
||||
DMA_ITConfig(TRANSPONDER_DMA_CHANNEL, DMA_IT_TC, ENABLE);
|
||||
|
||||
NVIC_InitTypeDef NVIC_InitStructure;
|
||||
|
||||
NVIC_InitStructure.NVIC_IRQChannel = TRANSPONDER_IRQ;
|
||||
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_TRANSPONDER_DMA);
|
||||
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_TRANSPONDER_DMA);
|
||||
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
|
||||
NVIC_Init(&NVIC_InitStructure);
|
||||
}
|
||||
|
||||
#ifdef USE_TRANSPONDER_ON_DMA1_CHANNEL3
|
||||
void DMA1_Channel3_IRQHandler(void)
|
||||
{
|
||||
if (DMA_GetFlagStatus(DMA1_FLAG_TC3)) {
|
||||
transponderIrDataTransferInProgress = 0;
|
||||
DMA_Cmd(DMA1_Channel3, DISABLE); // disable DMA channel
|
||||
DMA_ClearFlag(DMA1_FLAG_TC3); // clear DMA1 Channel transfer complete flag
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_TRANSPONDER_ON_DMA1_CHANNEL2
|
||||
void DMA1_Channel2_IRQHandler(void)
|
||||
{
|
||||
if (DMA_GetFlagStatus(DMA1_FLAG_TC2)) {
|
||||
transponderIrDataTransferInProgress = 0;
|
||||
DMA_Cmd(DMA1_Channel2, DISABLE); // disable DMA channel
|
||||
DMA_ClearFlag(DMA1_FLAG_TC2); // clear DMA1 Channel transfer complete flag
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_TRANSPONDER_ON_DMA1_CHANNEL7
|
||||
void DMA1_Channel7_IRQHandler(void)
|
||||
{
|
||||
if (DMA_GetFlagStatus(DMA1_FLAG_TC7)) {
|
||||
transponderIrDataTransferInProgress = 0;
|
||||
DMA_Cmd(DMA1_Channel7, DISABLE); // disable DMA channel
|
||||
DMA_ClearFlag(DMA1_FLAG_TC7); // clear DMA1 Channel transfer complete flag
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
void transponderIrDMAEnable(void)
|
||||
{
|
||||
DMA_SetCurrDataCounter(TRANSPONDER_DMA_CHANNEL, TRANSPONDER_DMA_BUFFER_SIZE); // load number of bytes to be transferred
|
||||
TIM_SetCounter(TRANSPONDER_TIMER, 0);
|
||||
TIM_Cmd(TRANSPONDER_TIMER, ENABLE);
|
||||
DMA_Cmd(TRANSPONDER_DMA_CHANNEL, ENABLE);
|
||||
}
|
||||
|
||||
|
107
src/main/io/transponder_ir.c
Normal file
107
src/main/io/transponder_ir.c
Normal file
|
@ -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 <stdlib.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <stdarg.h>
|
||||
|
||||
#include <platform.h>
|
||||
|
||||
#include <build_config.h>
|
||||
|
||||
#include "drivers/transponder_ir.h"
|
||||
#include "drivers/system.h"
|
||||
|
||||
#include "io/transponder_ir.h"
|
||||
#include "config/config.h"
|
||||
|
||||
static bool transponderInitialised = false;
|
||||
static bool transponderRepeat = false;
|
||||
|
||||
// timers
|
||||
static uint32_t nextUpdateAt = 0;
|
||||
|
||||
#define JITTER_DURATION_COUNT (sizeof(jitterDurations) / sizeof(uint8_t))
|
||||
static uint8_t jitterDurations[] = {0,9,4,8,3,9,6,7,1,6,9,7,8,2,6};
|
||||
|
||||
void updateTransponder(void)
|
||||
{
|
||||
static uint32_t jitterIndex = 0;
|
||||
|
||||
if (!(transponderInitialised && transponderRepeat && isTransponderIrReady())) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint32_t now = micros();
|
||||
|
||||
bool updateNow = (int32_t)(now - nextUpdateAt) >= 0L;
|
||||
if (!updateNow) {
|
||||
return;
|
||||
}
|
||||
|
||||
// TODO use a random number genenerator for random jitter? The idea here is to avoid multiple transmitters transmitting at the same time.
|
||||
uint32_t jitter = (1000 * jitterDurations[jitterIndex++]);
|
||||
if (jitterIndex >= JITTER_DURATION_COUNT) {
|
||||
jitterIndex = 0;
|
||||
}
|
||||
|
||||
nextUpdateAt = now + 4500 + jitter;
|
||||
|
||||
transponderIrTransmit();
|
||||
}
|
||||
|
||||
void transponderInit(uint8_t* transponderData)
|
||||
{
|
||||
transponderInitialised = false;
|
||||
transponderIrInit();
|
||||
transponderIrUpdateData(transponderData);
|
||||
}
|
||||
|
||||
void transponderEnable(void)
|
||||
{
|
||||
transponderInitialised = true;
|
||||
}
|
||||
|
||||
void transponderDisable(void)
|
||||
{
|
||||
transponderInitialised = false;
|
||||
}
|
||||
|
||||
void transponderStopRepeating(void)
|
||||
{
|
||||
transponderRepeat = false;
|
||||
}
|
||||
|
||||
void transponderStartRepeating(void)
|
||||
{
|
||||
transponderRepeat = true;
|
||||
}
|
||||
|
||||
void transponderUpdateData(uint8_t* transponderData)
|
||||
{
|
||||
transponderIrUpdateData(transponderData);
|
||||
}
|
||||
|
||||
void transponderTransmitOnce(void) {
|
||||
|
||||
if (!transponderInitialised) {
|
||||
return;
|
||||
}
|
||||
transponderIrTransmit();
|
||||
}
|
27
src/main/io/transponder_ir.h
Normal file
27
src/main/io/transponder_ir.h
Normal file
|
@ -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/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
void transponderEnable(void);
|
||||
void transponderDisable(void);
|
||||
void updateTransponder(void);
|
||||
void transponderUpdateData(uint8_t* transponderData);
|
||||
void transponderTransmitOnce(void);
|
||||
void transponderStartRepeating(void);
|
||||
void transponderStopRepeating(void);
|
|
@ -65,6 +65,7 @@
|
|||
#include "io/ledstrip.h"
|
||||
#include "io/display.h"
|
||||
#include "io/asyncfatfs/asyncfatfs.h"
|
||||
#include "io/transponder_ir.h"
|
||||
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/sonar.h"
|
||||
|
@ -126,6 +127,7 @@ void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse);
|
|||
void spektrumBind(rxConfig_t *rxConfig);
|
||||
const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig);
|
||||
void sonarInit(const sonarHardware_t *sonarHardware);
|
||||
void transponderInit(uint8_t* transponderCode);
|
||||
|
||||
#ifdef STM32F303xC
|
||||
// from system_stm32f30x.c
|
||||
|
@ -520,6 +522,12 @@ void init(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef TRANSPONDER
|
||||
transponderInit(masterConfig.transponderData);
|
||||
transponderEnable();
|
||||
transponderStartRepeating();
|
||||
#endif
|
||||
|
||||
#ifdef USE_FLASHFS
|
||||
#ifdef NAZE
|
||||
if (hardwareRevision == NAZE32_REV5) {
|
||||
|
|
|
@ -64,6 +64,8 @@
|
|||
#include "io/serial_msp.h"
|
||||
#include "io/statusindicator.h"
|
||||
#include "io/asyncfatfs/asyncfatfs.h"
|
||||
#include "io/transponder_ir.h"
|
||||
|
||||
|
||||
#include "rx/rx.h"
|
||||
#include "rx/msp.h"
|
||||
|
@ -754,6 +756,10 @@ void taskMainPidLoop(void)
|
|||
handleBlackbox();
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef TRANSPONDER
|
||||
updateTransponder();
|
||||
#endif
|
||||
}
|
||||
|
||||
// Function for loop trigger
|
||||
|
|
|
@ -169,24 +169,36 @@
|
|||
#define RSSI_ADC_GPIO_PIN GPIO_Pin_2
|
||||
#define RSSI_ADC_CHANNEL ADC_Channel_12
|
||||
|
||||
#define LED_STRIP
|
||||
#define LED_STRIP_TIMER TIM1
|
||||
//#define LED_STRIP
|
||||
//#define LED_STRIP_TIMER TIM1
|
||||
//
|
||||
//#define USE_LED_STRIP_ON_DMA1_CHANNEL2
|
||||
//#define WS2811_GPIO GPIOA
|
||||
//#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA
|
||||
//#define WS2811_GPIO_AF GPIO_AF_6
|
||||
//#define WS2811_PIN GPIO_Pin_8
|
||||
//#define WS2811_PIN_SOURCE GPIO_PinSource8
|
||||
//#define WS2811_TIMER TIM1
|
||||
//#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1
|
||||
//#define WS2811_DMA_CHANNEL DMA1_Channel2
|
||||
//#define WS2811_IRQ DMA1_Channel2_IRQn
|
||||
|
||||
#define USE_LED_STRIP_ON_DMA1_CHANNEL2
|
||||
#define WS2811_GPIO GPIOA
|
||||
#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA
|
||||
#define WS2811_GPIO_AF GPIO_AF_6
|
||||
#define WS2811_PIN GPIO_Pin_8
|
||||
#define WS2811_PIN_SOURCE GPIO_PinSource8
|
||||
#define WS2811_TIMER TIM1
|
||||
#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1
|
||||
#define WS2811_DMA_CHANNEL DMA1_Channel2
|
||||
#define WS2811_IRQ DMA1_Channel2_IRQn
|
||||
#define USE_TRANSPONDER_ON_DMA1_CHANNEL2
|
||||
#define TRANSPONDER_GPIO GPIOA
|
||||
#define TRANSPONDER_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA
|
||||
#define TRANSPONDER_GPIO_AF GPIO_AF_6
|
||||
#define TRANSPONDER_PIN GPIO_Pin_8
|
||||
#define TRANSPONDER_PIN_SOURCE GPIO_PinSource8
|
||||
#define TRANSPONDER_TIMER TIM1
|
||||
#define TRANSPONDER_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1
|
||||
#define TRANSPONDER_DMA_CHANNEL DMA1_Channel2
|
||||
#define TRANSPONDER_IRQ DMA1_Channel2_IRQn
|
||||
|
||||
#define GPS
|
||||
#define BLACKBOX
|
||||
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
|
||||
#define TELEMETRY
|
||||
#define TRANSPONDER
|
||||
#define SERIAL_RX
|
||||
#define AUTOTUNE
|
||||
#define DISPLAY
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue