forked from Firmware/Firmwares
250 lines
8.3 KiB
C++
250 lines
8.3 KiB
C++
//============================================================================
|
|
// Name : riscv-bldc.cpp
|
|
// Author : Eyck Jentzsch
|
|
// Version :
|
|
// Copyright : Your copyright notice
|
|
// Description : Hello World in C++, Ansi-style
|
|
//============================================================================
|
|
|
|
#include "riscv-bldc.h"
|
|
#include "delay.h"
|
|
#include "bsp.h"
|
|
#include "plic/plic_driver.h"
|
|
|
|
#include <cstdio>
|
|
#include <cstdint>
|
|
|
|
#include "hifive1_io.h"
|
|
|
|
volatile uint32_t nextCommutationStep;
|
|
/*
|
|
Kommutierungsblöcke
|
|
1 2 3 4 5 6
|
|
U 0 z +1 +1 z 0
|
|
V +1 +1 z 0 0 z
|
|
W z 0 0 z +1 +1
|
|
*/
|
|
std::array<uint32_t, 6> driveTable { //! Drive pattern for commutation, CW rotation
|
|
((1 << VH) | (1 << UL)), //1
|
|
((1 << VH) | (1 << WL)), //2
|
|
((1 << UH) | (1 << WL)), //3
|
|
((1 << UH) | (1 << VL)), //4
|
|
((1 << WH) | (1 << VL)), //5
|
|
((1 << WH) | (1 << UL)) //6
|
|
};
|
|
std::array<uint32_t, 6> senseTable { //! channels to sense during the applied pattern
|
|
SENSW_N, //1
|
|
SENSU_P, //2
|
|
SENSV_N, //3
|
|
SENSW_P, //4
|
|
SENSU_N, //5
|
|
SENSV_P //6
|
|
};
|
|
std::array<unsigned int, 18> startupDelays{
|
|
/*
|
|
200, 150, 100, 80, 70, 65,
|
|
60, 55, 50, 45, 40, 35,
|
|
25, 25, 25, 25, 25, 25,
|
|
25, 25, 25, 25, 25, 25
|
|
*/
|
|
150, 90, 70, 50, 40, 35,
|
|
30, 25, 25, 25, 25, 25,
|
|
25, 25, 25, 25, 25, 25
|
|
};
|
|
|
|
bool ccw=false;
|
|
|
|
typedef void (*function_ptr_t) (void);
|
|
// Instance data for the PLIC.
|
|
plic_instance_t g_plic;
|
|
std::array<function_ptr_t,PLIC_NUM_INTERRUPTS> g_ext_interrupt_handlers;
|
|
|
|
extern "C" void handle_m_ext_interrupt() {
|
|
plic_source int_num = PLIC_claim_interrupt(&g_plic);
|
|
if ((int_num >=1 ) && (int_num < PLIC_NUM_INTERRUPTS))
|
|
g_ext_interrupt_handlers[int_num]();
|
|
else
|
|
exit(1 + (uintptr_t) int_num);
|
|
PLIC_complete_interrupt(&g_plic, int_num);
|
|
}
|
|
|
|
// 1sec interval interrupt
|
|
extern "C" void handle_m_time_interrupt(){
|
|
clear_csr(mie, MIP_MTIP);
|
|
// Reset the timer for 3s in the future.
|
|
// This also clears the existing timer interrupt.
|
|
volatile uint64_t * mtime = (uint64_t*) (CLINT_CTRL_ADDR + CLINT_MTIME);
|
|
volatile uint64_t * mtimecmp = (uint64_t*) (CLINT_CTRL_ADDR + CLINT_MTIMECMP);
|
|
uint64_t now = *mtime;
|
|
uint64_t then = now + RTC_FREQ;
|
|
*mtimecmp = then;
|
|
// Re-enable the timer interrupt.
|
|
set_csr(mie, MIP_MTIP);
|
|
}
|
|
|
|
void no_interrupt_handler (void) {};
|
|
|
|
void configure_irq(size_t irq_num, function_ptr_t handler, unsigned char prio=1) {
|
|
g_ext_interrupt_handlers[irq_num] = handler;
|
|
// Priority must be set > 0 to trigger the interrupt.
|
|
PLIC_set_priority(&g_plic, irq_num, prio);
|
|
// Have to enable the interrupt both at the GPIO level, and at the PLIC level.
|
|
PLIC_enable_interrupt(&g_plic, irq_num);
|
|
}
|
|
|
|
void platform_init(){
|
|
// configure clocks
|
|
PRCI_use_hfxosc(1); // is equivalent to
|
|
// init UART0 at 115200 baud
|
|
auto baud_rate=115200;
|
|
gpio0::output_en_reg()=0xffffffff;
|
|
gpio0::iof_sel_reg()&=~IOF0_UART0_MASK;
|
|
gpio0::iof_en_reg()|= IOF0_UART0_MASK;
|
|
uart0::div_reg()=get_cpu_freq() / baud_rate - 1;
|
|
uart0::txctrl_reg().txen=1;
|
|
// init SPI
|
|
gpio0::iof_sel_reg()&=~IOF0_SPI1_MASK;
|
|
gpio0::iof_en_reg()|= IOF0_SPI1_MASK;
|
|
qspi1::sckdiv_reg() = 8;
|
|
|
|
F_CPU=PRCI_measure_mcycle_freq(20, RTC_FREQ);
|
|
printf("core freq at %d Hz\n", F_CPU);
|
|
// initialie interupt & trap handling
|
|
write_csr(mtvec, &trap_entry);
|
|
if (read_csr(misa) & (1 << ('F' - 'A'))) { // if F extension is present
|
|
write_csr(mstatus, MSTATUS_FS); // allow FPU instructions without trapping
|
|
write_csr(fcsr, 0); // initialize rounding mode, undefined at reset
|
|
}
|
|
|
|
PLIC_init(&g_plic, PLIC_CTRL_ADDR, PLIC_NUM_INTERRUPTS, PLIC_NUM_PRIORITIES);
|
|
// Disable the machine & timer interrupts until setup is done.
|
|
clear_csr(mie, MIP_MEIP);
|
|
clear_csr(mie, MIP_MTIP);
|
|
for (auto& h:g_ext_interrupt_handlers) h=no_interrupt_handler;
|
|
configure_irq(40, pwm0::pwm_interrupt_handler);
|
|
configure_irq(6, qspi1::spi_rx_interrupt_handler);
|
|
// Set the machine timer to go off in 1 second.
|
|
volatile uint64_t * mtime = (uint64_t*) (CLINT_CTRL_ADDR + CLINT_MTIME);
|
|
volatile uint64_t * mtimecmp = (uint64_t*) (CLINT_CTRL_ADDR + CLINT_MTIMECMP);
|
|
uint64_t now = *mtime;
|
|
uint64_t then = now + RTC_FREQ;
|
|
*mtimecmp = then;
|
|
// Enable the Machine-External bit in MIE
|
|
set_csr(mie, MIP_MEIP);
|
|
// Enable the Machine-Timer bit in MIE
|
|
set_csr(mie, MIP_MTIP);
|
|
// Enable interrupts in general.
|
|
set_csr(mstatus, MSTATUS_MIE);
|
|
}
|
|
|
|
unsigned read_adc(unsigned channel){
|
|
std::array<uint8_t, 3> bytes{
|
|
uint8_t(0x06 | (channel>>2 & 0x1)), /* start bit, single ended measurement, channel[2] */
|
|
uint8_t((channel&0x3)<<6), /* channel[1:0], fill*/
|
|
0x0 /* fill */
|
|
};
|
|
// set CS of target
|
|
qspi1::csid_reg()=0;
|
|
qspi1::transfer(bytes);
|
|
return (bytes[1]&0xf)*256+bytes[2];
|
|
}
|
|
|
|
/*! \brief Generates a delay used during startup
|
|
*
|
|
* This functions is used to generate a delay during the startup procedure.
|
|
* The length of the delay equals delay * STARTUP_DELAY_MULTIPLIER microseconds.
|
|
* Since Timer/Counter1 is used in this function, it must never be called when
|
|
* sensorless operation is running.
|
|
*/
|
|
void fixed_delay(unsigned short delay){
|
|
pwm0::oneshot_delay(STARTUP_DELAY_MULTIPLIER*delay);
|
|
}
|
|
|
|
unsigned short measured_zc_time(unsigned short max_delay){
|
|
long delay_us = max_delay * STARTUP_DELAY_MULTIPLIER;
|
|
auto scaling_factor=0;
|
|
while(delay_us/(1<<scaling_factor) > std::numeric_limits<unsigned short>::max()){
|
|
scaling_factor++;
|
|
}
|
|
pwm0::cfg_reg()=0;
|
|
pwm0::count_reg()=0;
|
|
pwm0::cfg_reg().scale = 4+scaling_factor; // divide by 16 so we get 1us per pwm clock
|
|
pwm0::cmp0_reg().cmp0 = delay_us/(1<<scaling_factor);
|
|
pwm0::set_active();
|
|
pwm0::cfg_reg().enoneshot=true;
|
|
uint32_t channel=senseTable[nextCommutationStep]&0x3;
|
|
bool zc_neg = senseTable[nextCommutationStep]>3;
|
|
uint32_t adc_res=0;
|
|
do{
|
|
adc_res=read_adc(channel);
|
|
if((zc_neg && adc_res<2048) || (!zc_neg && adc_res>2047))
|
|
break;
|
|
} while(pwm0::is_active());
|
|
uint32_t sreg = pwm0::s_reg();
|
|
pwm0::cfg_reg().enoneshot=false;
|
|
return sreg*(1<<scaling_factor);
|
|
}
|
|
|
|
void next_commutation_step(void) {
|
|
if (ccw) {
|
|
if (nextCommutationStep == 0)
|
|
nextCommutationStep = 0;
|
|
else
|
|
nextCommutationStep--;
|
|
} else {
|
|
if (nextCommutationStep == 5)
|
|
nextCommutationStep = 0;
|
|
else
|
|
nextCommutationStep++;
|
|
}
|
|
}
|
|
|
|
void start_motor(void){
|
|
nextCommutationStep = 0;
|
|
//Preposition.
|
|
gpio0::port_reg() = (gpio0::port_reg() & ~DRIVE_MASK) | driveTable[nextCommutationStep];
|
|
fixed_delay(STARTUP_LOCK_DELAY);
|
|
next_commutation_step();
|
|
auto nextDrivePattern = driveTable[nextCommutationStep];
|
|
const size_t size=startupDelays.size();
|
|
for (size_t i = 0; i < startupDelays.size()+6*10; i++){
|
|
gpio0::port_reg() = (gpio0::port_reg() & ~DRIVE_MASK & 0x00ffffff)
|
|
| nextDrivePattern | nextCommutationStep<<24;
|
|
auto channel=senseTable[nextCommutationStep]&0x3;
|
|
auto zcPolRise = senseTable[nextCommutationStep]<4;
|
|
auto bemf_0=read_adc(channel);
|
|
fixed_delay(startupDelays[i>=size?size-1:i]);
|
|
auto bemf_1=read_adc(channel);
|
|
auto bemf = bemf_1>bemf_0?bemf_1-bemf_0:bemf_0-bemf_1;
|
|
next_commutation_step();
|
|
nextDrivePattern = driveTable[nextCommutationStep];
|
|
// if(i>12 && bemf>32 && ((zcPolRise && bemf_0<2048 && bemf_1>2047) || (!zcPolRise && bemf_0>2047 && bemf_1<2048)))
|
|
// return;
|
|
}
|
|
}
|
|
|
|
void run_motor(void){
|
|
auto count=0;
|
|
auto zc_delay=0U;
|
|
auto tmp=0U;
|
|
auto nextDrivePattern = driveTable[nextCommutationStep];
|
|
for(;;){
|
|
gpio0::port_reg() = (gpio0::port_reg() & ~DRIVE_MASK & 0x00ffffff)
|
|
| nextDrivePattern | nextCommutationStep<<24;
|
|
zc_delay=measured_zc_time(500);
|
|
pwm0::oneshot_delay(zc_delay);
|
|
next_commutation_step();
|
|
nextDrivePattern = driveTable[nextCommutationStep];
|
|
}
|
|
}
|
|
|
|
int main() {
|
|
platform_init();
|
|
printf("Starting motor\n");
|
|
start_motor();
|
|
printf("done...\n");
|
|
// Switch to sensorless commutation.
|
|
run_motor();
|
|
return 0;
|
|
}
|