mirror of https://github.com/rusefi/rusefi-1.git
Hardware pwm (#1283)
* hard pwm * fix, comments * PC6-9 and PD12-15 * PB6-9 * dynamic frequency * f7 mcuconf * format * relocate to common, support variable frequency * error handling * oops * fix test build * noop kinetis * better guards * guard for bl * guards don't work if they have typos * more mapping * oops * don't use for slow pwm * enable pwm * guard properly * save a little memory * use less mem * correct timers * all your ram are belong to us * update limit & comment Co-authored-by: Matthew Kennedy <makenne@microsoft.com>
This commit is contained in:
parent
bef53d8ba5
commit
3a7c6f242b
|
@ -134,7 +134,7 @@
|
||||||
* @brief Enables the PWM subsystem.
|
* @brief Enables the PWM subsystem.
|
||||||
*/
|
*/
|
||||||
#if !defined(HAL_USE_PWM) || defined(__DOXYGEN__)
|
#if !defined(HAL_USE_PWM) || defined(__DOXYGEN__)
|
||||||
#define HAL_USE_PWM FALSE
|
#define HAL_USE_PWM TRUE
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
|
@ -272,9 +272,9 @@
|
||||||
#define STM32_PWM_USE_TIM1 FALSE
|
#define STM32_PWM_USE_TIM1 FALSE
|
||||||
#define STM32_PWM_USE_TIM2 FALSE
|
#define STM32_PWM_USE_TIM2 FALSE
|
||||||
#define STM32_PWM_USE_TIM3 FALSE
|
#define STM32_PWM_USE_TIM3 FALSE
|
||||||
#define STM32_PWM_USE_TIM4 FALSE
|
#define STM32_PWM_USE_TIM4 TRUE
|
||||||
#define STM32_PWM_USE_TIM5 FALSE
|
#define STM32_PWM_USE_TIM5 FALSE
|
||||||
#define STM32_PWM_USE_TIM8 FALSE
|
#define STM32_PWM_USE_TIM8 TRUE
|
||||||
#define STM32_PWM_USE_TIM9 FALSE
|
#define STM32_PWM_USE_TIM9 FALSE
|
||||||
#define STM32_PWM_TIM1_IRQ_PRIORITY 7
|
#define STM32_PWM_TIM1_IRQ_PRIORITY 7
|
||||||
#define STM32_PWM_TIM2_IRQ_PRIORITY 7
|
#define STM32_PWM_TIM2_IRQ_PRIORITY 7
|
||||||
|
|
|
@ -125,7 +125,7 @@
|
||||||
* @brief Enables the PWM subsystem.
|
* @brief Enables the PWM subsystem.
|
||||||
*/
|
*/
|
||||||
#if !defined(HAL_USE_PWM) || defined(__DOXYGEN__)
|
#if !defined(HAL_USE_PWM) || defined(__DOXYGEN__)
|
||||||
#define HAL_USE_PWM FALSE
|
#define HAL_USE_PWM TRUE
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
|
@ -283,9 +283,9 @@
|
||||||
#define STM32_PWM_USE_TIM1 FALSE
|
#define STM32_PWM_USE_TIM1 FALSE
|
||||||
#define STM32_PWM_USE_TIM2 FALSE
|
#define STM32_PWM_USE_TIM2 FALSE
|
||||||
#define STM32_PWM_USE_TIM3 FALSE
|
#define STM32_PWM_USE_TIM3 FALSE
|
||||||
#define STM32_PWM_USE_TIM4 FALSE
|
#define STM32_PWM_USE_TIM4 TRUE
|
||||||
#define STM32_PWM_USE_TIM5 FALSE
|
#define STM32_PWM_USE_TIM5 FALSE
|
||||||
#define STM32_PWM_USE_TIM8 FALSE
|
#define STM32_PWM_USE_TIM8 TRUE
|
||||||
#define STM32_PWM_USE_TIM9 FALSE
|
#define STM32_PWM_USE_TIM9 FALSE
|
||||||
#define STM32_PWM_TIM1_IRQ_PRIORITY 7
|
#define STM32_PWM_TIM1_IRQ_PRIORITY 7
|
||||||
#define STM32_PWM_TIM2_IRQ_PRIORITY 7
|
#define STM32_PWM_TIM2_IRQ_PRIORITY 7
|
||||||
|
|
|
@ -53,31 +53,30 @@ public:
|
||||||
// Configure the disable pin first - ensure things are in a safe state
|
// Configure the disable pin first - ensure things are in a safe state
|
||||||
m_disablePin.initPin("ETB Disable", pinDisable);
|
m_disablePin.initPin("ETB Disable", pinDisable);
|
||||||
|
|
||||||
m_pinEnable.initPin("ETB Enable", pinEnable);
|
|
||||||
m_pinDir1.initPin("ETB Dir 1", pinDir1);
|
|
||||||
m_pinDir2.initPin("ETB Dir 2", pinDir2);
|
|
||||||
|
|
||||||
// Clamp to >100hz
|
// Clamp to >100hz
|
||||||
int clampedFrequency = maxI(100, frequency);
|
int clampedFrequency = maxI(100, frequency);
|
||||||
|
|
||||||
// no need to complicate event queue with ETB PWM in unit tests
|
// no need to complicate event queue with ETB PWM in unit tests
|
||||||
#if ! EFI_UNIT_TEST
|
#if ! EFI_UNIT_TEST
|
||||||
startSimplePwm(&m_pwmEnable, "ETB Enable",
|
startSimplePwmHard(&m_pwmEnable, "ETB Enable",
|
||||||
executor,
|
executor,
|
||||||
|
pinEnable,
|
||||||
&m_pinEnable,
|
&m_pinEnable,
|
||||||
clampedFrequency,
|
clampedFrequency,
|
||||||
0
|
0
|
||||||
);
|
);
|
||||||
|
|
||||||
startSimplePwm(&m_pwmDir1, "ETB Dir 1",
|
startSimplePwmHard(&m_pwmDir1, "ETB Dir 1",
|
||||||
executor,
|
executor,
|
||||||
|
pinDir1,
|
||||||
&m_pinDir1,
|
&m_pinDir1,
|
||||||
clampedFrequency,
|
clampedFrequency,
|
||||||
0
|
0
|
||||||
);
|
);
|
||||||
|
|
||||||
startSimplePwm(&m_pwmDir2, "ETB Dir 2",
|
startSimplePwmHard(&m_pwmDir2, "ETB Dir 2",
|
||||||
executor,
|
executor,
|
||||||
|
pinDir2,
|
||||||
&m_pinDir2,
|
&m_pinDir2,
|
||||||
clampedFrequency,
|
clampedFrequency,
|
||||||
0
|
0
|
||||||
|
|
|
@ -1743,7 +1743,7 @@ typedef enum {
|
||||||
CUSTOM_INTERPOLATE_NAN = 6055,
|
CUSTOM_INTERPOLATE_NAN = 6055,
|
||||||
ERROR_HISTO_NAME = 6056,
|
ERROR_HISTO_NAME = 6056,
|
||||||
CUSTOM_AUX_OUT_OF_ORDER = 6057,
|
CUSTOM_AUX_OUT_OF_ORDER = 6057,
|
||||||
CUSTOM_OBD_58 = 6058,
|
CUSTOM_OBD_HIGH_FREQUENCY = 6058,
|
||||||
CUSTOM_OBD_59 = 6059,
|
CUSTOM_OBD_59 = 6059,
|
||||||
|
|
||||||
CUSTOM_OBD_MMC_START1 = 6060,
|
CUSTOM_OBD_MMC_START1 = 6060,
|
||||||
|
|
|
@ -696,7 +696,7 @@ void initEngineContoller(Logging *sharedLogger DECLARE_ENGINE_PARAMETER_SUFFIX)
|
||||||
* UNUSED_SIZE contants.
|
* UNUSED_SIZE contants.
|
||||||
*/
|
*/
|
||||||
#ifndef RAM_UNUSED_SIZE
|
#ifndef RAM_UNUSED_SIZE
|
||||||
#define RAM_UNUSED_SIZE 8800
|
#define RAM_UNUSED_SIZE 8600
|
||||||
#endif
|
#endif
|
||||||
#ifndef CCM_UNUSED_SIZE
|
#ifndef CCM_UNUSED_SIZE
|
||||||
#define CCM_UNUSED_SIZE 2900
|
#define CCM_UNUSED_SIZE 2900
|
||||||
|
|
|
@ -13,6 +13,10 @@
|
||||||
#include "pwm_generator_logic.h"
|
#include "pwm_generator_logic.h"
|
||||||
#include "perf_trace.h"
|
#include "perf_trace.h"
|
||||||
|
|
||||||
|
#if EFI_PROD_CODE
|
||||||
|
#include "mpu_util.h"
|
||||||
|
#endif
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* We need to limit the number of iterations in order to avoid precision loss while calculating
|
* We need to limit the number of iterations in order to avoid precision loss while calculating
|
||||||
* next toggle time
|
* next toggle time
|
||||||
|
@ -76,6 +80,13 @@ void SimplePwm::setSimplePwmDutyCycle(float dutyCycle) {
|
||||||
dutyCycle = 1;
|
dutyCycle = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#if EFI_PROD_CODE
|
||||||
|
if (hardPwm) {
|
||||||
|
hardPwm->setDuty(dutyCycle);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
// Handle zero and full duty cycle. This will cause the PWM output to behave like a plain digital output.
|
// Handle zero and full duty cycle. This will cause the PWM output to behave like a plain digital output.
|
||||||
if (dutyCycle == 0.0f && stateChangeCallback) {
|
if (dutyCycle == 0.0f && stateChangeCallback) {
|
||||||
// Manually fire falling edge
|
// Manually fire falling edge
|
||||||
|
@ -359,6 +370,23 @@ void startSimplePwmExt(SimplePwm *state, const char *msg,
|
||||||
startSimplePwm(state, msg, executor, output, frequency, dutyCycle, stateChangeCallback);
|
startSimplePwm(state, msg, executor, output, frequency, dutyCycle, stateChangeCallback);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void startSimplePwmHard(SimplePwm *state, const char *msg,
|
||||||
|
ExecutorInterface *executor,
|
||||||
|
brain_pin_e brainPin, OutputPin *output, float frequency,
|
||||||
|
float dutyCycle) {
|
||||||
|
#if EFI_PROD_CODE && HAL_USE_PWM
|
||||||
|
auto hardPwm = hardware_pwm::tryInitPin(msg, brainPin, frequency, dutyCycle);
|
||||||
|
|
||||||
|
if (hardPwm) {
|
||||||
|
state->hardPwm = hardPwm;
|
||||||
|
} else {
|
||||||
|
#endif
|
||||||
|
startSimplePwmExt(state, msg, executor, brainPin, output, frequency, dutyCycle);
|
||||||
|
#if EFI_PROD_CODE && HAL_USE_PWM
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* This method controls the actual hardware pins
|
* This method controls the actual hardware pins
|
||||||
*
|
*
|
||||||
|
|
|
@ -112,6 +112,7 @@ private:
|
||||||
float periodNt;
|
float periodNt;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
struct hardware_pwm;
|
||||||
|
|
||||||
class SimplePwm : public PwmConfig {
|
class SimplePwm : public PwmConfig {
|
||||||
public:
|
public:
|
||||||
|
@ -121,6 +122,8 @@ public:
|
||||||
pin_state_t pinStates[2];
|
pin_state_t pinStates[2];
|
||||||
SingleChannelStateSequence sr[1];
|
SingleChannelStateSequence sr[1];
|
||||||
float _switchTimes[2];
|
float _switchTimes[2];
|
||||||
|
hardware_pwm* hardPwm = nullptr;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
SingleChannelStateSequence waveInstance;
|
SingleChannelStateSequence waveInstance;
|
||||||
};
|
};
|
||||||
|
@ -152,6 +155,11 @@ void startSimplePwmExt(SimplePwm *state,
|
||||||
brain_pin_e brainPin, OutputPin *output,
|
brain_pin_e brainPin, OutputPin *output,
|
||||||
float frequency, float dutyCycle, pwm_gen_callback *stateChangeCallback = (pwm_gen_callback*)applyPinState);
|
float frequency, float dutyCycle, pwm_gen_callback *stateChangeCallback = (pwm_gen_callback*)applyPinState);
|
||||||
|
|
||||||
|
void startSimplePwmHard(SimplePwm *state, const char *msg,
|
||||||
|
ExecutorInterface *executor,
|
||||||
|
brain_pin_e brainPin, OutputPin *output, float frequency,
|
||||||
|
float dutyCycle);
|
||||||
|
|
||||||
void copyPwmParameters(PwmConfig *state, int phaseCount, float const *switchTimes,
|
void copyPwmParameters(PwmConfig *state, int phaseCount, float const *switchTimes,
|
||||||
int waveCount, pin_state_t *const *pinStates);
|
int waveCount, pin_state_t *const *pinStates);
|
||||||
|
|
||||||
|
|
|
@ -128,7 +128,6 @@ icuchannel_t getInputCaptureChannel(brain_pin_e hwPin) {
|
||||||
return ICU_CHANNEL_1;
|
return ICU_CHANNEL_1;
|
||||||
|
|
||||||
case GPIOA_1: // TIM2
|
case GPIOA_1: // TIM2
|
||||||
case GPIOA_3: // TIM9
|
|
||||||
case GPIOA_7: // TIM3
|
case GPIOA_7: // TIM3
|
||||||
case GPIOA_9: // TIM1
|
case GPIOA_9: // TIM1
|
||||||
case GPIOB_3: // TIM2
|
case GPIOB_3: // TIM2
|
||||||
|
|
|
@ -257,5 +257,10 @@ uintptr_t getFlashAddrSecondCopy() {
|
||||||
return 0x10008000;
|
return 0x10008000;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*static*/ hardware_pwm* hardware_pwm::tryInitPin(const char*, brain_pin_e, float, float) {
|
||||||
|
// TODO: implement me!
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
#endif /* EFI_PROD_CODE */
|
#endif /* EFI_PROD_CODE */
|
||||||
|
|
||||||
|
|
|
@ -30,6 +30,12 @@ void initSpiCs(SPIConfig *spiConfig, brain_pin_e csPin);
|
||||||
void turnOnSpi(spi_device_e device);
|
void turnOnSpi(spi_device_e device);
|
||||||
#endif // HAL_USE_SPI
|
#endif // HAL_USE_SPI
|
||||||
|
|
||||||
|
// Hardware PWM
|
||||||
|
struct hardware_pwm {
|
||||||
|
static hardware_pwm* tryInitPin(const char* msg, brain_pin_e pin, float frequencyHz, float duty);
|
||||||
|
virtual void setDuty(float duty) = 0;
|
||||||
|
};
|
||||||
|
|
||||||
// Brownout Reset
|
// Brownout Reset
|
||||||
typedef enum {
|
typedef enum {
|
||||||
BOR_Result_Ok = 0x00,
|
BOR_Result_Ok = 0x00,
|
||||||
|
|
|
@ -8,12 +8,14 @@
|
||||||
|
|
||||||
#include "global.h"
|
#include "global.h"
|
||||||
#include "efi_gpio.h"
|
#include "efi_gpio.h"
|
||||||
|
#include "expected.h"
|
||||||
|
|
||||||
#ifndef EFI_PIN_ADC9
|
#ifndef EFI_PIN_ADC9
|
||||||
#define EFI_PIN_ADC9 GPIOB_1
|
#define EFI_PIN_ADC9 GPIOB_1
|
||||||
#endif /* EFI_PIN_ADC9 */
|
#endif /* EFI_PIN_ADC9 */
|
||||||
|
|
||||||
#if EFI_PROD_CODE
|
#if EFI_PROD_CODE
|
||||||
|
#include "mpu_util.h"
|
||||||
#include "backup_ram.h"
|
#include "backup_ram.h"
|
||||||
extern ioportid_t PORTS[];
|
extern ioportid_t PORTS[];
|
||||||
#if defined(STM32F4XX) || defined(STM32F7XX)
|
#if defined(STM32F4XX) || defined(STM32F7XX)
|
||||||
|
@ -138,8 +140,168 @@ int getAdcChannelPin(adc_channel_e hwChannel) {
|
||||||
|
|
||||||
#endif /* HAL_USE_ADC */
|
#endif /* HAL_USE_ADC */
|
||||||
|
|
||||||
|
|
||||||
#if EFI_PROD_CODE
|
#if EFI_PROD_CODE
|
||||||
|
|
||||||
|
#if HAL_USE_PWM
|
||||||
|
namespace {
|
||||||
|
struct stm32_pwm_config {
|
||||||
|
PWMDriver* const Driver;
|
||||||
|
const uint8_t Channel;
|
||||||
|
const uint8_t AlternateFunc;
|
||||||
|
};
|
||||||
|
|
||||||
|
class stm32_hardware_pwm : public hardware_pwm {
|
||||||
|
public:
|
||||||
|
bool hasInit() const {
|
||||||
|
return m_driver != nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 2MHz, 16-bit timer gets us a usable frequency range of 31hz to 10khz
|
||||||
|
static constexpr uint32_t c_timerFrequency = 2000000;
|
||||||
|
|
||||||
|
void start(const char* msg, const stm32_pwm_config& config, float frequency, float duty) {
|
||||||
|
m_driver = config.Driver;
|
||||||
|
m_channel = config.Channel;
|
||||||
|
|
||||||
|
m_period = c_timerFrequency / frequency;
|
||||||
|
|
||||||
|
// These timers are only 16 bit - don't risk overflow
|
||||||
|
if (m_period > 0xFFF0) {
|
||||||
|
firmwareError(CUSTOM_OBD_LOW_FREQUENCY, "PWM Frequency too low %f hz on pin \"%s\"", frequency, msg);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// If we have too few usable bits, we run out of resolution, so don't allow that either.
|
||||||
|
// 200 counts = 0.5% resolution
|
||||||
|
if (m_period < 200) {
|
||||||
|
firmwareError(CUSTOM_OBD_HIGH_FREQUENCY, "PWM Frequency too high % hz on pin \"%s\"", frequency, msg);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
const PWMConfig pwmcfg = {
|
||||||
|
c_timerFrequency,
|
||||||
|
m_period,
|
||||||
|
nullptr,
|
||||||
|
{
|
||||||
|
{PWM_OUTPUT_ACTIVE_HIGH, nullptr},
|
||||||
|
{PWM_OUTPUT_ACTIVE_HIGH, nullptr},
|
||||||
|
{PWM_OUTPUT_ACTIVE_HIGH, nullptr},
|
||||||
|
{PWM_OUTPUT_ACTIVE_HIGH, nullptr}
|
||||||
|
},
|
||||||
|
0,
|
||||||
|
0
|
||||||
|
};
|
||||||
|
|
||||||
|
// Start the timer running
|
||||||
|
pwmStart(m_driver, &pwmcfg);
|
||||||
|
|
||||||
|
// Set initial duty cycle
|
||||||
|
setDuty(duty);
|
||||||
|
}
|
||||||
|
|
||||||
|
void setDuty(float duty) override {
|
||||||
|
if (!m_driver) {
|
||||||
|
firmwareError(OBD_PCM_Processor_Fault, "Attempted to set duty on null hard PWM device");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
pwm_lld_enable_channel(m_driver, m_channel, getHighTime(duty));
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
PWMDriver* m_driver = nullptr;
|
||||||
|
uint8_t m_channel = 0;
|
||||||
|
uint32_t m_period = 0;
|
||||||
|
|
||||||
|
pwmcnt_t getHighTime(float duty) const {
|
||||||
|
return m_period * duty;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
static expected<stm32_pwm_config> getConfigForPin(brain_pin_e pin) {
|
||||||
|
switch (pin) {
|
||||||
|
#if STM32_PWM_USE_TIM1
|
||||||
|
case GPIOA_8: return stm32_pwm_config{&PWMD1, 0, 1};
|
||||||
|
case GPIOA_9: return stm32_pwm_config{&PWMD1, 1, 1};
|
||||||
|
case GPIOA_10: return stm32_pwm_config{&PWMD1, 2, 1};
|
||||||
|
case GPIOA_11: return stm32_pwm_config{&PWMD1, 3, 1};
|
||||||
|
|
||||||
|
case GPIOE_9: return stm32_pwm_config{&PWMD1, 0, 1};
|
||||||
|
case GPIOE_11: return stm32_pwm_config{&PWMD1, 1, 1};
|
||||||
|
case GPIOE_13: return stm32_pwm_config{&PWMD1, 2, 1};
|
||||||
|
case GPIOE_14: return stm32_pwm_config{&PWMD1, 3, 1};
|
||||||
|
#endif
|
||||||
|
#if STM32_PWM_USE_TIM2
|
||||||
|
case GPIOA_15: return stm32_pwm_config{&PWMD2, 0, 1};
|
||||||
|
case GPIOB_3: return stm32_pwm_config{&PWMD2, 1, 1};
|
||||||
|
case GPIOB_10: return stm32_pwm_config{&PWMD2, 2, 1};
|
||||||
|
case GPIOB_11: return stm32_pwm_config{&PWMD2, 3, 1};
|
||||||
|
#endif
|
||||||
|
#if STM32_PWM_USE_TIM3
|
||||||
|
case GPIOB_4: return stm32_pwm_config{&PWMD3, 0, 2};
|
||||||
|
case GPIOB_5: return stm32_pwm_config{&PWMD3, 1, 2};
|
||||||
|
#endif
|
||||||
|
#if STM32_PWM_USE_TIM4
|
||||||
|
case GPIOB_6: return stm32_pwm_config{&PWMD4, 0, 2};
|
||||||
|
case GPIOB_7: return stm32_pwm_config{&PWMD4, 1, 2};
|
||||||
|
case GPIOB_8: return stm32_pwm_config{&PWMD4, 2, 2};
|
||||||
|
case GPIOB_9: return stm32_pwm_config{&PWMD4, 3, 2};
|
||||||
|
|
||||||
|
case GPIOD_12: return stm32_pwm_config{&PWMD4, 0, 2};
|
||||||
|
case GPIOD_13: return stm32_pwm_config{&PWMD4, 1, 2};
|
||||||
|
case GPIOD_14: return stm32_pwm_config{&PWMD4, 2, 2};
|
||||||
|
case GPIOD_15: return stm32_pwm_config{&PWMD4, 3, 2};
|
||||||
|
#endif
|
||||||
|
#if STM32_PWM_USE_TIM8
|
||||||
|
case GPIOC_6: return stm32_pwm_config{&PWMD8, 0, 3};
|
||||||
|
case GPIOC_7: return stm32_pwm_config{&PWMD8, 1, 3};
|
||||||
|
case GPIOC_8: return stm32_pwm_config{&PWMD8, 2, 3};
|
||||||
|
case GPIOC_9: return stm32_pwm_config{&PWMD8, 3, 3};
|
||||||
|
#endif
|
||||||
|
default: return unexpected;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
stm32_hardware_pwm pwms[5];
|
||||||
|
|
||||||
|
stm32_hardware_pwm* getNextPwmDevice() {
|
||||||
|
for (size_t i = 0; i < efi::size(pwms); i++) {
|
||||||
|
if (!pwms[i].hasInit()) {
|
||||||
|
return &pwms[i];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
firmwareError(OBD_PCM_Processor_Fault, "Run out of hardware PWM devices!");
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*static*/ hardware_pwm* hardware_pwm::tryInitPin(const char* msg, brain_pin_e pin, float frequencyHz, float duty) {
|
||||||
|
// Hardware PWM can't do very slow PWM - the timer counter is only 16 bits, so at 2MHz counting, that's a minimum of 31hz.
|
||||||
|
if (frequencyHz < 50) {
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
auto cfg = getConfigForPin(pin);
|
||||||
|
|
||||||
|
// This pin can't do hardware PWM
|
||||||
|
if (!cfg) {
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (stm32_hardware_pwm* device = getNextPwmDevice()) {
|
||||||
|
device->start(msg, cfg.Value, frequencyHz, duty);
|
||||||
|
|
||||||
|
// Finally connect the timer to physical pin
|
||||||
|
efiSetPadMode(msg, pin, PAL_MODE_ALTERNATE(cfg.Value.AlternateFunc));
|
||||||
|
|
||||||
|
return device;
|
||||||
|
}
|
||||||
|
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
void jump_to_bootloader() {
|
void jump_to_bootloader() {
|
||||||
// leave DFU breadcrumb which assmebly startup code would check, see [rusefi][DFU] section in assembly code
|
// leave DFU breadcrumb which assmebly startup code would check, see [rusefi][DFU] section in assembly code
|
||||||
*((unsigned long *)0x2001FFF0) = 0xDEADBEEF; // End of RAM
|
*((unsigned long *)0x2001FFF0) = 0xDEADBEEF; // End of RAM
|
||||||
|
|
Loading…
Reference in New Issue