From d6b9350c746d2f2fc48632828502382751d8f607 Mon Sep 17 00:00:00 2001 From: Faduf Date: Sat, 15 Jul 2017 15:50:20 +0200 Subject: [PATCH 1/2] Add specific configuration depending on hardware revisions of the YupiF4 --- src/main/drivers/pwm_output.c | 8 +-- src/main/drivers/pwm_output.h | 3 +- src/main/drivers/sound_beeper.c | 2 +- src/main/target/YUPIF4/config.c | 30 ++++++++-- src/main/target/YUPIF4/hardware_revision.c | 70 ++++++++++++++++++++++ src/main/target/YUPIF4/hardware_revision.h | 30 ++++++++++ src/main/target/YUPIF4/target.c | 18 +++--- src/main/target/YUPIF4/target.h | 27 +++++---- 8 files changed, 158 insertions(+), 30 deletions(-) create mode 100644 src/main/target/YUPIF4/hardware_revision.c create mode 100644 src/main/target/YUPIF4/hardware_revision.h diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 01c0eb8e9..4bcfa80a6 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -480,10 +480,10 @@ void pwmToggleBeeper(void) pwmWriteBeeper(!beeperPwm.enabled); } -void beeperPwmInit(IO_t io, uint16_t frequency) +void beeperPwmInit(const beeperDevConfig_t *beeperConfig) { - const ioTag_t tag=IO_TAG(BEEPER); - beeperPwm.io = io; + const ioTag_t tag = beeperConfig->ioTag; + beeperPwm.io = IOGetByTag(tag); const timerHardware_t *timer = timerGetByTag(tag, TIM_USE_BEEPER); if (beeperPwm.io && timer) { IOInit(beeperPwm.io, OWNER_BEEPER, RESOURCE_INDEX(0)); @@ -492,7 +492,7 @@ void beeperPwmInit(IO_t io, uint16_t frequency) #else IOConfigGPIO(beeperPwm.io, IOCFG_AF_PP); #endif - freqBeep = frequency; + freqBeep = beeperConfig->frequency; pwmOutConfig(&beeperPwm, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / freqBeep, (PWM_TIMER_1MHZ / freqBeep) / 2, 0); } *beeperPwm.ccr = 0; diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 9dcea1edb..ac200106f 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -18,6 +18,7 @@ #pragma once #include "drivers/io_types.h" +#include "drivers/sound_beeper.h" #include "timer.h" #ifndef MAX_SUPPORTED_MOTORS @@ -179,7 +180,7 @@ void pwmCompleteDshotMotorUpdate(uint8_t motorCount); #ifdef BEEPER void pwmWriteBeeper(bool onoffBeep); void pwmToggleBeeper(void); -void beeperPwmInit(IO_t io, uint16_t frequency); +void beeperPwmInit(const beeperDevConfig_t *beeperConfig); #endif void pwmWriteMotor(uint8_t index, float value); diff --git a/src/main/drivers/sound_beeper.c b/src/main/drivers/sound_beeper.c index ebef30d1d..e1e9f3a09 100644 --- a/src/main/drivers/sound_beeper.c +++ b/src/main/drivers/sound_beeper.c @@ -70,7 +70,7 @@ void beeperInit(const beeperDevConfig_t *config) systemBeep(false); } else { beeperIO = IOGetByTag(config->ioTag); - beeperPwmInit(beeperIO, beeperFrequency); + beeperPwmInit(config); } #else UNUSED(config); diff --git a/src/main/target/YUPIF4/config.c b/src/main/target/YUPIF4/config.c index e680f323e..a72a10e1f 100644 --- a/src/main/target/YUPIF4/config.c +++ b/src/main/target/YUPIF4/config.c @@ -21,19 +21,39 @@ #include #ifdef TARGET_CONFIG +#include "blackbox/blackbox.h" + +#include "fc/config.h" #include "flight/pid.h" +#include "hardware_revision.h" + + // alternative defaults settings for YuPiF4 targets void targetConfiguration(void) { - pidProfilesMutable(0)->pid[PID_ROLL].P = 35; + /* Changes depending on versions */ + if (hardwareRevision == YUPIF4_RACE2) { + beeperDevConfigMutable()->ioTag = IO_TAG(BEEPER_OPT); + } else if (hardwareRevision == YUPIF4_MINI) { + beeperDevConfigMutable()->frequency = 0; + blackboxConfigMutable()->device = BLACKBOX_DEVICE_NONE; + adcConfigMutable()->current.enabled = 0; + } else if (hardwareRevision == YUPIF4_NAV) { + beeperDevConfigMutable()->ioTag = IO_TAG(BEEPER_OPT); + } else { + adcConfigMutable()->current.enabled = 0; + } + + /* Specific PID values for YupiF4 */ + pidProfilesMutable(0)->pid[PID_ROLL].P = 30; pidProfilesMutable(0)->pid[PID_ROLL].I = 45; - pidProfilesMutable(0)->pid[PID_ROLL].D = 30; - pidProfilesMutable(0)->pid[PID_PITCH].P = 40; + pidProfilesMutable(0)->pid[PID_ROLL].D = 20; + pidProfilesMutable(0)->pid[PID_PITCH].P = 30; pidProfilesMutable(0)->pid[PID_PITCH].I = 50; - pidProfilesMutable(0)->pid[PID_PITCH].D = 30; - pidProfilesMutable(0)->pid[PID_YAW].P = 50; + pidProfilesMutable(0)->pid[PID_PITCH].D = 20; + pidProfilesMutable(0)->pid[PID_YAW].P = 40; pidProfilesMutable(0)->pid[PID_YAW].I = 50; } #endif diff --git a/src/main/target/YUPIF4/hardware_revision.c b/src/main/target/YUPIF4/hardware_revision.c new file mode 100644 index 000000000..c14035520 --- /dev/null +++ b/src/main/target/YUPIF4/hardware_revision.c @@ -0,0 +1,70 @@ +/* + * 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 "platform.h" + +#include "build/build_config.h" + +#include "drivers/io.h" +#include "drivers/time.h" + +#include "hardware_revision.h" + +uint8_t hardwareRevision = UNKNOWN; + +void detectHardwareRevision(void) +{ + IO_t pin1 = IOGetByTag(IO_TAG(PC13)); + IOInit(pin1, OWNER_SYSTEM, 1); + IOConfigGPIO(pin1, IOCFG_IPU); + + IO_t pin2 = IOGetByTag(IO_TAG(PC14)); + IOInit(pin2, OWNER_SYSTEM, 1); + IOConfigGPIO(pin2, IOCFG_IPU); + + IO_t pin3 = IOGetByTag(IO_TAG(PC15)); + IOInit(pin3, OWNER_SYSTEM, 1); + IOConfigGPIO(pin3, IOCFG_IPU); + + // Check hardware revision + delayMicroseconds(10); // allow configuration to settle + + /* + Hardware pins : Pin1 = PC13 / Pin2 = PC14 / Pin3 = PC15 + no Hardware pins tied to ground => Race V1 + if Pin 1 is the only one tied to ground => Mini + if Pin 2 is the only one tied to ground => Race V2 + if Pin 3 is the only one tied to ground => Navigation + Other combinations available for potential evolutions + */ + if (!IORead(pin1)) { + hardwareRevision = YUPIF4_MINI; + } else if (!IORead(pin2)) { + hardwareRevision = YUPIF4_RACE2; + } else if (!IORead(pin3)) { + hardwareRevision = YUPIF4_NAV; + } else { + hardwareRevision = YUPIF4_RACE1; + } +} + +void updateHardwareRevision(void) { +} diff --git a/src/main/target/YUPIF4/hardware_revision.h b/src/main/target/YUPIF4/hardware_revision.h new file mode 100644 index 000000000..83d58c160 --- /dev/null +++ b/src/main/target/YUPIF4/hardware_revision.h @@ -0,0 +1,30 @@ +/* + * 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 + +typedef enum yupif4HardwareRevision_t { + UNKNOWN = 0, + YUPIF4_RACE1, // Race V1 + YUPIF4_RACE2, // Race V2 + YUPIF4_MINI, // Mini + YUPIF4_NAV, // Navigation +} yupif4HardwareRevision_e; + +extern uint8_t hardwareRevision; + +void detectHardwareRevision(void); +void updateHardwareRevision(void); diff --git a/src/main/target/YUPIF4/target.c b/src/main/target/YUPIF4/target.c index 8f7f49277..77cf5aafc 100644 --- a/src/main/target/YUPIF4/target.c +++ b/src/main/target/YUPIF4/target.c @@ -25,13 +25,15 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - 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(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 + 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(TIM4, CH2, PB7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // S6_OUT - DMA1_ST3 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // S7_OUT + DEF_TIM(TIM3, CH4, PC9, TIM_USE_BEEPER, TIMER_OUTPUT_ENABLED, 0 ), // BEEPER PWM + DEF_TIM(TIM12, CH1, PB14, TIM_USE_BEEPER, TIMER_OUTPUT_ENABLED, 0 ), // BEEPER PWM OPT }; diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index e5a457aa8..59e47b5b8 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -21,12 +21,15 @@ #define USBD_PRODUCT_STRING "YupiF4" +#define USE_HARDWARE_REVISION_DETECTION + #define LED0_PIN PB6 #define LED1_PIN PB4 #define LED2_PIN PB5 #define BEEPER PC9 -#define BEEPER_PWM_HZ 2200 // Beeper PWM frequency in Hz +#define BEEPER_OPT PB14 +#define BEEPER_PWM_HZ 3150 // Beeper PWM frequency in Hz #define INVERTER_PIN_UART6 PB15 @@ -36,16 +39,16 @@ #define MPU_INT_EXTI PC4 //ICM 20689 -#define ICM20689_CS_PIN PA4 -#define ICM20689_SPI_INSTANCE SPI1 +#define ICM20689_CS_PIN PA4 +#define ICM20689_SPI_INSTANCE SPI1 #define ACC #define USE_ACC_SPI_ICM20689 -#define ACC_ICM20689_ALIGN CW90_DEG +#define ACC_ICM20689_ALIGN CW90_DEG #define GYRO #define USE_GYRO_SPI_ICM20689 -#define GYRO_ICM20689_ALIGN CW90_DEG +#define GYRO_ICM20689_ALIGN CW90_DEG // MPU 6500 #define MPU6500_CS_PIN PA4 @@ -78,6 +81,9 @@ #define UART6_TX_PIN PC6 #define USE_SOFTSERIAL1 +#define SOFTSERIAL1_RX_PIN PB0 // PWM5 +#define SOFTSERIAL1_TX_PIN PB1 // PWM7 + #define USE_SOFTSERIAL2 #define SERIAL_PORT_COUNT 6 // VCP, UART1, UART3, UART6, SOFTSERIAL x 2 @@ -103,7 +109,6 @@ #define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 #define SDCARD_DMA_CHANNEL DMA_Channel_0 - // SPI Ports #define USE_SPI @@ -127,18 +132,18 @@ #define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD*2) #define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) - // ADC inputs #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC #define USE_ADC -#define VBAT_ADC_PIN PC1 #define RSSI_ADC_GPIO_PIN PC0 +#define VBAT_ADC_PIN PC1 +#define CURRENT_METER_ADC_PIN PC2 // Default configuration -#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART6 +#define DEFAULT_FEATURES (FEATURE_OSD) // Target IO and timers #define USE_SERIAL_4WAY_BLHELI_INTERFACE @@ -148,5 +153,5 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) -#define USABLE_TIMER_CHANNEL_COUNT 8 -#define USED_TIMERS (TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8)) +#define USABLE_TIMER_CHANNEL_COUNT 10 +#define USED_TIMERS (TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) | TIM_N(12)) From 1f809048df638f8675486dc15777d42a979af720 Mon Sep 17 00:00:00 2001 From: Faduf Date: Sun, 16 Jul 2017 09:45:02 +0200 Subject: [PATCH 2/2] remove beeper dependency in pwm output --- src/main/drivers/pwm_output.c | 5 ++--- src/main/drivers/pwm_output.h | 3 +-- src/main/drivers/sound_beeper.c | 4 ++-- 3 files changed, 5 insertions(+), 7 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 4bcfa80a6..dd8018e9a 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -480,9 +480,8 @@ void pwmToggleBeeper(void) pwmWriteBeeper(!beeperPwm.enabled); } -void beeperPwmInit(const beeperDevConfig_t *beeperConfig) +void beeperPwmInit(const ioTag_t tag, uint16_t frequency) { - const ioTag_t tag = beeperConfig->ioTag; beeperPwm.io = IOGetByTag(tag); const timerHardware_t *timer = timerGetByTag(tag, TIM_USE_BEEPER); if (beeperPwm.io && timer) { @@ -492,7 +491,7 @@ void beeperPwmInit(const beeperDevConfig_t *beeperConfig) #else IOConfigGPIO(beeperPwm.io, IOCFG_AF_PP); #endif - freqBeep = beeperConfig->frequency; + freqBeep = frequency; pwmOutConfig(&beeperPwm, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / freqBeep, (PWM_TIMER_1MHZ / freqBeep) / 2, 0); } *beeperPwm.ccr = 0; diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index ac200106f..619dd2952 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -18,7 +18,6 @@ #pragma once #include "drivers/io_types.h" -#include "drivers/sound_beeper.h" #include "timer.h" #ifndef MAX_SUPPORTED_MOTORS @@ -180,7 +179,7 @@ void pwmCompleteDshotMotorUpdate(uint8_t motorCount); #ifdef BEEPER void pwmWriteBeeper(bool onoffBeep); void pwmToggleBeeper(void); -void beeperPwmInit(const beeperDevConfig_t *beeperConfig); +void beeperPwmInit(const ioTag_t tag, uint16_t frequency); #endif void pwmWriteMotor(uint8_t index, float value); diff --git a/src/main/drivers/sound_beeper.c b/src/main/drivers/sound_beeper.c index e1e9f3a09..2e47104ef 100644 --- a/src/main/drivers/sound_beeper.c +++ b/src/main/drivers/sound_beeper.c @@ -69,8 +69,8 @@ void beeperInit(const beeperDevConfig_t *config) } systemBeep(false); } else { - beeperIO = IOGetByTag(config->ioTag); - beeperPwmInit(config); + const ioTag_t beeperTag = config->ioTag; + beeperPwmInit(beeperTag, beeperFrequency); } #else UNUSED(config);