From 07cd37468ced20c97efb8ee25d7d3a7b1798dab9 Mon Sep 17 00:00:00 2001 From: Gergely Szell Date: Fri, 29 Jul 2016 10:20:49 +0200 Subject: [PATCH 1/8] Remove SoftSerial2 from X_RACERSPI In the v3.x (SPI) version, one of the previously used soft serial ports have been sacrificed for SPI. Soft serial 1 is available on PWM 5/6. Tested with board v3.0. --- src/main/target/X_RACERSPI/target.h | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index 024a3a6c28..7838b1f460 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -57,8 +57,7 @@ #define USE_UART2 #define USE_UART3 #define USE_SOFTSERIAL1 -#define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 5 +#define SERIAL_PORT_COUNT 4 #define UART1_TX_PIN PA9 #define UART1_RX_PIN PA10 @@ -70,11 +69,8 @@ #define UART3_RX_PIN PB11 // PB11 (AF7) #define SOFTSERIAL_1_TIMER TIM3 -#define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 -#define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 -#define SOFTSERIAL_2_TIMER TIM3 -#define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7 -#define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8 +#define SOFTSERIAL_1_TIMER_RX_HARDWARE 6 // PWM 5 +#define SOFTSERIAL_1_TIMER_TX_HARDWARE 7 // PWM 6 #define USE_I2C From 0e8d375a1c00ef2ffc7cd1c27b25b2a70b784756 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 30 Jul 2016 11:27:04 +1000 Subject: [PATCH 2/8] Update to incorporate hardware revision detection for BJF4 target --- src/main/drivers/flash_m25p16.c | 20 ++++- src/main/drivers/flash_m25p16.h | 3 +- src/main/drivers/io.h | 3 + src/main/main.c | 11 ++- src/main/target/BLUEJAYF4/hardware_revision.c | 86 +++++++++++++++++++ src/main/target/BLUEJAYF4/hardware_revision.h | 30 +++++++ src/main/target/BLUEJAYF4/target.h | 11 ++- src/main/target/BLUEJAYF4/target.mk | 2 +- 8 files changed, 153 insertions(+), 13 deletions(-) create mode 100644 src/main/target/BLUEJAYF4/hardware_revision.c create mode 100644 src/main/target/BLUEJAYF4/hardware_revision.h diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index 29620fbe11..d1e62cccbe 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -196,12 +196,26 @@ static bool m25p16_readIdentification() * Attempts to detect a connected m25p16. If found, true is returned and device capacity can be fetched with * m25p16_getGeometry(). */ -bool m25p16_init() +bool m25p16_init(ioTag_t csTag) { - + /* + if we have already detected a flash device we can simply exit + + TODO: change the init param in favour of flash CFG when ParamGroups work is done + then cs pin can be specified in hardware_revision.c or config.c (dependent on revision). + */ + if (geometry.sectors) { + return true; + } + + if (csTag) { + m25p16CsPin = IOGetByTag(csTag); + } else { #ifdef M25P16_CS_PIN - m25p16CsPin = IOGetByTag(IO_TAG(M25P16_CS_PIN)); + m25p16CsPin = IOGetByTag(IO_TAG(M25P16_CS_PIN)); +#else return false; #endif + } IOInit(m25p16CsPin, OWNER_FLASH, RESOURCE_SPI_CS, 0); IOConfigGPIO(m25p16CsPin, SPI_IO_CS_CFG); diff --git a/src/main/drivers/flash_m25p16.h b/src/main/drivers/flash_m25p16.h index 972fe10bbc..223efa1809 100644 --- a/src/main/drivers/flash_m25p16.h +++ b/src/main/drivers/flash_m25p16.h @@ -19,10 +19,11 @@ #include #include "flash.h" +#include "io.h" #define M25P16_PAGESIZE 256 -bool m25p16_init(); +bool m25p16_init(ioTag_t csTag); void m25p16_eraseSector(uint32_t address); void m25p16_eraseCompletely(); diff --git a/src/main/drivers/io.h b/src/main/drivers/io.h index af9deca810..d655646282 100644 --- a/src/main/drivers/io.h +++ b/src/main/drivers/io.h @@ -11,6 +11,9 @@ typedef uint8_t ioTag_t; // packet tag to specify IO pin typedef void* IO_t; // type specifying IO pin. Currently ioRec_t pointer, but this may change +// NONE initializer for ioTag_t variables +#define IOTAG_NONE ((ioTag_t)0) + // NONE initializer for IO_t variable #define IO_NONE ((IO_t)0) diff --git a/src/main/main.c b/src/main/main.c index 1948a72ac3..7be831d73c 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -179,8 +179,7 @@ void init(void) #ifdef ALIENFLIGHTF3 if (hardwareRevision == AFF3_REV_1) { ledInit(false); - } - else { + } else { ledInit(true); } #else @@ -581,10 +580,10 @@ void init(void) #ifdef USE_FLASHFS #ifdef NAZE if (hardwareRevision == NAZE32_REV5) { - m25p16_init(); + m25p16_init(IOTAG_NONE); } #elif defined(USE_FLASH_M25P16) - m25p16_init(); + m25p16_init(IOTAG_NONE); #endif flashfsInit(); @@ -599,7 +598,11 @@ void init(void) #if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL) // Ensure the SPI Tx DMA doesn't overlap with the led strip +#ifdef STM32F4 + sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_STREAM; +#else sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL; +#endif #else sdcardUseDMA = true; #endif diff --git a/src/main/target/BLUEJAYF4/hardware_revision.c b/src/main/target/BLUEJAYF4/hardware_revision.c new file mode 100644 index 0000000000..6ed07e16f9 --- /dev/null +++ b/src/main/target/BLUEJAYF4/hardware_revision.c @@ -0,0 +1,86 @@ +/* + * 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_config.h" + +#include "drivers/system.h" +#include "drivers/bus_spi.h" +#include "drivers/io.h" +#include "drivers/flash_m25p16.h" +#include "hardware_revision.h" + +static const char * const hardwareRevisionNames[] = { + "Unknown", + "BlueJay rev1", + "BlueJay rev2", + "BlueJay rev3", + "BlueJay rev3a" +}; + +uint8_t hardwareRevision = UNKNOWN; + +void detectHardwareRevision(void) +{ + IO_t pin1 = IOGetByTag(IO_TAG(PB12)); + IOInit(pin1, OWNER_SYSTEM, RESOURCE_INPUT, 1); + IOConfigGPIO(pin1, IOCFG_IPU); + + IO_t pin2 = IOGetByTag(IO_TAG(PB13)); + IOInit(pin2, OWNER_SYSTEM, RESOURCE_INPUT, 2); + IOConfigGPIO(pin2, IOCFG_IPU); + + // Check hardware revision + delayMicroseconds(10); // allow configuration to settle + + /* + if both PB12 and 13 are tied to GND then it is Rev3A (mini) + if only PB12 is tied to GND then it is a Rev3 (full size) + */ + if (!IORead(pin1)) { + if (!IORead(pin2)) { + hardwareRevision = BJF4_REV3A; + return; + } + hardwareRevision = BJF4_REV3; + return; + } + + hardwareRevision = BJF4_REV2; +} + +void updateHardwareRevision(void) +{ + if (hardwareRevision != BJF4_REV2) { + return; + } + + /* + if flash exists on PB3 then Rev1 + */ + if (m25p16_init(IO_TAG(PB3))) { + hardwareRevision = BJF4_REV1; + } else { + IOInit(IOGetByTag(IO_TAG(PB3)), OWNER_FREE, RESOURCE_NONE, 0); + } +} + + diff --git a/src/main/target/BLUEJAYF4/hardware_revision.h b/src/main/target/BLUEJAYF4/hardware_revision.h new file mode 100644 index 0000000000..3333d86aa0 --- /dev/null +++ b/src/main/target/BLUEJAYF4/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 bjf4HardwareRevision_t { + UNKNOWN = 0, + BJF4_REV1, // Flash + BJF4_REV2, // SDCard + BJF4_REV3, // SDCard + Flash + BJF4_REV3A, // Flash (20x20 mini format) +} bjf4HardwareRevision_e; + +extern uint8_t hardwareRevision; + +void updateHardwareRevision(void); +void detectHardwareRevision(void); \ No newline at end of file diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 34bdcf8aa7..cea7bb5b46 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -22,6 +22,9 @@ #define USBD_PRODUCT_STRING "BlueJayF4" +#define USE_HARDWARE_REVISION_DETECTION +#define HW_PIN PB2 + #define BOARD_HAS_VOLTAGE_DIVIDER #define USE_EXTI @@ -76,11 +79,11 @@ // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING -//#define M25P16_CS_PIN PB3 -//#define M25P16_SPI_INSTANCE SPI3 +#define M25P16_CS_PIN PB7 +#define M25P16_SPI_INSTANCE SPI3 -//#define USE_FLASHFS -//#define USE_FLASH_M25P16 +#define USE_FLASHFS +#define USE_FLASH_M25P16 #define USABLE_TIMER_CHANNEL_COUNT 7 diff --git a/src/main/target/BLUEJAYF4/target.mk b/src/main/target/BLUEJAYF4/target.mk index 23dd74507a..d0a982faad 100644 --- a/src/main/target/BLUEJAYF4/target.mk +++ b/src/main/target/BLUEJAYF4/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += SDCARD VCP +FEATURES += SDCARD VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro_spi_mpu6500.c \ From 0a9beee25ddef62cbe902f4d3cb3c93bfd4ba21f Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 30 Jul 2016 12:02:07 +1000 Subject: [PATCH 3/8] Added support to turn inversion off for UART1 will need to look at controlling it within configuration once param groups are in place. --- src/main/drivers/inverter.c | 9 +++++++-- src/main/target/BLUEJAYF4/hardware_revision.c | 19 ++++++++++++++++--- 2 files changed, 23 insertions(+), 5 deletions(-) diff --git a/src/main/drivers/inverter.c b/src/main/drivers/inverter.c index 967c87680a..336f850865 100644 --- a/src/main/drivers/inverter.c +++ b/src/main/drivers/inverter.c @@ -27,11 +27,16 @@ #include "inverter.h" -static const IO_t pin = DEFIO_IO(INVERTER); +/* + TODO: move this to support multiple inverters on different UARTs etc + possibly move to put it in the UART driver itself. +*/ +static IO_t pin = IO_NONE; void initInverter(void) { - IOInit(pin, OWNER_INVERTER, RESOURCE_OUTPUT, 0); + pin = IOGetByTag(IO_TAG(INVERTER)); + IOInit(pin, OWNER_INVERTER, RESOURCE_OUTPUT, 1); IOConfigGPIO(pin, IOCFG_OUT_PP); inverterSet(false); diff --git a/src/main/target/BLUEJAYF4/hardware_revision.c b/src/main/target/BLUEJAYF4/hardware_revision.c index 6ed07e16f9..3162e9f487 100644 --- a/src/main/target/BLUEJAYF4/hardware_revision.c +++ b/src/main/target/BLUEJAYF4/hardware_revision.c @@ -58,13 +58,26 @@ void detectHardwareRevision(void) if (!IORead(pin1)) { if (!IORead(pin2)) { hardwareRevision = BJF4_REV3A; - return; } hardwareRevision = BJF4_REV3; - return; } - hardwareRevision = BJF4_REV2; + if (hardwareRevision == UNKNOWN) { + hardwareRevision = BJF4_REV2; + return; + } + + /* + enable the UART1 inversion PC9 + + TODO: once param groups are in place, inverter outputs + can be moved to be simple IO outputs, and merely set them + HI or LO in configuration. + */ + IO_t uart1invert = IOGetByTag(IO_TAG(PC9)); + IOInit(uart1invert, OWNER_INVERTER, RESOURCE_OUTPUT, 2); + IOConfigGPIO(uart1invert, IOCFG_AF_PP); + IOLo(uart1invert); } void updateHardwareRevision(void) From 552e32a287bba811bb2b3799b0f218b539685d7b Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 30 Jul 2016 13:50:09 +1000 Subject: [PATCH 4/8] minor formatting --- src/main/target/ALIENFLIGHTF1/config.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/target/ALIENFLIGHTF1/config.c b/src/main/target/ALIENFLIGHTF1/config.c index 411f4e4154..b5bea8104a 100644 --- a/src/main/target/ALIENFLIGHTF1/config.c +++ b/src/main/target/ALIENFLIGHTF1/config.c @@ -76,7 +76,8 @@ #include "config/config_master.h" // alternative defaults settings for AlienFlight targets -void targetConfiguration(void) { +void targetConfiguration(void) +{ featureClear(FEATURE_ONESHOT125); masterConfig.rxConfig.spektrum_sat_bind = 5; masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; From 49f9eec1e9abf64ceaa8fa93c50a7ee47c28d646 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 30 Jul 2016 14:28:23 +1000 Subject: [PATCH 5/8] Gyro on Rev3 and Rev3a Bluejay are 0 aligned --- src/main/config/config.c | 6 +-- src/main/drivers/flash_m25p16.c | 3 +- src/main/target/BLUEJAYF4/config.c | 87 ++++++++++++++++++++++++++++++ src/main/target/BLUEJAYF4/target.h | 5 +- src/main/target/common.h | 2 + 5 files changed, 95 insertions(+), 8 deletions(-) create mode 100644 src/main/target/BLUEJAYF4/config.c diff --git a/src/main/config/config.c b/src/main/config/config.c index b0271ef278..4e16b8c755 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -530,11 +530,7 @@ static void resetConf(void) masterConfig.rxConfig.rcSmoothing = RC_SMOOTHING_OFF; masterConfig.rxConfig.rcSmoothInterval = 9; masterConfig.rxConfig.fpvCamAngleDegrees = 0; -#ifdef STM32F4 - masterConfig.rxConfig.max_aux_channel = 99; -#else - masterConfig.rxConfig.max_aux_channel = 6; -#endif + masterConfig.rxConfig.max_aux_channel = MAX_AUX_CHANNELS; masterConfig.rxConfig.airModeActivateThreshold = 1350; resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges); diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index d1e62cccbe..0ab3d6122b 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -213,7 +213,8 @@ bool m25p16_init(ioTag_t csTag) } else { #ifdef M25P16_CS_PIN m25p16CsPin = IOGetByTag(IO_TAG(M25P16_CS_PIN)); -#else return false; +#else + return false; #endif } IOInit(m25p16CsPin, OWNER_FLASH, RESOURCE_SPI_CS, 0); diff --git a/src/main/target/BLUEJAYF4/config.c b/src/main/target/BLUEJAYF4/config.c new file mode 100644 index 0000000000..71007bb806 --- /dev/null +++ b/src/main/target/BLUEJAYF4/config.c @@ -0,0 +1,87 @@ +/* + * 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 "build_config.h" + +#include "blackbox/blackbox_io.h" + +#include "common/color.h" +#include "common/axis.h" +#include "common/filter.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" +#include "drivers/system.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" +#include "drivers/serial.h" +#include "drivers/pwm_output.h" +#include "drivers/max7456.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/compass.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/boardalignment.h" +#include "sensors/battery.h" + +#include "io/beeper.h" +#include "io/serial.h" +#include "io/gimbal.h" +#include "io/escservo.h" +#include "io/rc_controls.h" +#include "io/rc_curves.h" +#include "io/ledstrip.h" +#include "io/gps.h" +#include "io/osd.h" +#include "io/vtx.h" + +#include "rx/rx.h" + +#include "telemetry/telemetry.h" + +#include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/failsafe.h" +#include "flight/altitudehold.h" +#include "flight/navigation.h" + +#include "config/runtime_config.h" +#include "config/config.h" + +#include "config/config_profile.h" +#include "config/config_master.h" + +#include "hardware_revision.h" + +// alternative defaults settings for BlueJayF4 targets +void targetConfiguration(void) +{ + if (hardwareRevision == BJF4_REV1 || hardwareRevision == BJF4_REV2) { + masterConfig.sensorAlignmentConfig.gyro_align = CW180_DEG; + masterConfig.sensorAlignmentConfig.acc_align = CW180_DEG; + } +} diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index cea7bb5b46..39cc723a64 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -17,6 +17,7 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "BJF4" +#define TARGET_CONFIG #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) @@ -44,12 +45,12 @@ #define ACC #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW180_DEG +#define ACC_MPU6500_ALIGN CW0_DEG #define GYRO #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW180_DEG +#define GYRO_MPU6500_ALIGN CW0_DEG //#define MAG //#define USE_MAG_AK8963 diff --git a/src/main/target/common.h b/src/main/target/common.h index 65ab02c871..0e74b56c89 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -24,6 +24,7 @@ /* STM32F4 specific settings that apply to all F4 targets */ #ifdef STM32F4 +#define MAX_AUX_CHANNELS 99 #define TASK_GYROPID_DESIRED_PERIOD 125 #define SCHEDULER_DELAY_LIMIT 10 #define USE_SLOW_SERIAL_CLI @@ -31,6 +32,7 @@ #else /* when not an F4 */ +#define MAX_AUX_CHANNELS 6 #define TASK_GYROPID_DESIRED_PERIOD 1000 #define SCHEDULER_DELAY_LIMIT 100 From df11d398aab1dd5cab9dca3609ea76c26c128752 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 31 Jul 2016 11:06:29 +1000 Subject: [PATCH 6/8] Warning removal where present if pidBetaflight is skipped. --- src/main/flight/pid.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 16aa6e45ac..4b916911d9 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -66,9 +66,10 @@ uint8_t PIDweight[3]; static int32_t errorGyroI[3]; static float errorGyroIf[3]; -#ifdef SKIP_PID_FLOAT + static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig); +#ifdef SKIP_PID_FLOAT pidControllerFuncPtr pid_controller = pidLegacy; // which pid controller are we using #else static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, @@ -107,6 +108,7 @@ const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; static pt1Filter_t deltaFilter[3]; static pt1Filter_t yawFilter; +#ifndef SKIP_PID_FLOAT // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. Based on 2DOF reference design (matlab) static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) @@ -302,6 +304,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc #endif } } +#endif // Legacy pid controller betaflight evolved pid rewrite based on 2.9 releae. Good for fastest cycletimes for those who believe in that. Don't expect much development in the future static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, From 1b73a8f2aaf7ef3972520e8d5ab0d0ddd395601b Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 31 Jul 2016 11:20:27 +1000 Subject: [PATCH 7/8] Whitespace cleanup and simplification of delay for serial cli F4 support --- src/main/io/serial_cli.c | 40 ++++++++++------------------------------ 1 file changed, 10 insertions(+), 30 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 9ac1946b19..87fd199559 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -826,7 +826,7 @@ const clivalue_t valueTable[] = { { "pid_max_velocity", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocity, .config.minmax = {0, 2000 } }, { "pid_max_velocity_yaw", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocityYaw, .config.minmax = {0, 2000 } }, - { "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, + { "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, { "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } }, { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } }, @@ -2009,11 +2009,6 @@ static void dumpValues(uint16_t valueSection) cliPrintf("set %s = ", valueTable[i].name); cliPrintVar(value, 0); cliPrint("\r\n"); - -#ifdef USE_SLOW_SERIAL_CLI - delay(2); -#endif - } } @@ -2058,7 +2053,7 @@ static void cliDump(char *cmdline) cliPrint("\r\n# version\r\n"); cliVersion(NULL); - printSectionBreak(); + printSectionBreak(); cliPrint("\r\n# name\r\n"); cliName(NULL); @@ -2089,9 +2084,6 @@ static void cliDump(char *cmdline) if (yaw < 0) cliWrite(' '); cliPrintf("%s\r\n", ftoa(yaw, buf)); -#ifdef USE_SLOW_SERIAL_CLI - delay(2); -#endif } #ifdef USE_SERVOS @@ -2113,10 +2105,6 @@ static void cliDump(char *cmdline) masterConfig.customServoMixer[i].max, masterConfig.customServoMixer[i].box ); - -#ifdef USE_SLOW_SERIAL_CLI - delay(2); -#endif } #endif @@ -2128,18 +2116,12 @@ static void cliDump(char *cmdline) if (featureNames[i] == NULL) break; cliPrintf("feature -%s\r\n", featureNames[i]); -#ifdef USE_SLOW_SERIAL_CLI - delay(2); -#endif } for (i = 0; ; i++) { // reenable what we want. if (featureNames[i] == NULL) break; if (mask & (1 << i)) cliPrintf("feature %s\r\n", featureNames[i]); -#ifdef USE_SLOW_SERIAL_CLI - delay(2); -#endif } #ifdef BEEPER @@ -2194,9 +2176,6 @@ static void cliDump(char *cmdline) for (channel = 0; channel < INPUT_SOURCE_COUNT; channel++) { if (servoDirection(i, channel) < 0) { cliPrintf("smix reverse %d %d r\r\n", i , channel); -#ifdef USE_SLOW_SERIAL_CLI - delay(2); -#endif } } } @@ -2227,9 +2206,6 @@ static void cliDump(char *cmdline) cliPrint("\r\n# restore original rateprofile selection\r\n"); changeControlRateProfile(currentRateIndex); cliRateProfile(""); -#ifdef USE_SLOW_SERIAL_CLI - delay(2); -#endif } cliPrint("\r\n# restore original profile selection\r\n"); @@ -2710,6 +2686,10 @@ static void cliPrint(const char *str) { while (*str) bufWriterAppend(cliWriter, *str++); + +#ifdef USE_SLOW_SERIAL_CLI + delay(1); +#endif } static void cliPutp(void *p, char ch) @@ -2723,6 +2703,10 @@ static void cliPrintf(const char *fmt, ...) va_start(va, fmt); tfp_format(cliWriter, cliPutp, fmt, va); va_end(va); + +#ifdef USE_SLOW_SERIAL_CLI + delay(1); +#endif } static void cliWrite(uint8_t ch) @@ -2856,10 +2840,6 @@ static void cliSet(char *cmdline) 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 cliPrint("\r\n"); - -#ifdef USE_SLOW_SERIAL_CLI - delay(2); -#endif } } else if ((eqptr = strstr(cmdline, "=")) != NULL) { // has equals From 53cefb9d1449fd1a053b1c6790749962aacac670 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 31 Jul 2016 15:39:26 +1000 Subject: [PATCH 8/8] Fixed beeper on BJF4 rev3 --- src/main/main.c | 6 ++++++ src/main/target/BLUEJAYF4/target.h | 3 ++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/main.c b/src/main/main.c index 7be831d73c..79e415c1c2 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -362,6 +362,12 @@ void init(void) beeperConfig.isInverted = true; } #endif +/* temp until PGs are implemented. */ +#ifdef BLUEJAYF4 + if (hardwareRevision <= BJF4_REV2) { + beeperConfig.ioTag = IO_TAG(BEEPER_OPT); + } +#endif #ifdef CC3D if (masterConfig.use_buzzer_p6 == 1) beeperConfig.ioTag = IO_TAG(BEEPER_OPT); diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 39cc723a64..73f7f86c99 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -33,7 +33,8 @@ #define LED1 PB5 #define LED2 PB4 -#define BEEPER PB7 +#define BEEPER PC1 +#define BEEPER_OPT PB7 #define BEEPER_INVERTED #define INVERTER PB15