From b7172570328fd92d03dfc9ad25fabb5af2b1516d Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 30 Aug 2023 22:15:59 +0200 Subject: [PATCH 01/39] Make PWM allocation a bit smarter. Only assign servo output if the timer is not used for a motor. Only assign a motor output if the timer is not used for a servo. Needs configurator update and more testing. Checking resource on the MATEKH743 on the CLI, with 3 motors, servos were correctly assigned to TIM4 outpus, instead of TIM5, even with outputs sets to MC_SERVO and MC_MOTOR --- src/main/drivers/pwm_mapping.c | 32 +++++++++++++++++++++++++----- src/main/target/MATEKH743/target.c | 24 +++++++++++----------- 2 files changed, 39 insertions(+), 17 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 4fc5a00b83e..e8e485d5922 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -237,6 +237,28 @@ static void timerHardwareOverride(timerHardware_t * timer) { } } +bool pwmHasMotorOnTimer(timMotorServoHardware_t * timOutputs, TIM_TypeDef *tim) +{ + for (int i = 0; i < timOutputs->maxTimMotorCount; ++i) { + if (timOutputs->timMotors[i]->tim == tim) { + return true; + } + } + + return false; +} + +bool pwmHasServoOnTimer(timMotorServoHardware_t * timOutputs, TIM_TypeDef *tim) +{ + for (int i = 0; i < timOutputs->maxTimServoCount; ++i) { + if (timOutputs->timServos[i]->tim == tim) { + return true; + } + } + + return false; +} + void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerUsingServos) { timOutputs->maxTimMotorCount = 0; @@ -269,19 +291,19 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU motorIdx += 1; } - // We enable mapping to servos if mixer is actually using them - if (isMixerUsingServos && timHw->usageFlags & TIM_USE_MC_SERVO) { + // We enable mapping to servos if mixer is actually using them and it does not conflict with used motors + if (isMixerUsingServos && timHw->usageFlags & TIM_USE_MC_SERVO && !pwmHasMotorOnTimer(timOutputs, timHw->tim)) { type = MAP_TO_SERVO_OUTPUT; } - else if (timHw->usageFlags & TIM_USE_MC_MOTOR) { + else if (timHw->usageFlags & TIM_USE_MC_MOTOR && !pwmHasServoOnTimer(timOutputs, timHw->tim)) { type = MAP_TO_MOTOR_OUTPUT; } } else { // Fixed wing or HELI (one/two motors and a lot of servos - if (timHw->usageFlags & TIM_USE_FW_SERVO) { + if (timHw->usageFlags & TIM_USE_FW_SERVO && !pwmHasMotorOnTimer(timOutputs, timHw->tim)) { type = MAP_TO_SERVO_OUTPUT; } - else if (timHw->usageFlags & TIM_USE_FW_MOTOR) { + else if (timHw->usageFlags & TIM_USE_FW_MOTOR && !pwmHasServoOnTimer(timOutputs, timHw->tim)) { type = MAP_TO_MOTOR_OUTPUT; } } diff --git a/src/main/target/MATEKH743/target.c b/src/main/target/MATEKH743/target.c index 706d5787865..6770cde95b0 100644 --- a/src/main/target/MATEKH743/target.c +++ b/src/main/target/MATEKH743/target.c @@ -35,18 +35,18 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // S3 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3), // S4 - DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 4), // S5 - DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 5), // S6 - - DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 6), // S7 - DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 7), // S8 - DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S9 - DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S10 DMA_NONE - - DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S11 - DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S12 DMA_NONE + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 2), // S3 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 3), // S4 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 4), // S5 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 5), // S6 + + DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 6), // S7 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 7), // S8 + DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S9 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S10 DMA_NONE + + DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S11 + DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S12 DMA_NONE DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER PWM From cada1cca145d02ea181ba3f704c4e79f59551d1e Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 30 Aug 2023 22:35:13 +0200 Subject: [PATCH 02/39] Fix AT32 build --- src/main/drivers/pwm_mapping.c | 4 ++-- src/main/drivers/timer.h | 6 ++++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index e8e485d5922..15184daff20 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -237,7 +237,7 @@ static void timerHardwareOverride(timerHardware_t * timer) { } } -bool pwmHasMotorOnTimer(timMotorServoHardware_t * timOutputs, TIM_TypeDef *tim) +bool pwmHasMotorOnTimer(timMotorServoHardware_t * timOutputs, HAL_Timer_t *tim) { for (int i = 0; i < timOutputs->maxTimMotorCount; ++i) { if (timOutputs->timMotors[i]->tim == tim) { @@ -248,7 +248,7 @@ bool pwmHasMotorOnTimer(timMotorServoHardware_t * timOutputs, TIM_TypeDef *tim) return false; } -bool pwmHasServoOnTimer(timMotorServoHardware_t * timOutputs, TIM_TypeDef *tim) +bool pwmHasServoOnTimer(timMotorServoHardware_t * timOutputs, HAL_Timer_t *tim) { for (int i = 0; i < timOutputs->maxTimServoCount; ++i) { if (timOutputs->timServos[i]->tim == tim) { diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 5272c804b8f..f7feb0eebe0 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -63,8 +63,9 @@ typedef uint32_t timCNT_t; #endif // tmr_type instead in AT32 #if defined(AT32F43x) +typedef tmr_type HAL_Timer_t; typedef struct timerDef_s { - tmr_type * tim; + HAL_Timer_t * tim; rccPeriphTag_t rcc; uint8_t irq; uint8_t secondIrq; @@ -82,8 +83,9 @@ typedef struct timerHardware_s { uint32_t dmaMuxid; //DMAMUX ID } timerHardware_t; #else +typedef TIM_TypeDef HAL_Timer_t; typedef struct timerDef_s { - TIM_TypeDef * tim; + HAL_Timer_t * tim; rccPeriphTag_t rcc; uint8_t irq; uint8_t secondIrq; From 0d16352fb8996884b670e293a580514b7a009ed6 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 30 Aug 2023 23:59:38 +0200 Subject: [PATCH 03/39] Add extra MSP message to get outputs with timer ids Timer ids are needed to correctly replicate pwm allocation logic in the configurator --- src/main/drivers/timer.c | 16 ++++++++++++++-- src/main/drivers/timer.h | 4 ++++ src/main/fc/fc_msp.c | 8 ++++++++ src/main/msp/msp_protocol_v2_inav.h | 1 + 4 files changed, 27 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index ecdf1ba48d0..a15e3c6fdf8 100644 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -38,8 +38,20 @@ timHardwareContext_t * timerCtx[HARDWARE_TIMER_DEFINITION_COUNT]; + +uint8_t timer2id(const HAL_Timer_t *tim) +{ + for(int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) + { + if(timerDefinitions[i].tim == tim) + return i; + } + + return (uint8_t)-1; +} + #if defined(AT32F43x) -uint8_t lookupTimerIndex(const tmr_type *tim) +uint8_t lookupTimerIndex(const HAL_Timer_t *tim) { int i; @@ -54,7 +66,7 @@ uint8_t lookupTimerIndex(const tmr_type *tim) } #else // return index of timer in timer table. Lowest timer has index 0 -uint8_t lookupTimerIndex(const TIM_TypeDef *tim) +uint8_t lookupTimerIndex(const HAL_Timer_t *tim) { int i; diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index f7feb0eebe0..c1de08503b0 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -25,6 +25,8 @@ #include "drivers/rcc_types.h" #include "drivers/timer_def.h" +#include "platform.h" + #define CC_CHANNELS_PER_TIMER 4 // TIM_Channel_1..4 typedef uint16_t captureCompare_t; // 16 bit on both 103 and 303, just register access must be 32bit sometimes (use timCCR_t) @@ -231,3 +233,5 @@ void timerPWMStopDMA(TCH_t * tch); bool timerPWMDMAInProgress(TCH_t * tch); volatile timCCR_t *timerCCR(TCH_t * tch); + +uint8_t timer2id(const HAL_Timer_t *tim); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 821e4526337..eb32c910b56 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1516,6 +1516,14 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } break; + case MSP2_INAV_OUTPUT_MAPPING_EXT: + for (uint8_t i = 0; i < timerHardwareCount; ++i) + if (!(timerHardware[i].usageFlags & (TIM_USE_PPM | TIM_USE_PWM))) { + sbufWriteU8(dst, timer2id(timerHardware[i].tim)); + sbufWriteU8(dst, timerHardware[i].usageFlags); + } + break; + case MSP2_INAV_MC_BRAKING: #ifdef USE_MR_BRAKING_MODE sbufWriteU16(dst, navConfig()->mc.braking_speed_threshold); diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 344b84f22f5..df7075f9132 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -31,6 +31,7 @@ #define MSP2_INAV_OUTPUT_MAPPING 0x200A #define MSP2_INAV_MC_BRAKING 0x200B #define MSP2_INAV_SET_MC_BRAKING 0x200C +#define MSP2_INAV_OUTPUT_MAPPING_EXT 0x200D #define MSP2_INAV_MIXER 0x2010 #define MSP2_INAV_SET_MIXER 0x2011 From 712b3a33462556e68d723e63057873d8101c61b3 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 31 Aug 2023 00:19:04 +0200 Subject: [PATCH 04/39] Fix sitl build --- src/main/drivers/timer.c | 6 ++---- src/main/fc/fc_msp.c | 4 ++++ 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index a15e3c6fdf8..2f63581ca1b 100644 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -41,10 +41,8 @@ timHardwareContext_t * timerCtx[HARDWARE_TIMER_DEFINITION_COUNT]; uint8_t timer2id(const HAL_Timer_t *tim) { - for(int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) - { - if(timerDefinitions[i].tim == tim) - return i; + for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) { + if (timerDefinitions[i].tim == tim) return i; } return (uint8_t)-1; diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index eb32c910b56..82907ee07e3 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1519,7 +1519,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP2_INAV_OUTPUT_MAPPING_EXT: for (uint8_t i = 0; i < timerHardwareCount; ++i) if (!(timerHardware[i].usageFlags & (TIM_USE_PPM | TIM_USE_PWM))) { + #if defined(SITL_BUILD) + sbufWriteU8(dst, i); + #else sbufWriteU8(dst, timer2id(timerHardware[i].tim)); + #endif sbufWriteU8(dst, timerHardware[i].usageFlags); } break; From 798abb81713cd34098dad5da85308d9105b9e905 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 31 Aug 2023 11:30:07 +0200 Subject: [PATCH 05/39] Test extreme scenario where all pins can be both motors and servos --- src/main/target/MATEKH743/target.c | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/target/MATEKH743/target.c b/src/main/target/MATEKH743/target.c index 6770cde95b0..89d02991862 100644 --- a/src/main/target/MATEKH743/target.c +++ b/src/main/target/MATEKH743/target.c @@ -32,21 +32,21 @@ BUSDEV_REGISTER_SPI_TAG(busdev_icm20602, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6 BUSDEV_REGISTER_SPI_TAG(busdev_icm42605, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 2, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_FW_MOTOR, 0, 1), // S2 - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 2), // S3 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 3), // S4 - DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 4), // S5 - DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 5), // S6 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_FW_MOTOR, 0, 2), // S3 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_FW_MOTOR, 0, 3), // S4 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_FW_MOTOR, 0, 4), // S5 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_FW_MOTOR, 0, 5), // S6 - DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 6), // S7 - DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 7), // S8 - DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S9 - DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S10 DMA_NONE + DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_FW_MOTOR, 0, 6), // S7 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_FW_MOTOR, 0, 7), // S8 + DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S9 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S10 DMA_NONE - DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S11 - DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S12 DMA_NONE + DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S11 + DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S12 DMA_NONE DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER PWM From c4277f450a85017857a712d0cf8f56565ce0a564 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 31 Aug 2023 22:50:56 +0200 Subject: [PATCH 06/39] Improve timer allocation logic The stress test MATEKH743 target seems to handle things well. If all outputs are both servo and motors, It will go through outputs in order and claim all outputs in order, until the desired number of motors is achieved. --- src/main/drivers/pwm_mapping.c | 102 +++++++++++++++++++++++++++++++-- 1 file changed, 98 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 15184daff20..ec63a412ebf 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -259,6 +259,85 @@ bool pwmHasServoOnTimer(timMotorServoHardware_t * timOutputs, HAL_Timer_t *tim) return false; } +uint8_t pwmClaimTimer(HAL_Timer_t *tim, uint32_t usageFlags) { + uint8_t changed = 0; + for (int idx = 0; idx < timerHardwareCount; idx++) { + timerHardware_t *timHw = &timerHardware[idx]; + if (timHw->tim == tim && timHw->usageFlags != usageFlags) { + timHw->usageFlags = usageFlags; + changed++; + } + } + + return changed; +} + +void pwmEnsureEnoughtMotors(uint8_t motorCount) +{ + uint8_t motorOnlyOutputs = 0; + uint8_t mcMotorOnlyOutputs = 0; + + if(motorCount == 0) + { + motorCount = 2; + } + + for (int idx = 0; idx < timerHardwareCount; idx++) { + timerHardware_t *timHw = &timerHardware[idx]; + + timerHardwareOverride(timHw); + + if (checkPwmTimerConflicts(timHw)) { + continue; + } + + if (timHw->usageFlags & (TIM_USE_MC_MOTOR) && + !(timHw->usageFlags & (TIM_USE_MC_SERVO))) { + mcMotorOnlyOutputs++; + mcMotorOnlyOutputs += pwmClaimTimer(timHw->tim, timHw->usageFlags); + } + if (timHw->usageFlags & (TIM_USE_FW_MOTOR) && + !(timHw->usageFlags & (TIM_USE_FW_SERVO))) { + motorOnlyOutputs++; + motorOnlyOutputs += pwmClaimTimer(timHw->tim, timHw->usageFlags); + } + } + + if (mixerConfig()->platformType == PLATFORM_MULTIROTOR || + mixerConfig()->platformType == PLATFORM_TRICOPTER) { + + for (int idx = 0; mcMotorOnlyOutputs < motorCount && idx < timerHardwareCount; idx++) { + timerHardware_t *timHw = &timerHardware[idx]; + + if (checkPwmTimerConflicts(timHw)) { + continue; + } + + if ((timHw->usageFlags & (TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO)) != TIM_USE_MC_MOTOR) { + timHw->usageFlags &= ~TIM_USE_MC_SERVO; + timHw->usageFlags |= TIM_USE_MC_MOTOR; + mcMotorOnlyOutputs++; + mcMotorOnlyOutputs += pwmClaimTimer(timHw->tim, timHw->usageFlags); + } + } + } else { + for (int idx = 0; motorOnlyOutputs < motorCount && idx < timerHardwareCount; idx++) { + timerHardware_t *timHw = &timerHardware[idx]; + + if (checkPwmTimerConflicts(timHw)) { + continue; + } + + if ((timHw->usageFlags & (TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO)) != TIM_USE_FW_MOTOR) { + timHw->usageFlags &= ~TIM_USE_FW_SERVO; + timHw->usageFlags |= TIM_USE_FW_MOTOR; + motorOnlyOutputs++; + motorOnlyOutputs += pwmClaimTimer(timHw->tim, timHw->usageFlags); + } + } + } +} + void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerUsingServos) { timOutputs->maxTimMotorCount = 0; @@ -267,6 +346,11 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU uint8_t motorCount = getMotorCount(); uint8_t motorIdx = 0; + if(motorCount == 0) + motorCount = 2; + + pwmEnsureEnoughtMotors(motorCount); + for (int idx = 0; idx < timerHardwareCount; idx++) { timerHardware_t *timHw = &timerHardware[idx]; @@ -288,32 +372,42 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU // Make sure first motorCount outputs get assigned to motor if ((timHw->usageFlags & TIM_USE_MC_MOTOR) && (motorIdx < motorCount)) { timHw->usageFlags = timHw->usageFlags & ~TIM_USE_MC_SERVO; + pwmClaimTimer(timHw->tim, timHw->usageFlags); motorIdx += 1; } // We enable mapping to servos if mixer is actually using them and it does not conflict with used motors if (isMixerUsingServos && timHw->usageFlags & TIM_USE_MC_SERVO && !pwmHasMotorOnTimer(timOutputs, timHw->tim)) { type = MAP_TO_SERVO_OUTPUT; - } - else if (timHw->usageFlags & TIM_USE_MC_MOTOR && !pwmHasServoOnTimer(timOutputs, timHw->tim)) { + } else if (timHw->usageFlags & TIM_USE_MC_MOTOR && !pwmHasServoOnTimer(timOutputs, timHw->tim)) { type = MAP_TO_MOTOR_OUTPUT; } } else { + // Make sure first motorCount motor outputs get assigned to motor + if ((timHw->usageFlags & TIM_USE_FW_MOTOR) && (motorIdx < motorCount)) { + timHw->usageFlags = timHw->usageFlags & ~TIM_USE_MC_SERVO; + pwmClaimTimer(timHw->tim, timHw->usageFlags); + motorIdx += 1; + } + // Fixed wing or HELI (one/two motors and a lot of servos if (timHw->usageFlags & TIM_USE_FW_SERVO && !pwmHasMotorOnTimer(timOutputs, timHw->tim)) { type = MAP_TO_SERVO_OUTPUT; - } - else if (timHw->usageFlags & TIM_USE_FW_MOTOR && !pwmHasServoOnTimer(timOutputs, timHw->tim)) { + } else if (timHw->usageFlags & TIM_USE_FW_MOTOR && !pwmHasServoOnTimer(timOutputs, timHw->tim)) { type = MAP_TO_MOTOR_OUTPUT; } } switch(type) { case MAP_TO_MOTOR_OUTPUT: + timHw->usageFlags &= (TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR); timOutputs->timMotors[timOutputs->maxTimMotorCount++] = timHw; + pwmClaimTimer(timHw->tim, timHw->usageFlags); break; case MAP_TO_SERVO_OUTPUT: + timHw->usageFlags &= (TIM_USE_MC_SERVO | TIM_USE_FW_SERVO); timOutputs->timServos[timOutputs->maxTimServoCount++] = timHw; + pwmClaimTimer(timHw->tim, timHw->usageFlags); break; default: break; From 5189bfefc346e62a30c528cd00d38329dcc9cfb0 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 31 Aug 2023 23:35:39 +0200 Subject: [PATCH 07/39] remove more debugging code --- src/main/drivers/pwm_mapping.c | 15 ++------------- 1 file changed, 2 insertions(+), 13 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index ec63a412ebf..3b61d95e385 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -277,11 +277,6 @@ void pwmEnsureEnoughtMotors(uint8_t motorCount) uint8_t motorOnlyOutputs = 0; uint8_t mcMotorOnlyOutputs = 0; - if(motorCount == 0) - { - motorCount = 2; - } - for (int idx = 0; idx < timerHardwareCount; idx++) { timerHardware_t *timHw = &timerHardware[idx]; @@ -291,13 +286,11 @@ void pwmEnsureEnoughtMotors(uint8_t motorCount) continue; } - if (timHw->usageFlags & (TIM_USE_MC_MOTOR) && - !(timHw->usageFlags & (TIM_USE_MC_SERVO))) { + if (timHw->usageFlags & (TIM_USE_MC_MOTOR) && !(timHw->usageFlags & (TIM_USE_MC_SERVO))) { mcMotorOnlyOutputs++; mcMotorOnlyOutputs += pwmClaimTimer(timHw->tim, timHw->usageFlags); } - if (timHw->usageFlags & (TIM_USE_FW_MOTOR) && - !(timHw->usageFlags & (TIM_USE_FW_SERVO))) { + if (timHw->usageFlags & (TIM_USE_FW_MOTOR) && !(timHw->usageFlags & (TIM_USE_FW_SERVO))) { motorOnlyOutputs++; motorOnlyOutputs += pwmClaimTimer(timHw->tim, timHw->usageFlags); } @@ -346,13 +339,9 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU uint8_t motorCount = getMotorCount(); uint8_t motorIdx = 0; - if(motorCount == 0) - motorCount = 2; - pwmEnsureEnoughtMotors(motorCount); for (int idx = 0; idx < timerHardwareCount; idx++) { - timerHardware_t *timHw = &timerHardware[idx]; timerHardwareOverride(timHw); From d67acb1abe75d929619c4bb7273dbe65fe00e735 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 1 Sep 2023 10:54:25 +0200 Subject: [PATCH 08/39] Remove _SERVO6 target. You should be able to achieve the same result with this pull request, hopefully before supporting overriding individual timer functions. --- src/main/target/MATEKF405/CMakeLists.txt | 1 - src/main/target/MATEKF405/target.c | 20 ++++++-------------- 2 files changed, 6 insertions(+), 15 deletions(-) diff --git a/src/main/target/MATEKF405/CMakeLists.txt b/src/main/target/MATEKF405/CMakeLists.txt index c4f839c1c1d..ebf457cda25 100644 --- a/src/main/target/MATEKF405/CMakeLists.txt +++ b/src/main/target/MATEKF405/CMakeLists.txt @@ -1,3 +1,2 @@ target_stm32f405xg(MATEKF405) -target_stm32f405xg(MATEKF405_SERVOS6) target_stm32f405xg(MATEKF405OSD) diff --git a/src/main/target/MATEKF405/target.c b/src/main/target/MATEKF405/target.c index 40ebf626e5f..a938452989e 100644 --- a/src/main/target/MATEKF405/target.c +++ b/src/main/target/MATEKF405/target.c @@ -24,23 +24,15 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM -#ifdef MATEKF405_SERVOS6 - DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 -#else - DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S1 -#endif - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S2 UP(2,1) - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 UP(2,1) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 UP(2,1) + DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S1 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S2 UP(2,1) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 UP(2,1) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 UP(2,1) DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_LED , 0, 0), // S5 UP(1,7) -#ifdef MATEKF405_SERVOS6 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 UP(2,5) -#else - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 UP(2,5) -#endif - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S7 D(1,7)!S5 UP(2,6) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 UP(2,5) + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 D(1,7)!S5 UP(2,6) DEF_TIM(TIM5, CH3, PA2, TIM_USE_PWM, 0, 0), // TX2 From dc53b12a772489fe3eafd7b57d6c6594761fe2b0 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 1 Sep 2023 11:01:20 +0200 Subject: [PATCH 09/39] Remove SERVO target --- src/main/target/MATEKF722/CMakeLists.txt | 1 - src/main/target/MATEKF722/target.c | 22 +++++++++------------- src/main/target/MATEKF722/target.h | 0 3 files changed, 9 insertions(+), 14 deletions(-) mode change 100755 => 100644 src/main/target/MATEKF722/target.c mode change 100755 => 100644 src/main/target/MATEKF722/target.h diff --git a/src/main/target/MATEKF722/CMakeLists.txt b/src/main/target/MATEKF722/CMakeLists.txt index eb384e12815..600a4401a20 100644 --- a/src/main/target/MATEKF722/CMakeLists.txt +++ b/src/main/target/MATEKF722/CMakeLists.txt @@ -1,2 +1 @@ target_stm32f722xe(MATEKF722) -target_stm32f722xe(MATEKF722_HEXSERVO) diff --git a/src/main/target/MATEKF722/target.c b/src/main/target/MATEKF722/target.c old mode 100755 new mode 100644 index dc06c7f508d..d64c20ecf60 --- a/src/main/target/MATEKF722/target.c +++ b/src/main/target/MATEKF722/target.c @@ -25,24 +25,20 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM + DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM - DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(1, 4, 5) - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2 D(2, 3, 7) - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 D(2, 4, 7) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 D(2, 7, 7) + DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S1 D(1, 4, 5) + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2 D(2, 3, 7) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 D(2, 4, 7) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 D(2, 7, 7) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S5 DMA1_ST2 -#ifdef MATEKF722_HEXSERVO - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 DMA2_ST6 -#else - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 DMA2_ST6 -#endif - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 DMA1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 DMA1_ST2 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 DMA2_ST6 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 DMA1_ST7 // DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), // TX2/S8 DMA1_ST0 - DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED STRIP D(1, 5, 3) + DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED STRIP D(1, 5, 3) }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MATEKF722/target.h b/src/main/target/MATEKF722/target.h old mode 100755 new mode 100644 From 9a54b3dd7a72fb3950f3aea3efa5dd95b90d00b2 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 1 Sep 2023 16:39:52 +0200 Subject: [PATCH 10/39] Add TIM_USE_OUTPUT_AUTO --- src/main/drivers/timer.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index c1de08503b0..93d4db93b2b 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -119,6 +119,7 @@ typedef enum { TIM_USE_BEEPER = (1 << 25), } timerUsageFlag_e; +#define TIM_USE_OUTPUT_AUTO (TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO) enum { TIMER_OUTPUT_NONE = 0x00, From 5f761ebb7c7d9e396aed7ecf956f7090a84b9bc5 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 1 Sep 2023 16:40:16 +0200 Subject: [PATCH 11/39] Update targets to use TIM_USE_OUTPUT_AUTO --- src/main/target/MATEKF405/target.c | 12 ++++++------ src/main/target/MATEKF722/target.c | 16 ++++++++-------- src/main/target/MATEKH743/target.c | 24 ++++++++++++------------ 3 files changed, 26 insertions(+), 26 deletions(-) diff --git a/src/main/target/MATEKF405/target.c b/src/main/target/MATEKF405/target.c index a938452989e..047e0f37751 100644 --- a/src/main/target/MATEKF405/target.c +++ b/src/main/target/MATEKF405/target.c @@ -24,15 +24,15 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM - DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S1 - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S2 UP(2,1) - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 UP(2,1) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 UP(2,1) + DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 UP(2,1) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S3 UP(2,1) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 UP(2,1) DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_LED , 0, 0), // S5 UP(1,7) - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 UP(2,5) - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 D(1,7)!S5 UP(2,6) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 UP(2,5) + DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 D(1,7)!S5 UP(2,6) DEF_TIM(TIM5, CH3, PA2, TIM_USE_PWM, 0, 0), // TX2 diff --git a/src/main/target/MATEKF722/target.c b/src/main/target/MATEKF722/target.c index d64c20ecf60..d2575f42f2d 100644 --- a/src/main/target/MATEKF722/target.c +++ b/src/main/target/MATEKF722/target.c @@ -27,14 +27,14 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), // PPM - DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S1 D(1, 4, 5) - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2 D(2, 3, 7) - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 D(2, 4, 7) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 D(2, 7, 7) - - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 DMA1_ST2 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 DMA2_ST6 - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 DMA1_ST7 + DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 D(1, 4, 5) + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(2, 3, 7) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 D(2, 4, 7) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 D(2, 7, 7) + + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 DMA1_ST2 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 DMA2_ST6 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 DMA1_ST7 // DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), // TX2/S8 DMA1_ST0 diff --git a/src/main/target/MATEKH743/target.c b/src/main/target/MATEKH743/target.c index 89d02991862..1923e6623a1 100644 --- a/src/main/target/MATEKH743/target.c +++ b/src/main/target/MATEKH743/target.c @@ -32,21 +32,21 @@ BUSDEV_REGISTER_SPI_TAG(busdev_icm20602, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6 BUSDEV_REGISTER_SPI_TAG(busdev_icm42605, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 2, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_FW_MOTOR, 0, 1), // S2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 - DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_FW_MOTOR, 0, 2), // S3 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_FW_MOTOR, 0, 3), // S4 - DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_FW_MOTOR, 0, 4), // S5 - DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_FW_MOTOR, 0, 5), // S6 + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 4), // S5 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 5), // S6 - DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_FW_MOTOR, 0, 6), // S7 - DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_FW_MOTOR, 0, 7), // S8 - DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S9 - DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S10 DMA_NONE + DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 6), // S7 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 7), // S8 + DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 DMA_NONE - DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S11 - DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S12 DMA_NONE + DEF_TIM(TIM15, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 + DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0), // S12 DMA_NONE DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER PWM From 26f2886e0e106eb84c27c10f3e282e1be058424b Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 1 Sep 2023 19:35:35 +0200 Subject: [PATCH 12/39] man, this sucks --- src/main/target/GEPRCF405/CMakeLists.txt | 2 +- src/main/target/GEPRCF405/target.c | 92 +++--- src/main/target/GEPRCF405/target.h | 354 +++++++++++------------ src/main/target/GEPRCF722/CMakeLists.txt | 2 +- src/main/target/GEPRCF722/target.c | 88 +++--- src/main/target/GEPRCF722/target.h | 346 +++++++++++----------- 6 files changed, 442 insertions(+), 442 deletions(-) diff --git a/src/main/target/GEPRCF405/CMakeLists.txt b/src/main/target/GEPRCF405/CMakeLists.txt index 2e3b1dec27f..d500b48baa1 100644 --- a/src/main/target/GEPRCF405/CMakeLists.txt +++ b/src/main/target/GEPRCF405/CMakeLists.txt @@ -1 +1 @@ -target_stm32f405xg(GEPRCF405) +target_stm32f405xg(GEPRCF405) diff --git a/src/main/target/GEPRCF405/target.c b/src/main/target/GEPRCF405/target.c index 38eee3688d0..ace9b307366 100644 --- a/src/main/target/GEPRCF405/target.c +++ b/src/main/target/GEPRCF405/target.c @@ -1,46 +1,46 @@ -/* -* This file is part of INAV Project. -* -* 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 "platform.h" -#include "drivers/io.h" - -#include "drivers/dma.h" -#include "drivers/timer.h" -#include "drivers/timer_def.h" - - - - - -timerHardware_t timerHardware[] = { - DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), - - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), - DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), - DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - - DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0), -}; - -const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); - - +/* +* This file is part of INAV Project. +* +* 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 "platform.h" +#include "drivers/io.h" + +#include "drivers/dma.h" +#include "drivers/timer.h" +#include "drivers/timer_def.h" + + + + + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), + DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), + DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + + DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0), +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); + + diff --git a/src/main/target/GEPRCF405/target.h b/src/main/target/GEPRCF405/target.h index 5df9af996eb..3a628ffe596 100644 --- a/src/main/target/GEPRCF405/target.h +++ b/src/main/target/GEPRCF405/target.h @@ -1,177 +1,177 @@ -/* - * This file is part of INAV Project. - * - * 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 "GEPR" - -#define USBD_PRODUCT_STRING "GEPRCF405" - -#define LED0 PC14 -#define LED1 PC15 - - - -#define BEEPER PC13 -#define BEEPER_INVERTED - -// *************** Gyro & ACC ********************** - - - -#define USE_SPI -#define USE_SPI_DEVICE_3 - -#define SPI3_SCK_PIN PB3 -#define SPI3_MISO_PIN PB4 -#define SPI3_MOSI_PIN PB5 - - -#define USE_IMU_MPU6000 -#define IMU_MPU6000_ALIGN CW180_DEG -#define MPU6000_CS_PIN PA15 -#define MPU6000_SPI_BUS BUS_SPI3 - -#define USE_IMU_BMI270 -#define IMU_BMI270_ALIGN CW180_DEG -#define BMI270_CS_PIN PA15 -#define BMI270_SPI_BUS BUS_SPI3 - -#define USE_IMU_ICM42605 -#define IMU_ICM42605_ALIGN CW180_DEG -#define ICM42605_CS_PIN PA15 -#define ICM42605_SPI_BUS BUS_SPI3 - -// *************** I2C/Baro/Mag ********************* -#define USE_I2C -#define USE_I2C_DEVICE_1 -#define I2C1_SCL PB8 -#define I2C1_SDA PB9 - -#define USE_BARO -#define BARO_I2C_BUS BUS_I2C1 -#define USE_BARO_BMP280 -#define USE_BARO_DPS310 -#define USE_BARO_MS5611 - -#define USE_MAG -#define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL - -#define TEMPERATURE_I2C_BUS BUS_I2C1 - -#define PITOT_I2C_BUS BUS_I2C1 - -#define USE_RANGEFINDER -#define RANGEFINDER_I2C_BUS BUS_I2C1 -#define BNO055_I2C_BUS BUS_I2C1 - -// *************** FLASH ************************** -#define M25P16_CS_PIN PB12 -#define M25P16_SPI_BUS BUS_SPI2 -#define USE_FLASHFS -#define USE_FLASH_M25P16 -#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT - -#define USE_SPI_DEVICE_2 -#define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PB14 -#define SPI2_MOSI_PIN PB15 - -// *************** OSD ***************************** -#define USE_SPI_DEVICE_1 -#define SPI1_SCK_PIN PA5 -#define SPI1_MISO_PIN PA6 -#define SPI1_MOSI_PIN PA7 - -#define USE_MAX7456 -#define MAX7456_SPI_BUS BUS_SPI1 -#define MAX7456_CS_PIN PA4 - -// *************** UART ***************************** -#define USE_VCP - -#define USE_UART1 -#define UART1_RX_PIN PB7 -#define UART1_TX_PIN PB6 - -#define USE_UART2 -#define UART2_RX_PIN PA3 -#define UART2_TX_PIN PA2 - -#define USE_UART3 -#define UART3_RX_PIN PC11 -#define UART3_TX_PIN PC10 - -#define USE_UART4 -#define UART4_RX_PIN PA1 -#define UART4_TX_PIN PA0 - -#define USE_UART5 -#define UART5_RX_PIN PD2 -#define UART5_TX_PIN PC12 - -#define USE_UART6 -#define UART6_RX_PIN PC7 -#define UART6_TX_PIN PC6 - -#define SERIAL_PORT_COUNT 7 - -#define DEFAULT_RX_TYPE RX_TYPE_SERIAL -#define SERIALRX_PROVIDER SERIALRX_SBUS -#define SERIALRX_UART SERIAL_PORT_USART2 - -// *************** ADC ***************************** -#define USE_ADC -#define ADC_INSTANCE ADC1 -#define ADC1_DMA_STREAM DMA2_Stream0 -#define ADC_CHANNEL_1_PIN PC1 -#define ADC_CHANNEL_2_PIN PC2 -#define ADC_CHANNEL_3_PIN PC0 - -#define VBAT_ADC_CHANNEL ADC_CHN_1 -#define RSSI_ADC_CHANNEL ADC_CHN_2 -#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 - - -#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) - - - -// *************** LEDSTRIP ************************ -#define USE_LED_STRIP -#define WS2811_PIN PB1 - - - -#define USE_SERIAL_4WAY_BLHELI_INTERFACE - -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) - - -#define MAX_PWM_OUTPUT_PORTS 6 - -#define USE_DSHOT -#define USE_ESC_SENSOR +/* + * This file is part of INAV Project. + * + * 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 "GEPR" + +#define USBD_PRODUCT_STRING "GEPRCF405" + +#define LED0 PC14 +#define LED1 PC15 + + + +#define BEEPER PC13 +#define BEEPER_INVERTED + +// *************** Gyro & ACC ********************** + + + +#define USE_SPI +#define USE_SPI_DEVICE_3 + +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW180_DEG +#define MPU6000_CS_PIN PA15 +#define MPU6000_SPI_BUS BUS_SPI3 + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW180_DEG +#define BMI270_CS_PIN PA15 +#define BMI270_SPI_BUS BUS_SPI3 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW180_DEG +#define ICM42605_CS_PIN PA15 +#define ICM42605_SPI_BUS BUS_SPI3 + +// *************** I2C/Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_DPS310 +#define USE_BARO_MS5611 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define TEMPERATURE_I2C_BUS BUS_I2C1 + +#define PITOT_I2C_BUS BUS_I2C1 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 + +// *************** FLASH ************************** +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_BUS BUS_SPI2 +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +// *************** OSD ***************************** +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI1 +#define MAX7456_CS_PIN PA4 + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PB7 +#define UART1_TX_PIN PB6 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PC11 +#define UART3_TX_PIN PC10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 7 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC1 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC0 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define RSSI_ADC_CHANNEL ADC_CHN_2 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 + + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) + + + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PB1 + + + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + + +#define MAX_PWM_OUTPUT_PORTS 6 + +#define USE_DSHOT +#define USE_ESC_SENSOR diff --git a/src/main/target/GEPRCF722/CMakeLists.txt b/src/main/target/GEPRCF722/CMakeLists.txt index 2d7f6c7d95f..8164cb86230 100644 --- a/src/main/target/GEPRCF722/CMakeLists.txt +++ b/src/main/target/GEPRCF722/CMakeLists.txt @@ -1 +1 @@ -target_stm32f722xe(GEPRCF722) +target_stm32f722xe(GEPRCF722) diff --git a/src/main/target/GEPRCF722/target.c b/src/main/target/GEPRCF722/target.c index 3514ca283ff..8e206e7b199 100644 --- a/src/main/target/GEPRCF722/target.c +++ b/src/main/target/GEPRCF722/target.c @@ -1,44 +1,44 @@ -/* -* This file is part of INAV Project. -* -* 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 "platform.h" - -#include "drivers/bus.h" -#include "drivers/io.h" -#include "drivers/pwm_mapping.h" -#include "drivers/timer.h" -#include "drivers/sensor.h" - - -timerHardware_t timerHardware[] = { - DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), - - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - - DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), - - -}; - -const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); +/* +* This file is part of INAV Project. +* +* 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 "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/sensor.h" + + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM, 0, 0), + + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), + + +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/GEPRCF722/target.h b/src/main/target/GEPRCF722/target.h index e4f3b9c72da..bc08477c451 100644 --- a/src/main/target/GEPRCF722/target.h +++ b/src/main/target/GEPRCF722/target.h @@ -1,173 +1,173 @@ -/* - * This file is part of INAV Project. - * - * 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 "GEPR" - -#define USBD_PRODUCT_STRING "GEPRCF722" - -#define LED0 PA13 -#define LED1 PA14 - -#define BEEPER PC13 -#define BEEPER_INVERTED - -// *************** Gyro & ACC ********************** - -#define USE_SPI -#define USE_SPI_DEVICE_3 - -#define SPI3_SCK_PIN PC10 -#define SPI3_MISO_PIN PC11 -#define SPI3_MOSI_PIN PC12 - - -#define USE_IMU_MPU6000 -#define IMU_MPU6000_ALIGN CW270_DEG - -#define MPU6000_CS_PIN PA15 -#define MPU6000_SPI_BUS BUS_SPI3 - -#define USE_IMU_BMI270 -#define IMU_BMI270_ALIGN CW180_DEG -#define BMI270_CS_PIN PA15 -#define BMI270_SPI_BUS BUS_SPI3 - -#define USE_IMU_ICM42605 -#define IMU_ICM42605_ALIGN CW180_DEG -#define ICM42605_CS_PIN PA15 -#define ICM42605_SPI_BUS BUS_SPI3 - -// *************** I2C/Baro/Mag ********************* -#define USE_I2C -#define USE_I2C_DEVICE_1 -#define I2C1_SCL PB8 -#define I2C1_SDA PB9 - -#define USE_BARO -#define BARO_I2C_BUS BUS_I2C1 -#define USE_BARO_BMP280 -#define USE_BARO_DPS310 -#define USE_BARO_MS5611 - -#define USE_MAG -#define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL - -#define TEMPERATURE_I2C_BUS BUS_I2C1 - -#define PITOT_I2C_BUS BUS_I2C1 - -#define USE_RANGEFINDER -#define RANGEFINDER_I2C_BUS BUS_I2C1 -#define BNO055_I2C_BUS BUS_I2C1 - -// *************** FLASH ************************** -#define M25P16_CS_PIN PB12 -#define M25P16_SPI_BUS BUS_SPI2 -#define USE_FLASHFS -#define USE_FLASH_M25P16 -#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT - -#define USE_SPI_DEVICE_2 -#define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PB14 -#define SPI2_MOSI_PIN PB15 - -// *************** OSD ***************************** -#define USE_SPI_DEVICE_1 -#define SPI1_SCK_PIN PA5 -#define SPI1_MISO_PIN PA6 -#define SPI1_MOSI_PIN PA7 - -#define USE_MAX7456 -#define MAX7456_SPI_BUS BUS_SPI1 -#define MAX7456_CS_PIN PB2 -// *************** UART ***************************** -#define USE_VCP - -#define USE_UART1 -#define UART1_RX_PIN PA10 -#define UART1_TX_PIN PA9 - -#define USE_UART2 -#define UART2_RX_PIN PA3 -#define UART2_TX_PIN PA2 - -#define USE_UART3 -#define UART3_RX_PIN PB11 -#define UART3_TX_PIN PB10 - -#define USE_UART4 -#define UART4_RX_PIN PA1 -#define UART4_TX_PIN PA0 - -#define USE_UART6 -#define UART6_RX_PIN PC7 -#define UART6_TX_PIN PC6 - -#define SERIAL_PORT_COUNT 6 - -#define DEFAULT_RX_TYPE RX_TYPE_SERIAL -#define SERIALRX_PROVIDER SERIALRX_SBUS -#define SERIALRX_UART SERIAL_PORT_USART2 - -// *************** ADC ***************************** -#define USE_ADC -#define ADC_INSTANCE ADC1 -#define ADC1_DMA_STREAM DMA2_Stream0 -#define ADC_CHANNEL_1_PIN PC0 -#define ADC_CHANNEL_2_PIN PC2 -#define ADC_CHANNEL_3_PIN PC1 - -#define VBAT_ADC_CHANNEL ADC_CHN_1 -#define RSSI_ADC_CHANNEL ADC_CHN_2 -#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 - - -#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) - - -// ************PINIO to other -#define PINIO1_PIN PC8 //Enable vsw -#define PINIO2_PIN PC9 - - - -// *************** LEDSTRIP ************************ -#define USE_LED_STRIP -#define WS2811_PIN PA8 - - - -#define USE_SERIAL_4WAY_BLHELI_INTERFACE - -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) - -#define MAX_PWM_OUTPUT_PORTS 6 - -#define USE_DSHOT -#define USE_ESC_SENSOR +/* + * This file is part of INAV Project. + * + * 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 "GEPR" + +#define USBD_PRODUCT_STRING "GEPRCF722" + +#define LED0 PA13 +#define LED1 PA14 + +#define BEEPER PC13 +#define BEEPER_INVERTED + +// *************** Gyro & ACC ********************** + +#define USE_SPI +#define USE_SPI_DEVICE_3 + +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW270_DEG + +#define MPU6000_CS_PIN PA15 +#define MPU6000_SPI_BUS BUS_SPI3 + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW180_DEG +#define BMI270_CS_PIN PA15 +#define BMI270_SPI_BUS BUS_SPI3 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW180_DEG +#define ICM42605_CS_PIN PA15 +#define ICM42605_SPI_BUS BUS_SPI3 + +// *************** I2C/Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_DPS310 +#define USE_BARO_MS5611 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define TEMPERATURE_I2C_BUS BUS_I2C1 + +#define PITOT_I2C_BUS BUS_I2C1 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 + +// *************** FLASH ************************** +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_BUS BUS_SPI2 +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +// *************** OSD ***************************** +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI1 +#define MAX7456_CS_PIN PB2 +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 6 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC1 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define RSSI_ADC_CHANNEL ADC_CHN_2 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 + + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) + + +// ************PINIO to other +#define PINIO1_PIN PC8 //Enable vsw +#define PINIO2_PIN PC9 + + + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + + + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 6 + +#define USE_DSHOT +#define USE_ESC_SENSOR From ba8e13b89aba7f04b9b2d053d7a4aa2512e67369 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 1 Sep 2023 22:33:58 +0200 Subject: [PATCH 13/39] Add cli command to change timer output --- src/main/fc/cli.c | 88 +++++++++++++++++++++++++++++++++++++++++ src/main/flight/mixer.c | 9 +++++ src/main/flight/mixer.h | 10 ++++- 3 files changed, 106 insertions(+), 1 deletion(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index a14aac38a28..73d1bf93a2a 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2514,6 +2514,90 @@ static void cliOsdLayout(char *cmdline) #endif +static char *outputModeName(outputMode_e outputMode) +{ + switch (outputMode) { + default: + FALLTHROUGH; + case OUTPUT_MODE_AUTO: + return "AUTO"; + case OUTPUT_MODE_MOTORS: + return "MOTORS"; + case OUTPUT_MODE_SERVOS: + return "SERVOS"; + } +} + +static void printTimerOutputModes(dumpFlags_e dumpFlags, int timer) +{ + for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) { + if (timer < 0 || timer == i) { + outputMode_e mode = timerOverrides(i)->outputMode; + cliDefaultPrintLinef(dumpFlags, mode == OUTPUT_MODE_AUTO, + "timer_output_mode %d %s", i, outputModeName(mode)); + cliDumpPrintLinef(dumpFlags, mode != OUTPUT_MODE_AUTO, + "timer_output_mode %d %s", i, outputModeName(mode)); + } + } +} + +static void cliTimerOutputMode(char *cmdline) +{ + char * saveptr; + + int timer = -1; + uint8_t mode; + char *tok = strtok_r(cmdline, " ", &saveptr); + + int ii; + + for (ii = 0; tok != NULL; ii++, tok = strtok_r(NULL, " ", &saveptr)) { + switch (ii) { + case 0: + timer = fastA2I(tok); + if (timer < 0 || timer >= HARDWARE_TIMER_DEFINITION_COUNT) { + cliShowParseError(); + return; + } + break; + case 1: + if(!sl_strcasecmp("AUTO", tok)) { + mode = OUTPUT_MODE_AUTO; + } else if(!sl_strcasecmp("MOTORS", tok)) { + mode = OUTPUT_MODE_MOTORS; + } else if(!sl_strcasecmp("SERVOS", tok)) { + mode = OUTPUT_MODE_SERVOS; + } else { + cliShowParseError(); + return; + } + break; + default: + cliShowParseError(); + return; + } + } + + switch (ii) { + case 0: + FALLTHROUGH; + case 1: + // No args, or just timer. If any of them not provided, + // it will be the -1 that we used during initialization, so printOsdLayout() + // won't use them for filtering. + printTimerOutputModes(DUMP_MASTER, timer); + break; + case 2: + timerOverridesMutable(timer)->outputMode = mode; + break; + default: + // Unhandled + cliShowParseError(); + return; + } + +} + static void printFeature(uint8_t dumpMask, const featureConfig_t *featureConfig, const featureConfig_t *featureConfigDefault) { uint32_t mask = featureConfig->enabledFeatures; @@ -3652,6 +3736,9 @@ static void printConfig(const char *cmdline, bool doDiff) cliPrintHashLine("resources"); //printResource(dumpMask, &defaultConfig); + cliPrintHashLine("Timer overrides"); + printTimerOutputModes(dumpMask, -1); + cliPrintHashLine("Mixer: motor mixer"); cliDumpPrintLinef(dumpMask, primaryMotorMixer_CopyArray[0].throttle == 0.0f, "\r\nmmix reset\r\n"); @@ -3968,6 +4055,7 @@ const clicmd_t cmdTable[] = { #ifdef USE_OSD CLI_COMMAND_DEF("osd_layout", "get or set the layout of OSD items", "[ [ [ []]]]", cliOsdLayout), #endif + CLI_COMMAND_DEF("timer_output_mode", "get or set the outputmode for a given timer.", "[ []]", cliTimerOutputMode), }; static void cliHelp(char *cmdline) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 765c2f1637d..cf72ed8c05b 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -103,8 +103,17 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0); +PG_REGISTER_ARRAY_WITH_RESET_FN(timerOverride_t, HARDWARE_TIMER_DEFINITION_COUNT, timerOverrides, PG_MIXER_CONFIG, 0); + #define CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN 0.15f +void pgResetFn_timerOverrides(timerOverride_t *instance) +{ + for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) { + instance[i].outputMode = OUTPUT_MODE_AUTO; + } +} + int getThrottleIdleValue(void) { if (!throttleIdleValue) { diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index f638d7335fd..f23cace9065 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -19,6 +19,8 @@ #include "config/parameter_group.h" +#include "drivers/timer.h" + #if defined(TARGET_MOTOR_COUNT) #define MAX_SUPPORTED_MOTORS TARGET_MOTOR_COUNT #else @@ -64,6 +66,12 @@ typedef struct motorMixer_s { PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer); +typedef struct timerOverride_s { + uint8_t outputMode; +} timerOverride_t; + +PG_DECLARE_ARRAY(timerOverride_t, HARDWARE_TIMER_DEFINITION_COUNT, timerOverrides); + typedef struct mixerConfig_s { int8_t motorDirectionInverted; uint8_t platformType; @@ -130,4 +138,4 @@ void stopMotors(void); void stopPwmAllMotors(void); void loadPrimaryMotorMixer(void); -bool areMotorsRunning(void); +bool areMotorsRunning(void); \ No newline at end of file From 37b882ae26c69927849200fbac91aa5cb667977b Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 2 Sep 2023 00:26:38 +0200 Subject: [PATCH 14/39] Fix overriding saving and plug it in output mode overriding code. output_mode sets default, inidividual timer is processed afterwards. --- src/main/config/parameter_group_ids.h | 3 ++- src/main/drivers/pwm_mapping.c | 15 ++++++++++++ src/main/fc/cli.c | 34 +++++++++++---------------- src/main/flight/mixer.c | 5 ++-- 4 files changed, 34 insertions(+), 23 deletions(-) diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 46293524b14..ac7c04e5dbe 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -119,7 +119,8 @@ #define PG_UNUSED_1 1029 #define PG_POWER_LIMITS_CONFIG 1030 #define PG_OSD_COMMON_CONFIG 1031 -#define PG_INAV_END 1031 +#define PG_TIMER_OVERRIDE_CONFIG 1032 +#define PG_INAV_END 1032 // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 3b61d95e385..6a91d946397 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -235,6 +235,21 @@ static void timerHardwareOverride(timerHardware_t * timer) { timer->usageFlags = timer->usageFlags | TIM_USE_FW_MOTOR; } } + + switch (timerOverrides(timer2id(timer->tim))->outputMode) { + case OUTPUT_MODE_MOTORS: + if (timer->usageFlags & (TIM_USE_MC_SERVO | TIM_USE_FW_SERVO)) { + timer->usageFlags &= ~(TIM_USE_MC_SERVO | TIM_USE_FW_SERVO); + timer->usageFlags |= TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR; + } + break; + case OUTPUT_MODE_SERVOS: + if (timer->usageFlags & (TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR)) { + timer->usageFlags &= ~(TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR); + timer->usageFlags |= TIM_USE_MC_SERVO | TIM_USE_FW_SERVO; + } + break; + } } bool pwmHasMotorOnTimer(timMotorServoHardware_t * timOutputs, HAL_Timer_t *tim) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 73d1bf93a2a..9a99f72955c 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -156,6 +156,13 @@ static const char * const featureNames[] = { "OSD", "FW_LAUNCH", "FW_AUTOTRIM", NULL }; +static const char * outputModeNames[] = { + "AUTO", + "MOTORS", + "SERVOS", + NULL +}; + #ifdef USE_BLACKBOX static const char * const blackboxIncludeFlagNames[] = { "NAV_ACC", @@ -2514,29 +2521,15 @@ static void cliOsdLayout(char *cmdline) #endif -static char *outputModeName(outputMode_e outputMode) -{ - switch (outputMode) { - default: - FALLTHROUGH; - case OUTPUT_MODE_AUTO: - return "AUTO"; - case OUTPUT_MODE_MOTORS: - return "MOTORS"; - case OUTPUT_MODE_SERVOS: - return "SERVOS"; - } -} - -static void printTimerOutputModes(dumpFlags_e dumpFlags, int timer) +static void printTimerOutputModes(dumpFlags_e dumpFlags, const timerOverride_t* to, int timer) { for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) { if (timer < 0 || timer == i) { - outputMode_e mode = timerOverrides(i)->outputMode; + outputMode_e mode = to[i].outputMode; cliDefaultPrintLinef(dumpFlags, mode == OUTPUT_MODE_AUTO, - "timer_output_mode %d %s", i, outputModeName(mode)); + "timer_output_mode %d %s", i, outputModeNames[mode]); cliDumpPrintLinef(dumpFlags, mode != OUTPUT_MODE_AUTO, - "timer_output_mode %d %s", i, outputModeName(mode)); + "timer_output_mode %d %s", i, outputModeNames[mode]); } } } @@ -2585,10 +2578,11 @@ static void cliTimerOutputMode(char *cmdline) // No args, or just timer. If any of them not provided, // it will be the -1 that we used during initialization, so printOsdLayout() // won't use them for filtering. - printTimerOutputModes(DUMP_MASTER, timer); + printTimerOutputModes(DUMP_MASTER, timerOverrides(0), timer); break; case 2: timerOverridesMutable(timer)->outputMode = mode; + printTimerOutputModes(DUMP_MASTER, timerOverrides(0), timer); break; default: // Unhandled @@ -3737,7 +3731,7 @@ static void printConfig(const char *cmdline, bool doDiff) //printResource(dumpMask, &defaultConfig); cliPrintHashLine("Timer overrides"); - printTimerOutputModes(dumpMask, -1); + printTimerOutputModes(dumpMask, timerOverrides_CopyArray, -1); cliPrintHashLine("Mixer: motor mixer"); cliDumpPrintLinef(dumpMask, primaryMotorMixer_CopyArray[0].throttle == 0.0f, "\r\nmmix reset\r\n"); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index cf72ed8c05b..dd17241e941 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -28,6 +28,7 @@ #include "common/maths.h" #include "common/utils.h" +#include "config/config_reset.h" #include "config/feature.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" @@ -103,14 +104,14 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0); -PG_REGISTER_ARRAY_WITH_RESET_FN(timerOverride_t, HARDWARE_TIMER_DEFINITION_COUNT, timerOverrides, PG_MIXER_CONFIG, 0); +PG_REGISTER_ARRAY_WITH_RESET_FN(timerOverride_t, HARDWARE_TIMER_DEFINITION_COUNT, timerOverrides, PG_TIMER_OVERRIDE_CONFIG, 0); #define CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN 0.15f void pgResetFn_timerOverrides(timerOverride_t *instance) { for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) { - instance[i].outputMode = OUTPUT_MODE_AUTO; + RESET_CONFIG(timerOverride_t, &instance[i], .outputMode = OUTPUT_MODE_AUTO); } } From 711945ecb41da71fc1ec5f77ed59ed8aa73d615a Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 2 Sep 2023 11:06:24 +0200 Subject: [PATCH 15/39] Fix motor allocation on fw, when overriding first timer group as servos only --- src/main/drivers/pwm_mapping.c | 32 ++++++++++++-------------------- 1 file changed, 12 insertions(+), 20 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 6a91d946397..a3594af2de2 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -212,27 +212,16 @@ static bool checkPwmTimerConflicts(const timerHardware_t *timHw) static void timerHardwareOverride(timerHardware_t * timer) { if (mixerConfig()->outputMode == OUTPUT_MODE_SERVOS) { - //Motors are rewritten as servos - if (timer->usageFlags & TIM_USE_MC_MOTOR) { - timer->usageFlags = timer->usageFlags & ~TIM_USE_MC_MOTOR; - timer->usageFlags = timer->usageFlags | TIM_USE_MC_SERVO; + if (timer->usageFlags & (TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR)) { + timer->usageFlags &= ~(TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR); + timer->usageFlags |= TIM_USE_MC_SERVO | TIM_USE_FW_SERVO; } - if (timer->usageFlags & TIM_USE_FW_MOTOR) { - timer->usageFlags = timer->usageFlags & ~TIM_USE_FW_MOTOR; - timer->usageFlags = timer->usageFlags | TIM_USE_FW_SERVO; - } - } else if (mixerConfig()->outputMode == OUTPUT_MODE_MOTORS) { - // Servos are rewritten as motors - if (timer->usageFlags & TIM_USE_MC_SERVO) { - timer->usageFlags = timer->usageFlags & ~TIM_USE_MC_SERVO; - timer->usageFlags = timer->usageFlags | TIM_USE_MC_MOTOR; - } - if (timer->usageFlags & TIM_USE_FW_SERVO) { - timer->usageFlags = timer->usageFlags & ~TIM_USE_FW_SERVO; - timer->usageFlags = timer->usageFlags | TIM_USE_FW_MOTOR; + if (timer->usageFlags & (TIM_USE_MC_SERVO | TIM_USE_FW_SERVO)) { + timer->usageFlags &= ~(TIM_USE_MC_SERVO | TIM_USE_FW_SERVO); + timer->usageFlags |= TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR; } } @@ -321,7 +310,9 @@ void pwmEnsureEnoughtMotors(uint8_t motorCount) continue; } - if ((timHw->usageFlags & (TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO)) != TIM_USE_MC_MOTOR) { + uint32_t mcFlags = timHw->usageFlags & (TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO); + + if (mcFlags && mcFlags != TIM_USE_MC_MOTOR) { timHw->usageFlags &= ~TIM_USE_MC_SERVO; timHw->usageFlags |= TIM_USE_MC_MOTOR; mcMotorOnlyOutputs++; @@ -336,7 +327,8 @@ void pwmEnsureEnoughtMotors(uint8_t motorCount) continue; } - if ((timHw->usageFlags & (TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO)) != TIM_USE_FW_MOTOR) { + uint32_t fwFlags = timHw->usageFlags & (TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO); + if (fwFlags && fwFlags != TIM_USE_FW_MOTOR) { timHw->usageFlags &= ~TIM_USE_FW_SERVO; timHw->usageFlags |= TIM_USE_FW_MOTOR; motorOnlyOutputs++; @@ -389,7 +381,7 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU } else { // Make sure first motorCount motor outputs get assigned to motor if ((timHw->usageFlags & TIM_USE_FW_MOTOR) && (motorIdx < motorCount)) { - timHw->usageFlags = timHw->usageFlags & ~TIM_USE_MC_SERVO; + timHw->usageFlags = timHw->usageFlags & ~TIM_USE_FW_SERVO; pwmClaimTimer(timHw->tim, timHw->usageFlags); motorIdx += 1; } From 6d1b33c0621a9361109e8f709c8b3b3363b2d5d6 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sat, 2 Sep 2023 11:38:34 +0100 Subject: [PATCH 16/39] initial build --- src/main/flight/mixer.c | 61 +++++++++++++++++++---------------------- src/main/io/osd.c | 39 ++++++++++++++------------ 2 files changed, 49 insertions(+), 51 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 765c2f1637d..a89255c1b12 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -208,6 +208,7 @@ void mixerInit(void) void mixerResetDisarmedMotors(void) { + getThrottleIdleValue(); if (feature(FEATURE_REVERSIBLE_MOTORS)) { motorZeroCommand = reversibleMotorsConfig()->neutral; @@ -215,7 +216,7 @@ void mixerResetDisarmedMotors(void) throttleRangeMax = motorConfig()->maxthrottle; } else { motorZeroCommand = motorConfig()->mincommand; - throttleRangeMin = getThrottleIdleValue(); + throttleRangeMin = throttleIdleValue; throttleRangeMax = motorConfig()->maxthrottle; } @@ -224,7 +225,7 @@ void mixerResetDisarmedMotors(void) if (feature(FEATURE_MOTOR_STOP)) { motorValueWhenStopped = motorZeroCommand; } else { - motorValueWhenStopped = getThrottleIdleValue(); + motorValueWhenStopped = throttleIdleValue; } // set disarmed motor values @@ -469,6 +470,19 @@ void FAST_CODE mixTable(void) return; } #endif +#ifdef USE_DEV_TOOLS + bool isDisarmed = !ARMING_FLAG(ARMED) || systemConfig()->groundTestMode; +#else + bool isDisarmed = !ARMING_FLAG(ARMED); +#endif + bool motorStopIsActive = getMotorStatus() != MOTOR_RUNNING && !isDisarmed; + if (isDisarmed || motorStopIsActive) { + for (int i = 0; i < motorCount; i++) { + motor[i] = isDisarmed ? motor_disarmed[i] : motorValueWhenStopped; + } + mixerThrottleCommand = motor[0]; + return; + } int16_t input[3]; // RPY, range [-500:+500] // Allow direct stick input to motors in passthrough mode on airplanes @@ -551,11 +565,11 @@ void FAST_CODE mixTable(void) throttleRangeMax = motorConfig()->maxthrottle; // Throttle scaling to limit max throttle when battery is full - #ifdef USE_PROGRAMMING_FRAMEWORK +#ifdef USE_PROGRAMMING_FRAMEWORK mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * getThrottleScale(currentBatteryProfile->motor.throttleScale)) + throttleRangeMin; - #else +#else mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * currentBatteryProfile->motor.throttleScale) + throttleRangeMin; - #endif +#endif // Throttle compensation based on battery voltage if (feature(FEATURE_THR_VBAT_COMP) && isAmperageConfigured() && feature(FEATURE_VBAT)) { mixerThrottleCommand = MIN(throttleRangeMin + (mixerThrottleCommand - throttleRangeMin) * calculateThrottleCompensationFactor(), throttleRangeMax); @@ -564,7 +578,6 @@ void FAST_CODE mixTable(void) throttleMin = throttleRangeMin; throttleMax = throttleRangeMax; - throttleRange = throttleMax - throttleMin; #define THROTTLE_CLIPPING_FACTOR 0.33f @@ -584,41 +597,23 @@ void FAST_CODE mixTable(void) // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. - if (ARMING_FLAG(ARMED)) { - const motorStatus_e currentMotorStatus = getMotorStatus(); - for (int i = 0; i < motorCount; i++) { - motor[i] = rpyMix[i] + constrain(mixerThrottleCommand * currentMixer[i].throttle, throttleMin, throttleMax); - - if (failsafeIsActive()) { - motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle); - } else { - motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax); - } + for (int i = 0; i < motorCount; i++) { + motor[i] = rpyMix[i] + constrain(mixerThrottleCommand * currentMixer[i].throttle, throttleMin, throttleMax); - // Motor stop handling - if (currentMotorStatus != MOTOR_RUNNING) { - motor[i] = motorValueWhenStopped; - } -#ifdef USE_DEV_TOOLS - if (systemConfig()->groundTestMode) { - motor[i] = motorZeroCommand; - } -#endif - } - } else { - for (int i = 0; i < motorCount; i++) { - motor[i] = motor_disarmed[i]; + if (failsafeIsActive()) { + motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle); + } else { + motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax); } } } int16_t getThrottlePercent(bool useScaled) { - int16_t thr = constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX); - const int idleThrottle = getThrottleIdleValue(); - + int16_t thr = constrain(mixerThrottleCommand, PWM_RANGE_MIN, PWM_RANGE_MAX); + if (useScaled) { - thr = (thr - idleThrottle) * 100 / (motorConfig()->maxthrottle - idleThrottle); + thr = (thr - throttleIdleValue) * 100 / (motorConfig()->maxthrottle - throttleIdleValue); } else { thr = (rxGetChannelValue(THROTTLE) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); } diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 8983fc5792d..d65aca572a6 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -991,10 +991,10 @@ static const char * osdFailsafeInfoMessage(void) #if defined(USE_SAFE_HOME) static const char * divertingToSafehomeMessage(void) { - if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && posControl.safehomeState.isApplied) { - return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME); - } - return NULL; + if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && posControl.safehomeState.isApplied) { + return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME); + } + return NULL; } #endif @@ -1116,11 +1116,7 @@ bool osdUsingScaledThrottle(void) **/ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes_t *elemAttr) { - if (useScaled) { - buff[0] = SYM_SCALE; - } else { - buff[0] = SYM_BLANK; - } + buff[0] = SYM_BLANK; buff[1] = SYM_THR; if (navigationIsControllingThrottle()) { buff[0] = SYM_AUTO_THR0; @@ -1135,7 +1131,14 @@ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr); } #endif - tfp_sprintf(buff + 2, "%3d", getThrottlePercent(useScaled)); + int8_t throttlePercent = getThrottlePercent(useScaled); + if (useScaled && throttlePercent <= 0) { + const char* message = ARMING_FLAG(ARMED) ? throttlePercent == 0 ? "IDLE" : "STOP" : "DARM"; + buff[0] = SYM_THR; + strcpy(buff + 1, message); + return; + } + tfp_sprintf(buff + 2, "%3d", throttlePercent); } /** @@ -4445,14 +4448,14 @@ static void osdShowArmed(void) if (posControl.safehomeState.distance) { // safehome found during arming if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) { strcpy(buf, "SAFEHOME FOUND; MODE OFF"); - } else { - char buf2[12]; // format the distance first - osdFormatDistanceStr(buf2, posControl.safehomeState.distance); - tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, posControl.safehomeState.index); - } - textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; - // write this message above the ARMED message to make it obvious - displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr); + } else { + char buf2[12]; // format the distance first + osdFormatDistanceStr(buf2, posControl.safehomeState.distance); + tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, posControl.safehomeState.index); + } + textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; + // write this message above the ARMED message to make it obvious + displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr); } #endif } else { From 52726419124fcf4b5c1448bedb93a28eabf52339 Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Sat, 2 Sep 2023 12:11:08 +0100 Subject: [PATCH 17/39] [doc] update Cli.md for PWM auto-allocation --- docs/Cli.md | 61 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 61 insertions(+) diff --git a/docs/Cli.md b/docs/Cli.md index c805fe9250b..a0dc097cc73 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -114,6 +114,7 @@ While connected to the CLI, all Logical Switches are temporarily disabled (5.1.0 | `status` | Show status. Error codes can be looked up [here](https://github.com/iNavFlight/inav/wiki/%22Something%22-is-disabled----Reasons) | | `tasks` | Show task stats | | `temp_sensor` | List or configure temperature sensor(s). See [temperature sensors documentation](Temperature-sensors.md) for more information. | +| `timer_output_mode` | Override automatic timer / pwm function allocation. [Additional Information](#timer_outout_mode)| | `version` | Show version | | `wp` | List or configure waypoints. See the [navigation documentation](Navigation.md#cli-command-wp-to-manage-waypoints). | @@ -170,6 +171,66 @@ serial 0 -4 `serial` can also be used without any argument to print the current configuration of all the serial ports. +### `timer_output_mode` + +Since INAV 7, the firmware can dynamically allocate servo and motor outputs. This removes the need for bespoke targets for special cases (e.g. `MATEKF405` and `MATEKF405_S6SERVO`. + +#### Syntax + +``` +timer_output_mode [timer [function]] +``` +where: +* Without parameters, lists the current timers and modes +* With just a `timer` lists the mode for that timer +* With both `timer` and `function`, sets the function for that timers + +Note: + +* `timer` identifies the timer **index** (from 0); thus is one less than the corresponding `TIMn` definition in a target's `target.c`. +* The function is one of `AUTO` (the default), `MOTORS` or `SERVOS`. + +Motors are allocated first, hence having a servo before a motor may require use of `timer_output_mode`. + +#### Example + +The original `MATEKF405` target defined a multi-rotor (MR) servo on output S1. The later `MATEKF405_S6SERVO` target defined (for MR) S1 as a motor and S6 as a servo. This was more logical, but annoying for anyone who had a legacy `MATEKF405` tricopter with the servo on S1. + +#### Solution + +There is now a single `MATEKF405` target. The `target.c` sets the relevant outputs as: + +``` +DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 +DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 UP(2,1) +DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S3 UP(2,1) +DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 UP(2,1) +DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_LED, 0, 0), // S5 UP(1,7) +DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 UP(2,5) +DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 D(1,7)!S5 UP(2,6) +``` + +Using the "motors first" allocation, the servo would end up on S6, which in the legacy "tricopter servo on S1" case is not desired. + +Forcing the S1 output (`TIM3`) to servo is achieved by: + +``` +timer_output_mode 2 SERVOS +``` + +with resulting `resource` output: + +``` +C06: SERVO4 OUT +C07: MOTOR1 OUT +C08: MOTOR2 OUT +C09: MOTOR3 OUT +``` + +Note that the `timer_id` **index** in the `timer_output_mode` line is one less than the mnemonic in `target.c`, `timer_id` of 2 for `TIM3`. + +Note that the usual caveat that one should not share a timer with both a motor and a servo still apply. + ## Flash chip management For targets that have a flash data chip, typically used for blackbox logs, the following additional comamnds are provided. From fe78217fc198e6971de085513787d34a9a700fa4 Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Sat, 2 Sep 2023 12:15:40 +0100 Subject: [PATCH 18/39] [doc] update Cli.md for PWM auto-allocation - typos --- docs/Cli.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/Cli.md b/docs/Cli.md index a0dc097cc73..b0cf7d20bfc 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -173,7 +173,7 @@ serial 0 -4 ### `timer_output_mode` -Since INAV 7, the firmware can dynamically allocate servo and motor outputs. This removes the need for bespoke targets for special cases (e.g. `MATEKF405` and `MATEKF405_S6SERVO`. +Since INAV 7, the firmware can dynamically allocate servo and motor outputs. This removes the need for bespoke targets for special cases (e.g. `MATEKF405` and `MATEKF405_SERVOS6`). #### Syntax @@ -194,7 +194,7 @@ Motors are allocated first, hence having a servo before a motor may require use #### Example -The original `MATEKF405` target defined a multi-rotor (MR) servo on output S1. The later `MATEKF405_S6SERVO` target defined (for MR) S1 as a motor and S6 as a servo. This was more logical, but annoying for anyone who had a legacy `MATEKF405` tricopter with the servo on S1. +The original `MATEKF405` target defined a multi-rotor (MR) servo on output S1. The later `MATEKF405_SERVOS6` target defined (for MR) S1 as a motor and S6 as a servo. This was more logical, but annoying for anyone who had a legacy `MATEKF405` tricopter with the servo on S1. #### Solution @@ -227,7 +227,7 @@ C08: MOTOR2 OUT C09: MOTOR3 OUT ``` -Note that the `timer_id` **index** in the `timer_output_mode` line is one less than the mnemonic in `target.c`, `timer_id` of 2 for `TIM3`. +Note that the `timer` **index** in the `timer_output_mode` line is one less than the mnemonic in `target.c`, `timer` of 2 for `TIM3`. Note that the usual caveat that one should not share a timer with both a motor and a servo still apply. From 06017a7ff284ac2e57b3ac16300a7e6e2a53aba8 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 2 Sep 2023 21:07:18 +0200 Subject: [PATCH 19/39] First attempt at msp to get/set timer overrides --- src/main/fc/fc_msp.c | 39 +++++++++++++++++++++++++++-- src/main/msp/msp_protocol_v2_inav.h | 2 ++ 2 files changed, 39 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 82907ee07e3..0c2608dbb6a 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1527,7 +1527,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, timerHardware[i].usageFlags); } break; - + case MSP2_INAV_MC_BRAKING: #ifdef USE_MR_BRAKING_MODE sbufWriteU16(dst, navConfig()->mc.braking_speed_threshold); @@ -3673,7 +3673,42 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu *ret = MSP_RESULT_ACK; break; #endif - + case MSP2_INAV_TIMER_OUTPUT_MODE: + if (dataSize == 0) { + for(int i =0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) { + sbufWriteU8(dst, i); + sbufWriteU8(dst, timerOverrides(i)->outputMode); + } + *ret = MSP_RESULT_ACK; + } else if(dataSize == 1) { + uint8_t timer = sbufReadU8(src); + if(timer < HARDWARE_TIMER_DEFINITION_COUNT) { + sbufWriteU8(dst, timer); + sbufWriteU8(dst, timerOverrides(timer)->outputMode); + *ret = MSP_RESULT_ACK; + } else { + *ret = MSP_RESULT_ERROR; + } + } else { + *ret = MSP_RESULT_ERROR; + } + break; + case MSP2_INAV_SET_TIMER_OUTPUT_MODE: + if(dataSize == 2) { + uint8_t timer = sbufReadU8(src); + uint8_t outputMode = sbufReadU8(src); + if(timer < HARDWARE_TIMER_DEFINITION_COUNT) { + timerOverridesMutable(timer)->outputMode = outputMode; + *ret = MSP_RESULT_ACK; + } else { + *ret = MSP_RESULT_ERROR; + } + } else { + *ret = MSP_RESULT_ERROR; + } + break; + + default: // Not handled return false; diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index df7075f9132..f746f5c8ff3 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -32,6 +32,8 @@ #define MSP2_INAV_MC_BRAKING 0x200B #define MSP2_INAV_SET_MC_BRAKING 0x200C #define MSP2_INAV_OUTPUT_MAPPING_EXT 0x200D +#define MSP2_INAV_TIMER_OUTPUT_MODE 0x200E +#define MSP2_INAV_SET_TIMER_OUTPUT_MODE 0x200F #define MSP2_INAV_MIXER 0x2010 #define MSP2_INAV_SET_MIXER 0x2011 From 1a93100f0a5339bbfeb9e95c6a3631cec2c37e4a Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 2 Sep 2023 21:27:08 +0200 Subject: [PATCH 20/39] Fix sitl build --- src/main/fc/fc_msp.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 0c2608dbb6a..a2bc170b0a1 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3673,9 +3673,10 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu *ret = MSP_RESULT_ACK; break; #endif +#ifndef SITL_BUILD case MSP2_INAV_TIMER_OUTPUT_MODE: if (dataSize == 0) { - for(int i =0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) { + for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) { sbufWriteU8(dst, i); sbufWriteU8(dst, timerOverrides(i)->outputMode); } @@ -3707,7 +3708,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu *ret = MSP_RESULT_ERROR; } break; - +#endif default: // Not handled From 053b50163c2065fd96d7ffd96a1f29b79a88bff4 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 5 Sep 2023 21:05:14 +0200 Subject: [PATCH 21/39] Switch MatekF405TE to use burst DMA --- src/main/target/MATEKF411TE/target.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/MATEKF411TE/target.h b/src/main/target/MATEKF411TE/target.h index 7ecb2d1cb67..38f91903dc7 100644 --- a/src/main/target/MATEKF411TE/target.h +++ b/src/main/target/MATEKF411TE/target.h @@ -135,6 +135,7 @@ #define CURRENT_METER_SCALE 250 //F411-WTE 132A #define USE_DSHOT +#define USE_DSHOT_DMAR #define USE_ESC_SENSOR #define USE_SERIAL_4WAY_BLHELI_INTERFACE From 514d53b89644befcc315b8d7897abdd5adf1f978 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 5 Sep 2023 21:38:09 +0200 Subject: [PATCH 22/39] Bump navConfig PG that was forgotten in #9220 --- src/main/navigation/navigation.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 14da4282f8c..fd47f7e058e 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -95,7 +95,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 4); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 5); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { From 812659d5e1e1e84dd5e7c9d0f517dd5b7528060a Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Tue, 5 Sep 2023 23:12:50 +0200 Subject: [PATCH 23/39] Fix diff all --- src/main/fc/cli.c | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 9a99f72955c..ea9ca53991d 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2521,15 +2521,20 @@ static void cliOsdLayout(char *cmdline) #endif -static void printTimerOutputModes(dumpFlags_e dumpFlags, const timerOverride_t* to, int timer) +static void printTimerOutputModes(dumpFlags_e dumpFlags, const timerOverride_t* to, const timerOverride_t* defaultTimerOverride, int timer) { + const char *format = "timer_output_mode %d %s"; + for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; ++i) { if (timer < 0 || timer == i) { outputMode_e mode = to[i].outputMode; - cliDefaultPrintLinef(dumpFlags, mode == OUTPUT_MODE_AUTO, - "timer_output_mode %d %s", i, outputModeNames[mode]); - cliDumpPrintLinef(dumpFlags, mode != OUTPUT_MODE_AUTO, - "timer_output_mode %d %s", i, outputModeNames[mode]); + bool equalsDefault = false; + if(defaultTimerOverride) { + outputMode_e defaultMode = defaultTimerOverride[i].outputMode; + equalsDefault = mode == defaultMode; + cliDefaultPrintLinef(dumpFlags, equalsDefault, format, i, outputModeNames[defaultMode]); + } + cliDumpPrintLinef(dumpFlags, equalsDefault, format, i, outputModeNames[mode]); } } } @@ -2578,11 +2583,11 @@ static void cliTimerOutputMode(char *cmdline) // No args, or just timer. If any of them not provided, // it will be the -1 that we used during initialization, so printOsdLayout() // won't use them for filtering. - printTimerOutputModes(DUMP_MASTER, timerOverrides(0), timer); + printTimerOutputModes(DUMP_MASTER, timerOverrides(0), NULL, timer); break; case 2: timerOverridesMutable(timer)->outputMode = mode; - printTimerOutputModes(DUMP_MASTER, timerOverrides(0), timer); + printTimerOutputModes(DUMP_MASTER, timerOverrides(0), NULL, timer); break; default: // Unhandled @@ -3731,7 +3736,7 @@ static void printConfig(const char *cmdline, bool doDiff) //printResource(dumpMask, &defaultConfig); cliPrintHashLine("Timer overrides"); - printTimerOutputModes(dumpMask, timerOverrides_CopyArray, -1); + printTimerOutputModes(dumpMask, timerOverrides_CopyArray, timerOverrides(0), -1); cliPrintHashLine("Mixer: motor mixer"); cliDumpPrintLinef(dumpMask, primaryMotorMixer_CopyArray[0].throttle == 0.0f, "\r\nmmix reset\r\n"); From 230a85be27ee2ffe4971795f95b8d7997a9736c4 Mon Sep 17 00:00:00 2001 From: Wayne <118108543+wchnflr@users.noreply.github.com> Date: Tue, 5 Sep 2023 22:40:35 -0700 Subject: [PATCH 24/39] Update Building in Windows 10 or 11 with Linux Subsystem.md Removed "python-yaml" as it is now python3-yaml, which is apparently included with python3. Keeping "python-yaml" results in an error message, "Package python-yaml is not available, but is referred to by another package. This may mean that the package is missing, has been obsoleted, or is only available from another source" --- .../Building in Windows 10 or 11 with Linux Subsystem.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/development/Building in Windows 10 or 11 with Linux Subsystem.md b/docs/development/Building in Windows 10 or 11 with Linux Subsystem.md index cf5cc08c061..a2c5894e72f 100644 --- a/docs/development/Building in Windows 10 or 11 with Linux Subsystem.md +++ b/docs/development/Building in Windows 10 or 11 with Linux Subsystem.md @@ -26,7 +26,7 @@ Install Git, Make, gcc and Ruby - `sudo apt-get install git make cmake ruby` Install python and python-yaml to allow updates to settings.md -- `sudo apt-get install python3 python-yaml` +- `sudo apt-get install python3` ### CMAKE and Ubuntu 18_04 From c68c866f48d97f2bead2b4e7483a702dd3d5441b Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 6 Sep 2023 12:32:20 +0200 Subject: [PATCH 25/39] Set correct target to use Burst DMA --- src/main/target/MATEKF405TE/target.h | 1 + src/main/target/MATEKF411TE/target.h | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/MATEKF405TE/target.h b/src/main/target/MATEKF405TE/target.h index 93ab3345def..acaed9daf23 100644 --- a/src/main/target/MATEKF405TE/target.h +++ b/src/main/target/MATEKF405TE/target.h @@ -178,5 +178,6 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define USE_DSHOT +#define USE_DSHOT_DMAR #define USE_ESC_SENSOR diff --git a/src/main/target/MATEKF411TE/target.h b/src/main/target/MATEKF411TE/target.h index 38f91903dc7..7ecb2d1cb67 100644 --- a/src/main/target/MATEKF411TE/target.h +++ b/src/main/target/MATEKF411TE/target.h @@ -135,7 +135,6 @@ #define CURRENT_METER_SCALE 250 //F411-WTE 132A #define USE_DSHOT -#define USE_DSHOT_DMAR #define USE_ESC_SENSOR #define USE_SERIAL_4WAY_BLHELI_INTERFACE From e9d686b9826ee7b2ca5e659f25fc8a9deaec471f Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 7 Sep 2023 12:02:23 +0100 Subject: [PATCH 26/39] Update osd.c --- src/main/io/osd.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index d65aca572a6..9a575e06648 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1116,6 +1116,8 @@ bool osdUsingScaledThrottle(void) **/ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes_t *elemAttr) { + bool scaledThrottleOptionUsed = useScaled; + buff[0] = SYM_BLANK; buff[1] = SYM_THR; if (navigationIsControllingThrottle()) { @@ -1132,7 +1134,7 @@ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes } #endif int8_t throttlePercent = getThrottlePercent(useScaled); - if (useScaled && throttlePercent <= 0) { + if (scaledThrottleOptionUsed && throttlePercent <= 0) { const char* message = ARMING_FLAG(ARMED) ? throttlePercent == 0 ? "IDLE" : "STOP" : "DARM"; buff[0] = SYM_THR; strcpy(buff + 1, message); From 8e25fca0e2131ebf72590bc6702cc459fb95f812 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 7 Sep 2023 18:16:50 +0200 Subject: [PATCH 27/39] Unofficial target limited testing only --- src/main/target/BETAFPVF435/CMakeLists.txt | 1 + src/main/target/BETAFPVF435/target.c | 44 ++++++ src/main/target/BETAFPVF435/target.h | 174 +++++++++++++++++++++ 3 files changed, 219 insertions(+) create mode 100644 src/main/target/BETAFPVF435/CMakeLists.txt create mode 100644 src/main/target/BETAFPVF435/target.c create mode 100644 src/main/target/BETAFPVF435/target.h diff --git a/src/main/target/BETAFPVF435/CMakeLists.txt b/src/main/target/BETAFPVF435/CMakeLists.txt new file mode 100644 index 00000000000..cfac04ed933 --- /dev/null +++ b/src/main/target/BETAFPVF435/CMakeLists.txt @@ -0,0 +1 @@ +target_at32f43x_xGT7(BETAFPVF435 SKIP_RELEASES) diff --git a/src/main/target/BETAFPVF435/target.c b/src/main/target/BETAFPVF435/target.c new file mode 100644 index 00000000000..815781e0c62 --- /dev/null +++ b/src/main/target/BETAFPVF435/target.c @@ -0,0 +1,44 @@ +/* + * This file is part of INAV. + * + * INAV are free software. You can redistribute + * this software and/or modify this software 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. + * + * INAV are distributed in the hope that they + * 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 this software. + * + * If not, see . + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TMR3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // TMR3_CH3 motor 1 + DEF_TIM(TMR3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 2), // TMR3_CH4 motor 2 + DEF_TIM(TMR2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 7), // TMR2_CH4 motor 3 + DEF_TIM(TMR2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 6), // TMR2_CH3 motor 4 + + DEF_TIM(TMR8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO |TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO | TIM_USE_ANY, 0, 4), // TMR8_CH3 motor 5 + DEF_TIM(TMR1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO |TIM_USE_FW_MOTOR | TIM_USE_FW_SERVO | TIM_USE_ANY, 0, 5), // TMR1_CH3 motor 6 + + DEF_TIM(TMR4, CH1, PB6, TIM_USE_LED, 0, 0), // TMR4_CH1 LED_STRIP +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); \ No newline at end of file diff --git a/src/main/target/BETAFPVF435/target.h b/src/main/target/BETAFPVF435/target.h new file mode 100644 index 00000000000..388528e5a22 --- /dev/null +++ b/src/main/target/BETAFPVF435/target.h @@ -0,0 +1,174 @@ +/* + * This file is part of INAV. + * + * INAV are free software. You can redistribute + * this software and/or modify this software 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. + * + * INAV are distributed in the hope that they + * 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 this software. + * + * If not, see . + */ +//https://www.arterychip.com/download/DS/DS_AT32F435_437_V2.02-EN.pdf +#pragma once + +#define TARGET_BOARD_IDENTIFIER "BHER" +#define USBD_PRODUCT_STRING "BETAFPVF435" + +#define LED0_PIN PB5 + +#define USE_BEEPER +#define BEEPER_PIN PB4 +#define BEEPER_INVERTED + +//#define ENABLE_DSHOT_DMAR DSHOT_DMAR_AUTO +//#define DSHOT_BITBANG_DEFAULT DSHOT_BITBANG_AUTO +#define ENABLE_DSHOT + +// *************** Gyro & ACC ********************** +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define SPI1_NSS_PIN PA4 +//#define GYRO_1_SPI_INSTANCE SPI1 + +//#define USE_EXTI +//#define USE_GYRO_EXTI +//#define GYRO_1_EXTI_PIN PC4 +//#define USE_MPU_DATA_READY_SIGNAL +//#define ENSURE_MPU_DATA_READY_IS_LOW + +// MPU6000 +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW270_DEG +#define MPU6000_SPI_BUS BUS_SPI1 +#define MPU6000_CS_PIN SPI1_NSS_PIN + +// ICM42605/ICM42688P +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW270_DEG +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN SPI1_NSS_PIN + +//#define USE_ACC +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW270_DEG +#define BMI270_SPI_BUS BUS_SPI1 +#define BMI270_CS_PIN SPI1_NSS_PIN + +// *************** Baro ************************** +#define USE_BARO +#define USE_BARO_BMP280 +#define SPI3_NSS_PIN PB3 +#define BMP280_SPI_BUS BUS_SPI3 +#define BMP280_CS_PIN SPI3_NSS_PIN +#define USE_BARO_DPS310 +#define DPS310_SPI_BUS BUS_SPI3 +#define DPS310_CS_PIN SPI3_NSS_PIN + +// *************** BLACKBOX ************************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define USE_FLASHFS +#define FLASH_CS_PIN PB12 + +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI2 +#define M25P16_CS_PIN FLASH_CS_PIN +#define USE_FLASH_W25N01G // 1Gb NAND flash support +#define W25N01G_SPI_BUS BUS_SPI2 +#define W25N01G_CS_PIN FLASH_CS_PIN +#define USE_FLASH_W25M // Stacked die support +#define W25M_SPI_BUS BUS_SPI2 +#define W25M_CS_PIN FLASH_CS_PIN +#define USE_FLASH_W25M512 // 512Kb (256Kb x 2 stacked) NOR flash support +#define W25M512_SPI_BUS BUS_SPI2 +#define W25M512_CS_PIN FLASH_CS_PIN +#define USE_FLASH_W25M02G // 2Gb (1Gb x 2 stacked) NAND flash support +#define W25M02G_SPI_BUS BUS_SPI2 +#define W25M02G_CS_PIN FLASH_CS_PIN + +// *************** OSD ***************************** +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI3 +#define MAX7456_CS_PIN PA15 + +// *************** UART ***************************** +#define USE_VCP +#define USE_USB_DETECT + + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 +//#define USE_UART_INVERTER +//#define INVERTER_PIN_UART3_RX PC9 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PD2 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 6 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART3 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_OPT 11 + +#define VBAT_ADC_PIN PC2 +#define CURRENT_METER_ADC_PIN PC1 + +#define USE_ESCSERIAL + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_LED_STRIP) +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC + +#define VBAT_SCALE_DEFAULT 110 +#define CURRENT_METER_SCALE_DEFAULT 800 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE BIT(2) + +#define MAX_PWM_OUTPUT_PORTS 28 +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(6) | TIM_N(7) | TIM_N(8) ) From dc08b8c0a9c78e4d852fad530616dabad9bc7043 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 7 Sep 2023 20:46:41 +0200 Subject: [PATCH 28/39] Boots and works with configurator --- src/main/target/BETAFPVF435/config.c | 34 +++++++++++++++++ src/main/target/BETAFPVF435/target.h | 57 ++++++++++++++++++++-------- 2 files changed, 76 insertions(+), 15 deletions(-) create mode 100644 src/main/target/BETAFPVF435/config.c diff --git a/src/main/target/BETAFPVF435/config.c b/src/main/target/BETAFPVF435/config.c new file mode 100644 index 00000000000..617d446d653 --- /dev/null +++ b/src/main/target/BETAFPVF435/config.c @@ -0,0 +1,34 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + * + * This target has been autgenerated by bf2inav.py + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "fc/config.h" + +#include "io/ledstrip.h" + +void targetConfiguration(void) +{ + DEFINE_LED(ledStripConfigMutable()->ledConfigs, 0, 0, COLOR_GREEN, 0, LED_FUNCTION_ARM_STATE, LED_FLAG_OVERLAY(LED_OVERLAY_WARNING), 0); + DEFINE_LED(ledStripConfigMutable()->ledConfigs+1, 0, 1, COLOR_GREEN, 0, LED_FUNCTION_ARM_STATE, LED_FLAG_OVERLAY(LED_OVERLAY_WARNING), 0); +} + diff --git a/src/main/target/BETAFPVF435/target.h b/src/main/target/BETAFPVF435/target.h index 388528e5a22..8478062123a 100644 --- a/src/main/target/BETAFPVF435/target.h +++ b/src/main/target/BETAFPVF435/target.h @@ -23,15 +23,15 @@ #define TARGET_BOARD_IDENTIFIER "BHER" #define USBD_PRODUCT_STRING "BETAFPVF435" -#define LED0_PIN PB5 +#define LED0 PB5 #define USE_BEEPER -#define BEEPER_PIN PB4 +#define BEEPER PB4 #define BEEPER_INVERTED //#define ENABLE_DSHOT_DMAR DSHOT_DMAR_AUTO //#define DSHOT_BITBANG_DEFAULT DSHOT_BITBANG_AUTO -#define ENABLE_DSHOT +//#define ENABLE_DSHOT // *************** Gyro & ACC ********************** #define USE_SPI @@ -110,13 +110,30 @@ #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 +#if 0 #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 +#endif + +#if 0 +// I2C +#define USE_I2C +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 // SCL pad +#define I2C2_SDA PB11 // SDA pad +#define USE_I2C_PULLUP +#endif + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C2 +#define USE_BARO_BMP280 +#define USE_BARO_DPS310 // *************** UART ***************************** #define USE_VCP #define USE_USB_DETECT +//#define USB_DETECT_PIN PC5 #define USE_UART1 @@ -126,8 +143,9 @@ #define USE_UART3 #define UART3_RX_PIN PB11 #define UART3_TX_PIN PB10 -//#define USE_UART_INVERTER -//#define INVERTER_PIN_UART3_RX PC9 +#define USE_UART_INVERTER +#define INVERTER_PIN_UART3_RX PC9 +#define INVERTER_PIN_USART3_RX PC9 #define USE_UART4 #define UART4_RX_PIN PA1 @@ -150,14 +168,18 @@ // *************** ADC ***************************** #define USE_ADC #define ADC_INSTANCE ADC1 -#define ADC1_DMA_OPT 11 - -#define VBAT_ADC_PIN PC2 -#define CURRENT_METER_ADC_PIN PC1 - -#define USE_ESCSERIAL - -#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_LED_STRIP) +#define ADC1_DMA_STREAM DMA2_CHANNEL5 +#define ADC_CHANNEL1_PIN PC2 +#define ADC_CHANNEL2_PIN PC1 +#define ADC_CHANNEL3_PIN PC0 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +#define USE_LED_STRIP +#define WS2811_PIN PB6 + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_LED_STRIP ) #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC @@ -170,5 +192,10 @@ #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTE BIT(2) -#define MAX_PWM_OUTPUT_PORTS 28 -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(6) | TIM_N(7) | TIM_N(8) ) + +#define MAX_PWM_OUTPUT_PORTS 8 + +//#define USE_SERIAL_4WAY_BLHELI_INTERFACE +//#define USE_DSHOT +//#define USE_ESC_SENSOR +//#define USE_ESCSERIAL \ No newline at end of file From 614875c43e39276996e435bf0572d50f25cf9477 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 7 Sep 2023 21:09:07 +0200 Subject: [PATCH 29/39] Fix DEFINE_LED macro --- src/main/io/ledstrip.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index bf057e830cd..ab5b72f3885 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -159,12 +159,12 @@ typedef struct ledStripConfig_s { PG_DECLARE(ledStripConfig_t, ledStripConfig); #define DEFINE_LED(ledConfigPtr, x, y, col, dir, func, ol, params) { \ - ledConfigPtr->led_position = CALCULATE_LED_XY(x, y); \ - ledConfigPtr->led_color = (col); \ - ledConfigPtr->led_direction = (dir); \ - ledConfigPtr->led_function = (func); \ - ledConfigPtr->led_overlay = (ol); \ - ledConfigPtr->led_params = (params); } + (ledConfigPtr)->led_position = CALCULATE_LED_XY(x, y); \ + (ledConfigPtr)->led_color = (col); \ + (ledConfigPtr)->led_direction = (dir); \ + (ledConfigPtr)->led_function = (func); \ + (ledConfigPtr)->led_overlay = (ol); \ + (ledConfigPtr)->led_params = (params); } static inline uint8_t ledGetXY(const ledConfig_t *lcfg) { return (lcfg->led_position); } static inline uint8_t ledGetX(const ledConfig_t *lcfg) { return ((lcfg->led_position >> (LED_X_BIT_OFFSET)) & LED_XY_MASK); } From 2141596381fe98e2ee8902d8bbc33f7578f3af97 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 7 Sep 2023 21:09:59 +0200 Subject: [PATCH 30/39] Polishing --- src/main/target/BETAFPVF435/config.c | 6 ++++-- src/main/target/BETAFPVF435/target.h | 4 ++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/target/BETAFPVF435/config.c b/src/main/target/BETAFPVF435/config.c index 617d446d653..895714f46bb 100644 --- a/src/main/target/BETAFPVF435/config.c +++ b/src/main/target/BETAFPVF435/config.c @@ -28,7 +28,9 @@ void targetConfiguration(void) { - DEFINE_LED(ledStripConfigMutable()->ledConfigs, 0, 0, COLOR_GREEN, 0, LED_FUNCTION_ARM_STATE, LED_FLAG_OVERLAY(LED_OVERLAY_WARNING), 0); - DEFINE_LED(ledStripConfigMutable()->ledConfigs+1, 0, 1, COLOR_GREEN, 0, LED_FUNCTION_ARM_STATE, LED_FLAG_OVERLAY(LED_OVERLAY_WARNING), 0); + ledStripConfig_t *config = ledStripConfigMutable(); + ledConfig_t *lc = config->ledConfigs; + DEFINE_LED(lc, 0, 0, COLOR_RED, 0, LED_FUNCTION_COLOR, LED_FLAG_OVERLAY(LED_OVERLAY_STROBE), 0); + DEFINE_LED(lc+1, 0, 1, COLOR_GREEN, 0, LED_FUNCTION_COLOR, LED_FLAG_OVERLAY(LED_OVERLAY_STROBE), 0); } diff --git a/src/main/target/BETAFPVF435/target.h b/src/main/target/BETAFPVF435/target.h index 8478062123a..c8e17bd7c48 100644 --- a/src/main/target/BETAFPVF435/target.h +++ b/src/main/target/BETAFPVF435/target.h @@ -195,7 +195,7 @@ #define MAX_PWM_OUTPUT_PORTS 8 -//#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define USE_SERIAL_4WAY_BLHELI_INTERFACE //#define USE_DSHOT //#define USE_ESC_SENSOR -//#define USE_ESCSERIAL \ No newline at end of file +#define USE_ESCSERIAL \ No newline at end of file From 313f5a8f1e9688c345d63dc989f75c073de74e7b Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 7 Sep 2023 21:42:52 +0100 Subject: [PATCH 31/39] Update osd.c --- src/main/io/osd.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 9a575e06648..e40fa1dee1b 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1116,8 +1116,6 @@ bool osdUsingScaledThrottle(void) **/ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes_t *elemAttr) { - bool scaledThrottleOptionUsed = useScaled; - buff[0] = SYM_BLANK; buff[1] = SYM_THR; if (navigationIsControllingThrottle()) { @@ -1134,7 +1132,7 @@ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes } #endif int8_t throttlePercent = getThrottlePercent(useScaled); - if (scaledThrottleOptionUsed && throttlePercent <= 0) { + if ((useScaled && throttlePercent <= 0) || !ARMING_FLAG(ARMED)) { const char* message = ARMING_FLAG(ARMED) ? throttlePercent == 0 ? "IDLE" : "STOP" : "DARM"; buff[0] = SYM_THR; strcpy(buff + 1, message); From f1afdf871585531e116db27c6a6c9866e76f6d18 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 7 Sep 2023 21:52:52 +0100 Subject: [PATCH 32/39] failsafe fixes --- src/main/flight/failsafe.c | 5 +++++ src/main/io/osd.c | 13 +++++++------ src/main/navigation/navigation.c | 4 +--- 3 files changed, 13 insertions(+), 9 deletions(-) diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 2e179f67bb1..2b23738de6c 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -146,6 +146,10 @@ static const failsafeProcedureLogic_t failsafeProcedureLogic[] = { */ void failsafeReset(void) { + if (failsafeState.active) { // can't reset if still active + return; + } + failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND; failsafeState.rxDataRecoveryPeriod = PERIOD_RXDATA_RECOVERY + failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND; failsafeState.validRxDataReceivedAt = 0; @@ -157,6 +161,7 @@ void failsafeReset(void) failsafeState.phase = FAILSAFE_IDLE; failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN; failsafeState.activeProcedure = failsafeConfig()->failsafe_procedure; + failsafeState.controlling = false; failsafeState.lastGoodRcCommand[ROLL] = 0; failsafeState.lastGoodRcCommand[PITCH] = 0; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f6c97e4733d..2845e946a1d 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -831,13 +831,14 @@ static const char * osdArmingDisabledReasonMessage(void) // See handling of FAILSAFE_RX_LOSS_MONITORING in failsafe.c if (failsafePhase() == FAILSAFE_RX_LOSS_MONITORING) { if (failsafeIsReceivingRxData()) { - // If we're not using sticks, it means the ARM switch - // hasn't been off since entering FAILSAFE_RX_LOSS_MONITORING - // yet - return OSD_MESSAGE_STR(OSD_MSG_TURN_ARM_SW_OFF); + // reminder to disarm to exit FAILSAFE_RX_LOSS_MONITORING once timeout period ends + if (IS_RC_MODE_ACTIVE(BOXARM)) { + return OSD_MESSAGE_STR(OSD_MSG_TURN_ARM_SW_OFF); + } + } else { + // Not receiving RX data + return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST); } - // Not receiving RX data - return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST); } return OSD_MESSAGE_STR(OSD_MSG_DISABLED_BY_FS); case ARMING_DISABLED_NOT_LEVEL: diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index fd47f7e058e..12076127458 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1806,8 +1806,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINIS { UNUSED(previousState); - disarm(DISARM_NAVIGATION); - return NAV_FSM_EVENT_NONE; } @@ -2840,7 +2838,7 @@ void updateLandingStatus(timeMs_t currentTimeMs) } } else if (STATE(LANDING_DETECTED)) { pidResetErrorAccumulators(); - if (navConfig()->general.flags.disarm_on_landing) { + if (navConfig()->general.flags.disarm_on_landing && !FLIGHT_MODE(FAILSAFE_MODE)) { ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED); disarm(DISARM_LANDING); } else if (!navigationInAutomaticThrottleMode()) { From c7094ba1a9f299690f38b127126bece625df65be Mon Sep 17 00:00:00 2001 From: MarkTan Date: Fri, 8 Sep 2023 18:45:04 +0800 Subject: [PATCH 33/39] Add DPS310 --- src/main/target/AOCODARCF4V2/target.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/AOCODARCF4V2/target.h b/src/main/target/AOCODARCF4V2/target.h index 5ad27e46570..ed524edc906 100644 --- a/src/main/target/AOCODARCF4V2/target.h +++ b/src/main/target/AOCODARCF4V2/target.h @@ -55,6 +55,7 @@ #define USE_BARO_BMP280 #define USE_BARO_MS5611 +#define USE_BARO_DPS310 //*********** Magnetometer / Compass ************* #define USE_MAG From 9c7775080b6557c53401b76ae70dc3beb5dcf58c Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 8 Sep 2023 18:59:47 +0200 Subject: [PATCH 34/39] Add some documentation goodness --- docs/ESC and servo outputs.md | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/docs/ESC and servo outputs.md b/docs/ESC and servo outputs.md index 78a5056590a..a9821603c06 100644 --- a/docs/ESC and servo outputs.md +++ b/docs/ESC and servo outputs.md @@ -28,8 +28,23 @@ While motors are usually ordered sequentially, here is no standard output layout ## Modifying output mapping -INAV 5 allows the limited output type mapping by allowing to change the function of *ALL* outputs at the same time. It can be done with the `output_mode` CLI setting. Allowed values: +### Modifying all outputs at the same time + +Since INAV 5, it has been possible to force *ALL* outputs to be `MOTORS` or `SERVOS`. + +Traditional ESCs usually can be controlled via a servo output, but would require calibration. + +This can be done with the `output_mode` CLI setting. Allowed values: * `AUTO` assigns outputs according to the default mapping * `SERVOS` assigns all outputs to servos -* `MOTORS` assigns all outputs to motors \ No newline at end of file +* `MOTORS` assigns all outputs to motors + +### Modifying only some outputs + +INAV 7 introduced extra functionality that let you force only some outputs to be either *MOTORS* or *SERVOS*, with some restrictions dictated by the hardware. + +The mains restrictions is that outputs need to be associated with timers, which are usually shared between multiple outputs. Two outputs on the same timer need to have the same function. + +The easiest way to modify outputs, is to use the Mixer tab in the Configurator, as it will clearly show you which timer is used by all outputs, but you can also use `timer_output_mode` on the cli. +This can be used in conjunction to the previous method, in that cass all outputs will follow `output_mode` and `timer_output_mode` overrides are applied after that. From 3a2412e5e680da0460f54f482d23e88e8ef128c1 Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Sat, 9 Sep 2023 08:34:28 +0100 Subject: [PATCH 35/39] [sitl] add --version, add git commit to version output (#9286) * [sitl] add --version, add git commit to version output * [sitl] take help path on invalid options --- src/main/target/SITL/target.c | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index 4bf53a791a5..0a686f60619 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -51,6 +51,7 @@ #include "drivers/timer.h" #include "drivers/serial.h" #include "config/config_streamer.h" +#include "build/version.h" #include "target/SITL/sim/realFlight.h" #include "target/SITL/sim/xplane.h" @@ -73,9 +74,12 @@ static int simPort = 0; static char **c_argv; -void systemInit(void) { +static void printVersion(void) { + fprintf(stderr, "INAV %d.%d.%d SITL (%s)\n", FC_VERSION_MAJOR, FC_VERSION_MINOR, FC_VERSION_PATCH_LEVEL, shortGitRevision); +} - fprintf(stderr, "INAV %d.%d.%d SITL\n", FC_VERSION_MAJOR, FC_VERSION_MINOR, FC_VERSION_PATCH_LEVEL); +void systemInit(void) { + printVersion(); clock_gettime(CLOCK_MONOTONIC, &start_time); fprintf(stderr, "[SYSTEM] Init...\n"); @@ -168,6 +172,7 @@ bool parseMapping(char* mapStr) void printCmdLineOptions(void) { + printVersion(); fprintf(stderr, "Avaiable options:\n"); fprintf(stderr, "--path=[path] Path and filename of eeprom.bin. If not specified 'eeprom.bin' in program directory is used.\n"); fprintf(stderr, "--sim=[rf|xp] Simulator interface: rf = RealFligt, xp = XPlane. Example: --sim=rf\n"); @@ -197,6 +202,7 @@ void parseArguments(int argc, char *argv[]) {"simport", required_argument, 0, 'p'}, {"help", no_argument, 0, 'h'}, {"path", required_argument, 0, 'e'}, + {"version", no_argument, 0, 'v'}, {NULL, 0, NULL, 0} }; @@ -236,10 +242,12 @@ void parseArguments(int argc, char *argv[]) fprintf(stderr, "[EEPROM] Invalid path, using eeprom file in program directory\n."); } break; - case 'h': + case 'v': + printVersion(); + exit(0); + default: printCmdLineOptions(); exit(0); - break; } } From fb7754fdc10a832307bc2d30a04f227aa03836c4 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sun, 10 Sep 2023 19:58:51 -0500 Subject: [PATCH 36/39] docs/programming user21 typo --- docs/Programming Framework.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index ed9647ae92a..a5b9a3fdbcd 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -158,7 +158,7 @@ The flight mode operands return `true` when the mode is active. These are modes | 7 | HORIZON | `true` when you are in the **Horizon** flight mode. | | 8 | AIR | `true` when you the **Airmode** flight mode modifier is active. | | 9 | USER1 | `true` when the **USER 1** mode is active. | -| 10 | USER2 | `true` when the **USER 21** mode is active. | +| 10 | USER2 | `true` when the **USER 2** mode is active. | | 11 | COURSE_HOLD | `true` when you are in the **Course Hold** flight mode. | | 12 | USER3 | `true` when the **USER 3** mode is active. | | 13 | USER4 | `true` when the **USER 4** mode is active. | From 03342b1091f30e4ef8c7ddc88da86cdea04f8f34 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sat, 16 Sep 2023 14:37:36 +0100 Subject: [PATCH 37/39] Ignore tasks.json and use fabs Added ignore flags for - .vscode\tasks.json - .vscode\c_cpp_properties.json Changed settings.json to use tabs, not spaces for C file types. --- .gitignore | 2 ++ .vscode/settings.json | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 3cbe312ed07..c86d809162b 100644 --- a/.gitignore +++ b/.gitignore @@ -32,3 +32,5 @@ README.pdf # local changes only make/local.mk launch.json +.vscode/tasks.json +.vscode/c_cpp_properties.json diff --git a/.vscode/settings.json b/.vscode/settings.json index 2cece3ee127..1d922281a17 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -5,7 +5,7 @@ "ranges": "c" }, "editor.tabSize": 4, - "editor.insertSpaces": true, + "editor.insertSpaces": false, "editor.detectIndentation": false, "editor.expandTabs": true, "C_Cpp.clang_format_fallbackStyle": "{ BasedOnStyle: Google, IndentWidth: 4, BreakBeforeBraces: Mozilla }" From 1d5cbb964350b965dc1af0a8984822a8380d5567 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sat, 16 Sep 2023 16:30:58 +0100 Subject: [PATCH 38/39] Update settings.json Use spaces not tabs --- .vscode/settings.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 1d922281a17..2cece3ee127 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -5,7 +5,7 @@ "ranges": "c" }, "editor.tabSize": 4, - "editor.insertSpaces": false, + "editor.insertSpaces": true, "editor.detectIndentation": false, "editor.expandTabs": true, "C_Cpp.clang_format_fallbackStyle": "{ BasedOnStyle: Google, IndentWidth: 4, BreakBeforeBraces: Mozilla }" From 83844e996fbd229a16dc83e76e553fcf297ed5c8 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Mon, 18 Sep 2023 08:00:12 +0100 Subject: [PATCH 39/39] Enable the virtual pitot by default --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 3fe84833ab7..6dc19b390e9 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4838,7 +4838,7 @@ Selection of pitot hardware. | Default | Min | Max | | --- | --- | --- | -| NONE | | | +| VIRTUAL | | | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 10a53df3e9b..032027a841c 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -588,7 +588,7 @@ groups: members: - name: pitot_hardware description: "Selection of pitot hardware." - default_value: "NONE" + default_value: "VIRTUAL" table: pitot_hardware - name: pitot_lpf_milli_hz min: 0