mawei
/
body
mirror of https://github.com/commaai/body.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
body/board/util.c

259 lines
8.4 KiB
C
Raw Blame History

#include <stdint.h>
#include "stm32f4xx_hal.h"
#include "defines.h"
#include "config.h"
#include "util.h"
#include "bldc/BLDC_controller.h"
#include "bldc/rtwtypes.h"
// TODO: simplify util.c to util.h
//------------------------------------------------------------------------
// Global variables set externally
//------------------------------------------------------------------------
extern int16_t batVoltage;
extern uint8_t buzzerCount; // global variable for the buzzer counts. can be 1, 2, 3, 4, 5, 6, 7...
extern uint8_t buzzerFreq; // global variable for the buzzer pitch. can be 1, 2, 3, 4, 5, 6, 7...
extern uint8_t buzzerPattern; // global variable for the buzzer pattern. can be 1, 2, 3, 4, 5, 6, 7...
extern uint8_t enable_motors; // global variable for motor enable
extern uint8_t ignition; // global variable for ignition on SBU2 line
extern uint8_t hw_type;
extern board_t board;
//------------------------------------------------------------------------
// Matlab defines - from auto-code generation
//---------------
RT_MODEL rtM_Left_; /* Real-time model */
RT_MODEL rtM_Right_; /* Real-time model */
RT_MODEL *const rtM_Left = &rtM_Left_;
RT_MODEL *const rtM_Right = &rtM_Right_;
extern P rtP_Left; /* Block parameters (auto storage) */
DW rtDW_Left; /* Observable states */
ExtU rtU_Left; /* External inputs */
ExtY rtY_Left; /* External outputs */
P rtP_Right; /* Block parameters (auto storage) */
DW rtDW_Right; /* Observable states */
ExtU rtU_Right; /* External inputs */
ExtY rtY_Right; /* External outputs */
//---------------
int16_t speedAvg; // average measured speed
int16_t speedAvgAbs; // average measured speed in absolute
uint8_t ctrlModReqRaw = CTRL_MOD_REQ;
uint8_t ctrlModReq = CTRL_MOD_REQ; // Final control mode request
void BLDC_Init(void) {
/* Set BLDC controller parameters */
rtP_Left.b_angleMeasEna = 0; // Motor angle input: 0 = estimated angle, 1 = measured angle (e.g. if encoder is available)
rtP_Left.z_selPhaCurMeasABC = 0; // Left motor measured current phases {Green, Blue} = {iA, iB} -> do NOT change
rtP_Left.z_ctrlTypSel = CTRL_TYP_SEL;
rtP_Left.b_diagEna = DIAG_ENA;
rtP_Left.i_max = (I_MOT_MAX * A2BIT_CONV) << 4; // fixdt(1,16,4)
rtP_Left.n_max = N_MOT_MAX << 4; // fixdt(1,16,4)
rtP_Left.b_fieldWeakEna = FIELD_WEAK_ENA;
rtP_Left.id_fieldWeakMax = (FIELD_WEAK_MAX * A2BIT_CONV) << 4; // fixdt(1,16,4)
rtP_Left.a_phaAdvMax = PHASE_ADV_MAX << 4; // fixdt(1,16,4)
rtP_Left.r_fieldWeakHi = FIELD_WEAK_HI << 4; // fixdt(1,16,4)
rtP_Left.r_fieldWeakLo = FIELD_WEAK_LO << 4; // fixdt(1,16,4)
rtP_Right = rtP_Left; // Copy the Left motor parameters to the Right motor parameters
rtP_Right.n_max = N_MOT_MAX << 4; // But add separate max RPM limit
rtP_Right.z_selPhaCurMeasABC = 1; // Right motor measured current phases {Blue, Yellow} = {iB, iC} -> do NOT change
/* Pack LEFT motor data into RTM */
rtM_Left->defaultParam = &rtP_Left;
rtM_Left->dwork = &rtDW_Left;
rtM_Left->inputs = &rtU_Left;
rtM_Left->outputs = &rtY_Left;
/* Pack RIGHT motor data into RTM */
rtM_Right->defaultParam = &rtP_Right;
rtM_Right->dwork = &rtDW_Right;
rtM_Right->inputs = &rtU_Right;
rtM_Right->outputs = &rtY_Right;
/* Initialize BLDC controllers */
BLDC_controller_initialize(rtM_Left);
BLDC_controller_initialize(rtM_Right);
}
void out_enable(uint8_t out, bool enabled) {
switch(out) {
case LED_GREEN:
HAL_GPIO_WritePin(board.led_portG, board.led_pinG, !enabled);
break;
case LED_RED:
HAL_GPIO_WritePin(board.led_portR, board.led_pinR, !enabled);
break;
case LED_BLUE:
HAL_GPIO_WritePin(board.led_portB, board.led_pinB, !enabled);
break;
case IGNITION:
HAL_GPIO_WritePin(board.ignition_port, board.ignition_pin, enabled);
break;
case POWERSWITCH:
HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, enabled);
break;
case TRANSCEIVER:
HAL_GPIO_WritePin(board.can_portEN, board.can_pinEN, !enabled);
break;
}
}
void poweronMelody(void) {
buzzerCount = 0;
for (int i = 8; i >= 0; i--) {
buzzerFreq = (uint8_t)i;
HAL_Delay(100);
}
buzzerFreq = 0;
}
void beepCount(uint8_t cnt, uint8_t freq, uint8_t pattern) {
buzzerCount = cnt;
buzzerFreq = freq;
buzzerPattern = pattern;
}
void beepLong(uint8_t freq) {
buzzerCount = 0;
buzzerFreq = freq;
HAL_Delay(500);
buzzerFreq = 0;
}
void beepShort(uint8_t freq) {
buzzerCount = 0;
buzzerFreq = freq;
HAL_Delay(100);
buzzerFreq = 0;
}
void beepShortMany(uint8_t cnt, int8_t dir) {
if (dir >= 0) { // increasing tone
for(uint8_t i = 2*cnt; i >= 2; i=i-2) {
beepShort(i + 3);
}
} else { // decreasing tone
for(uint8_t i = 2; i <= 2*cnt; i=i+2) {
beepShort(i + 3);
}
}
}
void calcAvgSpeed(void) {
// Calculate measured average speed. The minus sign (-) is because motors spin in opposite directions
speedAvg = ( rtY_Left.n_mot - rtY_Right.n_mot) / 2;
// Handle the case when SPEED_COEFFICIENT sign is negative (which is when most significant bit is 1)
if (SPEED_COEFFICIENT & (1 << 16)) {
speedAvg = -speedAvg;
}
speedAvgAbs = ABS(speedAvg);
}
void poweroff(void) {
enable_motors = 0;
buzzerCount = 0;
buzzerPattern = 0;
for (int i = 0; i < 8; i++) {
buzzerFreq = (uint8_t)i;
HAL_Delay(100);
}
out_enable(POWERSWITCH, false);
while(1) {
// Temporarily, to see that we went to power off but can't switch the latch
HAL_GPIO_TogglePin(board.led_portR, board.led_pinR);
HAL_Delay(100);
}
}
#define PULL_EFFECTIVE_DELAY 4096
uint8_t detect_with_pull(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin, uint32_t mode) {
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = GPIO_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = mode;
HAL_GPIO_Init(GPIOx, &GPIO_InitStruct);
for (volatile int i=0; i<PULL_EFFECTIVE_DELAY; i++);
bool ret = HAL_GPIO_ReadPin(GPIOx, GPIO_Pin);
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOx, &GPIO_InitStruct);
return ret;
}
uint8_t board_id(void) {
return (((!detect_with_pull(KEY1_PORT, KEY1_PIN, GPIO_PULLUP))) | (!detect_with_pull(KEY2_PORT, KEY2_PIN, GPIO_PULLUP)) << 1U);
}
uint8_t crc_checksum(uint8_t *dat, int len, const uint8_t poly) {
uint8_t crc = 0xFFU;
int i;
int j;
for (i = len - 1; i >= 0; i--) {
crc ^= dat[i];
for (j = 0; j < 8; j++) {
if ((crc & 0x80U) != 0U) {
crc = (uint8_t)((crc << 1) ^ poly);
}
else {
crc <<= 1;
}
}
}
return crc;
}
/* =========================== Filtering Functions =========================== */
/* Low pass filter fixed-point 32 bits: fixdt(1,32,16)
* Max: 32767.99998474121
* Min: -32768
* Res: 1.52587890625e-05
*
* Inputs: u = int16 or int32
* Outputs: y = fixdt(1,32,16)
* 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 >> 16); // the integer output is the fixed-point ouput shifted by 16 bits
*/
void filtLowPass32(int32_t u, uint16_t coef, int32_t *y) {
int64_t tmp;
tmp = ((int64_t)((u << 4) - (*y >> 12)) * coef) >> 4;
tmp = CLAMP(tmp, -2147483648LL, 2147483647LL); // Overflow protection: 2147483647LL = 2^31 - 1
*y = (int32_t)tmp + (*y);
}
/* rateLimiter16(int16_t u, int16_t rate, int16_t *y);
* Inputs: u = int16
* Outputs: y = fixdt(1,16,4)
* Parameters: rate = fixdt(1,16,4) = [0, 32767] Do NOT make rate negative (>32767)
*/
void rateLimiter16(int16_t u, int16_t rate, int16_t *y) {
int16_t q0;
int16_t q1;
q0 = (u << 4) - *y;
if (q0 > rate) {
q0 = rate;
} else {
q1 = -rate;
if (q0 < q1) {
q0 = q1;
}
}
*y = q0 + *y;
}
Reference in New Issue View Git Blame Copy Permalink