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; }