diff --git a/src/main/config/config.c b/src/main/config/config.c index 543e2fbdaa..16685c7d39 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -801,7 +801,7 @@ void activateConfig(void) #endif } -void validateAndFixConfig(void) +static void validateAndFixConfig(void) { if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP) || featureConfigured(FEATURE_RX_NRF24))) { featureSet(DEFAULT_RX_FEATURE); @@ -828,6 +828,19 @@ void validateAndFixConfig(void) validateNavConfig(&masterConfig.navConfig); #endif + if (featureConfigured(FEATURE_SOFTSPI)) { + featureClear(FEATURE_RX_PPM | FEATURE_RX_PARALLEL_PWM | FEATURE_SOFTSERIAL | FEATURE_VBAT); +#if defined(STM32F10X) + featureClear(FEATURE_LED_STRIP); + // rssi adc needs the same ports + featureClear(FEATURE_RSSI_ADC); + // current meter needs the same ports + if (masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) { + featureClear(FEATURE_CURRENT_METER); + } +#endif + } + if (featureConfigured(FEATURE_RX_PARALLEL_PWM)) { featureClear(FEATURE_RX_SERIAL | FEATURE_RX_MSP | FEATURE_RX_PPM | FEATURE_RX_NRF24); #if defined(STM32F10X) diff --git a/src/main/config/config.h b/src/main/config/config.h index e9f8bd5597..749a70ea05 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -45,6 +45,7 @@ typedef enum { FEATURE_BLACKBOX = 1 << 19, FEATURE_CHANNEL_FORWARDING = 1 << 20, FEATURE_RX_NRF24 = 1 << 21, + FEATURE_SOFTSPI = 1 << 22, } features_e; void handleOneshotFeatureChangeOnRestart(void); diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index ead51a078a..1470f29714 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -84,21 +84,20 @@ pwmIOConfiguration_t *pwmGetOutputConfiguration(void){ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) { - int i = 0; - const uint16_t *setup; - +#ifndef SKIP_RX_PWM int channelIndex = 0; - +#endif memset(&pwmIOConfiguration, 0, sizeof(pwmIOConfiguration)); // this is pretty hacky shit, but it will do for now. array of 4 config maps, [ multiPWM multiPPM airPWM airPPM ] + int i = 0; if (init->airplane) i = 2; // switch to air hardware config if (init->usePPM || init->useSerialRx) i++; // next index is for PPM - setup = hardwareMaps[i]; + const uint16_t *setup = hardwareMaps[i]; for (i = 0; i < USABLE_TIMER_CHANNEL_COUNT && setup[i] != 0xFFFF; i++) { uint8_t timerIndex = setup[i] & 0x00FF; @@ -175,27 +174,27 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) } #endif -#ifdef USE_NRF24_SOFTSPI - if (type == MAP_TO_PWM_INPUT) - continue; - if (type == MAP_TO_PPM_INPUT) - continue; - if (timerHardwarePtr->gpio == NRF24_CE_GPIO && timerHardwarePtr->pin == NRF24_CE_PIN) { - continue; - } - if (timerHardwarePtr->gpio == NRF24_CSN_GPIO && timerHardwarePtr->pin == NRF24_CSN_PIN) { - continue; - } - if (timerHardwarePtr->gpio == NRF24_SCK_GPIO && timerHardwarePtr->pin == NRF24_SCK_PIN) { - continue; - } - if (timerHardwarePtr->gpio == NRF24_MOSI_GPIO && timerHardwarePtr->pin == NRF24_MOSI_PIN) { - continue; - } - if (timerHardwarePtr->gpio == NRF24_MISO_GPIO && timerHardwarePtr->pin == NRF24_MISO_PIN) { - continue; - } -#endif + /*if (init->useSoftSPI) { + if (type == MAP_TO_PWM_INPUT) + continue; + if (type == MAP_TO_PPM_INPUT) + continue; + if (timerHardwarePtr->gpio == NRF24_CE_GPIO && timerHardwarePtr->pin == NRF24_CE_PIN) { + continue; + } + if (timerHardwarePtr->gpio == NRF24_CSN_GPIO && timerHardwarePtr->pin == NRF24_CSN_PIN) { + continue; + } + if (timerHardwarePtr->gpio == NRF24_SCK_GPIO && timerHardwarePtr->pin == NRF24_SCK_PIN) { + continue; + } + if (timerHardwarePtr->gpio == NRF24_MOSI_GPIO && timerHardwarePtr->pin == NRF24_MOSI_PIN) { + continue; + } + if (timerHardwarePtr->gpio == NRF24_MISO_GPIO && timerHardwarePtr->pin == NRF24_MISO_PIN) { + continue; + } + }*/ // hacks to allow current functionality if (type == MAP_TO_PWM_INPUT && !init->useParallelPWM) @@ -283,6 +282,7 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) #endif if (type == MAP_TO_PPM_INPUT) { +#ifndef SKIP_RX_PWM #ifdef CC3D_PPM1 if (init->useOneshot || isMotorBrushed(init->motorPwmRate)) { ppmAvoidPWMTimerClash(timerHardwarePtr, TIM4); @@ -296,11 +296,14 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) ppmInConfig(timerHardwarePtr); pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_PPM; pwmIOConfiguration.ppmInputCount++; +#endif } else if (type == MAP_TO_PWM_INPUT) { +#ifndef SKIP_RX_PWM pwmInConfig(timerHardwarePtr, channelIndex); pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_PWM; pwmIOConfiguration.pwmInputCount++; channelIndex++; +#endif } else if (type == MAP_TO_MOTOR_OUTPUT) { #if defined(CC3D) && !defined(CC3D_PPM1) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 5b24fe83bf..fc723d276c 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -185,7 +185,7 @@ static const char * const featureNames[] = { "SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE", "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125", - "BLACKBOX", "CHANNEL_FORWARDING", "RX_NRF24", NULL + "BLACKBOX", "CHANNEL_FORWARDING", "RX_NRF24", "SOFTSPI", NULL }; // sync this with rxFailsafeChannelMode_e diff --git a/src/main/main.c b/src/main/main.c index 16231c01a7..25aa601b91 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -161,9 +161,6 @@ void flashLedsAndBeep(void) void init(void) { - uint8_t i; - drv_pwm_config_t pwm_params; - printfSupportInit(); initEEPROM(); @@ -225,18 +222,21 @@ void init(void) mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer); #endif + drv_pwm_config_t pwm_params; memset(&pwm_params, 0, sizeof(pwm_params)); #ifdef SONAR sonarGPIOConfig_t sonarGPIOConfig; if (feature(FEATURE_SONAR)) { const sonarHcsr04Hardware_t *sonarHardware = sonarGetHardwareConfiguration(masterConfig.batteryConfig.currentMeterType); - sonarGPIOConfig.gpio = sonarHardware->echo_gpio; - sonarGPIOConfig.triggerPin = sonarHardware->echo_pin; - sonarGPIOConfig.echoPin = sonarHardware->trigger_pin; - pwm_params.sonarGPIOConfig = &sonarGPIOConfig; + if (sonarHardware) { + sonarGPIOConfig.gpio = sonarHardware->echo_gpio; + sonarGPIOConfig.triggerPin = sonarHardware->echo_pin; + sonarGPIOConfig.echoPin = sonarHardware->trigger_pin; + pwm_params.sonarGPIOConfig = &sonarGPIOConfig; + pwm_params.useSonar = true; + } } - pwm_params.useSonar = feature(FEATURE_SONAR); #endif // when using airplane/wing mixer, servo/motor outputs are remapped @@ -406,7 +406,7 @@ void init(void) LED1_ON; LED0_OFF; - for (i = 0; i < 10; i++) { + for (int i = 0; i < 10; i++) { LED1_TOGGLE; LED0_TOGGLE; delay(25); diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index 732c5acfca..7de7d4e16d 100644 --- a/src/main/sensors/rangefinder.c +++ b/src/main/sensors/rangefinder.c @@ -53,20 +53,10 @@ static float rangefinderMaxTiltCos; static int32_t calculatedAltitude; -static const sonarHcsr04Hardware_t *sonarGetHardwareConfigurationForHCSR04(currentSensor_e currentSensor) +static const sonarHcsr04Hardware_t *sonarGetHardwareConfigurationForHCSR04(sonarHCSR04Pins_e sonarHCSR04Pins) { + if (sonarHCSR04Pins == SONAR_HCSR04_PINS_PWM) { #if defined(SONAR_PWM_TRIGGER_PIN) -#endif -#if !defined(UNIT_TEST) -#endif -#if defined(UNIT_TEST) - UNUSED(currentSensor); - return 0; -#elif defined(SONAR_PWM_TRIGGER_PIN) - // If we are using softserial, parallel PWM or ADC current sensor, then use motor pins for sonar, otherwise use RC pins - if (feature(FEATURE_SOFTSERIAL) - || feature(FEATURE_RX_PARALLEL_PWM ) - || (feature(FEATURE_CURRENT_METER) && currentSensor == CURRENT_SENSOR_ADC)) { sonarHcsr04Hardware = (sonarHcsr04Hardware_t){ .trigger_pin = SONAR_PWM_TRIGGER_PIN, .trigger_gpio = SONAR_PWM_TRIGGER_GPIO, @@ -75,7 +65,12 @@ static const sonarHcsr04Hardware_t *sonarGetHardwareConfigurationForHCSR04(curre .exti_line = SONAR_PWM_EXTI_LINE, .exti_pin_source = SONAR_PWM_EXTI_PIN_SOURCE, .exti_irqn = SONAR_PWM_EXTI_IRQN }; +#else + // no PWM pins available for sonar, so return NULL + return NULL; +#endif } else { +#if defined(SONAR_TRIGGER_PIN) sonarHcsr04Hardware = (sonarHcsr04Hardware_t){ .trigger_pin = SONAR_TRIGGER_PIN, .trigger_gpio = SONAR_TRIGGER_GPIO, @@ -84,20 +79,11 @@ static const sonarHcsr04Hardware_t *sonarGetHardwareConfigurationForHCSR04(curre .exti_line = SONAR_EXTI_LINE, .exti_pin_source = SONAR_EXTI_PIN_SOURCE, .exti_irqn = SONAR_EXTI_IRQN }; - } -#elif defined(SONAR_TRIGGER_PIN) - UNUSED(currentSensor); - sonarHcsr04Hardware = (sonarHcsr04Hardware_t){ - .trigger_pin = SONAR_TRIGGER_PIN, - .trigger_gpio = SONAR_TRIGGER_GPIO, - .echo_pin = SONAR_ECHO_PIN, - .echo_gpio = SONAR_ECHO_GPIO, - .exti_line = SONAR_EXTI_LINE, - .exti_pin_source = SONAR_EXTI_PIN_SOURCE, - .exti_irqn = SONAR_EXTI_IRQN }; #else -#error Sonar not defined for target + // no RC pins available for sonar, so return NULL + return NULL; #endif + } return &sonarHcsr04Hardware; } @@ -132,7 +118,16 @@ const sonarHcsr04Hardware_t *sonarGetHardwareConfiguration(currentSensor_e curre // Return the configuration for the HC-SR04 hardware. // Unfortunately the I2C bus is not initialised at this point // so cannot detect if another sonar device is present - return sonarGetHardwareConfigurationForHCSR04(currentSensor); + // If we are using softserial, parallel PWM or ADC current sensor, then use motor pins for sonar, otherwise use RC pins + sonarHCSR04Pins_e sonarHCSR04Pins = SONAR_HCSR04_PINS_RC; + if (feature(FEATURE_SOFTSERIAL) + || feature(FEATURE_SOFTSPI) + || feature(FEATURE_RX_PARALLEL_PWM ) + || (feature(FEATURE_CURRENT_METER) && currentSensor == CURRENT_SENSOR_ADC)) { + // cannot use the RC pins for sonar + sonarHCSR04Pins = SONAR_HCSR04_PINS_PWM; + } + return sonarGetHardwareConfigurationForHCSR04(sonarHCSR04Pins); } void rangefinderInit(rangefinderType_e rangefinderType) diff --git a/src/main/sensors/sonar.h b/src/main/sensors/sonar.h new file mode 100644 index 0000000000..b3ab2cfe84 --- /dev/null +++ b/src/main/sensors/sonar.h @@ -0,0 +1,52 @@ +/* + * 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 "drivers/sonar_hcsr04.h" +#include "drivers/sonar.h" +#include "io/rc_controls.h" // for throttleStatus_e required by battery.h +#include "sensors/battery.h" + +typedef enum { + SONAR_NONE = 0, + SONAR_HCSR04, + SONAR_SRF10 +} sonarHardwareType_e; + +typedef enum { + SONAR_HCSR04_PINS_RC, + SONAR_HCSR04_PINS_PWM, +} sonarHCSR04Pins_e; + +typedef void (*sonarInitFunctionPtr)(sonarRange_t *sonarRange); +typedef void (*sonarUpdateFunctionPtr)(void); +typedef int32_t (*sonarReadFunctionPtr)(void); + +typedef struct sonarFunctionPointers_s { + sonarInitFunctionPtr init; + sonarUpdateFunctionPtr update; + sonarReadFunctionPtr read; +} sonarFunctionPointers_t; + +const sonarHcsr04Hardware_t *sonarGetHardwareConfiguration(currentSensor_e currentSensor); +int32_t sonarCalculateAltitude(int32_t sonarDistance, float cosTiltAngle); +int32_t sonarGetLatestAltitude(void); +void sonarInit(void); +void sonarUpdate(void); +int32_t sonarRead(void); + diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 9c58aa4f64..1a861b08e6 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -93,35 +93,24 @@ #define USE_I2C #define I2C_DEVICE (I2CDEV_2) // Flex port - SCL/PB10, SDA/PB11 +//#define USE_RX_NRF24 #ifdef USE_RX_NRF24 #undef SERIAL_RX #define SKIP_RX_MSP -#define SKIP_RX_PWM +//#define SKIP_RX_PWM #define DEFAULT_RX_FEATURE FEATURE_RX_NRF24 +#define DEFAULT_FEATURES FEATURE_SOFTSPI #define USE_RX_V202 #define USE_RX_SYMA #define USE_RX_CX10 #define USE_RX_H8_3D //#define NRF24_DEFAULT_PROTOCOL NRF24RX_V202_1M -//#define NRF24_DEFAULT_PROTOCOL NRF24RX_SYMA_X5C -#define NRF24_DEFAULT_PROTOCOL NRF24RX_H8_3D +#define NRF24_DEFAULT_PROTOCOL NRF24RX_SYMA_X5C +//#define NRF24_DEFAULT_PROTOCOL NRF24RX_H8_3D #define USE_SOFTSPI #define USE_NRF24_SOFTSPI -#else -#define SPEKTRUM_BIND -// USART3, PB11 (Flexport) -#define BIND_PORT GPIOB -#define BIND_PIN Pin_11 -#endif // USE_RX_NRF24 - - -#ifdef USE_NRF24_SOFTSPI - -#undef USE_SOFTSERIAL1 -#undef SERIAL_PORT_COUNT -#define SERIAL_PORT_COUNT 3 // RC pinouts // RC3 PB6/TIM4 unused @@ -145,9 +134,24 @@ #define NRF24_MISO_GPIO GPIOB #define NRF24_MISO_PIN GPIO_Pin_0 +#else +#define SPEKTRUM_BIND +// USART3, PB11 (Flexport) +#define BIND_PORT GPIOB +#define BIND_PIN Pin_11 +#endif // USE_RX_NRF24 + + +/*#ifdef USE_NRF24_SOFTSPI + +#undef USE_SOFTSERIAL1 +#undef SERIAL_PORT_COUNT +#define SERIAL_PORT_COUNT 3 + + //#define SOFTSPI_NSS_PIN -#else +#else*/ #define USE_ADC @@ -179,14 +183,15 @@ #define SONAR_EXTI_PIN_SOURCE GPIO_PinSource0 #define SONAR_EXTI_IRQN EXTI0_IRQn -#endif // USE_NRF24_SOFTSPI - // LED strip is on PWM5 output pin -//#define LED_STRIP -//#define LED_STRIP_TIMER TIM3 +#define LED_STRIP +#define LED_STRIP_TIMER TIM3 //#define USE_SERIAL_4WAY_BLHELI_INTERFACE +//#endif // USE_NRF24_SOFTSPI + + #define NAV //#define NAV_AUTO_MAG_DECLINATION #define NAV_GPS_GLITCH_DETECTION @@ -202,6 +207,10 @@ #undef BLACKBOX #undef TELEMETRY #undef TELEMETRY_LTM +#undef SERIAL_RX +#ifdef USE_RX_NRF24 +#undef BLACKBOX +#endif #endif // DEBUG