From 2f65e0d7f72e43b93e007c605531800d52215716 Mon Sep 17 00:00:00 2001 From: Maxim Khomenko Date: Sun, 6 Mar 2016 15:28:26 +0200 Subject: [PATCH 01/12] Some reworking of INDICATOR's and RING's functionality. * RING: phase start and direction of animation now depends on LED's X:Y coordinates on the grid (X+Y) * INDICATOR: it works only at LEDs with defined direction (North, South, East, West). Also the colors are changed: Orange for turns, Blue for acceleration, Red for decceleration. --- src/main/io/ledstrip.c | 112 ++++++++++++----------------------------- 1 file changed, 31 insertions(+), 81 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 701d29a0a5..1fd4bae7a7 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -558,42 +558,6 @@ void applyDirectionalModeColor(const uint8_t ledIndex, const ledConfig_t *ledCon } -typedef enum { - QUADRANT_NORTH_EAST = 1, - QUADRANT_SOUTH_EAST, - QUADRANT_SOUTH_WEST, - QUADRANT_NORTH_WEST -} quadrant_e; - -void applyQuadrantColor(const uint8_t ledIndex, const ledConfig_t *ledConfig, const quadrant_e quadrant, const hsvColor_t *color) -{ - switch (quadrant) { - case QUADRANT_NORTH_EAST: - if (GET_LED_Y(ledConfig) <= highestYValueForNorth && GET_LED_X(ledConfig) >= lowestXValueForEast) { - setLedHsv(ledIndex, color); - } - return; - - case QUADRANT_SOUTH_EAST: - if (GET_LED_Y(ledConfig) >= lowestYValueForSouth && GET_LED_X(ledConfig) >= lowestXValueForEast) { - setLedHsv(ledIndex, color); - } - return; - - case QUADRANT_SOUTH_WEST: - if (GET_LED_Y(ledConfig) >= lowestYValueForSouth && GET_LED_X(ledConfig) <= highestXValueForWest) { - setLedHsv(ledIndex, color); - } - return; - - case QUADRANT_NORTH_WEST: - if (GET_LED_Y(ledConfig) <= highestYValueForNorth && GET_LED_X(ledConfig) <= highestXValueForWest) { - setLedHsv(ledIndex, color); - } - return; - } -} - void applyLedModeLayer(void) { const ledConfig_t *ledConfig; @@ -713,18 +677,24 @@ void applyLedWarningLayer(uint8_t updateNow) void applyLedIndicatorLayer(uint8_t indicatorFlashState) { const ledConfig_t *ledConfig; - static const hsvColor_t *flashColor; + static const hsvColor_t *turnColor; + static const hsvColor_t *accColor; + static const hsvColor_t *decColor; if (!rxIsReceivingSignal()) { return; } if (indicatorFlashState == 0) { - flashColor = &hsv_orange; + turnColor = &hsv_orange; + accColor = &hsv_blue; + decColor = &hsv_red; } else { - flashColor = &hsv_black; + turnColor = &hsv_black; + accColor = &hsv_black; + decColor = &hsv_black; } - + uint8_t ledIndex; for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { @@ -734,25 +704,31 @@ void applyLedIndicatorLayer(uint8_t indicatorFlashState) if (!(ledConfig->flags & LED_FUNCTION_INDICATOR)) { continue; } - + if (rcCommand[ROLL] > INDICATOR_DEADBAND) { - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_EAST, flashColor); - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_EAST, flashColor); + if ((ledConfig->flags & LED_DIRECTION_EAST)) { + setLedHsv(ledIndex, turnColor); + continue; + } } if (rcCommand[ROLL] < -INDICATOR_DEADBAND) { - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_WEST, flashColor); - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_WEST, flashColor); + if ((ledConfig->flags & LED_DIRECTION_WEST)) { + setLedHsv(ledIndex, turnColor); + continue; + } } if (rcCommand[PITCH] > INDICATOR_DEADBAND) { - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_EAST, flashColor); - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_WEST, flashColor); + if ((ledConfig->flags & LED_DIRECTION_NORTH)) { + setLedHsv(ledIndex, accColor); + } } if (rcCommand[PITCH] < -INDICATOR_DEADBAND) { - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_EAST, flashColor); - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_WEST, flashColor); + if ((ledConfig->flags & LED_DIRECTION_SOUTH)) { + setLedHsv(ledIndex, decColor); + } } } } @@ -790,11 +766,8 @@ void applyLedThrustRingLayer(void) uint8_t ledIndex; // initialised to special value instead of using more memory for a flag. - static uint8_t rotationSeqLedCount = RING_PATTERN_NOT_CALCULATED; static uint8_t rotationPhase = ROTATION_SEQUENCE_LED_COUNT; - bool nextLedOn = false; - uint8_t ledRingIndex = 0; for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { ledConfig = &ledConfigs[ledIndex]; @@ -803,16 +776,15 @@ void applyLedThrustRingLayer(void) continue; } + uint8_t ledRingIndex = GET_LED_X(ledConfig) + GET_LED_Y(ledConfig); bool applyColor = false; + if (ARMING_FLAG(ARMED)) { - if ((ledRingIndex + rotationPhase) % rotationSeqLedCount < ROTATION_SEQUENCE_LED_WIDTH) { + if ((ledRingIndex + rotationPhase) % ROTATION_SEQUENCE_LED_COUNT < ROTATION_SEQUENCE_LED_WIDTH) { applyColor = true; } } else { - if (nextLedOn == false) { - applyColor = true; - } - nextLedOn = !nextLedOn; + applyColor = (ledRingIndex % 2) == 0; } if (applyColor) { @@ -822,33 +794,11 @@ void applyLedThrustRingLayer(void) } setLedHsv(ledIndex, &ringColor); - - ledRingIndex++; } - - uint8_t ledRingLedCount = ledRingIndex; - if (rotationSeqLedCount == RING_PATTERN_NOT_CALCULATED) { - // update ring pattern according to total number of ring leds found - - rotationSeqLedCount = ledRingLedCount; - - // try to split in segments/rings of exactly ROTATION_SEQUENCE_LED_COUNT leds - if ((ledRingLedCount % ROTATION_SEQUENCE_LED_COUNT) == 0) { - rotationSeqLedCount = ROTATION_SEQUENCE_LED_COUNT; - } else { - // else split up in equal segments/rings of at most ROTATION_SEQUENCE_LED_COUNT leds - while ((rotationSeqLedCount > ROTATION_SEQUENCE_LED_COUNT) && ((rotationSeqLedCount % 2) == 0)) { - rotationSeqLedCount >>= 1; - } - } - - // trigger start over - rotationPhase = 1; - } - + rotationPhase--; if (rotationPhase == 0) { - rotationPhase = rotationSeqLedCount; + rotationPhase = ROTATION_SEQUENCE_LED_COUNT; } } From 7f96eafc2d4291d0a7648b61cf66537c8433af16 Mon Sep 17 00:00:00 2001 From: Maxim Khomenko Date: Mon, 8 Aug 2016 23:37:34 +0300 Subject: [PATCH 02/12] Revert "Some reworking of INDICATOR's and RING's functionality." This reverts commit 2f65e0d7f72e43b93e007c605531800d52215716. --- src/main/io/ledstrip.c | 112 +++++++++++++++++++++++++++++------------ 1 file changed, 81 insertions(+), 31 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 1fd4bae7a7..701d29a0a5 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -558,6 +558,42 @@ void applyDirectionalModeColor(const uint8_t ledIndex, const ledConfig_t *ledCon } +typedef enum { + QUADRANT_NORTH_EAST = 1, + QUADRANT_SOUTH_EAST, + QUADRANT_SOUTH_WEST, + QUADRANT_NORTH_WEST +} quadrant_e; + +void applyQuadrantColor(const uint8_t ledIndex, const ledConfig_t *ledConfig, const quadrant_e quadrant, const hsvColor_t *color) +{ + switch (quadrant) { + case QUADRANT_NORTH_EAST: + if (GET_LED_Y(ledConfig) <= highestYValueForNorth && GET_LED_X(ledConfig) >= lowestXValueForEast) { + setLedHsv(ledIndex, color); + } + return; + + case QUADRANT_SOUTH_EAST: + if (GET_LED_Y(ledConfig) >= lowestYValueForSouth && GET_LED_X(ledConfig) >= lowestXValueForEast) { + setLedHsv(ledIndex, color); + } + return; + + case QUADRANT_SOUTH_WEST: + if (GET_LED_Y(ledConfig) >= lowestYValueForSouth && GET_LED_X(ledConfig) <= highestXValueForWest) { + setLedHsv(ledIndex, color); + } + return; + + case QUADRANT_NORTH_WEST: + if (GET_LED_Y(ledConfig) <= highestYValueForNorth && GET_LED_X(ledConfig) <= highestXValueForWest) { + setLedHsv(ledIndex, color); + } + return; + } +} + void applyLedModeLayer(void) { const ledConfig_t *ledConfig; @@ -677,24 +713,18 @@ void applyLedWarningLayer(uint8_t updateNow) void applyLedIndicatorLayer(uint8_t indicatorFlashState) { const ledConfig_t *ledConfig; - static const hsvColor_t *turnColor; - static const hsvColor_t *accColor; - static const hsvColor_t *decColor; + static const hsvColor_t *flashColor; if (!rxIsReceivingSignal()) { return; } if (indicatorFlashState == 0) { - turnColor = &hsv_orange; - accColor = &hsv_blue; - decColor = &hsv_red; + flashColor = &hsv_orange; } else { - turnColor = &hsv_black; - accColor = &hsv_black; - decColor = &hsv_black; + flashColor = &hsv_black; } - + uint8_t ledIndex; for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { @@ -704,31 +734,25 @@ void applyLedIndicatorLayer(uint8_t indicatorFlashState) if (!(ledConfig->flags & LED_FUNCTION_INDICATOR)) { continue; } - + if (rcCommand[ROLL] > INDICATOR_DEADBAND) { - if ((ledConfig->flags & LED_DIRECTION_EAST)) { - setLedHsv(ledIndex, turnColor); - continue; - } + applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_EAST, flashColor); + applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_EAST, flashColor); } if (rcCommand[ROLL] < -INDICATOR_DEADBAND) { - if ((ledConfig->flags & LED_DIRECTION_WEST)) { - setLedHsv(ledIndex, turnColor); - continue; - } + applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_WEST, flashColor); + applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_WEST, flashColor); } if (rcCommand[PITCH] > INDICATOR_DEADBAND) { - if ((ledConfig->flags & LED_DIRECTION_NORTH)) { - setLedHsv(ledIndex, accColor); - } + applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_EAST, flashColor); + applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_WEST, flashColor); } if (rcCommand[PITCH] < -INDICATOR_DEADBAND) { - if ((ledConfig->flags & LED_DIRECTION_SOUTH)) { - setLedHsv(ledIndex, decColor); - } + applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_EAST, flashColor); + applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_WEST, flashColor); } } } @@ -766,8 +790,11 @@ void applyLedThrustRingLayer(void) uint8_t ledIndex; // initialised to special value instead of using more memory for a flag. + static uint8_t rotationSeqLedCount = RING_PATTERN_NOT_CALCULATED; static uint8_t rotationPhase = ROTATION_SEQUENCE_LED_COUNT; + bool nextLedOn = false; + uint8_t ledRingIndex = 0; for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { ledConfig = &ledConfigs[ledIndex]; @@ -776,15 +803,16 @@ void applyLedThrustRingLayer(void) continue; } - uint8_t ledRingIndex = GET_LED_X(ledConfig) + GET_LED_Y(ledConfig); bool applyColor = false; - if (ARMING_FLAG(ARMED)) { - if ((ledRingIndex + rotationPhase) % ROTATION_SEQUENCE_LED_COUNT < ROTATION_SEQUENCE_LED_WIDTH) { + if ((ledRingIndex + rotationPhase) % rotationSeqLedCount < ROTATION_SEQUENCE_LED_WIDTH) { applyColor = true; } } else { - applyColor = (ledRingIndex % 2) == 0; + if (nextLedOn == false) { + applyColor = true; + } + nextLedOn = !nextLedOn; } if (applyColor) { @@ -794,11 +822,33 @@ void applyLedThrustRingLayer(void) } setLedHsv(ledIndex, &ringColor); + + ledRingIndex++; } - + + uint8_t ledRingLedCount = ledRingIndex; + if (rotationSeqLedCount == RING_PATTERN_NOT_CALCULATED) { + // update ring pattern according to total number of ring leds found + + rotationSeqLedCount = ledRingLedCount; + + // try to split in segments/rings of exactly ROTATION_SEQUENCE_LED_COUNT leds + if ((ledRingLedCount % ROTATION_SEQUENCE_LED_COUNT) == 0) { + rotationSeqLedCount = ROTATION_SEQUENCE_LED_COUNT; + } else { + // else split up in equal segments/rings of at most ROTATION_SEQUENCE_LED_COUNT leds + while ((rotationSeqLedCount > ROTATION_SEQUENCE_LED_COUNT) && ((rotationSeqLedCount % 2) == 0)) { + rotationSeqLedCount >>= 1; + } + } + + // trigger start over + rotationPhase = 1; + } + rotationPhase--; if (rotationPhase == 0) { - rotationPhase = ROTATION_SEQUENCE_LED_COUNT; + rotationPhase = rotationSeqLedCount; } } From e9f99462195336ca7594ef3e13354de5e54bc6c9 Mon Sep 17 00:00:00 2001 From: Maxim Khomenko Date: Tue, 9 Aug 2016 00:15:44 +0300 Subject: [PATCH 03/12] Target for I-Shaped F3 FC from BG: http://www.banggood.com/I-shaped-F3-10DF-Flight-Controller-5V-2A-BEC-Bent-Straight-Pins-For-RC-Multirotors-p-1047005.html --- src/main/target/ISHAPEDF3/target.c | 121 +++++++++++++++++++++++++++ src/main/target/ISHAPEDF3/target.h | 122 ++++++++++++++++++++++++++++ src/main/target/ISHAPEDF3/target.mk | 11 +++ 3 files changed, 254 insertions(+) create mode 100644 src/main/target/ISHAPEDF3/target.c create mode 100644 src/main/target/ISHAPEDF3/target.h create mode 100644 src/main/target/ISHAPEDF3/target.mk diff --git a/src/main/target/ISHAPEDF3/target.c b/src/main/target/ISHAPEDF3/target.c new file mode 100644 index 0000000000..ec9176a27f --- /dev/null +++ b/src/main/target/ISHAPEDF3/target.c @@ -0,0 +1,121 @@ +/* + * 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/pwm_mapping.h" + +const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM8 | (MAP_TO_PWM_INPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), + PWM13 | (MAP_TO_SERVO_OUTPUT << 8), + PWM14 | (MAP_TO_SERVO_OUTPUT << 8), + PWM15 | (MAP_TO_SERVO_OUTPUT << 8), + PWM16 | (MAP_TO_SERVO_OUTPUT << 8), + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // servo #10 + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM8 | (MAP_TO_PWM_INPUT << 8), // input #8 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), + PWM13 | (MAP_TO_SERVO_OUTPUT << 8), + PWM14 | (MAP_TO_SERVO_OUTPUT << 8), + PWM15 | (MAP_TO_SERVO_OUTPUT << 8), + PWM16 | (MAP_TO_SERVO_OUTPUT << 8), // server #6 + 0xFFFF +}; + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + // Production boards swapped RC_CH3/4 swapped to make it easier to use SerialRX using supplied cables - compared to first prototype. + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 + + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP +}; + diff --git a/src/main/target/ISHAPEDF3/target.h b/src/main/target/ISHAPEDF3/target.h new file mode 100644 index 0000000000..5efad550fd --- /dev/null +++ b/src/main/target/ISHAPEDF3/target.h @@ -0,0 +1,122 @@ +/* + * 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 "ISF3" + +#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE + +#define LED0 PB3 + +#define BEEPER PC15 +#define BEEPER_INVERTED + +#define EXTI15_10_CALLBACK_HANDLER_COUNT 1 + +#define USE_EXTI +#define MPU_INT_EXTI PC13 +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + +#define USE_MAG_DATA_READY_SIGNAL +#define ENSURE_MAG_DATA_READY_IS_HIGH + +#define GYRO +#define USE_GYRO_MPU6500 +//#define USE_GYRO_SPI_MPU6500 + +#define ACC +#define USE_ACC_MPU6500 +//#define USE_ACC_SPI_MPU6500 + +#define BARO +#define USE_BARO_BMP280 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 + +#define SONAR +#define SONAR_TRIGGER_PIN PB0 +#define SONAR_ECHO_PIN PB1 + +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_SOFTSERIAL1 +#define USE_SOFTSERIAL2 +#define SERIAL_PORT_COUNT 5 + +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define UART2_TX_PIN PA14 // PA14 / SWCLK +#define UART2_RX_PIN PA15 + +#define UART3_TX_PIN PB10 // PB10 (AF7) +#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 USE_I2C +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA + +#define USE_SPI +#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 + +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_INSTANCE SPI2 + +#define BOARD_HAS_VOLTAGE_DIVIDER +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 + +#define LED_STRIP +#define WS2811_PIN PA8 +#define WS2811_TIMER TIM1 +#define WS2811_DMA_CHANNEL DMA1_Channel2 +#define WS2811_IRQ DMA1_Channel2_IRQn +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER + +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES FEATURE_BLACKBOX + +#define SPEKTRUM_BIND +// USART3, +#define BIND_PIN PB11 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +// 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(3)|BIT(4)) + +#define USABLE_TIMER_CHANNEL_COUNT 17 +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) + + diff --git a/src/main/target/ISHAPEDF3/target.mk b/src/main/target/ISHAPEDF3/target.mk new file mode 100644 index 0000000000..94676e07dc --- /dev/null +++ b/src/main/target/ISHAPEDF3/target.mk @@ -0,0 +1,11 @@ +F3_TARGETS += $(TARGET) +FEATURES = ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6500.c \ + drivers/barometer_bmp280.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_softserial.c \ + drivers/sonar_hcsr04.c From f823a815406d2d931363bc5a7d38e197a91b1ff8 Mon Sep 17 00:00:00 2001 From: blckmn Date: Fri, 19 Aug 2016 22:51:59 +1000 Subject: [PATCH 04/12] Small PPM fixes and updates to REVO --- src/main/drivers/pwm_mapping.c | 20 +++++++++++--------- src/main/target/REVO/target.c | 11 ++++++----- 2 files changed, 17 insertions(+), 14 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 1466aa2d25..bc9336cbee 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -109,7 +109,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) #else setup = hardwareMaps[i]; #endif - + TIM_TypeDef* ppmTimer = NULL; for (i = 0; i < USABLE_TIMER_CHANNEL_COUNT && setup[i] != 0xFFFF; i++) { uint8_t timerIndex = setup[i] & 0x00FF; uint8_t type = (setup[i] & 0xFF00) >> 8; @@ -162,13 +162,13 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) } #endif -#ifdef RSSI_ADC_GPIO +#ifdef RSSI_ADC_PIN if (init->useRSSIADC && timerHardwarePtr->tag == IO_TAG(RSSI_ADC_PIN)) { continue; } #endif -#ifdef CURRENT_METER_ADC_GPIO +#ifdef CURRENT_METER_ADC_PIN if (init->useCurrentMeterADC && timerHardwarePtr->tag == IO_TAG(CURRENT_METER_ADC_PIN)) { continue; } @@ -292,11 +292,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) if (type == MAP_TO_PPM_INPUT) { #ifndef SKIP_RX_PWM_PPM -#if defined(SPARKY) || defined(ALIENFLIGHTF3) - if (!(init->pwmProtocolType == PWM_TYPE_CONVENTIONAL)) { - ppmAvoidPWMTimerClash(timerHardwarePtr, TIM2, init->pwmProtocolType); - } -#endif + ppmTimer = timerHardwarePtr->tim; ppmInConfig(timerHardwarePtr); #endif } else if (type == MAP_TO_PWM_INPUT) { @@ -313,9 +309,15 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) continue; } #endif + if (init->usePPM) { + if (init->pwmProtocolType != PWM_TYPE_CONVENTIONAL && timerHardwarePtr->tim == ppmTimer) { + ppmAvoidPWMTimerClash(timerHardwarePtr, ppmTimer, init->pwmProtocolType); + } + } + if (init->useFastPwm) { pwmFastPwmMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse, init->pwmProtocolType); - pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM | PWM_PF_OUTPUT_PROTOCOL_ONESHOT; + pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM | PWM_PF_OUTPUT_PROTOCOL_ONESHOT; } else if (init->pwmProtocolType == PWM_TYPE_BRUSHED) { pwmBrushedMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate); pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM | PWM_PF_MOTOR_MODE_BRUSHED; diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index e33f1a58c8..065e0d0f71 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input @@ -29,11 +30,11 @@ const uint16_t multiPPM[] = { PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM2 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed + PWM3 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; From 0668493ddd6da6834f7d802c54160dfbda04e230 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 20 Aug 2016 23:24:25 +0200 Subject: [PATCH 05/12] Busy wait reduction // Cleanup // New defaults --- src/main/config/config.c | 32 ++++++++++++------------- src/main/config/config_master.h | 1 - src/main/io/serial_cli.c | 13 +++++++---- src/main/main.c | 2 +- src/main/mw.c | 41 ++++++++++++++++----------------- 5 files changed, 45 insertions(+), 44 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 52f705d385..5f0cf31eae 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -185,11 +185,11 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) { controlRateConfig->rcRate8 = 100; controlRateConfig->rcYawRate8 = 100; - controlRateConfig->rcExpo8 = 10; + controlRateConfig->rcExpo8 = 0; controlRateConfig->thrMid8 = 50; controlRateConfig->thrExpo8 = 0; - controlRateConfig->dynThrPID = 20; - controlRateConfig->rcYawExpo8 = 10; + controlRateConfig->dynThrPID = 10; + controlRateConfig->rcYawExpo8 = 0; controlRateConfig->tpa_breakpoint = 1650; for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) { @@ -205,7 +205,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->I8[ROLL] = 40; pidProfile->D8[ROLL] = 20; pidProfile->P8[PITCH] = 60; - pidProfile->I8[PITCH] = 60; + pidProfile->I8[PITCH] = 65; pidProfile->D8[PITCH] = 22; pidProfile->P8[YAW] = 80; pidProfile->I8[YAW] = 45; @@ -236,11 +236,11 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yawItermIgnoreRate = 32; pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_lpf_hz = 100; // filtering ON by default - pidProfile->dterm_notch_hz = 0; - pidProfile->dterm_notch_cutoff = 150; + pidProfile->dterm_notch_hz = 260; + pidProfile->dterm_notch_cutoff = 160; pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->vbatPidCompensation = 0; - pidProfile->pidAtMinThrottle = PID_STABILISATION_OFF; + pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; // Betaflight PID controller parameters pidProfile->ptermSetpointWeight = 75; @@ -504,9 +504,9 @@ void createDefaultConfig(master_t *config) config->gyroConfig.gyroMovementCalibrationThreshold = 32; // xxx_hardware: 0:default/autodetect, 1: disable - config->mag_hardware = 0; + config->mag_hardware = 1; - config->baro_hardware = 0; + config->baro_hardware = 1; resetBatteryConfig(&config->batteryConfig); @@ -585,8 +585,6 @@ void createDefaultConfig(master_t *config) resetSerialConfig(&config->serialConfig); - config->emf_avoidance = 0; // TODO - needs removal - resetProfile(&config->profile[0]); resetRollAndPitchTrims(&config->accelerometerTrims); @@ -614,12 +612,12 @@ void createDefaultConfig(master_t *config) config->throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv // Failsafe Variables - config->failsafeConfig.failsafe_delay = 10; // 1sec - config->failsafeConfig.failsafe_off_delay = 10; // 1sec - config->failsafeConfig.failsafe_throttle = 1000; // default throttle off. - config->failsafeConfig.failsafe_kill_switch = 0; // default failsafe switch action is identical to rc link loss - config->failsafeConfig.failsafe_throttle_low_delay = 100; // default throttle low delay for "just disarm" on failsafe condition - config->failsafeConfig.failsafe_procedure = 0; // default full failsafe procedure is 0: auto-landing + config->failsafeConfig.failsafe_delay = 10; // 1sec + config->failsafeConfig.failsafe_off_delay = 10; // 1sec + config->failsafeConfig.failsafe_throttle = 1000; // default throttle off. + config->failsafeConfig.failsafe_kill_switch = 0; // default failsafe switch action is identical to rc link loss + config->failsafeConfig.failsafe_throttle_low_delay = 100; // default throttle low delay for "just disarm" on failsafe condition + config->failsafeConfig.failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT;// default full failsafe procedure is 0: auto-landing #ifdef USE_SERVOS // servos diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index aa572fac0a..cf93ca3b45 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -25,7 +25,6 @@ typedef struct master_t { uint8_t mixerMode; uint32_t enabledFeatures; - uint8_t emf_avoidance; // change pll settings to avoid noise in the uhf band // motor/esc/servo related stuff motorMixer_t customMotorMixer[MAX_SUPPORTED_MOTORS]; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 9b333b35e4..e3546636f6 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -517,6 +517,10 @@ static const char * const lookupTableLowpassType[] = { "NORMAL", "HIGH" }; +static const char * const lookupTableFailsafe[] = { + "AUTO-LAND", "DROP" +}; + typedef struct lookupTableEntry_s { const char * const *values; const uint8_t valueCount; @@ -555,6 +559,7 @@ typedef enum { TABLE_DELTA_METHOD, TABLE_RC_INTERPOLATION, TABLE_LOWPASS_TYPE, + TABLE_FAILSAFE, #ifdef OSD TABLE_OSD, #endif @@ -593,6 +598,7 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTableDeltaMethod, sizeof(lookupTableDeltaMethod) / sizeof(char *) }, { lookupTableRcInterpolation, sizeof(lookupTableRcInterpolation) / sizeof(char *) }, { lookupTableLowpassType, sizeof(lookupTableLowpassType) / sizeof(char *) }, + { lookupTableFailsafe, sizeof(lookupTableFailsafe) / sizeof(char *) }, #ifdef OSD { lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) }, #endif @@ -646,7 +652,6 @@ typedef struct { } clivalue_t; const clivalue_t valueTable[] = { -// { "emf_avoidance", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.emf_avoidance, .config.lookup = { TABLE_OFF_ON } }, { "mid_rc", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.midrc, .config.minmax = { 1200, 1700 } }, { "min_check", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.mincheck, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "max_check", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.maxcheck, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, @@ -808,7 +813,7 @@ const clivalue_t valueTable[] = { { "failsafe_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_throttle, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX } }, { "failsafe_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.failsafeConfig.failsafe_kill_switch, .config.lookup = { TABLE_OFF_ON } }, { "failsafe_throttle_low_delay",VAR_UINT16 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_throttle_low_delay, .config.minmax = { 0, 300 } }, - { "failsafe_procedure", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.failsafeConfig.failsafe_procedure, .config.lookup = { TABLE_OFF_ON } }, + { "failsafe_procedure", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.failsafeConfig.failsafe_procedure, .config.lookup = { TABLE_FAILSAFE } }, { "rx_min_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_min_usec, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX } }, { "rx_max_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_max_usec, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX } }, @@ -852,8 +857,8 @@ const clivalue_t valueTable[] = { { "yaw_rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0, 1000 } }, { "rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0, 1000 } }, - { "accumulation_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, - { "yaw_accumulation_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } }, + { "accum_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, + { "yaw_accum_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 } }, diff --git a/src/main/main.c b/src/main/main.c index 24b983308d..2d961c442c 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -675,7 +675,7 @@ void main_init(void) /* Setup scheduler */ schedulerInit(); - rescheduleTask(TASK_GYROPID, gyro.targetLooptime); + rescheduleTask(TASK_GYROPID, gyro.targetLooptime + 5); // Add a littlebit of extra time to reduce busy wait setTaskEnabled(TASK_GYROPID, true); if (sensors(SENSOR_ACC)) { diff --git a/src/main/mw.c b/src/main/mw.c index 86abfd6c63..80605784e6 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -816,8 +816,6 @@ void taskMainPidLoopCheck(void) static uint32_t previousTime; static bool runTaskMainSubprocesses; - const uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF); - cycleTime = micros() - previousTime; previousTime = micros(); @@ -827,30 +825,31 @@ void taskMainPidLoopCheck(void) } const uint32_t startTime = micros(); + while (true) { - if (gyroSyncCheckUpdate(&gyro) || ((currentDeltaTime + (micros() - previousTime)) >= (gyro.targetLooptime + GYRO_WATCHDOG_DELAY))) { - static uint8_t pidUpdateCountdown; - + if (gyroSyncCheckUpdate(&gyro)) { if (debugMode == DEBUG_PIDLOOP) {debug[0] = micros() - startTime;} // time spent busy waiting - if (runTaskMainSubprocesses) { - subTaskMainSubprocesses(); - runTaskMainSubprocesses = false; - } - - gyroUpdate(); - - if (pidUpdateCountdown) { - pidUpdateCountdown--; - } else { - pidUpdateCountdown = setPidUpdateCountDown(); - subTaskPidController(); - subTaskMotorUpdate(); - runTaskMainSubprocesses = true; - } - break; } } + + static uint8_t pidUpdateCountdown; + + if (runTaskMainSubprocesses) { + subTaskMainSubprocesses(); + runTaskMainSubprocesses = false; + } + + gyroUpdate(); + + if (pidUpdateCountdown) { + pidUpdateCountdown--; + } else { + pidUpdateCountdown = setPidUpdateCountDown(); + subTaskPidController(); + subTaskMotorUpdate(); + runTaskMainSubprocesses = true; + } } void taskUpdateAccelerometer(void) From 4e2ff7e7e9647c50671a99df6112a84f141a53db Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 21 Aug 2016 00:13:14 +0200 Subject: [PATCH 06/12] Improve dterm filter accuracy // magic number --- src/main/flight/pid.c | 4 ++-- src/main/flight/pid.h | 2 +- src/main/main.c | 6 ++++-- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 829b62c0f3..b6ffc3d586 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -76,9 +76,9 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc pidControllerFuncPtr pid_controller = pidBetaflight; // which pid controller are we using #endif -void setTargetPidLooptime(uint8_t pidProcessDenom) +void setTargetPidLooptime(uint32_t pidLooptime) { - targetPidLooptime = gyro.targetLooptime * pidProcessDenom; + targetPidLooptime = pidLooptime; } void pidResetErrorGyroState(void) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 12fa8324e8..fd1fb08f5a 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -126,5 +126,5 @@ extern uint32_t targetPidLooptime; void pidSetController(pidControllerType_e type); void pidResetErrorGyroState(void); void pidStabilisationState(pidStabilisationState_e pidControllerState); -void setTargetPidLooptime(uint8_t pidProcessDenom); +void setTargetPidLooptime(uint32_t pidLooptime); diff --git a/src/main/main.c b/src/main/main.c index 2d961c442c..eb5e0f90c7 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -106,6 +106,8 @@ #include "config/config_profile.h" #include "config/config_master.h" +#define LOOPTIME_SUSPEND_TIME 5 // Prevent too long busy wait times + #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif @@ -597,7 +599,7 @@ void init(void) masterConfig.gyro_sync_denom = 1; } - setTargetPidLooptime(masterConfig.pid_process_denom); // Initialize pid looptime + setTargetPidLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime #ifdef BLACKBOX @@ -675,7 +677,7 @@ void main_init(void) /* Setup scheduler */ schedulerInit(); - rescheduleTask(TASK_GYROPID, gyro.targetLooptime + 5); // Add a littlebit of extra time to reduce busy wait + rescheduleTask(TASK_GYROPID, gyro.targetLooptime + LOOPTIME_SUSPEND_TIME); // Add a littlebit of extra time to reduce busy wait setTaskEnabled(TASK_GYROPID, true); if (sensors(SENSOR_ACC)) { From 4043f5e5b4722d90e6790f9d03762b0d1ff3c7c6 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 21 Aug 2016 00:22:02 +0200 Subject: [PATCH 07/12] Increase Battery Priority --- src/main/scheduler/scheduler_tasks.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/scheduler/scheduler_tasks.c b/src/main/scheduler/scheduler_tasks.c index 9d5062ee21..30920724a8 100644 --- a/src/main/scheduler/scheduler_tasks.c +++ b/src/main/scheduler/scheduler_tasks.c @@ -73,7 +73,7 @@ cfTask_t cfTasks[TASK_COUNT] = { .taskName = "BATTERY", .taskFunc = taskUpdateBattery, .desiredPeriod = 1000000 / 50, // 50 Hz - .staticPriority = TASK_PRIORITY_LOW, + .staticPriority = TASK_PRIORITY_MEDIUM, }, #ifdef BEEPER From a321f7f766319d024328d6164f2f418545fe4abe Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 21 Aug 2016 01:06:13 +0200 Subject: [PATCH 08/12] Reduce Suspend time --- src/main/main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/main.c b/src/main/main.c index eb5e0f90c7..7949e1a560 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -106,7 +106,7 @@ #include "config/config_profile.h" #include "config/config_master.h" -#define LOOPTIME_SUSPEND_TIME 5 // Prevent too long busy wait times +#define LOOPTIME_SUSPEND_TIME 3 // Prevent too long busy wait times #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" From 525dbcc3082f2d7fd04ec70d05b341834c88ac4a Mon Sep 17 00:00:00 2001 From: nathan Date: Sat, 20 Aug 2016 22:55:03 -0700 Subject: [PATCH 09/12] unswap omnibus motors 3 and 4 --- src/main/target/OMNIBUS/target.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/OMNIBUS/target.c b/src/main/target/OMNIBUS/target.c index 977d623f85..8a87602bea 100644 --- a/src/main/target/OMNIBUS/target.c +++ b/src/main/target/OMNIBUS/target.c @@ -87,8 +87,8 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - PB8 { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM3 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM4 - PA3 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM3 - PA3 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM4 - PA2 // UART3 RX/TX { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 - PB10 - TIM2_CH3 / UART3_TX (AF7) From f15eb69d2c05a8cd045cf3a412c1edebf080d36a Mon Sep 17 00:00:00 2001 From: jflyper Date: Sun, 21 Aug 2016 22:39:00 +0900 Subject: [PATCH 10/12] Update RMDO MPU interrupt support The RMDO/target.h was under maintained that it was probably outdated by EXTI changes at some stage. --- src/main/target/RMDO/target.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index 67d2232e68..57155d209b 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -26,11 +26,13 @@ #define BEEPER PC15 #define BEEPER_INVERTED +#define USE_EXTI +#define MPU_INT_EXTI PC13 #define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready and MAG data ready #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW -#define USE_MAG_DATA_READY_SIGNAL -#define ENSURE_MAG_DATA_READY_IS_HIGH +//#define USE_MAG_DATA_READY_SIGNAL // XXX Do RMDO has onboard mag??? +//#define ENSURE_MAG_DATA_READY_IS_HIGH #define GYRO #define USE_GYRO_MPU6050 From 174c45b67f129847c6b9154e353590740cb7235a Mon Sep 17 00:00:00 2001 From: dexX7 Date: Sun, 21 Aug 2016 21:52:29 +0200 Subject: [PATCH 11/12] Add missing pin to IRCFUSION3 target Without the pin for the IRCFUSION3 target, connecting to the FC was not possible. To fix it, the missing pin is added to the target. --- src/main/target/IRCFUSIONF3/target.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h index 8dbd6e55ba..62eec67fc5 100644 --- a/src/main/target/IRCFUSIONF3/target.h +++ b/src/main/target/IRCFUSIONF3/target.h @@ -25,6 +25,9 @@ #define USABLE_TIMER_CHANNEL_COUNT 17 +#define USE_EXTI +#define MPU_INT_EXTI PC13 + #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready, no MAG #define USE_MPU_DATA_READY_SIGNAL From 2f14f6119039b6feee9a2388253cf608f7fbf60e Mon Sep 17 00:00:00 2001 From: mikeller Date: Tue, 23 Aug 2016 01:21:19 +1200 Subject: [PATCH 12/12] Fixed lockups on LED_STRIP configuration. --- src/main/io/ledstrip.c | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 4af92de4d3..c41c3a884b 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -257,10 +257,10 @@ STATIC_UNIT_TESTED void determineLedStripDimensions(void) STATIC_UNIT_TESTED void determineOrientationLimits(void) { - highestYValueForNorth = (ledGridHeight / 2) - 1; - lowestYValueForSouth = ((ledGridHeight + 1) / 2); - highestXValueForWest = (ledGridWidth / 2) - 1; - lowestXValueForEast = ((ledGridWidth + 1) / 2); + highestYValueForNorth = MIN((ledGridHeight / 2) - 1, 0); + lowestYValueForSouth = (ledGridHeight + 1) / 2; + highestXValueForWest = MIN((ledGridWidth / 2) - 1, 0); + lowestXValueForEast = (ledGridWidth + 1) / 2; } STATIC_UNIT_TESTED void updateLedCount(void) @@ -533,7 +533,11 @@ static void applyLedFixedLayers() case LED_FUNCTION_FLIGHT_MODE: for (unsigned i = 0; i < ARRAYLEN(flightModeToLed); i++) if (!flightModeToLed[i].flightMode || FLIGHT_MODE(flightModeToLed[i].flightMode)) { - color = *getDirectionalModeColor(ledIndex, &masterConfig.modeColors[flightModeToLed[i].ledMode]); + hsvColor_t *directionalColor = getDirectionalModeColor(ledIndex, &masterConfig.modeColors[flightModeToLed[i].ledMode]); + if (directionalColor) { + color = *directionalColor; + } + break; // stop on first match } break;