RPM calculation and longboard app updates

This commit is contained in:
Benjamin Vedder 2014-08-01 15:43:36 +02:00
parent 8f79c061aa
commit 7051ccd557
7 changed files with 146 additions and 95 deletions

View File

@ -36,8 +36,8 @@
#define TIMEOUT 500 #define TIMEOUT 500
#define HYST 0.10 #define HYST 0.10
// 29000rpm = 20kmh // 29000rpm = 20kmh
#define RPM_MAX_1 41000 // Start decreasing output here #define RPM_MAX_1 41000.0 // Start decreasing output here
#define RPM_MAX_2 44000 // Completely stop output here #define RPM_MAX_2 44000.0 // Completely stop output here
// Private variables // Private variables
static volatile float out_received = 0.0; static volatile float out_received = 0.0;
@ -162,10 +162,18 @@ static void set_output(float output) {
if (rpm > RPM_MAX_2) { if (rpm > RPM_MAX_2) {
current = -MCPWM_CURRENT_CONTROL_MIN; current = -MCPWM_CURRENT_CONTROL_MIN;
} else if (rpm > RPM_MAX_1) { } else if (rpm > RPM_MAX_1) {
current = utils_map(rpm, RPM_MAX_2, RPM_MAX_1, -MCPWM_CURRENT_CONTROL_MIN, output); current = utils_map(rpm, RPM_MAX_1, RPM_MAX_2, current, -MCPWM_CURRENT_CONTROL_MIN);
if (fabsf(current) < MCPWM_CURRENT_CONTROL_MIN) { }
current = -MCPWM_CURRENT_CONTROL_MIN;
} // Some low-pass filtering
static float current_p1 = 0.0;
static float current_p2 = 0.0;
current = (current + current_p1 + current_p2) / 3;
current_p2 = current_p1;
current_p1 = current;
if (fabsf(current) < MCPWM_CURRENT_CONTROL_MIN) {
current = -MCPWM_CURRENT_CONTROL_MIN;
} }
mcpwm_set_current(current); mcpwm_set_current(current);

View File

@ -38,9 +38,11 @@
// Sensorless settings // Sensorless settings
#define MCPWM_IS_SENSORLESS 1 // Use sensorless commutation #define MCPWM_IS_SENSORLESS 1 // Use sensorless commutation
#define MCPWM_MIN_RPM 300 // Auto-commutate below this RPM #define MCPWM_MIN_RPM 200 // Auto-commutate below this RPM
#define MCPWM_CYCLE_INT_LIMIT_LOW 250.0 // Flux integrator limit 0 ERPM #define MCPWM_CYCLE_INT_START_RPM_BR 6000.0 // RPM border between the START and LOW interval
#define MCPWM_CYCLE_INT_LIMIT_HIGH 30.0 // Flux integrator limit 50K ERPM #define MCPWM_CYCLE_INT_LIMIT_START 1700.0 // Flux integrator limit 0 ERPM
#define MCPWM_CYCLE_INT_LIMIT_LOW 260.0 // Flux integrator limit MCPWM_CYCLE_INT_START_RPM_BR ERPM
#define MCPWM_CYCLE_INT_LIMIT_HIGH 30.0 // Flux integrator limit 80K ERPM
// Speed PID parameters // Speed PID parameters
#define MCPWM_PID_KP 0.0001 // Proportional gain #define MCPWM_PID_KP 0.0001 // Proportional gain

View File

@ -38,8 +38,10 @@
// Sensorless settings // Sensorless settings
#define MCPWM_IS_SENSORLESS 1 // Use sensorless commutation #define MCPWM_IS_SENSORLESS 1 // Use sensorless commutation
#define MCPWM_MIN_RPM 200 // Auto-commutate below this RPM #define MCPWM_MIN_RPM 200 // Auto-commutate below this RPM
#define MCPWM_CYCLE_INT_LIMIT_LOW 150.0 // Flux integrator limit 0 ERPM #define MCPWM_CYCLE_INT_START_RPM_BR 5000.0 // RPM border between the START and LOW interval
#define MCPWM_CYCLE_INT_LIMIT_HIGH 20.0 // Flux integrator limit 50K ERPM #define MCPWM_CYCLE_INT_LIMIT_START 1200.0 // Flux integrator limit 0 ERPM
#define MCPWM_CYCLE_INT_LIMIT_LOW 120.0 // Flux integrator limit MCPWM_CYCLE_INT_START_RPM_BR ERPM
#define MCPWM_CYCLE_INT_LIMIT_HIGH 20.0 // Flux integrator limit 80K ERPM
// Speed PID parameters // Speed PID parameters
#define MCPWM_PID_KP 0.0001 // Proportional gain #define MCPWM_PID_KP 0.0001 // Proportional gain

View File

@ -40,8 +40,10 @@
// Sensorless settings // Sensorless settings
#define MCPWM_IS_SENSORLESS 1 // Use sensorless commutation #define MCPWM_IS_SENSORLESS 1 // Use sensorless commutation
#define MCPWM_MIN_RPM 300 // Auto-commutate below this RPM #define MCPWM_MIN_RPM 300 // Auto-commutate below this RPM
#define MCPWM_CYCLE_INT_LIMIT_LOW 50.0 // Flux integrator limit 0 ERPM #define MCPWM_CYCLE_INT_START_RPM_BR 5000.0 // RPM border between the START and LOW interval
#define MCPWM_CYCLE_INT_LIMIT_HIGH 10.0 // Flux integrator limit 50K ERPM #define MCPWM_CYCLE_INT_LIMIT_START 600.0 // Flux integrator limit 0 ERPM
#define MCPWM_CYCLE_INT_LIMIT_LOW 50.0 // Flux integrator limit MCPWM_CYCLE_INT_START_RPM_BR ERPM
#define MCPWM_CYCLE_INT_LIMIT_HIGH 10.0 // Flux integrator limit 80K ERPM
// Hall sensor settings // Hall sensor settings
#define MCPWM_HALL_DIR 0 // Hall sensor direction [0 or 1] #define MCPWM_HALL_DIR 0 // Hall sensor direction [0 or 1]

View File

@ -28,7 +28,7 @@
/* /*
* Parameters * Parameters
*/ */
#define MCPWM_CURRENT_MAX 40.0 // Current limit in Amperes (Upper) #define MCPWM_CURRENT_MAX 45.0 // Current limit in Amperes (Upper)
#define MCPWM_CURRENT_MIN -30.0 // Current limit in Amperes (Lower) #define MCPWM_CURRENT_MIN -30.0 // Current limit in Amperes (Lower)
#define MCPWM_IN_CURRENT_MAX 25.0 // Input current limit in Amperes (Upper) #define MCPWM_IN_CURRENT_MAX 25.0 // Input current limit in Amperes (Upper)
#define MCPWM_IN_CURRENT_MIN -25.0 // Input current limit in Amperes (Lower) #define MCPWM_IN_CURRENT_MIN -25.0 // Input current limit in Amperes (Lower)
@ -42,9 +42,11 @@
// Sensorless settings // Sensorless settings
#define MCPWM_IS_SENSORLESS 1 // Use sensorless commutation #define MCPWM_IS_SENSORLESS 1 // Use sensorless commutation
#define MCPWM_MIN_RPM 250 // Auto-commutate below this RPM #define MCPWM_MIN_RPM 300 // Auto-commutate below this RPM
#define MCPWM_CYCLE_INT_LIMIT_LOW 200.0 // Flux integrator limit 0 ERPM #define MCPWM_CYCLE_INT_START_RPM_BR 6000.0 // RPM border between the START and LOW interval
#define MCPWM_CYCLE_INT_LIMIT_HIGH 30.0 // Flux integrator limit 50K ERPM #define MCPWM_CYCLE_INT_LIMIT_START 1700.0 // Flux integrator limit 0 ERPM
#define MCPWM_CYCLE_INT_LIMIT_LOW 260.0 // Flux integrator limit MCPWM_CYCLE_INT_START_RPM_BR ERPM
#define MCPWM_CYCLE_INT_LIMIT_HIGH 30.0 // Flux integrator limit 80K ERPM
// Speed PID parameters // Speed PID parameters
#define MCPWM_PID_KP 0.0001 // Proportional gain #define MCPWM_PID_KP 0.0001 // Proportional gain
@ -54,6 +56,6 @@
// Current control parameters // Current control parameters
#define MCPWM_CURRENT_CONTROL_GAIN 0.0016 // Current controller error gain #define MCPWM_CURRENT_CONTROL_GAIN 0.0016 // Current controller error gain
#define MCPWM_CURRENT_CONTROL_MIN 0.2 // Minimum allowed current #define MCPWM_CURRENT_CONTROL_MIN 0.05 // Minimum allowed current
#endif /* MCCONF_STEN_H_ */ #endif /* MCCONF_STEN_H_ */

184
mcpwm.c
View File

@ -46,11 +46,17 @@ typedef struct {
volatile unsigned int curr2_sample; volatile unsigned int curr2_sample;
} mc_timer_struct; } mc_timer_struct;
typedef struct {
volatile float cycle_int_limit;
volatile float cycle_int_limit_running;
volatile uint32_t comms;
volatile uint32_t time_at_comm;
} rpm_dep_struct;
// Private variables // Private variables
static volatile int comm_step; // Range [1 6] static volatile int comm_step; // Range [1 6]
static volatile int detect_step; // Range [0 5] static volatile int detect_step; // Range [0 5]
static volatile int direction; static volatile int direction;
static volatile int last_comm_time;
static volatile float dutycycle_set; static volatile float dutycycle_set;
static volatile float dutycycle_now; static volatile float dutycycle_now;
static volatile float rpm_now; static volatile float rpm_now;
@ -84,6 +90,7 @@ static int hall_to_phase_table[16];
static volatile unsigned int cycles_running; static volatile unsigned int cycles_running;
static volatile unsigned int slow_ramping_cycles; static volatile unsigned int slow_ramping_cycles;
static volatile int has_commutated; static volatile int has_commutated;
static volatile rpm_dep_struct rpm_dep;
#if MCPWM_IS_SENSORLESS #if MCPWM_IS_SENSORLESS
static volatile float cycle_integrator; static volatile float cycle_integrator;
@ -146,13 +153,14 @@ static int try_input(void);
static void do_dc_cal(void); static void do_dc_cal(void);
// Defines // Defines
#define ADC_CDR_ADDRESS ((uint32_t)0x40012308)
#define CYCLE_INT_START (0.0) #define CYCLE_INT_START (0.0)
#define IS_DETECTING() (state == MC_STATE_DETECTING) #define IS_DETECTING() (state == MC_STATE_DETECTING)
// Threads // Threads
static WORKING_AREA(timer_thread_wa, 1024); static WORKING_AREA(timer_thread_wa, 1024);
static msg_t timer_thread(void *arg); static msg_t timer_thread(void *arg);
static WORKING_AREA(rpm_thread_wa, 1024);
static msg_t rpm_thread(void *arg);
void mcpwm_init(void) { void mcpwm_init(void) {
chSysLock(); chSysLock();
@ -166,7 +174,6 @@ void mcpwm_init(void) {
detect_step = 0; detect_step = 0;
direction = 1; direction = 1;
rpm_now = 0; rpm_now = 0;
last_comm_time = 0;
dutycycle_set = 0.0; dutycycle_set = 0.0;
dutycycle_now = 0.0; dutycycle_now = 0.0;
speed_pid_set_rpm = 0.0; speed_pid_set_rpm = 0.0;
@ -190,6 +197,7 @@ void mcpwm_init(void) {
cycles_running = 0; cycles_running = 0;
slow_ramping_cycles = 0; slow_ramping_cycles = 0;
has_commutated = 0; has_commutated = 0;
memset((void*)&rpm_dep, 0, sizeof(rpm_dep));
#if MCPWM_IS_SENSORLESS #if MCPWM_IS_SENSORLESS
cycle_integrator = CYCLE_INT_START; cycle_integrator = CYCLE_INT_START;
@ -273,7 +281,7 @@ void mcpwm_init(void) {
// DMA for the ADC // DMA for the ADC
DMA_InitStructure.DMA_Channel = DMA_Channel_0; DMA_InitStructure.DMA_Channel = DMA_Channel_0;
DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&ADC_Value; DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&ADC_Value;
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)ADC_CDR_ADDRESS; DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC->CDR;
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
DMA_InitStructure.DMA_BufferSize = HW_ADC_CHANNELS; DMA_InitStructure.DMA_BufferSize = HW_ADC_CHANNELS;
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
@ -438,8 +446,9 @@ void mcpwm_init(void) {
// TIM3 enable counter // TIM3 enable counter
TIM_Cmd(TIM4, ENABLE); TIM_Cmd(TIM4, ENABLE);
// Start the timer thread // Start threads
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL); chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
chThdCreateStatic(rpm_thread_wa, sizeof(rpm_thread_wa), NORMALPRIO, rpm_thread, NULL);
// WWDG configuration // WWDG configuration
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, ENABLE); RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, ENABLE);
@ -981,6 +990,58 @@ static void run_pid_controller(void) {
set_duty_cycle_hl(output); set_duty_cycle_hl(output);
} }
static msg_t rpm_thread(void *arg) {
(void)arg;
chRegSetThreadName("rpm timer");
for (;;) {
if (rpm_dep.comms > 0.0) {
chSysLock();
const float comms = (float) rpm_dep.comms;
const float time_at_comm = (float) rpm_dep.time_at_comm;
rpm_dep.comms = 0;
rpm_dep.time_at_comm = 0;
chSysUnlock();
rpm_now = (comms * MCPWM_RPM_TIMER_FREQ * 60.0)
/ (time_at_comm * 6.0);
} else {
// In case we have slowed down
float rpm_tmp = (MCPWM_RPM_TIMER_FREQ * 60.0)
/ ((float) TIM2 ->CNT * 6.0);
if (rpm_tmp < rpm_now) {
rpm_now = rpm_tmp;
}
}
// Some low-pass filtering
static float rpm_p1 = 0.0;
static float rpm_p2 = 0.0;
rpm_now = (rpm_now + rpm_p1 + rpm_p2) / 3;
rpm_p2 = rpm_p1;
rpm_p1 = rpm_now;
// Update the cycle integrator limit
rpm_dep.cycle_int_limit = utils_map(rpm_now,
MCPWM_CYCLE_INT_START_RPM_BR, 80000.0,
MCPWM_CYCLE_INT_LIMIT_LOW, MCPWM_CYCLE_INT_LIMIT_HIGH);
if (rpm_now < MCPWM_CYCLE_INT_START_RPM_BR) {
rpm_dep.cycle_int_limit_running = utils_map(rpm_now, 0,
MCPWM_CYCLE_INT_START_RPM_BR, MCPWM_CYCLE_INT_LIMIT_START,
MCPWM_CYCLE_INT_LIMIT_LOW);
} else {
rpm_dep.cycle_int_limit_running = rpm_dep.cycle_int_limit;
}
chThdSleepMilliseconds(1);
}
return 0;
}
static msg_t timer_thread(void *arg) { static msg_t timer_thread(void *arg) {
(void)arg; (void)arg;
@ -994,21 +1055,6 @@ static msg_t timer_thread(void *arg) {
#endif #endif
for(;;) { for(;;) {
// Update RPM in case it has slowed down
uint32_t tim_val = TIM2->CNT;
uint32_t tim_diff = tim_val - last_comm_time;
if (tim_diff > 0) {
float rpm_tmp = ((float)MCPWM_AVG_COM_RPM * MCPWM_RPM_TIMER_FREQ * 60.0) /
((float)tim_diff * 6.0);
// Re-calculate RPM between commutations
// This will end up being used when slowing down
if (rpm_tmp < rpm_now) {
rpm_now = rpm_tmp;
}
}
if (state != MC_STATE_OFF) { if (state != MC_STATE_OFF) {
tachometer_for_direction = 0; tachometer_for_direction = 0;
} }
@ -1271,9 +1317,9 @@ void mcpwm_adc_int_handler(void *p, uint32_t flags) {
WWDG_SetCounter(100); WWDG_SetCounter(100);
const float input_voltage = GET_INPUT_VOLTAGE(); const float input_voltage = GET_INPUT_VOLTAGE();
volatile int ph1, ph2, ph3; int ph1, ph2, ph3;
static volatile int direction_before = 1; static int direction_before = 1;
if (state == MC_STATE_RUNNING && direction == direction_before) { if (state == MC_STATE_RUNNING && direction == direction_before) {
cycles_running++; cycles_running++;
} else { } else {
@ -1300,7 +1346,7 @@ void mcpwm_adc_int_handler(void *p, uint32_t flags) {
* If the motor has been running for a while use half the input voltage * If the motor has been running for a while use half the input voltage
* as the zero reference. Otherwise, calculate the zero reference manually. * as the zero reference. Otherwise, calculate the zero reference manually.
*/ */
if (cycles_running > 1000) { if (cycles_running > 1000.0) {
mcpwm_vzero = ADC_V_ZERO; mcpwm_vzero = ADC_V_ZERO;
} else { } else {
mcpwm_vzero = (ADC_V_L1 + ADC_V_L2 + ADC_V_L3) / 3; mcpwm_vzero = (ADC_V_L1 + ADC_V_L2 + ADC_V_L3) / 3;
@ -1318,10 +1364,10 @@ void mcpwm_adc_int_handler(void *p, uint32_t flags) {
float amp = 0.0; float amp = 0.0;
if (cycles_running == 0) { if (has_commutated) {
amp = sqrtf((float)(ph1*ph1 + ph2*ph2 + ph3*ph3)) * sqrtf(2.0);
} else {
amp = fabsf(dutycycle_now) * (float)ADC_Value[ADC_IND_VIN_SENS]; amp = fabsf(dutycycle_now) * (float)ADC_Value[ADC_IND_VIN_SENS];
} else {
amp = sqrtf((float)(ph1*ph1 + ph2*ph2 + ph3*ph3)) * sqrtf(2.0);
} }
// Fill the amplitude FIR filter // Fill the amplitude FIR filter
@ -1340,47 +1386,48 @@ void mcpwm_adc_int_handler(void *p, uint32_t flags) {
} }
} }
int v_diff = 0; if (state == MC_STATE_RUNNING || state == MC_STATE_OFF) {
int v_diff = 0;
switch (comm_step) {
case 1:
v_diff = ph1;
break;
case 2:
v_diff = -ph2;
break;
case 3:
v_diff = ph3;
break;
case 4:
v_diff = -ph1;
break;
case 5:
v_diff = ph2;
break;
case 6:
v_diff = -ph3;
break;
default:
break;
}
switch (comm_step) { if (v_diff > 0) {
case 1: cycle_integrator += (float)v_diff / (float)switching_frequency_now;
v_diff = ph1;
break;
case 2:
v_diff = -ph2;
break;
case 3:
v_diff = ph3;
break;
case 4:
v_diff = -ph1;
break;
case 5:
v_diff = ph2;
break;
case 6:
v_diff = -ph3;
break;
default:
break;
}
if (v_diff > 0) { float limit;
if (state == MC_STATE_RUNNING || state == MC_STATE_OFF) { if (has_commutated) {
cycle_integrator += v_diff / (float)switching_frequency_now; limit = rpm_dep.cycle_int_limit_running * 0.0005;
} else {
limit = rpm_dep.cycle_int_limit * 0.0005;
}
const float rpm_fac = rpm_now / 50000.0; if ((cycle_integrator >= MCPWM_CYCLE_INT_LIMIT_START * 0.0005 || pwm_cycles_sum > last_pwm_cycles_sum / 3.0 || !has_commutated)
const float cycle_int_limit = MCPWM_CYCLE_INT_LIMIT_LOW * (1.0 - rpm_fac) +
MCPWM_CYCLE_INT_LIMIT_HIGH * rpm_fac;
const float limit = (cycle_int_limit * 0.0005);
if ((cycle_integrator >= 10 * limit || pwm_cycles_sum > last_pwm_cycles_sum / 3.0 || state != MC_STATE_RUNNING)
&& (pwm_cycles_sum > 2)
&& cycle_integrator >= limit) { && cycle_integrator >= limit) {
commutate(); commutate();
cycle_integrator = CYCLE_INT_START; cycle_integrator = CYCLE_INT_START;
} }
} else {
cycle_integrator = CYCLE_INT_START;
} }
} else { } else {
cycle_integrator = CYCLE_INT_START; cycle_integrator = CYCLE_INT_START;
@ -1782,20 +1829,9 @@ static void update_adc_sample_pos(mc_timer_struct *timer_tmp) {
} }
static void update_rpm_tacho(void) { static void update_rpm_tacho(void) {
static uint32_t comm_counter = 0; rpm_dep.comms++;
comm_counter++; rpm_dep.time_at_comm += TIM2->CNT;
TIM2->CNT = 0;
if (comm_counter == MCPWM_AVG_COM_RPM) {
comm_counter = 0;
uint32_t tim_val = TIM2->CNT;
uint32_t tim_diff = tim_val - last_comm_time;
last_comm_time = tim_val;
if (tim_diff > 0) {
rpm_now = ((float)MCPWM_AVG_COM_RPM * MCPWM_RPM_TIMER_FREQ * 60.0) /
((float)tim_diff * 6.0);
}
}
static int last_step = 0; static int last_step = 0;
int tacho_diff = 0; int tacho_diff = 0;

View File

@ -111,14 +111,13 @@ extern volatile int mcpwm_vzero;
/* /*
* Parameters * Parameters
*/ */
#define MCPWM_SWITCH_FREQUENCY_MIN 4000 // The lowest switching frequency in Hz #define MCPWM_SWITCH_FREQUENCY_MIN 3000 // The lowest switching frequency in Hz
#define MCPWM_SWITCH_FREQUENCY_MAX 30000 // The highest switching frequency in Hz #define MCPWM_SWITCH_FREQUENCY_MAX 35000 // The highest switching frequency in Hz
#define MCPWM_DEAD_TIME_CYCLES 80 // Dead time #define MCPWM_DEAD_TIME_CYCLES 80 // Dead time
#define MCPWM_RPM_TIMER_FREQ 1000000.0 // Frequency of the RPM measurement timer #define MCPWM_RPM_TIMER_FREQ 1000000.0 // Frequency of the RPM measurement timer
#define MCPWM_PWM_MODE PWM_MODE_SYNCHRONOUS // Default PWM mode #define MCPWM_PWM_MODE PWM_MODE_SYNCHRONOUS // Default PWM mode
#define MCPWM_MIN_DUTY_CYCLE 0.005 // Minimum duty cycle #define MCPWM_MIN_DUTY_CYCLE 0.005 // Minimum duty cycle
#define MCPWM_MAX_DUTY_CYCLE 0.95 // Maximum duty cycle #define MCPWM_MAX_DUTY_CYCLE 0.95 // Maximum duty cycle
#define MCPWM_AVG_COM_RPM 3 // Number of commutations to average RPM over
#define MCPWM_RAMP_STEP 0.01 // Ramping step (1000 times/sec) at maximum duty cycle #define MCPWM_RAMP_STEP 0.01 // Ramping step (1000 times/sec) at maximum duty cycle
#define MCPWM_RAMP_STEP_CURRENT_MAX 0.04 // Maximum ramping step (1000 times/sec) for the current control #define MCPWM_RAMP_STEP_CURRENT_MAX 0.04 // Maximum ramping step (1000 times/sec) for the current control
#define MCPWM_RAMP_STEP_RPM_LIMIT 0.0005 // Ramping step when limiting the RPM #define MCPWM_RAMP_STEP_RPM_LIMIT 0.0005 // Ramping step when limiting the RPM