diff --git a/Makefile b/Makefile index 5d4c39945..ebf1d2189 100644 --- a/Makefile +++ b/Makefile @@ -681,6 +681,7 @@ COMMON_SRC = \ drivers/bus_i2c_config.c \ drivers/bus_i2c_soft.c \ drivers/bus_spi.c \ + drivers/bus_spi_config.c \ drivers/bus_spi_soft.c \ drivers/buttons.c \ drivers/display.c \ @@ -711,6 +712,7 @@ COMMON_SRC = \ io/serial.c \ io/statusindicator.c \ io/transponder_ir.c \ + io/rcsplit.c \ msp/msp_serial.c \ scheduler/scheduler.c \ sensors/battery.c \ @@ -900,6 +902,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ drivers/bus_i2c_config.c \ + drivers/bus_spi_config.c \ drivers/serial_escserial.c \ drivers/serial_pinconfig.c \ drivers/serial_uart_init.c \ @@ -1026,9 +1029,10 @@ F7EXCLUDES = \ SITLEXCLUDES = \ drivers/adc.c \ - drivers/bus_spi.c \ drivers/bus_i2c.c \ drivers/bus_i2c_config.c \ + drivers/bus_spi.c \ + drivers/bus_spi_config.c \ drivers/dma.c \ drivers/pwm_output.c \ drivers/timer.c \ diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index e0fcfee6b..40cf9c3ba 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -323,7 +323,7 @@ typedef struct blackboxSlowState_s { } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp() //From mixer.c: -extern uint16_t motorOutputHigh, motorOutputLow; +extern float motorOutputHigh, motorOutputLow; //From rc_controls.c extern boxBitmask_t rcModeActivationMask; @@ -1188,6 +1188,9 @@ static bool sendFieldDefinition(char mainFrameChar, char deltaFrameChar, const v */ static bool blackboxWriteSysinfo(void) { + const uint16_t motorOutputLowInt = lrintf(motorOutputLow); + const uint16_t motorOutputHighInt = lrintf(motorOutputHigh); + // Make sure we have enough room in the buffer for our longest line (as of this writing, the "Firmware date" line) if (blackboxDeviceReserveBufferSpace(64) != BLACKBOX_RESERVE_SUCCESS) { return false; @@ -1203,7 +1206,7 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", motorConfig()->minthrottle); BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle); BLACKBOX_PRINT_HEADER_LINE("gyro_scale","0x%x", castFloatBytesToInt(1.0f)); - BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", motorOutputLow,motorOutputHigh); + BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", motorOutputLowInt,motorOutputHighInt); BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G); BLACKBOX_PRINT_HEADER_LINE_CUSTOM( diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index bca4df053..2a811bc7b 100644 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -137,7 +137,7 @@ OSD_Entry menuOsdActiveElemsEntries[] = {"YAW PID", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_YAW_PIDS], 0}, {"PROFILES", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_PIDRATE_PROFILE], 0}, {"DEBUG", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_DEBUG], 0}, - {"BATT WARN", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_MAIN_BATT_WARNING], 0}, + {"WARNINGS", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_WARNINGS], 0}, {"DISARMED", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_DISARMED], 0}, {"PIT ANG", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_PITCH_ANGLE], 0}, {"ROL ANG", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_ROLL_ANGLE], 0}, diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi160.c b/src/main/drivers/accgyro/accgyro_spi_bmi160.c index 3852fa34d..835ab25b2 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi160.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi160.c @@ -103,6 +103,7 @@ bool bmi160Detect(const busDevice_t *bus) return true; IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0); IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG); + IOHi(bus->spi.csnPin); spiSetDivisor(BMI160_SPI_INSTANCE, BMI160_SPI_DIVISOR); diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20689.c b/src/main/drivers/accgyro/accgyro_spi_icm20689.c index 9764f6173..cb73bf431 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20689.c @@ -69,6 +69,7 @@ static void icm20689SpiInit(const busDevice_t *bus) IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0); IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG); + IOHi(bus->spi.csnPin); spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD); diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c index fd9455a55..3ce84e4a5 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c @@ -155,6 +155,7 @@ bool mpu6000SpiDetect(const busDevice_t *bus) IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0); IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG); + IOHi(bus->spi.csnPin); spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c index e477c3a04..1afecf509 100755 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c @@ -77,6 +77,7 @@ static void mpu6500SpiInit(const busDevice_t *bus) IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0); IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG); + IOHi(bus->spi.csnPin); spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST); diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index b5aae9eca..eb0c2fdbc 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -72,14 +72,14 @@ static spiDevice_t spiHardwareMap[] = { #if defined(STM32F1) - { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = 0, false }, - { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = 0, false }, + { .dev = SPI1, .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = 0, false }, + { .dev = SPI2, .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = 0, false }, #else - { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF_SPI1, false }, - { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF_SPI2, false }, + { .dev = SPI1, .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF_SPI1, false }, + { .dev = SPI2, .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF_SPI2, false }, #endif #if defined(STM32F3) || defined(STM32F4) - { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF_SPI3, false } + { .dev = SPI3, .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF_SPI3, false } #endif }; @@ -124,21 +124,11 @@ void spiInitDevice(SPIDevice device) IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_CFG, spi->af); IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_CFG, spi->af); IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af); - - if (spi->nss) { - IOInit(IOGetByTag(spi->nss), OWNER_SPI_CS, RESOURCE_INDEX(device)); - IOConfigGPIOAF(IOGetByTag(spi->nss), SPI_IO_CS_CFG, spi->af); - } #endif #if defined(STM32F10X) IOConfigGPIO(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG); IOConfigGPIO(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG); IOConfigGPIO(IOGetByTag(spi->mosi), SPI_IO_AF_MOSI_CFG); - - if (spi->nss) { - IOInit(IOGetByTag(spi->nss), OWNER_SPI_CS, RESOURCE_INDEX(device)); - IOConfigGPIO(IOGetByTag(spi->nss), SPI_IO_CS_CFG); - } #endif // Init SPI hardware @@ -168,11 +158,6 @@ void spiInitDevice(SPIDevice device) SPI_Init(spi->dev, &spiInit); SPI_Cmd(spi->dev, ENABLE); - - if (spi->nss) { - // Drive NSS high to disable connected SPI device. - IOHi(IOGetByTag(spi->nss)); - } } bool spiInit(SPIDevice device) diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 4b0bbf22e..9803ae77d 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -71,7 +71,6 @@ typedef enum SPIDevice { typedef struct SPIDevice_s { SPI_TypeDef *dev; - ioTag_t nss; ioTag_t sck; ioTag_t mosi; ioTag_t miso; @@ -86,6 +85,7 @@ typedef struct SPIDevice_s { #endif } spiDevice_t; +void spiPreInitCs(ioTag_t iotag); bool spiInit(SPIDevice device); void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor); uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t in); diff --git a/src/main/drivers/bus_spi_config.c b/src/main/drivers/bus_spi_config.c new file mode 100644 index 000000000..68643797f --- /dev/null +++ b/src/main/drivers/bus_spi_config.c @@ -0,0 +1,37 @@ +/* + * 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 "drivers/bus_spi.h" +#include "drivers/io.h" + +// Bring a pin for possible CS line to pull-up state in preparation for +// sequential initialization by relevant drivers. +// Note that the pin is set to input for safety at this point. + +void spiPreInitCs(ioTag_t iotag) +{ + IO_t io = IOGetByTag(iotag); + if (io) { + IOInit(io, OWNER_SPI_PREINIT, 0); + IOConfigGPIO(io, IOCFG_IPU); + } +} diff --git a/src/main/drivers/bus_spi_hal.c b/src/main/drivers/bus_spi_hal.c index 598559e66..e33b94321 100644 --- a/src/main/drivers/bus_spi_hal.c +++ b/src/main/drivers/bus_spi_hal.c @@ -70,10 +70,10 @@ static spiDevice_t spiHardwareMap[] = { - { .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .leadingEdge = false, .dmaIrqHandler = DMA2_ST3_HANDLER }, - { .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF5_SPI2, .leadingEdge = false, .dmaIrqHandler = DMA1_ST4_HANDLER }, - { .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF6_SPI3, .leadingEdge = false, .dmaIrqHandler = DMA1_ST7_HANDLER }, - { .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .af = GPIO_AF5_SPI4, .leadingEdge = false, .dmaIrqHandler = DMA2_ST1_HANDLER } + { .dev = SPI1, .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .leadingEdge = false, .dmaIrqHandler = DMA2_ST3_HANDLER }, + { .dev = SPI2, .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF5_SPI2, .leadingEdge = false, .dmaIrqHandler = DMA1_ST4_HANDLER }, + { .dev = SPI3, .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF6_SPI3, .leadingEdge = false, .dmaIrqHandler = DMA1_ST7_HANDLER }, + { .dev = SPI4, .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .af = GPIO_AF5_SPI4, .leadingEdge = false, .dmaIrqHandler = DMA2_ST1_HANDLER } }; SPIDevice spiDeviceByInstance(SPI_TypeDef *instance) @@ -159,21 +159,11 @@ void spiInitDevice(SPIDevice device) IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->af); IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG, spi->af); IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af); - - if (spi->nss) { - IOInit(IOGetByTag(spi->nss), OWNER_SPI_CS, RESOURCE_INDEX(device)); - IOConfigGPIO(IOGetByTag(spi->nss), SPI_IO_CS_CFG); - } #endif #if defined(STM32F10X) IOConfigGPIO(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG); IOConfigGPIO(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG); IOConfigGPIO(IOGetByTag(spi->mosi), SPI_IO_AF_MOSI_CFG); - - if (spi->nss) { - IOInit(IOGetByTag(spi->nss), OWNER_SPI_CS, RESOURCE_INDEX(device)); - IOConfigGPIO(IOGetByTag(spi->nss), SPI_IO_CS_CFG); - } #endif spiHardwareMap[device].hspi.Instance = spi->dev; // Init SPI hardware @@ -198,11 +188,7 @@ void spiInitDevice(SPIDevice device) spiHardwareMap[device].hspi.Init.CLKPhase = SPI_PHASE_2EDGE; } - if (HAL_SPI_Init(&spiHardwareMap[device].hspi) == HAL_OK) - { - if (spi->nss) { - IOHi(IOGetByTag(spi->nss)); - } + if (HAL_SPI_Init(&spiHardwareMap[device].hspi) == HAL_OK) { } } diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 11c69fe26..3ba0c6098 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -397,6 +397,7 @@ void max7456Init(const vcdProfile_t *pVcdProfile) #endif IOInit(max7456CsPin, OWNER_OSD_CS, 0); IOConfigGPIO(max7456CsPin, SPI_IO_CS_CFG); + IOHi(max7456CsPin); spiSetDivisor(MAX7456_SPI_INSTANCE, SPI_CLOCK_STANDARD); // force soft reset on Max7456 diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 82bcc7a40..232589060 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -31,11 +31,13 @@ #define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5) #define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f) -#define DSHOT_MAX_COMMAND 47 - -static pwmWriteFuncPtr pwmWritePtr; +static pwmWriteFunc *pwmWrite; static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; -static pwmCompleteWriteFuncPtr pwmCompleteWritePtr = NULL; +static pwmCompleteWriteFunc *pwmCompleteWrite = NULL; + +#ifdef USE_DSHOT +loadDmaBufferFunc *loadDmaBuffer; +#endif #ifdef USE_SERVOS static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS]; @@ -47,6 +49,7 @@ static uint16_t freqBeep=0; #endif bool pwmMotorsEnabled = false; +bool isDigital = false; static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output) { @@ -125,41 +128,68 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard *port->ccr = 0; } -static void pwmWriteUnused(uint8_t index, uint16_t value) +static void pwmWriteUnused(uint8_t index, float value) { UNUSED(index); UNUSED(value); } -static void pwmWriteBrushed(uint8_t index, uint16_t value) +static void pwmWriteBrushed(uint8_t index, float value) { - *motors[index].ccr = (value - 1000) * motors[index].period / 1000; + *motors[index].ccr = lrintf((value - 1000) * motors[index].period / 1000); } -static void pwmWriteStandard(uint8_t index, uint16_t value) +static void pwmWriteStandard(uint8_t index, float value) { - *motors[index].ccr = value; + *motors[index].ccr = lrintf(value); } -static void pwmWriteOneShot125(uint8_t index, uint16_t value) +static void pwmWriteOneShot125(uint8_t index, float value) { - *motors[index].ccr = lrintf((float)(value * ONESHOT125_TIMER_MHZ/8.0f)); + *motors[index].ccr = lrintf(value * ONESHOT125_TIMER_MHZ/8.0f); } -static void pwmWriteOneShot42(uint8_t index, uint16_t value) +static void pwmWriteOneShot42(uint8_t index, float value) { - *motors[index].ccr = lrintf((float)(value * ONESHOT42_TIMER_MHZ/24.0f)); + *motors[index].ccr = lrintf(value * ONESHOT42_TIMER_MHZ/24.0f); } -static void pwmWriteMultiShot(uint8_t index, uint16_t value) +static void pwmWriteMultiShot(uint8_t index, float value) { - *motors[index].ccr = lrintf(((float)(value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW); + *motors[index].ccr = lrintf(((value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW); } -void pwmWriteMotor(uint8_t index, uint16_t value) +#ifdef USE_DSHOT +static void pwmWriteDigital(uint8_t index, float value) +{ + pwmWriteDigitalInt(index, lrintf(value)); +} + +static uint8_t loadDmaBufferDshot(motorDmaOutput_t *const motor, uint16_t packet) +{ + for (int i = 0; i < 16; i++) { + motor->dmaBuffer[i] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first + packet <<= 1; + } + + return DSHOT_DMA_BUFFER_SIZE; +} + +static uint8_t loadDmaBufferProshot(motorDmaOutput_t *const motor, uint16_t packet) +{ + for (int i = 0; i < 4; i++) { + motor->dmaBuffer[i] = PROSHOT_BASE_SYMBOL + ((packet & 0xF000) >> 12) * PROSHOT_BIT_WIDTH; // Most significant nibble first + packet <<= 4; // Shift 4 bits + } + + return PROSHOT_DMA_BUFFER_SIZE; +} +#endif + +void pwmWriteMotor(uint8_t index, float value) { if (pwmMotorsEnabled) { - pwmWritePtr(index, value); + pwmWrite(index, value); } } @@ -182,7 +212,7 @@ void pwmDisableMotors(void) void pwmEnableMotors(void) { /* check motors can be enabled */ - pwmMotorsEnabled = (pwmWritePtr != pwmWriteUnused); + pwmMotorsEnabled = (pwmWrite != &pwmWriteUnused); } bool pwmAreMotorsEnabled(void) @@ -209,7 +239,7 @@ static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) void pwmCompleteMotorUpdate(uint8_t motorCount) { - pwmCompleteWritePtr(motorCount); + pwmCompleteWrite(motorCount); } void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount) @@ -218,53 +248,54 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8 uint32_t timerMhzCounter = 0; bool useUnsyncedPwm = motorConfig->useUnsyncedPwm; - bool isDigital = false; switch (motorConfig->motorPwmProtocol) { default: case PWM_TYPE_ONESHOT125: timerMhzCounter = ONESHOT125_TIMER_MHZ; - pwmWritePtr = pwmWriteOneShot125; + pwmWrite = &pwmWriteOneShot125; break; case PWM_TYPE_ONESHOT42: timerMhzCounter = ONESHOT42_TIMER_MHZ; - pwmWritePtr = pwmWriteOneShot42; + pwmWrite = &pwmWriteOneShot42; break; case PWM_TYPE_MULTISHOT: timerMhzCounter = MULTISHOT_TIMER_MHZ; - pwmWritePtr = pwmWriteMultiShot; + pwmWrite = &pwmWriteMultiShot; break; case PWM_TYPE_BRUSHED: timerMhzCounter = PWM_BRUSHED_TIMER_MHZ; - pwmWritePtr = pwmWriteBrushed; + pwmWrite = &pwmWriteBrushed; useUnsyncedPwm = true; idlePulse = 0; break; case PWM_TYPE_STANDARD: timerMhzCounter = PWM_TIMER_MHZ; - pwmWritePtr = pwmWriteStandard; + pwmWrite = &pwmWriteStandard; useUnsyncedPwm = true; idlePulse = 0; break; #ifdef USE_DSHOT case PWM_TYPE_PROSHOT1000: - pwmWritePtr = pwmWriteProShot; - pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate; + pwmWrite = &pwmWriteDigital; + loadDmaBuffer = &loadDmaBufferProshot; + pwmCompleteWrite = &pwmCompleteDigitalMotorUpdate; isDigital = true; break; case PWM_TYPE_DSHOT1200: case PWM_TYPE_DSHOT600: case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT150: - pwmWritePtr = pwmWriteDshot; - pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate; + pwmWrite = &pwmWriteDigital; + loadDmaBuffer = &loadDmaBufferDshot; + pwmCompleteWrite = &pwmCompleteDigitalMotorUpdate; isDigital = true; break; #endif } if (!isDigital) { - pwmCompleteWritePtr = useUnsyncedPwm ? pwmCompleteWriteUnused : pwmCompleteOneshotMotorUpdate; + pwmCompleteWrite = useUnsyncedPwm ? &pwmCompleteWriteUnused : &pwmCompleteOneshotMotorUpdate; } for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { @@ -273,8 +304,8 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8 if (timerHardware == NULL) { /* not enough motors initialised for the mixer or a break in the motors */ - pwmWritePtr = pwmWriteUnused; - pwmCompleteWritePtr = pwmCompleteWriteUnused; + pwmWrite = &pwmWriteUnused; + pwmCompleteWrite = &pwmCompleteWriteUnused; /* TODO: block arming and add reason system cannot arm */ return; } @@ -343,7 +374,7 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) void pwmWriteDshotCommand(uint8_t index, uint8_t command) { - if (command <= DSHOT_MAX_COMMAND) { + if (isDigital && (command <= DSHOT_MAX_COMMAND)) { motorDmaOutput_t *const motor = getMotorDmaOutput(index); unsigned repeats; @@ -364,20 +395,39 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t command) for (; repeats; repeats--) { motor->requestTelemetry = true; - pwmWritePtr(index, command); + pwmWriteDigitalInt(index, command); pwmCompleteMotorUpdate(0); delay(1); } } } + +uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, const uint16_t value) +{ + uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0); + motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row + + // compute checksum + int csum = 0; + int csum_data = packet; + for (int i = 0; i < 3; i++) { + csum ^= csum_data; // xor data by nibbles + csum_data >>= 4; + } + csum &= 0xf; + // append checksum + packet = (packet << 4) | csum; + + return packet; +} #endif #ifdef USE_SERVOS -void pwmWriteServo(uint8_t index, uint16_t value) +void pwmWriteServo(uint8_t index, float value) { if (index < MAX_SUPPORTED_SERVOS && servos[index].ccr) { - *servos[index].ccr = value; + *servos[index].ccr = lrintf(value); } } diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index e87014a62..16d7f37c3 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -28,6 +28,8 @@ #define MAX_SUPPORTED_SERVOS 8 #endif +#define DSHOT_MAX_COMMAND 47 + typedef enum { DSHOT_CMD_MOTOR_STOP = 0, DSHOT_CMD_BEEP1, @@ -131,8 +133,8 @@ motorDmaOutput_t *getMotorDmaOutput(uint8_t index); extern bool pwmMotorsEnabled; struct timerHardware_s; -typedef void(*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors -typedef void(*pwmCompleteWriteFuncPtr)(uint8_t motorCount); // function pointer used after motors are written +typedef void pwmWriteFunc(uint8_t index, float value); // function pointer used to write motors +typedef void pwmCompleteWriteFunc(uint8_t motorCount); // function pointer used after motors are written typedef struct { volatile timCCR_t *ccr; @@ -165,10 +167,15 @@ void servoDevInit(const servoDevConfig_t *servoDevConfig); void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse); #ifdef USE_DSHOT +typedef uint8_t loadDmaBufferFunc(motorDmaOutput_t *const motor, uint16_t packet); // function pointer used to encode a digital motor value into the DMA buffer representation + +uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, uint16_t value); + +extern loadDmaBufferFunc *loadDmaBuffer; + uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType); void pwmWriteDshotCommand(uint8_t index, uint8_t command); -void pwmWriteProShot(uint8_t index, uint16_t value); -void pwmWriteDshot(uint8_t index, uint16_t value); +void pwmWriteDigitalInt(uint8_t index, uint16_t value); void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output); void pwmCompleteDigitalMotorUpdate(uint8_t motorCount); #endif @@ -179,11 +186,11 @@ void pwmToggleBeeper(void); void beeperPwmInit(IO_t io, uint16_t frequency); #endif -void pwmWriteMotor(uint8_t index, uint16_t value); +void pwmWriteMotor(uint8_t index, float value); void pwmShutdownPulsesForAllMotors(uint8_t motorCount); void pwmCompleteMotorUpdate(uint8_t motorCount); -void pwmWriteServo(uint8_t index, uint16_t value); +void pwmWriteServo(uint8_t index, float value); pwmOutputPort_t *pwmGetMotors(void); bool pwmIsSynced(void); diff --git a/src/main/drivers/pwm_output_dshot.c b/src/main/drivers/pwm_output_dshot.c index 35fbad1ec..2c97bc0ef 100644 --- a/src/main/drivers/pwm_output_dshot.c +++ b/src/main/drivers/pwm_output_dshot.c @@ -54,66 +54,19 @@ uint8_t getTimerIndex(TIM_TypeDef *timer) return dmaMotorTimerCount-1; } -void pwmWriteDshot(uint8_t index, uint16_t value) +void pwmWriteDigitalInt(uint8_t index, uint16_t value) { - motorDmaOutput_t * const motor = &dmaMotors[index]; + motorDmaOutput_t *const motor = &dmaMotors[index]; if (!motor->timerHardware || !motor->timerHardware->dmaRef) { return; } - uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0); - motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row + uint16_t packet = prepareDshotPacket(motor, value); - // compute checksum - int csum = 0; - int csum_data = packet; - for (int i = 0; i < 3; i++) { - csum ^= csum_data; // xor data by nibbles - csum_data >>= 4; - } - csum &= 0xf; - // append checksum - packet = (packet << 4) | csum; - // generate pulses for whole packet - for (int i = 0; i < 16; i++) { - motor->dmaBuffer[i] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first - packet <<= 1; - } + uint8_t bufferSize = loadDmaBuffer(motor, packet); - DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, DSHOT_DMA_BUFFER_SIZE); - DMA_Cmd(motor->timerHardware->dmaRef, ENABLE); -} - -void pwmWriteProShot(uint8_t index, uint16_t value) -{ - motorDmaOutput_t * const motor = &dmaMotors[index]; - - if (!motor->timerHardware || !motor->timerHardware->dmaRef) { - return; - } - - uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0); - motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row - - // compute checksum - int csum = 0; - int csum_data = packet; - for (int i = 0; i < 3; i++) { - csum ^= csum_data; // xor data by nibbles - csum_data >>= 4; - } - csum &= 0xf; - // append checksum - packet = (packet << 4) | csum; - - // generate pulses for Proshot - for (int i = 0; i < 4; i++) { - motor->dmaBuffer[i] = PROSHOT_BASE_SYMBOL + ((packet & 0xF000) >> 12) * PROSHOT_BIT_WIDTH; // Most significant nibble first - packet <<= 4; // Shift 4 bits - } - - DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, PROSHOT_DMA_BUFFER_SIZE); + DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, bufferSize); DMA_Cmd(motor->timerHardware->dmaRef, ENABLE); } diff --git a/src/main/drivers/pwm_output_dshot_hal.c b/src/main/drivers/pwm_output_dshot_hal.c index c1a1137f3..de270044b 100644 --- a/src/main/drivers/pwm_output_dshot_hal.c +++ b/src/main/drivers/pwm_output_dshot_hal.c @@ -49,81 +49,25 @@ uint8_t getTimerIndex(TIM_TypeDef *timer) return dmaMotorTimerCount - 1; } -void pwmWriteDshot(uint8_t index, uint16_t value) +void pwmWriteDigitalInt(uint8_t index, uint16_t value) { - motorDmaOutput_t * const motor = &dmaMotors[index]; + motorDmaOutput_t *const motor = &dmaMotors[index]; if (!motor->timerHardware || !motor->timerHardware->dmaRef) { return; } - uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0); - motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row + uint16_t packet = prepareDshotPacket(motor, value); - // compute checksum - int csum = 0; - int csum_data = packet; - for (int i = 0; i < 3; i++) { - csum ^= csum_data; // xor data by nibbles - csum_data >>= 4; - } - csum &= 0xf; - // append checksum - packet = (packet << 4) | csum; - // generate pulses for whole packet - for (int i = 0; i < 16; i++) { - motor->dmaBuffer[i] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first - packet <<= 1; - } + uint8_t bufferSize = loadDmaBuffer(motor, packet); if (motor->timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { - if (HAL_TIMEx_PWMN_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, DSHOT_DMA_BUFFER_SIZE) != HAL_OK) { + if (HAL_TIMEx_PWMN_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, bufferSize) != HAL_OK) { /* Starting PWM generation Error */ return; } } else { - if (HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, DSHOT_DMA_BUFFER_SIZE) != HAL_OK) { - /* Starting PWM generation Error */ - return; - } - } -} - -void pwmWriteProShot(uint8_t index, uint16_t value) -{ - motorDmaOutput_t * const motor = &dmaMotors[index]; - - if (!motor->timerHardware || !motor->timerHardware->dmaRef) { - return; - } - - uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0); - motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row - - // compute checksum - int csum = 0; - int csum_data = packet; - for (int i = 0; i < 3; i++) { - csum ^= csum_data; // xor data by nibbles - csum_data >>= 4; - } - csum &= 0xf; - // append checksum - packet = (packet << 4) | csum; - - // generate pulses for Proshot - for (int i = 0; i < 4; i++) { - motor->dmaBuffer[i] = PROSHOT_BASE_SYMBOL + ((packet & 0xF000) >> 12) * PROSHOT_BIT_WIDTH; // Most significant nibble first - packet <<= 4; // Shift 4 bits - } - - if (motor->timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { - if (HAL_TIMEx_PWMN_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, PROSHOT_DMA_BUFFER_SIZE) != HAL_OK) { - /* Starting PWM generation Error */ - return; - } - } else { - if (HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, PROSHOT_DMA_BUFFER_SIZE) != HAL_OK) { + if (HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, bufferSize) != HAL_OK) { /* Starting PWM generation Error */ return; } diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index ed81611ec..68fb5f404 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -125,10 +125,19 @@ extern uint8_t __config_end; static serialPort_t *cliPort; -static bufWriter_t *cliWriter; -static uint8_t cliWriteBuffer[sizeof(*cliWriter) + 128]; -static char cliBuffer[64]; +#ifdef STM32F1 +#define CLI_IN_BUFFER_SIZE 128 +#else +// Space required to set array parameters +#define CLI_IN_BUFFER_SIZE 256 +#endif +#define CLI_OUT_BUFFER_SIZE 64 + +static bufWriter_t *cliWriter; +static uint8_t cliWriteBuffer[sizeof(*cliWriter) + CLI_OUT_BUFFER_SIZE]; + +static char cliBuffer[CLI_IN_BUFFER_SIZE]; static uint32_t bufferIndex = 0; static bool configIsInCopy = false; @@ -294,33 +303,44 @@ static void cliPrintLinef(const char *format, ...) static void printValuePointer(const clivalue_t *var, const void *valuePointer, bool full) { - int value = 0; + if ((var->type & VALUE_MODE_MASK) == MODE_ARRAY) { + for (int i = 0; i < var->config.array.length; i++) { + uint8_t value = ((uint8_t *)valuePointer)[i]; - switch (var->type & VALUE_TYPE_MASK) { - case VAR_UINT8: - value = *(uint8_t *)valuePointer; - break; - - case VAR_INT8: - value = *(int8_t *)valuePointer; - break; - - case VAR_UINT16: - case VAR_INT16: - value = *(int16_t *)valuePointer; - break; - } - - switch(var->type & VALUE_MODE_MASK) { - case MODE_DIRECT: - cliPrintf("%d", value); - if (full) { - cliPrintf(" %d %d", var->config.minmax.min, var->config.minmax.max); + cliPrintf("%d", value); + if (i < var->config.array.length - 1) { + cliPrint(","); + } + } + } else { + int value = 0; + + switch (var->type & VALUE_TYPE_MASK) { + case VAR_UINT8: + value = *(uint8_t *)valuePointer; + break; + + case VAR_INT8: + value = *(int8_t *)valuePointer; + break; + + case VAR_UINT16: + case VAR_INT16: + value = *(int16_t *)valuePointer; + break; + } + + switch(var->type & VALUE_MODE_MASK) { + case MODE_DIRECT: + cliPrintf("%d", value); + if (full) { + cliPrintf(" %d %d", var->config.minmax.min, var->config.minmax.max); + } + break; + case MODE_LOOKUP: + cliPrint(lookupTables[var->config.lookup.tableIndex].values[value]); + break; } - break; - case MODE_LOOKUP: - cliPrint(lookupTables[var->config.lookup.tableIndex].values[value]); - break; } } @@ -378,14 +398,15 @@ static void dumpPgValue(const clivalue_t *value, uint8_t dumpMask) const char *defaultFormat = "#set %s = "; const int valueOffset = getValueOffset(value); const bool equalsDefault = valuePtrEqualsDefault(value->type, pg->copy + valueOffset, pg->address + valueOffset); + if (((dumpMask & DO_DIFF) == 0) || !equalsDefault) { if (dumpMask & SHOW_DEFAULTS && !equalsDefault) { cliPrintf(defaultFormat, value->name); - printValuePointer(value, (uint8_t*)pg->address + valueOffset, 0); + printValuePointer(value, (uint8_t*)pg->address + valueOffset, false); cliPrintLinefeed(); } cliPrintf(format, value->name); - printValuePointer(value, pg->copy + valueOffset, 0); + printValuePointer(value, pg->copy + valueOffset, false); cliPrintLinefeed(); } } @@ -425,26 +446,31 @@ static void cliPrintVarRange(const clivalue_t *var) } cliPrintLinefeed(); } + break; + case (MODE_ARRAY): { + cliPrintLinef("Array length: %d", var->config.array.length); + } + break; } } -static void cliSetVar(const clivalue_t *var, const cliVar_t value) +static void cliSetVar(const clivalue_t *var, const int16_t value) { void *ptr = getValuePointer(var); switch (var->type & VALUE_TYPE_MASK) { case VAR_UINT8: - *(uint8_t *)ptr = value.uint8; + *(uint8_t *)ptr = value; break; case VAR_INT8: - *(int8_t *)ptr = value.int8; + *(int8_t *)ptr = value; break; case VAR_UINT16: case VAR_INT16: - *(int16_t *)ptr = value.int16; + *(int16_t *)ptr = value; break; } } @@ -2508,18 +2534,34 @@ static void cliGet(char *cmdline) cliPrintLine("Invalid name"); } +static char *skipSpace(char *buffer) +{ + while (*(buffer) == ' ') { + buffer++; + } + + return buffer; +} + +static uint8_t getWordLength(char *bufBegin, char *bufEnd) +{ + while (*(bufEnd - 1) == ' ') { + bufEnd--; + } + + return bufEnd - bufBegin; +} + static void cliSet(char *cmdline) { - uint32_t len; - const clivalue_t *val; - char *eqptr = NULL; - - len = strlen(cmdline); + const uint32_t len = strlen(cmdline); + char *eqptr; if (len == 0 || (len == 1 && cmdline[0] == '*')) { cliPrintLine("Current settings: "); + for (uint32_t i = 0; i < valueTableEntryCount; i++) { - val = &valueTable[i]; + const clivalue_t *val = &valueTable[i]; cliPrintf("%s = ", valueTable[i].name); cliPrintVar(val, len); // when len is 1 (when * is passed as argument), it will print min/max values as well, for gui cliPrintLinefeed(); @@ -2527,52 +2569,81 @@ static void cliSet(char *cmdline) } else if ((eqptr = strstr(cmdline, "=")) != NULL) { // has equals - char *lastNonSpaceCharacter = eqptr; - while (*(lastNonSpaceCharacter - 1) == ' ') { - lastNonSpaceCharacter--; - } - uint8_t variableNameLength = lastNonSpaceCharacter - cmdline; + uint8_t variableNameLength = getWordLength(cmdline, eqptr); // skip the '=' and any ' ' characters eqptr++; - while (*(eqptr) == ' ') { - eqptr++; - } + eqptr = skipSpace(eqptr); for (uint32_t i = 0; i < valueTableEntryCount; i++) { - val = &valueTable[i]; + const clivalue_t *val = &valueTable[i]; // ensure exact match when setting to prevent setting variables with shorter names if (strncasecmp(cmdline, valueTable[i].name, strlen(valueTable[i].name)) == 0 && variableNameLength == strlen(valueTable[i].name)) { - bool changeValue = false; - cliVar_t value = { .int16 = 0 }; + bool valueChanged = false; + int16_t value = 0; switch (valueTable[i].type & VALUE_MODE_MASK) { case MODE_DIRECT: { - value.int16 = atoi(eqptr); + int16_t value = atoi(eqptr); - if (value.int16 >= valueTable[i].config.minmax.min && value.int16 <= valueTable[i].config.minmax.max) { - changeValue = true; - } + if (value >= valueTable[i].config.minmax.min && value <= valueTable[i].config.minmax.max) { + cliSetVar(val, value); + valueChanged = true; } - break; + } + + break; case MODE_LOOKUP: { - const lookupTableEntry_t *tableEntry = &lookupTables[valueTable[i].config.lookup.tableIndex]; - bool matched = false; - for (uint32_t tableValueIndex = 0; tableValueIndex < tableEntry->valueCount && !matched; tableValueIndex++) { - matched = strcasecmp(tableEntry->values[tableValueIndex], eqptr) == 0; + const lookupTableEntry_t *tableEntry = &lookupTables[valueTable[i].config.lookup.tableIndex]; + bool matched = false; + for (uint32_t tableValueIndex = 0; tableValueIndex < tableEntry->valueCount && !matched; tableValueIndex++) { + matched = strcasecmp(tableEntry->values[tableValueIndex], eqptr) == 0; - if (matched) { - value.int16 = tableValueIndex; - changeValue = true; - } + if (matched) { + value = tableValueIndex; + + cliSetVar(val, value); + valueChanged = true; } } - break; + } + + break; + case MODE_ARRAY: { + const uint8_t arrayLength = valueTable[i].config.array.length; + char *valPtr = eqptr; + uint8_t array[256]; + char curVal[4]; + for (int i = 0; i < arrayLength; i++) { + valPtr = skipSpace(valPtr); + char *valEnd = strstr(valPtr, ","); + if ((valEnd != NULL) && (i < arrayLength - 1)) { + uint8_t varLength = getWordLength(valPtr, valEnd); + if (varLength <= 3) { + strncpy(curVal, valPtr, getWordLength(valPtr, valEnd)); + curVal[varLength] = '\0'; + array[i] = (uint8_t)atoi((const char *)curVal); + valPtr = valEnd + 1; + } else { + break; + } + } else if ((valEnd == NULL) && (i == arrayLength - 1)) { + array[i] = atoi(valPtr); + + uint8_t *ptr = getValuePointer(val); + memcpy(ptr, array, arrayLength); + valueChanged = true; + } else { + break; + } + } + } + + break; + } - if (changeValue) { - cliSetVar(val, value); - + if (valueChanged) { cliPrintf("%s set to ", valueTable[i].name); cliPrintVar(val, 0); } else { diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 4c7b2a612..9f7544324 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -107,7 +107,7 @@ int16_t magHold; int16_t headFreeModeHold; uint8_t motorControlEnable = false; -static bool reverseMotors; +static bool reverseMotors = false; static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero bool isRXDataNew; diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 45dc3c6fc..546322f28 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -124,6 +124,7 @@ #include "flight/pid.h" #include "flight/servos.h" +#include "io/rcsplit.h" #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" @@ -184,6 +185,61 @@ static IO_t busSwitchResetPin = IO_NONE; } #endif +#ifdef USE_SPI +// Pre-initialize all CS pins to input with pull-up. +// It's sad that we can't do this with an initialized array, +// since we will be taking care of configurable CS pins shortly. + +void spiPreInit(void) +{ +#ifdef USE_ACC_SPI_MPU6000 + spiPreInitCs(IO_TAG(MPU6000_CS_PIN)); +#endif +#ifdef USE_ACC_SPI_MPU6500 + spiPreInitCs(IO_TAG(MPU6500_CS_PIN)); +#endif +#ifdef USE_GYRO_SPI_MPU9250 + spiPreInitCs(IO_TAG(MPU9250_CS_PIN)); +#endif +#ifdef USE_GYRO_SPI_ICM20689 + spiPreInitCs(IO_TAG(ICM20689_CS_PIN)); +#endif +#ifdef USE_ACCGYRO_BMI160 + spiPreInitCs(IO_TAG(BMI160_CS_PIN)); +#endif +#ifdef USE_GYRO_L3GD20 + spiPreInitCs(IO_TAG(L3GD20_CS_PIN)); +#endif +#ifdef USE_MAX7456 + spiPreInitCs(IO_TAG(MAX7456_SPI_CS_PIN)); +#endif +#ifdef USE_SDCARD + spiPreInitCs(IO_TAG(SDCARD_SPI_CS_PIN)); +#endif +#ifdef USE_BARO_SPI_BMP280 + spiPreInitCs(IO_TAG(BMP280_CS_PIN)); +#endif +#ifdef USE_BARO_SPI_MS5611 + spiPreInitCs(IO_TAG(MS5611_CS_PIN)); +#endif +#ifdef USE_MAG_SPI_HMC5883 + spiPreInitCs(IO_TAG(HMC5883_CS_PIN)); +#endif +#ifdef USE_MAG_SPI_AK8963 + spiPreInitCs(IO_TAG(AK8963_CS_PIN)); +#endif +#ifdef RTC6705_CS_PIN // XXX VTX_RTC6705? Should use USE_ format. + spiPreInitCs(IO_TAG(RTC6705_CS_PIN)); +#endif +#ifdef M25P16_CS_PIN // XXX Should use USE_ format. + spiPreInitCs(IO_TAG(M25P16_CS_PIN)); +#endif +#if defined(USE_RX_SPI) && !defined(USE_RX_SOFTSPI) + spiPreInitCs(IO_TAG(RX_NSS_PIN)); +#endif +} +#endif + void init(void) { #ifdef USE_HAL_DRIVER @@ -352,6 +408,9 @@ void init(void) #else #ifdef USE_SPI + // Initialize CS lines and keep them high + spiPreInit(); + #ifdef USE_SPI_DEVICE_1 spiInit(SPIDEV_1); #endif @@ -637,5 +696,10 @@ void init(void) #else fcTasksInit(); #endif + +#ifdef USE_RCSPLIT + rcSplitInit(); +#endif // USE_RCSPLIT + systemState |= SYSTEM_STATE_READY; } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 28f6ea425..ce8a74520 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -153,6 +153,9 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = { { BOX3DDISABLESWITCH, "DISABLE 3D SWITCH", 29}, { BOXFPVANGLEMIX, "FPV ANGLE MIX", 30}, { BOXBLACKBOXERASE, "BLACKBOX ERASE (>30s)", 31 }, + { BOXCAMERA1, "CAMERA CONTROL 1", 32}, + { BOXCAMERA2, "CAMERA CONTROL 2", 33}, + { BOXCAMERA3, "CAMERA CONTROL 3", 34 }, }; // mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index @@ -420,6 +423,12 @@ void initActiveBoxIds(void) } #endif +#ifdef USE_RCSPLIT + BME(BOXCAMERA1); + BME(BOXCAMERA2); + BME(BOXCAMERA3); +#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/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 4bfd4ec84..013189292 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -84,6 +84,7 @@ #include "telemetry/telemetry.h" #include "io/osd_slave.h" +#include "io/rcsplit.h" #ifdef USE_BST void taskBstMasterProcess(timeUs_t currentTimeUs); @@ -594,5 +595,14 @@ cfTask_t cfTasks[TASK_COUNT] = { .staticPriority = TASK_PRIORITY_IDLE, }, #endif + +#ifdef USE_RCSPLIT + [TASK_RCSPLIT] = { + .taskName = "RCSPLIT", + .taskFunc = rcSplitProcess, + .desiredPeriod = TASK_PERIOD_HZ(10), // 10 Hz, 100ms + .staticPriority = TASK_PRIORITY_MEDIUM, + }, +#endif #endif }; diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 2b0d22378..17c35f7e2 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -54,6 +54,9 @@ typedef enum { BOX3DDISABLESWITCH, BOXFPVANGLEMIX, BOXBLACKBOXERASE, + BOXCAMERA1, + BOXCAMERA2, + BOXCAMERA3, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 6ccfbfb10..829b59c09 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -228,7 +228,7 @@ static const char * const lookupTableSuperExpoYaw[] = { static const char * const lookupTablePwmProtocol[] = { "OFF", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED", #ifdef USE_DSHOT - "DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200" + "DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200", "PROSHOT1000" #endif }; @@ -655,7 +655,7 @@ const clivalue_t valueTable[] = { { "osd_debug_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_DEBUG]) }, { "osd_power_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_POWER]) }, { "osd_pidrate_profile_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_PIDRATE_PROFILE]) }, - { "osd_battery_warning_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_MAIN_BATT_WARNING]) }, + { "osd_warnings_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_WARNINGS]) }, { "osd_avg_cell_voltage_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_AVG_CELL_VOLTAGE]) }, { "osd_pit_ang_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_PITCH_ANGLE]) }, { "osd_rol_ang_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ROLL_ANGLE]) }, @@ -664,6 +664,8 @@ const clivalue_t valueTable[] = { { "osd_disarmed_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_DISARMED]) }, { "osd_nheading_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_NUMERICAL_HEADING]) }, { "osd_nvario_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_NUMERICAL_VARIO]) }, + { "osd_esc_tmp_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ESC_TMP]) }, + { "osd_esc_rpm_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ESC_RPM]) }, { "osd_stat_max_spd", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_MAX_SPEED])}, { "osd_stat_max_dist", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_MAX_DISTANCE])}, diff --git a/src/main/fc/settings.h b/src/main/fc/settings.h index 62ceb0477..52ddb0a3f 100644 --- a/src/main/fc/settings.h +++ b/src/main/fc/settings.h @@ -69,34 +69,31 @@ typedef struct lookupTableEntry_s { #define VALUE_TYPE_OFFSET 0 -#define VALUE_SECTION_OFFSET 4 -#define VALUE_MODE_OFFSET 6 +#define VALUE_SECTION_OFFSET 2 +#define VALUE_MODE_OFFSET 4 typedef enum { - // value type, bits 0-3 + // value type, bits 0-1 VAR_UINT8 = (0 << VALUE_TYPE_OFFSET), VAR_INT8 = (1 << VALUE_TYPE_OFFSET), VAR_UINT16 = (2 << VALUE_TYPE_OFFSET), VAR_INT16 = (3 << VALUE_TYPE_OFFSET), - // value section, bits 4-5 + // value section, bits 2-3 MASTER_VALUE = (0 << VALUE_SECTION_OFFSET), PROFILE_VALUE = (1 << VALUE_SECTION_OFFSET), - PROFILE_RATE_VALUE = (2 << VALUE_SECTION_OFFSET), // 0x20 - // value mode - MODE_DIRECT = (0 << VALUE_MODE_OFFSET), // 0x40 - MODE_LOOKUP = (1 << VALUE_MODE_OFFSET) // 0x80 + PROFILE_RATE_VALUE = (2 << VALUE_SECTION_OFFSET), + + // value mode, bits 4-5 + MODE_DIRECT = (0 << VALUE_MODE_OFFSET), + MODE_LOOKUP = (1 << VALUE_MODE_OFFSET), + MODE_ARRAY = (2 << VALUE_MODE_OFFSET) } cliValueFlag_e; -#define VALUE_TYPE_MASK (0x0F) -#define VALUE_SECTION_MASK (0x30) -#define VALUE_MODE_MASK (0xC0) -typedef union { - int8_t int8; - uint8_t uint8; - int16_t int16; -} cliVar_t; +#define VALUE_TYPE_MASK (0x03) +#define VALUE_SECTION_MASK (0x0c) +#define VALUE_MODE_MASK (0x30) typedef struct cliMinMaxConfig_s { const int16_t min; @@ -107,9 +104,14 @@ typedef struct cliLookupTableConfig_s { const lookupTableIndex_e tableIndex; } cliLookupTableConfig_t; +typedef struct cliArrayLengthConfig_s { + const uint8_t length; +} cliArrayLengthConfig_t; + typedef union { cliLookupTableConfig_t lookup; cliMinMaxConfig_t minmax; + cliArrayLengthConfig_t array; } cliValueConfig_t; typedef struct { diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index f5fcd647e..fa3382475 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -114,8 +114,8 @@ PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR static uint8_t motorCount; static float motorMixRange; -int16_t motor[MAX_SUPPORTED_MOTORS]; -int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; +float motor[MAX_SUPPORTED_MOTORS]; +float motor_disarmed[MAX_SUPPORTED_MOTORS]; mixerMode_e currentMixerMode; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; @@ -312,8 +312,8 @@ const mixer_t mixers[] = { }; #endif // !USE_QUAD_MIXER_ONLY -static uint16_t disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow; -uint16_t motorOutputHigh, motorOutputLow; +static float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow; +float motorOutputHigh, motorOutputLow; static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh; uint8_t getMotorCount() @@ -339,6 +339,7 @@ bool mixerIsOutputSaturated(int axis, float errorRate) bool isMotorProtocolDshot(void) { #ifdef USE_DSHOT switch(motorConfig()->dev.motorPwmProtocol) { + case PWM_TYPE_PROSHOT1000: case PWM_TYPE_DSHOT1200: case PWM_TYPE_DSHOT600: case PWM_TYPE_DSHOT300: @@ -352,17 +353,18 @@ bool isMotorProtocolDshot(void) { #endif } -// Add here scaled ESC outputs for digital protol +// All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator +// DSHOT scaling is done to the actual dshot range void initEscEndpoints(void) { #ifdef USE_DSHOT if (isMotorProtocolDshot()) { disarmMotorOutput = DSHOT_DISARM_COMMAND; if (feature(FEATURE_3D)) - motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue)); + motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); else - motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue)); + motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); motorOutputHigh = DSHOT_MAX_THROTTLE; - deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue)); // TODO - Not working yet !! Mixer requires some throttle rescaling changes + deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + ((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); // TODO - Not working yet !! Mixer requires some throttle rescaling changes deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW; } else #endif @@ -509,7 +511,7 @@ void mixTable(pidProfile_t *pidProfile) // Scale roll/pitch/yaw uniformly to fit within throttle range // Initial mixer concept by bdoiron74 reused and optimized for Air Mode float throttle = 0, currentThrottleInputRange = 0; - uint16_t motorOutputMin, motorOutputMax; + float motorOutputMin, motorOutputMax; static uint16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions bool mixerInversion = false; @@ -608,7 +610,7 @@ void mixTable(pidProfile_t *pidProfile) // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. for (uint32_t i = 0; i < motorCount; i++) { - float motorOutput = motorOutputMin + lrintf(motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle))); + float motorOutput = motorOutputMin + motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle)); // Dshot works exactly opposite in lower 3D section. if (mixerInversion) { @@ -642,7 +644,7 @@ void mixTable(pidProfile_t *pidProfile) } } -uint16_t convertExternalToMotor(uint16_t externalValue) +float convertExternalToMotor(uint16_t externalValue) { uint16_t motorValue = externalValue; #ifdef USE_DSHOT @@ -661,12 +663,12 @@ uint16_t convertExternalToMotor(uint16_t externalValue) } #endif - return motorValue; + return (float)motorValue; } -uint16_t convertMotorToExternal(uint16_t motorValue) +uint16_t convertMotorToExternal(float motorValue) { - uint16_t externalValue = motorValue; + uint16_t externalValue = lrintf(motorValue); #ifdef USE_DSHOT if (isMotorProtocolDshot()) { if (feature(FEATURE_3D) && motorValue >= DSHOT_MIN_THROTTLE && motorValue <= DSHOT_3D_DEADBAND_LOW) { diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 51733d31e..665b39fd2 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -118,8 +118,8 @@ PG_DECLARE(motorConfig_t, motorConfig); #define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF extern const mixer_t mixers[]; -extern int16_t motor[MAX_SUPPORTED_MOTORS]; -extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; +extern float motor[MAX_SUPPORTED_MOTORS]; +extern float motor_disarmed[MAX_SUPPORTED_MOTORS]; struct rxConfig_s; uint8_t getMotorCount(); @@ -141,5 +141,5 @@ void stopMotors(void); void stopPwmAllMotors(void); bool isMotorProtocolDshot(void); -uint16_t convertExternalToMotor(uint16_t externalValue); -uint16_t convertMotorToExternal(uint16_t motorValue); +float convertExternalToMotor(uint16_t externalValue); +uint16_t convertMotorToExternal(float motorValue); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 88d12a18d..c06a5202a 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -58,6 +58,7 @@ #include "drivers/vtx_common.h" #include "io/asyncfatfs/asyncfatfs.h" +#include "io/beeper.h" #include "io/flashfs.h" #include "io/gps.h" #include "io/osd.h" @@ -79,6 +80,7 @@ #include "sensors/barometer.h" #include "sensors/battery.h" #include "sensors/sensors.h" +#include "sensors/esc_sensor.h" #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" @@ -95,7 +97,8 @@ // Blink control -bool blinkState = true; +static bool blinkState = true; +static bool showVisualBeeper = false; static uint32_t blinkBits[(OSD_ITEM_COUNT + 31)/32]; #define SET_BLINK(item) (blinkBits[(item) / 32] |= (1 << ((item) % 32))) @@ -154,6 +157,10 @@ static const char compassBar[] = { PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 0); +#ifdef USE_ESC_SENSOR +static escSensorData_t *escData; +#endif + /** * Gets the correct altitude symbol for the current unit system */ @@ -518,7 +525,7 @@ static void osdDrawSingleElement(uint8_t item) break; } - case OSD_MAIN_BATT_WARNING: + case OSD_WARNINGS: switch(getBatteryState()) { case BATTERY_WARNING: tfp_sprintf(buff, "LOW BATTERY"); @@ -529,7 +536,13 @@ static void osdDrawSingleElement(uint8_t item) break; default: - return; + if (showVisualBeeper) { + tfp_sprintf(buff, " * * * *"); + } else { + return; + } + break; + } break; @@ -605,6 +618,16 @@ static void osdDrawSingleElement(uint8_t item) tfp_sprintf(buff, "%c%01d.%01d", directionSymbol, abs(verticalSpeed / 100), abs((verticalSpeed % 100) / 10)); break; } +#ifdef USE_ESC_SENSOR + case OSD_ESC_TMP: + buff[0] = SYM_TEMP_C; + tfp_sprintf(buff + 1, "%d", escData == NULL ? 0 : escData->temperature); + break; + + case OSD_ESC_RPM: + tfp_sprintf(buff, "%d", escData == NULL ? 0 : escData->rpm); + break; +#endif default: return; @@ -613,7 +636,7 @@ static void osdDrawSingleElement(uint8_t item) displayWrite(osdDisplayPort, elemPosX + elemOffsetX, elemPosY, buff); } -void osdDrawElements(void) +static void osdDrawElements(void) { displayClearScreen(osdDisplayPort); @@ -621,19 +644,7 @@ void osdDrawElements(void) if (IS_RC_MODE_ACTIVE(BOXOSD)) return; -#if 0 - if (currentElement) - osdDrawElementPositioningHelp(); -#else - if (false) - ; -#endif -#ifdef CMS - else if (sensors(SENSOR_ACC) || displayIsGrabbed(osdDisplayPort)) -#else - else if (sensors(SENSOR_ACC)) -#endif - { + if (sensors(SENSOR_ACC)) { osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); } @@ -654,7 +665,7 @@ void osdDrawElements(void) osdDrawSingleElement(OSD_YAW_PIDS); osdDrawSingleElement(OSD_POWER); osdDrawSingleElement(OSD_PIDRATE_PROFILE); - osdDrawSingleElement(OSD_MAIN_BATT_WARNING); + osdDrawSingleElement(OSD_WARNINGS); osdDrawSingleElement(OSD_AVG_CELL_VOLTAGE); osdDrawSingleElement(OSD_DEBUG); osdDrawSingleElement(OSD_PITCH_ANGLE); @@ -667,12 +678,7 @@ void osdDrawElements(void) osdDrawSingleElement(OSD_COMPASS_BAR); #ifdef GPS -#ifdef CMS - if (sensors(SENSOR_GPS) || displayIsGrabbed(osdDisplayPort)) -#else - if (sensors(SENSOR_GPS)) -#endif - { + if (sensors(SENSOR_GPS)) { osdDrawSingleElement(OSD_GPS_SATS); osdDrawSingleElement(OSD_GPS_SPEED); osdDrawSingleElement(OSD_GPS_LAT); @@ -681,6 +687,13 @@ void osdDrawElements(void) osdDrawSingleElement(OSD_HOME_DIR); } #endif // GPS + +#ifdef USE_ESC_SENSOR + if (feature(FEATURE_ESC_SENSOR)) { + osdDrawSingleElement(OSD_ESC_TMP); + osdDrawSingleElement(OSD_ESC_RPM); + } +#endif } void pgResetFn_osdConfig(osdConfig_t *osdConfig) @@ -706,7 +719,7 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->item_pos[OSD_YAW_PIDS] = OSD_POS(7, 15) | VISIBLE_FLAG; osdConfig->item_pos[OSD_POWER] = OSD_POS(1, 10) | VISIBLE_FLAG; osdConfig->item_pos[OSD_PIDRATE_PROFILE] = OSD_POS(25, 10) | VISIBLE_FLAG; - osdConfig->item_pos[OSD_MAIN_BATT_WARNING] = OSD_POS(9, 10) | VISIBLE_FLAG; + osdConfig->item_pos[OSD_WARNINGS] = OSD_POS(9, 10) | VISIBLE_FLAG; osdConfig->item_pos[OSD_AVG_CELL_VOLTAGE] = OSD_POS(12, 2) | VISIBLE_FLAG; osdConfig->item_pos[OSD_DEBUG] = OSD_POS(1, 0) | VISIBLE_FLAG; osdConfig->item_pos[OSD_PITCH_ANGLE] = OSD_POS(1, 8) | VISIBLE_FLAG; @@ -721,6 +734,8 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->item_pos[OSD_DISARMED] = OSD_POS(10, 4) | VISIBLE_FLAG; osdConfig->item_pos[OSD_NUMERICAL_HEADING] = OSD_POS(23, 9) | VISIBLE_FLAG; osdConfig->item_pos[OSD_NUMERICAL_VARIO] = OSD_POS(23, 8) | VISIBLE_FLAG; + osdConfig->item_pos[OSD_ESC_TMP] = OSD_POS(18, 2) | VISIBLE_FLAG; + osdConfig->item_pos[OSD_ESC_RPM] = OSD_POS(19, 2) | VISIBLE_FLAG; osdConfig->enabled_stats[OSD_STAT_MAX_SPEED] = true; osdConfig->enabled_stats[OSD_STAT_MIN_BATTERY] = true; @@ -802,12 +817,12 @@ void osdUpdateAlarms(void) CLR_BLINK(OSD_RSSI_VALUE); if (getBatteryState() == BATTERY_OK) { + CLR_BLINK(OSD_WARNINGS); CLR_BLINK(OSD_MAIN_BATT_VOLTAGE); - CLR_BLINK(OSD_MAIN_BATT_WARNING); CLR_BLINK(OSD_AVG_CELL_VOLTAGE); } else { + SET_BLINK(OSD_WARNINGS); SET_BLINK(OSD_MAIN_BATT_VOLTAGE); - SET_BLINK(OSD_MAIN_BATT_WARNING); SET_BLINK(OSD_AVG_CELL_VOLTAGE); } @@ -839,7 +854,7 @@ void osdResetAlarms(void) { CLR_BLINK(OSD_RSSI_VALUE); CLR_BLINK(OSD_MAIN_BATT_VOLTAGE); - CLR_BLINK(OSD_MAIN_BATT_WARNING); + CLR_BLINK(OSD_WARNINGS); CLR_BLINK(OSD_GPS_SATS); CLR_BLINK(OSD_FLYTIME); CLR_BLINK(OSD_MAH_DRAWN); @@ -1064,6 +1079,12 @@ static void osdRefresh(timeUs_t currentTimeUs) blinkState = (currentTimeUs / 200000) % 2; +#ifdef USE_ESC_SENSOR + if (feature(FEATURE_ESC_SENSOR)) { + escData = getEscSensorData(ESC_SENSOR_COMBINED); + } +#endif + #ifdef CMS if (!displayIsGrabbed(osdDisplayPort)) { osdUpdateAlarms(); @@ -1083,6 +1104,11 @@ static void osdRefresh(timeUs_t currentTimeUs) void osdUpdate(timeUs_t currentTimeUs) { static uint32_t counter = 0; + + if (isBeeperOn()) { + showVisualBeeper = true; + } + #ifdef MAX7456_DMA_CHANNEL_TX // don't touch buffers if DMA transaction is in progress if (displayIsTransferInProgress(osdDisplayPort)) { @@ -1108,6 +1134,8 @@ void osdUpdate(timeUs_t currentTimeUs) if (counter++ % DRAW_FREQ_DENOM == 0) { osdRefresh(currentTimeUs); + + showVisualBeeper = false; } else { // rest of time redraw screen 10 chars per idle so it doesn't lock the main idle displayDrawScreen(osdDisplayPort); } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 90ef5eb0b..133517e7a 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -48,7 +48,7 @@ typedef enum { OSD_YAW_PIDS, OSD_POWER, OSD_PIDRATE_PROFILE, - OSD_MAIN_BATT_WARNING, + OSD_WARNINGS, OSD_AVG_CELL_VOLTAGE, OSD_GPS_LON, OSD_GPS_LAT, @@ -63,6 +63,8 @@ typedef enum { OSD_NUMERICAL_HEADING, OSD_NUMERICAL_VARIO, OSD_COMPASS_BAR, + OSD_ESC_TMP, + OSD_ESC_RPM, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; diff --git a/src/main/io/rcsplit.c b/src/main/io/rcsplit.c new file mode 100644 index 000000000..e9533bed6 --- /dev/null +++ b/src/main/io/rcsplit.c @@ -0,0 +1,163 @@ +/* + * 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 "common/utils.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "fc/rc_controls.h" + +#include "io/beeper.h" +#include "io/serial.h" + +#include "scheduler/scheduler.h" + +#include "drivers/serial.h" + +#include "io/rcsplit.h" + +// communicate with camera device variables +serialPort_t *rcSplitSerialPort = NULL; +rcsplit_switch_state_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1]; +rcsplit_state_e cameraState = RCSPLIT_STATE_UNKNOWN; + +static uint8_t crc_high_first(uint8_t *ptr, uint8_t len) +{ + uint8_t i; + uint8_t crc=0x00; + while (len--) { + crc ^= *ptr++; + for (i=8; i>0; --i) { + if (crc & 0x80) + crc = (crc << 1) ^ 0x31; + else + crc = (crc << 1); + } + } + return (crc); +} + +static void sendCtrlCommand(rcsplit_ctrl_argument_e argument) +{ + if (!rcSplitSerialPort) + return ; + + uint8_t uart_buffer[5] = {0}; + uint8_t crc = 0; + + uart_buffer[0] = RCSPLIT_PACKET_HEADER; + uart_buffer[1] = RCSPLIT_PACKET_CMD_CTRL; + uart_buffer[2] = argument; + uart_buffer[3] = RCSPLIT_PACKET_TAIL; + crc = crc_high_first(uart_buffer, 4); + + // build up a full request [header]+[command]+[argument]+[crc]+[tail] + uart_buffer[3] = crc; + uart_buffer[4] = RCSPLIT_PACKET_TAIL; + + // write to device + serialWriteBuf(rcSplitSerialPort, uart_buffer, 5); +} + +static void rcSplitProcessMode() +{ + // if the device not ready, do not handle any mode change event + if (RCSPLIT_STATE_IS_READY != cameraState) + return ; + + for (boxId_e i = BOXCAMERA1; i <= BOXCAMERA3; i++) { + uint8_t switchIndex = i - BOXCAMERA1; + if (IS_RC_MODE_ACTIVE(i)) { + // check last state of this mode, if it's true, then ignore it. + // Here is a logic to make a toggle control for this mode + if (switchStates[switchIndex].isActivated) { + continue; + } + + uint8_t argument = RCSPLIT_CTRL_ARGU_INVALID; + switch (i) { + case BOXCAMERA1: + argument = RCSPLIT_CTRL_ARGU_WIFI_BTN; + break; + case BOXCAMERA2: + argument = RCSPLIT_CTRL_ARGU_POWER_BTN; + break; + case BOXCAMERA3: + argument = RCSPLIT_CTRL_ARGU_CHANGE_MODE; + break; + default: + argument = RCSPLIT_CTRL_ARGU_INVALID; + break; + } + + if (argument != RCSPLIT_CTRL_ARGU_INVALID) { + sendCtrlCommand(argument); + switchStates[switchIndex].isActivated = true; + } + } else { + switchStates[switchIndex].isActivated = false; + } + } +} + +bool rcSplitInit(void) +{ + // found the port config with FUNCTION_RUNCAM_SPLIT_CONTROL + // User must set some UART inteface with RunCam Split at peripherals column in Ports tab + serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RCSPLIT); + if (portConfig) { + rcSplitSerialPort = openSerialPort(portConfig->identifier, FUNCTION_RCSPLIT, NULL, 115200, MODE_RXTX, 0); + } + + if (!rcSplitSerialPort) { + return false; + } + + // set init value to true, to avoid the action auto run when the flight board start and the switch is on. + for (boxId_e i = BOXCAMERA1; i <= BOXCAMERA3; i++) { + uint8_t switchIndex = i - BOXCAMERA1; + switchStates[switchIndex].boxId = 1 << i; + switchStates[switchIndex].isActivated = true; + } + + cameraState = RCSPLIT_STATE_IS_READY; + +#ifdef USE_RCSPLIT + setTaskEnabled(TASK_RCSPLIT, true); +#endif + + return true; +} + +void rcSplitProcess(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + + if (rcSplitSerialPort == NULL) + return ; + + // process rcsplit custom mode if has any changed + rcSplitProcessMode(); +} diff --git a/src/main/io/rcsplit.h b/src/main/io/rcsplit.h new file mode 100644 index 000000000..6900700ee --- /dev/null +++ b/src/main/io/rcsplit.h @@ -0,0 +1,56 @@ +/* + * 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 . + */ + +#pragma once + +#include +#include "common/time.h" +#include "fc/fc_msp.h" + +typedef struct { + uint8_t boxId; + bool isActivated; +} rcsplit_switch_state_t; + +typedef enum { + RCSPLIT_STATE_UNKNOWN = 0, + RCSPLIT_STATE_INITIALIZING, + RCSPLIT_STATE_IS_READY, +} rcsplit_state_e; + +// packet header and tail +#define RCSPLIT_PACKET_HEADER 0x55 +#define RCSPLIT_PACKET_CMD_CTRL 0x01 +#define RCSPLIT_PACKET_TAIL 0xaa + + +// the commands of RunCam Split serial protocol +typedef enum { + RCSPLIT_CTRL_ARGU_INVALID = 0x0, + RCSPLIT_CTRL_ARGU_WIFI_BTN = 0x1, + RCSPLIT_CTRL_ARGU_POWER_BTN = 0x2, + RCSPLIT_CTRL_ARGU_CHANGE_MODE = 0x3, + RCSPLIT_CTRL_ARGU_WHO_ARE_YOU = 0xFF, +} rcsplit_ctrl_argument_e; + +bool rcSplitInit(void); +void rcSplitProcess(timeUs_t currentTimeUs); + +// only for unit test +extern rcsplit_state_e cameraState; +extern serialPort_t *rcSplitSerialPort; +extern rcsplit_switch_state_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1]; diff --git a/src/main/io/serial.h b/src/main/io/serial.h index e7fb4d4c6..5d0009d1b 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -44,6 +44,7 @@ typedef enum { FUNCTION_VTX_SMARTAUDIO = (1 << 11), // 2048 FUNCTION_TELEMETRY_IBUS = (1 << 12), // 4096 FUNCTION_VTX_TRAMP = (1 << 13), // 8192 + FUNCTION_RCSPLIT = (1 << 14), // 16384 } serialPortFunction_e; typedef enum { diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 7f819b6df..f943d950c 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -111,6 +111,10 @@ typedef enum { TASK_VTXCTRL, #endif +#ifdef USE_RCSPLIT + TASK_RCSPLIT, +#endif + /* Count of real tasks */ TASK_COUNT, diff --git a/src/main/target/CJMCU/initialisation.c b/src/main/target/CJMCU/initialisation.c index 01986ce33..4f6b7ecbe 100644 --- a/src/main/target/CJMCU/initialisation.c +++ b/src/main/target/CJMCU/initialisation.c @@ -24,9 +24,12 @@ #include "drivers/bus_spi.h" #include "io/serial.h" +extern void spiPreInit(void); // XXX In fc/fc_init.c + void targetBusInit(void) { #if defined(USE_SPI) && defined(USE_SPI_DEVICE_1) + spiPreInit(); spiInit(SPIDEV_1); #endif diff --git a/src/main/target/CLRACINGF7/target.c b/src/main/target/CLRACINGF7/target.c index 1ff2591a0..710a85c90 100644 --- a/src/main/target/CLRACINGF7/target.c +++ b/src/main/target/CLRACINGF7/target.c @@ -30,14 +30,14 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM4, CH3, PB8, TIM_USE_PPM, TIMER_INPUT_ENABLED, 0), // PPM -DMA1_ST7 - DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 1), // PWM1 - DMA1_ST6 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 1), // PWM2 - DMA2_ST2 - DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM3 - DMA1_ST1 - DEF_TIM(TIM3, CH4, PC9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM4 - DMA1_ST2 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 2), // PWM5 - DMA2_ST3 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM6 - DMA1_ST0 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 1), // PWM1 - DMA1_ST6 D(1, 7, 3),D(1, 6, 3) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 1), // PWM2 - DMA2_ST2 D(2, 4, 7),D(2, 2, 0) + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM3 - DMA1_ST1 D(1, 1, 3) + DEF_TIM(TIM3, CH4, PC9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM4 - DMA1_ST2 D(1, 2, 5) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 2), // PWM5 - DMA2_ST3 D(2, 6, 0),D(2, 1, 6),D(2, 3, 6) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM6 - DMA1_ST0 D(1, 0, 2) - DEF_TIM(TIM1, CH3N, PB1, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED, 0), // S5_OUT - DMA2_ST6 + DEF_TIM(TIM1, CH3N, PB1, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED, 0), // S5_OUT - DMA2_ST6 D(2, 6, 0),D(2, 6, 6) }; diff --git a/src/main/target/CLRACINGF7/target.h b/src/main/target/CLRACINGF7/target.h index 12da28684..73ec82022 100644 --- a/src/main/target/CLRACINGF7/target.h +++ b/src/main/target/CLRACINGF7/target.h @@ -42,11 +42,28 @@ #define USE_ACC_SPI_MPU6000 #define GYRO #define USE_GYRO_SPI_MPU6000 + #define GYRO_MPU6000_ALIGN CW0_DEG #define ACC_MPU6000_ALIGN CW0_DEG #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 +// ICM-20602 +#define USE_ACC_MPU6500 +#define USE_ACC_SPI_MPU6500 +#define USE_GYRO_MPU6500 +#define USE_GYRO_SPI_MPU6500 + +#define ACC_MPU6500_ALIGN CW0_DEG +#define GYRO_MPU6500_ALIGN CW0_DEG +#define MPU6500_CS_PIN SPI1_NSS_PIN +#define MPU6500_SPI_INSTANCE SPI1 + +// MPU interrupts +#define USE_EXTI +#define MPU_INT_EXTI PC4 +#define USE_MPU_DATA_READY_SIGNAL + #define OSD #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI3 @@ -64,8 +81,8 @@ #define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 8 // 27MHz #define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF1_5 -#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA2 +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF1_4 +#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 #define SDCARD_DMA_CHANNEL DMA_CHANNEL_0 #define USE_VCP @@ -116,7 +133,7 @@ #define CURRENT_METER_ADC_PIN PC1 #define VBAT_ADC_PIN PC2 #define RSSI_ADC_PIN PC3 -#define CURRENT_METER_SCALE_DEFAULT 250 // 3/120A = 25mv/A +#define CURRENT_METER_SCALE_DEFAULT 250 // 3.3/120A = 25mv/A // LED strip configuration. #define LED_STRIP diff --git a/src/main/target/CLRACINGF7/target.mk b/src/main/target/CLRACINGF7/target.mk index 719339dc7..ab85a4ba9 100644 --- a/src/main/target/CLRACINGF7/target.mk +++ b/src/main/target/CLRACINGF7/target.mk @@ -1,8 +1,12 @@ F7X2RE_TARGETS += $(TARGET) FEATURES += SDCARD VCP TARGET_SRC = \ + drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_spi_icm20689.c\ + drivers/accgyro/accgyro_mpu6500.c \ + drivers/accgyro/accgyro_spi_mpu6500.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/light_ws2811strip.c \ drivers/light_ws2811strip_hal.c \ drivers/max7456.c + diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 34a62f384..9da520de1 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -139,7 +139,7 @@ static uint8_t activeBoxIds[CHECKBOX_ITEM_COUNT]; // this is the number of filled indexes in above array static uint8_t activeBoxIdCount = 0; // from mixer.c -extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; +extern float motor_disarmed[MAX_SUPPORTED_MOTORS]; // cause reboot after BST processing complete static bool isRebootScheduled = false; diff --git a/src/main/target/FRSKYF4/target.h b/src/main/target/FRSKYF4/target.h index 82674200d..06470b0ac 100644 --- a/src/main/target/FRSKYF4/target.h +++ b/src/main/target/FRSKYF4/target.h @@ -41,13 +41,7 @@ #define MPU_INT_EXTI PC4 #define USE_MPU_DATA_READY_SIGNAL -//#define MAG -//#define USE_MAG_HMC5883 -//#define MAG_HMC5883_ALIGN CW90_DEG - #define BARO -//#define USE_BARO_MS5611 - #define USE_BARO_BMP280 #define USE_BARO_SPI_BMP280 #define BMP280_SPI_INSTANCE SPI3 diff --git a/src/main/target/NAZE/initialisation.c b/src/main/target/NAZE/initialisation.c index 6f30d3260..c6b37d054 100644 --- a/src/main/target/NAZE/initialisation.c +++ b/src/main/target/NAZE/initialisation.c @@ -25,9 +25,12 @@ #include "io/serial.h" #include "hardware_revision.h" +extern void spiPreInit(void); // XXX In fc/fc_init.c + void targetBusInit(void) { #ifdef USE_SPI + spiPreInit(); #ifdef USE_SPI_DEVICE_2 spiInit(SPIDEV_2); #endif diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index c233dafa7..bb0d756b0 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -392,7 +392,7 @@ pwmOutputPort_t *pwmGetMotors(void) { bool pwmAreMotorsEnabled(void) { return pwmMotorsEnabled; } -void pwmWriteMotor(uint8_t index, uint16_t value) { +void pwmWriteMotor(uint8_t index, float value) { motorsPwm[index] = value - idlePulse; } void pwmShutdownPulsesForAllMotors(uint8_t motorCount) { @@ -419,7 +419,7 @@ void pwmCompleteMotorUpdate(uint8_t motorCount) { udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet)); // printf("[pwm]%u:%u,%u,%u,%u\n", idlePulse, motorsPwm[0], motorsPwm[1], motorsPwm[2], motorsPwm[3]); } -void pwmWriteServo(uint8_t index, uint16_t value) { +void pwmWriteServo(uint8_t index, float value) { servosPwm[index] = value; } diff --git a/src/main/target/YUPIF4/target.c b/src/main/target/YUPIF4/target.c index 8f1ab8b62..8f7f49277 100644 --- a/src/main/target/YUPIF4/target.c +++ b/src/main/target/YUPIF4/target.c @@ -25,13 +25,13 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, TIMER_INPUT_ENABLED, 0 ), // PPM IN + DEF_TIM(TIM8, CH3, PC8, TIM_USE_PPM, TIMER_INPUT_ENABLED, 0 ), // PPM IN DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // S1_OUT - DMA1_ST2 DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // S2_OUT - DMA1_ST4 DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // S3_OUT - DMA1_ST1 DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 1 ), // S4_OUT - DMA1_ST6 DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_ENABLED, 0 ), // S5_OUT - DMA1_ST7 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // S6_OUT - DMA1_ST2 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_BEEPER, TIMER_OUTPUT_ENABLED, 0 ), // BEEPER PWM + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // S6_OUT - DMA1_ST2 + DEF_TIM(TIM3, CH4, PC9, TIM_USE_BEEPER, TIMER_OUTPUT_ENABLED, 0 ), // BEEPER PWM }; diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index bcb65b557..9ab590d7c 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -149,4 +149,4 @@ #define TARGET_IO_PORTD (BIT(2)) #define USABLE_TIMER_CHANNEL_COUNT 8 -#define USED_TIMERS (TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8)) +#define USED_TIMERS (TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8)) diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 3f86f13b5..b53a0ddb7 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -135,3 +135,5 @@ #define USE_UNCOMMON_MIXERS #endif + +#define USE_RCSPLIT diff --git a/src/test/Makefile b/src/test/Makefile index 6a02179fd..c97ed1872 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -185,6 +185,15 @@ type_conversion_unittest_SRC := \ ws2811_unittest_SRC := \ $(USER_DIR)/drivers/light_ws2811strip.c +rcsplit_unittest_SRC := \ + $(USER_DIR)/common/bitarray.c \ + $(USER_DIR)/fc/rc_modes.c \ + $(USER_DIR)/io/rcsplit.c + +rcsplit_unitest_DEFINES := \ + USE_UART3 \ + USE_RCSPLIT \ + # Please tweak the following variable definitions as needed by your # project, except GTEST_HEADERS, which you can use in your own targets # but shouldn't modify. diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 415f9deb1..5cf1c57e6 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -25,6 +25,7 @@ extern "C" { #include "common/maths.h" #include "common/axis.h" + #include "common/bitarray.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" @@ -156,14 +157,14 @@ TEST_F(RcControlsModesTest, updateActivatedModesUsingValidAuxConfigurationAndRXV rcData[AUX7] = 950; // value equal to range step upper boundary should not activate the mode // and - uint32_t expectedMask = 0; - expectedMask |= (1 << 0); - expectedMask |= (1 << 1); - expectedMask |= (1 << 2); - expectedMask |= (1 << 3); - expectedMask |= (1 << 4); - expectedMask |= (1 << 5); - expectedMask |= (0 << 6); + boxBitmask_t activeBoxIds; + memset(&activeBoxIds, 0, sizeof(boxBitmask_t)); + bitArraySet(&activeBoxIds, 0); + bitArraySet(&activeBoxIds, 1); + bitArraySet(&activeBoxIds, 2); + bitArraySet(&activeBoxIds, 3); + bitArraySet(&activeBoxIds, 4); + bitArraySet(&activeBoxIds, 5); // when updateActivatedModes(); @@ -171,9 +172,9 @@ TEST_F(RcControlsModesTest, updateActivatedModesUsingValidAuxConfigurationAndRXV // then for (int index = 0; index < CHECKBOX_ITEM_COUNT; index++) { #ifdef DEBUG_RC_CONTROLS - printf("iteration: %d\n", index); + printf("iteration: %d, %d\n", index, (bool)(bitArrayGet(&activeBoxIds, index))); #endif - EXPECT_EQ((bool)(expectedMask & (1 << index)), IS_RC_MODE_ACTIVE((boxId_e)index)); + EXPECT_EQ((bool)(bitArrayGet(&activeBoxIds, index)), IS_RC_MODE_ACTIVE((boxId_e)index)); } } diff --git a/src/test/unit/rcsplit_unittest.cc b/src/test/unit/rcsplit_unittest.cc new file mode 100644 index 000000000..67be33ae7 --- /dev/null +++ b/src/test/unit/rcsplit_unittest.cc @@ -0,0 +1,431 @@ +/* + * 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 "gtest/gtest.h" + +extern "C" { + #include + #include + #include + + #include "platform.h" + + #include "common/utils.h" + #include "common/maths.h" + #include "common/bitarray.h" + + #include "config/parameter_group.h" + #include "config/parameter_group_ids.h" + + #include "fc/rc_controls.h" + #include "fc/rc_modes.h" + + + #include "io/beeper.h" + #include "io/serial.h" + + #include "scheduler/scheduler.h" + #include "drivers/serial.h" + #include "io/rcsplit.h" + + #include "rx/rx.h" + + int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] + + rcsplit_state_e unitTestRCsplitState() + { + return cameraState; + } + + bool unitTestIsSwitchActivited(boxId_e boxId) + { + uint8_t adjustBoxID = boxId - BOXCAMERA1; + rcsplit_switch_state_t switchState = switchStates[adjustBoxID]; + return switchState.isActivated; + } + + void unitTestResetRCSplit() + { + rcSplitSerialPort = NULL; + cameraState = RCSPLIT_STATE_UNKNOWN; + } +} + +typedef struct testData_s { + bool isRunCamSplitPortConfigurated; + bool isRunCamSplitOpenPortSupported; + int8_t maxTimesOfRespDataAvailable; + bool isAllowBufferReadWrite; +} testData_t; + +static testData_t testData; + +TEST(RCSplitTest, TestRCSplitInitWithoutPortConfigurated) +{ + memset(&testData, 0, sizeof(testData)); + unitTestResetRCSplit(); + bool result = rcSplitInit(); + EXPECT_EQ(false, result); + EXPECT_EQ(RCSPLIT_STATE_UNKNOWN, unitTestRCsplitState()); +} + +TEST(RCSplitTest, TestRCSplitInitWithoutOpenPortConfigurated) +{ + memset(&testData, 0, sizeof(testData)); + unitTestResetRCSplit(); + testData.isRunCamSplitOpenPortSupported = false; + testData.isRunCamSplitPortConfigurated = true; + + bool result = rcSplitInit(); + EXPECT_EQ(false, result); + EXPECT_EQ(RCSPLIT_STATE_UNKNOWN, unitTestRCsplitState()); +} + +TEST(RCSplitTest, TestRCSplitInit) +{ + memset(&testData, 0, sizeof(testData)); + unitTestResetRCSplit(); + testData.isRunCamSplitOpenPortSupported = true; + testData.isRunCamSplitPortConfigurated = true; + + bool result = rcSplitInit(); + EXPECT_EQ(true, result); + EXPECT_EQ(RCSPLIT_STATE_IS_READY, unitTestRCsplitState()); +} + +TEST(RCSplitTest, TestRecvWhoAreYouResponse) +{ + memset(&testData, 0, sizeof(testData)); + unitTestResetRCSplit(); + testData.isRunCamSplitOpenPortSupported = true; + testData.isRunCamSplitPortConfigurated = true; + + bool result = rcSplitInit(); + EXPECT_EQ(true, result); + + // here will generate a number in [6-255], it's make the serialRxBytesWaiting() and serialRead() run at least 5 times, + // so the "who are you response" will full received, and cause the state change to RCSPLIT_STATE_IS_READY; + int8_t randNum = rand() % 127 + 6; + testData.maxTimesOfRespDataAvailable = randNum; + rcSplitProcess((timeUs_t)0); + + EXPECT_EQ(RCSPLIT_STATE_IS_READY, unitTestRCsplitState()); +} + +TEST(RCSplitTest, TestWifiModeChangeWithDeviceUnready) +{ + memset(&testData, 0, sizeof(testData)); + unitTestResetRCSplit(); + testData.isRunCamSplitOpenPortSupported = true; + testData.isRunCamSplitPortConfigurated = true; + testData.maxTimesOfRespDataAvailable = 0; + + bool result = rcSplitInit(); + EXPECT_EQ(true, result); + + // bind aux1, aux2, aux3 channel to wifi button, power button and change mode + for (uint8_t i = 0; i <= (BOXCAMERA3 - BOXCAMERA1); i++) { + memset(modeActivationConditionsMutable(i), 0, sizeof(modeActivationCondition_t)); + } + + // bind aux1 to wifi button with range [900,1600] + modeActivationConditionsMutable(0)->auxChannelIndex = 0; + modeActivationConditionsMutable(0)->modeId = BOXCAMERA1; + modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MIN); + modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(1600); + + // bind aux2 to power button with range [1900, 2100] + modeActivationConditionsMutable(1)->auxChannelIndex = 1; + modeActivationConditionsMutable(1)->modeId = BOXCAMERA2; + modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1900); + modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(2100); + + // bind aux3 to change mode with range [1300, 1600] + modeActivationConditionsMutable(2)->auxChannelIndex = 2; + modeActivationConditionsMutable(2)->modeId = BOXCAMERA3; + modeActivationConditionsMutable(2)->range.startStep = CHANNEL_VALUE_TO_STEP(1300); + modeActivationConditionsMutable(2)->range.endStep = CHANNEL_VALUE_TO_STEP(1600); + + // make the binded mode inactive + rcData[modeActivationConditions(0)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1800; + rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 900; + rcData[modeActivationConditions(2)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 900; + + updateActivatedModes(); + + // runn process loop + rcSplitProcess(0); + + EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA1)); + EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA2)); + EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA3)); +} + +TEST(RCSplitTest, TestWifiModeChangeWithDeviceReady) +{ + memset(&testData, 0, sizeof(testData)); + unitTestResetRCSplit(); + testData.isRunCamSplitOpenPortSupported = true; + testData.isRunCamSplitPortConfigurated = true; + testData.maxTimesOfRespDataAvailable = 0; + + bool result = rcSplitInit(); + EXPECT_EQ(true, result); + + // bind aux1, aux2, aux3 channel to wifi button, power button and change mode + for (uint8_t i = 0; i <= BOXCAMERA3 - BOXCAMERA1; i++) { + memset(modeActivationConditionsMutable(i), 0, sizeof(modeActivationCondition_t)); + } + + + // bind aux1 to wifi button with range [900,1600] + modeActivationConditionsMutable(0)->auxChannelIndex = 0; + modeActivationConditionsMutable(0)->modeId = BOXCAMERA1; + modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MIN); + modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(1600); + + // bind aux2 to power button with range [1900, 2100] + modeActivationConditionsMutable(1)->auxChannelIndex = 1; + modeActivationConditionsMutable(1)->modeId = BOXCAMERA2; + modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1900); + modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(2100); + + // bind aux3 to change mode with range [1300, 1600] + modeActivationConditionsMutable(2)->auxChannelIndex = 2; + modeActivationConditionsMutable(2)->modeId = BOXCAMERA3; + modeActivationConditionsMutable(2)->range.startStep = CHANNEL_VALUE_TO_STEP(1900); + modeActivationConditionsMutable(2)->range.endStep = CHANNEL_VALUE_TO_STEP(2100); + + rcData[modeActivationConditions(0)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1700; + rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 2000; + rcData[modeActivationConditions(2)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1700; + + updateActivatedModes(); + + // runn process loop + int8_t randNum = rand() % 127 + 6; + testData.maxTimesOfRespDataAvailable = randNum; + rcSplitProcess((timeUs_t)0); + + EXPECT_EQ(RCSPLIT_STATE_IS_READY, unitTestRCsplitState()); + + EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA1)); + EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA2)); + EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA3)); +} + +TEST(RCSplitTest, TestWifiModeChangeCombine) +{ + memset(&testData, 0, sizeof(testData)); + unitTestResetRCSplit(); + testData.isRunCamSplitOpenPortSupported = true; + testData.isRunCamSplitPortConfigurated = true; + testData.maxTimesOfRespDataAvailable = 0; + + bool result = rcSplitInit(); + EXPECT_EQ(true, result); + + // bind aux1, aux2, aux3 channel to wifi button, power button and change mode + for (uint8_t i = 0; i <= BOXCAMERA3 - BOXCAMERA1; i++) { + memset(modeActivationConditionsMutable(i), 0, sizeof(modeActivationCondition_t)); + } + + + // bind aux1 to wifi button with range [900,1600] + modeActivationConditionsMutable(0)->auxChannelIndex = 0; + modeActivationConditionsMutable(0)->modeId = BOXCAMERA1; + modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MIN); + modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(1600); + + // bind aux2 to power button with range [1900, 2100] + modeActivationConditionsMutable(1)->auxChannelIndex = 1; + modeActivationConditionsMutable(1)->modeId = BOXCAMERA2; + modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1900); + modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(2100); + + // bind aux3 to change mode with range [1300, 1600] + modeActivationConditionsMutable(2)->auxChannelIndex = 2; + modeActivationConditionsMutable(2)->modeId = BOXCAMERA3; + modeActivationConditionsMutable(2)->range.startStep = CHANNEL_VALUE_TO_STEP(1900); + modeActivationConditionsMutable(2)->range.endStep = CHANNEL_VALUE_TO_STEP(2100); + + // // make the binded mode inactive + rcData[modeActivationConditions(0)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1700; + rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 2000; + rcData[modeActivationConditions(2)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1700; + updateActivatedModes(); + + // runn process loop + int8_t randNum = rand() % 127 + 6; + testData.maxTimesOfRespDataAvailable = randNum; + rcSplitProcess((timeUs_t)0); + + EXPECT_EQ(RCSPLIT_STATE_IS_READY, unitTestRCsplitState()); + + EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA1)); + EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA2)); + EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA3)); + + + // // make the binded mode inactive + rcData[modeActivationConditions(0)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1500; + rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1300; + rcData[modeActivationConditions(2)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1900; + updateActivatedModes(); + rcSplitProcess((timeUs_t)0); + EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA1)); + EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA2)); + EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA3)); + + + rcData[modeActivationConditions(2)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1899; + updateActivatedModes(); + rcSplitProcess((timeUs_t)0); + EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA3)); + + rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 2001; + updateActivatedModes(); + rcSplitProcess((timeUs_t)0); + EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA1)); + EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA2)); + EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA3)); +} + +extern "C" { + serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e functionMask, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, portOptions_t options) + { + UNUSED(identifier); + UNUSED(functionMask); + UNUSED(baudRate); + UNUSED(callback); + UNUSED(mode); + UNUSED(options); + + if (testData.isRunCamSplitOpenPortSupported) { + static serialPort_t s; + s.vTable = NULL; + + // common serial initialisation code should move to serialPort::init() + s.rxBufferHead = s.rxBufferTail = 0; + s.txBufferHead = s.txBufferTail = 0; + s.rxBufferSize = 0; + s.txBufferSize = 0; + s.rxBuffer = s.rxBuffer; + s.txBuffer = s.txBuffer; + + // callback works for IRQ-based RX ONLY + s.rxCallback = NULL; + s.baudRate = 0; + + return (serialPort_t *)&s; + } + + return NULL; + } + + serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function) + { + UNUSED(function); + if (testData.isRunCamSplitPortConfigurated) { + static serialPortConfig_t portConfig; + + portConfig.identifier = SERIAL_PORT_USART3; + portConfig.msp_baudrateIndex = BAUD_115200; + portConfig.gps_baudrateIndex = BAUD_57600; + portConfig.telemetry_baudrateIndex = BAUD_AUTO; + portConfig.blackbox_baudrateIndex = BAUD_115200; + portConfig.functionMask = FUNCTION_MSP; + + return &portConfig; + } + + return NULL; + } + + uint32_t serialRxBytesWaiting(const serialPort_t *instance) + { + UNUSED(instance); + + testData.maxTimesOfRespDataAvailable--; + if (testData.maxTimesOfRespDataAvailable > 0) { + return 1; + } + + return 0; + } + + uint8_t serialRead(serialPort_t *instance) + { + UNUSED(instance); + + if (testData.maxTimesOfRespDataAvailable > 0) { + static uint8_t i = 0; + static uint8_t buffer[] = { 0x55, 0x01, 0xFF, 0xad, 0xaa }; + + if (i >= 5) { + i = 0; + } + + return buffer[i++]; + } + + return 0; + } + + void sbufWriteString(sbuf_t *dst, const char *string) + { + UNUSED(dst); UNUSED(string); + + if (testData.isAllowBufferReadWrite) { + sbufWriteData(dst, string, strlen(string)); + } + } + void sbufWriteU8(sbuf_t *dst, uint8_t val) + { + UNUSED(dst); UNUSED(val); + + if (testData.isAllowBufferReadWrite) { + *dst->ptr++ = val; + } + } + + void sbufWriteData(sbuf_t *dst, const void *data, int len) + { + UNUSED(dst); UNUSED(data); UNUSED(len); + + if (testData.isAllowBufferReadWrite) { + memcpy(dst->ptr, data, len); + dst->ptr += len; + + } + } + + // modifies streambuf so that written data are prepared for reading + void sbufSwitchToReader(sbuf_t *buf, uint8_t *base) + { + UNUSED(buf); UNUSED(base); + + if (testData.isAllowBufferReadWrite) { + buf->end = buf->ptr; + buf->ptr = base; + } + } + + bool feature(uint32_t) { return false;} + void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count) { UNUSED(instance); UNUSED(data); UNUSED(count); } +} \ No newline at end of file