Sync commit
This commit is contained in:
parent
250fa831d0
commit
215767a724
Binary file not shown.
|
@ -0,0 +1,16 @@
|
|||
/*
|
||||
* peripherals.c
|
||||
*
|
||||
* Created on: 10.09.2018
|
||||
* Author: eyck
|
||||
*/
|
||||
|
||||
#include "peripherals.h"
|
||||
|
||||
template<> volatile bool qspi0::spi_active=false;
|
||||
template<> volatile bool qspi1::spi_active=false;
|
||||
template<> volatile bool qspi2::spi_active=false;
|
||||
template<> volatile bool pwm0::pwm_active=false;
|
||||
template<> volatile bool pwm1::pwm_active=false;
|
||||
template<> volatile bool pwm2::pwm_active=false;
|
||||
|
|
@ -10,6 +10,7 @@
|
|||
|
||||
#include <sifive/devices/pwm.h>
|
||||
#include "util/bit_field.h"
|
||||
#include <limits>
|
||||
#include <cstdint>
|
||||
|
||||
template<uint32_t BASE_ADDR>
|
||||
|
@ -84,6 +85,37 @@ public:
|
|||
return *reinterpret_cast<pwmcmp3_t*>(BASE_ADDR+PWM_CMP3);
|
||||
}
|
||||
|
||||
static inline bool oneshot_delay(long delay_us){
|
||||
auto scaling_factor=0;
|
||||
while(delay_us/(1<<scaling_factor) > std::numeric_limits<unsigned short>::max()){
|
||||
scaling_factor++;
|
||||
}
|
||||
cfg_reg()=0;
|
||||
count_reg()=0;
|
||||
cfg_reg().scale=4+scaling_factor; // divide by 16 so we get 1us per pwm clock
|
||||
cmp0_reg().cmp0 = delay_us/(1<<scaling_factor);
|
||||
pwm_active=true;
|
||||
cfg_reg().enoneshot=true;
|
||||
do{
|
||||
asm("wfi");
|
||||
asm("nop");
|
||||
}while(pwm_active);
|
||||
return true;
|
||||
}
|
||||
|
||||
static void pwm_interrupt_handler(){
|
||||
cfg_reg().cmp0ip=false;
|
||||
pwm_active=false;
|
||||
}
|
||||
|
||||
inline
|
||||
static bool is_active(){ return pwm_active; }
|
||||
|
||||
inline
|
||||
static void set_active() {pwm_active=true;}
|
||||
|
||||
private:
|
||||
static volatile bool pwm_active;
|
||||
};
|
||||
|
||||
|
||||
|
|
|
@ -12,39 +12,45 @@
|
|||
#include "bsp.h"
|
||||
#include "plic/plic_driver.h"
|
||||
|
||||
#include <array>
|
||||
#include <limits>
|
||||
#include <cstdio>
|
||||
#include <cstdint>
|
||||
|
||||
volatile uint32_t nextCommutationStep;
|
||||
volatile uint32_t nextDrivePattern;
|
||||
volatile uint32_t zcPolarity;
|
||||
volatile uint32_t filteredTimeSinceCommutation;
|
||||
|
||||
|
||||
std::array<uint32_t, 6> cwDriveTable {
|
||||
DRIVE_PATTERN_CW::STEP1,
|
||||
DRIVE_PATTERN_CW::STEP2,
|
||||
DRIVE_PATTERN_CW::STEP3,
|
||||
DRIVE_PATTERN_CW::STEP4,
|
||||
DRIVE_PATTERN_CW::STEP5,
|
||||
DRIVE_PATTERN_CW::STEP6
|
||||
std::array<uint32_t, 6> cwDriveTable { //! Drive pattern for commutation, CW rotation
|
||||
((1 << VH) | (1 << WL)), ((1 << UH) | (1 << WL)), ((1 << UH) | (1 << VL)),
|
||||
((1 << WH) | (1 << VL)), ((1 << WH) | (1 << UL)), ((1 << VH) | (1 << UL))
|
||||
};
|
||||
std::array<uint32_t, 6> ccwDriveTable{
|
||||
DRIVE_PATTERN_CCW::STEP1,
|
||||
DRIVE_PATTERN_CCW::STEP2,
|
||||
DRIVE_PATTERN_CCW::STEP3,
|
||||
DRIVE_PATTERN_CCW::STEP4,
|
||||
DRIVE_PATTERN_CCW::STEP5,
|
||||
DRIVE_PATTERN_CCW::STEP6
|
||||
std::array<uint32_t, 6> cwSenseTable { //! channels to sense during the applied pattern
|
||||
SENSU_P, SENSV_N, SENSW_P,
|
||||
SENSU_N, SENSV_P, SENSW_N
|
||||
};
|
||||
std::array<uint32_t, 6> ccwDriveTable{ //! Drive pattern for commutation, CCW rotation.
|
||||
((1 << UL) | (1 << VH)), ((1 << UL) | (1 << WH)), ((1 << VL) | (1 << WH)),
|
||||
((1 << VL) | (1 << UH)), ((1 << WL) | (1 << UH)), ((1 << WL) | (1 << VH))
|
||||
};
|
||||
std::array<uint32_t, 6> ccwSenseTable { //! channels to sense during the applied pattern
|
||||
SENSW_P, SENSV_N, SENSU_P,
|
||||
SENSW_N, SENSV_P, SENSU_N
|
||||
};
|
||||
std::array<unsigned int, 24> 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, 25, 25, 25, 25, 25, 25
|
||||
200, 150, 100, 80, 70, 65, 60, 55, 50, 50, 50, 50, 50, 50, 50, 50, 50, 40, 40, 40, 40, 40, 40, 40
|
||||
/*
|
||||
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, 50, 50,
|
||||
50, 50, 50, 40, 40, 40,
|
||||
40, 40, 40, 30, 30, 30,
|
||||
30, 30, 30, 25, 25, 25,
|
||||
};
|
||||
|
||||
bool ccw=false;
|
||||
|
||||
auto& driveTable = ccw?ccwDriveTable:cwDriveTable;
|
||||
auto& senseTable = ccw?ccwSenseTable:cwSenseTable;
|
||||
|
||||
typedef void (*function_ptr_t) (void);
|
||||
// Instance data for the PLIC.
|
||||
plic_instance_t g_plic;
|
||||
|
@ -75,10 +81,12 @@ extern "C" void handle_m_time_interrupt(){
|
|||
|
||||
void no_interrupt_handler (void) {};
|
||||
|
||||
volatile bool pwm_active=false;
|
||||
void pwm_interrupt_handler(){
|
||||
pwm0::cfg_reg().cmp0ip=false;
|
||||
pwm_active=false;
|
||||
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(){
|
||||
|
@ -94,7 +102,7 @@ void platform_init(){
|
|||
// init SPI
|
||||
gpio0::iof_sel_reg()&=~IOF0_SPI1_MASK;
|
||||
gpio0::iof_en_reg()|= IOF0_SPI1_MASK;
|
||||
qspi0::sckdiv_reg() = 8;
|
||||
qspi1::sckdiv_reg() = 8;
|
||||
|
||||
F_CPU=PRCI_measure_mcycle_freq(20, RTC_FREQ);
|
||||
printf("core freq at %d Hz\n", F_CPU);
|
||||
|
@ -110,14 +118,8 @@ void platform_init(){
|
|||
clear_csr(mie, MIP_MEIP);
|
||||
clear_csr(mie, MIP_MTIP);
|
||||
for (auto& h:g_ext_interrupt_handlers) h=no_interrupt_handler;
|
||||
g_ext_interrupt_handlers[40] = pwm_interrupt_handler;
|
||||
// Priority must be set > 0 to trigger the interrupt.
|
||||
PLIC_set_priority(&g_plic, 40, 1);
|
||||
// Have to enable the interrupt both at the GPIO level, and at the PLIC level.
|
||||
PLIC_enable_interrupt (&g_plic, 40);
|
||||
// enable peripheral interrupt
|
||||
// GPIO_REG(GPIO_RISE_IE) |= (1 << BUTTON_0_OFFSET);
|
||||
|
||||
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);
|
||||
|
@ -132,6 +134,18 @@ void platform_init(){
|
|||
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.
|
||||
|
@ -139,64 +153,86 @@ void platform_init(){
|
|||
* Since Timer/Counter1 is used in this function, it must never be called when
|
||||
* sensorless operation is running.
|
||||
*/
|
||||
void StartupDelay(unsigned short delay){
|
||||
#if 0
|
||||
delayUS(delay * STARTUP_DELAY_MULTIPLIER);
|
||||
#else
|
||||
void fixed_delay(unsigned short delay){
|
||||
pwm0::oneshot_delay(STARTUP_DELAY_MULTIPLIER*delay);
|
||||
}
|
||||
|
||||
unsigned short measured_zc_time(unsigned short max_delay, unsigned state){
|
||||
long delay_us = max_delay * STARTUP_DELAY_MULTIPLIER;
|
||||
auto scaling_factor=0;
|
||||
unsigned d = delay * STARTUP_DELAY_MULTIPLIER;
|
||||
while(d/(1<<scaling_factor) > std::numeric_limits<unsigned short>::max()){
|
||||
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 = d/(1<<scaling_factor);
|
||||
pwm_active=true;
|
||||
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{
|
||||
asm("wfi");
|
||||
asm("nop");
|
||||
}while(pwm_active);
|
||||
#endif
|
||||
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 StartMotor(void){
|
||||
auto& driveTable = ccw?ccwDriveTable:cwDriveTable;
|
||||
void start_motor(void){
|
||||
nextCommutationStep = 0;
|
||||
//Preposition.
|
||||
gpio0::port_reg() = driveTable[nextCommutationStep];
|
||||
printf("init\n");
|
||||
StartupDelay(STARTUP_LOCK_DELAY);
|
||||
gpio0::port_reg() = (gpio0::port_reg() & ~DRIVE_MASK) | driveTable[nextCommutationStep];
|
||||
fixed_delay(STARTUP_LOCK_DELAY);
|
||||
nextCommutationStep++;
|
||||
nextDrivePattern = driveTable[nextCommutationStep];
|
||||
auto nextDrivePattern = driveTable[nextCommutationStep];
|
||||
const size_t size=startupDelays.size();
|
||||
for (size_t i = 0; i < startupDelays.size()+100; i++){
|
||||
printf("step%d\n", i);
|
||||
gpio0::port_reg() = nextDrivePattern;
|
||||
auto index = i>=size?size-1:i;
|
||||
StartupDelay(startupDelays[index]);
|
||||
|
||||
// switch ADC input
|
||||
// ADMUX = ADMUXTable[nextCommutationStep];
|
||||
|
||||
// Use LSB of nextCommutationStep to determine zero crossing polarity.
|
||||
zcPolarity = nextCommutationStep & 0x01;
|
||||
|
||||
for (size_t i = 0; i < startupDelays.size()+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;
|
||||
nextCommutationStep++;
|
||||
if (nextCommutationStep >= 6){
|
||||
nextCommutationStep = 0;
|
||||
}
|
||||
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;
|
||||
for(;;){
|
||||
gpio0::port_reg() = (gpio0::port_reg() & ~DRIVE_MASK & 0x00ffffff)
|
||||
| driveTable[nextCommutationStep] | nextCommutationStep<<24;
|
||||
zc_delay=measured_zc_time(50, senseTable[nextCommutationStep]);
|
||||
// tmp=zc_delay>>2;
|
||||
// pwm0::oneshot_delay(zc_delay>tmp?zc_delay:zc_delay/2+zc_delay/4+zc_delay/8);
|
||||
pwm0::oneshot_delay(zc_delay);
|
||||
nextCommutationStep++;
|
||||
if (nextCommutationStep >= 6)
|
||||
nextCommutationStep = 0;
|
||||
}
|
||||
// Switch to sensorless commutation.
|
||||
// Set filteredTimeSinceCommutation to the time to the next commutation.
|
||||
filteredTimeSinceCommutation = startupDelays[startupDelays.size() - 1] * (STARTUP_DELAY_MULTIPLIER / 2);
|
||||
}
|
||||
|
||||
int main() {
|
||||
platform_init();
|
||||
StartMotor();
|
||||
printf("done...");
|
||||
printf("Starting motor\n");
|
||||
start_motor();
|
||||
printf("done...\n");
|
||||
// Switch to sensorless commutation.
|
||||
run_motor();
|
||||
return 0;
|
||||
}
|
||||
|
|
|
@ -13,46 +13,33 @@
|
|||
extern uint32_t pwm;
|
||||
extern uint32_t DRIVE_PORT;
|
||||
|
||||
enum PINS{
|
||||
UL=1, //! Port pin connected to phase U, low side enable switch.
|
||||
UH=0, //! Port pin connected to phase U, high side enable switch.
|
||||
VL=11,//! Port pin connected to phase V, low side enable switch.
|
||||
VH=10,//! Port pin connected to phase V, high side enable switch.
|
||||
WL=19,//! Port pin connected to phase W, low side enable switch.
|
||||
WH=20 //! Port pin connected to phase W, high side enable switch.
|
||||
enum {
|
||||
UL=1, //! Port pin connected to phase U, low side enable switch.
|
||||
UH=0, //! Port pin connected to phase U, high side enable switch.
|
||||
VL=11, //! Port pin connected to phase V, low side enable switch.
|
||||
VH=10, //! Port pin connected to phase V, high side enable switch.
|
||||
WL=19, //! Port pin connected to phase W, low side enable switch.
|
||||
WH=20, //! Port pin connected to phase W, high side enable switch.
|
||||
CW=0, //! Clockwise rotation flag.
|
||||
CCW=1, //! Counterclockwise rotation flag.
|
||||
SENSU_P=0, //! Phase U voltage to sense
|
||||
SENSV_P=1, //! Phase V voltage to sense
|
||||
SENSW_P=2, //! Phase W voltage to sense
|
||||
SENSU_N=4, //! Phase U voltage to sense
|
||||
SENSV_N=5, //! Phase V voltage to sense
|
||||
SENSW_N=6, //! Phase W voltage to sense
|
||||
DRIVE_MASK=(1<<UL)|(1<<UH)| (1<<VL)|(1<<VH)| (1<<WL)|(1<<WH)
|
||||
};
|
||||
|
||||
enum {
|
||||
CW=0, //! Clockwise rotation flag.
|
||||
CCW=1 //! Counterclockwise rotation flag.
|
||||
};
|
||||
|
||||
namespace DRIVE_PATTERN_CCW{
|
||||
enum {
|
||||
STEP1=((1 << UL) | (1 << VH)),//! Drive pattern for commutation step 1, CCW rotation.
|
||||
STEP2=((1 << UL) | (1 << WH)),//! Drive pattern for commutation step 2, CCW rotation.
|
||||
STEP3=((1 << VL) | (1 << WH)),//! Drive pattern for commutation step 3, CCW rotation.
|
||||
STEP4=((1 << VL) | (1 << UH)),//! Drive pattern for commutation step 4, CCW rotation.
|
||||
STEP5=((1 << WL) | (1 << UH)),//! Drive pattern for commutation step 5, CCW rotation.
|
||||
STEP6=((1 << WL) | (1 << VH)) //! Drive pattern for commutation step 6, CCW rotation.
|
||||
};
|
||||
}
|
||||
namespace DRIVE_PATTERN_CW {
|
||||
enum {
|
||||
STEP1=((1 << VH) | (1 << WL)),//! Drive pattern for commutation step 1, CW rotation.
|
||||
STEP2=((1 << UH) | (1 << WL)),//! Drive pattern for commutation step 2, CW rotation.
|
||||
STEP3=((1 << UH) | (1 << VL)),//! Drive pattern for commutation step 3, CW rotation.
|
||||
STEP4=((1 << WH) | (1 << VL)),//! Drive pattern for commutation step 4, CW rotation.
|
||||
STEP5=((1 << WH) | (1 << UL)),//! Drive pattern for commutation step 5, CW rotation.
|
||||
STEP6=((1 << VH) | (1 << UL)) //! Drive pattern for commutation step 6, CW rotation.
|
||||
};
|
||||
}
|
||||
//! Startup delays are given in microseconds times STARTUP_DELAY_MULTIPLIER.
|
||||
const auto STARTUP_DELAY_MULTIPLIER=1000;
|
||||
/*!
|
||||
* Number of milliseconds to lock rotor in first commutation step before
|
||||
* the timed startup sequence is initiated.
|
||||
*/
|
||||
const auto STARTUP_LOCK_DELAY=50;
|
||||
const auto STARTUP_LOCK_DELAY=200;
|
||||
|
||||
extern "C" void handle_m_ext_interrupt();
|
||||
extern "C" void handle_m_time_interrupt();
|
||||
|
||||
#endif /* RISCV_BLDC_H_ */
|
||||
|
|
|
@ -10,6 +10,7 @@
|
|||
|
||||
#include <sifive/devices/spi.h>
|
||||
#include "util/bit_field.h"
|
||||
#include <array>
|
||||
#include <cstdint>
|
||||
|
||||
template<uint32_t BASE_ADDR>
|
||||
|
@ -165,6 +166,35 @@ public:
|
|||
return *reinterpret_cast<ip_t*>(BASE_ADDR+SPI_REG_IP);
|
||||
}
|
||||
|
||||
template<size_t SIZE>
|
||||
static bool transfer(std::array<uint8_t, SIZE>& bytes){
|
||||
csmode_reg().mode=2; // HOLD mode
|
||||
rxctrl_reg().rxmark=bytes.size(); // trigger irq if 3 bytes are received;
|
||||
ie_reg().rxwm=1;
|
||||
// write data bytes
|
||||
for(size_t i=0; i<bytes.size(); ++i)
|
||||
txfifo_reg()=bytes[i];
|
||||
// wait until SPI is done
|
||||
spi_active=true;
|
||||
do{
|
||||
asm("wfi");
|
||||
asm("nop");
|
||||
}while(spi_active);
|
||||
// deactivate SPI
|
||||
csmode_reg().mode=0; // AUTO mode, deactivates CS
|
||||
// fetch results
|
||||
for(size_t i=0; i<bytes.size(); ++i) bytes[i]=rxfifo_reg();
|
||||
return true;
|
||||
}
|
||||
|
||||
static void spi_rx_interrupt_handler(){
|
||||
ip_reg().rxwm=0;
|
||||
ie_reg().rxwm=0;
|
||||
spi_active=false;
|
||||
}
|
||||
|
||||
private:
|
||||
static volatile bool spi_active;
|
||||
};
|
||||
|
||||
#endif /* SPI_H_ */
|
||||
|
|
Loading…
Reference in New Issue