feedc0de/bobbycar-controller-firmware
1
0
Fork 0
Code Issues 8 Pull Requests 5 Actions Packages Projects Releases Wiki Activity

More improvements

Browse Source
This commit is contained in:
0xFEEDC0DE64
2021-05-23 15:51:29 +02:00
parent 58b308a2b1
commit 0e014ad6c2
1 changed files with 103 additions and 85 deletions
Download Patch File Download Diff File Expand all files Collapse all files

188
main.cpp
View File

@@ -45,6 +45,8 @@ extern const P rtP_Left; // default settings defined in BLDC_controller_data.c
} }
namespace { namespace {
const P &defaultP{rtP_Left};
TIM_HandleTypeDef htim_right; TIM_HandleTypeDef htim_right;
TIM_HandleTypeDef htim_left; TIM_HandleTypeDef htim_left;
ADC_HandleTypeDef hadc1; ADC_HandleTypeDef hadc1;
@@ -180,10 +182,6 @@ int16_t offsetdcl = 2000;
int16_t offsetdcr = 2000; int16_t offsetdcr = 2000;
int16_t batVoltage = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE; int16_t batVoltage = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE;
int32_t batVoltageFixdt = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE << 20; // Fixed-point filter output initialized at 400 V*100/cell = 4 V/cell converted to fixed-point
int32_t board_temp_adcFixdt = adc_buffer.temp << 20; // Fixed-point filter output initialized with current ADC converted to fixed-point
int16_t board_temp_adcFilt = adc_buffer.temp;
int16_t board_temp_deg_c; int16_t board_temp_deg_c;
struct { struct {
@@ -193,9 +191,9 @@ struct {
ExtU rtU; /* External inputs */ ExtU rtU; /* External inputs */
ExtY rtY; /* External outputs */ ExtY rtY; /* External outputs */
bool enable{true}; std::atomic<bool> enable{true};
int16_t iDcMax{17}; std::atomic<int16_t> iDcMax{7};
uint32_t chops{}; std::atomic<uint32_t> chops{};
uint8_t hallBits() const uint8_t hallBits() const
{ {
@@ -209,8 +207,6 @@ struct {
uint32_t timer = 0; uint32_t timer = 0;
} buzzer; } buzzer;
void filtLowPass32(int16_t u, uint16_t coef, int32_t *y);
void SystemClock_Config(); void SystemClock_Config();
#ifdef HUARN2 #ifdef HUARN2
@@ -256,6 +252,12 @@ void applyIncomingCanMessage();
void sendCanFeedback(); void sendCanFeedback();
#endif #endif
#ifdef FEATURE_BUTTON
void handleButton();
#endif
void updateSensors();
void applyDefaultSettings(); void applyDefaultSettings();
} // anonymous namespace } // anonymous namespace
@@ -296,7 +298,7 @@ int main()
enum { CurrentMeasAB, CurrentMeasBC, CurrentMeasAC }; enum { CurrentMeasAB, CurrentMeasBC, CurrentMeasAC };
left.rtP = rtP_Left; left.rtP = defaultP;
left.rtP.b_angleMeasEna = 0; left.rtP.b_angleMeasEna = 0;
#ifdef PETERS_PLATINE #ifdef PETERS_PLATINE
left.rtP.z_selPhaCurMeasABC = CurrentMeasBC; left.rtP.z_selPhaCurMeasABC = CurrentMeasBC;
@@ -308,7 +310,7 @@ int main()
left.rtP.r_fieldWeakHi = FIELD_WEAK_HI << 4; left.rtP.r_fieldWeakHi = FIELD_WEAK_HI << 4;
left.rtP.r_fieldWeakLo = FIELD_WEAK_LO << 4; left.rtP.r_fieldWeakLo = FIELD_WEAK_LO << 4;
right.rtP = rtP_Left; right.rtP = defaultP;
right.rtP.b_angleMeasEna = 0; right.rtP.b_angleMeasEna = 0;
right.rtP.z_selPhaCurMeasABC = CurrentMeasBC; right.rtP.z_selPhaCurMeasABC = CurrentMeasBC;
right.rtP.b_diagEna = DIAG_ENA; right.rtP.b_diagEna = DIAG_ENA;
@@ -361,7 +363,8 @@ int main()
while (true) while (true)
{ {
const auto DELAY_WITH_CAN_POLL = [](uint32_t Delay){ #ifdef FEATURE_CAN
constexpr auto DELAY_WITH_CAN_POLL = [](uint32_t Delay){
uint32_t tickstart = HAL_GetTick(); uint32_t tickstart = HAL_GetTick();
uint32_t wait = Delay; uint32_t wait = Delay;
@@ -373,14 +376,16 @@ int main()
while ((HAL_GetTick() - tickstart) < wait) while ((HAL_GetTick() - tickstart) < wait)
{ {
#ifdef FEATURE_CAN
applyIncomingCanMessage(); applyIncomingCanMessage();
#endif
} }
}; };
//DELAY_WITH_CAN_POLL(5); //delay in ms
#endif
HAL_Delay(5); //delay in ms HAL_Delay(5); //delay in ms
updateSensors();
#ifdef MOTOR_TEST #ifdef MOTOR_TEST
doMotorTest(); doMotorTest();
#endif #endif
@@ -400,29 +405,9 @@ int main()
#endif #endif
#ifdef FEATURE_BUTTON #ifdef FEATURE_BUTTON
if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) handleButton();
{
left.enable = false;
right.enable = false;
while (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {} // wait until button is released
if(__HAL_RCC_GET_FLAG(RCC_FLAG_SFTRST)) { // do not power off after software reset (from a programmer/debugger)
__HAL_RCC_CLEAR_RESET_FLAGS(); // clear reset flags
} else {
poweroff(); // release power-latch
}
}
#endif #endif
// ####### CALC BOARD TEMPERATURE #######
filtLowPass32(adc_buffer.temp, TEMP_FILT_COEF, &board_temp_adcFixdt);
board_temp_adcFilt = (int16_t)(board_temp_adcFixdt >> 20); // convert fixed-point to integer
board_temp_deg_c = (TEMP_CAL_HIGH_DEG_C - TEMP_CAL_LOW_DEG_C) * (board_temp_adcFilt - TEMP_CAL_LOW_ADC) / (TEMP_CAL_HIGH_ADC - TEMP_CAL_LOW_ADC) + TEMP_CAL_LOW_DEG_C;
filtLowPass32(adc_buffer.batt1, BAT_FILT_COEF, &batVoltageFixdt);
batVoltage = (int16_t)(batVoltageFixdt >> 20); // convert fixed-point to integer
main_loop_counter++; main_loop_counter++;
} }
} }
@@ -480,24 +465,26 @@ void updateMotors()
#endif #endif
int16_t curR_DC = (int16_t)(offsetdcr - adc_buffer.dcr); int16_t curR_DC = (int16_t)(offsetdcr - adc_buffer.dcr);
const bool chopL = std::abs(curL_DC) > (left.iDcMax * A2BIT_CONV); const bool chopL = std::abs(curL_DC) > (left.iDcMax.load() * A2BIT_CONV);
if (chopL) if (chopL)
left.chops++; left.chops++;
const bool chopR = std::abs(curR_DC) > (right.iDcMax * A2BIT_CONV); const bool chopR = std::abs(curR_DC) > (right.iDcMax.load() * A2BIT_CONV);
if (chopR) if (chopR)
right.chops++; right.chops++;
const uint32_t timeoutVal = ++timeout; const uint32_t timeoutVal = ++timeout;
const bool leftEnable = left.enable.load();
const bool rightEnable = right.enable.load();
// Disable PWM when current limit is reached (current chopping) // Disable PWM when current limit is reached (current chopping)
// This is the Level 2 of current protection. The Level 1 should kick in first given by I_MOT_MAX // This is the Level 2 of current protection. The Level 1 should kick in first given by I_MOT_MAX
if (chopL || timeoutVal > 500 || !left.enable) if (chopL || timeoutVal > 500 || !leftEnable)
LEFT_TIM->BDTR &= ~TIM_BDTR_MOE; LEFT_TIM->BDTR &= ~TIM_BDTR_MOE;
else else
LEFT_TIM->BDTR |= TIM_BDTR_MOE; LEFT_TIM->BDTR |= TIM_BDTR_MOE;
if (chopR || timeoutVal > 500 || !right.enable) if (chopR || timeoutVal > 500 || !rightEnable)
RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE; RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE;
else else
RIGHT_TIM->BDTR |= TIM_BDTR_MOE; RIGHT_TIM->BDTR |= TIM_BDTR_MOE;
@@ -524,8 +511,8 @@ void updateMotors()
#endif #endif
; ;
const bool enableLFin = left.enable && left.rtY.z_errCode == 0 && (right.rtY.z_errCode == 0 || ignoreOtherMotor); const bool enableLFin = leftEnable && left.rtY.z_errCode == 0 && (right.rtY.z_errCode == 0 || ignoreOtherMotor);
const bool enableRFin = right.enable && (left.rtY.z_errCode == 0 || ignoreOtherMotor) && right.rtY.z_errCode == 0; const bool enableRFin = rightEnable && (left.rtY.z_errCode == 0 || ignoreOtherMotor) && right.rtY.z_errCode == 0;
// ========================= LEFT MOTOR ============================ // ========================= LEFT MOTOR ============================
// Get hall sensors values // Get hall sensors values
@@ -590,30 +577,6 @@ void updateMotors()
OverrunFlag = false; OverrunFlag = false;
} }
// ===========================================================
/* Low pass filter fixed-point 32 bits: fixdt(1,32,20)
* Max: 2047.9375
* Min: -2048
* Res: 0.0625
*
* Inputs: u = int16
* Outputs: y = fixdt(1,32,20)
* Parameters: coef = fixdt(0,16,16) = [0,65535U]
*
* Example:
* If coef = 0.8 (in floating point), then coef = 0.8 * 2^16 = 52429 (in fixed-point)
* filtLowPass16(u, 52429, &y);
* yint = (int16_t)(y >> 20); // the integer output is the fixed-point ouput shifted by 20 bits
*/
void filtLowPass32(int16_t u, uint16_t coef, int32_t *y)
{
int tmp;
tmp = (int16_t)(u << 4) - (*y >> 16);
tmp = std::clamp(tmp, -32768, 32767); // Overflow protection
*y = coef * tmp + (*y);
}
// =========================================================== // ===========================================================
/** System Clock Configuration /** System Clock Configuration
@@ -1620,6 +1583,10 @@ void applyIncomingCanMessage()
case MotorController<isBackBoard, true>::Command::FieldWeakMax: right.rtP.id_fieldWeakMax = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; case MotorController<isBackBoard, true>::Command::FieldWeakMax: right.rtP.id_fieldWeakMax = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break;
case MotorController<isBackBoard, false>::Command::PhaseAdvMax: left.rtP.a_phaAdvMax = *((uint16_t*)buf) << 4; break; case MotorController<isBackBoard, false>::Command::PhaseAdvMax: left.rtP.a_phaAdvMax = *((uint16_t*)buf) << 4; break;
case MotorController<isBackBoard, true>::Command::PhaseAdvMax: right.rtP.a_phaAdvMax = *((uint16_t*)buf) << 4; break; case MotorController<isBackBoard, true>::Command::PhaseAdvMax: right.rtP.a_phaAdvMax = *((uint16_t*)buf) << 4; break;
case MotorController<isBackBoard, false>::Command::CruiseCtrlEna: left.rtP.b_cruiseCtrlEna = *((bool*)buf); break;
case MotorController<isBackBoard, true>::Command::CruiseCtrlEna: right.rtP.b_cruiseCtrlEna = *((bool*)buf); break;
case MotorController<isBackBoard, false>::Command::CruiseMotTgt: left.rtP.n_cruiseMotTgt = *((int16_T*)buf); break;
case MotorController<isBackBoard, true>::Command::CruiseMotTgt: right.rtP.n_cruiseMotTgt = *((int16_T*)buf); break;
case MotorController<isBackBoard, false>::Command::BuzzerFreq: buzzer.freq = *((uint8_t*)buf); break; case MotorController<isBackBoard, false>::Command::BuzzerFreq: buzzer.freq = *((uint8_t*)buf); break;
case MotorController<isBackBoard, true>::Command::BuzzerFreq: buzzer.freq = *((uint8_t*)buf); break; case MotorController<isBackBoard, true>::Command::BuzzerFreq: buzzer.freq = *((uint8_t*)buf); break;
case MotorController<isBackBoard, false>::Command::BuzzerPattern: buzzer.pattern = *((uint8_t*)buf); break; case MotorController<isBackBoard, false>::Command::BuzzerPattern: buzzer.pattern = *((uint8_t*)buf); break;
@@ -1709,8 +1676,8 @@ void sendCanFeedback()
case 11: send(MotorController<isBackBoard, true>::Feedback::DcPhaB, right.rtY.DC_phaB); break; case 11: send(MotorController<isBackBoard, true>::Feedback::DcPhaB, right.rtY.DC_phaB); break;
case 12: send(MotorController<isBackBoard, false>::Feedback::DcPhaC, left. rtY.DC_phaC); break; case 12: send(MotorController<isBackBoard, false>::Feedback::DcPhaC, left. rtY.DC_phaC); break;
case 13: send(MotorController<isBackBoard, true>::Feedback::DcPhaC, right.rtY.DC_phaC); break; case 13: send(MotorController<isBackBoard, true>::Feedback::DcPhaC, right.rtY.DC_phaC); break;
case 14: send(MotorController<isBackBoard, false>::Feedback::Chops, left. chops); break; case 14: send(MotorController<isBackBoard, false>::Feedback::Chops, left. chops.exchange(0)); break;
case 15: send(MotorController<isBackBoard, true>::Feedback::Chops, right.chops); break; case 15: send(MotorController<isBackBoard, true>::Feedback::Chops, right.chops.exchange(0)); break;
case 16: send(MotorController<isBackBoard, false>::Feedback::Hall, left.hallBits()); break; case 16: send(MotorController<isBackBoard, false>::Feedback::Hall, left.hallBits()); break;
case 17: send(MotorController<isBackBoard, true>::Feedback::Hall, right.hallBits()); break; case 17: send(MotorController<isBackBoard, true>::Feedback::Hall, right.hallBits()); break;
case 18: send(MotorController<isBackBoard, false>::Feedback::Voltage, batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); break; case 18: send(MotorController<isBackBoard, false>::Feedback::Voltage, batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); break;
@@ -1722,27 +1689,78 @@ void sendCanFeedback()
} }
#endif #endif
#ifdef FEATURE_BUTTON
void handleButton()
{
if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN))
{
left.enable = false;
right.enable = false;
while (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {} // wait until button is released
if(__HAL_RCC_GET_FLAG(RCC_FLAG_SFTRST)) { // do not power off after software reset (from a programmer/debugger)
__HAL_RCC_CLEAR_RESET_FLAGS(); // clear reset flags
} else {
poweroff(); // release power-latch
}
}
}
#endif
void updateSensors()
{
/* Low pass filter fixed-point 32 bits: fixdt(1,32,20)
* Max: 2047.9375
* Min: -2048
* Res: 0.0625
*
* Inputs: u = int16
* Outputs: y = fixdt(1,32,20)
* Parameters: coef = fixdt(0,16,16) = [0,65535U]
*
* Example:
* If coef = 0.8 (in floating point), then coef = 0.8 * 2^16 = 52429 (in fixed-point)
* filtLowPass16(u, 52429, &y);
* yint = (int16_t)(y >> 20); // the integer output is the fixed-point ouput shifted by 20 bits
*/
constexpr auto filtLowPass32 = [](int16_t u, uint16_t coef, int32_t &y)
{
int tmp = (int16_t)(u << 4) - (y >> 16);
tmp = std::clamp(tmp, -32768, 32767); // Overflow protection
y = coef * tmp + y;
};
// Fixed-point filter output initialized with current ADC converted to fixed-point
static int32_t board_temp_adcFixdt{} /*= adc_buffer.temp << 20*/;
filtLowPass32(adc_buffer.temp, TEMP_FILT_COEF, board_temp_adcFixdt);
int16_t board_temp_adcFilt = (int16_t)(board_temp_adcFixdt >> 20); // convert fixed-point to integer
board_temp_deg_c = (TEMP_CAL_HIGH_DEG_C - TEMP_CAL_LOW_DEG_C) * (board_temp_adcFilt - TEMP_CAL_LOW_ADC) / (TEMP_CAL_HIGH_ADC - TEMP_CAL_LOW_ADC) + TEMP_CAL_LOW_DEG_C;
// Fixed-point filter output initialized at 400 V*100/cell = 4 V/cell converted to fixed-point
static int32_t batVoltageFixdt = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE << 20;
filtLowPass32(adc_buffer.batt1, BAT_FILT_COEF, batVoltageFixdt);
batVoltage = (int16_t)(batVoltageFixdt >> 20); // convert fixed-point to integer
}
void applyDefaultSettings() void applyDefaultSettings()
{ {
left.enable = true; constexpr auto doIt = [](auto &motor){
left.rtU.r_inpTgt = 0; motor.enable = true;
left.rtP.z_ctrlTypSel = uint8_t(ControlType::FieldOrientedControl); motor.rtU.r_inpTgt = 0;
left.rtU.z_ctrlModReq = uint8_t(ControlMode::OpenMode); motor.rtP.z_ctrlTypSel = uint8_t(ControlType::FieldOrientedControl);
left.rtP.i_max = (5 * A2BIT_CONV) << 4; motor.rtU.z_ctrlModReq = uint8_t(ControlMode::OpenMode);
left.iDcMax = 7; motor.rtP.i_max = (5 * A2BIT_CONV) << 4;
left.rtP.n_max = 1000 << 4; motor.iDcMax = 7;
left.rtP.id_fieldWeakMax = (5 * A2BIT_CONV) << 4; motor.rtP.n_max = 1000 << 4;
left.rtP.a_phaAdvMax = 40 << 4; motor.rtP.id_fieldWeakMax = (1 * A2BIT_CONV) << 4;
motor.rtP.a_phaAdvMax = 40 << 4;
motor.rtP.b_cruiseCtrlEna = false;
motor.rtP.n_cruiseMotTgt = 0;
};
right.enable = true; doIt(left);
right.rtU.r_inpTgt = 0; doIt(right);
right.rtP.z_ctrlTypSel = uint8_t(ControlType::FieldOrientedControl);
right.rtU.z_ctrlModReq = uint8_t(ControlMode::OpenMode);
right.rtP.i_max = (5 * A2BIT_CONV) << 4;
right.iDcMax = 7;
right.rtP.n_max = 1000 << 4;
right.rtP.id_fieldWeakMax = (5 * A2BIT_CONV) << 4;
right.rtP.a_phaAdvMax = 40 << 4;
} }
} // anonymous namespace } // anonymous namespace