bool Flasher_::write(PageID page, volatile const Data *bufp) {
R2P_ASSERT(is_aligned(const_cast<const Data *>(bufp)));
if (!check_bounds(address_of(page), PAGE_SIZE, get_program_start(),
get_program_length())) {
return false;
}
volatile Data *const flashp = reinterpret_cast<volatile Data *>(
reinterpret_cast<uintptr_t>(address_of(page))
);
chSysDisable();
// Unlock flash for write access
if (!flash_unlock()) {
flash_lock();
chSysEnable();
return false;
}
flash_busy_wait();
for (size_t pos = 0; pos < PAGE_SIZE / WORD_ALIGNMENT; ++pos) {
// Enter flash programming mode
FLASH->CR |= FLASH_CR_PG;
// Write half-word to flash
flashp[pos] = bufp[pos];
flash_busy_wait();
// Exit flash programming mode
FLASH->CR &= ~FLASH_CR_PG;
// Check for flash error
if (flashp[pos] != bufp[pos]) {
flash_lock();
chSysEnable();
return false;
}
}
flash_lock();
chSysEnable();
return true;
}
/**
Disable a stepper motor.
@param stepper Which stepper (0 or 1).
*/
void stepperDisable(int stepper)
{
Stepper* s = &steppers[stepper];
if (s->timerRunning) {
chSysDisable();
fasttimerStop(&s->fastTimer);
chSysEnable();
}
int i;
for (i = 0; i < 4; i++)
pinOff(s->pins[i]);
int pwm = stepper * 2;
pwmDisableChannel(pwm);
pwmDisableChannel(pwm + 1);
}
/**
* Stop the system and jump to the bootloader.
*/
void flash_helper_jump_to_bootloader(void) {
typedef void (*pFunction)(void);
mc_interface_unlock();
mc_interface_release_motor();
usbDisconnectBus(&USBD1);
usbStop(&USBD1);
sdStop(&HW_UART_DEV);
palSetPadMode(HW_UART_TX_PORT, HW_UART_TX_PIN, PAL_MODE_INPUT);
palSetPadMode(HW_UART_RX_PORT, HW_UART_RX_PIN, PAL_MODE_INPUT);
// Disable watchdog
timeout_configure_IWDT_slowest();
chSysDisable();
pFunction jump_to_bootloader;
// Variable that will be loaded with the start address of the application
volatile uint32_t* jump_address;
const volatile uint32_t* bootloader_address = (volatile uint32_t*)0x080E0000;
// Get jump address from application vector table
jump_address = (volatile uint32_t*) bootloader_address[1];
// Load this address into function pointer
jump_to_bootloader = (pFunction) jump_address;
// Clear pending interrupts
SCB->ICSR = SCB_ICSR_PENDSVCLR_Msk;
// Disable all interrupts
for(int i = 0;i < 8;i++) {
NVIC->ICER[i] = NVIC->IABR[i];
}
// Set stack pointer
__set_MSP((uint32_t) (bootloader_address[0]));
// Jump to the bootloader
jump_to_bootloader();
}
/**
* Stop the system and jump to the bootloader.
*/
void flash_helper_jump_to_bootloader(void) {
typedef void (*pFunction)(void);
mcpwm_unlock();
mcpwm_release_motor();
usbDisconnectBus(&USBD1);
usbStop(&USBD1);
uartStop(&HW_UART_DEV);
palSetPadMode(HW_UART_TX_PORT, HW_UART_TX_PIN, PAL_MODE_INPUT);
palSetPadMode(HW_UART_RX_PORT, HW_UART_RX_PIN, PAL_MODE_INPUT);
// Disable watchdog
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, DISABLE);
chSysDisable();
pFunction jump_to_bootloader;
// Variable that will be loaded with the start address of the application
vu32* jump_address;
const vu32* bootloader_address = (vu32*)0x080E0000;
// Get jump address from application vector table
jump_address = (vu32*) bootloader_address[1];
// Load this address into function pointer
jump_to_bootloader = (pFunction) jump_address;
// Clear pending interrupts
SCB_ICSR = ICSR_PENDSVCLR;
// Disable all interrupts
for(int i = 0;i < 8;i++) {
NVIC->ICER[i] = NVIC->IABR[i];
}
// Set stack pointer
__set_MSP((u32) (bootloader_address[0]));
// Jump to the bootloader
jump_to_bootloader();
}
int motor_adc_init(void)
{
_shunt_resistance = configGet("mot_i_shunt_mr") / 1000.0f;
chSysDisable();
RCC->AHBENR |= RCC_AHBENR_DMA1EN; // Never disabled
RCC->CFGR &= ~RCC_CFGR_ADCPRE;
#if STM32_PCLK2 == 72000000
// ADC clock 72 / 6 = 12 MHz
RCC->CFGR |= RCC_CFGR_ADCPRE_DIV6;
#else
# error "What's wrong with PCLK2?"
#endif
chSysEnable();
enable();
return 0;
}
static void
jump_to_app(uint32_t address)
{
uint32_t jump_addr = *(volatile uint32_t*)(address + 0x04); /* reset ptr in vector table */
app_entry_t app_entry = (app_entry_t)jump_addr;
/* Reset all interrupts to default */
chSysDisable();
/* Clear pending interrupts just to be on the safe side */
SCB_ICSR = ICSR_PENDSVCLR;
/* Disable all interrupts */
int i;
for(i = 0; i < 8; ++i)
NVIC->ICER[i] = NVIC->IABR[i];
/* Set stack pointer as in application's vector table */
__set_MSP(*(volatile uint32_t*)address);
app_entry();
}
/*
* @brief Used to terminate all running tasks and disable the OS. All
* deinit of drivers and modules should be placed here.
*/
void vSystemDeinit(void)
{
/* Starting the shutdown sequence. */
if (system_state == SYSTEM_RUNNING)
system_state = SYSTEM_TERMINATING;
else
osalSysHalt("System is not running, invalid operation");
/* Terminate critical tasks */
vSystemTerminateCriticalTasks();
/* Stop drivers */
vSystemDeinitList();
/* Stop SysTick */
chSysDisable();
vSystemDisableSystick();
chSysEnable();
system_state = SYSTEM_STOPPED;
}
void flashJumpApplication(uint32_t address)
{
typedef void (*funcPtr)(void);
u32 jumpAddr = *(vu32*)(address + 0x04); /* reset ptr in vector table */
funcPtr usrMain = (funcPtr)jumpAddr;
/* Reset all interrupts to default */
chSysDisable();
/* Clear pending interrupts just to be on the save side */
SCB_ICSR = ICSR_PENDSVCLR;
/* Disable all interrupts */
int i;
for(i = 0; i < 8; ++i)
NVIC->ICER[i] = NVIC->IABR[i];
/* Set stack pointer as in application's vector table */
__set_MSP(*(vu32*)address);
usrMain();
}
void Flasher_::jump_to(const uint8_t *address) {
typedef void (*Proc)(void);
// Load jump address into function pointer
Proc proc = reinterpret_cast<Proc>(
reinterpret_cast<const uint32_t *>(address)[1]
);
// Reset all interrupts to default
chSysDisable();
// Clear pending interrupts just to be on the safe side
SCB_ICSR = ICSR_PENDSVCLR;
// Disable all interrupts
for (unsigned i = 0; i < 8; ++i) {
NVIC->ICER[i] = NVIC->IABR[i];
}
// Set stack pointer as in application's vector table
__set_MSP((reinterpret_cast<const uint32_t *>(address))[0]);
proc();
}
static void enable(void)
{
// DMA
DMA1_Channel1->CCR = 0; // Deinitialize
DMA1_Channel1->CMAR = (uint32_t)_adc1_2_dma_buffer;
DMA1_Channel1->CNDTR = sizeof(_adc1_2_dma_buffer) / 4;
DMA1_Channel1->CPAR = (uint32_t)&ADC1->DR;
DMA1_Channel1->CCR =
DMA_CCR_PL_0 | DMA_CCR_PL_1 | // Max priority
DMA_CCR_MSIZE_1 | // 32 bit
DMA_CCR_PSIZE_1 | // 32 bit
DMA_CCR_MINC |
DMA_CCR_CIRC |
DMA_CCR_EN;
// ADC enable, reset
const uint32_t enr_mask = RCC_APB2ENR_ADC1EN | RCC_APB2ENR_ADC2EN;
const uint32_t rst_mask = RCC_APB2RSTR_ADC1RST | RCC_APB2RSTR_ADC2RST;
chSysDisable();
RCC->APB2ENR |= enr_mask;
RCC->APB2RSTR |= rst_mask;
RCC->APB2RSTR &= ~rst_mask;
chSysEnable();
usleep(5); // Sequence: enable ADC, wait 2+ cycles, poweron, calibrate?
// ADC calibration
ADC1->CR2 = ADC_CR2_ADON;
adc_calibrate(ADC1);
ADC2->CR2 = ADC_CR2_ADON;
adc_calibrate(ADC2);
/*
* ADC channel sampling:
* A B C A B C VOLT
* C A B C A B CURR
*/
ADC1->SQR1 = ADC_SQR1_L_1 | ADC_SQR1_L_2;
ADC1->SQR3 =
ADC_SQR3_SQ1_0 |
ADC_SQR3_SQ2_1 |
ADC_SQR3_SQ3_0 | ADC_SQR3_SQ3_1 |
ADC_SQR3_SQ4_0 |
ADC_SQR3_SQ5_1 |
ADC_SQR3_SQ6_0 | ADC_SQR3_SQ6_1;
ADC1->SQR2 = ADC_SQR2_SQ7_2;
ADC2->SQR1 = ADC1->SQR1;
ADC2->SQR3 =
ADC_SQR3_SQ1_0 | ADC_SQR3_SQ1_1 |
ADC_SQR3_SQ2_0 |
ADC_SQR3_SQ3_1 |
ADC_SQR3_SQ4_0 | ADC_SQR3_SQ4_1 |
ADC_SQR3_SQ5_0 |
ADC_SQR3_SQ6_1;
ADC2->SQR2 = ADC_SQR2_SQ7_2 | ADC_SQR2_SQ7_0;
// SMPR registers are not configured because they have right values by default
// ADC initialization
ADC1->CR1 = ADC_CR1_DUALMOD_1 | ADC_CR1_DUALMOD_2 | ADC_CR1_SCAN | ADC_CR1_EOCIE;
ADC1->CR2 = ADC_CR2_ADON | ADC_CR2_EXTTRIG | MOTOR_ADC1_2_TRIGGER | ADC_CR2_DMA;
ADC2->CR1 = ADC_CR1_DUALMOD_1 | ADC_CR1_DUALMOD_2 | ADC_CR1_SCAN;
ADC2->CR2 = ADC_CR2_ADON | ADC_CR2_EXTTRIG | ADC_CR2_EXTSEL_0 | ADC_CR2_EXTSEL_1 | ADC_CR2_EXTSEL_2;
// ADC IRQ (only ADC1 IRQ is used because ADC2 is configured in slave mode)
chSysDisable();
nvicEnableVector(ADC1_2_IRQn, MOTOR_IRQ_PRIORITY_MASK);
chSysEnable();
}
static void init_timers(void)
{
assert_always(_pwm_top > 0); // Make sure it was initialized
chSysDisable();
// TIM1
RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
RCC->APB2RSTR |= RCC_APB2RSTR_TIM1RST;
RCC->APB2RSTR &= ~RCC_APB2RSTR_TIM1RST;
// TIM2
RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;
RCC->APB1RSTR |= RCC_APB1RSTR_TIM2RST;
RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM2RST;
chSysEnable();
// Reload value
TIM2->ARR = TIM1->ARR = _pwm_top;
// Left-aligned PWM, direction up (will be enabled later)
TIM2->CR1 = TIM1->CR1 = 0;
// Output idle state 0, buffered updates
TIM1->CR2 = TIM_CR2_CCUS | TIM_CR2_CCPC;
/*
* OC channels
* TIM1 CC1, CC2, CC3 are used to control the FETs; TIM1 CC4 is not used.
* TIM2 CC2 is used to trigger the ADC conversion.
*/
// Phase A, phase B
TIM1->CCMR1 =
TIM_CCMR1_OC1PE | TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1 |
TIM_CCMR1_OC2PE | TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2M_1;
// Phase C
TIM1->CCMR2 =
TIM_CCMR2_OC3PE | TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC3M_1;
// ADC sync
TIM2->CCMR1 =
TIM_CCMR1_OC2PE | TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2M_1 | TIM_CCMR1_OC2M_0;
// OC polarity (no inversion, all disabled except ADC sync)
TIM1->CCER = 0;
TIM2->CCER = TIM_CCER_CC2E;
/*
* Dead time generator setup.
* DTS clock divider set 0, hence fDTS = input clock.
* DTG bit 7 must be 0, otherwise it will change multiplier which is not supported yet.
* At 72 MHz one tick ~ 13.9 nsec, max 127 * 13.9 ~ 1.764 usec, which is large enough.
*/
const float pwm_dead_time = PWM_DEAD_TIME_NANOSEC / 1e9f;
const float pwm_dead_time_ticks_float = pwm_dead_time / (1.f / PWM_TIMER_FREQUENCY);
assert(pwm_dead_time_ticks_float > 0);
assert(pwm_dead_time_ticks_float < (_pwm_top * 0.2f));
uint16_t dead_time_ticks = (uint16_t)pwm_dead_time_ticks_float;
if (dead_time_ticks > 127) {
assert(0);
dead_time_ticks = 127;
}
lowsyslog("Motor: PWM dead time %u ticks\n", (unsigned)dead_time_ticks);
TIM1->BDTR = TIM_BDTR_AOE | TIM_BDTR_MOE | dead_time_ticks;
/*
* Default ADC sync config, will be adjusted dynamically
*/
TIM2->CCR2 = _pwm_top / 4;
// Timers are configured now but not started yet. Starting is tricky because of synchronization, see below.
TIM1->EGR = TIM_EGR_UG | TIM_EGR_COMG;
TIM2->EGR = TIM_EGR_UG | TIM_EGR_COMG;
}
