mirror of https://github.com/rusefi/bldc.git
150 lines
3.7 KiB
C
150 lines
3.7 KiB
C
#include "app.h"
|
|
|
|
#include "ch.h" // ChibiOS
|
|
#include "hal.h" // ChibiOS HAL
|
|
#include "stm32f4xx_conf.h"
|
|
#include "mc_interface.h" // Motor control functions
|
|
#include "hw.h" // Pin mapping on this hardware
|
|
#include "timeout.h" // To reset the timeout
|
|
#include "utils.h"
|
|
#include "commands.h"
|
|
#include <math.h>
|
|
#include "isr_vector_table.h"
|
|
#include "stdlib.h"
|
|
|
|
#define SPEED_STEP 0.05
|
|
#define SPEED_MAX 1.00
|
|
#define SPEED_MIN 0.10
|
|
#define SPEED_OFF 0.00
|
|
|
|
//private variables
|
|
static volatile bool stop_now = true;
|
|
static volatile bool is_running = false;
|
|
static float targetSpeed=SPEED_MIN;
|
|
|
|
//Threads
|
|
static THD_FUNCTION(dpv_thread, arg);
|
|
static THD_WORKING_AREA(dpv_thread_wa, 2048); // 2kb stack for this thread
|
|
static thread_t *dpv_tp;
|
|
virtual_timer_t dpv_vt;
|
|
|
|
//private functions
|
|
static void update(void *p);
|
|
|
|
void app_custom_configure(app_configuration *conf)
|
|
{
|
|
(void)conf;
|
|
}
|
|
|
|
void app_custom_stop(void)
|
|
{
|
|
stop_now = true;
|
|
|
|
if (is_running) {
|
|
chEvtSignalI(dpv_tp, (eventmask_t) 1);
|
|
mc_interface_release_motor();
|
|
}
|
|
|
|
while (is_running) {
|
|
chThdSleepMilliseconds(1);
|
|
}
|
|
|
|
}
|
|
|
|
void app_custom_start(void) {
|
|
|
|
stop_now = false;
|
|
//Config GPIO
|
|
palSetPadMode(HW_HALL_TRIGGER_GPIO, HW_HALL_TRIGGER_PIN, PAL_MODE_INPUT_PULLUP);
|
|
|
|
// Start the dv thread
|
|
chThdCreateStatic(dpv_thread_wa, sizeof(dpv_thread_wa), NORMALPRIO, dpv_thread, NULL);
|
|
|
|
hw_start_i2c();
|
|
app_uartcomm_start(UART_PORT_COMM_HEADER);
|
|
app_uartcomm_start(UART_PORT_BUILTIN);
|
|
chSysLock();
|
|
chVTSetI(&dpv_vt, MS2ST(1), update, NULL);
|
|
chSysUnlock();
|
|
}
|
|
|
|
static void update(void *p) {
|
|
if (!is_running) {
|
|
return;
|
|
}
|
|
|
|
chSysLockFromISR();
|
|
chVTSetI(&dpv_vt, MS2ST(2), update, p);
|
|
chEvtSignalI(dpv_tp, (eventmask_t) 1);
|
|
chSysUnlockFromISR();
|
|
}
|
|
|
|
static THD_FUNCTION(dpv_thread, arg) {
|
|
(void)arg;
|
|
|
|
int angle=0, oldangle=0;
|
|
|
|
chRegSetThreadName("APP_DPV");
|
|
dpv_tp = chThdGetSelfX();
|
|
|
|
angle = gsvesc_get_angle();
|
|
oldangle=angle;
|
|
|
|
is_running = true;
|
|
for(;;) {
|
|
chEvtWaitAny((eventmask_t) 1);
|
|
if (stop_now) {
|
|
is_running = false;
|
|
return;
|
|
}
|
|
|
|
const volatile mc_configuration *mcconf = mc_interface_get_configuration();
|
|
float motorSpeed=SPEED_MIN;
|
|
|
|
if (app_is_output_disabled() ) {
|
|
continue;
|
|
}
|
|
|
|
if ( mc_interface_get_fault() != FAULT_CODE_NONE) {
|
|
continue;
|
|
}
|
|
|
|
// Get position of speed poti
|
|
angle = gsvesc_get_angle();
|
|
|
|
if (abs(oldangle - angle) > 1000) { // sprung ?
|
|
angle = oldangle;
|
|
} else {
|
|
oldangle = angle;
|
|
}
|
|
if (angle > 3800) angle = 3800;
|
|
if (angle < 400) angle = 400;
|
|
targetSpeed= utils_map(angle,400,3800,SPEED_MIN,SPEED_MAX);
|
|
// Apply ramping
|
|
|
|
static systime_t last_time = 0;
|
|
static float motorSpeed_val_ramp = 0.0;
|
|
float ramp_time;
|
|
if ( ! palReadPad(HW_HALL_TRIGGER_GPIO, HW_HALL_TRIGGER_PIN)) {
|
|
motorSpeed=targetSpeed;
|
|
} else {
|
|
motorSpeed=SPEED_OFF;
|
|
}
|
|
ramp_time = fabsf(motorSpeed) > fabsf(motorSpeed_val_ramp) ? 5.0 : 0.5;
|
|
if (fabsf(motorSpeed) > 0.01) {
|
|
ramp_time = fminf(3.0, 3.0);
|
|
}
|
|
if (ramp_time > 0.01) {
|
|
const float ramp_step = (float)ST2MS(chVTTimeElapsedSinceX(last_time)) / (ramp_time * 1000.0);
|
|
utils_step_towards(&motorSpeed_val_ramp, motorSpeed, ramp_step);
|
|
last_time = chVTGetSystemTimeX();
|
|
motorSpeed = motorSpeed_val_ramp;
|
|
}
|
|
//mc_interface_set_pid_speed(motorSpeed*mcconf->l_max_erpm);
|
|
mc_interface_set_duty(motorSpeed*mcconf->l_max_duty);
|
|
chThdSleepMilliseconds(5);
|
|
// Reset the timeout
|
|
timeout_reset();
|
|
}
|
|
}
|