From 3e94abe81f2eafa020bd87d6ad0ac9ae3ca35aa2 Mon Sep 17 00:00:00 2001 From: czchc Date: Wed, 11 Oct 2017 10:22:58 +0800 Subject: [PATCH] rewrite BEEBRAIN_V2D target as variant of BEEBRAIN_V2F --- src/main/target/BEEBRAIN_V2D/config.c | 144 ------------------- src/main/target/BEEBRAIN_V2D/target.c | 33 ----- src/main/target/BEEBRAIN_V2D/target.h | 114 --------------- src/main/target/BEEBRAIN_V2D/target.mk | 9 -- src/main/target/BEEBRAIN_V2F/BEEBRAIN_V2D.mk | 1 + src/main/target/BEEBRAIN_V2F/config.c | 15 +- src/main/target/BEEBRAIN_V2F/target.h | 15 +- 7 files changed, 25 insertions(+), 306 deletions(-) delete mode 100755 src/main/target/BEEBRAIN_V2D/config.c delete mode 100755 src/main/target/BEEBRAIN_V2D/target.c delete mode 100755 src/main/target/BEEBRAIN_V2D/target.h delete mode 100755 src/main/target/BEEBRAIN_V2D/target.mk create mode 100755 src/main/target/BEEBRAIN_V2F/BEEBRAIN_V2D.mk diff --git a/src/main/target/BEEBRAIN_V2D/config.c b/src/main/target/BEEBRAIN_V2D/config.c deleted file mode 100755 index 685b6a045..000000000 --- a/src/main/target/BEEBRAIN_V2D/config.c +++ /dev/null @@ -1,144 +0,0 @@ -/* - * 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 - -#ifdef TARGET_CONFIG - -#include "common/axis.h" -#include "common/maths.h" - -#include "config/feature.h" - -#include "drivers/light_led.h" -#include "drivers/pwm_esc_detect.h" - -#include "fc/config.h" -#include "fc/controlrate_profile.h" - -#include "flight/mixer.h" -#include "flight/pid.h" - -#include "rx/rx.h" - -#include "io/serial.h" -#include "io/osd.h" - -#include "sensors/battery.h" -#include "sensors/gyro.h" - -#include "telemetry/telemetry.h" - -#ifdef BRUSHED_MOTORS_PWM_RATE -#undef BRUSHED_MOTORS_PWM_RATE -#endif - -#define BRUSHED_MOTORS_PWM_RATE 32000 // 32kHz - -void targetConfiguration(void) -{ - if (hardwareMotorType == MOTOR_BRUSHED) { - motorConfigMutable()->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; - motorConfigMutable()->minthrottle = 1030; - pidConfigMutable()->pid_process_denom = 1; - } - - for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) { - pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex); - - pidProfile->pid[PID_ROLL].P = 86; - pidProfile->pid[PID_ROLL].I = 50; - pidProfile->pid[PID_ROLL].D = 60; - pidProfile->pid[PID_PITCH].P = 90; - pidProfile->pid[PID_PITCH].I = 55; - pidProfile->pid[PID_PITCH].D = 60; - pidProfile->pid[PID_YAW].P = 123; - pidProfile->pid[PID_YAW].I = 75; - } - - for (uint8_t rateProfileIndex = 0; rateProfileIndex < CONTROL_RATE_PROFILE_COUNT; rateProfileIndex++) { - controlRateConfig_t *controlRateConfig = controlRateProfilesMutable(rateProfileIndex); - - controlRateConfig->rcYawRate8 = 120; - controlRateConfig->rcExpo8 = 15; - controlRateConfig->rcYawExpo8 = 15; - controlRateConfig->rates[FD_ROLL] = 85; - controlRateConfig->rates[FD_PITCH] = 85; - } - - for (uint8_t rxRangeIndex = 0; rxRangeIndex < NON_AUX_CHANNEL_COUNT; rxRangeIndex++) { - rxChannelRangeConfig_t *channelRangeConfig = rxChannelRangeConfigsMutable(rxRangeIndex); - - channelRangeConfig->min = 1160; - channelRangeConfig->max = 1840; - } - - batteryConfigMutable()->batteryCapacity = 250; - batteryConfigMutable()->vbatmincellvoltage = 28; - batteryConfigMutable()->vbatwarningcellvoltage = 33; - - *customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R - *customMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R - *customMotorMixerMutable(2) = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L - *customMotorMixerMutable(3) = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L - - vcdProfileMutable()->video_system = VIDEO_SYSTEM_NTSC; - strcpy(pilotConfigMutable()->name, "BeeBrain V2"); - osdConfigMutable()->cap_alarm = 250; - osdConfigMutable()->item_pos[OSD_CRAFT_NAME] = OSD_POS(9, 11) | VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_MAIN_BATT_VOLTAGE] = OSD_POS(23, 10) | VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_ITEM_TIMER_2] = OSD_POS(2, 10) | VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_FLYMODE] = OSD_POS(17, 10) | VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_VTX_CHANNEL] = OSD_POS(10, 10) | VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_RSSI_VALUE] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_ITEM_TIMER_1] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_THROTTLE_POS] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_CROSSHAIRS] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_HORIZON_SIDEBARS] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_ARTIFICIAL_HORIZON] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_CURRENT_DRAW] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_MAH_DRAWN] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_GPS_SPEED] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_GPS_LON] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_GPS_LAT] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_GPS_SATS] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_HOME_DIR] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_HOME_DIST] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_COMPASS_BAR] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_ALTITUDE] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_ROLL_PIDS] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_PITCH_PIDS] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_YAW_PIDS] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_DEBUG] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_POWER] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_PIDRATE_PROFILE] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_WARNINGS] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_AVG_CELL_VOLTAGE] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_PITCH_ANGLE] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_ROLL_ANGLE] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_MAIN_BATT_USAGE] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_DISARMED] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_NUMERICAL_HEADING] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_NUMERICAL_VARIO] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_ESC_TMP] &= ~VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_ESC_RPM] &= ~VISIBLE_FLAG; -} -#endif diff --git a/src/main/target/BEEBRAIN_V2D/target.c b/src/main/target/BEEBRAIN_V2D/target.c deleted file mode 100755 index e7d1c90e4..000000000 --- a/src/main/target/BEEBRAIN_V2D/target.c +++ /dev/null @@ -1,33 +0,0 @@ -/* - * 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 "drivers/io.h" - -#include "drivers/dma.h" -#include "drivers/timer.h" -#include "drivers/timer_def.h" - -const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - DEF_TIM(TIM1, CH3N, PB15, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED ), // PWM1 - PB15 - DMA1_CH6 - *TIM1_CH3N, TIM15_CH1N, TIM15_CH2 - DEF_TIM(TIM15, CH1, PB14, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM2 - PB14 - DMA1_CH5 - TIM1_CH2N, *TIM15_CH1 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM3 - PA8 - DMA1_CH2 - *TIM1_CH1, TIM4_ETR - DEF_TIM(TIM8, CH2N, PB0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_INVERTED ), // PWM4 - PB0 - DMA2_CH5 - TIM3_CH3, TIM1_CH2N, *TIM8_CH2N - DEF_TIM(TIM16, CH1, PB8, TIM_USE_TRANSPONDER, TIMER_OUTPUT_ENABLED ), -}; diff --git a/src/main/target/BEEBRAIN_V2D/target.h b/src/main/target/BEEBRAIN_V2D/target.h deleted file mode 100755 index a794efa0c..000000000 --- a/src/main/target/BEEBRAIN_V2D/target.h +++ /dev/null @@ -1,114 +0,0 @@ -/* - * 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 - -#define TARGET_BOARD_IDENTIFIER "BBV2D" // BeeBrain V2 DSM -#define TARGET_CONFIG - -#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT - -#define BRUSHED_ESC_AUTODETECT - -#define LED0_PIN PB1 -#define LED1_PIN PB2 - -#define USE_EXTI -// #define DEBUG_MPU_DATA_READY_INTERRUPT -#define MPU_INT_EXTI PB6 -#define USE_MPU_DATA_READY_SIGNAL - -#define GYRO -#define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW270_DEG - -#define ACC -#define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG - -#define SERIAL_PORT_COUNT 4 - -#define USE_VCP -#define USE_UART1 -#define USE_UART2 -#define USE_UART3 - -#define USE_MSP_UART - -#define AVOID_UART2_FOR_PWM_PPM - -#define UART1_TX_PIN PA9 -#define UART1_RX_PIN PA10 - -#define UART2_TX_PIN PA2 -#define UART2_RX_PIN PA3 - -#define UART3_TX_PIN PB10 -#define UART3_RX_PIN PB11 - -#define USE_SPI -#define USE_SPI_DEVICE_1 -#define USE_SPI_DEVICE_3 - -#define SPI1_NSS_PIN PA4 -#define SPI1_SCK_PIN PA5 -#define SPI1_MISO_PIN PA6 -#define SPI1_MOSI_PIN PA7 - -#define SPI3_NSS_PIN PA15 -#define SPI3_SCK_PIN PB3 -#define SPI3_MISO_PIN PB4 -#define SPI3_MOSI_PIN PB5 - -#define MPU6500_CS_PIN PA15 -#define MPU6500_SPI_INSTANCE SPI3 - -#define OSD -#define USE_MAX7456 -#define MAX7456_SPI_INSTANCE SPI1 -#define MAX7456_SPI_CS_PIN PA4 - -#define VTX_RTC6705 -#define VTX_RTC6705SOFTSPI -#define VTX_CONTROL -#define RTC6705_SPIDATA_PIN PC15 -#define RTC6705_SPILE_PIN PB12 -#define RTC6705_SPICLK_PIN PC13 - -#define USE_ADC -#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC -#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_NONE -#define ADC_INSTANCE ADC3 -#define VBAT_ADC_PIN PB13 - -#define TRANSPONDER -#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define SERIALRX_UART SERIAL_PORT_USART2 -#define RX_CHANNELS_TAER - -// Receiver - DSM -#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_MOTOR_STOP | FEATURE_OSD) -#define SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048 - -// IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) - -#define USABLE_TIMER_CHANNEL_COUNT 5 -#define USED_TIMERS ( TIM_N(1) | TIM_N(8) | TIM_N(15) | TIM_N(16) ) diff --git a/src/main/target/BEEBRAIN_V2D/target.mk b/src/main/target/BEEBRAIN_V2D/target.mk deleted file mode 100755 index 1f0a08a94..000000000 --- a/src/main/target/BEEBRAIN_V2D/target.mk +++ /dev/null @@ -1,9 +0,0 @@ -F3_TARGETS += $(TARGET) -FEATURES = VCP - -TARGET_SRC = \ - drivers/accgyro/accgyro_mpu.c \ - drivers/accgyro/accgyro_mpu6500.c \ - drivers/accgyro/accgyro_spi_mpu6500.c \ - drivers/vtx_rtc6705_soft_spi.c \ - drivers/max7456.c diff --git a/src/main/target/BEEBRAIN_V2F/BEEBRAIN_V2D.mk b/src/main/target/BEEBRAIN_V2F/BEEBRAIN_V2D.mk new file mode 100755 index 000000000..22dd2d922 --- /dev/null +++ b/src/main/target/BEEBRAIN_V2F/BEEBRAIN_V2D.mk @@ -0,0 +1 @@ +# BEEBRAIN_V2D is a variant of BEEBRAIN_V2F with DSM receiver \ No newline at end of file diff --git a/src/main/target/BEEBRAIN_V2F/config.c b/src/main/target/BEEBRAIN_V2F/config.c index d76111e6b..01b6c95d6 100755 --- a/src/main/target/BEEBRAIN_V2F/config.c +++ b/src/main/target/BEEBRAIN_V2F/config.c @@ -47,7 +47,9 @@ #include "telemetry/telemetry.h" +#if !defined(BEEBRAIN_V2D) #define BBV2_FRSKY_RSSI_CH_IDX 9 +#endif #ifdef BRUSHED_MOTORS_PWM_RATE #undef BRUSHED_MOTORS_PWM_RATE @@ -103,7 +105,7 @@ void targetConfiguration(void) osdConfigMutable()->item_pos[OSD_ITEM_TIMER_2] = OSD_POS(2, 10) | VISIBLE_FLAG; osdConfigMutable()->item_pos[OSD_FLYMODE] = OSD_POS(17, 10) | VISIBLE_FLAG; osdConfigMutable()->item_pos[OSD_VTX_CHANNEL] = OSD_POS(10, 10) | VISIBLE_FLAG; - osdConfigMutable()->item_pos[OSD_RSSI_VALUE] = OSD_POS(2, 11) | VISIBLE_FLAG; + osdConfigMutable()->item_pos[OSD_RSSI_VALUE] &= ~VISIBLE_FLAG; osdConfigMutable()->item_pos[OSD_ITEM_TIMER_1] &= ~VISIBLE_FLAG; osdConfigMutable()->item_pos[OSD_THROTTLE_POS] &= ~VISIBLE_FLAG; osdConfigMutable()->item_pos[OSD_CROSSHAIRS] &= ~VISIBLE_FLAG; @@ -136,11 +138,22 @@ void targetConfiguration(void) osdConfigMutable()->item_pos[OSD_ESC_TMP] &= ~VISIBLE_FLAG; osdConfigMutable()->item_pos[OSD_ESC_RPM] &= ~VISIBLE_FLAG; +#if defined(BEEBRAIN_V2D) + // DSM version + for (uint8_t rxRangeIndex = 0; rxRangeIndex < NON_AUX_CHANNEL_COUNT; rxRangeIndex++) { + rxChannelRangeConfig_t *channelRangeConfig = rxChannelRangeConfigsMutable(rxRangeIndex); + + channelRangeConfig->min = 1160; + channelRangeConfig->max = 1840; + } +#else // Frsky version serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIALRX_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY | FUNCTION_RX_SERIAL; rxConfigMutable()->rssi_channel = BBV2_FRSKY_RSSI_CH_IDX; rxFailsafeChannelConfig_t *channelFailsafeConfig = rxFailsafeChannelConfigsMutable(BBV2_FRSKY_RSSI_CH_IDX - 1); channelFailsafeConfig->mode = RX_FAILSAFE_MODE_SET; channelFailsafeConfig->step = CHANNEL_VALUE_TO_RXFAIL_STEP(1000); + osdConfigMutable()->item_pos[OSD_RSSI_VALUE] = OSD_POS(2, 11) | VISIBLE_FLAG; +#endif } #endif diff --git a/src/main/target/BEEBRAIN_V2F/target.h b/src/main/target/BEEBRAIN_V2F/target.h index 9d9bd06f8..71724ef58 100755 --- a/src/main/target/BEEBRAIN_V2F/target.h +++ b/src/main/target/BEEBRAIN_V2F/target.h @@ -17,7 +17,7 @@ #pragma once -#define TARGET_BOARD_IDENTIFIER "BBV2F" // BeeBrain V2 Frsky +#define TARGET_BOARD_IDENTIFIER "BBV2" // BeeBrain V2. #define TARGET_CONFIG #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT @@ -28,7 +28,6 @@ #define LED1_PIN PB2 #define USE_EXTI -// #define DEBUG_MPU_DATA_READY_INTERRUPT #define MPU_INT_EXTI PB6 #define USE_MPU_DATA_READY_SIGNAL @@ -100,9 +99,15 @@ #define SERIALRX_UART SERIAL_PORT_USART2 #define RX_CHANNELS_TAER -// Receiver - Frsky -#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_MOTOR_STOP | FEATURE_OSD | FEATURE_TELEMETRY) -#define SERIALRX_PROVIDER SERIALRX_SBUS +#if defined(BEEBRAIN_V2D) + // Receiver - DSM + #define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_MOTOR_STOP | FEATURE_OSD) + #define SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048 +#else + // Receiver - Frsky + #define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_MOTOR_STOP | FEATURE_OSD | FEATURE_TELEMETRY) + #define SERIALRX_PROVIDER SERIALRX_SBUS +#endif // IO - stm32f303cc in 48pin package #define TARGET_IO_PORTA 0xffff