rusefi
/
bldc
mirror of https://github.com/rusefi/bldc.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'dev_fw_5_02' of https://github.com/vedderb/bldc into dev_fw_5_02

This commit is contained in:
Jeffrey M. Friesen 2020-07-21 08:27:33 -07:00
parent ca41e3f9a6 821174141d
commit f41a54c23d
78 changed files with 2122 additions and 407 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
CHANGELOG
View File

@ -5,6 +5,14 @@
* Fixed motor temperature reading on hw with ADC mux.
* Added speed PID input ramping option.
* Added LSM6DS3 IMU support.
* Added MTPA support. See See: https://github.com/vedderb/bldc/pull/179
* Added HW_HD75 support.
* NRF52 UICR write fix.
* App PPM rework. See https://github.com/vedderb/bldc/pull/192
* Added bm_reset terminal command.
* Added bm support for STM32F30x and STM32L47x.
* App Balance updates. See https://github.com/vedderb/bldc/pull/193
* Motor current now based on magnitude of both axes.
=== FW 5.01 ===
* Fixed PPM bug in previous release.

1
Makefile
View File

@ -155,6 +155,7 @@ CSRC = $(STARTUPSRC) \
confgenerator.c \
timer.c \
i2c_bb.c \
spi_bb.c \
virtual_motor.c \
shutdown.c \
mempools.c \

115
appconf/appconf_default.h
View File

@ -272,106 +272,121 @@
// Balance app
#ifndef APPCONF_BALANCE_KP
#define APPCONF_BALANCE_KP 0.0
#define APPCONF_BALANCE_KP 0.0
#endif
#ifndef APPCONF_BALANCE_KI
#define APPCONF_BALANCE_KI 0.0
#define APPCONF_BALANCE_KI 0.0
#endif
#ifndef APPCONF_BALANCE_KD
#define APPCONF_BALANCE_KD 0.0
#define APPCONF_BALANCE_KD 0.0
#endif
#ifndef APPCONF_BALANCE_HERTZ
#define APPCONF_BALANCE_HERTZ 1000
#define APPCONF_BALANCE_HERTZ 1000
#endif
#ifndef APPCONF_BALANCE_PITCH_FAULT
#define APPCONF_BALANCE_PITCH_FAULT 20
#ifndef APPCONF_BALANCE_FAULT_PITCH
#define APPCONF_BALANCE_FAULT_PITCH 20
#endif
#ifndef APPCONF_BALANCE_ROLL_FAULT
#define APPCONF_BALANCE_ROLL_FAULT 45
#ifndef APPCONF_BALANCE_FAULT_ROLL
#define APPCONF_BALANCE_FAULT_ROLL 45
#endif
#ifndef APPCONF_BALANCE_ADC1
#define APPCONF_BALANCE_ADC1 0.0
#ifndef APPCONF_BALANCE_FAULT_DUTY
#define APPCONF_BALANCE_FAULT_DUTY 0.9
#endif
#ifndef APPCONF_BALANCE_ADC2
#define APPCONF_BALANCE_ADC2 0.0
#ifndef APPCONF_BALANCE_FAULT_ADC1
#define APPCONF_BALANCE_FAULT_ADC1 0.0
#endif
#ifndef APPCONF_BALANCE_OVERSPEED_DUTY
#define APPCONF_BALANCE_OVERSPEED_DUTY 0.9
#ifndef APPCONF_BALANCE_FAULT_ADC2
#define APPCONF_BALANCE_FAULT_ADC2 0.0
#endif
#ifndef APPCONF_BALANCE_TILTBACK_DUTY
#define APPCONF_BALANCE_TILTBACK_DUTY 0.75
#ifndef APPCONF_BALANCE_FAULT_DELAY_PITCH
#define APPCONF_BALANCE_FAULT_DELAY_PITCH 0
#endif
#ifndef APPCONF_BALANCE_FAULT_DELAY_ROLL
#define APPCONF_BALANCE_FAULT_DELAY_ROLL 0
#endif
#ifndef APPCONF_BALANCE_FAULT_DELAY_DUTY
#define APPCONF_BALANCE_FAULT_DELAY_DUTY 0
#endif
#ifndef APPCONF_BALANCE_FAULT_DELAY_SWITCH_HALF
#define APPCONF_BALANCE_FAULT_DELAY_SWITCH_HALF 0
#endif
#ifndef APPCONF_BALANCE_FAULT_DELAY_SWITCH_FULL
#define APPCONF_BALANCE_FAULT_DELAY_SWITCH_FULL 0
#endif
#ifndef APPCONF_BALANCE_FAULT_ADC_HALF_ERPM
#define APPCONF_BALANCE_FAULT_ADC_HALF_ERPM 1000
#endif
#ifndef APPCONF_BALANCE_TILTBACK_ANGLE
#define APPCONF_BALANCE_TILTBACK_ANGLE 15.0
#define APPCONF_BALANCE_TILTBACK_ANGLE 15.0
#endif
#ifndef APPCONF_BALANCE_TILTBACK_SPEED
#define APPCONF_BALANCE_TILTBACK_SPEED 5.0
#define APPCONF_BALANCE_TILTBACK_SPEED 5.0
#endif
#ifndef APPCONF_BALANCE_TILTBACK_DUTY
#define APPCONF_BALANCE_TILTBACK_DUTY 0.75
#endif
#ifndef APPCONF_BALANCE_TILTBACK_HIGH_V
#define APPCONF_BALANCE_TILTBACK_HIGH_V 100.0
#define APPCONF_BALANCE_TILTBACK_HIGH_V 200.0
#endif
#ifndef APPCONF_BALANCE_TILTBACK_LOW_V
#define APPCONF_BALANCE_TILTBACK_LOW_V 0.0
#define APPCONF_BALANCE_TILTBACK_LOW_V 0.0
#endif
#ifndef APPCONF_BALANCE_TILTBACK_CONSTANT
#define APPCONF_BALANCE_TILTBACK_CONSTANT 0.0
#endif
#ifndef APPCONF_BALANCE_TILTBACK_CONSTANT_ERPM
#define APPCONF_BALANCE_TILTBACK_CONSTANT_ERPM 500
#endif
#ifndef APPCONF_BALANCE_STARTUP_PITCH_TOLERANCE
#define APPCONF_BALANCE_STARTUP_PITCH_TOLERANCE 20.0
#endif
#ifndef APPCONF_BALANCE_STARTUP_ROLL_TOLERANCE
#define APPCONF_BALANCE_STARTUP_ROLL_TOLERANCE 8.0
#define APPCONF_BALANCE_STARTUP_ROLL_TOLERANCE 8.0
#endif
#ifndef APPCONF_BALANCE_STARTUP_SPEED
#define APPCONF_BALANCE_STARTUP_SPEED 30.0
#define APPCONF_BALANCE_STARTUP_SPEED 30.0
#endif
#ifndef APPCONF_BALANCE_DEADZONE
#define APPCONF_BALANCE_DEADZONE 0.0
#define APPCONF_BALANCE_DEADZONE 0.0
#endif
#ifndef APPCONF_BALANCE_CURRENT_BOOST
#define APPCONF_BALANCE_CURRENT_BOOST 0.0
#define APPCONF_BALANCE_CURRENT_BOOST 0.0
#endif
#ifndef APPCONF_BALANCE_MULTI_ESC
#define APPCONF_BALANCE_MULTI_ESC false
#define APPCONF_BALANCE_MULTI_ESC false
#endif
#ifndef APPCONF_BALANCE_YAW_KP
#define APPCONF_BALANCE_YAW_KP 0.0
#define APPCONF_BALANCE_YAW_KP 0.0
#endif
#ifndef APPCONF_BALANCE_YAW_KI
#define APPCONF_BALANCE_YAW_KI 0.0
#define APPCONF_BALANCE_YAW_KI 0.0
#endif
#ifndef APPCONF_BALANCE_YAW_KD
#define APPCONF_BALANCE_YAW_KD 0.0
#define APPCONF_BALANCE_YAW_KD 0.0
#endif
#ifndef APPCONF_BALANCE_ROLL_STEER_KP
#define APPCONF_BALANCE_ROLL_STEER_KP 0.0
#endif
#ifndef APPCONF_BALANCE_BRAKE_CURRENT
#define APPCONF_BALANCE_BRAKE_CURRENT 0.0
#endif
#ifndef APPCONF_BALANCE_OVERSPEED_DELAY
#define APPCONF_BALANCE_OVERSPEED_DELAY 0
#endif
#ifndef APPCONF_BALANCE_FAULT_DELAY
#define APPCONF_BALANCE_FAULT_DELAY 0
#endif
#ifndef APPCONF_BALANCE_TILTBACK_CONSTANT
#define APPCONF_BALANCE_TILTBACK_CONSTANT 0.0
#define APPCONF_BALANCE_ROLL_STEER_KP 0.0
#endif
#ifndef APPCONF_BALANCE_ROLL_STEER_ERPM_KP
#define APPCONF_BALANCE_ROLL_STEER_ERPM_KP 0.0
#define APPCONF_BALANCE_ROLL_STEER_ERPM_KP 0.0
#endif
#ifndef APPCONF_BALANCE_BRAKE_CURRENT
#define APPCONF_BALANCE_BRAKE_CURRENT 0.0
#endif
#ifndef APPCONF_BALANCE_YAW_CURRENT_CLAMP
#define APPCONF_BALANCE_YAW_CURRENT_CLAMP 0.0
#endif
#ifndef APPCONF_BALANCE_ADC_HALF_FAULT_ERPM
#define APPCONF_BALANCE_ADC_HALF_FAULT_ERPM 1000
#define APPCONF_BALANCE_YAW_CURRENT_CLAMP 0.0
#endif
#ifndef APPCONF_BALANCE_SETPOINT_PITCH_FILTER
#define APPCONF_BALANCE_SETPOINT_PITCH_FILTER 0.0
#define APPCONF_BALANCE_SETPOINT_PITCH_FILTER 0.0
#endif
#ifndef APPCONF_BALANCE_SETPOINT_TARGET_FILTER
#define APPCONF_BALANCE_SETPOINT_TARGET_FILTER 1.0
#define APPCONF_BALANCE_SETPOINT_TARGET_FILTER 1.0
#endif
#ifndef APPCONF_BALANCE_SETPOINT_CLAMP
#define APPCONF_BALANCE_SETPOINT_CLAMP 80.0
#ifndef APPCONF_BALANCE_SETPOINT_FILTER_CLAMP
#define APPCONF_BALANCE_SETPOINT_FILTER_CLAMP 8.0
#endif
#ifndef APPCONF_BALANCE_KD_PT1_FREQUENCY
#define APPCONF_BALANCE_KD_PT1_FREQUENCY 0
#endif
// IMU

353
applications/app_balance.c
View File

@ -41,8 +41,16 @@
typedef enum {
STARTUP = 0,
RUNNING,
FAULT,
DEAD
RUNNING_TILTBACK_DUTY,
RUNNING_TILTBACK_HIGH_VOLTAGE,
RUNNING_TILTBACK_LOW_VOLTAGE,
RUNNING_TILTBACK_CONSTANT,
FAULT_ANGLE_PITCH,
FAULT_ANGLE_ROLL,
FAULT_SWITCH_HALF,
FAULT_SWITCH_FULL,
FAULT_DUTY,
FAULT_STARTUP
} BalanceState;
typedef enum {
@ -60,81 +68,73 @@ typedef enum {
static THD_FUNCTION(balance_thread, arg);
static THD_WORKING_AREA(balance_thread_wa, 2048); // 2kb stack for this thread
static volatile balance_config balance_conf;
static volatile imu_config imu_conf;
static thread_t *app_thread;
// Values used in loop
static BalanceState state;
// Config values
static volatile balance_config balance_conf;
static volatile imu_config imu_conf;
static float startup_step_size, tiltback_step_size;
// Runtime values read from elsewhere
static float pitch_angle, roll_angle;
static float gyro[3];
static float duty_cycle, abs_duty_cycle;
static float erpm, abs_erpm, avg_erpm;
static float proportional, integral, derivative;
static float last_proportional;
static float pid_value;
static float yaw_proportional, yaw_integral, yaw_derivative, yaw_last_proportional, yaw_pid_value, yaw_setpoint;
static float setpoint, setpoint_target, setpoint_target_interpolated;
static SetpointAdjustmentType setpointAdjustmentType;
static float startup_step_size, tiltback_step_size;
static systime_t current_time, last_time, diff_time;
static systime_t startup_start_time, startup_diff_time;
static systime_t dead_start_time;
static systime_t fault_start_time;
static float motor_current;
static float motor_position;
static float adc1, adc2;
static SwitchState switch_state;
// Values read to pass in app data to GUI
static float motor_current;
static float motor_position;
// Rumtime state values
static BalanceState state;
static float proportional, integral, derivative;
static float last_proportional;
static float pid_value;
static float setpoint, setpoint_target, setpoint_target_interpolated;
static SetpointAdjustmentType setpointAdjustmentType;
static float yaw_proportional, yaw_integral, yaw_derivative, yaw_last_proportional, yaw_pid_value, yaw_setpoint;
static systime_t current_time, last_time, diff_time;
static systime_t fault_angle_pitch_timer, fault_angle_roll_timer, fault_switch_timer, fault_switch_half_timer, fault_duty_timer;
static float d_pt1_state, d_pt1_k;
void app_balance_configure(balance_config *conf, imu_config *conf2) {
balance_conf = *conf;
imu_conf = *conf2;
// Set calculated values from config
if(balance_conf.kd_pt1_frequency > 0){
float dT = 1.0 / balance_conf.hertz;
float RC = 1.0 / ( 2.0 * M_PI * balance_conf.kd_pt1_frequency);
d_pt1_k = dT / (RC + dT);
}
startup_step_size = balance_conf.startup_speed / balance_conf.hertz;
tiltback_step_size = balance_conf.tiltback_speed / balance_conf.hertz;
}
void app_balance_start(void) {
// Reset all Values
// First start only, override state to startup
state = STARTUP;
pitch_angle = 0;
roll_angle = 0;
gyro[0] = 0;
gyro[1] = 0;
gyro[2] = 0;
duty_cycle = 0;
abs_duty_cycle = 0;
erpm = 0;
abs_erpm = 0;
avg_erpm = 0;
adc1 = 0;
adc2 = 0;
switch_state = OFF;
proportional = 0;
// Start the balance thread
app_thread = chThdCreateStatic(balance_thread_wa, sizeof(balance_thread_wa), NORMALPRIO, balance_thread, NULL);
}
void reset_vars(void){
// Clear accumulated values.
integral = 0;
derivative = 0;
last_proportional = 0;
pid_value = 0;
yaw_proportional = 0;
yaw_integral = 0;
yaw_derivative = 0;
yaw_last_proportional = 0;
yaw_pid_value = 0;
yaw_setpoint = 0;
setpoint = 0;
d_pt1_state = 0;
// Set values for startup
setpoint = pitch_angle;
setpoint_target_interpolated = pitch_angle;
setpoint_target = 0;
setpoint_target_interpolated = 0;
setpointAdjustmentType = CENTERING;
startup_step_size = 0;
tiltback_step_size = 0;
yaw_setpoint = 0;
state = RUNNING;
current_time = 0;
last_time = 0;
diff_time = 0;
startup_start_time = 0;
startup_diff_time = 0;
// Start the balance thread
app_thread = chThdCreateStatic(balance_thread_wa, sizeof(balance_thread_wa), NORMALPRIO, balance_thread, NULL);
}
float app_balance_get_pid_output(void) {
@ -178,6 +178,119 @@ float get_setpoint_adjustment_step_size(void){
return 0;
}
// Fault checking order does not really matter. From a UX perspective, switch should be before angle.
bool check_faults(bool ignoreTimers){
// Check switch
// Switch fully open
if(switch_state == OFF){
if(ST2MS(current_time - fault_switch_timer) > balance_conf.fault_delay_switch_full || ignoreTimers){
state = FAULT_SWITCH_FULL;
return true;
}
} else {
fault_switch_timer = current_time;
}
// Switch partially open and stopped
if(switch_state == HALF && abs_erpm < balance_conf.fault_adc_half_erpm){
if(ST2MS(current_time - fault_switch_half_timer) > balance_conf.fault_delay_switch_half || ignoreTimers){
state = FAULT_SWITCH_HALF;
return true;
}
} else {
fault_switch_half_timer = current_time;
}
// Check pitch angle
if(fabsf(pitch_angle) > balance_conf.fault_pitch){
if(ST2MS(current_time - fault_angle_pitch_timer) > balance_conf.fault_delay_pitch || ignoreTimers){
state = FAULT_ANGLE_PITCH;
return true;
}
}else{
fault_angle_pitch_timer = current_time;
}
// Check roll angle
if(fabsf(roll_angle) > balance_conf.fault_roll){
if(ST2MS(current_time - fault_angle_roll_timer) > balance_conf.fault_delay_roll || ignoreTimers){
state = FAULT_ANGLE_ROLL;
return true;
}
}else{
fault_angle_roll_timer = current_time;
}
// Check for duty
if(abs_duty_cycle > balance_conf.fault_duty){
if(ST2MS(current_time - fault_duty_timer) > balance_conf.fault_delay_duty || ignoreTimers){
state = FAULT_DUTY;
return true;
}
} else {
fault_duty_timer = current_time;
}
return false;
}
void calculate_setpoint_target(void){
if(setpointAdjustmentType == CENTERING && setpoint_target_interpolated != setpoint_target){
// Ignore tiltback during centering sequence
state = RUNNING;
}else if(abs_duty_cycle > balance_conf.tiltback_duty){
if(erpm > 0){
setpoint_target = balance_conf.tiltback_angle;
} else {
setpoint_target = -balance_conf.tiltback_angle;
}
setpointAdjustmentType = TILTBACK;
state = RUNNING_TILTBACK_DUTY;
}else if(abs_duty_cycle > 0.05 && GET_INPUT_VOLTAGE() > balance_conf.tiltback_high_voltage){
if(erpm > 0){
setpoint_target = balance_conf.tiltback_angle;
} else {
setpoint_target = -balance_conf.tiltback_angle;
}
setpointAdjustmentType = TILTBACK;
state = RUNNING_TILTBACK_HIGH_VOLTAGE;
}else if(abs_duty_cycle > 0.05 && GET_INPUT_VOLTAGE() < balance_conf.tiltback_low_voltage){
if(erpm > 0){
setpoint_target = balance_conf.tiltback_angle;
} else {
setpoint_target = -balance_conf.tiltback_angle;
}
setpointAdjustmentType = TILTBACK;
state = RUNNING_TILTBACK_LOW_VOLTAGE;
}else if(balance_conf.tiltback_constant != 0 && abs_erpm > balance_conf.tiltback_constant_erpm){
// Nose angle adjustment
if(erpm > 0){
setpoint_target = balance_conf.tiltback_constant;
} else {
setpoint_target = -balance_conf.tiltback_constant;
}
setpointAdjustmentType = TILTBACK;
state = RUNNING_TILTBACK_CONSTANT;
}else{
setpointAdjustmentType = TILTBACK;
setpoint_target = 0;
state = RUNNING;
}
}
void calculate_setpoint_interpolated(void){
if(setpoint_target_interpolated != setpoint_target){
// If we are less than one step size away, go all the way
if(fabsf(setpoint_target - setpoint_target_interpolated) < get_setpoint_adjustment_step_size()){
setpoint_target_interpolated = setpoint_target;
}else if (setpoint_target - setpoint_target_interpolated > 0){
setpoint_target_interpolated += get_setpoint_adjustment_step_size();
}else{
setpoint_target_interpolated -= get_setpoint_adjustment_step_size();
}
}
}
float apply_deadzone(float error){
if(balance_conf.deadzone == 0){
return error;
@ -237,12 +350,6 @@ static THD_FUNCTION(balance_thread, arg) {
(void)arg;
chRegSetThreadName("APP_BALANCE");
// Do one off config
startup_step_size = balance_conf.startup_speed / balance_conf.hertz;
tiltback_step_size = balance_conf.tiltback_speed / balance_conf.hertz;
state = STARTUP;
while (!chThdShouldTerminateX()) {
// Update times
current_time = chVTGetSystemTimeX();
@ -282,24 +389,24 @@ static THD_FUNCTION(balance_thread, arg) {
#endif
// Calculate switch state from ADC values
if(balance_conf.adc1 == 0 && balance_conf.adc2 == 0){ // No Switch
if(balance_conf.fault_adc1 == 0 && balance_conf.fault_adc2 == 0){ // No Switch
switch_state = ON;
}else if(balance_conf.adc2 == 0){ // Single switch on ADC1
if(adc1 > balance_conf.adc1){
}else if(balance_conf.fault_adc2 == 0){ // Single switch on ADC1
if(adc1 > balance_conf.fault_adc1){
switch_state = ON;
} else {
switch_state = OFF;
}
}else if(balance_conf.adc1 == 0){ // Single switch on ADC2
if(adc2 > balance_conf.adc2){
}else if(balance_conf.fault_adc1 == 0){ // Single switch on ADC2
if(adc2 > balance_conf.fault_adc2){
switch_state = ON;
} else {
switch_state = OFF;
}
}else{ // Double switch
if(adc1 > balance_conf.adc1 && adc2 > balance_conf.adc2){
if(adc1 > balance_conf.fault_adc1 && adc2 > balance_conf.fault_adc2){
switch_state = ON;
}else if(adc1 > balance_conf.adc1 || adc2 > balance_conf.adc2){
}else if(adc1 > balance_conf.fault_adc1 || adc2 > balance_conf.fault_adc2){
switch_state = HALF;
}else{
switch_state = OFF;
@ -307,7 +414,7 @@ static THD_FUNCTION(balance_thread, arg) {
}
// State based logic
// Control Loop State Logic
switch(state){
case (STARTUP):
while(!imu_startup_done()){
@ -316,70 +423,23 @@ static THD_FUNCTION(balance_thread, arg) {
// Wait
chThdSleepMilliseconds(50);
}
state = FAULT;
startup_start_time = 0;
startup_diff_time = 0;
reset_vars();
state = FAULT_STARTUP; // Trigger a fault so we need to meet start conditions to start
break;
case (RUNNING):
// Check for overspeed
if(abs_duty_cycle > balance_conf.overspeed_duty){
if(ST2MS(current_time - dead_start_time) > balance_conf.overspeed_delay){
state = DEAD;
}
} else {
dead_start_time = current_time;
case (RUNNING_TILTBACK_DUTY):
case (RUNNING_TILTBACK_HIGH_VOLTAGE):
case (RUNNING_TILTBACK_LOW_VOLTAGE):
case (RUNNING_TILTBACK_CONSTANT):
// Check for faults
if(check_faults(false)){
break;
}
// Check for fault
if(
fabsf(pitch_angle) > balance_conf.pitch_fault || // Balnce axis tip over
fabsf(roll_angle) > balance_conf.roll_fault || // Cross axis tip over
switch_state == OFF || // Switch fully open
(switch_state == HALF && abs_erpm < balance_conf.adc_half_fault_erpm) // Switch partially open and stopped
){
if(ST2MS(current_time - fault_start_time) > balance_conf.fault_delay){
state = FAULT;
}
} else {
fault_start_time = current_time;
}
// Over speed tilt back safety
if(setpointAdjustmentType == CENTERING && setpoint_target_interpolated != setpoint_target){
// Ignore tiltback during centering sequence
}else if(abs_duty_cycle > balance_conf.tiltback_duty ||
(abs_duty_cycle > 0.05 && GET_INPUT_VOLTAGE() > balance_conf.tiltback_high_voltage) ||
(abs_duty_cycle > 0.05 && GET_INPUT_VOLTAGE() < balance_conf.tiltback_low_voltage)){
if(duty_cycle > 0){
setpoint_target = balance_conf.tiltback_angle;
} else {
setpoint_target = -balance_conf.tiltback_angle;
}
setpointAdjustmentType = TILTBACK;
}else if(abs_duty_cycle > 0.03){
// Nose angle adjustment
if(duty_cycle > 0){
setpoint_target = balance_conf.tiltback_constant;
} else {
setpoint_target = -balance_conf.tiltback_constant;
}
setpointAdjustmentType = TILTBACK;
}else{
setpointAdjustmentType = TILTBACK;
setpoint_target = 0;
}
// Adjust setpoint
if(setpoint_target_interpolated != setpoint_target){
// If we are less than one step size away, go all the way
if(fabsf(setpoint_target - setpoint_target_interpolated) < get_setpoint_adjustment_step_size()){
setpoint_target_interpolated = setpoint_target;
}else if (setpoint_target - setpoint_target_interpolated > 0){
setpoint_target_interpolated += get_setpoint_adjustment_step_size();
}else{
setpoint_target_interpolated -= get_setpoint_adjustment_step_size();
}
}
// Calculate setpoint and interpolation
calculate_setpoint_target();
calculate_setpoint_interpolated();
// Apply setpoint filtering
if(setpointAdjustmentType == CENTERING){
@ -388,10 +448,14 @@ static THD_FUNCTION(balance_thread, arg) {
}else{
setpoint = (setpoint * (1-balance_conf.setpoint_pitch_filter)) + (pitch_angle * balance_conf.setpoint_pitch_filter);
setpoint = (setpoint * (1-balance_conf.setpoint_target_filter)) + (setpoint_target_interpolated * balance_conf.setpoint_target_filter);
if(setpoint > balance_conf.setpoint_clamp){
setpoint = balance_conf.setpoint_clamp;
}else if (setpoint < -balance_conf.setpoint_clamp){
setpoint = -balance_conf.setpoint_clamp;
}
// Clamp setpoint
if(setpointAdjustmentType != CENTERING){
if(setpoint - setpoint_target_interpolated > balance_conf.setpoint_filter_clamp){
setpoint = setpoint_target_interpolated + balance_conf.setpoint_filter_clamp;
}else if (setpoint - setpoint_target_interpolated < -balance_conf.setpoint_filter_clamp){
setpoint = setpoint_target_interpolated - balance_conf.setpoint_filter_clamp;
}
}
@ -403,6 +467,12 @@ static THD_FUNCTION(balance_thread, arg) {
integral = integral + proportional;
derivative = proportional - last_proportional;
// Apply D term only filter
if(balance_conf.kd_pt1_frequency > 0){
d_pt1_state = d_pt1_state + d_pt1_k * (derivative - d_pt1_state);
derivative = d_pt1_state;
}
pid_value = (balance_conf.kp * proportional) + (balance_conf.ki * integral) + (balance_conf.kd * derivative);
last_proportional = proportional;
@ -414,6 +484,7 @@ static THD_FUNCTION(balance_thread, arg) {
pid_value -= balance_conf.current_boost;
}
if(balance_conf.multi_esc){
// Calculate setpoint
if(abs_duty_cycle < .02){
@ -442,26 +513,24 @@ static THD_FUNCTION(balance_thread, arg) {
// Output to motor
set_current(pid_value, yaw_pid_value);
break;
case (FAULT):
case (FAULT_ANGLE_PITCH):
case (FAULT_ANGLE_ROLL):
case (FAULT_SWITCH_HALF):
case (FAULT_SWITCH_FULL):
case (FAULT_STARTUP):
// Check for valid startup position and switch state
if(fabsf(pitch_angle) < balance_conf.startup_pitch_tolerance && fabsf(roll_angle) < balance_conf.startup_roll_tolerance && switch_state == ON){
// Clear accumulated values.
integral = 0;
last_proportional = 0;
yaw_integral = 0;
yaw_last_proportional = 0;
// Set values for startup
setpoint = pitch_angle;
setpoint_target = 0;
setpoint_target_interpolated = pitch_angle;
setpointAdjustmentType = CENTERING;
state = RUNNING;
reset_vars();
break;
}
// Disable output
brake();
break;
case (DEAD):
case (FAULT_DUTY):
// We need another fault to clear duty fault.
// Otherwise duty fault will clear itself as soon as motor pauses, then motor will spool up again.
// Rendering this fault useless.
check_faults(true);
// Disable output
brake();
break;

126
applications/app_ppm.c
View File

@ -136,6 +136,7 @@ static THD_FUNCTION(ppm_thread, arg) {
const float rpm_now = mc_interface_get_rpm();
float servo_val = servodec_get_servo(0);
float servo_ms = utils_map(servo_val, -1.0, 1.0, config.pulse_start, config.pulse_end);
static bool servoError = false;
switch (config.ctrl_type) {
case PPM_CTRL_TYPE_CURRENT_NOREV:
@ -165,10 +166,23 @@ static THD_FUNCTION(ppm_thread, arg) {
continue;
}
if (timeout_has_timeout() || servodec_get_time_since_update() > timeout_get_timeout_msec() ||
mc_interface_get_fault() != FAULT_CODE_NONE) {
if (timeout_has_timeout() || servodec_get_time_since_update() > timeout_get_timeout_msec()) {
pulses_without_power = 0;
servoError = true;
float timeoutCurrent = timeout_get_brake_current();
mc_interface_set_brake_current(timeoutCurrent);
if(config.multi_esc){
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
comm_can_set_current_brake(msg->id, timeoutCurrent);
}
}
}
continue;
} else if (mc_interface_get_fault() != FAULT_CODE_NONE){
pulses_without_power = 0;
}
// Apply deadband
@ -202,9 +216,10 @@ static THD_FUNCTION(ppm_thread, arg) {
bool send_current = false;
bool send_duty = false;
static bool force_brake = true;
static int8_t did_idle_once = 0;
static int8_t did_idle_once = 0; //0 = haven't idle ;1 = idle once ; 2 = idle twice
float rpm_local = mc_interface_get_rpm();
float rpm_lowest = rpm_local;
float rpm_highest = rpm_local;
switch (config.ctrl_type) {
case PPM_CTRL_TYPE_CURRENT_BRAKE_REV_HYST:
@ -241,7 +256,7 @@ static THD_FUNCTION(ppm_thread, arg) {
// too fast
if (force_brake){
current_mode_brake = true;
} else{
} else {
// not too fast backwards
if (rpm_local > -config.max_erpm_for_dir) { // for 2500 it's -2500
// first time that we brake and we are not too fast
@ -278,9 +293,10 @@ static THD_FUNCTION(ppm_thread, arg) {
case PPM_CTRL_TYPE_CURRENT:
case PPM_CTRL_TYPE_CURRENT_NOREV:
current_mode = true;
if ((servo_val >= 0.0 && rpm_now > 0.0) || (servo_val < 0.0 && rpm_now < 0.0)) {
if ((servo_val >= 0.0 && rpm_now >= 0.0) || (servo_val < 0.0 && rpm_now <= 0.0)) { //Accelerate
current = servo_val * mcconf->lo_current_motor_max_now;
} else {
} else { //Brake
current = servo_val * fabsf(mcconf->lo_current_motor_min_now);
}
@ -294,9 +310,9 @@ static THD_FUNCTION(ppm_thread, arg) {
current_mode = true;
current_mode_brake = servo_val < 0.0;
if (servo_val >= 0.0 && rpm_now > 0.0) {
if (servo_val >= 0.0 && rpm_now > 0.0) { //Positive input AND going forward = accelerating
current = servo_val * mcconf->lo_current_motor_max_now;
} else {
} else { //Negative input OR going backwards = brake (no reverse allowed in those control types)
current = fabsf(servo_val * mcconf->lo_current_motor_min_now);
}
@ -332,21 +348,29 @@ static THD_FUNCTION(ppm_thread, arg) {
default:
continue;
}
//Safe start : If startup, servo timeout or fault, check if idle has been verified for some pulses before driving the motor
if (pulses_without_power < MIN_PULSES_WITHOUT_POWER && config.safe_start) {
static int pulses_without_power_before = 0;
if (pulses_without_power == pulses_without_power_before) {
pulses_without_power = 0;
}
pulses_without_power_before = pulses_without_power;
mc_interface_set_brake_current(timeout_get_brake_current());
continue;
if (servoError){
continue;
}
if (current_mode) {
current = 0.0;
}
} else {
servoError = false;
}
const float duty_now = mc_interface_get_duty_cycle_now();
float current_highest_abs = fabsf(mc_interface_get_tot_current_directional_filtered());
float duty_highest_abs = fabsf(duty_now);
//If multiple VESCs over CAN, store highest/lowest running values of the whole setup
if (config.multi_esc) {
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
@ -356,6 +380,10 @@ static THD_FUNCTION(ppm_thread, arg) {
rpm_lowest = msg->rpm;
}
if (fabsf(msg->rpm) > fabsf(rpm_highest)) {
rpm_highest = msg->rpm;
}
if (fabsf(msg->current) > current_highest_abs) {
current_highest_abs = fabsf(msg->current);
}
@ -403,7 +431,7 @@ static THD_FUNCTION(ppm_thread, arg) {
was_duty_control = false;
}
}
//CTRL TYPE PID_NOREV & DUTY_NOREV : Acting as master, send motor control command to all slave VESCs detected on the CANbus
if ((send_current || send_duty) && config.multi_esc) {
float current_filtered = mc_interface_get_tot_current_directional_filtered();
float duty = mc_interface_get_duty_cycle_now();
@ -420,57 +448,67 @@ static THD_FUNCTION(ppm_thread, arg) {
}
}
}
//CTRL TYPE CURRENT
if (current_mode) {
if (current_mode_brake) {
mc_interface_set_brake_current(current);
if (current_mode_brake) { //If braking applied
mc_interface_set_brake_current(fabsf(current));
// Send brake command to all ESCs seen recently on the CAN bus
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
comm_can_set_current_brake(msg->id, current);
}
}
} else {
float current_out = current;
bool is_reverse = false;
if (current_out < 0.0) {
is_reverse = true;
current_out = -current_out;
current = -current;
rpm_local = -rpm_local;
rpm_lowest = -rpm_lowest;
}
// Traction control
if (config.multi_esc) {
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
if (config.tc) {
comm_can_set_current_brake_rel(msg->id, fabsf(servo_val));
}
}
}
} else {
float current_out = current;
bool is_reverse = false;
static bool autoTCdisengaged = false;
if (current_out < 0.0) { // Not braking AND negative current = reverse engaged
is_reverse = true;
current_out = -current_out;
current = -current;
rpm_local = -rpm_local;
rpm_lowest = -rpm_lowest;
rpm_highest = -rpm_highest;
servo_val = -servo_val;
}
// Send acceleration command to all ESCs seen recently on the CAN bus
if (config.multi_esc) {
if (config.tc) {
if(mc_interface_get_fault() != FAULT_CODE_NONE) {
autoTCdisengaged = true;
} else if (autoTCdisengaged && rpm_highest < rpm_local + config.tc_max_diff && rpm_lowest > rpm_local - config.tc_max_diff) { //No Fault anymore and no traction control action needed = re-enable traction control
autoTCdisengaged = false;
}
}
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
//Traction Control - Applied to slaves except if a fault has occured on the local VESC (undriven wheel may generate fake RPM)
if (config.tc && !autoTCdisengaged) {
float rpm_tmp = msg->rpm;
if (is_reverse) {
rpm_tmp = -rpm_tmp;
}
float diff = rpm_tmp - rpm_lowest;
current_out = utils_map(diff, 0.0, config.tc_max_diff, current, 0.0);
if (current_out < mcconf->cc_min_current) {
current_out = 0.0;
}
servo_val = utils_map(diff, 0.0, config.tc_max_diff, servo_val, 0.0);
}
//Send motor drive command to slaves
if (is_reverse) {
comm_can_set_current(msg->id, -current_out);
comm_can_set_current_rel(msg->id, -servo_val);
} else {
comm_can_set_current(msg->id, current_out);
comm_can_set_current_rel(msg->id, servo_val);
}
}
}
//Traction Control - Applying locally
if (config.tc) {
float diff = rpm_local - rpm_lowest;
current_out = utils_map(diff, 0.0, config.tc_max_diff, current, 0.0);
@ -479,7 +517,7 @@ static THD_FUNCTION(ppm_thread, arg) {
}
}
}
//Drive local motor
if (is_reverse) {
mc_interface_set_current(-current_out);
} else {

25
blackmagic/bm_if.c
View File

@ -83,7 +83,7 @@ static int idcode_to_device(uint32_t idcode) {
int ret = -1;
switch (idcode) {
case 0x413: ret = 1; break;
case 0x413: ret = 1; break; // STM32F40x
case 0x001D: /* nRF51822 (rev 1) QFAA CA/C0 */
case 0x001E: /* nRF51422 (rev 1) QFAA CA */
@ -137,6 +137,11 @@ static int idcode_to_device(uint32_t idcode) {
case 0x0150: /* nRF52840 QIAA C0 */
case 0x015B: /* nRF52840 ?? */
ret = 8; break;
case 0x422: ret = 9; break; // STM32F30x
case 0x415: ret = 10; break; // STM32L47x
default: ret = -2; break;
}
@ -261,6 +266,18 @@ static void terminal_target_cmd(int argc, const char **argv) {
}
}
static void terminal_reset(int argc, const char **argv) {
(void)argc;
(void)argv;
if (cur_target) {
target_reset(cur_target);
commands_printf("Done.\n");
} else {
commands_printf("No target attached\n");
}
}
static void terminal_detach(int argc, const char **argv) {
(void)argc;
(void)argv;
@ -330,6 +347,12 @@ void bm_init(void) {
"[...]",
terminal_target_cmd);
terminal_register_command_callback(
"bm_reset",
"BlackMagic: Reset target",
0,
terminal_reset);
terminal_register_command_callback(
"bm_detach",
"BlackMagic: Detach target",

29
blackmagic/target/nrf51.c
View File

@ -31,6 +31,7 @@ static int nrf51_flash_write(struct target_flash *f,
target_addr dest, const void *src, size_t len);
static bool nrf51_cmd_erase_all(target *t);
static bool nrf51_cmd_erase_uicr(target *t);
static bool nrf51_cmd_read_hwid(target *t);
static bool nrf51_cmd_read_fwid(target *t);
static bool nrf51_cmd_read_deviceid(target *t);
@ -40,6 +41,7 @@ static bool nrf51_cmd_read(target *t, int argc, const char *argv[]);
const struct command_s nrf51_cmd_list[] = {
{"erase_mass", (cmd_handler)nrf51_cmd_erase_all, "Erase entire flash memory"},
{"erase_uicr", (cmd_handler)nrf51_cmd_erase_uicr, "Erase UICR registers"},
{"read", (cmd_handler)nrf51_cmd_read, "Read device parameters"},
{NULL, NULL, NULL}
};
@ -171,7 +173,7 @@ bool nrf51_probe(target *t)
t->driver = "Nordic nRF52";
target_add_ram(t, 0x20000000, 64*1024);
nrf51_add_flash(t, 0x00000000, 512*1024, NRF52_PAGE_SIZE);
nrf51_add_flash(t, NRF51_UICR, 0x100, 0x100);
nrf51_add_flash(t, NRF51_UICR, 1024, 1024);
target_add_commands(t, nrf51_cmd_list, "nRF52");
return true;
case 0x00EB: /* nRF52840 Preview QIAA AA0 */
@ -180,7 +182,7 @@ bool nrf51_probe(target *t)
t->driver = "Nordic nRF52";
target_add_ram(t, 0x20000000, 256*1024);
nrf51_add_flash(t, 0x00000000, 1024*1024, NRF52_PAGE_SIZE);
nrf51_add_flash(t, NRF51_UICR, 0x100, 0x100);
nrf51_add_flash(t, NRF51_UICR, 1024, 1024);
target_add_commands(t, nrf51_cmd_list, "nRF52");
return true;
}
@ -277,6 +279,29 @@ static bool nrf51_cmd_erase_all(target *t)
return true;
}
static bool nrf51_cmd_erase_uicr(target *t)
{
tc_printf(t, "erase..\n");
/* Enable erase */
target_mem_write32(t, NRF51_NVMC_CONFIG, NRF51_NVMC_CONFIG_EEN);
/* Poll for NVMC_READY */
while (target_mem_read32(t, NRF51_NVMC_READY) == 0)
if(target_check_error(t))
return false;
/* Erase UICR */
target_mem_write32(t, NRF51_NVMC_ERASEUICR, 1);
/* Poll for NVMC_READY */
while (target_mem_read32(t, NRF51_NVMC_READY) == 0)
if(target_check_error(t))
return false;
return true;
}
static bool nrf51_cmd_read_hwid(target *t)
{
uint32_t hwid = target_mem_read32(t, NRF51_FICR_CONFIGID) & 0xFFFF;

BIN
build_all/100_250/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/100_250/VESC_default_no_hw_limits.bin
View File

Binary file not shown.

BIN
build_all/410_o_411_o_412/VESC_0005ohm.bin
View File

Binary file not shown.

BIN
build_all/410_o_411_o_412/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/410_o_411_o_412/VESC_default_no_hw_limits.bin
View File

Binary file not shown.

BIN
build_all/46_o_47/VESC_0005ohm.bin
View File

Binary file not shown.

BIN
build_all/46_o_47/VESC_33k.bin
View File

Binary file not shown.

BIN
build_all/46_o_47/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/48/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/60/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/60/VESC_default_no_hw_limits.bin
View File

Binary file not shown.

BIN
build_all/60_MK3/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/60_MK3/VESC_default_no_hw_limits.bin
View File

Binary file not shown.

BIN
build_all/60_MK4/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/60_MK4/VESC_default_no_hw_limits.bin
View File

Binary file not shown.

BIN
build_all/75_300/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/75_300/VESC_default_no_hw_limits.bin
View File

Binary file not shown.

BIN
build_all/75_300_R2/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/75_300_R2/VESC_default_no_hw_limits.bin
View File

Binary file not shown.

BIN
build_all/75_300_R3/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/75_300_R3/VESC_default_no_hw_limits.bin
View File

Binary file not shown.

BIN
build_all/A200S_V21/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/A200S_V22/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/AXIOM/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/DAS_RS/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/HD/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/HD/VESC_default_no_hw_limits.bin
View File

Binary file not shown.

BIN
build_all/HD75/VESC_default.bin Executable file
View File

Binary file not shown.

BIN
build_all/HD75/VESC_default_no_hw_limits.bin Executable file
View File

Binary file not shown.

BIN
build_all/LUNA_BBSHD/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/STORMCORE_100D/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/STORMCORE_100D/VESC_default_no_hw_limits.bin
View File

Binary file not shown.

BIN
build_all/STORMCORE_100S/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/STORMCORE_100S/VESC_default_no_hw_limits.bin
View File

Binary file not shown.

BIN
build_all/STORMCORE_60D/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/STORMCORE_60D/VESC_default_no_hw_limits.bin
View File

Binary file not shown.

BIN
build_all/UAVC_OMEGA/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/UNITY/VESC_default.bin
View File

Binary file not shown.

BIN
build_all/UNITY/VESC_default_no_hw_limits.bin
View File

Binary file not shown.

24
build_all/rebuild_all
View File

@ -289,6 +289,30 @@ cd $FWPATH
make clean
cd $DIR
#################### HW HD75 ########################
COPYDIR=HD75
rm -f $COPYDIR/*
# default
cd $FWPATH
touch conf_general.h
make -j8 build_args='-DHW_SOURCE=\"hw_hd75.c\" -DHW_HEADER=\"hw_hd75.h\"' USE_VERBOSE_COMPILE=no
cd $DIR
cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_default.bin
# default with HW limits disables
cd $FWPATH
touch conf_general.h
make -j8 build_args='-DDISABLE_HW_LIMITS -DHW_SOURCE=\"hw_hd75.c\" -DHW_HEADER=\"hw_hd75.h\"' USE_VERBOSE_COMPILE=no
cd $DIR
cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_default_no_hw_limits.bin
# Clean
cd $FWPATH
make clean
cd $DIR
#################### HW A200S 2.1 ########################
COPYDIR=A200S_V21

20
conf_general.h
View File

@ -24,7 +24,7 @@
#define FW_VERSION_MAJOR 5
#define FW_VERSION_MINOR 02
// Set to 0 for building a release and iterate during beta test builds
#define FW_TEST_VERSION_NUMBER 3
#define FW_TEST_VERSION_NUMBER 7
#include "datatypes.h"
@ -122,8 +122,11 @@
//#define HW_SOURCE "hw_binar_v1.c"
//#define HW_HEADER "hw_binar_v1.h"
//#define HW_SOURCE "hw_hd.c"
//#define HW_HEADER "hw_hd.h"
#define HW_SOURCE "hw_hd.c"
#define HW_HEADER "hw_hd.h"
//#define HW_SOURCE "hw_hd75.c"
//#define HW_HEADER "hw_hd75.h"
//#define HW_SOURCE "hw_a200s_v2.c"
//#define HW_HEADER "hw_a200s_v2.h"
@ -137,9 +140,12 @@
//#define HW_SOURCE "hw_unity.c"
//#define HW_HEADER "hw_unity.h"
#define HW_DUAL_CONFIG_PARALLEL
#define HW_SOURCE "hw_stormcore_100d.c"
#define HW_HEADER "hw_stormcore_100d.h"
//#define HW_SOURCE "hw_uxv_sr.c"
//#define HW_HEADER "hw_uxv_sr.h"
//#define HW_DUAL_CONFIG_PARALLEL
//#define HW_SOURCE "hw_stormcore_100d.c"
//#define HW_HEADER "hw_stormcore_100d.h"
//#define HW_SOURCE "hw_stormcore_60d.c"
//#define HW_HEADER "hw_stormcore_60d.h"
@ -185,7 +191,7 @@
*/
//#define APP_CUSTOM_TO_USE "app_custom_template.c"
//#define APP_CUSTOM_TO_USE "app_motor_heater.c"
//#include "app_erockit_conf.h"
//#include "app_erockit_conf_v2.h"
#include "hw.h"
#include "mcconf_default.h"

90
confgenerator.c
View File

@ -72,6 +72,7 @@ int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *
buffer_append_float32_auto(buffer, conf->foc_pll_kp, &ind);
buffer_append_float32_auto(buffer, conf->foc_pll_ki, &ind);
buffer_append_float32_auto(buffer, conf->foc_motor_l, &ind);
buffer_append_float32_auto(buffer, conf->foc_motor_ld_lq_diff, &ind);
buffer_append_float32_auto(buffer, conf->foc_motor_r, &ind);
buffer_append_float32_auto(buffer, conf->foc_motor_flux_linkage, &ind);
buffer_append_float32_auto(buffer, conf->foc_observer_gain, &ind);
@ -240,16 +241,24 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration
buffer_append_float32_auto(buffer, conf->app_balance_conf.ki, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.kd, &ind);
buffer_append_uint16(buffer, conf->app_balance_conf.hertz, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.pitch_fault, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.roll_fault, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.adc1, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.adc2, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.overspeed_duty, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_duty, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.fault_pitch, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.fault_roll, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.fault_duty, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.fault_adc1, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.fault_adc2, &ind);
buffer_append_uint16(buffer, conf->app_balance_conf.fault_delay_pitch, &ind);
buffer_append_uint16(buffer, conf->app_balance_conf.fault_delay_roll, &ind);
buffer_append_uint16(buffer, conf->app_balance_conf.fault_delay_duty, &ind);
buffer_append_uint16(buffer, conf->app_balance_conf.fault_delay_switch_half, &ind);
buffer_append_uint16(buffer, conf->app_balance_conf.fault_delay_switch_full, &ind);
buffer_append_uint16(buffer, conf->app_balance_conf.fault_adc_half_erpm, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_angle, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_speed, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_duty, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_high_voltage, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_low_voltage, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_constant, &ind);
buffer_append_uint16(buffer, conf->app_balance_conf.tiltback_constant_erpm, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.startup_pitch_tolerance, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.startup_roll_tolerance, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.startup_speed, &ind);
@ -260,16 +269,13 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration
buffer_append_float32_auto(buffer, conf->app_balance_conf.yaw_ki, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.yaw_kd, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.roll_steer_kp, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.brake_current, &ind);
buffer_append_uint16(buffer, conf->app_balance_conf.overspeed_delay, &ind);
buffer_append_uint16(buffer, conf->app_balance_conf.fault_delay, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_constant, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.roll_steer_erpm_kp, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.brake_current, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.yaw_current_clamp, &ind);
buffer_append_uint16(buffer, conf->app_balance_conf.adc_half_fault_erpm, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.setpoint_pitch_filter, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.setpoint_target_filter, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.setpoint_clamp, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.setpoint_filter_clamp, &ind);
buffer_append_uint16(buffer, conf->app_balance_conf.kd_pt1_frequency, &ind);
buffer[ind++] = conf->imu_conf.type;
buffer[ind++] = conf->imu_conf.mode;
buffer_append_uint16(buffer, conf->imu_conf.sample_rate_hz, &ind);
@ -365,6 +371,7 @@ bool confgenerator_deserialize_mcconf(const uint8_t *buffer, mc_configuration *c
conf->foc_pll_kp = buffer_get_float32_auto(buffer, &ind);
conf->foc_pll_ki = buffer_get_float32_auto(buffer, &ind);
conf->foc_motor_l = buffer_get_float32_auto(buffer, &ind);
conf->foc_motor_ld_lq_diff = buffer_get_float32_auto(buffer, &ind);
conf->foc_motor_r = buffer_get_float32_auto(buffer, &ind);
conf->foc_motor_flux_linkage = buffer_get_float32_auto(buffer, &ind);
conf->foc_observer_gain = buffer_get_float32_auto(buffer, &ind);
@ -536,16 +543,24 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration
conf->app_balance_conf.ki = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.kd = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.hertz = buffer_get_uint16(buffer, &ind);
conf->app_balance_conf.pitch_fault = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.roll_fault = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.adc1 = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.adc2 = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.overspeed_duty = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.tiltback_duty = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.fault_pitch = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.fault_roll = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.fault_duty = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.fault_adc1 = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.fault_adc2 = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.fault_delay_pitch = buffer_get_uint16(buffer, &ind);
conf->app_balance_conf.fault_delay_roll = buffer_get_uint16(buffer, &ind);
conf->app_balance_conf.fault_delay_duty = buffer_get_uint16(buffer, &ind);
conf->app_balance_conf.fault_delay_switch_half = buffer_get_uint16(buffer, &ind);
conf->app_balance_conf.fault_delay_switch_full = buffer_get_uint16(buffer, &ind);
conf->app_balance_conf.fault_adc_half_erpm = buffer_get_uint16(buffer, &ind);
conf->app_balance_conf.tiltback_angle = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.tiltback_speed = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.tiltback_duty = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.tiltback_high_voltage = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.tiltback_low_voltage = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.tiltback_constant = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.tiltback_constant_erpm = buffer_get_uint16(buffer, &ind);
conf->app_balance_conf.startup_pitch_tolerance = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.startup_roll_tolerance = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.startup_speed = buffer_get_float32_auto(buffer, &ind);
@ -556,16 +571,13 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration
conf->app_balance_conf.yaw_ki = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.yaw_kd = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.roll_steer_kp = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.brake_current = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.overspeed_delay = buffer_get_uint16(buffer, &ind);
conf->app_balance_conf.fault_delay = buffer_get_uint16(buffer, &ind);
conf->app_balance_conf.tiltback_constant = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.roll_steer_erpm_kp = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.brake_current = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.yaw_current_clamp = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.adc_half_fault_erpm = buffer_get_uint16(buffer, &ind);
conf->app_balance_conf.setpoint_pitch_filter = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.setpoint_target_filter = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.setpoint_clamp = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.setpoint_filter_clamp = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.kd_pt1_frequency = buffer_get_uint16(buffer, &ind);
conf->imu_conf.type = buffer[ind++];
conf->imu_conf.mode = buffer[ind++];
conf->imu_conf.sample_rate_hz = buffer_get_uint16(buffer, &ind);
@ -654,6 +666,7 @@ void confgenerator_set_defaults_mcconf(mc_configuration *conf) {
conf->foc_pll_kp = MCCONF_FOC_PLL_KP;
conf->foc_pll_ki = MCCONF_FOC_PLL_KI;
conf->foc_motor_l = MCCONF_FOC_MOTOR_L;
conf->foc_motor_ld_lq_diff = MCCONF_FOC_MOTOR_LD_LQ_DIFF;
conf->foc_motor_r = MCCONF_FOC_MOTOR_R;
conf->foc_motor_flux_linkage = MCCONF_FOC_MOTOR_FLUX_LINKAGE;
conf->foc_observer_gain = MCCONF_FOC_OBSERVER_GAIN;
@ -816,16 +829,24 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) {
conf->app_balance_conf.ki = APPCONF_BALANCE_KI;
conf->app_balance_conf.kd = APPCONF_BALANCE_KD;
conf->app_balance_conf.hertz = APPCONF_BALANCE_HERTZ;
conf->app_balance_conf.pitch_fault = APPCONF_BALANCE_PITCH_FAULT;
conf->app_balance_conf.roll_fault = APPCONF_BALANCE_ROLL_FAULT;
conf->app_balance_conf.adc1 = APPCONF_BALANCE_ADC1;
conf->app_balance_conf.adc2 = APPCONF_BALANCE_ADC2;
conf->app_balance_conf.overspeed_duty = APPCONF_BALANCE_OVERSPEED_DUTY;
conf->app_balance_conf.tiltback_duty = APPCONF_BALANCE_TILTBACK_DUTY;
conf->app_balance_conf.fault_pitch = APPCONF_BALANCE_FAULT_PITCH;
conf->app_balance_conf.fault_roll = APPCONF_BALANCE_FAULT_ROLL;
conf->app_balance_conf.fault_duty = APPCONF_BALANCE_FAULT_DUTY;
conf->app_balance_conf.fault_adc1 = APPCONF_BALANCE_FAULT_ADC1;
conf->app_balance_conf.fault_adc2 = APPCONF_BALANCE_FAULT_ADC2;
conf->app_balance_conf.fault_delay_pitch = APPCONF_BALANCE_FAULT_DELAY_PITCH;
conf->app_balance_conf.fault_delay_roll = APPCONF_BALANCE_FAULT_DELAY_ROLL;
conf->app_balance_conf.fault_delay_duty = APPCONF_BALANCE_FAULT_DELAY_DUTY;
conf->app_balance_conf.fault_delay_switch_half = APPCONF_BALANCE_FAULT_DELAY_SWITCH_HALF;
conf->app_balance_conf.fault_delay_switch_full = APPCONF_BALANCE_FAULT_DELAY_SWITCH_FULL;
conf->app_balance_conf.fault_adc_half_erpm = APPCONF_BALANCE_FAULT_ADC_HALF_ERPM;
conf->app_balance_conf.tiltback_angle = APPCONF_BALANCE_TILTBACK_ANGLE;
conf->app_balance_conf.tiltback_speed = APPCONF_BALANCE_TILTBACK_SPEED;
conf->app_balance_conf.tiltback_duty = APPCONF_BALANCE_TILTBACK_DUTY;
conf->app_balance_conf.tiltback_high_voltage = APPCONF_BALANCE_TILTBACK_HIGH_V;
conf->app_balance_conf.tiltback_low_voltage = APPCONF_BALANCE_TILTBACK_LOW_V;
conf->app_balance_conf.tiltback_constant = APPCONF_BALANCE_TILTBACK_CONSTANT;
conf->app_balance_conf.tiltback_constant_erpm = APPCONF_BALANCE_TILTBACK_CONSTANT_ERPM;
conf->app_balance_conf.startup_pitch_tolerance = APPCONF_BALANCE_STARTUP_PITCH_TOLERANCE;
conf->app_balance_conf.startup_roll_tolerance = APPCONF_BALANCE_STARTUP_ROLL_TOLERANCE;
conf->app_balance_conf.startup_speed = APPCONF_BALANCE_STARTUP_SPEED;
@ -836,16 +857,13 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) {
conf->app_balance_conf.yaw_ki = APPCONF_BALANCE_YAW_KI;
conf->app_balance_conf.yaw_kd = APPCONF_BALANCE_YAW_KD;
conf->app_balance_conf.roll_steer_kp = APPCONF_BALANCE_ROLL_STEER_KP;
conf->app_balance_conf.brake_current = APPCONF_BALANCE_BRAKE_CURRENT;
conf->app_balance_conf.overspeed_delay = APPCONF_BALANCE_OVERSPEED_DELAY;
conf->app_balance_conf.fault_delay = APPCONF_BALANCE_FAULT_DELAY;
conf->app_balance_conf.tiltback_constant = APPCONF_BALANCE_TILTBACK_CONSTANT;
conf->app_balance_conf.roll_steer_erpm_kp = APPCONF_BALANCE_ROLL_STEER_ERPM_KP;
conf->app_balance_conf.brake_current = APPCONF_BALANCE_BRAKE_CURRENT;
conf->app_balance_conf.yaw_current_clamp = APPCONF_BALANCE_YAW_CURRENT_CLAMP;
conf->app_balance_conf.adc_half_fault_erpm = APPCONF_BALANCE_ADC_HALF_FAULT_ERPM;
conf->app_balance_conf.setpoint_pitch_filter = APPCONF_BALANCE_SETPOINT_PITCH_FILTER;
conf->app_balance_conf.setpoint_target_filter = APPCONF_BALANCE_SETPOINT_TARGET_FILTER;
conf->app_balance_conf.setpoint_clamp = APPCONF_BALANCE_SETPOINT_CLAMP;
conf->app_balance_conf.setpoint_filter_clamp = APPCONF_BALANCE_SETPOINT_FILTER_CLAMP;
conf->app_balance_conf.kd_pt1_frequency = APPCONF_BALANCE_KD_PT1_FREQUENCY;
conf->imu_conf.type = APPCONF_IMU_TYPE;
conf->imu_conf.mode = APPCONF_IMU_AHRS_MODE;
conf->imu_conf.sample_rate_hz = APPCONF_IMU_SAMPLE_RATE_HZ;

4
confgenerator.h
View File

@ -8,8 +8,8 @@
#include <stdbool.h>
// Constants
#define MCCONF_SIGNATURE 1775793947
#define APPCONF_SIGNATURE 2662993821
#define MCCONF_SIGNATURE 1358025204
#define APPCONF_SIGNATURE 664237692
// Functions
int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf);

30
datatypes.h
View File

@ -277,6 +277,7 @@ typedef struct {
float foc_encoder_cos_gain;
float foc_encoder_sincos_filter_constant;
float foc_motor_l;
float foc_motor_ld_lq_diff;
float foc_motor_r;
float foc_motor_flux_linkage;
float foc_observer_gain;
@ -546,16 +547,24 @@ typedef struct {
float ki;
float kd;
uint16_t hertz;
float pitch_fault;
float roll_fault;
float adc1;
float adc2;
float overspeed_duty;
float tiltback_duty;
float fault_pitch;
float fault_roll;
float fault_duty;
float fault_adc1;
float fault_adc2;
uint16_t fault_delay_pitch;
uint16_t fault_delay_roll;
uint16_t fault_delay_duty;
uint16_t fault_delay_switch_half;
uint16_t fault_delay_switch_full;
uint16_t fault_adc_half_erpm;
float tiltback_angle;
float tiltback_speed;
float tiltback_duty;
float tiltback_high_voltage;
float tiltback_low_voltage;
float tiltback_constant;
uint16_t tiltback_constant_erpm;
float startup_pitch_tolerance;
float startup_roll_tolerance;
float startup_speed;
@ -566,16 +575,13 @@ typedef struct {
float yaw_ki;
float yaw_kd;
float roll_steer_kp;
float brake_current;
uint16_t overspeed_delay;
uint16_t fault_delay;
float tiltback_constant;
float roll_steer_erpm_kp;
float brake_current;
float yaw_current_clamp;
uint16_t adc_half_fault_erpm;
float setpoint_pitch_filter;
float setpoint_target_filter;
float setpoint_clamp;
float setpoint_filter_clamp;
uint16_t kd_pt1_frequency;
} balance_config;
// CAN status modes

2
hwconf/drv8301.c
View File

@ -17,7 +17,7 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "hw.h"
#include "conf_general.h"
#ifdef HW_HAS_DRV8301
#include "drv8301.h"

2
hwconf/drv8305.c
View File

@ -17,7 +17,7 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "hw.h"
#include "conf_general.h"
#ifdef HW_HAS_DRV8305
#include "drv8305.h"

2
hwconf/drv8320s.c
View File

@ -17,7 +17,7 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "hw.h"
#include "conf_general.h"
#ifdef HW_HAS_DRV8320S
#include "drv8320s.h"

2
hwconf/drv8323s.c
View File

@ -17,7 +17,7 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "hw.h"
#include "conf_general.h"
#ifdef HW_HAS_DRV8323S
#include "drv8323s.h"

54
hwconf/hw.c
View File

@ -19,8 +19,62 @@
#include "conf_general.h"
#include "utils.h"
#include <math.h>
#include HW_SOURCE
uint8_t hw_id_from_uuid(void) {
return utils_crc32c(STM32_UUID_8, 12) & 0x7F;
}
#if defined(HW_ID_PIN_GPIOS) && defined(HW_ID_PIN_PINS)
uint8_t hw_id_from_pins(void) {
stm32_gpio_t *hw_id_ports[]={HW_ID_PIN_GPIOS};
const uint16_t hw_id_pins[] = {HW_ID_PIN_PINS};
const uint16_t hw_id_pins_size = sizeof(hw_id_pins)/sizeof(uint16_t);
const uint16_t DELAY_MS = 5;
uint8_t trits[hw_id_pins_size];
uint8_t id = 1u; //Start at 1
for (uint8_t i=0; i<hw_id_pins_size; i++)
{
//Initialize pulldown
palSetPadMode(hw_id_ports[i], hw_id_pins[i], PAL_MODE_INPUT_PULLDOWN);
//Delay a little for the resistor to take affect
chThdSleepMilliseconds(DELAY_MS);
bool pin_set_pulldown = (palReadPad(hw_id_ports[i], hw_id_pins[i]));
//Initialize pullup
palSetPadMode(hw_id_ports[i], hw_id_pins[i], PAL_MODE_INPUT_PULLUP);
//Delay a little for the resistor to take affect
chThdSleepMilliseconds(DELAY_MS);
bool pin_set_pullup = (palReadPad(hw_id_ports[i], hw_id_pins[i]));
//Now determine the trit state
if (!pin_set_pulldown && !pin_set_pullup)
{
//Tied to GND
trits[i] = 1u;
}
else if (pin_set_pulldown && pin_set_pullup)
{
//Tied to VCC
trits[i] = 2u;
}
else if (!pin_set_pulldown && pin_set_pullup)
{
//Floating
trits[i] = 0u;
}
else
{
return hw_id_from_uuid();
//To satisfy compiler warning
trits[i] = 3u;
}
id += trits[i] * pow(3, i);
palSetPadMode(hw_id_ports[i], hw_id_pins[i], PAL_MODE_INPUT);
}
return id;
}
#endif //defined(HW_ID_PIN_GPIOS) && defined(HW_ID_PIN_PINS)

2
hwconf/hw.h
View File

@ -20,7 +20,6 @@
#ifndef HW_H_
#define HW_H_
#include "conf_general.h"
#include "stm32f4xx_conf.h"
#include HW_HEADER
@ -460,5 +459,6 @@ void hw_start_i2c(void);
void hw_stop_i2c(void);
void hw_try_restore_i2c(void);
uint8_t hw_id_from_uuid(void);
uint8_t hw_id_from_pins(void);
#endif /* HW_H_ */

10
hwconf/hw_das_rs.h
View File

@ -201,6 +201,16 @@
#define HW_SPI_PORT_MISO GPIOA
#define HW_SPI_PIN_MISO 6
//BMI160 SPI pins
#define BMI160_SPI_PORT_NSS GPIOC
#define BMI160_SPI_PIN_NSS 9
#define BMI160_SPI_PORT_SCK GPIOC
#define BMI160_SPI_PIN_SCK 10
#define BMI160_SPI_PORT_MOSI GPIOC
#define BMI160_SPI_PIN_MOSI 12
#define BMI160_SPI_PORT_MISO GPIOC
#define BMI160_SPI_PIN_MISO 11
// Measurement macros
#define ADC_V_L1 ADC_Value[ADC_IND_SENS1]
#define ADC_V_L2 ADC_Value[ADC_IND_SENS2]

6
hwconf/hw_hd.c
View File

@ -100,6 +100,12 @@ void hw_init_gpio(void) {
palSetPadMode(HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2, PAL_MODE_INPUT_PULLUP);
palSetPadMode(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3, PAL_MODE_INPUT_PULLUP);
// Phase filters
palSetPadMode(PHASE_FILTER_GPIO, PHASE_FILTER_PIN,
PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST);
PHASE_FILTER_OFF();
// Fault pin
palSetPadMode(GPIOB, 7, PAL_MODE_INPUT_PULLUP);

5
hwconf/hw_hd.h
View File

@ -50,6 +50,11 @@
#define HW_EARLY_INIT() palSetPadMode(HW_SHUTDOWN_GPIO, HW_SHUTDOWN_PIN, PAL_MODE_OUTPUT_PUSHPULL); \
HW_SHUTDOWN_HOLD_ON();
#define PHASE_FILTER_GPIO GPIOC
#define PHASE_FILTER_PIN 13
#define PHASE_FILTER_ON() palSetPad(PHASE_FILTER_GPIO, PHASE_FILTER_PIN)
#define PHASE_FILTER_OFF() palClearPad(PHASE_FILTER_GPIO, PHASE_FILTER_PIN)
/*
* ADC Vector
*

306
hwconf/hw_hd75.c Normal file
View File

@ -0,0 +1,306 @@
/*
Copyright 2019 Benjamin Vedder benjamin@vedder.se
This file is part of the VESC firmware.
The VESC firmware is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
The VESC firmware is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "hw.h"
#include "ch.h"
#include "hal.h"
#include "stm32f4xx_conf.h"
#include "utils.h"
#include "drv8323s.h"
#include "terminal.h"
#include "commands.h"
#include "mc_interface.h"
// Variables
static volatile bool i2c_running = false;
static mutex_t shutdown_mutex;
static float bt_diff = 0.0;
// I2C configuration
static const I2CConfig i2cfg = {
OPMODE_I2C,
100000,
STD_DUTY_CYCLE
};
// Private functions
static void terminal_shutdown_now(int argc, const char **argv);
static void terminal_button_test(int argc, const char **argv);
void hw_init_gpio(void) {
chMtxObjectInit(&shutdown_mutex);
// GPIO clock enable
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
// LEDs
palSetPadMode(GPIOB, 0,
PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(GPIOB, 1,
PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST);
// ENABLE_GATE
palSetPadMode(GPIOB, 5,
PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST);
// Disable DCCAL
palSetPadMode(GPIOD, 2,
PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST);
palClearPad(GPIOD, 2);
ENABLE_GATE();
// GPIOA Configuration: Channel 1 to 3 as alternate function push-pull
palSetPadMode(GPIOA, 8, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
PAL_STM32_OSPEED_HIGHEST |
PAL_STM32_PUDR_FLOATING);
palSetPadMode(GPIOA, 9, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
PAL_STM32_OSPEED_HIGHEST |
PAL_STM32_PUDR_FLOATING);
palSetPadMode(GPIOA, 10, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
PAL_STM32_OSPEED_HIGHEST |
PAL_STM32_PUDR_FLOATING);
palSetPadMode(GPIOB, 13, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
PAL_STM32_OSPEED_HIGHEST |
PAL_STM32_PUDR_FLOATING);
palSetPadMode(GPIOB, 14, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
PAL_STM32_OSPEED_HIGHEST |
PAL_STM32_PUDR_FLOATING);
palSetPadMode(GPIOB, 15, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
PAL_STM32_OSPEED_HIGHEST |
PAL_STM32_PUDR_FLOATING);
// Hall sensors
palSetPadMode(HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1, PAL_MODE_INPUT_PULLUP);
palSetPadMode(HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2, PAL_MODE_INPUT_PULLUP);
palSetPadMode(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3, PAL_MODE_INPUT_PULLUP);
// Phase filters
palSetPadMode(PHASE_FILTER_GPIO, PHASE_FILTER_PIN,
PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST);
PHASE_FILTER_OFF();
// Fault pin
palSetPadMode(GPIOB, 7, PAL_MODE_INPUT_PULLUP);
// ADC Pins
palSetPadMode(GPIOA, 0, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOA, 1, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOA, 2, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOA, 3, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOA, 5, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOA, 6, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOC, 0, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOC, 1, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOC, 2, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOC, 3, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOC, 4, PAL_MODE_INPUT_ANALOG);
drv8323s_init();
terminal_register_command_callback(
"shutdown",
"Shutdown VESC now.",
0,
terminal_shutdown_now);
terminal_register_command_callback(
"test_button",
"Try sampling the shutdown button",
0,
terminal_button_test);
}
void hw_setup_adc_channels(void) {
// ADC1 regular channels
ADC_RegularChannelConfig(ADC1, ADC_Channel_0, 1, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC1, ADC_Channel_10, 2, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC1, ADC_Channel_5, 3, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC1, ADC_Channel_14, 4, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC1, ADC_Channel_Vrefint, 5, ADC_SampleTime_15Cycles);
// ADC2 regular channels
ADC_RegularChannelConfig(ADC2, ADC_Channel_1, 1, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC2, ADC_Channel_11, 2, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC2, ADC_Channel_6, 3, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC2, ADC_Channel_15, 4, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC2, ADC_Channel_0, 5, ADC_SampleTime_15Cycles);
// ADC3 regular channels
ADC_RegularChannelConfig(ADC3, ADC_Channel_2, 1, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC3, ADC_Channel_12, 2, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC3, ADC_Channel_3, 3, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC3, ADC_Channel_13, 4, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC3, ADC_Channel_1, 5, ADC_SampleTime_15Cycles);
// Injected channels
ADC_InjectedChannelConfig(ADC1, ADC_Channel_10, 1, ADC_SampleTime_15Cycles);
ADC_InjectedChannelConfig(ADC2, ADC_Channel_11, 1, ADC_SampleTime_15Cycles);
ADC_InjectedChannelConfig(ADC3, ADC_Channel_12, 1, ADC_SampleTime_15Cycles);
ADC_InjectedChannelConfig(ADC1, ADC_Channel_10, 2, ADC_SampleTime_15Cycles);
ADC_InjectedChannelConfig(ADC2, ADC_Channel_11, 2, ADC_SampleTime_15Cycles);
ADC_InjectedChannelConfig(ADC3, ADC_Channel_12, 2, ADC_SampleTime_15Cycles);
ADC_InjectedChannelConfig(ADC1, ADC_Channel_10, 3, ADC_SampleTime_15Cycles);
ADC_InjectedChannelConfig(ADC2, ADC_Channel_11, 3, ADC_SampleTime_15Cycles);
ADC_InjectedChannelConfig(ADC3, ADC_Channel_12, 3, ADC_SampleTime_15Cycles);
}
void hw_start_i2c(void) {
i2cAcquireBus(&HW_I2C_DEV);
if (!i2c_running) {
palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN,
PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) |
PAL_STM32_OTYPE_OPENDRAIN |
PAL_STM32_OSPEED_MID1 |
PAL_STM32_PUDR_PULLUP);
palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) |
PAL_STM32_OTYPE_OPENDRAIN |
PAL_STM32_OSPEED_MID1 |
PAL_STM32_PUDR_PULLUP);
i2cStart(&HW_I2C_DEV, &i2cfg);
i2c_running = true;
}
i2cReleaseBus(&HW_I2C_DEV);
}
void hw_stop_i2c(void) {
i2cAcquireBus(&HW_I2C_DEV);
if (i2c_running) {
palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN, PAL_MODE_INPUT);
palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN, PAL_MODE_INPUT);
i2cStop(&HW_I2C_DEV);
i2c_running = false;
}
i2cReleaseBus(&HW_I2C_DEV);
}
/**
* Try to restore the i2c bus
*/
void hw_try_restore_i2c(void) {
if (i2c_running) {
i2cAcquireBus(&HW_I2C_DEV);
palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN,
PAL_STM32_OTYPE_OPENDRAIN |
PAL_STM32_OSPEED_MID1 |
PAL_STM32_PUDR_PULLUP);
palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
PAL_STM32_OTYPE_OPENDRAIN |
PAL_STM32_OSPEED_MID1 |
PAL_STM32_PUDR_PULLUP);
palSetPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
palSetPad(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN);
chThdSleep(1);
for(int i = 0;i < 16;i++) {
palClearPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
chThdSleep(1);
palSetPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
chThdSleep(1);
}
// Generate start then stop condition
palClearPad(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN);
chThdSleep(1);
palClearPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
chThdSleep(1);
palSetPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
chThdSleep(1);
palSetPad(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN);
palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN,
PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) |
PAL_STM32_OTYPE_OPENDRAIN |
PAL_STM32_OSPEED_MID1 |
PAL_STM32_PUDR_PULLUP);
palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) |
PAL_STM32_OTYPE_OPENDRAIN |
PAL_STM32_OSPEED_MID1 |
PAL_STM32_PUDR_PULLUP);
HW_I2C_DEV.state = I2C_STOP;
i2cStart(&HW_I2C_DEV, &i2cfg);
i2cReleaseBus(&HW_I2C_DEV);
}
}
bool hw_sample_shutdown_button(void) {
chMtxLock(&shutdown_mutex);
bt_diff = 0.0;
for (int i = 0;i < 3;i++) {
palSetPadMode(HW_SHUTDOWN_GPIO, HW_SHUTDOWN_PIN, PAL_MODE_INPUT_ANALOG);
chThdSleep(5);
float val1 = ADC_VOLTS(ADC_IND_SHUTDOWN);
chThdSleepMilliseconds(1);
float val2 = ADC_VOLTS(ADC_IND_SHUTDOWN);
palSetPadMode(HW_SHUTDOWN_GPIO, HW_SHUTDOWN_PIN, PAL_MODE_OUTPUT_PUSHPULL);
chThdSleepMilliseconds(1);
bt_diff += (val1 - val2);
}
chMtxUnlock(&shutdown_mutex);
return (bt_diff > 0.12);
}
static void terminal_shutdown_now(int argc, const char **argv) {
(void)argc;
(void)argv;
DISABLE_GATE();
HW_SHUTDOWN_HOLD_OFF();
}
static void terminal_button_test(int argc, const char **argv) {
(void)argc;
(void)argv;
for (int i = 0;i < 40;i++) {
commands_printf("BT: %d %.2f", HW_SAMPLE_SHUTDOWN(), (double)bt_diff);
chThdSleepMilliseconds(100);
}
}

296
hwconf/hw_hd75.h Normal file
View File

@ -0,0 +1,296 @@
/*
Copyright 2019 Benjamin Vedder benjamin@vedder.se
This file is part of the VESC firmware.
The VESC firmware is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
The VESC firmware is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef HW_HD_H_
#define HW_HD_H_
#include "drv8323s.h"
#define HW_NAME "HD75"
// HW properties
#define HW_HAS_DRV8323S
#define HW_HAS_3_SHUNTS
#define DRV8323S_CUSTOM_SETTINGS() drv8323s_set_current_amp_gain(CURRENT_AMP_GAIN); \
drv8323s_write_reg(3,0x3af); \
drv8323s_write_reg(4,0x7af);
// Macros
#define ENABLE_GATE() palSetPad(GPIOB, 5)
#define DISABLE_GATE() palClearPad(GPIOB, 5)
#define IS_DRV_FAULT() (!palReadPad(GPIOB, 7))
#define LED_GREEN_ON() palSetPad(GPIOB, 0)
#define LED_GREEN_OFF() palClearPad(GPIOB, 0)
#define LED_RED_ON() palSetPad(GPIOB, 1)
#define LED_RED_OFF() palClearPad(GPIOB, 1)
// Shutdown pin
#define HW_SHUTDOWN_GPIO GPIOC
#define HW_SHUTDOWN_PIN 5
#define HW_SHUTDOWN_HOLD_ON() palSetPad(HW_SHUTDOWN_GPIO, HW_SHUTDOWN_PIN)
#define HW_SHUTDOWN_HOLD_OFF() palClearPad(HW_SHUTDOWN_GPIO, HW_SHUTDOWN_PIN)
#define HW_SAMPLE_SHUTDOWN() hw_sample_shutdown_button()
// Hold shutdown pin early to wake up on short pulses
#define HW_EARLY_INIT() palSetPadMode(HW_SHUTDOWN_GPIO, HW_SHUTDOWN_PIN, PAL_MODE_OUTPUT_PUSHPULL); \
HW_SHUTDOWN_HOLD_ON();
#define PHASE_FILTER_GPIO GPIOC
#define PHASE_FILTER_PIN 13
#define PHASE_FILTER_ON() palSetPad(PHASE_FILTER_GPIO, PHASE_FILTER_PIN)
#define PHASE_FILTER_OFF() palClearPad(PHASE_FILTER_GPIO, PHASE_FILTER_PIN)
/*
* ADC Vector
*
* 0: IN0 SENS1
* 1: IN1 SENS2
* 2: IN2 SENS3
* 3: IN10 CURR1
* 4: IN11 CURR2
* 5: IN12 CURR3
* 6: IN5 ADC_EXT1
* 7: IN6 ADC_EXT2
* 8: IN3 TEMP_PCB
* 9: IN14 TEMP_MOTOR
* 10: IN15 Shutdown
* 11: IN13 AN_IN
* 12: Vrefint
* 13: IN0 SENS1
* 14: IN1 SENS2
*/
#define HW_ADC_CHANNELS 15
#define HW_ADC_INJ_CHANNELS 3
#define HW_ADC_NBR_CONV 5
// ADC Indexes
#define ADC_IND_SENS1 0
#define ADC_IND_SENS2 1
#define ADC_IND_SENS3 2
#define ADC_IND_CURR1 3
#define ADC_IND_CURR2 4
#define ADC_IND_CURR3 5
#define ADC_IND_VIN_SENS 11
#define ADC_IND_EXT 6
#define ADC_IND_EXT2 7
#define ADC_IND_TEMP_MOS 8
#define ADC_IND_TEMP_MOTOR 9
#define ADC_IND_VREFINT 12
#define ADC_IND_SHUTDOWN 10
// ADC macros and settings
// Component parameters (can be overridden)
#ifndef V_REG
#define V_REG 3.3
#endif
#ifndef VIN_R1
#define VIN_R1 56000.0
#endif
#ifndef VIN_R2
#define VIN_R2 2200.0
#endif
#ifndef CURRENT_AMP_GAIN
#define CURRENT_AMP_GAIN 20.0
#endif
#ifndef CURRENT_SHUNT_RES
#define CURRENT_SHUNT_RES 0.0005
#endif
// Input voltage
#define GET_INPUT_VOLTAGE() ((V_REG / 4095.0) * (float)ADC_Value[ADC_IND_VIN_SENS] * ((VIN_R1 + VIN_R2) / VIN_R2))
// NTC Termistors
#define NTC_RES(adc_val) ((4095.0 * 10000.0) / adc_val - 10000.0)
#define NTC_TEMP(adc_ind) (1.0 / ((logf(NTC_RES(ADC_Value[adc_ind]) / 10000.0) / 3380.0) + (1.0 / 298.15)) - 273.15)
#define NTC_RES_MOTOR(adc_val) (10000.0 / ((4095.0 / (float)adc_val) - 1.0)) // Motor temp sensor on low side
#define NTC_TEMP_MOTOR(beta) (1.0 / ((logf(NTC_RES_MOTOR(ADC_Value[ADC_IND_TEMP_MOTOR]) / 10000.0) / beta) + (1.0 / 298.15)) - 273.15)
// Voltage on ADC channel
#define ADC_VOLTS(ch) ((float)ADC_Value[ch] / 4096.0 * V_REG)
// Double samples in beginning and end for positive current measurement.
// Useful when the shunt sense traces have noise that causes offset.
#ifndef CURR1_DOUBLE_SAMPLE
#define CURR1_DOUBLE_SAMPLE 0
#endif
#ifndef CURR2_DOUBLE_SAMPLE
#define CURR2_DOUBLE_SAMPLE 0
#endif
#ifndef CURR3_DOUBLE_SAMPLE
#define CURR3_DOUBLE_SAMPLE 0
#endif
// COMM-port ADC GPIOs
#define HW_ADC_EXT_GPIO GPIOA
#define HW_ADC_EXT_PIN 5
#define HW_ADC_EXT2_GPIO GPIOA
#define HW_ADC_EXT2_PIN 6
// UART Peripheral
#define HW_UART_DEV SD3
#define HW_UART_GPIO_AF GPIO_AF_USART3
#define HW_UART_TX_PORT GPIOB
#define HW_UART_TX_PIN 10
#define HW_UART_RX_PORT GPIOB
#define HW_UART_RX_PIN 11
// Permanent UART Peripheral (for NRF52)
#define HW_UART_P_BAUD 115200
#define HW_UART_P_DEV SD4
#define HW_UART_P_GPIO_AF GPIO_AF_UART4
#define HW_UART_P_TX_PORT GPIOC
#define HW_UART_P_TX_PIN 10
#define HW_UART_P_RX_PORT GPIOC
#define HW_UART_P_RX_PIN 11
// ICU Peripheral for servo decoding
#define HW_USE_SERVO_TIM4
#define HW_ICU_TIMER TIM4
#define HW_ICU_TIM_CLK_EN() RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE)
#define HW_ICU_DEV ICUD4
#define HW_ICU_CHANNEL ICU_CHANNEL_1
#define HW_ICU_GPIO_AF GPIO_AF_TIM4
#define HW_ICU_GPIO GPIOB
#define HW_ICU_PIN 6
// I2C Peripheral
#define HW_I2C_DEV I2CD2
#define HW_I2C_GPIO_AF GPIO_AF_I2C2
#define HW_I2C_SCL_PORT GPIOB
#define HW_I2C_SCL_PIN 10
#define HW_I2C_SDA_PORT GPIOB
#define HW_I2C_SDA_PIN 11
// Hall/encoder pins
#define HW_HALL_ENC_GPIO1 GPIOC
#define HW_HALL_ENC_PIN1 6
#define HW_HALL_ENC_GPIO2 GPIOC
#define HW_HALL_ENC_PIN2 7
#define HW_HALL_ENC_GPIO3 GPIOC
#define HW_HALL_ENC_PIN3 8
#define HW_ENC_TIM TIM3
#define HW_ENC_TIM_AF GPIO_AF_TIM3
#define HW_ENC_TIM_CLK_EN() RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE)
#define HW_ENC_EXTI_PORTSRC EXTI_PortSourceGPIOC
#define HW_ENC_EXTI_PINSRC EXTI_PinSource8
#define HW_ENC_EXTI_CH EXTI9_5_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line8
#define HW_ENC_EXTI_ISR_VEC EXTI9_5_IRQHandler
#define HW_ENC_TIM_ISR_CH TIM3_IRQn
#define HW_ENC_TIM_ISR_VEC TIM3_IRQHandler
// SPI pins
#define HW_SPI_DEV SPID1
#define HW_SPI_GPIO_AF GPIO_AF_SPI1
#define HW_SPI_PORT_NSS GPIOB
#define HW_SPI_PIN_NSS 11
#define HW_SPI_PORT_SCK GPIOA
#define HW_SPI_PIN_SCK 5
#define HW_SPI_PORT_MOSI GPIOA
#define HW_SPI_PIN_MOSI 7
#define HW_SPI_PORT_MISO GPIOA
#define HW_SPI_PIN_MISO 6
// SPI for DRV8323S
#define DRV8323S_MOSI_GPIO GPIOC
#define DRV8323S_MOSI_PIN 12
#define DRV8323S_MISO_GPIO GPIOB
#define DRV8323S_MISO_PIN 3
#define DRV8323S_SCK_GPIO GPIOB
#define DRV8323S_SCK_PIN 4
#define DRV8323S_CS_GPIO GPIOC
#define DRV8323S_CS_PIN 9
// BMI160
#define BMI160_SDA_GPIO GPIOB
#define BMI160_SDA_PIN 2
#define BMI160_SCL_GPIO GPIOA
#define BMI160_SCL_PIN 15
#define IMU_FLIP
// NRF SWD
#define NRF5x_SWDIO_GPIO GPIOB
#define NRF5x_SWDIO_PIN 12
#define NRF5x_SWCLK_GPIO GPIOA
#define NRF5x_SWCLK_PIN 4
// Measurement macros
#define ADC_V_L1 ADC_Value[ADC_IND_SENS1]
#define ADC_V_L2 ADC_Value[ADC_IND_SENS2]
#define ADC_V_L3 ADC_Value[ADC_IND_SENS3]
#define ADC_V_ZERO (ADC_Value[ADC_IND_VIN_SENS] / 2)
// Macros
#define READ_HALL1() palReadPad(HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1)
#define READ_HALL2() palReadPad(HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2)
#define READ_HALL3() palReadPad(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3)
// Default setting overrides
#ifndef MCCONF_L_MIN_VOLTAGE
#define MCCONF_L_MIN_VOLTAGE 12.0 // Minimum input voltage
#endif
#ifndef MCCONF_L_MAX_VOLTAGE
#define MCCONF_L_MAX_VOLTAGE 74.0 // Maximum input voltage
#endif
#ifndef MCCONF_L_CURRENT_MAX
#define MCCONF_L_CURRENT_MAX 60.0 // Current limit in Amperes (Upper)
#endif
#ifndef MCCONF_L_CURRENT_MIN
#define MCCONF_L_CURRENT_MIN -60.0 // Current limit in Amperes (Lower)
#endif
#ifndef MCCONF_L_IN_CURRENT_MAX
#define MCCONF_L_IN_CURRENT_MAX 60.0 // Input current limit in Amperes (Upper)
#endif
#ifndef MCCONF_L_IN_CURRENT_MIN
#define MCCONF_L_IN_CURRENT_MIN -60.0 // Input current limit in Amperes (Lower)
#endif
#ifndef MCCONF_L_MAX_ABS_CURRENT
#define MCCONF_L_MAX_ABS_CURRENT 120.0 // The maximum absolute current above which a fault is generated
#endif
#ifndef MCCONF_M_DRV8301_OC_ADJ
#define MCCONF_M_DRV8301_OC_ADJ 18 // DRV8301 over current protection threshold
#endif
#ifndef MCCONF_DEFAULT_MOTOR_TYPE
#define MCCONF_DEFAULT_MOTOR_TYPE MOTOR_TYPE_FOC
#endif
#ifndef MCCONF_FOC_F_SW
#define MCCONF_FOC_F_SW 30000.0
#endif
// Setting limits
#define HW_LIM_CURRENT -80.0, 80.0
#define HW_LIM_CURRENT_IN -80.0, 80.0
#define HW_LIM_CURRENT_ABS 0.0, 150.0
#define HW_LIM_VIN 6.0, 74.0
#define HW_LIM_ERPM -200e3, 200e3
#define HW_LIM_DUTY_MIN 0.0, 0.1
#define HW_LIM_DUTY_MAX 0.0, 0.99
#define HW_LIM_TEMP_FET -40.0, 110.0
// Functions
bool hw_sample_shutdown_button(void);
#endif /* HW_UAVC_QCUBE_H_ */

245
hwconf/hw_uxv_sr.c Normal file
View File

@ -0,0 +1,245 @@
/*
Copyright 2019 Benjamin Vedder benjamin@vedder.se
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "hw.h"
#include "ch.h"
#include "hal.h"
#include "stm32f4xx_conf.h"
#include "utils.h"
#include "drv8323s.h"
// Variables
static volatile bool i2c_running = false;
// I2C configuration
static const I2CConfig i2cfg = {
OPMODE_I2C,
100000,
STD_DUTY_CYCLE
};
void hw_init_gpio(void) {
// GPIO clock enable
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
// LEDs
palSetPadMode(GPIOB, 0,
PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(GPIOB, 1,
PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST);
// ENABLE_GATE
palSetPadMode(GPIOB, 5,
PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST);
// Disable BMI160
palSetPadMode(GPIOA, 15,
PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST);
palSetPad(GPIOA, 15);
// Disable DCCAL
palSetPadMode(GPIOD, 2,
PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST);
palClearPad(GPIOD, 2);
ENABLE_GATE();
// GPIOA Configuration: Channel 1 to 3 as alternate function push-pull
palSetPadMode(GPIOA, 8, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
PAL_STM32_OSPEED_HIGHEST |
PAL_STM32_PUDR_FLOATING);
palSetPadMode(GPIOA, 9, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
PAL_STM32_OSPEED_HIGHEST |
PAL_STM32_PUDR_FLOATING);
palSetPadMode(GPIOA, 10, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
PAL_STM32_OSPEED_HIGHEST |
PAL_STM32_PUDR_FLOATING);
palSetPadMode(GPIOB, 13, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
PAL_STM32_OSPEED_HIGHEST |
PAL_STM32_PUDR_FLOATING);
palSetPadMode(GPIOB, 14, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
PAL_STM32_OSPEED_HIGHEST |
PAL_STM32_PUDR_FLOATING);
palSetPadMode(GPIOB, 15, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
PAL_STM32_OSPEED_HIGHEST |
PAL_STM32_PUDR_FLOATING);
// Hall sensors
palSetPadMode(HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1, PAL_MODE_INPUT_PULLUP);
palSetPadMode(HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2, PAL_MODE_INPUT_PULLUP);
palSetPadMode(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3, PAL_MODE_INPUT_PULLUP);
// Fault pin
palSetPadMode(GPIOB, 7, PAL_MODE_INPUT_PULLUP);
// ADC Pins
palSetPadMode(GPIOA, 0, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOA, 1, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOA, 2, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOA, 3, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOA, 5, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOA, 6, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOC, 0, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOC, 1, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOC, 2, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOC, 3, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOC, 4, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOC, 5, PAL_MODE_INPUT_ANALOG);
drv8323s_init();
}
void hw_setup_adc_channels(void) {
// ADC1 regular channels
ADC_RegularChannelConfig(ADC1, ADC_Channel_0, 1, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC1, ADC_Channel_10, 2, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC1, ADC_Channel_5, 3, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC1, ADC_Channel_14, 4, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC1, ADC_Channel_Vrefint, 5, ADC_SampleTime_15Cycles);
// ADC2 regular channels
ADC_RegularChannelConfig(ADC2, ADC_Channel_1, 1, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC2, ADC_Channel_11, 2, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC2, ADC_Channel_6, 3, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC2, ADC_Channel_15, 4, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC2, ADC_Channel_0, 5, ADC_SampleTime_15Cycles);
// ADC3 regular channels
ADC_RegularChannelConfig(ADC3, ADC_Channel_2, 1, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC3, ADC_Channel_12, 2, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC3, ADC_Channel_3, 3, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC3, ADC_Channel_13, 4, ADC_SampleTime_15Cycles);
ADC_RegularChannelConfig(ADC3, ADC_Channel_1, 5, ADC_SampleTime_15Cycles);
// Injected channels
ADC_InjectedChannelConfig(ADC1, ADC_Channel_10, 1, ADC_SampleTime_15Cycles);
ADC_InjectedChannelConfig(ADC2, ADC_Channel_11, 1, ADC_SampleTime_15Cycles);
ADC_InjectedChannelConfig(ADC3, ADC_Channel_12, 1, ADC_SampleTime_15Cycles);
ADC_InjectedChannelConfig(ADC1, ADC_Channel_10, 2, ADC_SampleTime_15Cycles);
ADC_InjectedChannelConfig(ADC2, ADC_Channel_11, 2, ADC_SampleTime_15Cycles);
ADC_InjectedChannelConfig(ADC3, ADC_Channel_12, 2, ADC_SampleTime_15Cycles);
ADC_InjectedChannelConfig(ADC1, ADC_Channel_10, 3, ADC_SampleTime_15Cycles);
ADC_InjectedChannelConfig(ADC2, ADC_Channel_11, 3, ADC_SampleTime_15Cycles);
ADC_InjectedChannelConfig(ADC3, ADC_Channel_12, 3, ADC_SampleTime_15Cycles);
}
void hw_start_i2c(void) {
i2cAcquireBus(&HW_I2C_DEV);
if (!i2c_running) {
palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN,
PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) |
PAL_STM32_OTYPE_OPENDRAIN |
PAL_STM32_OSPEED_MID1 |
PAL_STM32_PUDR_PULLUP);
palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) |
PAL_STM32_OTYPE_OPENDRAIN |
PAL_STM32_OSPEED_MID1 |
PAL_STM32_PUDR_PULLUP);
i2cStart(&HW_I2C_DEV, &i2cfg);
i2c_running = true;
}
i2cReleaseBus(&HW_I2C_DEV);
}
void hw_stop_i2c(void) {
i2cAcquireBus(&HW_I2C_DEV);
if (i2c_running) {
palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN, PAL_MODE_INPUT);
palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN, PAL_MODE_INPUT);
i2cStop(&HW_I2C_DEV);
i2c_running = false;
}
i2cReleaseBus(&HW_I2C_DEV);
}
/**
* Try to restore the i2c bus
*/
void hw_try_restore_i2c(void) {
if (i2c_running) {
i2cAcquireBus(&HW_I2C_DEV);
palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN,
PAL_STM32_OTYPE_OPENDRAIN |
PAL_STM32_OSPEED_MID1 |
PAL_STM32_PUDR_PULLUP);
palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
PAL_STM32_OTYPE_OPENDRAIN |
PAL_STM32_OSPEED_MID1 |
PAL_STM32_PUDR_PULLUP);
palSetPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
palSetPad(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN);
chThdSleep(1);
for(int i = 0;i < 16;i++) {
palClearPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
chThdSleep(1);
palSetPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
chThdSleep(1);
}
// Generate start then stop condition
palClearPad(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN);
chThdSleep(1);
palClearPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
chThdSleep(1);
palSetPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
chThdSleep(1);
palSetPad(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN);
palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN,
PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) |
PAL_STM32_OTYPE_OPENDRAIN |
PAL_STM32_OSPEED_MID1 |
PAL_STM32_PUDR_PULLUP);
palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) |
PAL_STM32_OTYPE_OPENDRAIN |
PAL_STM32_OSPEED_MID1 |
PAL_STM32_PUDR_PULLUP);
HW_I2C_DEV.state = I2C_STOP;
i2cStart(&HW_I2C_DEV, &i2cfg);
i2cReleaseBus(&HW_I2C_DEV);
}
}

251
hwconf/hw_uxv_sr.h Normal file
View File

@ -0,0 +1,251 @@
/*
Copyright 2019 Benjamin Vedder benjamin@vedder.se
This file is part of the VESC firmware.
The VESC firmware is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
The VESC firmware is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef HW_UXV_SR_H_
#define HW_UXV_SR_H_
#define HW_NAME "UXV_SR"
// HW properties
#define HW_HAS_DRV8323S
#define HW_HAS_3_SHUNTS
#define HW_HAS_PHASE_SHUNTS
// Macros
#define ENABLE_GATE() palSetPad(GPIOB, 5)
#define DISABLE_GATE() palClearPad(GPIOB, 5)
#define IS_DRV_FAULT() (!palReadPad(GPIOB, 7))
#define LED_GREEN_ON() palSetPad(GPIOB, 0)
#define LED_GREEN_OFF() palClearPad(GPIOB, 0)
#define LED_RED_ON() palSetPad(GPIOB, 1)
#define LED_RED_OFF() palClearPad(GPIOB, 1)
/*
* ADC Vector
*
* 0: IN0 SENS1
* 1: IN1 SENS2
* 2: IN2 SENS3
* 3: IN10 CURR1
* 4: IN11 CURR2
* 5: IN12 CURR3
* 6: IN5 ADC_EXT1
* 7: IN6 ADC_EXT2
* 8: IN3 TEMP_PCB
* 9: IN14 TEMP_MOTOR
* 10: IN15 ADC_EXT3
* 11: IN13 AN_IN
* 12: Vrefint
* 13: IN0 SENS1
* 14: IN1 SENS2
*/
#define HW_ADC_CHANNELS 15
#define HW_ADC_INJ_CHANNELS 3
#define HW_ADC_NBR_CONV 5
// ADC Indexes
#define ADC_IND_SENS1 0
#define ADC_IND_SENS2 1
#define ADC_IND_SENS3 2
#define ADC_IND_CURR1 3
#define ADC_IND_CURR2 4
#define ADC_IND_CURR3 5
#define ADC_IND_VIN_SENS 11
#define ADC_IND_EXT 6
#define ADC_IND_EXT2 7
#define ADC_IND_TEMP_MOS 8
#define ADC_IND_TEMP_MOTOR 9
#define ADC_IND_VREFINT 12
// ADC macros and settings
// Component parameters (can be overridden)
#ifndef V_REG
#define V_REG 3.3
#endif
#ifndef VIN_R1
#define VIN_R1 39000.0
#endif
#ifndef VIN_R2
#define VIN_R2 2200.0
#endif
#ifndef CURRENT_AMP_GAIN
#define CURRENT_AMP_GAIN 20.0
#endif
#ifndef CURRENT_SHUNT_RES
#define CURRENT_SHUNT_RES 0.0002
#endif
// Input voltage
#define GET_INPUT_VOLTAGE() ((V_REG / 4095.0) * (float)ADC_Value[ADC_IND_VIN_SENS] * ((VIN_R1 + VIN_R2) / VIN_R2))
// NTC Termistors
#define NTC_RES(adc_val) ((4095.0 * 10000.0) / adc_val - 10000.0)
#define NTC_TEMP(adc_ind) (1.0 / ((logf(NTC_RES(ADC_Value[adc_ind]) / 10000.0) / 3380.0) + (1.0 / 298.15)) - 273.15)
#define NTC_RES_MOTOR(adc_val) (10000.0 / ((4095.0 / (float)adc_val) - 1.0)) // Motor temp sensor on low side
#define NTC_TEMP_MOTOR(beta) (1.0 / ((logf(NTC_RES_MOTOR(ADC_Value[ADC_IND_TEMP_MOTOR]) / 10000.0) / beta) + (1.0 / 298.15)) - 273.15)
// Voltage on ADC channel
#define ADC_VOLTS(ch) ((float)ADC_Value[ch] / 4096.0 * V_REG)
// Double samples in beginning and end for positive current measurement.
// Useful when the shunt sense traces have noise that causes offset.
#ifndef CURR1_DOUBLE_SAMPLE
#define CURR1_DOUBLE_SAMPLE 0
#endif
#ifndef CURR2_DOUBLE_SAMPLE
#define CURR2_DOUBLE_SAMPLE 0
#endif
#ifndef CURR3_DOUBLE_SAMPLE
#define CURR3_DOUBLE_SAMPLE 0
#endif
// COMM-port ADC GPIOs
#define HW_ADC_EXT_GPIO GPIOA
#define HW_ADC_EXT_PIN 5
#define HW_ADC_EXT2_GPIO GPIOA
#define HW_ADC_EXT2_PIN 6
// UART Peripheral
#define HW_UART_DEV SD3
#define HW_UART_GPIO_AF GPIO_AF_USART3
#define HW_UART_TX_PORT GPIOB
#define HW_UART_TX_PIN 10
#define HW_UART_RX_PORT GPIOB
#define HW_UART_RX_PIN 11
// ICU Peripheral for servo decoding
#define HW_USE_SERVO_TIM4
#define HW_ICU_TIMER TIM4
#define HW_ICU_TIM_CLK_EN() RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE)
#define HW_ICU_DEV ICUD4
#define HW_ICU_CHANNEL ICU_CHANNEL_1
#define HW_ICU_GPIO_AF GPIO_AF_TIM4
#define HW_ICU_GPIO GPIOB
#define HW_ICU_PIN 6
// I2C Peripheral
#define HW_I2C_DEV I2CD2
#define HW_I2C_GPIO_AF GPIO_AF_I2C2
#define HW_I2C_SCL_PORT GPIOB
#define HW_I2C_SCL_PIN 10
#define HW_I2C_SDA_PORT GPIOB
#define HW_I2C_SDA_PIN 11
// Hall/encoder pins
#define HW_HALL_ENC_GPIO1 GPIOC
#define HW_HALL_ENC_PIN1 6
#define HW_HALL_ENC_GPIO2 GPIOC
#define HW_HALL_ENC_PIN2 7
#define HW_HALL_ENC_GPIO3 GPIOC
#define HW_HALL_ENC_PIN3 8
#define HW_ENC_TIM TIM3
#define HW_ENC_TIM_AF GPIO_AF_TIM3
#define HW_ENC_TIM_CLK_EN() RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE)
#define HW_ENC_EXTI_PORTSRC EXTI_PortSourceGPIOC
#define HW_ENC_EXTI_PINSRC EXTI_PinSource8
#define HW_ENC_EXTI_CH EXTI9_5_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line8
#define HW_ENC_EXTI_ISR_VEC EXTI9_5_IRQHandler
#define HW_ENC_TIM_ISR_CH TIM3_IRQn
#define HW_ENC_TIM_ISR_VEC TIM3_IRQHandler
// SPI pins
#define HW_SPI_DEV SPID1
#define HW_SPI_GPIO_AF GPIO_AF_SPI1
#define HW_SPI_PORT_NSS GPIOA
#define HW_SPI_PIN_NSS 4
#define HW_SPI_PORT_SCK GPIOA
#define HW_SPI_PIN_SCK 5
#define HW_SPI_PORT_MOSI GPIOA
#define HW_SPI_PIN_MOSI 7
#define HW_SPI_PORT_MISO GPIOA
#define HW_SPI_PIN_MISO 6
// SPI for DRV8323S
#define DRV8323S_MOSI_GPIO GPIOC
#define DRV8323S_MOSI_PIN 12
#define DRV8323S_MISO_GPIO GPIOC
#define DRV8323S_MISO_PIN 11
#define DRV8323S_SCK_GPIO GPIOC
#define DRV8323S_SCK_PIN 10
#define DRV8323S_CS_GPIO GPIOC
#define DRV8323S_CS_PIN 9
// Pins for ID detection
//List of three state (trinary digit) pins used for hardware identification
//States are: NC/Floating = 0 GND = 1, VCC = 2, in order P_LSB, ..., P_MSB
#define HW_DEFAULT_ID (APPCONF_CONTROLLER_ID >= 0 ? APPCONF_CONTROLLER_ID : hw_id_from_pins())
#define HW_ID_PIN_GPIOS GPIOC, GPIOC
#define HW_ID_PIN_PINS 14, 15
//APP settings
#define APPCONF_UAVCAN_ESC_INDEX (HW_DEFAULT_ID - 1)
#define APPCONF_APP_TO_USE APP_PPM
// Measurement macros
#define ADC_V_L1 ADC_Value[ADC_IND_SENS1]
#define ADC_V_L2 ADC_Value[ADC_IND_SENS2]
#define ADC_V_L3 ADC_Value[ADC_IND_SENS3]
#define ADC_V_ZERO (ADC_Value[ADC_IND_VIN_SENS] / 2)
// Macros
#define READ_HALL1() palReadPad(HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1)
#define READ_HALL2() palReadPad(HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2)
#define READ_HALL3() palReadPad(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3)
// Default setting overrides
#ifndef MCCONF_L_CURRENT_MAX
#define MCCONF_L_CURRENT_MAX 120.0 // Current limit in Amperes (Upper)
#endif
#ifndef MCCONF_L_CURRENT_MIN
#define MCCONF_L_CURRENT_MIN -60.0 // Current limit in Amperes (Lower)
#endif
#ifndef MCCONF_L_IN_CURRENT_MAX
#define MCCONF_L_IN_CURRENT_MAX 120.0 // Input current limit in Amperes (Upper)
#endif
#ifndef MCCONF_L_IN_CURRENT_MIN
#define MCCONF_L_IN_CURRENT_MIN -60.0 // Input current limit in Amperes (Lower)
#endif
#ifndef MCCONF_L_MAX_ABS_CURRENT
#define MCCONF_L_MAX_ABS_CURRENT 180.0 // The maximum absolute current above which a fault is generated
#endif
#ifndef MCCONF_DEFAULT_MOTOR_TYPE
#define MCCONF_DEFAULT_MOTOR_TYPE MOTOR_TYPE_FOC
#endif
#ifndef MCCONF_FOC_F_SW
#define MCCONF_FOC_F_SW 30000.0
#endif
// Setting limits
#define HW_LIM_CURRENT -60.0, 120.0
#define HW_LIM_CURRENT_IN -60.0, 120.0
#define HW_LIM_CURRENT_ABS 0.0, 180.0
#define HW_LIM_VIN 6.0, 57.0
#define HW_LIM_ERPM -200e3, 200e3
#define HW_LIM_DUTY_MIN 0.0, 0.1
#define HW_LIM_DUTY_MAX 0.0, 0.99
#define HW_LIM_TEMP_FET -40.0, 110.0
#endif /* HW_UXV_SR_H_ */

1
imu/bmi160_wrapper.h
View File

@ -27,6 +27,7 @@
#include <stdbool.h>
#include "i2c_bb.h"
#include "spi_bb.h"
#include "bmi160.h"
typedef struct {

122
imu/imu.c
View File

@ -37,6 +37,7 @@ static ATTITUDE_INFO m_att;
static float m_accel[3], m_gyro[3], m_mag[3];
static stkalign_t m_thd_work_area[THD_WORKING_AREA_SIZE(2048) / sizeof(stkalign_t)];
static i2c_bb_state m_i2c_bb;
static spi_bb_state m_spi_bb;
static ICM20948_STATE m_icm20948_state;
static BMI_STATE m_bmi_state;
static imu_config m_settings;
@ -50,6 +51,8 @@ static void terminal_gyro_info(int argc, const char **argv);
static void rotate(float *input, float *rotation, float *output);
int8_t user_i2c_read(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len);
int8_t user_i2c_write(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len);
int8_t user_spi_read(uint8_t dev_id, uint8_t reg_addr, uint8_t *data, uint16_t len);
int8_t user_spi_write(uint8_t dev_id, uint8_t reg_addr, uint8_t *data, uint16_t len);
void imu_init(imu_config *set) {
m_settings = *set;
@ -80,7 +83,7 @@ void imu_init(imu_config *set) {
#endif
#ifdef BMI160_SDA_GPIO
imu_init_bmi160(BMI160_SDA_GPIO, BMI160_SDA_PIN,
imu_init_bmi160_i2c(BMI160_SDA_GPIO, BMI160_SDA_PIN,
BMI160_SCL_GPIO, BMI160_SCL_PIN);
#endif
@ -88,6 +91,14 @@ void imu_init(imu_config *set) {
imu_init_lsm6ds3(LSM6DS3_SDA_GPIO, LSM6DS3_SDA_PIN,
LSM6DS3_SCL_GPIO, LSM6DS3_SCL_PIN);
#endif
#ifdef BMI160_SPI_PORT_NSS
imu_init_bmi160_spi(
BMI160_SPI_PORT_NSS, BMI160_SPI_PIN_NSS,
BMI160_SPI_PORT_SCK, BMI160_SPI_PIN_SCK,
BMI160_SPI_PORT_MOSI, BMI160_SPI_PIN_MOSI,
BMI160_SPI_PORT_MISO, BMI160_SPI_PIN_MISO);
#endif
} else if (set->type == IMU_TYPE_EXTERNAL_MPU9X50) {
imu_init_mpu9x50(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
@ -95,11 +106,14 @@ void imu_init(imu_config *set) {
imu_init_icm20948(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
HW_I2C_SCL_PORT, HW_I2C_SCL_PIN, 0);
} else if (set->type == IMU_TYPE_EXTERNAL_BMI160) {
imu_init_bmi160(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
imu_init_bmi160_i2c(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
}else if(set->type == IMU_TYPE_EXTERNAL_LSM6DS3){
} else if(set->type == IMU_TYPE_EXTERNAL_LSM6DS3) {
imu_init_lsm6ds3(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
} else if (set->type == IMU_TYPE_EXTERNAL_BMI160) {
imu_init_bmi160_i2c(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
}
terminal_register_command_callback(
@ -139,7 +153,7 @@ void imu_init_icm20948(stm32_gpio_t *sda_gpio, int sda_pin,
icm20948_set_read_callback(&m_icm20948_state, imu_read_callback);
}
void imu_init_bmi160(stm32_gpio_t *sda_gpio, int sda_pin,
void imu_init_bmi160_i2c(stm32_gpio_t *sda_gpio, int sda_pin,
stm32_gpio_t *scl_gpio, int scl_pin) {
imu_stop();
@ -158,6 +172,32 @@ void imu_init_bmi160(stm32_gpio_t *sda_gpio, int sda_pin,
bmi160_wrapper_set_read_callback(&m_bmi_state, imu_read_callback);
}
void imu_init_bmi160_spi(stm32_gpio_t *nss_gpio, int nss_pin,
stm32_gpio_t *sck_gpio, int sck_pin, stm32_gpio_t *mosi_gpio, int mosi_pin,
stm32_gpio_t *miso_gpio, int miso_pin) {
imu_stop();
m_spi_bb.nss_gpio = nss_gpio;
m_spi_bb.nss_pin = nss_pin;
m_spi_bb.sck_gpio = sck_gpio;
m_spi_bb.sck_pin = sck_pin;
m_spi_bb.mosi_gpio = mosi_gpio;
m_spi_bb.mosi_pin = mosi_pin;
m_spi_bb.miso_gpio = miso_gpio;
m_spi_bb.miso_pin = miso_pin;
spi_bb_init(&m_spi_bb);
m_bmi_state.sensor.id = 0;
m_bmi_state.sensor.interface = BMI160_SPI_INTF;
m_bmi_state.sensor.read = user_spi_read;
m_bmi_state.sensor.write = user_spi_write;
bmi160_wrapper_init(&m_bmi_state, m_thd_work_area, sizeof(m_thd_work_area));
bmi160_wrapper_set_read_callback(&m_bmi_state, imu_read_callback);
}
void imu_init_lsm6ds3(stm32_gpio_t *sda_gpio, int sda_pin,
stm32_gpio_t *scl_gpio, int scl_pin) {
@ -240,7 +280,7 @@ void imu_get_quaternions(float *q) {
q[3] = m_att.q3;
}
void imu_get_calibration(float yaw, float *imu_cal){
void imu_get_calibration(float yaw, float *imu_cal) {
// Backup current settings
float backup_sample_rate = m_settings.sample_rate_hz;
AHRS_MODE backup_ahrs_mode = m_settings.mode;
@ -280,8 +320,8 @@ void imu_get_calibration(float yaw, float *imu_cal){
m_gyro_offset[2] = 0;
// Sample gyro for offsets
float original_gyro_offsets[3] = {0,0,0};
for(int i = 0; i < 1000; i++){
float original_gyro_offsets[3] = {0, 0, 0};
for (int i = 0; i < 1000; i++) {
original_gyro_offsets[0] += m_gyro[0];
original_gyro_offsets[1] += m_gyro[1];
original_gyro_offsets[2] += m_gyro[2];
@ -302,14 +342,14 @@ void imu_get_calibration(float yaw, float *imu_cal){
// Sample roll
float roll_sample = 0;
for(int i = 0; i < 250; i++){
for (int i = 0; i < 250; i++) {
roll_sample += imu_get_roll();
chThdSleepMilliseconds(1);
}
roll_sample = roll_sample/250;
roll_sample = roll_sample / 250;
// Set roll rotations to level out roll axis
m_settings.rot_roll = -roll_sample * (180/M_PI);
m_settings.rot_roll = -roll_sample * (180 / M_PI);
// Rotate gyro offsets to match new IMU orientation
float rotation1[3] = {m_settings.rot_roll, m_settings.rot_pitch, m_settings.rot_yaw};
@ -321,14 +361,14 @@ void imu_get_calibration(float yaw, float *imu_cal){
// Sample pitch
float pitch_sample = 0;
for(int i = 0; i < 250; i++){
for (int i = 0; i < 250; i++) {
pitch_sample += imu_get_pitch();
chThdSleepMilliseconds(1);
}
pitch_sample = pitch_sample/250;
pitch_sample = pitch_sample / 250;
// Set pitch rotation to level out pitch axis
m_settings.rot_pitch = pitch_sample * (180/M_PI);
m_settings.rot_pitch = pitch_sample * (180 / M_PI);
// Rotate imu offsets to match
float rotation2[3] = {m_settings.rot_roll, m_settings.rot_pitch, m_settings.rot_yaw};
@ -385,7 +425,7 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) {
float dt = timer_seconds_elapsed_since(last_time);
last_time = timer_time_now();
if(!imu_ready && ST2MS(chVTGetSystemTimeX() - init_time) > 1000){
if (!imu_ready && ST2MS(chVTGetSystemTimeX() - init_time) > 1000) {
ahrs_update_all_parameters(
m_settings.accel_confidence_decay,
m_settings.mahony_kp,
@ -438,7 +478,7 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) {
m_mag[2] = mag[0] * m31 + mag[1] * m32 + mag[2] * m33;
// Accelerometer and Gyro offset compensation and estimation
for (int i = 0;i < 3;i++) {
for (int i = 0; i < 3; i++) {
m_accel[i] -= m_settings.accel_offsets[i];
m_gyro[i] -= m_settings.gyro_offsets[i];
@ -457,12 +497,12 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) {
gyro_rad[1] = m_gyro[1] * M_PI / 180.0;
gyro_rad[2] = m_gyro[2] * M_PI / 180.0;
switch (m_settings.mode){
switch (m_settings.mode) {
case (AHRS_MODE_MADGWICK):
ahrs_update_madgwick_imu(gyro_rad, m_accel, dt, (ATTITUDE_INFO*)&m_att);
ahrs_update_madgwick_imu(gyro_rad, m_accel, dt, (ATTITUDE_INFO *)&m_att);
break;
case (AHRS_MODE_MAHONY):
ahrs_update_mahony_imu(gyro_rad, m_accel, dt, (ATTITUDE_INFO*)&m_att);
ahrs_update_mahony_imu(gyro_rad, m_accel, dt, (ATTITUDE_INFO *)&m_att);
break;
}
}
@ -477,7 +517,7 @@ static void terminal_gyro_info(int argc, const char **argv) {
(double)(m_settings.gyro_offsets[2] + m_gyro_offset[2]));
}
void rotate(float *input, float *rotation, float *output){
void rotate(float *input, float *rotation, float *output) {
// Rotate imu offsets to match
float s1 = sinf(rotation[2] * M_PI / 180.0);
float c1 = cosf(rotation[2] * M_PI / 180.0);
@ -511,3 +551,47 @@ int8_t user_i2c_write(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_
memcpy(txbuf + 1, data, len);
return i2c_bb_tx_rx(&m_i2c_bb, dev_addr, txbuf, len + 1, 0, 0) ? BMI160_OK : BMI160_E_COM_FAIL;
}
int8_t user_spi_read(uint8_t dev_id, uint8_t reg_addr, uint8_t *data, uint16_t len) {
(void)dev_id;
int8_t rslt = BMI160_OK; // Return 0 for Success, non-zero for failure
reg_addr = (reg_addr | BMI160_SPI_RD_MASK);
chThdSleepMicroseconds(200); // #FIXME Wont work without this- Why?
chMtxLock(&m_spi_bb.mutex);
spi_bb_begin(&m_spi_bb);
spi_bb_exchange_8(&m_spi_bb, reg_addr);
spi_bb_delay();
for (int i = 0; i < len; i++) {
data[i] = spi_bb_exchange_8(&m_spi_bb, 0);
}
spi_bb_end(&m_spi_bb);
chMtxUnlock(&m_spi_bb.mutex);
return rslt;
}
int8_t user_spi_write(uint8_t dev_id, uint8_t reg_addr, uint8_t *data, uint16_t len) {
(void)dev_id;
int8_t rslt = BMI160_OK; /* Return 0 for Success, non-zero for failure */
chMtxLock(&m_spi_bb.mutex);
spi_bb_begin(&m_spi_bb);
reg_addr = (reg_addr & BMI160_SPI_WR_MASK);
spi_bb_exchange_8(&m_spi_bb, reg_addr);
spi_bb_delay();
for (int i = 0; i < len; i++) {
spi_bb_exchange_8(&m_spi_bb, *data);
data++;
}
spi_bb_end(&m_spi_bb);
chMtxUnlock(&m_spi_bb.mutex);
return rslt;
}

8
imu/imu.h
View File

@ -23,6 +23,7 @@
#include "ch.h"
#include "hal.h"
#include "i2c_bb.h"
#include "spi_bb.h"
void imu_init(imu_config *set);
i2c_bb_state *imu_get_i2c(void);
@ -30,10 +31,15 @@ void imu_init_mpu9x50(stm32_gpio_t *sda_gpio, int sda_pin,
stm32_gpio_t *scl_gpio, int scl_pin);
void imu_init_icm20948(stm32_gpio_t *sda_gpio, int sda_pin,
stm32_gpio_t *scl_gpio, int scl_pin, int ad0_val);
void imu_init_bmi160(stm32_gpio_t *sda_gpio, int sda_pin,
void imu_init_bmi160_i2c(stm32_gpio_t *sda_gpio, int sda_pin,
stm32_gpio_t *scl_gpio, int scl_pin);
void imu_init_lsm6ds3(stm32_gpio_t *sda_gpio, int sda_pin,
stm32_gpio_t *scl_gpio, int scl_pin);
void imu_init_bmi160_spi(
stm32_gpio_t *nss_gpio, int nss_pin,
stm32_gpio_t *sck_gpio, int sck_pin,
stm32_gpio_t *mosi_gpio, int mosi_pin,
stm32_gpio_t *miso_gpio, int miso_pin);
void imu_stop(void);
bool imu_startup_done(void);
float imu_get_roll(void);

2
ledpwm.h
View File

@ -20,7 +20,7 @@
#ifndef LEDPWM_H_
#define LEDPWM_H_
#include "hw.h"
#include "conf_general.h"
#ifdef LED_PWM4_ON
#define HW_LEDPWM_CH 4

4
mc_interface.c
View File

@ -1211,7 +1211,7 @@ float mc_interface_read_reset_avg_id(void) {
float res = motor_now()->m_motor_id_sum / motor_now()->m_motor_id_iterations;
motor_now()->m_motor_id_sum = 0.0;
motor_now()->m_motor_id_iterations = 0.0;
return DIR_MULT * res; // TODO: DIR_MULT?
return res;
}
/**
@ -1237,7 +1237,7 @@ float mc_interface_read_reset_avg_vd(void) {
float res = motor_now()->m_motor_vd_sum / motor_now()->m_motor_vd_iterations;
motor_now()->m_motor_vd_sum = 0.0;
motor_now()->m_motor_vd_iterations = 0.0;
return DIR_MULT * res;
return res;
}
/**

3
mcconf/mcconf_default.h
View File

@ -263,6 +263,9 @@
#ifndef MCCONF_FOC_MOTOR_FLUX_LINKAGE
#define MCCONF_FOC_MOTOR_FLUX_LINKAGE 0.00245
#endif
#ifndef MCCONF_FOC_MOTOR_LD_LQ_DIFF
#define MCCONF_FOC_MOTOR_LD_LQ_DIFF 0.0
#endif
#ifndef MCCONF_FOC_OBSERVER_GAIN
#define MCCONF_FOC_OBSERVER_GAIN 9e7 // Can be something like 600 / L
#endif

55
mcpwm_foc.c
View File

@ -529,7 +529,7 @@ void mcpwm_foc_init(volatile mc_configuration *conf_m1, volatile mc_configuratio
update_hfi_samples(m_motor_2.m_conf->foc_hfi_samples, &m_motor_2);
#endif
virtual_motor_init();
virtual_motor_init(conf_m1);
TIM_DeInit(TIM1);
TIM_DeInit(TIM2);
@ -723,6 +723,8 @@ void mcpwm_foc_set_configuration(volatile mc_configuration *configuration) {
stop_pwm_hw(motor_now());
update_hfi_samples(motor_now()->m_conf->foc_hfi_samples, motor_now());
}
virtual_motor_set_configuration(configuration);
}
mc_state mcpwm_foc_get_state(void) {
@ -812,7 +814,6 @@ void mcpwm_foc_set_pid_speed(float rpm) {
}
motor_now()->m_control_mode = CONTROL_MODE_SPEED;
motor_now()->m_speed_pid_set_rpm = rpm;
if (motor_now()->m_state != MC_STATE_RUNNING) {
motor_now()->m_state = MC_STATE_RUNNING;
@ -1083,20 +1084,20 @@ bool mcpwm_foc_is_using_encoder(void) {
float mcpwm_foc_get_tot_current_motor(bool is_second_motor) {
#ifdef HW_HAS_DUAL_MOTORS
volatile motor_all_state_t *motor = is_second_motor ? &m_motor_2 : &m_motor_1;
return SIGN(motor->m_motor_state.vq) * motor->m_motor_state.iq;
return SIGN(motor->m_motor_state.vq * motor->m_motor_state.iq) * motor->m_motor_state.i_abs;
#else
(void)is_second_motor;
return SIGN(m_motor_1.m_motor_state.vq) * m_motor_1.m_motor_state.iq;
return SIGN(m_motor_1.m_motor_state.vq * m_motor_1.m_motor_state.iq) * m_motor_1.m_motor_state.i_abs;
#endif
}
float mcpwm_foc_get_tot_current_filtered_motor(bool is_second_motor) {
#ifdef HW_HAS_DUAL_MOTORS
volatile motor_all_state_t *motor = is_second_motor ? &m_motor_2 : &m_motor_1;
return SIGN(motor->m_motor_state.vq) * motor->m_motor_state.iq_filter;
return SIGN(motor->m_motor_state.vq * motor->m_motor_state.iq_filter) * motor->m_motor_state.i_abs_filter;
#else
(void)is_second_motor;
return SIGN(m_motor_1.m_motor_state.vq) * m_motor_1.m_motor_state.iq_filter;
return SIGN(m_motor_1.m_motor_state.vq * m_motor_1.m_motor_state.iq_filter) * m_motor_1.m_motor_state.i_abs_filter;
#endif
}
@ -1182,7 +1183,8 @@ float mcpwm_foc_get_rpm_faster(void) {
* The motor current.
*/
float mcpwm_foc_get_tot_current(void) {
return SIGN(motor_now()->m_motor_state.vq) * motor_now()->m_motor_state.iq;
volatile motor_all_state_t *motor = motor_now();
return SIGN(motor->m_motor_state.vq * motor->m_motor_state.iq) * motor->m_motor_state.i_abs;
}
/**
@ -1194,7 +1196,8 @@ float mcpwm_foc_get_tot_current(void) {
* The filtered motor current.
*/
float mcpwm_foc_get_tot_current_filtered(void) {
return SIGN(motor_now()->m_motor_state.vq) * motor_now()->m_motor_state.iq_filter;
volatile motor_all_state_t *motor = motor_now();
return SIGN(motor->m_motor_state.vq * motor->m_motor_state.iq_filter) * motor->m_motor_state.i_abs_filter;
}
/**
@ -1462,10 +1465,12 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r
float offset_old = motor->m_conf->foc_encoder_offset;
float inverted_old = motor->m_conf->foc_encoder_inverted;
float ratio_old = motor->m_conf->foc_encoder_ratio;
float ldiff_old = motor->m_conf->foc_motor_ld_lq_diff;
motor->m_conf->foc_encoder_offset = 0.0;
motor->m_conf->foc_encoder_inverted = false;
motor->m_conf->foc_encoder_ratio = 1.0;
motor->m_conf->foc_motor_ld_lq_diff = 0.0;
// Find index
int cnt = 0;
@ -1651,6 +1656,7 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r
motor->m_conf->foc_encoder_inverted = inverted_old;
motor->m_conf->foc_encoder_offset = offset_old;
motor->m_conf->foc_encoder_ratio = ratio_old;
motor->m_conf->foc_motor_ld_lq_diff = ldiff_old;
// Enable timeout
timeout_configure(tout, tout_c);
@ -2324,14 +2330,22 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
UTILS_LP_FAST(motor_now->m_motor_state.v_bus, GET_INPUT_VOLTAGE(), 0.1);
float enc_ang = 0;
if (encoder_is_configured()) {
if (virtual_motor_is_connected()){
enc_ang = virtual_motor_get_angle_deg();
} else {
enc_ang = encoder_read_deg();
}
volatile float enc_ang = 0;
volatile bool encoder_is_being_used = false;
if(virtual_motor_is_connected()){
if(conf_now->foc_sensor_mode == FOC_SENSOR_MODE_ENCODER ){
enc_ang = virtual_motor_get_angle_deg();
encoder_is_being_used = true;
}
}else{
if (encoder_is_configured()) {
enc_ang = encoder_read_deg();
encoder_is_being_used = true;
}
}
if(encoder_is_being_used){
float phase_tmp = enc_ang;
if (conf_now->foc_encoder_inverted) {
phase_tmp = 360.0 - phase_tmp;
@ -2455,7 +2469,7 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
switch (conf_now->foc_sensor_mode) {
case FOC_SENSOR_MODE_ENCODER:
if (encoder_index_found()) {
if (encoder_index_found() || virtual_motor_is_connected()) {
motor_now->m_motor_state.phase = correct_encoder(
motor_now->m_phase_now_observer,
motor_now->m_phase_now_encoder,
@ -2541,6 +2555,15 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
motor_now->m_motor_state.phase = motor_now->m_phase_now_override;
}
// Apply MTPA. See: https://github.com/vedderb/bldc/pull/179
float ld_lq_diff = conf_now->foc_motor_ld_lq_diff;
if (ld_lq_diff != 0.0) {
float lambda = conf_now->foc_motor_flux_linkage;
id_set_tmp = (lambda - sqrtf(SQ(lambda) + 8.0 * SQ(ld_lq_diff) * SQ(iq_set_tmp))) / (4.0 * ld_lq_diff);
iq_set_tmp = SIGN(iq_set_tmp) * sqrtf(SQ(iq_set_tmp) - SQ(id_set_tmp));
}
// Apply current limits
// TODO: Consider D axis current for the input current as well.
const float mod_q = motor_now->m_motor_state.mod_q;

1
mcuconf.h
View File

@ -17,6 +17,7 @@
#ifndef _MCUCONF_H_
#define _MCUCONF_H_
#include "conf_general.h"
#include "hw.h"
/*

2
nrf/spi_sw.h
View File

@ -19,7 +19,7 @@
#ifndef SPI_SW_H_
#define SPI_SW_H_
#include "hw.h"
#include "conf_general.h"
#include "ch.h"
#include "hal.h"

1
servo_simple.c
View File

@ -20,7 +20,6 @@
#include "servo_simple.h"
#include "ch.h"
#include "hal.h"
#include "hw.h"
#include "conf_general.h"
#include "stm32f4xx_conf.h"
#include "utils.h"

102
spi_bb.c Normal file
View File

@ -0,0 +1,102 @@
/*
Copyright 2019 Benjamin Vedder benjamin@vedder.se
This file is part of the VESC firmware.
The VESC firmware is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
The VESC firmware is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "spi_bb.h"
#include "timer.h"
// Software SPI
void spi_bb_init(spi_bb_state *s) {
chMtxObjectInit(&s->mutex);
palSetPadMode(s->miso_gpio, s->miso_pin, PAL_MODE_INPUT_PULLUP);
palSetPadMode(s->sck_gpio, s->sck_pin, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(s->nss_gpio, s->nss_pin, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(s->mosi_gpio, s->mosi_pin, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
palSetPad(s->mosi_gpio, s->mosi_pin);
palSetPad(s->nss_gpio, s->nss_pin);
s->has_started = false;
s->has_error = false;
}
uint8_t spi_bb_exchange_8(spi_bb_state *s, uint8_t x) {
uint8_t rx;
spi_bb_transfer_8(s ,&rx, &x, 1);
return rx;
}
void spi_bb_transfer_8(spi_bb_state *s, uint8_t *in_buf, const uint8_t *out_buf, int length) {
for (int i = 0; i < length; i++) {
uint8_t send = out_buf ? out_buf[i] : 0xFF;
uint8_t receive = 0;
for (int bit = 0; bit < 8; bit++) {
palWritePad(s->mosi_gpio, s->mosi_pin, send >> 7);
send <<= 1;
palSetPad(s->sck_gpio, s->sck_pin);
spi_bb_delay();
int samples = 0;
samples += palReadPad(s->miso_gpio, s->miso_pin);
__NOP();
samples += palReadPad(s->miso_gpio, s->miso_pin);
__NOP();
samples += palReadPad(s->miso_gpio, s->miso_pin);
__NOP();
samples += palReadPad(s->miso_gpio, s->miso_pin);
__NOP();
samples += palReadPad(s->miso_gpio, s->miso_pin);
palClearPad(s->sck_gpio, s->sck_pin);
// does 5 samples of each pad read, to minimize noise
receive <<= 1;
if (samples > 2) {
receive |= 1;
}
spi_bb_delay();
}
if (in_buf) {
in_buf[i] = receive;
}
}
}
void spi_bb_begin(spi_bb_state *s) {
spi_bb_delay();
palClearPad(s->nss_gpio, s->nss_pin);
spi_bb_delay();
}
void spi_bb_end(spi_bb_state *s) {
spi_bb_delay();
palSetPad(s->nss_gpio, s->nss_pin);
spi_bb_delay();
}
void spi_bb_delay(void) {
for (volatile int i = 0; i < 40; i++) {
__NOP();
}
}

49
spi_bb.h Normal file
View File

@ -0,0 +1,49 @@
/*
Copyright 2019 Benjamin Vedder benjamin@vedder.se
This file is part of the VESC firmware.
The VESC firmware is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
The VESC firmware is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef SPI_BB_H_
#define SPI_BB_H_
#include "ch.h"
#include "hal.h"
#include "stdint.h"
#include "stdbool.h"
typedef struct {
stm32_gpio_t *nss_gpio;
int nss_pin;
stm32_gpio_t *sck_gpio;
int sck_pin;
stm32_gpio_t *mosi_gpio;
int mosi_pin;
stm32_gpio_t *miso_gpio;
int miso_pin;
bool has_started;
bool has_error;
mutex_t mutex;
} spi_bb_state;
void spi_bb_init(spi_bb_state *s);
uint8_t spi_bb_exchange_8(spi_bb_state *s, uint8_t x);
void spi_bb_transfer_8(spi_bb_state *s, uint8_t *in_buf, const uint8_t *out_buf, int length);
void spi_bb_begin(spi_bb_state *s);
void spi_bb_end(spi_bb_state *s);
void spi_bb_delay(void);
#endif /* SPI_BB_H_ */

168
virtual_motor.c
View File

@ -29,14 +29,12 @@
typedef struct{
//constant variables
float Ts; //Sample Time in s
float Rs; //Stator Resistance Phase to Neutral in Ohms
float Ld; //Inductance in d-Direction in H
float Lq; //Inductance in q-Direction in H
float flux_linkage; //Flux linkage of the permanent magnets in mWb
float J; //Rotor/Load Inertia in Nm*s^2
int v_max_adc; //max voltage that ADC can measure
int pole_pairs; //number of pole pairs ( pole numbers / 2)
float km; //constant = 1.5 * pole pairs
float ld; //motor inductance in D axis in uHy
float lq; //motor inductance in Q axis in uHy
//non constant variables
float id; //Current in d-Direction in Amps
@ -68,10 +66,10 @@ static volatile virtual_motor_t virtual_motor;
static volatile int m_curr0_offset_backup;
static volatile int m_curr1_offset_backup;
static volatile int m_curr2_offset_backup;
static volatile mc_configuration *m_conf;
//private functions
static void connect_virtual_motor(float ml, float J, float Ld, float Lq,
float Rs,float lambda,float Vbus);
static void connect_virtual_motor(float ml, float J, float Vbus);
static void disconnect_virtual_motor(void);
static inline void run_virtual_motor_electrical(float v_alpha, float v_beta);
static inline void run_virtual_motor_mechanics(float ml);
@ -85,7 +83,10 @@ static void terminal_cmd_disconnect_virtual_motor(int argc, const char **argv);
/**
* Virtual motor initialization
*/
void virtual_motor_init(void){
void virtual_motor_init(volatile mc_configuration *conf){
virtual_motor_set_configuration(conf);
//virtual motor variables init
virtual_motor.connected = false; //disconnected
@ -103,15 +104,12 @@ void virtual_motor_init(void){
virtual_motor.i_beta = 0.0;
virtual_motor.id_int = 0.0;
virtual_motor.iq = 0.0;
const volatile mc_configuration *conf = mc_interface_get_configuration();
virtual_motor.pole_pairs = conf->si_motor_poles / 2;
virtual_motor.km = 1.5 * virtual_motor.pole_pairs;
// Register terminal callbacks used for virtual motor setup
terminal_register_command_callback(
"connect_virtual_motor",
"connects virtual motor",
"[ml][J][Ld][Lq][Rs][lambda][Vbus]",
"[ml][J][Vbus]",
terminal_cmd_connect_virtual_motor);
terminal_register_command_callback(
@ -121,6 +119,31 @@ void virtual_motor_init(void){
terminal_cmd_disconnect_virtual_motor);
}
void virtual_motor_set_configuration(volatile mc_configuration *conf){
m_conf = conf;
//recalculate constants that depend on m_conf
virtual_motor.pole_pairs = m_conf->si_motor_poles / 2;
virtual_motor.km = 1.5 * virtual_motor.pole_pairs;
#ifdef HW_HAS_PHASE_SHUNTS
if (m_conf->foc_sample_v0_v7) {
virtual_motor.Ts = (1.0 / m_conf->foc_f_sw) ;
} else {
virtual_motor.Ts = (1.0 / (m_conf->foc_f_sw / 2.0));
}
#else
virtual_motor.Ts = (1.0 / m_conf->foc_f_sw) ;
#endif
if(m_conf->foc_motor_ld_lq_diff > 0.0){
virtual_motor.lq = m_conf->foc_motor_l + m_conf->foc_motor_ld_lq_diff /2;
virtual_motor.ld = m_conf->foc_motor_l - m_conf->foc_motor_ld_lq_diff /2;
}else{
virtual_motor.lq = m_conf->foc_motor_l ;
virtual_motor.ld = m_conf->foc_motor_l ;
}
}
/**
* Virtual motor interrupt handler
*/
@ -151,14 +174,9 @@ float virtual_motor_get_angle_deg(void){
*
* @param ml : torque present at motor axis in Nm
* @param J: rotor inertia Nm*s^2
* @param Ld: inductance at d axis in Hy
* @param Lq: inductance at q axis in Hy
* @param Rs: resistance in ohms
* @param lambda: flux linkage in Vs/rad
* @param Vbus: Bus voltage in Volts
*/
static void connect_virtual_motor(float ml , float J, float Ld, float Lq,
float Rs, float lambda, float Vbus){
static void connect_virtual_motor(float ml , float J, float Vbus){
if(virtual_motor.connected == false){
//first we send 0.0 current command to make system stop PWM outputs
mcpwm_foc_set_current(0.0);
@ -184,32 +202,40 @@ static void connect_virtual_motor(float ml , float J, float Ld, float Lq,
mcpwm_foc_set_current_offsets(2048, 2048, 2048);
//sets virtual motor variables
ADC_Value[ ADC_IND_VIN_SENS ] = Vbus * VOLTAGE_TO_ADC_FACTOR;
ADC_Value[ ADC_IND_TEMP_MOS ] = 2048;
ADC_Value[ ADC_IND_TEMP_MOTOR ] = 2048;
#ifdef HW_VERSION_AXIOM
ADC_Value[ ADC_IND_VOUT_GATE_DRV ] = 1692;
// 1692 gives 15.0 as Gate Driver Voltage
//( 15.0 = (ADC_Value[] * 11.0 * 3.3) / 4096 )
#ifdef HW_HAS_GATE_DRIVER_SUPPLY_MONITOR
//we load 1 to get the transfer function indirectly
ADC_Value[ ADC_IND_VOUT_GATE_DRV ] = 1;
float tempVoltage = GET_GATE_DRIVER_SUPPLY_VOLTAGE();
if(tempVoltage != 0.0){
ADC_Value[ ADC_IND_VOUT_GATE_DRV ] = ( (HW_GATE_DRIVER_SUPPLY_MAX_VOLTAGE + HW_GATE_DRIVER_SUPPLY_MIN_VOLTAGE) / 2.0 ) /
GET_GATE_DRIVER_SUPPLY_VOLTAGE();
}
#endif
virtual_motor.phi = mcpwm_foc_get_phase() * M_PI / 180.0;// 0.0;//m_motor_state.phase;
virtual_motor.phi = mcpwm_foc_get_phase() * M_PI / 180.0;
utils_fast_sincos_better(virtual_motor.phi, (float*)&virtual_motor.sin_phi,
(float*)&virtual_motor.cos_phi);
if(m_conf->foc_sensor_mode == FOC_SENSOR_MODE_ENCODER){
encoder_deinit();
}
}
//we load 1 to get the transfer function indirectly
ADC_Value[ ADC_IND_VIN_SENS ] = 1;
float tempVoltage = GET_INPUT_VOLTAGE();
if(tempVoltage != 0.0){
ADC_Value[ ADC_IND_VIN_SENS ] = Vbus / GET_INPUT_VOLTAGE();
}
//initialize constants
virtual_motor.Ts = mcpwm_foc_get_ts();
virtual_motor.v_max_adc = Vbus;
virtual_motor.J = J;
virtual_motor.Ld = Ld;
virtual_motor.Lq = Lq;
virtual_motor.flux_linkage = lambda;
virtual_motor.Rs = Rs;
virtual_motor.ml = ml;
virtual_motor.tsj = virtual_motor.Ts / virtual_motor.J;
const volatile mc_configuration *conf = mc_interface_get_configuration();
virtual_motor.pole_pairs = conf->si_motor_poles / 2;
virtual_motor.km = 1.5 * virtual_motor.pole_pairs;
virtual_motor.ml = ml;
virtual_motor.connected = true;
}
@ -246,6 +272,31 @@ static void disconnect_virtual_motor( void ){
ADC_InitStructure.ADC_NbrOfConversion = HW_ADC_NBR_CONV;
ADC_Init(ADC1, &ADC_InitStructure);
if(m_conf->foc_sensor_mode == FOC_SENSOR_MODE_ENCODER){
switch (m_conf->m_sensor_port_mode) {
case SENSOR_PORT_MODE_ABI:
encoder_init_abi(m_conf->m_encoder_counts);
break;
case SENSOR_PORT_MODE_AS5047_SPI:
encoder_init_as5047p_spi();
break;
case SENSOR_PORT_MODE_AD2S1205:
encoder_init_ad2s1205_spi();
break;
case SENSOR_PORT_MODE_SINCOS:
encoder_init_sincos(m_conf->foc_encoder_sin_gain, m_conf->foc_encoder_sin_offset,
m_conf->foc_encoder_cos_gain, m_conf->foc_encoder_cos_offset,
m_conf->foc_encoder_sincos_filter_constant);
break;
default:
break;
}
}
}
}
@ -271,9 +322,6 @@ static inline void run_virtual_motor(float v_alpha, float v_beta, float ml){
*/
static inline void run_virtual_motor_electrical(float v_alpha, float v_beta){
utils_fast_sincos_better( virtual_motor.phi , (float*)&virtual_motor.sin_phi,
(float*)&virtual_motor.cos_phi );
virtual_motor.vd = virtual_motor.cos_phi * v_alpha + virtual_motor.sin_phi * v_beta;
virtual_motor.vq = virtual_motor.cos_phi * v_beta - virtual_motor.sin_phi * v_alpha;
@ -281,18 +329,22 @@ static inline void run_virtual_motor_electrical(float v_alpha, float v_beta){
virtual_motor.id_int += ((virtual_motor.vd +
virtual_motor.we *
virtual_motor.pole_pairs *
virtual_motor.Lq * virtual_motor.iq -
virtual_motor.Rs * virtual_motor.id )
* virtual_motor.Ts ) / virtual_motor.Ld;
virtual_motor.id = virtual_motor.id_int - virtual_motor.flux_linkage / virtual_motor.Ld;
virtual_motor.lq * virtual_motor.iq -
m_conf->foc_motor_r * virtual_motor.id )
* virtual_motor.Ts ) / virtual_motor.ld;
virtual_motor.id = virtual_motor.id_int - m_conf->foc_motor_flux_linkage / virtual_motor.ld;
// q axis current
virtual_motor.iq += (virtual_motor.vq -
virtual_motor.we *
virtual_motor.pole_pairs *
(virtual_motor.Ld * virtual_motor.id + virtual_motor.flux_linkage) -
virtual_motor.Rs * virtual_motor.iq )
* virtual_motor.Ts / virtual_motor.Lq;
(virtual_motor.ld * virtual_motor.id + m_conf->foc_motor_flux_linkage) -
m_conf->foc_motor_r * virtual_motor.iq )
* virtual_motor.Ts / virtual_motor.lq;
// // limit current maximum values
utils_truncate_number_abs((float *) &(virtual_motor.iq) , (2048 * FAC_CURRENT) );
utils_truncate_number_abs((float *) &(virtual_motor.id) , (2048 * FAC_CURRENT) );
}
/**
@ -300,17 +352,11 @@ static inline void run_virtual_motor_electrical(float v_alpha, float v_beta){
* @param ml externally applied load torque in Nm
*/
static inline void run_virtual_motor_mechanics(float ml){
virtual_motor.me = virtual_motor.km * (virtual_motor.flux_linkage +
(virtual_motor.Ld - virtual_motor.Lq) *
virtual_motor.me = virtual_motor.km * (m_conf->foc_motor_flux_linkage +
(virtual_motor.ld - virtual_motor.lq) *
virtual_motor.id ) * virtual_motor.iq;
// omega
float w_aux = virtual_motor.we + virtual_motor.tsj * (virtual_motor.me - ml);
if( w_aux < 0.0 ){
virtual_motor.we = 0;
}else{
virtual_motor.we = w_aux;
}
virtual_motor.we += virtual_motor.tsj * (virtual_motor.me - ml);
// phi
virtual_motor.phi += virtual_motor.we * virtual_motor.Ts;
@ -323,13 +369,15 @@ static inline void run_virtual_motor_mechanics(float ml){
while( virtual_motor.phi < -1.0 * M_PI ){
virtual_motor.phi += ( 2 * M_PI);
}
}
/**
* Take the id and iq calculated values and translate them into ADC_Values
*/
static inline void run_virtual_motor_park_clark_inverse( void ){
utils_fast_sincos_better( virtual_motor.phi , (float*)&virtual_motor.sin_phi,
(float*)&virtual_motor.cos_phi );
// Park Inverse
virtual_motor.i_alpha = virtual_motor.cos_phi * virtual_motor.id -
virtual_motor.sin_phi * virtual_motor.iq;
@ -366,28 +414,20 @@ static inline void run_virtual_motor_park_clark_inverse( void ){
* connect_virtual_motor command
*/
static void terminal_cmd_connect_virtual_motor(int argc, const char **argv) {
if( argc == 8 ){
if( argc == 4 ){
float ml; //torque load in motor axis
float Ld; //inductance in d axis
float Lq; //inductance in q axis
float J; //rotor inertia
float Rs; //resistance of motor inductance
float lambda;//rotor flux linkage
float Vbus;//Bus voltage
sscanf(argv[1], "%f", &ml);
sscanf(argv[2], "%f", &J);
sscanf(argv[3], "%f", &Ld);
sscanf(argv[4], "%f", &Lq);
sscanf(argv[5], "%f", &Rs);
sscanf(argv[6], "%f", &lambda);
sscanf(argv[7], "%f", &Vbus);
sscanf(argv[3], "%f", &Vbus);
connect_virtual_motor( ml , J, Ld , Lq , Rs, lambda, Vbus);
connect_virtual_motor( ml , J, Vbus);
commands_printf("virtual motor connected");
}
else{
commands_printf("arguments should be 7" );
commands_printf("arguments should be 3" );
}
}

3
virtual_motor.h
View File

@ -23,7 +23,8 @@
#include "datatypes.h"
void virtual_motor_init(void);
void virtual_motor_init(volatile mc_configuration *conf);
void virtual_motor_set_configuration(volatile mc_configuration *conf);
void virtual_motor_int_handler(float v_alpha, float v_beta);
bool virtual_motor_is_connected(void);
float virtual_motor_get_angle_deg(void);