From 925001b8efec8156ea393029bac6802b595f8eb1 Mon Sep 17 00:00:00 2001 From: 0xFEEDC0DE64 Date: Sat, 15 May 2021 01:36:41 +0200 Subject: [PATCH 01/15] First tries with can --- CMakeLists.txt | 7 +- main.cpp | 342 ++++++++++++++++++++++++++++++++++++++++++- stm32f1xx_hal_conf.h | 4 +- 3 files changed, 347 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index da25ab4..d4463da 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -40,8 +40,10 @@ add_definitions(-DFEATURE_IGNORE_OTHER_MOTOR) add_definitions(-DPETERS_PLATINE) add_definitions(-DHUARN2) #add_definitions(-DHUARN3) -add_definitions(-DFEATURE_SERIAL_CONTROL) -add_definitions(-DFEATURE_SERIAL_FEEDBACK) +#add_definitions(-DFEATURE_SERIAL_CONTROL) +#add_definitions(-DFEATURE_SERIAL_FEEDBACK) +add_definitions(-DLOG_TO_SERIAL) +add_definitions(-DFEATURE_CAN) add_executable(firmware.elf STM32CubeF1/Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal.c @@ -59,6 +61,7 @@ add_executable(firmware.elf STM32CubeF1/Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_adc.c STM32CubeF1/Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_uart.c STM32CubeF1/Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_dma.c + STM32CubeF1/Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_can.c bobbycar-foc-model/BLDC_controller.h bobbycar-foc-model/BLDC_controller.c diff --git a/main.cpp b/main.cpp index bbc2c29..952bda3 100644 --- a/main.cpp +++ b/main.cpp @@ -57,6 +57,69 @@ DMA_HandleTypeDef hdma_usart3_rx; DMA_HandleTypeDef hdma_usart3_tx; #endif +#ifdef FEATURE_CAN +CAN_HandleTypeDef CanHandle; + +/* Definition for CANx clock resources */ +#define CANx CAN1 +#define CANx_CLK_ENABLE() __HAL_RCC_CAN1_CLK_ENABLE() +#define CANx_GPIO_CLK_ENABLE() __HAL_RCC_GPIOB_CLK_ENABLE() + +#define CANx_FORCE_RESET() __HAL_RCC_CAN1_FORCE_RESET() +#define CANx_RELEASE_RESET() __HAL_RCC_CAN1_RELEASE_RESET() + +/* Definition for CANx Pins */ +#define CANx_TX_PIN GPIO_PIN_9 +#define CANx_TX_GPIO_PORT GPIOB +#define CANx_RX_PIN GPIO_PIN_8 +#define CANx_RX_GPIO_PORT GPIOB + +/* Definition for CANx AFIO Remap */ +#define CANx_AFIO_REMAP_CLK_ENABLE() __HAL_RCC_AFIO_CLK_ENABLE() +#define CANx_AFIO_REMAP_RX_TX_PIN() __HAL_AFIO_REMAP_CAN1_2() + +/* Definition for CAN's NVIC */ +#define CANx_RX_IRQn USB_LP_CAN1_RX0_IRQn +#define CANx_RX_IRQHandler USB_LP_CAN1_RX0_IRQHandler +#define CANx_TX_IRQn USB_HP_CAN1_TX_IRQn +#define CANx_TX_IRQHandler USB_HP_CAN1_TX_IRQHandler +#endif + +#ifdef LOG_TO_SERIAL +char logBuffer[512]; +#endif + + +template +void myPrintf(const char (&format)[formatLength], Targs ... args) +{ +#ifdef LOG_TO_SERIAL +#ifdef HUARN2 +#define UART_DMA_CHANNEL DMA1_Channel7 +#endif +#ifdef HUARN3 +#define UART_DMA_CHANNEL DMA1_Channel2 +#endif + + while (UART_DMA_CHANNEL->CNDTR != 0); + + char processedFormat[formatLength+2]; + std::copy(std::begin(format), std::end(format), std::begin(processedFormat)); + processedFormat[formatLength-1] = '\r'; + processedFormat[formatLength] = '\n'; + processedFormat[formatLength+1] = '\0'; + + const auto size = std::snprintf(logBuffer, sizeof(logBuffer), processedFormat, args ...); + if (size < 0) + return; + + UART_DMA_CHANNEL->CCR &= ~DMA_CCR_EN; + UART_DMA_CHANNEL->CNDTR = size; + UART_DMA_CHANNEL->CMAR = uint64_t(logBuffer); + UART_DMA_CHANNEL->CCR |= DMA_CCR_EN; +#endif +} + volatile struct { uint16_t dcr; uint16_t dcl; @@ -127,6 +190,10 @@ void UART2_Init(); void UART3_Init(); #endif +#ifdef FEATURE_CAN +void CAN_Init(); +#endif + void MX_GPIO_Init(); void MX_TIM_Init(); @@ -145,6 +212,10 @@ void parseCommand(); void sendFeedback(); #endif +#ifdef FEATURE_CAN +void sendCanFeedback(); +#endif + } // anonymous namespace int main() @@ -241,6 +312,10 @@ int main() UART3_Init(); #endif +#ifdef FEATURE_CAN + CAN_Init(); +#endif + #ifdef MOTOR_TEST int pwm = 0; int8_t dir = 1; @@ -296,10 +371,15 @@ int main() 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; #ifdef FEATURE_SERIAL_FEEDBACK - if (main_loop_counter % 50 != 0) // Send data periodically + if (main_loop_counter % 50 == 0) // Send data periodically sendFeedback(); #endif +#ifdef FEATURE_CAN + if (main_loop_counter % 100 == 0) + sendCanFeedback(); +#endif + #ifdef FEATURE_BUTTON if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { @@ -736,6 +816,222 @@ void UART3_Init() } #endif +#ifdef FEATURE_CAN +void CAN_MspInit(CAN_HandleTypeDef *hcan); +void CAN_MspDeInit(CAN_HandleTypeDef *hcan); +void CAN_MspDeInit(CAN_HandleTypeDef *hcan); +void CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *CanHandle); +void CAN_TxMailboxCompleteCallback(CAN_HandleTypeDef *hcan); + +void CAN_Init() +{ + myPrintf("CAN_Init() called"); + + CAN_FilterTypeDef sFilterConfig; + + /* Configure the CAN peripheral */ + CanHandle.Instance = CANx; + + CanHandle.MspInitCallback = CAN_MspInit; + CanHandle.MspDeInitCallback = CAN_MspDeInit; + + CanHandle.Init.TimeTriggeredMode = DISABLE; + CanHandle.Init.AutoBusOff = ENABLE; + CanHandle.Init.AutoWakeUp = DISABLE; + CanHandle.Init.AutoRetransmission = ENABLE; + CanHandle.Init.ReceiveFifoLocked = DISABLE; + CanHandle.Init.TransmitFifoPriority = DISABLE; + CanHandle.Init.Mode = CAN_MODE_NORMAL; + CanHandle.Init.SyncJumpWidth = CAN_SJW_1TQ; + CanHandle.Init.TimeSeg1 = CAN_BS1_6TQ; + CanHandle.Init.TimeSeg2 = CAN_BS2_1TQ; + CanHandle.Init.Prescaler = 4; + + if (const auto result = HAL_CAN_Init(&CanHandle); result == HAL_OK) + myPrintf("HAL_CAN_Init() succeeded"); + else + { + myPrintf("HAL_CAN_Init() failed with %i", result); + while (true); + } + + /* Configure the CAN Filter */ + sFilterConfig.FilterBank = 0; + sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; + sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT; + sFilterConfig.FilterIdHigh = 0x0000; // TODO + sFilterConfig.FilterIdLow = 0b11111111111; // TODO + sFilterConfig.FilterMaskIdHigh = 0x0000; // TODO + sFilterConfig.FilterMaskIdLow = 0b11111111111; // TODO + sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0; + sFilterConfig.FilterActivation = ENABLE; + sFilterConfig.SlaveStartFilterBank = 14; + + if (const auto result = HAL_CAN_ConfigFilter(&CanHandle, &sFilterConfig); result == HAL_OK) + myPrintf("HAL_CAN_ConfigFilter() succeeded"); + else + { + myPrintf("HAL_CAN_ConfigFilter() failed with %i", result); + while (true); + } + + if (const auto result = HAL_CAN_RegisterCallback(&CanHandle, HAL_CAN_RX_FIFO0_MSG_PENDING_CB_ID, CAN_RxFifo0MsgPendingCallback); result == HAL_OK) + myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_RX_FIFO0_MSG_PENDING_CB_ID succeeded"); + else + { + myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_RX_FIFO0_MSG_PENDING_CB_ID failed with %i", result); + while (true); + } + + if (const auto result = HAL_CAN_RegisterCallback(&CanHandle, HAL_CAN_TX_MAILBOX0_COMPLETE_CB_ID, CAN_TxMailboxCompleteCallback); result == HAL_OK) + myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX0_COMPLETE_CB_ID succeeded"); + else + { + myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX0_COMPLETE_CB_ID failed with %i", result); + while (true); + } + + if (const auto result = HAL_CAN_RegisterCallback(&CanHandle, HAL_CAN_TX_MAILBOX1_COMPLETE_CB_ID, CAN_TxMailboxCompleteCallback); result == HAL_OK) + myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX1_COMPLETE_CB_ID succeeded"); + else + { + myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX1_COMPLETE_CB_ID failed with %i", result); + while (true); + } + + if (const auto result = HAL_CAN_RegisterCallback(&CanHandle, HAL_CAN_TX_MAILBOX2_COMPLETE_CB_ID, CAN_TxMailboxCompleteCallback); result == HAL_OK) + myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX2_COMPLETE_CB_ID succeeded"); + else + { + myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX2_COMPLETE_CB_ID failed with %i", result); + while (true); + } + + /* Start the CAN peripheral */ + if (const auto result = HAL_CAN_Start(&CanHandle); result == HAL_OK) + myPrintf("HAL_CAN_Start() succeeded"); + else + { + myPrintf("HAL_CAN_Start() failed with %i", result); + while (true); + } + + /* Activate CAN RX notification */ + if (const auto result = HAL_CAN_ActivateNotification(&CanHandle, CAN_IT_RX_FIFO0_MSG_PENDING); result == HAL_OK) + myPrintf("HAL_CAN_ActivateNotification() CAN_IT_RX_FIFO0_MSG_PENDING succeeded"); + else + { + myPrintf("HAL_CAN_ActivateNotification() CAN_IT_RX_FIFO0_MSG_PENDING failed with %i", result); + while (true); + } + + /* Activate CAN TX notification */ + if (const auto result = HAL_CAN_ActivateNotification(&CanHandle, CAN_IT_TX_MAILBOX_EMPTY); result == HAL_OK) + myPrintf("HAL_CAN_ActivateNotification() CAN_IT_TX_MAILBOX_EMPTY succeeded"); + else + { + myPrintf("HAL_CAN_ActivateNotification() CAN_IT_TX_MAILBOX_EMPTY failed with %i", result); + while (true); + } +} + +void CAN_MspInit(CAN_HandleTypeDef *hcan) +{ + myPrintf("CAN_MspInit() called"); + + GPIO_InitTypeDef GPIO_InitStruct; + + /*##-1- Enable peripherals and GPIO Clocks #################################*/ + /* CAN1 Periph clock enable */ + CANx_CLK_ENABLE(); + /* Enable GPIO clock ****************************************/ + CANx_GPIO_CLK_ENABLE(); + /* Enable AFIO clock and Remap CAN PINs to PB8 and PB9*******/ + CANx_AFIO_REMAP_CLK_ENABLE(); + CANx_AFIO_REMAP_RX_TX_PIN(); + + /*##-2- Configure peripheral GPIO ##########################################*/ + /* CAN1 TX GPIO pin configuration */ + GPIO_InitStruct.Pin = CANx_TX_PIN; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; + GPIO_InitStruct.Pull = GPIO_PULLUP; + + HAL_GPIO_Init(CANx_TX_GPIO_PORT, &GPIO_InitStruct); + + /* CAN1 RX GPIO pin configuration */ + GPIO_InitStruct.Pin = CANx_RX_PIN; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; + GPIO_InitStruct.Pull = GPIO_PULLUP; + + HAL_GPIO_Init(CANx_RX_GPIO_PORT, &GPIO_InitStruct); + + /*##-3- Configure the NVIC #################################################*/ + /* NVIC configuration for CAN1 Reception complete interrupt */ + HAL_NVIC_SetPriority(CANx_RX_IRQn, 1, 0); + HAL_NVIC_EnableIRQ(CANx_RX_IRQn); + + HAL_NVIC_SetPriority(CANx_TX_IRQn, 1, 0); + HAL_NVIC_EnableIRQ(CANx_TX_IRQn); +} + +void CAN_MspDeInit(CAN_HandleTypeDef *hcan) +{ + myPrintf("CAN_MspDeInit() called"); + + /*##-1- Reset peripherals ##################################################*/ + CANx_FORCE_RESET(); + CANx_RELEASE_RESET(); + + /*##-2- Disable peripherals and GPIO Clocks ################################*/ + /* De-initialize the CAN1 TX GPIO pin */ + HAL_GPIO_DeInit(CANx_TX_GPIO_PORT, CANx_TX_PIN); + /* De-initialize the CAN1 RX GPIO pin */ + HAL_GPIO_DeInit(CANx_RX_GPIO_PORT, CANx_RX_PIN); + + /*##-4- Disable the NVIC for CAN reception #################################*/ + HAL_NVIC_DisableIRQ(CANx_RX_IRQn); +} + +void CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *CanHandle) +{ + myPrintf("CAN_RxFifo0MsgPendingCallback() called"); + + CAN_RxHeaderTypeDef header; + uint8_t buf[8]; + if (const auto result = HAL_CAN_GetRxMessage(CanHandle, CAN_RX_FIFO0, &header, buf); result != HAL_OK) + { + myPrintf("HAL_CAN_GetRxMessage() failed with %i", result); + while (true); + } + + //if (header.IDE == CAN_ID_STD && + // (header.StdId == CAN_ID_COMMAND_STW_TO_BOARD(BOARD_INDEX) || + // header.StdId == CAN_ID_FEEDBACK_STW_TO_BOARD(BOARD_INDEX))) + //{ + // can_feedc0de_handle_frame(header.StdId, RxData, dlc_to_len(header.DLC)); + //} + + // slightly yucky, but we don't want to block inside the IRQ handler + //if (HAL_CAN_GetTxMailboxesFreeLevel(CanHandle) >= 2) + //{ + // can_feedc0de_poll(); + //} +} + +void CAN_TxMailboxCompleteCallback(CAN_HandleTypeDef *hcan) +{ + myPrintf("CAN_TxMailboxCompleteCallback() called"); + + // slightly yucky, but we don't want to block inside the IRQ handler + //if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) >= 2) + //{ + // can_feedc0de_poll(); + //} +} +#endif + void MX_GPIO_Init() { GPIO_InitTypeDef GPIO_InitStruct; @@ -1138,7 +1434,7 @@ void parseCommand() left.state = right.state = {.enable=true}; - buzzer.state = { 24, 1 }; + //buzzer.state = { 24, 1 }; HAL_GPIO_WritePin(LED_PORT, LED_PIN, GPIO_PIN_RESET); @@ -1211,6 +1507,36 @@ void sendFeedback() } #endif +#ifdef FEATURE_CAN +void sendCanFeedback() +{ + const auto free = HAL_CAN_GetTxMailboxesFreeLevel(&CanHandle); + myPrintf("sendCanFeedback() called (free=%u)", free); + + if (free < 1) + return; + + CAN_TxHeaderTypeDef TxHeader; + TxHeader.StdId = 0x321; + TxHeader.ExtId = 0x01; + TxHeader.RTR = CAN_RTR_DATA; + TxHeader.IDE = CAN_ID_STD; + TxHeader.DLC = 2; + TxHeader.TransmitGlobalTime = DISABLE; + + uint8_t buf[8]; + + static uint32_t TxMailbox; + if (const auto result = HAL_CAN_AddTxMessage(&CanHandle, &TxHeader, buf, &TxMailbox); result == HAL_OK) + myPrintf("HAL_CAN_AddTxMessage() succeeded"); + else + { + myPrintf("HAL_CAN_AddTxMessage() failed with %i", result); + while (true); + } +} +#endif + } // anonymous namespace /******************************************************************************/ @@ -1409,3 +1735,15 @@ extern "C" void DMA1_Channel3_IRQHandler() /* USER CODE END DMA1_Channel3_IRQn 1 */ } #endif + +#ifdef FEATURE_CAN +extern "C" void CANx_RX_IRQHandler(void) +{ + HAL_CAN_IRQHandler(&CanHandle); +} + +extern "C" void CANx_TX_IRQHandler(void) +{ + HAL_CAN_IRQHandler(&CanHandle); +} +#endif diff --git a/stm32f1xx_hal_conf.h b/stm32f1xx_hal_conf.h index 42311da..118bb67 100644 --- a/stm32f1xx_hal_conf.h +++ b/stm32f1xx_hal_conf.h @@ -36,7 +36,7 @@ extern "C" { */ #define HAL_MODULE_ENABLED #define HAL_ADC_MODULE_ENABLED -/* #define HAL_CAN_MODULE_ENABLED */ +#define HAL_CAN_MODULE_ENABLED /* #define HAL_CAN_LEGACY_MODULE_ENABLED */ /* #define HAL_CEC_MODULE_ENABLED */ #define HAL_CORTEX_MODULE_ENABLED @@ -129,7 +129,7 @@ extern "C" { #define PREFETCH_ENABLE 1U #define USE_HAL_ADC_REGISTER_CALLBACKS 0U /* ADC register callback disabled */ -#define USE_HAL_CAN_REGISTER_CALLBACKS 0U /* CAN register callback disabled */ +#define USE_HAL_CAN_REGISTER_CALLBACKS 1 /* CAN register callback disabled */ #define USE_HAL_CEC_REGISTER_CALLBACKS 0U /* CEC register callback disabled */ #define USE_HAL_DAC_REGISTER_CALLBACKS 0U /* DAC register callback disabled */ #define USE_HAL_ETH_REGISTER_CALLBACKS 0U /* ETH register callback disabled */ -- 2.50.1 From b799f5110fb07a2079b202e15a3b3990aafb5ed2 Mon Sep 17 00:00:00 2001 From: 0xFEEDC0DE64 Date: Sun, 16 May 2021 20:09:24 +0200 Subject: [PATCH 02/15] More implementations --- main.cpp | 145 +++++++++++++++++++++++++++++++++++++++++++------------ 1 file changed, 114 insertions(+), 31 deletions(-) diff --git a/main.cpp b/main.cpp index 952bda3..ed77a87 100644 --- a/main.cpp +++ b/main.cpp @@ -23,6 +23,7 @@ #include #include +#include #include "stm32f1xx_hal.h" @@ -174,8 +175,6 @@ struct { } buzzer; Command command; -Feedback feedback; - void filtLowPass32(int16_t u, uint16_t coef, int32_t *y); @@ -842,10 +841,11 @@ void CAN_Init() CanHandle.Init.ReceiveFifoLocked = DISABLE; CanHandle.Init.TransmitFifoPriority = DISABLE; CanHandle.Init.Mode = CAN_MODE_NORMAL; + CanHandle.Init.SyncJumpWidth = CAN_SJW_1TQ; - CanHandle.Init.TimeSeg1 = CAN_BS1_6TQ; - CanHandle.Init.TimeSeg2 = CAN_BS2_1TQ; - CanHandle.Init.Prescaler = 4; + CanHandle.Init.TimeSeg1 = CAN_BS1_3TQ; + CanHandle.Init.TimeSeg2 = CAN_BS2_4TQ; + CanHandle.Init.Prescaler = 16; if (const auto result = HAL_CAN_Init(&CanHandle); result == HAL_OK) myPrintf("HAL_CAN_Init() succeeded"); @@ -860,9 +860,9 @@ void CAN_Init() sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT; sFilterConfig.FilterIdHigh = 0x0000; // TODO - sFilterConfig.FilterIdLow = 0b11111111111; // TODO + sFilterConfig.FilterIdLow = 0x0000; // TODO sFilterConfig.FilterMaskIdHigh = 0x0000; // TODO - sFilterConfig.FilterMaskIdLow = 0b11111111111; // TODO + sFilterConfig.FilterMaskIdLow = 0x0000; // TODO sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0; sFilterConfig.FilterActivation = ENABLE; sFilterConfig.SlaveStartFilterBank = 14; @@ -1000,7 +1000,18 @@ void CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *CanHandle) CAN_RxHeaderTypeDef header; uint8_t buf[8]; - if (const auto result = HAL_CAN_GetRxMessage(CanHandle, CAN_RX_FIFO0, &header, buf); result != HAL_OK) + if (const auto result = HAL_CAN_GetRxMessage(CanHandle, CAN_RX_FIFO0, &header, buf); result == HAL_OK) + { + myPrintf("HAL_CAN_GetRxMessage() succeeded"); + myPrintf("StdId = %x %u", header.StdId, header.StdId); + myPrintf("ExtId = %x %u", header.ExtId, header.ExtId); + myPrintf("IDE = %x %u", header.IDE, header.IDE); + myPrintf("RTR = %x %u", header.RTR, header.RTR); + myPrintf("DLC = %x %u", header.DLC, header.DLC); + myPrintf("Timestamp = %x %u", header.Timestamp, header.Timestamp); + myPrintf("FilterMatchIndex = %x %u", header.FilterMatchIndex, header.FilterMatchIndex); + } + else { myPrintf("HAL_CAN_GetRxMessage() failed with %i", result); while (true); @@ -1022,7 +1033,7 @@ void CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *CanHandle) void CAN_TxMailboxCompleteCallback(CAN_HandleTypeDef *hcan) { - myPrintf("CAN_TxMailboxCompleteCallback() called"); + //myPrintf("CAN_TxMailboxCompleteCallback() called"); // slightly yucky, but we don't want to block inside the IRQ handler //if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) >= 2) @@ -1468,6 +1479,8 @@ void sendFeedback() if (UART_DMA_CHANNEL->CNDTR != 0) return; + static Feedback feedback; + feedback.start = Feedback::VALID_HEADER; feedback.left.angle = left.rtY.a_elecAngle; @@ -1510,29 +1523,99 @@ void sendFeedback() #ifdef FEATURE_CAN void sendCanFeedback() { - const auto free = HAL_CAN_GetTxMailboxesFreeLevel(&CanHandle); - myPrintf("sendCanFeedback() called (free=%u)", free); - - if (free < 1) - return; - - CAN_TxHeaderTypeDef TxHeader; - TxHeader.StdId = 0x321; - TxHeader.ExtId = 0x01; - TxHeader.RTR = CAN_RTR_DATA; - TxHeader.IDE = CAN_ID_STD; - TxHeader.DLC = 2; - TxHeader.TransmitGlobalTime = DISABLE; - - uint8_t buf[8]; - - static uint32_t TxMailbox; - if (const auto result = HAL_CAN_AddTxMessage(&CanHandle, &TxHeader, buf, &TxMailbox); result == HAL_OK) - myPrintf("HAL_CAN_AddTxMessage() succeeded"); - else + myPrintf("sendCanFeedback() called"); + while (const auto free = HAL_CAN_GetTxMailboxesFreeLevel(&CanHandle)) { - myPrintf("HAL_CAN_AddTxMessage() failed with %i", result); - while (true); + myPrintf("free=%u", free); + + if (free < 1) + return; + + enum SendState : uint8_t { + LeftCurrent, + RightCurrent, + LeftSpeed, + RightSpeed, + Restart + }; + + static union { + SendState sendState{LeftCurrent}; + uint8_t asUint8; + }; + + constexpr auto send = [](uint32_t addr, auto value){ + CAN_TxHeaderTypeDef header; + header.StdId = addr; + header.ExtId = 0x01; + header.RTR = CAN_RTR_DATA; + header.IDE = CAN_ID_STD; + header.DLC = 8; + header.TransmitGlobalTime = DISABLE; + + uint8_t buf[8]; + std::fill(std::begin(buf), std::end(buf), 0x69); + + static uint32_t TxMailbox; + if (const auto result = HAL_CAN_AddTxMessage(&CanHandle, &header, buf, &TxMailbox); result != HAL_OK) + { + myPrintf("HAL_CAN_AddTxMessage() failed with %i", result); + while (true); + } + }; + + enum { // vv + DeviceTypeMotorController = 0b00000000000 + }; + + enum { // ..vv + MotorControllerRec = 0b00000000000, + MotorControllerSend = 0b00010000000, + }; + + enum { // ....v + MotorControllerFront = 0b00000000000, + MotorControllerBack = 0b00001000000, + }; + + enum { // .....v + MotorControllerLeft = 0b00000000000, + MotorControllerRight = 0b00000100000, + }; + + enum { // ......vvvvv + MotorControllerCurrent = 0b00000000000, + MotorControllerSpeed = 0b00000000001 + }; + + enum { + MotorControllerFrontLeftCurrent = DeviceTypeMotorController | MotorControllerSend | MotorControllerCurrent | MotorControllerLeft | MotorControllerFront, + MotorControllerFrontRightCurrent = DeviceTypeMotorController | MotorControllerSend | MotorControllerCurrent | MotorControllerRight | MotorControllerFront, + MotorControllerBackLeftCurrent = DeviceTypeMotorController | MotorControllerSend | MotorControllerCurrent | MotorControllerLeft | MotorControllerBack, + MotorControllerBackRightCurrent = DeviceTypeMotorController | MotorControllerSend | MotorControllerCurrent | MotorControllerRight | MotorControllerBack, + MotorControllerFrontLeftSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerLeft | MotorControllerFront, + MotorControllerFrontRightSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerRight | MotorControllerFront, + MotorControllerBackLeftSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerLeft | MotorControllerBack, + MotorControllerBackRightSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerRight | MotorControllerBack, + }; + + switch (sendState) + { +#ifdef VORNE + case LeftCurrent: send(0b00010000000, left.rtU.i_DCLink); break; + case RightCurrent: send(0b00010100000, right.rtU.i_DCLink); break; +#else + case LeftCurrent: send(0b00011000000, left.rtU.i_DCLink); break; + case RightCurrent: send(0b00011100000, right.rtU.i_DCLink); break; +#endif + default: + __builtin_unreachable(); + } + + asUint8++; + + if (sendState >= Restart) + sendState = LeftCurrent; } } #endif -- 2.50.1 From 8f1e9ce266bf18c78bb9004d457ec3b6c1b02cf3 Mon Sep 17 00:00:00 2001 From: 0xFEEDC0DE64 Date: Sun, 16 May 2021 21:51:47 +0200 Subject: [PATCH 03/15] More registers --- main.cpp | 254 +++++++++++++++++++++++++++++++++++-------------------- 1 file changed, 162 insertions(+), 92 deletions(-) diff --git a/main.cpp b/main.cpp index ed77a87..d6b1e1c 100644 --- a/main.cpp +++ b/main.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include "stm32f1xx_hal.h" @@ -166,6 +167,11 @@ struct { MotorState state; uint32_t chops = 0; + + uint8_t hallBits() const + { + return (rtU.b_hallA ? 0 : 1) | (rtU.b_hallB ? 0 : 2) | (rtU.b_hallC ? 0 : 4); + } } left, right; struct { @@ -370,13 +376,12 @@ int main() 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; #ifdef FEATURE_SERIAL_FEEDBACK - if (main_loop_counter % 50 == 0) // Send data periodically + if (main_loop_counter % 50 == 0) sendFeedback(); #endif #ifdef FEATURE_CAN - if (main_loop_counter % 100 == 0) - sendCanFeedback(); + sendCanFeedback(); #endif #ifdef FEATURE_BUTTON @@ -859,10 +864,10 @@ void CAN_Init() sFilterConfig.FilterBank = 0; sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT; - sFilterConfig.FilterIdHigh = 0x0000; // TODO - sFilterConfig.FilterIdLow = 0x0000; // TODO - sFilterConfig.FilterMaskIdHigh = 0x0000; // TODO - sFilterConfig.FilterMaskIdLow = 0x0000; // TODO + sFilterConfig.FilterIdHigh = 0xFFFF; // TODO + sFilterConfig.FilterIdLow = 0xFFFF; // TODO + sFilterConfig.FilterMaskIdHigh = 0xFFFF; // TODO + sFilterConfig.FilterMaskIdLow = 0xFFFF; // TODO sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0; sFilterConfig.FilterActivation = ENABLE; sFilterConfig.SlaveStartFilterBank = 14; @@ -996,20 +1001,21 @@ void CAN_MspDeInit(CAN_HandleTypeDef *hcan) void CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *CanHandle) { - myPrintf("CAN_RxFifo0MsgPendingCallback() called"); + //myPrintf("CAN_RxFifo0MsgPendingCallback() called"); CAN_RxHeaderTypeDef header; uint8_t buf[8]; if (const auto result = HAL_CAN_GetRxMessage(CanHandle, CAN_RX_FIFO0, &header, buf); result == HAL_OK) { - myPrintf("HAL_CAN_GetRxMessage() succeeded"); - myPrintf("StdId = %x %u", header.StdId, header.StdId); - myPrintf("ExtId = %x %u", header.ExtId, header.ExtId); - myPrintf("IDE = %x %u", header.IDE, header.IDE); - myPrintf("RTR = %x %u", header.RTR, header.RTR); - myPrintf("DLC = %x %u", header.DLC, header.DLC); - myPrintf("Timestamp = %x %u", header.Timestamp, header.Timestamp); - myPrintf("FilterMatchIndex = %x %u", header.FilterMatchIndex, header.FilterMatchIndex); + myPrintf("StdId=%x %u ExtId=%x %u IDE=%x %u RTR=%x %u DLC=%x %u Timestamp=%x %u FilterMatchIndex=%x %u", + header.StdId, header.StdId, + header.ExtId, header.ExtId, + header.IDE, header.IDE, + header.RTR, header.RTR, + header.DLC, header.DLC, + header.Timestamp, header.Timestamp, + header.FilterMatchIndex, header.FilterMatchIndex + ); } else { @@ -1524,99 +1530,163 @@ void sendFeedback() void sendCanFeedback() { myPrintf("sendCanFeedback() called"); - while (const auto free = HAL_CAN_GetTxMailboxesFreeLevel(&CanHandle)) - { - myPrintf("free=%u", free); - if (free < 1) - return; + const auto free = HAL_CAN_GetTxMailboxesFreeLevel(&CanHandle); + if (!free) + return; - enum SendState : uint8_t { - LeftCurrent, - RightCurrent, - LeftSpeed, - RightSpeed, - Restart - }; + enum { // vv + DeviceTypeMotorController = 0b00000000000 + }; - static union { - SendState sendState{LeftCurrent}; - uint8_t asUint8; - }; + enum { // ..vv + MotorControllerRec = 0b00000000000, + MotorControllerSend = 0b00010000000, + }; - constexpr auto send = [](uint32_t addr, auto value){ - CAN_TxHeaderTypeDef header; - header.StdId = addr; - header.ExtId = 0x01; - header.RTR = CAN_RTR_DATA; - header.IDE = CAN_ID_STD; - header.DLC = 8; - header.TransmitGlobalTime = DISABLE; + enum { // ....vvvvv + MotorControllerDcLink = 0b00000000000, + MotorControllerSpeed = 0b00000000100, + MotorControllerError = 0b00000001000, + MotorControllerAngle = 0b00000001100, + MotorControllerDcPhaA = 0b00000010000, + MotorControllerDcPhaB = 0b00000010100, + MotorControllerDcPhaC = 0b00000011000, + MotorControllerChops = 0b00000011100, + MotorControllerHall = 0b00000100000 + }; - uint8_t buf[8]; - std::fill(std::begin(buf), std::end(buf), 0x69); + enum { // .........v + MotorControllerFront = 0b00000000000, + MotorControllerBack = 0b00000000010, + }; - static uint32_t TxMailbox; - if (const auto result = HAL_CAN_AddTxMessage(&CanHandle, &header, buf, &TxMailbox); result != HAL_OK) - { - myPrintf("HAL_CAN_AddTxMessage() failed with %i", result); - while (true); - } - }; + enum { // ..........v + MotorControllerLeft = 0b00000000000, + MotorControllerRight = 0b00000000001, + }; - enum { // vv - DeviceTypeMotorController = 0b00000000000 - }; + enum { + MotorControllerFrontLeftDcLink = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcLink | MotorControllerFront | MotorControllerLeft, + MotorControllerFrontRightDcLink = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcLink | MotorControllerFront | MotorControllerRight, + MotorControllerBackLeftDcLink = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcLink | MotorControllerBack | MotorControllerLeft, + MotorControllerBackRightDcLink = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcLink | MotorControllerBack | MotorControllerRight, - enum { // ..vv - MotorControllerRec = 0b00000000000, - MotorControllerSend = 0b00010000000, - }; + MotorControllerFrontLeftSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerFront | MotorControllerLeft, + MotorControllerFrontRightSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerFront | MotorControllerRight, + MotorControllerBackLeftSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerBack | MotorControllerLeft, + MotorControllerBackRightSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerBack | MotorControllerRight, - enum { // ....v - MotorControllerFront = 0b00000000000, - MotorControllerBack = 0b00001000000, - }; + MotorControllerFrontLeftError = DeviceTypeMotorController | MotorControllerSend | MotorControllerError | MotorControllerFront | MotorControllerLeft, + MotorControllerFrontRightError = DeviceTypeMotorController | MotorControllerSend | MotorControllerError | MotorControllerFront | MotorControllerRight, + MotorControllerBackLeftError = DeviceTypeMotorController | MotorControllerSend | MotorControllerError | MotorControllerBack | MotorControllerLeft, + MotorControllerBackRightError = DeviceTypeMotorController | MotorControllerSend | MotorControllerError | MotorControllerBack | MotorControllerRight, - enum { // .....v - MotorControllerLeft = 0b00000000000, - MotorControllerRight = 0b00000100000, - }; + MotorControllerFrontLeftAngle = DeviceTypeMotorController | MotorControllerSend | MotorControllerAngle | MotorControllerFront | MotorControllerLeft, + MotorControllerFrontRightAngle = DeviceTypeMotorController | MotorControllerSend | MotorControllerAngle | MotorControllerFront | MotorControllerRight, + MotorControllerBackLeftAngle = DeviceTypeMotorController | MotorControllerSend | MotorControllerAngle | MotorControllerBack | MotorControllerLeft, + MotorControllerBackRightAngle = DeviceTypeMotorController | MotorControllerSend | MotorControllerAngle | MotorControllerBack | MotorControllerRight, - enum { // ......vvvvv - MotorControllerCurrent = 0b00000000000, - MotorControllerSpeed = 0b00000000001 - }; + MotorControllerFrontLeftDcPhaA = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaA | MotorControllerFront | MotorControllerLeft, + MotorControllerFrontRightDcPhaA = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaA | MotorControllerFront | MotorControllerRight, + MotorControllerBackLeftDcPhaA = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaA | MotorControllerBack | MotorControllerLeft, + MotorControllerBackRightDcPhaA = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaA | MotorControllerBack | MotorControllerRight, - enum { - MotorControllerFrontLeftCurrent = DeviceTypeMotorController | MotorControllerSend | MotorControllerCurrent | MotorControllerLeft | MotorControllerFront, - MotorControllerFrontRightCurrent = DeviceTypeMotorController | MotorControllerSend | MotorControllerCurrent | MotorControllerRight | MotorControllerFront, - MotorControllerBackLeftCurrent = DeviceTypeMotorController | MotorControllerSend | MotorControllerCurrent | MotorControllerLeft | MotorControllerBack, - MotorControllerBackRightCurrent = DeviceTypeMotorController | MotorControllerSend | MotorControllerCurrent | MotorControllerRight | MotorControllerBack, - MotorControllerFrontLeftSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerLeft | MotorControllerFront, - MotorControllerFrontRightSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerRight | MotorControllerFront, - MotorControllerBackLeftSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerLeft | MotorControllerBack, - MotorControllerBackRightSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerRight | MotorControllerBack, - }; + MotorControllerFrontLeftDcPhaB = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaB | MotorControllerFront | MotorControllerLeft, + MotorControllerFrontRightDcPhaB = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaB | MotorControllerFront | MotorControllerRight, + MotorControllerBackLeftDcPhaB = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaB | MotorControllerBack | MotorControllerLeft, + MotorControllerBackRightDcPhaB = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaB | MotorControllerBack | MotorControllerRight, - switch (sendState) + MotorControllerFrontLeftDcPhaC = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaC | MotorControllerFront | MotorControllerLeft, + MotorControllerFrontRightDcPhaC = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaC | MotorControllerFront | MotorControllerRight, + MotorControllerBackLeftDcPhaC = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaC | MotorControllerBack | MotorControllerLeft, + MotorControllerBackRightDcPhaC = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaC | MotorControllerBack | MotorControllerRight, + + MotorControllerFrontLeftChops = DeviceTypeMotorController | MotorControllerSend | MotorControllerChops | MotorControllerFront | MotorControllerLeft, + MotorControllerFrontRightChops = DeviceTypeMotorController | MotorControllerSend | MotorControllerChops | MotorControllerFront | MotorControllerRight, + MotorControllerBackLeftChops = DeviceTypeMotorController | MotorControllerSend | MotorControllerChops | MotorControllerBack | MotorControllerLeft, + MotorControllerBackRightChops = DeviceTypeMotorController | MotorControllerSend | MotorControllerChops | MotorControllerBack | MotorControllerRight, + + MotorControllerFrontLeftHall = DeviceTypeMotorController | MotorControllerSend | MotorControllerHall | MotorControllerFront | MotorControllerLeft, + MotorControllerFrontRightHall = DeviceTypeMotorController | MotorControllerSend | MotorControllerHall | MotorControllerFront | MotorControllerRight, + MotorControllerBackLeftHall = DeviceTypeMotorController | MotorControllerSend | MotorControllerHall | MotorControllerBack | MotorControllerLeft, + MotorControllerBackRightHall = DeviceTypeMotorController | MotorControllerSend | MotorControllerHall | MotorControllerBack | MotorControllerRight, + }; + + enum SendState : uint8_t { + LeftDcLink, + RightDcLink, + LeftSpeed, + RightSpeed, + LeftError, + RightError, + LeftAngle, + RightAngle, + LeftDcPhaA, + RightDcPhaA, + LeftDcPhaB, + RightDcPhaB, + LeftDcPhaC, + RightDcPhaC, + LeftChops, + RightChops, + LeftHall, + RightHall, + Restart + }; + + static union { + SendState sendState{LeftDcLink}; + uint8_t asUint8; + }; + + constexpr auto send = [](uint32_t addr, auto value){ + CAN_TxHeaderTypeDef header; + header.StdId = addr; + header.ExtId = 0x01; + header.RTR = CAN_RTR_DATA; + header.IDE = CAN_ID_STD; + header.DLC = sizeof(value); + header.TransmitGlobalTime = DISABLE; + + uint8_t buf[8] {0}; + std::memcpy(buf, &value, sizeof(value)); + + static uint32_t TxMailbox; + if (const auto result = HAL_CAN_AddTxMessage(&CanHandle, &header, buf, &TxMailbox); result != HAL_OK) { -#ifdef VORNE - case LeftCurrent: send(0b00010000000, left.rtU.i_DCLink); break; - case RightCurrent: send(0b00010100000, right.rtU.i_DCLink); break; -#else - case LeftCurrent: send(0b00011000000, left.rtU.i_DCLink); break; - case RightCurrent: send(0b00011100000, right.rtU.i_DCLink); break; -#endif - default: - __builtin_unreachable(); + myPrintf("HAL_CAN_AddTxMessage() failed with %i", result); + //while (true); } + }; - asUint8++; - - if (sendState >= Restart) - sendState = LeftCurrent; + switch (sendState) + { + case LeftDcLink: myPrintf("LeftCurrent free=%u", free); send(MotorControllerFrontLeftDcLink, left. rtU.i_DCLink); break; + case RightDcLink: myPrintf("RightCurrent free=%u", free); send(MotorControllerFrontRightDcLink, right.rtU.i_DCLink); break; + case LeftSpeed: myPrintf("LeftSpeed free=%u", free); send(MotorControllerFrontLeftSpeed, left. rtY.n_mot); break; + case RightSpeed: myPrintf("RightSpeed free=%u", free); send(MotorControllerFrontRightSpeed, right.rtY.n_mot); break; + case LeftError: myPrintf("LeftError free=%u", free); send(MotorControllerFrontLeftError, left. rtY.z_errCode); break; + case RightError: myPrintf("RightError free=%u", free); send(MotorControllerFrontRightError, right.rtY.z_errCode); break; + case LeftAngle: myPrintf("LeftAngle free=%u", free); send(MotorControllerFrontLeftAngle, left. rtY.a_elecAngle); break; + case RightAngle: myPrintf("RightAngle free=%u", free); send(MotorControllerFrontRightAngle, right.rtY.a_elecAngle); break; + case LeftDcPhaA: myPrintf("LeftDcPhaA free=%u", free); send(MotorControllerFrontLeftDcPhaA, left. rtY.DC_phaA); break; + case RightDcPhaA: myPrintf("RightDcPhaA free=%u", free); send(MotorControllerFrontRightDcPhaA, right.rtY.DC_phaA); break; + case LeftDcPhaB: myPrintf("LeftDcPhaB free=%u", free); send(MotorControllerFrontLeftDcPhaB, left. rtY.DC_phaB); break; + case RightDcPhaB: myPrintf("RightDcPhaB free=%u", free); send(MotorControllerFrontRightDcPhaB, right.rtY.DC_phaB); break; + case LeftDcPhaC: myPrintf("LeftDcPhaC free=%u", free); send(MotorControllerFrontLeftDcPhaC, left. rtY.DC_phaC); break; + case RightDcPhaC: myPrintf("RightDcPhaC free=%u", free); send(MotorControllerFrontRightDcPhaC, right.rtY.DC_phaC); break; + case LeftChops: myPrintf("LeftChops free=%u", free); send(MotorControllerFrontLeftChops, left. chops); break; + case RightChops: myPrintf("RightChops free=%u", free); send(MotorControllerFrontRightChops, right.chops); break; + case LeftHall: myPrintf("LeftHall free=%u", free); send(MotorControllerFrontLeftHall, left.hallBits()); break; + case RightHall: myPrintf("RightHall free=%u", free); send(MotorControllerFrontRightHall, right.hallBits()); break; + default: __builtin_unreachable(); } + + asUint8++; + + if (sendState >= Restart) + sendState = LeftDcLink; } #endif -- 2.50.1 From a9f1e476f5b02c60026b8eb560980b5a13116a52 Mon Sep 17 00:00:00 2001 From: 0xFEEDC0DE64 Date: Sun, 16 May 2021 22:47:56 +0200 Subject: [PATCH 04/15] More implementations --- main.cpp | 62 +++++++++++++++++++++++++++++++++++++------------------- 1 file changed, 41 insertions(+), 21 deletions(-) diff --git a/main.cpp b/main.cpp index d6b1e1c..83a17ed 100644 --- a/main.cpp +++ b/main.cpp @@ -337,7 +337,7 @@ int main() while (true) { - HAL_Delay(DELAY_IN_MAIN_LOOP); //delay in ms + //HAL_Delay(DELAY_IN_MAIN_LOOP); //delay in ms #ifdef FEATURE_SERIAL_CONTROL parseCommand(); @@ -1529,7 +1529,7 @@ void sendFeedback() #ifdef FEATURE_CAN void sendCanFeedback() { - myPrintf("sendCanFeedback() called"); + //myPrintf("sendCanFeedback() called"); const auto free = HAL_CAN_GetTxMailboxesFreeLevel(&CanHandle); if (!free) @@ -1553,7 +1553,9 @@ void sendCanFeedback() MotorControllerDcPhaB = 0b00000010100, MotorControllerDcPhaC = 0b00000011000, MotorControllerChops = 0b00000011100, - MotorControllerHall = 0b00000100000 + MotorControllerHall = 0b00000100000, + MotorControllerVoltage = 0b00000100100, + MotorControllerTemp = 0b00000101000 }; enum { // .........v @@ -1611,6 +1613,16 @@ void sendCanFeedback() MotorControllerFrontRightHall = DeviceTypeMotorController | MotorControllerSend | MotorControllerHall | MotorControllerFront | MotorControllerRight, MotorControllerBackLeftHall = DeviceTypeMotorController | MotorControllerSend | MotorControllerHall | MotorControllerBack | MotorControllerLeft, MotorControllerBackRightHall = DeviceTypeMotorController | MotorControllerSend | MotorControllerHall | MotorControllerBack | MotorControllerRight, + + MotorControllerFrontLeftVoltage = DeviceTypeMotorController | MotorControllerSend | MotorControllerVoltage | MotorControllerFront | MotorControllerLeft, + MotorControllerFrontRightVoltage = DeviceTypeMotorController | MotorControllerSend | MotorControllerVoltage | MotorControllerFront | MotorControllerRight, + MotorControllerBackLeftVoltage = DeviceTypeMotorController | MotorControllerSend | MotorControllerVoltage | MotorControllerBack | MotorControllerLeft, + MotorControllerBackRightVoltage = DeviceTypeMotorController | MotorControllerSend | MotorControllerVoltage | MotorControllerBack | MotorControllerRight, + + MotorControllerFrontLeftTemp = DeviceTypeMotorController | MotorControllerSend | MotorControllerTemp | MotorControllerFront | MotorControllerLeft, + MotorControllerFrontRightTemp = DeviceTypeMotorController | MotorControllerSend | MotorControllerTemp | MotorControllerFront | MotorControllerRight, + MotorControllerBackLeftTemp = DeviceTypeMotorController | MotorControllerSend | MotorControllerTemp | MotorControllerBack | MotorControllerLeft, + MotorControllerBackRightTemp = DeviceTypeMotorController | MotorControllerSend | MotorControllerTemp | MotorControllerBack | MotorControllerRight, }; enum SendState : uint8_t { @@ -1632,6 +1644,10 @@ void sendCanFeedback() RightChops, LeftHall, RightHall, + LeftVoltage, + RightVoltage, + LeftTemp, + RightTemp, Restart }; @@ -1662,24 +1678,28 @@ void sendCanFeedback() switch (sendState) { - case LeftDcLink: myPrintf("LeftCurrent free=%u", free); send(MotorControllerFrontLeftDcLink, left. rtU.i_DCLink); break; - case RightDcLink: myPrintf("RightCurrent free=%u", free); send(MotorControllerFrontRightDcLink, right.rtU.i_DCLink); break; - case LeftSpeed: myPrintf("LeftSpeed free=%u", free); send(MotorControllerFrontLeftSpeed, left. rtY.n_mot); break; - case RightSpeed: myPrintf("RightSpeed free=%u", free); send(MotorControllerFrontRightSpeed, right.rtY.n_mot); break; - case LeftError: myPrintf("LeftError free=%u", free); send(MotorControllerFrontLeftError, left. rtY.z_errCode); break; - case RightError: myPrintf("RightError free=%u", free); send(MotorControllerFrontRightError, right.rtY.z_errCode); break; - case LeftAngle: myPrintf("LeftAngle free=%u", free); send(MotorControllerFrontLeftAngle, left. rtY.a_elecAngle); break; - case RightAngle: myPrintf("RightAngle free=%u", free); send(MotorControllerFrontRightAngle, right.rtY.a_elecAngle); break; - case LeftDcPhaA: myPrintf("LeftDcPhaA free=%u", free); send(MotorControllerFrontLeftDcPhaA, left. rtY.DC_phaA); break; - case RightDcPhaA: myPrintf("RightDcPhaA free=%u", free); send(MotorControllerFrontRightDcPhaA, right.rtY.DC_phaA); break; - case LeftDcPhaB: myPrintf("LeftDcPhaB free=%u", free); send(MotorControllerFrontLeftDcPhaB, left. rtY.DC_phaB); break; - case RightDcPhaB: myPrintf("RightDcPhaB free=%u", free); send(MotorControllerFrontRightDcPhaB, right.rtY.DC_phaB); break; - case LeftDcPhaC: myPrintf("LeftDcPhaC free=%u", free); send(MotorControllerFrontLeftDcPhaC, left. rtY.DC_phaC); break; - case RightDcPhaC: myPrintf("RightDcPhaC free=%u", free); send(MotorControllerFrontRightDcPhaC, right.rtY.DC_phaC); break; - case LeftChops: myPrintf("LeftChops free=%u", free); send(MotorControllerFrontLeftChops, left. chops); break; - case RightChops: myPrintf("RightChops free=%u", free); send(MotorControllerFrontRightChops, right.chops); break; - case LeftHall: myPrintf("LeftHall free=%u", free); send(MotorControllerFrontLeftHall, left.hallBits()); break; - case RightHall: myPrintf("RightHall free=%u", free); send(MotorControllerFrontRightHall, right.hallBits()); break; + case LeftDcLink: /* myPrintf("LeftCurrent free=%u", free); */ send(MotorControllerFrontLeftDcLink, left. rtU.i_DCLink); break; + case RightDcLink: /* myPrintf("RightCurrent free=%u", free); */ send(MotorControllerFrontRightDcLink, right.rtU.i_DCLink); break; + case LeftSpeed: /* myPrintf("LeftSpeed free=%u", free); */ send(MotorControllerFrontLeftSpeed, left. rtY.n_mot); break; + case RightSpeed: /* myPrintf("RightSpeed free=%u", free); */ send(MotorControllerFrontRightSpeed, right.rtY.n_mot); break; + case LeftError: /* myPrintf("LeftError free=%u", free); */ send(MotorControllerFrontLeftError, left. rtY.z_errCode); break; + case RightError: /* myPrintf("RightError free=%u", free); */ send(MotorControllerFrontRightError, right.rtY.z_errCode); break; + case LeftAngle: /* myPrintf("LeftAngle free=%u", free); */ send(MotorControllerFrontLeftAngle, left. rtY.a_elecAngle); break; + case RightAngle: /* myPrintf("RightAngle free=%u", free); */ send(MotorControllerFrontRightAngle, right.rtY.a_elecAngle); break; + case LeftDcPhaA: /* myPrintf("LeftDcPhaA free=%u", free); */ send(MotorControllerFrontLeftDcPhaA, left. rtY.DC_phaA); break; + case RightDcPhaA: /* myPrintf("RightDcPhaA free=%u", free); */ send(MotorControllerFrontRightDcPhaA, right.rtY.DC_phaA); break; + case LeftDcPhaB: /* myPrintf("LeftDcPhaB free=%u", free); */ send(MotorControllerFrontLeftDcPhaB, left. rtY.DC_phaB); break; + case RightDcPhaB: /* myPrintf("RightDcPhaB free=%u", free); */ send(MotorControllerFrontRightDcPhaB, right.rtY.DC_phaB); break; + case LeftDcPhaC: /* myPrintf("LeftDcPhaC free=%u", free); */ send(MotorControllerFrontLeftDcPhaC, left. rtY.DC_phaC); break; + case RightDcPhaC: /* myPrintf("RightDcPhaC free=%u", free); */ send(MotorControllerFrontRightDcPhaC, right.rtY.DC_phaC); break; + case LeftChops: /* myPrintf("LeftChops free=%u", free); */ send(MotorControllerFrontLeftChops, left. chops); break; + case RightChops: /* myPrintf("RightChops free=%u", free); */ send(MotorControllerFrontRightChops, right.chops); break; + case LeftHall: /* myPrintf("LeftHall free=%u", free); */ send(MotorControllerFrontLeftHall, left.hallBits()); break; + case RightHall: /* myPrintf("RightHall free=%u", free); */ send(MotorControllerFrontRightHall, right.hallBits()); break; + case LeftVoltage: /* myPrintf("LeftVoltage free=%u", free); */ send(MotorControllerFrontLeftVoltage, batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); break; + case RightVoltage: /* myPrintf("RightVoltage free=%u", free); */ send(MotorControllerFrontRightVoltage, batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); break; + case LeftTemp: /* myPrintf("LeftTemp free=%u", free); */ send(MotorControllerFrontLeftTemp, board_temp_deg_c); break; + case RightTemp: /* myPrintf("RightTemp free=%u", free); */ send(MotorControllerFrontRightTemp, board_temp_deg_c); break; default: __builtin_unreachable(); } -- 2.50.1 From cd4c4b9ed1846a5b7b1ef974d4948ccdaa749f09 Mon Sep 17 00:00:00 2001 From: 0xFEEDC0DE64 Date: Mon, 17 May 2021 23:17:12 +0200 Subject: [PATCH 05/15] Moved canbus registers into protocol submodule --- CMakeLists.txt | 4 +- bobbycar-protocol | 2 +- config.h | 13 -- main.cpp | 507 +++++++++++++++++++++------------------------- 4 files changed, 232 insertions(+), 294 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d4463da..6bc6fe2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -68,7 +68,9 @@ add_executable(firmware.elf bobbycar-foc-model/BLDC_controller_data.c bobbycar-foc-model/rtwtypes.h - bobbycar-protocol/protocol.h + bobbycar-protocol/bobbycar-can.h + bobbycar-protocol/bobbycar-common.h + bobbycar-protocol/bobbycar-serial.h startup_stm32f103xe.s system_stm32f1xx.c diff --git a/bobbycar-protocol b/bobbycar-protocol index 39f76cb..236d1a2 160000 --- a/bobbycar-protocol +++ b/bobbycar-protocol @@ -1 +1 @@ -Subproject commit 39f76cb62b32da2ea8d7a7154e1b2854dbf792ab +Subproject commit 236d1a2d33b31443238d6490889e3b7d29ee2119 diff --git a/config.h b/config.h index 0e698b2..6afe11f 100644 --- a/config.h +++ b/config.h @@ -6,12 +6,6 @@ #define PWM_FREQ 16000 // PWM frequency in Hz #endif #define DEAD_TIME 48 // PWM deadtime -#ifdef MOTOR_TEST - #define DELAY_IN_MAIN_LOOP 20 -#else - #define DELAY_IN_MAIN_LOOP 5 -#endif -#define TIMEOUT 5 // number of wrong / missing input commands before emergency off #define A2BIT_CONV 50 // A to bit for current conversion on ADC. Example: 1 A = 50, 2 A = 100, etc // ADC conversion time definitions @@ -70,13 +64,6 @@ #define TEMP_CAL_LOW_DEG_C 358 // temperature 1: measured temperature [°C * 10]. Here 35.8 °C #define TEMP_CAL_HIGH_ADC 1588 // temperature 2: ADC value #define TEMP_CAL_HIGH_DEG_C 489 // temperature 2: measured temperature [°C * 10]. Here 48.9 °C -#define TEMP_WARNING_ENABLE 0 // to beep or not to beep, 1 or 0, DO NOT ACTIVITE WITHOUT CALIBRATION! -#define TEMP_WARNING 600 // annoying fast beeps [°C * 10]. Here 60.0 °C -#define TEMP_POWEROFF_ENABLE 0 // to poweroff or not to poweroff, 1 or 0, DO NOT ACTIVITE WITHOUT CALIBRATION! -#define TEMP_POWEROFF 650 // overheat poweroff. (while not driving) [°C * 10]. Here 65.0 °C - -#define INACTIVITY_TIMEOUT 8 // minutes of not driving until poweroff. it is not very precise. - // ############################### INPUT ############################### diff --git a/main.cpp b/main.cpp index 83a17ed..eed27a5 100644 --- a/main.cpp +++ b/main.cpp @@ -30,7 +30,14 @@ #include "defines.h" #include "config.h" -#include "protocol.h" + +#include "bobbycar-common.h" +#if FEATURE_SERIAL_CONTROL || FEATURE_SERIAL_FEEDBACK +#include "bobbycar-serial.h" +#endif +#ifdef FEATURE_CAN +#include "bobbycar-can.h" +#endif extern "C" { #include "BLDC_controller.h" @@ -138,8 +145,18 @@ volatile struct { // ############################################################################### std::atomic timeout; + +#ifdef FEATURE_SERIAL_CONTROL int16_t timeoutCntSerial = 0; // Timeout counter for Rx Serial command +Command command; +#endif + +#ifdef FEATURE_CAN +std::atomic timeoutCntLeft = 0; +std::atomic timeoutCntRight = 0; +#endif + uint32_t main_loop_counter; uint16_t offsetcount = 0; @@ -164,9 +181,9 @@ struct { ExtU rtU; /* External inputs */ ExtY rtY; /* External outputs */ - MotorState state; - - uint32_t chops = 0; + bool enable{true}; + int16_t iDcMax{17}; + uint32_t chops{}; uint8_t hallBits() const { @@ -175,14 +192,11 @@ struct { } left, right; struct { - BuzzerState state; - + uint8_t freq = 0; + uint8_t pattern = 0; uint32_t timer = 0; } buzzer; -Command command; - - void filtLowPass32(int16_t u, uint16_t coef, int32_t *y); void SystemClock_Config(); @@ -207,7 +221,14 @@ void MX_ADC1_Init(); void MX_ADC2_Init(); + +#ifdef FEATURE_BUTTON void poweroff(); +#endif + +#ifdef MOTOR_TEST +void doMotorTest(); +#endif #ifdef FEATURE_SERIAL_CONTROL void parseCommand(); @@ -218,6 +239,7 @@ void sendFeedback(); #endif #ifdef FEATURE_CAN +void parseCanCommand(); void sendCanFeedback(); #endif @@ -262,32 +284,34 @@ int main() left.rtP = rtP_Left; left.rtP.b_angleMeasEna = 0; #ifdef PETERS_PLATINE - left.rtP.z_selPhaCurMeasABC = CurrentMeasBC; // Left motor measured current phases = {iB, iC} -> do NOT change + left.rtP.z_selPhaCurMeasABC = CurrentMeasBC; #else - left.rtP.z_selPhaCurMeasABC = CurrentMeasAB; // Left motor measured current phases = {iA, iB} -> do NOT change + left.rtP.z_selPhaCurMeasABC = CurrentMeasAB; #endif - left.rtP.z_ctrlTypSel = uint8_t(left.state.ctrlTyp); left.rtP.b_diagEna = DIAG_ENA; - left.rtP.i_max = (left.state.iMotMax * A2BIT_CONV) << 4; // fixdt(1,16,4) - left.rtP.n_max = left.state.nMotMax << 4; // fixdt(1,16,4) left.rtP.b_fieldWeakEna = FIELD_WEAK_ENA; - left.rtP.id_fieldWeakMax = (left.state.fieldWeakMax * A2BIT_CONV) << 4; // fixdt(1,16,4) - left.rtP.a_phaAdvMax = left.state.phaseAdvMax << 4; // fixdt(1,16,4) - left.rtP.r_fieldWeakHi = FIELD_WEAK_HI << 4; // fixdt(1,16,4) - left.rtP.r_fieldWeakLo = FIELD_WEAK_LO << 4; // fixdt(1,16,4) + left.rtP.r_fieldWeakHi = FIELD_WEAK_HI << 4; + left.rtP.r_fieldWeakLo = FIELD_WEAK_LO << 4; + + left.rtP.z_ctrlTypSel = uint8_t(ControlType::FieldOrientedControl); + left.rtP.i_max = (15 * A2BIT_CONV) << 4; + left.rtP.n_max = 1000 << 4; + left.rtP.id_fieldWeakMax = (5 * A2BIT_CONV) << 4; + left.rtP.a_phaAdvMax = 40 << 4; right.rtP = rtP_Left; right.rtP.b_angleMeasEna = 0; - right.rtP.z_selPhaCurMeasABC = CurrentMeasBC; // Right motor measured current phases = {iB, iC} -> do NOT change - right.rtP.z_ctrlTypSel = uint8_t(right.state.ctrlTyp); + right.rtP.z_selPhaCurMeasABC = CurrentMeasBC; right.rtP.b_diagEna = DIAG_ENA; - right.rtP.i_max = (right.state.iMotMax * A2BIT_CONV) << 4; // fixdt(1,16,4) - right.rtP.n_max = right.state.nMotMax << 4; // fixdt(1,16,4) right.rtP.b_fieldWeakEna = FIELD_WEAK_ENA; - right.rtP.id_fieldWeakMax = (right.state.fieldWeakMax * A2BIT_CONV) << 4; // fixdt(1,16,4) - right.rtP.a_phaAdvMax = right.state.phaseAdvMax << 4; // fixdt(1,16,4) - right.rtP.r_fieldWeakHi = FIELD_WEAK_HI << 4; // fixdt(1,16,4) - right.rtP.r_fieldWeakLo = FIELD_WEAK_LO << 4; // fixdt(1,16,4) + right.rtP.r_fieldWeakHi = FIELD_WEAK_HI << 4; + right.rtP.r_fieldWeakLo = FIELD_WEAK_LO << 4; + + right.rtP.z_ctrlTypSel = uint8_t(ControlType::FieldOrientedControl); + right.rtP.i_max = (15 * A2BIT_CONV) << 4; + right.rtP.n_max = 1000 << 4; + right.rtP.id_fieldWeakMax = (5 * A2BIT_CONV) << 4; + right.rtP.a_phaAdvMax = 40 << 4; left.rtM.defaultParam = &left.rtP; left.rtM.dwork = &left.rtDW; @@ -305,10 +329,10 @@ int main() for (int i = 8; i >= 0; i--) { - buzzer.state.freq = (uint8_t)i; + buzzer.freq = (uint8_t)i; HAL_Delay(50); } - buzzer.state.freq = 0; + buzzer.freq = 0; #ifdef HUARN2 UART2_Init(); @@ -337,57 +361,31 @@ int main() while (true) { - //HAL_Delay(DELAY_IN_MAIN_LOOP); //delay in ms + HAL_Delay(5); //delay in ms + +#ifdef MOTOR_TEST + doMotorTest(); +#endif #ifdef FEATURE_SERIAL_CONTROL parseCommand(); #endif - timeout = 0; - -#ifdef MOTOR_TEST - left.state.enable = true; - left.state.ctrlMod = ControlMode::Voltage; - left.state.ctrlTyp = ControlType::FieldOrientedControl; - left.state.pwm = pwm; - left.state.iMotMax = 2; - - right.state.enable = true; - right.state.ctrlMod = ControlMode::Voltage; - right.state.ctrlTyp = ControlType::FieldOrientedControl; - right.state.pwm = pwm; - right.state.iMotMax = 2; - - constexpr auto pwmMax = 250; - - pwm += dir; - if (pwm > pwmMax) { - pwm = pwmMax; - dir = -1; - } else if (pwm < -pwmMax) { - pwm = -pwmMax; - dir = 1; - } -#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; - #ifdef FEATURE_SERIAL_FEEDBACK - if (main_loop_counter % 50 == 0) - sendFeedback(); + sendFeedback(); #endif #ifdef FEATURE_CAN + parseCanCommand(); + sendCanFeedback(); #endif #ifdef FEATURE_BUTTON if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) { - left.state.enable = right.state.enable = 0; // disable motors + left.enable = false; + right.enable = false; while (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {} // wait until button is released @@ -399,52 +397,25 @@ int main() } #endif - left.rtP.z_ctrlTypSel = uint8_t(left.state.ctrlTyp); - left.rtP.i_max = (left.state.iMotMax * A2BIT_CONV) << 4; // fixdt(1,16,4) - left.rtP.n_max = left.state.nMotMax << 4; // fixdt(1,16,4) - left.rtP.id_fieldWeakMax = (left.state.fieldWeakMax * A2BIT_CONV) << 4; // fixdt(1,16,4) - left.rtP.a_phaAdvMax = left.state.phaseAdvMax << 4; // fixdt(1,16,4) + // ####### 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; - left.rtU.z_ctrlModReq = uint8_t(left.state.ctrlMod); - left.rtU.r_inpTgt = left.state.pwm; - - right.rtP.z_ctrlTypSel = uint8_t(right.state.ctrlTyp); - right.rtP.i_max = (right.state.iMotMax * A2BIT_CONV) << 4; // fixdt(1,16,4) - right.rtP.n_max = right.state.nMotMax << 4; // fixdt(1,16,4) - right.rtP.id_fieldWeakMax = (right.state.fieldWeakMax * A2BIT_CONV) << 4; // fixdt(1,16,4) - right.rtP.a_phaAdvMax = right.state.phaseAdvMax << 4; // fixdt(1,16,4) - - right.rtU.z_ctrlModReq = uint8_t(right.state.ctrlMod); - right.rtU.r_inpTgt = right.state.pwm; + filtLowPass32(adc_buffer.batt1, BAT_FILT_COEF, &batVoltageFixdt); + batVoltage = (int16_t)(batVoltageFixdt >> 20); // convert fixed-point to integer main_loop_counter++; - timeout++; } } namespace { -void updateBatVoltage() -{ - if (buzzer.timer % 1000 == 0) // because you get float rounding errors if it would run every time -> not any more, everything converted to fixed-point - { - filtLowPass32(adc_buffer.batt1, BAT_FILT_COEF, &batVoltageFixdt); - batVoltage = (int16_t)(batVoltageFixdt >> 20); // convert fixed-point to integer - } -} - void updateBuzzer() { - if (buzzer.timer % 1000 == 0) // because you get float rounding errors if it would run every time -> not any more, everything converted to fixed-point - { - filtLowPass32(adc_buffer.batt1, BAT_FILT_COEF, &batVoltageFixdt); - batVoltage = (int16_t)(batVoltageFixdt >> 20); // convert fixed-point to integer - } - - //create square wave for buzzer buzzer.timer++; - if (buzzer.state.freq != 0 && (buzzer.timer / 1000) % (buzzer.state.pattern + 1) == 0) + if (buzzer.freq != 0 && (buzzer.timer / 1000) % (buzzer.pattern + 1) == 0) { - if (buzzer.timer % buzzer.state.freq == 0) + if (buzzer.timer % buzzer.freq == 0) { HAL_GPIO_TogglePin(BUZZER_PORT, BUZZER_PIN); } @@ -491,24 +462,24 @@ void updateMotors() #endif int16_t curR_DC = (int16_t)(offsetdcr - adc_buffer.dcr); - const bool chopL = std::abs(curL_DC) > (left.state.iDcMax * A2BIT_CONV); + const bool chopL = std::abs(curL_DC) > (left.iDcMax * A2BIT_CONV); if (chopL) left.chops++; - const bool chopR = std::abs(curR_DC) > (right.state.iDcMax * A2BIT_CONV); + const bool chopR = std::abs(curR_DC) > (right.iDcMax * A2BIT_CONV); if (chopR) right.chops++; - const auto timeoutVal = timeout.load(); + const uint32_t timeoutVal = ++timeout; // 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 - if (chopL || timeoutVal > TIMEOUT || !left.state.enable) + if (chopL || timeoutVal > 500 || !left.enable) LEFT_TIM->BDTR &= ~TIM_BDTR_MOE; else LEFT_TIM->BDTR |= TIM_BDTR_MOE; - if (chopR || timeoutVal > TIMEOUT || !right.state.enable) + if (chopR || timeoutVal > 500 || !right.enable) RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE; else RIGHT_TIM->BDTR |= TIM_BDTR_MOE; @@ -535,8 +506,8 @@ void updateMotors() #endif ; - const bool enableLFin = left.state.enable && left.rtY.z_errCode == 0 && (right.rtY.z_errCode == 0 || ignoreOtherMotor); - const bool enableRFin = right.state.enable && (left.rtY.z_errCode == 0 || ignoreOtherMotor) && right.rtY.z_errCode == 0; + const bool enableLFin = left.enable && 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; // ========================= LEFT MOTOR ============================ // Get hall sensors values @@ -888,28 +859,31 @@ void CAN_Init() while (true); } - if (const auto result = HAL_CAN_RegisterCallback(&CanHandle, HAL_CAN_TX_MAILBOX0_COMPLETE_CB_ID, CAN_TxMailboxCompleteCallback); result == HAL_OK) - myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX0_COMPLETE_CB_ID succeeded"); - else + if (false) { - myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX0_COMPLETE_CB_ID failed with %i", result); - while (true); - } + if (const auto result = HAL_CAN_RegisterCallback(&CanHandle, HAL_CAN_TX_MAILBOX0_COMPLETE_CB_ID, CAN_TxMailboxCompleteCallback); result == HAL_OK) + myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX0_COMPLETE_CB_ID succeeded"); + else + { + myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX0_COMPLETE_CB_ID failed with %i", result); + while (true); + } - if (const auto result = HAL_CAN_RegisterCallback(&CanHandle, HAL_CAN_TX_MAILBOX1_COMPLETE_CB_ID, CAN_TxMailboxCompleteCallback); result == HAL_OK) - myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX1_COMPLETE_CB_ID succeeded"); - else - { - myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX1_COMPLETE_CB_ID failed with %i", result); - while (true); - } + if (const auto result = HAL_CAN_RegisterCallback(&CanHandle, HAL_CAN_TX_MAILBOX1_COMPLETE_CB_ID, CAN_TxMailboxCompleteCallback); result == HAL_OK) + myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX1_COMPLETE_CB_ID succeeded"); + else + { + myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX1_COMPLETE_CB_ID failed with %i", result); + while (true); + } - if (const auto result = HAL_CAN_RegisterCallback(&CanHandle, HAL_CAN_TX_MAILBOX2_COMPLETE_CB_ID, CAN_TxMailboxCompleteCallback); result == HAL_OK) - myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX2_COMPLETE_CB_ID succeeded"); - else - { - myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX2_COMPLETE_CB_ID failed with %i", result); - while (true); + if (const auto result = HAL_CAN_RegisterCallback(&CanHandle, HAL_CAN_TX_MAILBOX2_COMPLETE_CB_ID, CAN_TxMailboxCompleteCallback); result == HAL_OK) + myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX2_COMPLETE_CB_ID succeeded"); + else + { + myPrintf("HAL_CAN_RegisterCallback() HAL_CAN_TX_MAILBOX2_COMPLETE_CB_ID failed with %i", result); + while (true); + } } /* Start the CAN peripheral */ @@ -1005,41 +979,33 @@ void CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *CanHandle) CAN_RxHeaderTypeDef header; uint8_t buf[8]; - if (const auto result = HAL_CAN_GetRxMessage(CanHandle, CAN_RX_FIFO0, &header, buf); result == HAL_OK) - { - myPrintf("StdId=%x %u ExtId=%x %u IDE=%x %u RTR=%x %u DLC=%x %u Timestamp=%x %u FilterMatchIndex=%x %u", - header.StdId, header.StdId, - header.ExtId, header.ExtId, - header.IDE, header.IDE, - header.RTR, header.RTR, - header.DLC, header.DLC, - header.Timestamp, header.Timestamp, - header.FilterMatchIndex, header.FilterMatchIndex - ); - } - else + if (const auto result = HAL_CAN_GetRxMessage(CanHandle, CAN_RX_FIFO0, &header, buf); result != HAL_OK) { myPrintf("HAL_CAN_GetRxMessage() failed with %i", result); - while (true); + //while (true); + return; } - //if (header.IDE == CAN_ID_STD && - // (header.StdId == CAN_ID_COMMAND_STW_TO_BOARD(BOARD_INDEX) || - // header.StdId == CAN_ID_FEEDBACK_STW_TO_BOARD(BOARD_INDEX))) - //{ - // can_feedc0de_handle_frame(header.StdId, RxData, dlc_to_len(header.DLC)); - //} - // slightly yucky, but we don't want to block inside the IRQ handler - //if (HAL_CAN_GetTxMailboxesFreeLevel(CanHandle) >= 2) - //{ - // can_feedc0de_poll(); - //} + myPrintf("StdId=%x %u ExtId=%x %u IDE=%x %u RTR=%x %u DLC=%x %u Timestamp=%x %u FilterMatchIndex=%x %u", + header.StdId, header.StdId, + header.ExtId, header.ExtId, + header.IDE, header.IDE, + header.RTR, header.RTR, + header.DLC, header.DLC, + header.Timestamp, header.Timestamp, + header.FilterMatchIndex, header.FilterMatchIndex + ); + + // TODO apply changes and reset timeout qualification + + timeoutCntLeft = 0; + timeoutCntRight = 0; } void CAN_TxMailboxCompleteCallback(CAN_HandleTypeDef *hcan) { - //myPrintf("CAN_TxMailboxCompleteCallback() called"); + myPrintf("CAN_TxMailboxCompleteCallback() called"); // slightly yucky, but we don't want to block inside the IRQ handler //if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) >= 2) @@ -1397,25 +1363,72 @@ void MX_ADC2_Init() __HAL_ADC_ENABLE(&hadc2); } +#ifdef FEATURE_BUTTON void poweroff() { - // if (abs(speed) < 20) { // wait for the speed to drop, then shut down -> this is commented out for SAFETY reasons - buzzer.state.pattern = 0; - left.state.enable = right.state.enable = 0; - for (int i = 0; i < 8; i++) { - buzzer.state.freq = (uint8_t)i; - HAL_Delay(50); - } - HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_RESET); - for (int i = 0; i < 5; i++) - HAL_Delay(1000); - // } +// if (abs(speed) < 20) { // wait for the speed to drop, then shut down -> this is commented out for SAFETY reasons + buzzer.pattern = 0; + left.enable = false; + right.enable = 0; + + for (int i = 0; i < 8; i++) { + buzzer.freq = (uint8_t)i; + HAL_Delay(50); + } + HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_RESET); + for (int i = 0; i < 5; i++) + HAL_Delay(1000); +// } } +#endif + +void communicationTimeout() +{ + left.enable = false; + right.enable = true; + + // TODO all the other params + + buzzer.freq = 24; + buzzer.pattern = 1; + + HAL_GPIO_WritePin(LED_PORT, LED_PIN, GPIO_PIN_RESET); +} + +#ifdef MOTOR_TEST +void doMotorTest() +{ + timeout = 0; // proove, that the controlling code is still running + + left.state.enable = true; + left.state.ctrlMod = ControlMode::Voltage; + left.state.ctrlTyp = ControlType::FieldOrientedControl; + left.state.pwm = pwm; + left.state.iMotMax = 2; + + right.state.enable = true; + right.state.ctrlMod = ControlMode::Voltage; + right.state.ctrlTyp = ControlType::FieldOrientedControl; + right.state.pwm = pwm; + right.state.iMotMax = 2; + + constexpr auto pwmMax = 250; + + pwm += dir; + if (pwm > pwmMax) { + pwm = pwmMax; + dir = -1; + } else if (pwm < -pwmMax) { + pwm = -pwmMax; + dir = 1; + } +} +#endif #ifdef FEATURE_SERIAL_CONTROL void parseCommand() { - bool any_parsed{false}; + timeout = 0; // proove, that the controlling code is still running for (int i = 0; i < 1; i++) { @@ -1426,47 +1439,59 @@ void parseCommand() if (command.checksum != checksum) continue; - left.state = command.left; - right.state = command.right; + left.iDcMax = command.left.iDcMax; - buzzer.state = command.buzzer; + left.rtP.z_ctrlTypSel = uint8_t(command.left.ctrlTyp); + left.rtP.i_max = (command.left.iMotMax * A2BIT_CONV) << 4; + left.rtP.n_max = command.left.nMotMax << 4; + left.rtP.id_fieldWeakMax = (command.left.fieldWeakMax * A2BIT_CONV) << 4; + left.rtP.a_phaAdvMax = command.left.phaseAdvMax << 4; + left.rtU.z_ctrlModReq = uint8_t(command.left.ctrlMod); + left.rtU.r_inpTgt = command.left.pwm; + right.iDcMax = command.right.iDcMax; + + right.rtP.z_ctrlTypSel = uint8_t(command.right.ctrlTyp); + right.rtP.i_max = (command.right.iMotMax * A2BIT_CONV) << 4; // fixdt(1,16,4) + right.rtP.n_max = command.right.nMotMax << 4; // fixdt(1,16,4) + right.rtP.id_fieldWeakMax = (command.right.fieldWeakMax * A2BIT_CONV) << 4; // fixdt(1,16,4) + right.rtP.a_phaAdvMax = command.right.phaseAdvMax << 4; // fixdt(1,16,4) + right.rtU.z_ctrlModReq = uint8_t(command.right.ctrlMod); + right.rtU.r_inpTgt = command.right.pwm; + + buzzer.freq = command.buzzer.freq; + buzzer.pattern = command.buzzer.pattern; + +#ifdef FEATURE_BUTTON if (command.poweroff) poweroff(); +#endif HAL_GPIO_WritePin(LED_PORT, LED_PIN, command.led ? GPIO_PIN_RESET : GPIO_PIN_SET); command.start = Command::INVALID_HEADER; // Change the Start Frame for timeout detection in the next cycle timeoutCntSerial = 0; // Reset the timeout counter - any_parsed = true; - break; + return; } - if (!any_parsed) + if (timeoutCntSerial++ >= 100) // Timeout qualification { - if (timeoutCntSerial++ >= 100) // Timeout qualification + timeoutCntSerial = 100; // Limit timout counter value + + communicationTimeout(); + + // Check periodically the received Start Frame. Try to re-sync by reseting the DMA + if (main_loop_counter % 25 == 0) { - timeoutCntSerial = 100; // Limit timout counter value - - left.state = right.state = {.enable=true}; - - //buzzer.state = { 24, 1 }; - - HAL_GPIO_WritePin(LED_PORT, LED_PIN, GPIO_PIN_RESET); - - // Check periodically the received Start Frame. Try to re-sync by reseting the DMA - if (main_loop_counter % 25 == 0) - { #ifdef HUARN2 - HAL_UART_DMAStop(&huart2); - HAL_UART_Receive_DMA(&huart2, (uint8_t *)&command, sizeof(command)); + HAL_UART_DMAStop(&huart2); + HAL_UART_Receive_DMA(&huart2, (uint8_t *)&command, sizeof(command)); #endif #ifdef HUARN3 - HAL_UART_DMAStop(&huart3); - HAL_UART_Receive_DMA(&huart3, (uint8_t *)&command, sizeof(command)); + HAL_UART_DMAStop(&huart3); + HAL_UART_Receive_DMA(&huart3, (uint8_t *)&command, sizeof(command)); #endif - } } } } @@ -1527,104 +1552,29 @@ void sendFeedback() #endif #ifdef FEATURE_CAN +void parseCanCommand() +{ + timeout = 0; // proove, that the controlling code is still running + + const auto l = ++timeoutCntLeft; + const auto r = ++timeoutCntRight; + if (l >= 99 || r >= 99) + { + if (l > 100) + timeoutCntLeft = 100; + if (r > 100) + timeoutCntRight = 100; + + communicationTimeout(); + } +} + void sendCanFeedback() { - //myPrintf("sendCanFeedback() called"); - const auto free = HAL_CAN_GetTxMailboxesFreeLevel(&CanHandle); if (!free) return; - enum { // vv - DeviceTypeMotorController = 0b00000000000 - }; - - enum { // ..vv - MotorControllerRec = 0b00000000000, - MotorControllerSend = 0b00010000000, - }; - - enum { // ....vvvvv - MotorControllerDcLink = 0b00000000000, - MotorControllerSpeed = 0b00000000100, - MotorControllerError = 0b00000001000, - MotorControllerAngle = 0b00000001100, - MotorControllerDcPhaA = 0b00000010000, - MotorControllerDcPhaB = 0b00000010100, - MotorControllerDcPhaC = 0b00000011000, - MotorControllerChops = 0b00000011100, - MotorControllerHall = 0b00000100000, - MotorControllerVoltage = 0b00000100100, - MotorControllerTemp = 0b00000101000 - }; - - enum { // .........v - MotorControllerFront = 0b00000000000, - MotorControllerBack = 0b00000000010, - }; - - enum { // ..........v - MotorControllerLeft = 0b00000000000, - MotorControllerRight = 0b00000000001, - }; - - enum { - MotorControllerFrontLeftDcLink = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcLink | MotorControllerFront | MotorControllerLeft, - MotorControllerFrontRightDcLink = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcLink | MotorControllerFront | MotorControllerRight, - MotorControllerBackLeftDcLink = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcLink | MotorControllerBack | MotorControllerLeft, - MotorControllerBackRightDcLink = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcLink | MotorControllerBack | MotorControllerRight, - - MotorControllerFrontLeftSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerFront | MotorControllerLeft, - MotorControllerFrontRightSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerFront | MotorControllerRight, - MotorControllerBackLeftSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerBack | MotorControllerLeft, - MotorControllerBackRightSpeed = DeviceTypeMotorController | MotorControllerSend | MotorControllerSpeed | MotorControllerBack | MotorControllerRight, - - MotorControllerFrontLeftError = DeviceTypeMotorController | MotorControllerSend | MotorControllerError | MotorControllerFront | MotorControllerLeft, - MotorControllerFrontRightError = DeviceTypeMotorController | MotorControllerSend | MotorControllerError | MotorControllerFront | MotorControllerRight, - MotorControllerBackLeftError = DeviceTypeMotorController | MotorControllerSend | MotorControllerError | MotorControllerBack | MotorControllerLeft, - MotorControllerBackRightError = DeviceTypeMotorController | MotorControllerSend | MotorControllerError | MotorControllerBack | MotorControllerRight, - - MotorControllerFrontLeftAngle = DeviceTypeMotorController | MotorControllerSend | MotorControllerAngle | MotorControllerFront | MotorControllerLeft, - MotorControllerFrontRightAngle = DeviceTypeMotorController | MotorControllerSend | MotorControllerAngle | MotorControllerFront | MotorControllerRight, - MotorControllerBackLeftAngle = DeviceTypeMotorController | MotorControllerSend | MotorControllerAngle | MotorControllerBack | MotorControllerLeft, - MotorControllerBackRightAngle = DeviceTypeMotorController | MotorControllerSend | MotorControllerAngle | MotorControllerBack | MotorControllerRight, - - MotorControllerFrontLeftDcPhaA = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaA | MotorControllerFront | MotorControllerLeft, - MotorControllerFrontRightDcPhaA = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaA | MotorControllerFront | MotorControllerRight, - MotorControllerBackLeftDcPhaA = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaA | MotorControllerBack | MotorControllerLeft, - MotorControllerBackRightDcPhaA = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaA | MotorControllerBack | MotorControllerRight, - - MotorControllerFrontLeftDcPhaB = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaB | MotorControllerFront | MotorControllerLeft, - MotorControllerFrontRightDcPhaB = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaB | MotorControllerFront | MotorControllerRight, - MotorControllerBackLeftDcPhaB = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaB | MotorControllerBack | MotorControllerLeft, - MotorControllerBackRightDcPhaB = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaB | MotorControllerBack | MotorControllerRight, - - MotorControllerFrontLeftDcPhaC = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaC | MotorControllerFront | MotorControllerLeft, - MotorControllerFrontRightDcPhaC = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaC | MotorControllerFront | MotorControllerRight, - MotorControllerBackLeftDcPhaC = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaC | MotorControllerBack | MotorControllerLeft, - MotorControllerBackRightDcPhaC = DeviceTypeMotorController | MotorControllerSend | MotorControllerDcPhaC | MotorControllerBack | MotorControllerRight, - - MotorControllerFrontLeftChops = DeviceTypeMotorController | MotorControllerSend | MotorControllerChops | MotorControllerFront | MotorControllerLeft, - MotorControllerFrontRightChops = DeviceTypeMotorController | MotorControllerSend | MotorControllerChops | MotorControllerFront | MotorControllerRight, - MotorControllerBackLeftChops = DeviceTypeMotorController | MotorControllerSend | MotorControllerChops | MotorControllerBack | MotorControllerLeft, - MotorControllerBackRightChops = DeviceTypeMotorController | MotorControllerSend | MotorControllerChops | MotorControllerBack | MotorControllerRight, - - MotorControllerFrontLeftHall = DeviceTypeMotorController | MotorControllerSend | MotorControllerHall | MotorControllerFront | MotorControllerLeft, - MotorControllerFrontRightHall = DeviceTypeMotorController | MotorControllerSend | MotorControllerHall | MotorControllerFront | MotorControllerRight, - MotorControllerBackLeftHall = DeviceTypeMotorController | MotorControllerSend | MotorControllerHall | MotorControllerBack | MotorControllerLeft, - MotorControllerBackRightHall = DeviceTypeMotorController | MotorControllerSend | MotorControllerHall | MotorControllerBack | MotorControllerRight, - - MotorControllerFrontLeftVoltage = DeviceTypeMotorController | MotorControllerSend | MotorControllerVoltage | MotorControllerFront | MotorControllerLeft, - MotorControllerFrontRightVoltage = DeviceTypeMotorController | MotorControllerSend | MotorControllerVoltage | MotorControllerFront | MotorControllerRight, - MotorControllerBackLeftVoltage = DeviceTypeMotorController | MotorControllerSend | MotorControllerVoltage | MotorControllerBack | MotorControllerLeft, - MotorControllerBackRightVoltage = DeviceTypeMotorController | MotorControllerSend | MotorControllerVoltage | MotorControllerBack | MotorControllerRight, - - MotorControllerFrontLeftTemp = DeviceTypeMotorController | MotorControllerSend | MotorControllerTemp | MotorControllerFront | MotorControllerLeft, - MotorControllerFrontRightTemp = DeviceTypeMotorController | MotorControllerSend | MotorControllerTemp | MotorControllerFront | MotorControllerRight, - MotorControllerBackLeftTemp = DeviceTypeMotorController | MotorControllerSend | MotorControllerTemp | MotorControllerBack | MotorControllerLeft, - MotorControllerBackRightTemp = DeviceTypeMotorController | MotorControllerSend | MotorControllerTemp | MotorControllerBack | MotorControllerRight, - }; - enum SendState : uint8_t { LeftDcLink, RightDcLink, @@ -1845,7 +1795,6 @@ extern "C" void DMA1_Channel1_IRQHandler() /* USER CODE END DMA1_Channel1_IRQn 0 */ updateMotors(); - updateBatVoltage(); updateBuzzer(); /* USER CODE BEGIN DMA1_Channel1_IRQn 1 */ -- 2.50.1 From c9b436e65f9a69595bb67b08c2b292762926a981 Mon Sep 17 00:00:00 2001 From: 0xFEEDC0DE64 Date: Thu, 20 May 2021 21:28:48 +0200 Subject: [PATCH 06/15] CAN finishing work --- bobbycar-protocol | 2 +- main.cpp | 149 ++++++++++++++++++++++++++++++---------------- 2 files changed, 100 insertions(+), 51 deletions(-) diff --git a/bobbycar-protocol b/bobbycar-protocol index 236d1a2..994d0c9 160000 --- a/bobbycar-protocol +++ b/bobbycar-protocol @@ -1 +1 @@ -Subproject commit 236d1a2d33b31443238d6490889e3b7d29ee2119 +Subproject commit 994d0c95a72ed8e42a6875f771c778481073d470 diff --git a/main.cpp b/main.cpp index eed27a5..6efb661 100644 --- a/main.cpp +++ b/main.cpp @@ -146,6 +146,11 @@ volatile struct { std::atomic timeout; +#ifdef MOTOR_TEST +int pwm = 0; +int8_t dir = 1; +#endif + #ifdef FEATURE_SERIAL_CONTROL int16_t timeoutCntSerial = 0; // Timeout counter for Rx Serial command @@ -243,6 +248,8 @@ void parseCanCommand(); void sendCanFeedback(); #endif +void applyDefaultSettings(); + } // anonymous namespace int main() @@ -293,12 +300,6 @@ int main() left.rtP.r_fieldWeakHi = FIELD_WEAK_HI << 4; left.rtP.r_fieldWeakLo = FIELD_WEAK_LO << 4; - left.rtP.z_ctrlTypSel = uint8_t(ControlType::FieldOrientedControl); - left.rtP.i_max = (15 * A2BIT_CONV) << 4; - left.rtP.n_max = 1000 << 4; - left.rtP.id_fieldWeakMax = (5 * A2BIT_CONV) << 4; - left.rtP.a_phaAdvMax = 40 << 4; - right.rtP = rtP_Left; right.rtP.b_angleMeasEna = 0; right.rtP.z_selPhaCurMeasABC = CurrentMeasBC; @@ -307,12 +308,6 @@ int main() right.rtP.r_fieldWeakHi = FIELD_WEAK_HI << 4; right.rtP.r_fieldWeakLo = FIELD_WEAK_LO << 4; - right.rtP.z_ctrlTypSel = uint8_t(ControlType::FieldOrientedControl); - right.rtP.i_max = (15 * A2BIT_CONV) << 4; - right.rtP.n_max = 1000 << 4; - right.rtP.id_fieldWeakMax = (5 * A2BIT_CONV) << 4; - right.rtP.a_phaAdvMax = 40 << 4; - left.rtM.defaultParam = &left.rtP; left.rtM.dwork = &left.rtDW; left.rtM.inputs = &left.rtU; @@ -323,6 +318,8 @@ int main() right.rtM.inputs = &right.rtU; right.rtM.outputs = &right.rtY; + applyDefaultSettings(); + /* Initialize BLDC controllers */ BLDC_controller_initialize(&left.rtM); BLDC_controller_initialize(&right.rtM); @@ -345,11 +342,6 @@ int main() CAN_Init(); #endif -#ifdef MOTOR_TEST - int pwm = 0; - int8_t dir = 1; -#endif - #ifdef FEATURE_SERIAL_CONTROL #ifdef HUARN2 HAL_UART_Receive_DMA(&huart2, (uint8_t *)&command, sizeof(command)); @@ -835,10 +827,14 @@ void CAN_Init() sFilterConfig.FilterBank = 0; sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT; - sFilterConfig.FilterIdHigh = 0xFFFF; // TODO - sFilterConfig.FilterIdLow = 0xFFFF; // TODO - sFilterConfig.FilterMaskIdHigh = 0xFFFF; // TODO - sFilterConfig.FilterMaskIdLow = 0xFFFF; // TODO + + // + // TTRR.....FL + sFilterConfig.FilterIdHigh = 0b00000000000; // TODO + sFilterConfig.FilterIdLow = 0b00000000000; // TODO + sFilterConfig.FilterMaskIdHigh = 0b00000000000; // TODO + sFilterConfig.FilterMaskIdLow = 0b00000000000; // TODO + sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0; sFilterConfig.FilterActivation = ENABLE; sFilterConfig.SlaveStartFilterBank = 14; @@ -986,21 +982,45 @@ void CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *CanHandle) return; } - - myPrintf("StdId=%x %u ExtId=%x %u IDE=%x %u RTR=%x %u DLC=%x %u Timestamp=%x %u FilterMatchIndex=%x %u", - header.StdId, header.StdId, - header.ExtId, header.ExtId, - header.IDE, header.IDE, - header.RTR, header.RTR, - header.DLC, header.DLC, - header.Timestamp, header.Timestamp, - header.FilterMatchIndex, header.FilterMatchIndex - ); - - // TODO apply changes and reset timeout qualification - - timeoutCntLeft = 0; - timeoutCntRight = 0; + switch (header.StdId) + { + case MotorControllerFrontLeftEnable: /* myPrintf("MotorControllerFrontLeftEnable"); */ left.enable = *((bool *)buf); break; + case MotorControllerFrontRightEnable: /* myPrintf("MotorControllerFrontRightEnable"); */ right.enable = *((bool *)buf); break; + case MotorControllerFrontLeftInpTgt: /* myPrintf("MotorControllerFrontLeftInpTgt"); */ timeoutCntLeft = 0; left.rtU.r_inpTgt = *((int16_t*)buf); break; + case MotorControllerFrontRightInpTgt: /* myPrintf("MotorControllerFrontRightInpTgt"); */ timeoutCntRight = 0; right.rtU.r_inpTgt = *((int16_t*)buf); break; + case MotorControllerFrontLeftCtrlTyp: /* myPrintf("MotorControllerFrontLeftCtrlTyp"); */ left.rtP.z_ctrlTypSel = *((uint8_t*)buf); break; + case MotorControllerFrontRightCtrlTyp: /* myPrintf("MotorControllerFrontRightCtrlTyp"); */ right.rtP.z_ctrlTypSel = *((uint8_t*)buf); break; + case MotorControllerFrontLeftCtrlMod: /* myPrintf("MotorControllerFrontLeftCtrlMod"); */ left.rtU.z_ctrlModReq = *((uint8_t*)buf); break; + case MotorControllerFrontRightCtrlMod: /* myPrintf("MotorControllerFrontRightCtrlMod"); */ right.rtU.z_ctrlModReq = *((uint8_t*)buf); break; + case MotorControllerFrontLeftIMotMax: /* myPrintf("MotorControllerFrontLeftIMotMax"); */ left.rtP.i_max = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; + case MotorControllerFrontRightIMotMax: /* myPrintf("MotorControllerFrontRightIMotMax"); */ right.rtP.i_max = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; + case MotorControllerFrontLeftIDcMax: /* myPrintf("MotorControllerFrontLeftIDcMax"); */ left.iDcMax = *((uint8_t*)buf); break; + case MotorControllerFrontRightIDcMax: /* myPrintf("MotorControllerFrontRightIDcMax"); */ right.iDcMax = *((uint8_t*)buf); break; + case MotorControllerFrontLeftNMotMax: /* myPrintf("MotorControllerFrontLeftNMotMax"); */ left.rtP.n_max = *((uint16_t*)buf) << 4; break; + case MotorControllerFrontRightNMotMax: /* myPrintf("MotorControllerFrontRightNMotMax"); */ right.rtP.n_max = *((uint16_t*)buf) << 4; break; + case MotorControllerFrontLeftFieldWeakMax: /* myPrintf("MotorControllerFrontLeftFieldWeakMax"); */ left.rtP.id_fieldWeakMax = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; + case MotorControllerFrontRightFieldWeakMax: /* myPrintf("MotorControllerFrontRightFieldWeakMax"); */ right.rtP.id_fieldWeakMax = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; + case MotorControllerFrontLeftPhaseAdvMax: /* myPrintf("MotorControllerFrontLeftPhaseAdvMax"); */ left.rtP.a_phaAdvMax = *((uint16_t*)buf) << 4; break; + case MotorControllerFrontRightPhaseAdvMax: /* myPrintf("MotorControllerFrontRightPhaseAdvMax"); */ right.rtP.a_phaAdvMax = *((uint16_t*)buf) << 4; break; + case MotorControllerFrontLeftBuzzerFreq: /* myPrintf("MotorControllerFrontLeftBuzzerFreq"); */ buzzer.freq = *((uint8_t*)buf); break; + case MotorControllerFrontRightBuzzerFreq: /* myPrintf("MotorControllerFrontRightBuzzerFreq"); */ buzzer.freq = *((uint8_t*)buf); break; + case MotorControllerFrontLeftBuzzerPattern: /* myPrintf("MotorControllerFrontLeftBuzzerPattern"); */ buzzer.pattern = *((uint8_t*)buf); break; + case MotorControllerFrontRightBuzzerPattern: /* myPrintf("MotorControllerFrontRightBuzzerPattern"); */ buzzer.pattern = *((uint8_t*)buf); break; + case MotorControllerFrontLeftLed: /* myPrintf("MotorControllerFrontLeftLed"); */ break; + case MotorControllerFrontRightLed: /* myPrintf("MotorControllerFrontRightLed"); */ break; + case MotorControllerFrontLeftPoweroff: /* myPrintf("MotorControllerFrontLeftPoweroff"); */ break; + case MotorControllerFrontRightPoweroff: /* myPrintf("MotorControllerFrontRightPoweroff"); */ break; + default: + myPrintf("UNKNOWN StdId=%x %u ExtId=%x %u IDE=%x %u RTR=%x %u DLC=%x %u Timestamp=%x %u FilterMatchIndex=%x %u", + header.StdId, header.StdId, + header.ExtId, header.ExtId, + header.IDE, header.IDE, + header.RTR, header.RTR, + header.DLC, header.DLC, + header.Timestamp, header.Timestamp, + header.FilterMatchIndex, header.FilterMatchIndex + ); + } } void CAN_TxMailboxCompleteCallback(CAN_HandleTypeDef *hcan) @@ -1384,10 +1404,7 @@ void poweroff() void communicationTimeout() { - left.enable = false; - right.enable = true; - - // TODO all the other params + applyDefaultSettings(); buzzer.freq = 24; buzzer.pattern = 1; @@ -1400,17 +1417,26 @@ void doMotorTest() { timeout = 0; // proove, that the controlling code is still running - left.state.enable = true; - left.state.ctrlMod = ControlMode::Voltage; - left.state.ctrlTyp = ControlType::FieldOrientedControl; - left.state.pwm = pwm; - left.state.iMotMax = 2; - right.state.enable = true; - right.state.ctrlMod = ControlMode::Voltage; - right.state.ctrlTyp = ControlType::FieldOrientedControl; - right.state.pwm = pwm; - right.state.iMotMax = 2; + left.enable = true; + left.rtU.r_inpTgt = pwm; + left.rtP.z_ctrlTypSel = uint8_t(ControlType::FieldOrientedControl); + left.rtU.z_ctrlModReq = uint8_t(ControlMode::Speed); + left.rtP.i_max = (2 * A2BIT_CONV) << 4; + left.iDcMax = 4; + left.rtP.n_max = 1000 << 4; + left.rtP.id_fieldWeakMax = (0 * A2BIT_CONV) << 4; + left.rtP.a_phaAdvMax = 40 << 4; + + right.enable = true; + right.rtU.r_inpTgt = pwm; + right.rtP.z_ctrlTypSel = uint8_t(ControlType::FieldOrientedControl); + right.rtU.z_ctrlModReq = uint8_t(ControlMode::Speed); + right.rtP.i_max = (2 * A2BIT_CONV) << 4; + right.iDcMax = 4; + right.rtP.n_max = 1000 << 4; + right.rtP.id_fieldWeakMax = (0 * A2BIT_CONV) << 4; + right.rtP.a_phaAdvMax = 40 << 4; constexpr auto pwmMax = 250; @@ -1660,6 +1686,29 @@ void sendCanFeedback() } #endif +void applyDefaultSettings() +{ + left.enable = true; + left.rtU.r_inpTgt = 0; + left.rtP.z_ctrlTypSel = uint8_t(ControlType::FieldOrientedControl); + left.rtU.z_ctrlModReq = uint8_t(ControlMode::OpenMode); + left.rtP.i_max = (15 * A2BIT_CONV) << 4; + left.iDcMax = 17; + left.rtP.n_max = 1000 << 4; + left.rtP.id_fieldWeakMax = (5 * A2BIT_CONV) << 4; + left.rtP.a_phaAdvMax = 40 << 4; + + right.enable = true; + right.rtU.r_inpTgt = 0; + right.rtP.z_ctrlTypSel = uint8_t(ControlType::FieldOrientedControl); + right.rtU.z_ctrlModReq = uint8_t(ControlMode::OpenMode); + right.rtP.i_max = (15 * A2BIT_CONV) << 4; + right.iDcMax = 17; + right.rtP.n_max = 1000 << 4; + right.rtP.id_fieldWeakMax = (5 * A2BIT_CONV) << 4; + right.rtP.a_phaAdvMax = 40 << 4; +} + } // anonymous namespace /******************************************************************************/ -- 2.50.1 From bb10aa079652fab1f201d504a322543891e1b655 Mon Sep 17 00:00:00 2001 From: 0xFEEDC0DE64 Date: Thu, 20 May 2021 23:22:46 +0200 Subject: [PATCH 07/15] Implemented debug utilities --- CMakeLists.txt | 1 + bobbycar-protocol | 2 +- main.cpp | 299 ++++++++++++++++++++++++---------------------- 3 files changed, 160 insertions(+), 142 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6bc6fe2..2e319e8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -44,6 +44,7 @@ add_definitions(-DHUARN2) #add_definitions(-DFEATURE_SERIAL_FEEDBACK) add_definitions(-DLOG_TO_SERIAL) add_definitions(-DFEATURE_CAN) +#add_definitions(-DCAN_LOG_UNKNOWN_ADDR) add_executable(firmware.elf STM32CubeF1/Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal.c diff --git a/bobbycar-protocol b/bobbycar-protocol index 994d0c9..49fa47c 160000 --- a/bobbycar-protocol +++ b/bobbycar-protocol @@ -1 +1 @@ -Subproject commit 994d0c95a72ed8e42a6875f771c778481073d470 +Subproject commit 49fa47c90ac24de50a8f2b1eef040ac9738dd00f diff --git a/main.cpp b/main.cpp index 6efb661..d8f942d 100644 --- a/main.cpp +++ b/main.cpp @@ -245,6 +245,7 @@ void sendFeedback(); #ifdef FEATURE_CAN void parseCanCommand(); +void applyIncomingCanMessage(); void sendCanFeedback(); #endif @@ -353,6 +354,22 @@ int main() while (true) { + const auto DELAY_WITH_CAN_POLL = [](uint32_t Delay){ + uint32_t tickstart = HAL_GetTick(); + uint32_t wait = Delay; + + /* Add a freq to guarantee minimum wait */ + if (wait < HAL_MAX_DELAY) + { + wait += (uint32_t)(uwTickFreq); + } + + while ((HAL_GetTick() - tickstart) < wait) + { + applyIncomingCanMessage(); + } + }; + HAL_Delay(5); //delay in ms #ifdef MOTOR_TEST @@ -794,8 +811,6 @@ void CAN_Init() { myPrintf("CAN_Init() called"); - CAN_FilterTypeDef sFilterConfig; - /* Configure the CAN peripheral */ CanHandle.Instance = CANx; @@ -823,28 +838,33 @@ void CAN_Init() while (true); } - /* Configure the CAN Filter */ - sFilterConfig.FilterBank = 0; - sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; - sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT; - - // - // TTRR.....FL - sFilterConfig.FilterIdHigh = 0b00000000000; // TODO - sFilterConfig.FilterIdLow = 0b00000000000; // TODO - sFilterConfig.FilterMaskIdHigh = 0b00000000000; // TODO - sFilterConfig.FilterMaskIdLow = 0b00000000000; // TODO - - sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0; - sFilterConfig.FilterActivation = ENABLE; - sFilterConfig.SlaveStartFilterBank = 14; - - if (const auto result = HAL_CAN_ConfigFilter(&CanHandle, &sFilterConfig); result == HAL_OK) - myPrintf("HAL_CAN_ConfigFilter() succeeded"); - else { - myPrintf("HAL_CAN_ConfigFilter() failed with %i", result); - while (true); + /* Configure the CAN Filter */ + CAN_FilterTypeDef sFilterConfig; + sFilterConfig.FilterBank = 0; + sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; + sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT; + + // + // TTRR.....FL + sFilterConfig.FilterIdHigh = 0b00000000000; + sFilterConfig.FilterIdLow = 0b00000000000; + sFilterConfig.FilterMaskIdHigh = 0b00000000000; + sFilterConfig.FilterMaskIdLow = 0b11110000010; + // 0b0000.....0. + + + sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0; + sFilterConfig.FilterActivation = CAN_FILTER_ENABLE; + sFilterConfig.SlaveStartFilterBank = 14; + + if (const auto result = HAL_CAN_ConfigFilter(&CanHandle, &sFilterConfig); result == HAL_OK) + myPrintf("HAL_CAN_ConfigFilter() succeeded"); + else + { + myPrintf("HAL_CAN_ConfigFilter() failed with %i", result); + while (true); + } } if (const auto result = HAL_CAN_RegisterCallback(&CanHandle, HAL_CAN_RX_FIFO0_MSG_PENDING_CB_ID, CAN_RxFifo0MsgPendingCallback); result == HAL_OK) @@ -900,13 +920,16 @@ void CAN_Init() while (true); } - /* Activate CAN TX notification */ - if (const auto result = HAL_CAN_ActivateNotification(&CanHandle, CAN_IT_TX_MAILBOX_EMPTY); result == HAL_OK) - myPrintf("HAL_CAN_ActivateNotification() CAN_IT_TX_MAILBOX_EMPTY succeeded"); - else + if (false) { - myPrintf("HAL_CAN_ActivateNotification() CAN_IT_TX_MAILBOX_EMPTY failed with %i", result); - while (true); + /* Activate CAN TX notification */ + if (const auto result = HAL_CAN_ActivateNotification(&CanHandle, CAN_IT_TX_MAILBOX_EMPTY); result == HAL_OK) + myPrintf("HAL_CAN_ActivateNotification() CAN_IT_TX_MAILBOX_EMPTY succeeded"); + else + { + myPrintf("HAL_CAN_ActivateNotification() CAN_IT_TX_MAILBOX_EMPTY failed with %i", result); + while (true); + } } } @@ -973,54 +996,7 @@ void CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *CanHandle) { //myPrintf("CAN_RxFifo0MsgPendingCallback() called"); - CAN_RxHeaderTypeDef header; - uint8_t buf[8]; - if (const auto result = HAL_CAN_GetRxMessage(CanHandle, CAN_RX_FIFO0, &header, buf); result != HAL_OK) - { - myPrintf("HAL_CAN_GetRxMessage() failed with %i", result); - //while (true); - return; - } - - switch (header.StdId) - { - case MotorControllerFrontLeftEnable: /* myPrintf("MotorControllerFrontLeftEnable"); */ left.enable = *((bool *)buf); break; - case MotorControllerFrontRightEnable: /* myPrintf("MotorControllerFrontRightEnable"); */ right.enable = *((bool *)buf); break; - case MotorControllerFrontLeftInpTgt: /* myPrintf("MotorControllerFrontLeftInpTgt"); */ timeoutCntLeft = 0; left.rtU.r_inpTgt = *((int16_t*)buf); break; - case MotorControllerFrontRightInpTgt: /* myPrintf("MotorControllerFrontRightInpTgt"); */ timeoutCntRight = 0; right.rtU.r_inpTgt = *((int16_t*)buf); break; - case MotorControllerFrontLeftCtrlTyp: /* myPrintf("MotorControllerFrontLeftCtrlTyp"); */ left.rtP.z_ctrlTypSel = *((uint8_t*)buf); break; - case MotorControllerFrontRightCtrlTyp: /* myPrintf("MotorControllerFrontRightCtrlTyp"); */ right.rtP.z_ctrlTypSel = *((uint8_t*)buf); break; - case MotorControllerFrontLeftCtrlMod: /* myPrintf("MotorControllerFrontLeftCtrlMod"); */ left.rtU.z_ctrlModReq = *((uint8_t*)buf); break; - case MotorControllerFrontRightCtrlMod: /* myPrintf("MotorControllerFrontRightCtrlMod"); */ right.rtU.z_ctrlModReq = *((uint8_t*)buf); break; - case MotorControllerFrontLeftIMotMax: /* myPrintf("MotorControllerFrontLeftIMotMax"); */ left.rtP.i_max = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; - case MotorControllerFrontRightIMotMax: /* myPrintf("MotorControllerFrontRightIMotMax"); */ right.rtP.i_max = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; - case MotorControllerFrontLeftIDcMax: /* myPrintf("MotorControllerFrontLeftIDcMax"); */ left.iDcMax = *((uint8_t*)buf); break; - case MotorControllerFrontRightIDcMax: /* myPrintf("MotorControllerFrontRightIDcMax"); */ right.iDcMax = *((uint8_t*)buf); break; - case MotorControllerFrontLeftNMotMax: /* myPrintf("MotorControllerFrontLeftNMotMax"); */ left.rtP.n_max = *((uint16_t*)buf) << 4; break; - case MotorControllerFrontRightNMotMax: /* myPrintf("MotorControllerFrontRightNMotMax"); */ right.rtP.n_max = *((uint16_t*)buf) << 4; break; - case MotorControllerFrontLeftFieldWeakMax: /* myPrintf("MotorControllerFrontLeftFieldWeakMax"); */ left.rtP.id_fieldWeakMax = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; - case MotorControllerFrontRightFieldWeakMax: /* myPrintf("MotorControllerFrontRightFieldWeakMax"); */ right.rtP.id_fieldWeakMax = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; - case MotorControllerFrontLeftPhaseAdvMax: /* myPrintf("MotorControllerFrontLeftPhaseAdvMax"); */ left.rtP.a_phaAdvMax = *((uint16_t*)buf) << 4; break; - case MotorControllerFrontRightPhaseAdvMax: /* myPrintf("MotorControllerFrontRightPhaseAdvMax"); */ right.rtP.a_phaAdvMax = *((uint16_t*)buf) << 4; break; - case MotorControllerFrontLeftBuzzerFreq: /* myPrintf("MotorControllerFrontLeftBuzzerFreq"); */ buzzer.freq = *((uint8_t*)buf); break; - case MotorControllerFrontRightBuzzerFreq: /* myPrintf("MotorControllerFrontRightBuzzerFreq"); */ buzzer.freq = *((uint8_t*)buf); break; - case MotorControllerFrontLeftBuzzerPattern: /* myPrintf("MotorControllerFrontLeftBuzzerPattern"); */ buzzer.pattern = *((uint8_t*)buf); break; - case MotorControllerFrontRightBuzzerPattern: /* myPrintf("MotorControllerFrontRightBuzzerPattern"); */ buzzer.pattern = *((uint8_t*)buf); break; - case MotorControllerFrontLeftLed: /* myPrintf("MotorControllerFrontLeftLed"); */ break; - case MotorControllerFrontRightLed: /* myPrintf("MotorControllerFrontRightLed"); */ break; - case MotorControllerFrontLeftPoweroff: /* myPrintf("MotorControllerFrontLeftPoweroff"); */ break; - case MotorControllerFrontRightPoweroff: /* myPrintf("MotorControllerFrontRightPoweroff"); */ break; - default: - myPrintf("UNKNOWN StdId=%x %u ExtId=%x %u IDE=%x %u RTR=%x %u DLC=%x %u Timestamp=%x %u FilterMatchIndex=%x %u", - header.StdId, header.StdId, - header.ExtId, header.ExtId, - header.IDE, header.IDE, - header.RTR, header.RTR, - header.DLC, header.DLC, - header.Timestamp, header.Timestamp, - header.FilterMatchIndex, header.FilterMatchIndex - ); - } + applyIncomingCanMessage(); } void CAN_TxMailboxCompleteCallback(CAN_HandleTypeDef *hcan) @@ -1595,43 +1571,87 @@ void parseCanCommand() } } +void applyIncomingCanMessage() +{ + CAN_RxHeaderTypeDef header; + uint8_t buf[8]; + if (const auto result = HAL_CAN_GetRxMessage(&CanHandle, CAN_RX_FIFO0, &header, buf); result != HAL_OK) + { + myPrintf("HAL_CAN_GetRxMessage() failed with %i", result); + //while (true); + return; + } + + switch (header.StdId) + { + case MotorControllerFrontLeftEnable: /* myPrintf("MotorControllerFrontLeftEnable"); */ left.enable = *((bool *)buf); break; + case MotorControllerFrontRightEnable: /* myPrintf("MotorControllerFrontRightEnable"); */ right.enable = *((bool *)buf); break; + case MotorControllerFrontLeftInpTgt: /* myPrintf("MotorControllerFrontLeftInpTgt"); */ timeoutCntLeft = 0; left.rtU.r_inpTgt = *((int16_t*)buf); break; + case MotorControllerFrontRightInpTgt: /* myPrintf("MotorControllerFrontRightInpTgt"); */ timeoutCntRight = 0; right.rtU.r_inpTgt = *((int16_t*)buf); break; + case MotorControllerFrontLeftCtrlTyp: /* myPrintf("MotorControllerFrontLeftCtrlTyp"); */ left.rtP.z_ctrlTypSel = *((uint8_t*)buf); break; + case MotorControllerFrontRightCtrlTyp: /* myPrintf("MotorControllerFrontRightCtrlTyp"); */ right.rtP.z_ctrlTypSel = *((uint8_t*)buf); break; + case MotorControllerFrontLeftCtrlMod: /* myPrintf("MotorControllerFrontLeftCtrlMod"); */ left.rtU.z_ctrlModReq = *((uint8_t*)buf); break; + case MotorControllerFrontRightCtrlMod: /* myPrintf("MotorControllerFrontRightCtrlMod"); */ right.rtU.z_ctrlModReq = *((uint8_t*)buf); break; + case MotorControllerFrontLeftIMotMax: /* myPrintf("MotorControllerFrontLeftIMotMax"); */ left.rtP.i_max = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; + case MotorControllerFrontRightIMotMax: /* myPrintf("MotorControllerFrontRightIMotMax"); */ right.rtP.i_max = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; + case MotorControllerFrontLeftIDcMax: /* myPrintf("MotorControllerFrontLeftIDcMax"); */ left.iDcMax = *((uint8_t*)buf); break; + case MotorControllerFrontRightIDcMax: /* myPrintf("MotorControllerFrontRightIDcMax"); */ right.iDcMax = *((uint8_t*)buf); break; + case MotorControllerFrontLeftNMotMax: /* myPrintf("MotorControllerFrontLeftNMotMax"); */ left.rtP.n_max = *((uint16_t*)buf) << 4; break; + case MotorControllerFrontRightNMotMax: /* myPrintf("MotorControllerFrontRightNMotMax"); */ right.rtP.n_max = *((uint16_t*)buf) << 4; break; + case MotorControllerFrontLeftFieldWeakMax: /* myPrintf("MotorControllerFrontLeftFieldWeakMax"); */ left.rtP.id_fieldWeakMax = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; + case MotorControllerFrontRightFieldWeakMax: /* myPrintf("MotorControllerFrontRightFieldWeakMax"); */ right.rtP.id_fieldWeakMax = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; + case MotorControllerFrontLeftPhaseAdvMax: /* myPrintf("MotorControllerFrontLeftPhaseAdvMax"); */ left.rtP.a_phaAdvMax = *((uint16_t*)buf) << 4; break; + case MotorControllerFrontRightPhaseAdvMax: /* myPrintf("MotorControllerFrontRightPhaseAdvMax"); */ right.rtP.a_phaAdvMax = *((uint16_t*)buf) << 4; break; + case MotorControllerFrontLeftBuzzerFreq: /* myPrintf("MotorControllerFrontLeftBuzzerFreq"); */ buzzer.freq = *((uint8_t*)buf); break; + case MotorControllerFrontRightBuzzerFreq: /* myPrintf("MotorControllerFrontRightBuzzerFreq"); */ buzzer.freq = *((uint8_t*)buf); break; + case MotorControllerFrontLeftBuzzerPattern: /* myPrintf("MotorControllerFrontLeftBuzzerPattern"); */ buzzer.pattern = *((uint8_t*)buf); break; + case MotorControllerFrontRightBuzzerPattern: /* myPrintf("MotorControllerFrontRightBuzzerPattern"); */ buzzer.pattern = *((uint8_t*)buf); break; + case MotorControllerFrontLeftLed: /* myPrintf("MotorControllerFrontLeftLed"); */ break; + case MotorControllerFrontRightLed: /* myPrintf("MotorControllerFrontRightLed"); */ break; + case MotorControllerFrontLeftPoweroff: /* myPrintf("MotorControllerFrontLeftPoweroff"); */ break; + case MotorControllerFrontRightPoweroff: /* myPrintf("MotorControllerFrontRightPoweroff"); */ break; + default: +#ifndef CAN_LOG_UNKNOWN_ADDR + if constexpr (false) +#endif + myPrintf("UNKNOWN %c%c %c%c %c%c%c%c%c %c%c %s", + header.StdId&1024?'1':'0', + header.StdId&512?'1':'0', + + header.StdId&256?'1':'0', + header.StdId&128?'1':'0', + + header.StdId&64?'1':'0', + header.StdId&32?'1':'0', + header.StdId&16?'1':'0', + header.StdId&8?'1':'0', + header.StdId&4?'1':'0', + + header.StdId&2?'1':'0', + header.StdId&1?'1':'0', + + bobbycarCanIdDesc(header.StdId) + ); + + if constexpr (false) + myPrintf("UNKNOWN StdId=%x %u ExtId=%x %u IDE=%x %u RTR=%x %u DLC=%x %u Timestamp=%x %u FilterMatchIndex=%x %u", + header.StdId, header.StdId, + header.ExtId, header.ExtId, + header.IDE, header.IDE, + header.RTR, header.RTR, + header.DLC, header.DLC, + header.Timestamp, header.Timestamp, + header.FilterMatchIndex, header.FilterMatchIndex + ); + } +} + void sendCanFeedback() { const auto free = HAL_CAN_GetTxMailboxesFreeLevel(&CanHandle); if (!free) return; - enum SendState : uint8_t { - LeftDcLink, - RightDcLink, - LeftSpeed, - RightSpeed, - LeftError, - RightError, - LeftAngle, - RightAngle, - LeftDcPhaA, - RightDcPhaA, - LeftDcPhaB, - RightDcPhaB, - LeftDcPhaC, - RightDcPhaC, - LeftChops, - RightChops, - LeftHall, - RightHall, - LeftVoltage, - RightVoltage, - LeftTemp, - RightTemp, - Restart - }; - - static union { - SendState sendState{LeftDcLink}; - uint8_t asUint8; - }; - constexpr auto send = [](uint32_t addr, auto value){ CAN_TxHeaderTypeDef header; header.StdId = addr; @@ -1652,37 +1672,34 @@ void sendCanFeedback() } }; - switch (sendState) + static uint8_t whichToSend{}; + + switch (whichToSend++) { - case LeftDcLink: /* myPrintf("LeftCurrent free=%u", free); */ send(MotorControllerFrontLeftDcLink, left. rtU.i_DCLink); break; - case RightDcLink: /* myPrintf("RightCurrent free=%u", free); */ send(MotorControllerFrontRightDcLink, right.rtU.i_DCLink); break; - case LeftSpeed: /* myPrintf("LeftSpeed free=%u", free); */ send(MotorControllerFrontLeftSpeed, left. rtY.n_mot); break; - case RightSpeed: /* myPrintf("RightSpeed free=%u", free); */ send(MotorControllerFrontRightSpeed, right.rtY.n_mot); break; - case LeftError: /* myPrintf("LeftError free=%u", free); */ send(MotorControllerFrontLeftError, left. rtY.z_errCode); break; - case RightError: /* myPrintf("RightError free=%u", free); */ send(MotorControllerFrontRightError, right.rtY.z_errCode); break; - case LeftAngle: /* myPrintf("LeftAngle free=%u", free); */ send(MotorControllerFrontLeftAngle, left. rtY.a_elecAngle); break; - case RightAngle: /* myPrintf("RightAngle free=%u", free); */ send(MotorControllerFrontRightAngle, right.rtY.a_elecAngle); break; - case LeftDcPhaA: /* myPrintf("LeftDcPhaA free=%u", free); */ send(MotorControllerFrontLeftDcPhaA, left. rtY.DC_phaA); break; - case RightDcPhaA: /* myPrintf("RightDcPhaA free=%u", free); */ send(MotorControllerFrontRightDcPhaA, right.rtY.DC_phaA); break; - case LeftDcPhaB: /* myPrintf("LeftDcPhaB free=%u", free); */ send(MotorControllerFrontLeftDcPhaB, left. rtY.DC_phaB); break; - case RightDcPhaB: /* myPrintf("RightDcPhaB free=%u", free); */ send(MotorControllerFrontRightDcPhaB, right.rtY.DC_phaB); break; - case LeftDcPhaC: /* myPrintf("LeftDcPhaC free=%u", free); */ send(MotorControllerFrontLeftDcPhaC, left. rtY.DC_phaC); break; - case RightDcPhaC: /* myPrintf("RightDcPhaC free=%u", free); */ send(MotorControllerFrontRightDcPhaC, right.rtY.DC_phaC); break; - case LeftChops: /* myPrintf("LeftChops free=%u", free); */ send(MotorControllerFrontLeftChops, left. chops); break; - case RightChops: /* myPrintf("RightChops free=%u", free); */ send(MotorControllerFrontRightChops, right.chops); break; - case LeftHall: /* myPrintf("LeftHall free=%u", free); */ send(MotorControllerFrontLeftHall, left.hallBits()); break; - case RightHall: /* myPrintf("RightHall free=%u", free); */ send(MotorControllerFrontRightHall, right.hallBits()); break; - case LeftVoltage: /* myPrintf("LeftVoltage free=%u", free); */ send(MotorControllerFrontLeftVoltage, batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); break; - case RightVoltage: /* myPrintf("RightVoltage free=%u", free); */ send(MotorControllerFrontRightVoltage, batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); break; - case LeftTemp: /* myPrintf("LeftTemp free=%u", free); */ send(MotorControllerFrontLeftTemp, board_temp_deg_c); break; - case RightTemp: /* myPrintf("RightTemp free=%u", free); */ send(MotorControllerFrontRightTemp, board_temp_deg_c); break; - default: __builtin_unreachable(); + case 0: /* myPrintf("LeftCurrent free=%u", free); */ send(MotorControllerFrontLeftDcLink, left. rtU.i_DCLink); break; + case 1: /* myPrintf("RightCurrent free=%u", free); */ send(MotorControllerFrontRightDcLink, right.rtU.i_DCLink); break; + case 2: /* myPrintf("LeftSpeed free=%u", free); */ send(MotorControllerFrontLeftSpeed, left. rtY.n_mot); break; + case 3: /* myPrintf("RightSpeed free=%u", free); */ send(MotorControllerFrontRightSpeed, right.rtY.n_mot); break; + case 4: /* myPrintf("LeftError free=%u", free); */ send(MotorControllerFrontLeftError, left. rtY.z_errCode); break; + case 5: /* myPrintf("RightError free=%u", free); */ send(MotorControllerFrontRightError, right.rtY.z_errCode); break; + case 6: /* myPrintf("LeftAngle free=%u", free); */ send(MotorControllerFrontLeftAngle, left. rtY.a_elecAngle); break; + case 7: /* myPrintf("RightAngle free=%u", free); */ send(MotorControllerFrontRightAngle, right.rtY.a_elecAngle); break; + case 8: /* myPrintf("LeftDcPhaA free=%u", free); */ send(MotorControllerFrontLeftDcPhaA, left. rtY.DC_phaA); break; + case 9: /* myPrintf("RightDcPhaA free=%u", free); */ send(MotorControllerFrontRightDcPhaA, right.rtY.DC_phaA); break; + case 10: /* myPrintf("LeftDcPhaB free=%u", free); */ send(MotorControllerFrontLeftDcPhaB, left. rtY.DC_phaB); break; + case 11: /* myPrintf("RightDcPhaB free=%u", free); */ send(MotorControllerFrontRightDcPhaB, right.rtY.DC_phaB); break; + case 12: /* myPrintf("LeftDcPhaC free=%u", free); */ send(MotorControllerFrontLeftDcPhaC, left. rtY.DC_phaC); break; + case 13: /* myPrintf("RightDcPhaC free=%u", free); */ send(MotorControllerFrontRightDcPhaC, right.rtY.DC_phaC); break; + case 14: /* myPrintf("LeftChops free=%u", free); */ send(MotorControllerFrontLeftChops, left. chops); break; + case 15: /* myPrintf("RightChops free=%u", free); */ send(MotorControllerFrontRightChops, right.chops); break; + case 16: /* myPrintf("LeftHall free=%u", free); */ send(MotorControllerFrontLeftHall, left.hallBits()); break; + case 17: /* myPrintf("RightHall free=%u", free); */ send(MotorControllerFrontRightHall, right.hallBits()); break; + case 18: /* myPrintf("LeftVoltage free=%u", free); */ send(MotorControllerFrontLeftVoltage, batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); break; + case 19: /* myPrintf("RightVoltage free=%u", free); */ send(MotorControllerFrontRightVoltage, batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); break; + case 20: /* myPrintf("LeftTemp free=%u", free); */ send(MotorControllerFrontLeftTemp, board_temp_deg_c); break; + case 21: /* myPrintf("RightTemp free=%u", free); */ send(MotorControllerFrontRightTemp, board_temp_deg_c); whichToSend = 0; break; + default: myPrintf("unreachable"); } - - asUint8++; - - if (sendState >= Restart) - sendState = LeftDcLink; } #endif @@ -1692,8 +1709,8 @@ void applyDefaultSettings() left.rtU.r_inpTgt = 0; left.rtP.z_ctrlTypSel = uint8_t(ControlType::FieldOrientedControl); left.rtU.z_ctrlModReq = uint8_t(ControlMode::OpenMode); - left.rtP.i_max = (15 * A2BIT_CONV) << 4; - left.iDcMax = 17; + left.rtP.i_max = (5 * A2BIT_CONV) << 4; + left.iDcMax = 7; left.rtP.n_max = 1000 << 4; left.rtP.id_fieldWeakMax = (5 * A2BIT_CONV) << 4; left.rtP.a_phaAdvMax = 40 << 4; @@ -1702,8 +1719,8 @@ void applyDefaultSettings() right.rtU.r_inpTgt = 0; right.rtP.z_ctrlTypSel = uint8_t(ControlType::FieldOrientedControl); right.rtU.z_ctrlModReq = uint8_t(ControlMode::OpenMode); - right.rtP.i_max = (15 * A2BIT_CONV) << 4; - right.iDcMax = 17; + 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; -- 2.50.1 From 0fbd4e62d0d692768f1caf81383d8da27a57d21a Mon Sep 17 00:00:00 2001 From: 0xFEEDC0DE64 Date: Fri, 21 May 2021 00:07:17 +0200 Subject: [PATCH 08/15] back/front board compile time defines --- CMakeLists.txt | 1 + bobbycar-protocol | 2 +- main.cpp | 107 +++++++++++++++++++++++++--------------------- 3 files changed, 60 insertions(+), 50 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2e319e8..79d1267 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -45,6 +45,7 @@ add_definitions(-DHUARN2) add_definitions(-DLOG_TO_SERIAL) add_definitions(-DFEATURE_CAN) #add_definitions(-DCAN_LOG_UNKNOWN_ADDR) +add_definitions(-DIS_BACK) add_executable(firmware.elf STM32CubeF1/Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal.c diff --git a/bobbycar-protocol b/bobbycar-protocol index 49fa47c..fee1175 160000 --- a/bobbycar-protocol +++ b/bobbycar-protocol @@ -1 +1 @@ -Subproject commit 49fa47c90ac24de50a8f2b1eef040ac9738dd00f +Subproject commit fee1175bf819826222086aa1e3c7fbb66d37a33c diff --git a/main.cpp b/main.cpp index d8f942d..5e00566 100644 --- a/main.cpp +++ b/main.cpp @@ -98,6 +98,13 @@ CAN_HandleTypeDef CanHandle; char logBuffer[512]; #endif +constexpr bool isBackBoard = +#ifdef IS_BACK + true +#else + false +#endif + ; template void myPrintf(const char (&format)[formatLength], Targs ... args) @@ -1584,32 +1591,33 @@ void applyIncomingCanMessage() switch (header.StdId) { - case MotorControllerFrontLeftEnable: /* myPrintf("MotorControllerFrontLeftEnable"); */ left.enable = *((bool *)buf); break; - case MotorControllerFrontRightEnable: /* myPrintf("MotorControllerFrontRightEnable"); */ right.enable = *((bool *)buf); break; - case MotorControllerFrontLeftInpTgt: /* myPrintf("MotorControllerFrontLeftInpTgt"); */ timeoutCntLeft = 0; left.rtU.r_inpTgt = *((int16_t*)buf); break; - case MotorControllerFrontRightInpTgt: /* myPrintf("MotorControllerFrontRightInpTgt"); */ timeoutCntRight = 0; right.rtU.r_inpTgt = *((int16_t*)buf); break; - case MotorControllerFrontLeftCtrlTyp: /* myPrintf("MotorControllerFrontLeftCtrlTyp"); */ left.rtP.z_ctrlTypSel = *((uint8_t*)buf); break; - case MotorControllerFrontRightCtrlTyp: /* myPrintf("MotorControllerFrontRightCtrlTyp"); */ right.rtP.z_ctrlTypSel = *((uint8_t*)buf); break; - case MotorControllerFrontLeftCtrlMod: /* myPrintf("MotorControllerFrontLeftCtrlMod"); */ left.rtU.z_ctrlModReq = *((uint8_t*)buf); break; - case MotorControllerFrontRightCtrlMod: /* myPrintf("MotorControllerFrontRightCtrlMod"); */ right.rtU.z_ctrlModReq = *((uint8_t*)buf); break; - case MotorControllerFrontLeftIMotMax: /* myPrintf("MotorControllerFrontLeftIMotMax"); */ left.rtP.i_max = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; - case MotorControllerFrontRightIMotMax: /* myPrintf("MotorControllerFrontRightIMotMax"); */ right.rtP.i_max = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; - case MotorControllerFrontLeftIDcMax: /* myPrintf("MotorControllerFrontLeftIDcMax"); */ left.iDcMax = *((uint8_t*)buf); break; - case MotorControllerFrontRightIDcMax: /* myPrintf("MotorControllerFrontRightIDcMax"); */ right.iDcMax = *((uint8_t*)buf); break; - case MotorControllerFrontLeftNMotMax: /* myPrintf("MotorControllerFrontLeftNMotMax"); */ left.rtP.n_max = *((uint16_t*)buf) << 4; break; - case MotorControllerFrontRightNMotMax: /* myPrintf("MotorControllerFrontRightNMotMax"); */ right.rtP.n_max = *((uint16_t*)buf) << 4; break; - case MotorControllerFrontLeftFieldWeakMax: /* myPrintf("MotorControllerFrontLeftFieldWeakMax"); */ left.rtP.id_fieldWeakMax = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; - case MotorControllerFrontRightFieldWeakMax: /* myPrintf("MotorControllerFrontRightFieldWeakMax"); */ right.rtP.id_fieldWeakMax = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; - case MotorControllerFrontLeftPhaseAdvMax: /* myPrintf("MotorControllerFrontLeftPhaseAdvMax"); */ left.rtP.a_phaAdvMax = *((uint16_t*)buf) << 4; break; - case MotorControllerFrontRightPhaseAdvMax: /* myPrintf("MotorControllerFrontRightPhaseAdvMax"); */ right.rtP.a_phaAdvMax = *((uint16_t*)buf) << 4; break; - case MotorControllerFrontLeftBuzzerFreq: /* myPrintf("MotorControllerFrontLeftBuzzerFreq"); */ buzzer.freq = *((uint8_t*)buf); break; - case MotorControllerFrontRightBuzzerFreq: /* myPrintf("MotorControllerFrontRightBuzzerFreq"); */ buzzer.freq = *((uint8_t*)buf); break; - case MotorControllerFrontLeftBuzzerPattern: /* myPrintf("MotorControllerFrontLeftBuzzerPattern"); */ buzzer.pattern = *((uint8_t*)buf); break; - case MotorControllerFrontRightBuzzerPattern: /* myPrintf("MotorControllerFrontRightBuzzerPattern"); */ buzzer.pattern = *((uint8_t*)buf); break; - case MotorControllerFrontLeftLed: /* myPrintf("MotorControllerFrontLeftLed"); */ break; - case MotorControllerFrontRightLed: /* myPrintf("MotorControllerFrontRightLed"); */ break; - case MotorControllerFrontLeftPoweroff: /* myPrintf("MotorControllerFrontLeftPoweroff"); */ break; - case MotorControllerFrontRightPoweroff: /* myPrintf("MotorControllerFrontRightPoweroff"); */ break; + using namespace bobbycar::can; + case MotorController::Command::Enable: left.enable = *((bool *)buf); break; + case MotorController::Command::Enable: right.enable = *((bool *)buf); break; + case MotorController::Command::InpTgt: timeoutCntLeft = 0; left.rtU.r_inpTgt = *((int16_t*)buf); break; + case MotorController::Command::InpTgt: timeoutCntRight = 0; right.rtU.r_inpTgt = *((int16_t*)buf); break; + case MotorController::Command::CtrlTyp: left.rtP.z_ctrlTypSel = *((uint8_t*)buf); break; + case MotorController::Command::CtrlTyp: right.rtP.z_ctrlTypSel = *((uint8_t*)buf); break; + case MotorController::Command::CtrlMod: left.rtU.z_ctrlModReq = *((uint8_t*)buf); break; + case MotorController::Command::CtrlMod: right.rtU.z_ctrlModReq = *((uint8_t*)buf); break; + case MotorController::Command::IMotMax: left.rtP.i_max = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; + case MotorController::Command::IMotMax: right.rtP.i_max = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; + case MotorController::Command::IDcMax: left.iDcMax = *((uint8_t*)buf); break; + case MotorController::Command::IDcMax: right.iDcMax = *((uint8_t*)buf); break; + case MotorController::Command::NMotMax: left.rtP.n_max = *((uint16_t*)buf) << 4; break; + case MotorController::Command::NMotMax: right.rtP.n_max = *((uint16_t*)buf) << 4; break; + case MotorController::Command::FieldWeakMax: left.rtP.id_fieldWeakMax = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; + case MotorController::Command::FieldWeakMax: right.rtP.id_fieldWeakMax = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; + case MotorController::Command::PhaseAdvMax: left.rtP.a_phaAdvMax = *((uint16_t*)buf) << 4; break; + case MotorController::Command::PhaseAdvMax: right.rtP.a_phaAdvMax = *((uint16_t*)buf) << 4; break; + case MotorController::Command::BuzzerFreq: buzzer.freq = *((uint8_t*)buf); break; + case MotorController::Command::BuzzerFreq: buzzer.freq = *((uint8_t*)buf); break; + case MotorController::Command::BuzzerPattern: buzzer.pattern = *((uint8_t*)buf); break; + case MotorController::Command::BuzzerPattern: buzzer.pattern = *((uint8_t*)buf); break; + case MotorController::Command::Led: break; + case MotorController::Command::Led: break; + case MotorController::Command::Poweroff: break; + case MotorController::Command::Poweroff: break; default: #ifndef CAN_LOG_UNKNOWN_ADDR if constexpr (false) @@ -1624,7 +1632,7 @@ void applyIncomingCanMessage() header.StdId&64?'1':'0', header.StdId&32?'1':'0', header.StdId&16?'1':'0', - header.StdId&8?'1':'0', + header.StdId&8?' 1':'0', header.StdId&4?'1':'0', header.StdId&2?'1':'0', @@ -1676,28 +1684,29 @@ void sendCanFeedback() switch (whichToSend++) { - case 0: /* myPrintf("LeftCurrent free=%u", free); */ send(MotorControllerFrontLeftDcLink, left. rtU.i_DCLink); break; - case 1: /* myPrintf("RightCurrent free=%u", free); */ send(MotorControllerFrontRightDcLink, right.rtU.i_DCLink); break; - case 2: /* myPrintf("LeftSpeed free=%u", free); */ send(MotorControllerFrontLeftSpeed, left. rtY.n_mot); break; - case 3: /* myPrintf("RightSpeed free=%u", free); */ send(MotorControllerFrontRightSpeed, right.rtY.n_mot); break; - case 4: /* myPrintf("LeftError free=%u", free); */ send(MotorControllerFrontLeftError, left. rtY.z_errCode); break; - case 5: /* myPrintf("RightError free=%u", free); */ send(MotorControllerFrontRightError, right.rtY.z_errCode); break; - case 6: /* myPrintf("LeftAngle free=%u", free); */ send(MotorControllerFrontLeftAngle, left. rtY.a_elecAngle); break; - case 7: /* myPrintf("RightAngle free=%u", free); */ send(MotorControllerFrontRightAngle, right.rtY.a_elecAngle); break; - case 8: /* myPrintf("LeftDcPhaA free=%u", free); */ send(MotorControllerFrontLeftDcPhaA, left. rtY.DC_phaA); break; - case 9: /* myPrintf("RightDcPhaA free=%u", free); */ send(MotorControllerFrontRightDcPhaA, right.rtY.DC_phaA); break; - case 10: /* myPrintf("LeftDcPhaB free=%u", free); */ send(MotorControllerFrontLeftDcPhaB, left. rtY.DC_phaB); break; - case 11: /* myPrintf("RightDcPhaB free=%u", free); */ send(MotorControllerFrontRightDcPhaB, right.rtY.DC_phaB); break; - case 12: /* myPrintf("LeftDcPhaC free=%u", free); */ send(MotorControllerFrontLeftDcPhaC, left. rtY.DC_phaC); break; - case 13: /* myPrintf("RightDcPhaC free=%u", free); */ send(MotorControllerFrontRightDcPhaC, right.rtY.DC_phaC); break; - case 14: /* myPrintf("LeftChops free=%u", free); */ send(MotorControllerFrontLeftChops, left. chops); break; - case 15: /* myPrintf("RightChops free=%u", free); */ send(MotorControllerFrontRightChops, right.chops); break; - case 16: /* myPrintf("LeftHall free=%u", free); */ send(MotorControllerFrontLeftHall, left.hallBits()); break; - case 17: /* myPrintf("RightHall free=%u", free); */ send(MotorControllerFrontRightHall, right.hallBits()); break; - case 18: /* myPrintf("LeftVoltage free=%u", free); */ send(MotorControllerFrontLeftVoltage, batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); break; - case 19: /* myPrintf("RightVoltage free=%u", free); */ send(MotorControllerFrontRightVoltage, batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); break; - case 20: /* myPrintf("LeftTemp free=%u", free); */ send(MotorControllerFrontLeftTemp, board_temp_deg_c); break; - case 21: /* myPrintf("RightTemp free=%u", free); */ send(MotorControllerFrontRightTemp, board_temp_deg_c); whichToSend = 0; break; + using namespace bobbycar::can; + case 0: send(MotorController::Feedback::DcLink, left. rtU.i_DCLink); break; + case 1: send(MotorController::Feedback::DcLink, right.rtU.i_DCLink); break; + case 2: send(MotorController::Feedback::Speed, left. rtY.n_mot); break; + case 3: send(MotorController::Feedback::Speed, right.rtY.n_mot); break; + case 4: send(MotorController::Feedback::Error, left. rtY.z_errCode); break; + case 5: send(MotorController::Feedback::Error, right.rtY.z_errCode); break; + case 6: send(MotorController::Feedback::Angle, left. rtY.a_elecAngle); break; + case 7: send(MotorController::Feedback::Angle, right.rtY.a_elecAngle); break; + case 8: send(MotorController::Feedback::DcPhaA, left. rtY.DC_phaA); break; + case 9: send(MotorController::Feedback::DcPhaA, right.rtY.DC_phaA); break; + case 10: send(MotorController::Feedback::DcPhaB, left. rtY.DC_phaB); break; + case 11: send(MotorController::Feedback::DcPhaB, right.rtY.DC_phaB); break; + case 12: send(MotorController::Feedback::DcPhaC, left. rtY.DC_phaC); break; + case 13: send(MotorController::Feedback::DcPhaC, right.rtY.DC_phaC); break; + case 14: send(MotorController::Feedback::Chops, left. chops); break; + case 15: send(MotorController::Feedback::Chops, right.chops); break; + case 16: send(MotorController::Feedback::Hall, left.hallBits()); break; + case 17: send(MotorController::Feedback::Hall, right.hallBits()); break; + case 18: send(MotorController::Feedback::Voltage, batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); break; + case 19: send(MotorController::Feedback::Voltage, batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); break; + case 20: send(MotorController::Feedback::Temp, board_temp_deg_c); break; + case 21: send(MotorController::Feedback::Temp, board_temp_deg_c); whichToSend = 0; break; default: myPrintf("unreachable"); } } -- 2.50.1 From a031d1d7048f90fd10e73e777d9a18ce415cad16 Mon Sep 17 00:00:00 2001 From: 0xFEEDC0DE64 Date: Sat, 22 May 2021 04:54:31 +0200 Subject: [PATCH 09/15] More compile defines --- CMakeLists.txt | 6 ++++-- config.h | 2 +- defines.h | 24 ++++++++++++++++++++++++ main.cpp | 4 +++- 4 files changed, 32 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 79d1267..62aec4b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -36,13 +36,15 @@ include_directories( #add_definitions(-DMOTOR_TEST) add_definitions(-DFEATURE_IGNORE_OTHER_MOTOR) +add_definitions(-DGSCHISSENES_HALL) +#add_definitions(-DGSCHISSENE_PWM_FREQ) #add_definitions(-DFEATURE_BUTTON) add_definitions(-DPETERS_PLATINE) -add_definitions(-DHUARN2) +#add_definitions(-DHUARN2) #add_definitions(-DHUARN3) #add_definitions(-DFEATURE_SERIAL_CONTROL) #add_definitions(-DFEATURE_SERIAL_FEEDBACK) -add_definitions(-DLOG_TO_SERIAL) +#add_definitions(-DLOG_TO_SERIAL) add_definitions(-DFEATURE_CAN) #add_definitions(-DCAN_LOG_UNKNOWN_ADDR) add_definitions(-DIS_BACK) diff --git a/config.h b/config.h index 6afe11f..0e1d0d7 100644 --- a/config.h +++ b/config.h @@ -1,6 +1,6 @@ #pragma once -#ifdef PETERS_PLATINE +#ifdef GSCHISSENE_PWM_FREQ #define PWM_FREQ 12000 // PWM frequency in Hz #else #define PWM_FREQ 16000 // PWM frequency in Hz diff --git a/defines.h b/defines.h index 67e30e4..604d407 100644 --- a/defines.h +++ b/defines.h @@ -23,33 +23,57 @@ #include "stm32f1xx_hal.h" #ifdef PETERS_PLATINE +#ifdef GSCHISSENES_HALL + #define LEFT_HALL_U_PIN GPIO_PIN_10 + #define LEFT_HALL_V_PIN GPIO_PIN_11 + #define LEFT_HALL_W_PIN GPIO_PIN_12 +#else #define LEFT_HALL_U_PIN GPIO_PIN_12 #define LEFT_HALL_V_PIN GPIO_PIN_11 #define LEFT_HALL_W_PIN GPIO_PIN_10 +#endif #define LEFT_HALL_U_PORT GPIOC #define LEFT_HALL_V_PORT GPIOC #define LEFT_HALL_W_PORT GPIOC +#ifdef GSCHISSENES_HALL + #define RIGHT_HALL_U_PIN GPIO_PIN_7 + #define RIGHT_HALL_V_PIN GPIO_PIN_6 + #define RIGHT_HALL_W_PIN GPIO_PIN_5 +#else #define RIGHT_HALL_U_PIN GPIO_PIN_5 #define RIGHT_HALL_V_PIN GPIO_PIN_6 #define RIGHT_HALL_W_PIN GPIO_PIN_7 +#endif #define RIGHT_HALL_U_PORT GPIOB #define RIGHT_HALL_V_PORT GPIOB #define RIGHT_HALL_W_PORT GPIOB +#else +#ifdef GSCHISSENES_HALL + #define LEFT_HALL_U_PIN GPIO_PIN_7 + #define LEFT_HALL_V_PIN GPIO_PIN_6 + #define LEFT_HALL_W_PIN GPIO_PIN_5 #else #define LEFT_HALL_U_PIN GPIO_PIN_5 #define LEFT_HALL_V_PIN GPIO_PIN_6 #define LEFT_HALL_W_PIN GPIO_PIN_7 +#endif #define LEFT_HALL_U_PORT GPIOB #define LEFT_HALL_V_PORT GPIOB #define LEFT_HALL_W_PORT GPIOB +#ifdef GSCHISSENES_HALL + #define RIGHT_HALL_U_PIN GPIO_PIN_12 + #define RIGHT_HALL_V_PIN GPIO_PIN_11 + #define RIGHT_HALL_W_PIN GPIO_PIN_10 +#else #define RIGHT_HALL_U_PIN GPIO_PIN_10 #define RIGHT_HALL_V_PIN GPIO_PIN_11 #define RIGHT_HALL_W_PIN GPIO_PIN_12 +#endif #define RIGHT_HALL_U_PORT GPIOC #define RIGHT_HALL_V_PORT GPIOC diff --git a/main.cpp b/main.cpp index 5e00566..df4565f 100644 --- a/main.cpp +++ b/main.cpp @@ -373,7 +373,9 @@ int main() while ((HAL_GetTick() - tickstart) < wait) { +#ifdef FEATURE_CAN applyIncomingCanMessage(); +#endif } }; @@ -1421,7 +1423,7 @@ void doMotorTest() right.rtP.id_fieldWeakMax = (0 * A2BIT_CONV) << 4; right.rtP.a_phaAdvMax = 40 << 4; - constexpr auto pwmMax = 250; + constexpr auto pwmMax = 400; pwm += dir; if (pwm > pwmMax) { -- 2.50.1 From 58b308a2b1a07b41ec5521583bcebdf0f33ea2df Mon Sep 17 00:00:00 2001 From: 0xFEEDC0DE64 Date: Sun, 23 May 2021 00:43:42 +0200 Subject: [PATCH 10/15] Fixed disable mosfets stuck --- bobbycar-protocol | 2 +- main.cpp | 8 ++++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/bobbycar-protocol b/bobbycar-protocol index fee1175..059a98a 160000 --- a/bobbycar-protocol +++ b/bobbycar-protocol @@ -1 +1 @@ -Subproject commit fee1175bf819826222086aa1e3c7fbb66d37a33c +Subproject commit 059a98a1a5a6f4817184a3962eb4f46ed2ddf42b diff --git a/main.cpp b/main.cpp index df4565f..adc22e4 100644 --- a/main.cpp +++ b/main.cpp @@ -1171,7 +1171,11 @@ void MX_TIM_Init() #endif sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET; +#ifdef PETERS_PLATINE + sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; +#else sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_SET; +#endif HAL_TIM_PWM_ConfigChannel(&htim_right, &sConfigOC, TIM_CHANNEL_1); HAL_TIM_PWM_ConfigChannel(&htim_right, &sConfigOC, TIM_CHANNEL_2); HAL_TIM_PWM_ConfigChannel(&htim_right, &sConfigOC, TIM_CHANNEL_3); @@ -1216,7 +1220,11 @@ void MX_TIM_Init() #endif sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET; +#ifdef PETERS_PLATINE + sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; +#else sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_SET; +#endif HAL_TIM_PWM_ConfigChannel(&htim_left, &sConfigOC, TIM_CHANNEL_1); HAL_TIM_PWM_ConfigChannel(&htim_left, &sConfigOC, TIM_CHANNEL_2); HAL_TIM_PWM_ConfigChannel(&htim_left, &sConfigOC, TIM_CHANNEL_3); -- 2.50.1 From 0e014ad6c2ed1ebb8cd820ccb52ea74de3994e4a Mon Sep 17 00:00:00 2001 From: 0xFEEDC0DE64 Date: Sun, 23 May 2021 15:51:29 +0200 Subject: [PATCH 11/15] More improvements --- main.cpp | 188 ++++++++++++++++++++++++++++++------------------------- 1 file changed, 103 insertions(+), 85 deletions(-) diff --git a/main.cpp b/main.cpp index adc22e4..7d994f6 100644 --- a/main.cpp +++ b/main.cpp @@ -45,6 +45,8 @@ extern const P rtP_Left; // default settings defined in BLDC_controller_data.c } namespace { +const P &defaultP{rtP_Left}; + TIM_HandleTypeDef htim_right; TIM_HandleTypeDef htim_left; ADC_HandleTypeDef hadc1; @@ -180,10 +182,6 @@ int16_t offsetdcl = 2000; int16_t offsetdcr = 2000; 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; struct { @@ -193,9 +191,9 @@ struct { ExtU rtU; /* External inputs */ ExtY rtY; /* External outputs */ - bool enable{true}; - int16_t iDcMax{17}; - uint32_t chops{}; + std::atomic enable{true}; + std::atomic iDcMax{7}; + std::atomic chops{}; uint8_t hallBits() const { @@ -209,8 +207,6 @@ struct { uint32_t timer = 0; } buzzer; -void filtLowPass32(int16_t u, uint16_t coef, int32_t *y); - void SystemClock_Config(); #ifdef HUARN2 @@ -256,6 +252,12 @@ void applyIncomingCanMessage(); void sendCanFeedback(); #endif +#ifdef FEATURE_BUTTON +void handleButton(); +#endif + +void updateSensors(); + void applyDefaultSettings(); } // anonymous namespace @@ -296,7 +298,7 @@ int main() enum { CurrentMeasAB, CurrentMeasBC, CurrentMeasAC }; - left.rtP = rtP_Left; + left.rtP = defaultP; left.rtP.b_angleMeasEna = 0; #ifdef PETERS_PLATINE left.rtP.z_selPhaCurMeasABC = CurrentMeasBC; @@ -308,7 +310,7 @@ int main() left.rtP.r_fieldWeakHi = FIELD_WEAK_HI << 4; left.rtP.r_fieldWeakLo = FIELD_WEAK_LO << 4; - right.rtP = rtP_Left; + right.rtP = defaultP; right.rtP.b_angleMeasEna = 0; right.rtP.z_selPhaCurMeasABC = CurrentMeasBC; right.rtP.b_diagEna = DIAG_ENA; @@ -361,7 +363,8 @@ int main() 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 wait = Delay; @@ -373,14 +376,16 @@ int main() while ((HAL_GetTick() - tickstart) < wait) { -#ifdef FEATURE_CAN applyIncomingCanMessage(); -#endif } }; + //DELAY_WITH_CAN_POLL(5); //delay in ms +#endif HAL_Delay(5); //delay in ms + updateSensors(); + #ifdef MOTOR_TEST doMotorTest(); #endif @@ -400,29 +405,9 @@ int main() #endif #ifdef FEATURE_BUTTON - 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 - } - } + handleButton(); #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++; } } @@ -480,24 +465,26 @@ void updateMotors() #endif 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) 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) right.chops++; 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) // 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; else LEFT_TIM->BDTR |= TIM_BDTR_MOE; - if (chopR || timeoutVal > 500 || !right.enable) + if (chopR || timeoutVal > 500 || !rightEnable) RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE; else RIGHT_TIM->BDTR |= TIM_BDTR_MOE; @@ -524,8 +511,8 @@ void updateMotors() #endif ; - const bool enableLFin = left.enable && 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 enableLFin = leftEnable && left.rtY.z_errCode == 0 && (right.rtY.z_errCode == 0 || ignoreOtherMotor); + const bool enableRFin = rightEnable && (left.rtY.z_errCode == 0 || ignoreOtherMotor) && right.rtY.z_errCode == 0; // ========================= LEFT MOTOR ============================ // Get hall sensors values @@ -590,30 +577,6 @@ void updateMotors() 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 @@ -1620,6 +1583,10 @@ void applyIncomingCanMessage() case MotorController::Command::FieldWeakMax: right.rtP.id_fieldWeakMax = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; case MotorController::Command::PhaseAdvMax: left.rtP.a_phaAdvMax = *((uint16_t*)buf) << 4; break; case MotorController::Command::PhaseAdvMax: right.rtP.a_phaAdvMax = *((uint16_t*)buf) << 4; break; + case MotorController::Command::CruiseCtrlEna: left.rtP.b_cruiseCtrlEna = *((bool*)buf); break; + case MotorController::Command::CruiseCtrlEna: right.rtP.b_cruiseCtrlEna = *((bool*)buf); break; + case MotorController::Command::CruiseMotTgt: left.rtP.n_cruiseMotTgt = *((int16_T*)buf); break; + case MotorController::Command::CruiseMotTgt: right.rtP.n_cruiseMotTgt = *((int16_T*)buf); break; case MotorController::Command::BuzzerFreq: buzzer.freq = *((uint8_t*)buf); break; case MotorController::Command::BuzzerFreq: buzzer.freq = *((uint8_t*)buf); break; case MotorController::Command::BuzzerPattern: buzzer.pattern = *((uint8_t*)buf); break; @@ -1709,8 +1676,8 @@ void sendCanFeedback() case 11: send(MotorController::Feedback::DcPhaB, right.rtY.DC_phaB); break; case 12: send(MotorController::Feedback::DcPhaC, left. rtY.DC_phaC); break; case 13: send(MotorController::Feedback::DcPhaC, right.rtY.DC_phaC); break; - case 14: send(MotorController::Feedback::Chops, left. chops); break; - case 15: send(MotorController::Feedback::Chops, right.chops); break; + case 14: send(MotorController::Feedback::Chops, left. chops.exchange(0)); break; + case 15: send(MotorController::Feedback::Chops, right.chops.exchange(0)); break; case 16: send(MotorController::Feedback::Hall, left.hallBits()); break; case 17: send(MotorController::Feedback::Hall, right.hallBits()); break; case 18: send(MotorController::Feedback::Voltage, batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); break; @@ -1722,27 +1689,78 @@ void sendCanFeedback() } #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() { - left.enable = true; - left.rtU.r_inpTgt = 0; - left.rtP.z_ctrlTypSel = uint8_t(ControlType::FieldOrientedControl); - left.rtU.z_ctrlModReq = uint8_t(ControlMode::OpenMode); - left.rtP.i_max = (5 * A2BIT_CONV) << 4; - left.iDcMax = 7; - left.rtP.n_max = 1000 << 4; - left.rtP.id_fieldWeakMax = (5 * A2BIT_CONV) << 4; - left.rtP.a_phaAdvMax = 40 << 4; + constexpr auto doIt = [](auto &motor){ + motor.enable = true; + motor.rtU.r_inpTgt = 0; + motor.rtP.z_ctrlTypSel = uint8_t(ControlType::FieldOrientedControl); + motor.rtU.z_ctrlModReq = uint8_t(ControlMode::OpenMode); + motor.rtP.i_max = (5 * A2BIT_CONV) << 4; + motor.iDcMax = 7; + motor.rtP.n_max = 1000 << 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; - right.rtU.r_inpTgt = 0; - 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; + doIt(left); + doIt(right); } } // anonymous namespace -- 2.50.1 From facbbcf798864c274b1cd1b9cab286f85a533122 Mon Sep 17 00:00:00 2001 From: 0xFEEDC0DE64 Date: Sun, 23 May 2021 17:30:14 +0200 Subject: [PATCH 12/15] More refactorings --- main.cpp | 42 +++++++++++++++++++++--------------------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/main.cpp b/main.cpp index 7d994f6..096ff1a 100644 --- a/main.cpp +++ b/main.cpp @@ -68,6 +68,19 @@ DMA_HandleTypeDef hdma_usart3_rx; DMA_HandleTypeDef hdma_usart3_tx; #endif +volatile struct { + uint16_t dcr; + uint16_t dcl; + uint16_t rl1; + uint16_t rl2; + uint16_t rr1; + uint16_t rr2; + uint16_t batt1; + uint16_t l_tx2; + uint16_t temp; + uint16_t l_rx2; +} adc_buffer; + #ifdef FEATURE_CAN CAN_HandleTypeDef CanHandle; @@ -138,19 +151,6 @@ void myPrintf(const char (&format)[formatLength], Targs ... args) #endif } -volatile struct { - uint16_t dcr; - uint16_t dcl; - uint16_t rl1; - uint16_t rl2; - uint16_t rr1; - uint16_t rr2; - uint16_t batt1; - uint16_t l_tx2; - uint16_t temp; - uint16_t l_rx2; -} adc_buffer; - // ############################################################################### std::atomic timeout; @@ -173,14 +173,6 @@ std::atomic timeoutCntRight = 0; uint32_t main_loop_counter; -uint16_t offsetcount = 0; -int16_t offsetrl1 = 2000; -int16_t offsetrl2 = 2000; -int16_t offsetrr1 = 2000; -int16_t offsetrr2 = 2000; -int16_t offsetdcl = 2000; -int16_t offsetdcr = 2000; - int16_t batVoltage = (400 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE; int16_t board_temp_deg_c; @@ -433,6 +425,14 @@ void updateMotors() { DMA1->IFCR = DMA_IFCR_CTCIF1; + static uint16_t offsetcount{}; + static int16_t offsetrl1{2000}; + static int16_t offsetrl2{2000}; + static int16_t offsetrr1{2000}; + static int16_t offsetrr2{2000}; + static int16_t offsetdcl{2000}; + static int16_t offsetdcr{2000}; + if (offsetcount < 2000) // calibrate ADC offsets { offsetcount++; -- 2.50.1 From d27eb54fe30cd0e49a6a3c6cca853112dcfabffe Mon Sep 17 00:00:00 2001 From: 0xFEEDC0DE64 Date: Sun, 23 May 2021 17:43:44 +0200 Subject: [PATCH 13/15] Cleanups --- config.h | 8 +--- main.cpp | 109 +++++++++++++++++++++++++++++-------------------------- 2 files changed, 59 insertions(+), 58 deletions(-) diff --git a/config.h b/config.h index 0e1d0d7..d495c51 100644 --- a/config.h +++ b/config.h @@ -44,13 +44,7 @@ #define BAT_CALIB_REAL_VOLTAGE 3970 // input voltage measured by multimeter (multiplied by 100). For example 43.00 V * 100 = 4300 #define BAT_CALIB_ADC 1492 // adc-value measured by mainboard (value nr 5 on UART debug output) -#define BAT_CELLS 10 // battery number of cells. Normal Hoverboard battery: 10s -#define BAT_LOW_LVL1_ENABLE 0 // to beep or not to beep, 1 or 0 -#define BAT_LOW_LVL2_ENABLE 1 // to beep or not to beep, 1 or 0 -#define BAT_LOW_LVL1 (360 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE // gently beeps at this voltage level. [V*100/cell]. In this case 3.60 V/cell -#define BAT_LOW_LVL2 (350 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE // your battery is almost empty. Charge now! [V*100/cell]. In this case 3.50 V/cell -#define BAT_LOW_DEAD (337 * BAT_CELLS * BAT_CALIB_ADC) / BAT_CALIB_REAL_VOLTAGE // undervoltage poweroff. (while not driving) [V*100/cell]. In this case 3.37 V/cell - +#define BAT_CELLS 12 // battery number of cells. Normal Hoverboard battery: 10s /* Board overheat detection: the sensor is inside the STM/GD chip. * It is very inaccurate without calibration (up to 45°C). So only enable this funcion after calibration! diff --git a/main.cpp b/main.cpp index 096ff1a..0845129 100644 --- a/main.cpp +++ b/main.cpp @@ -1565,36 +1565,43 @@ void applyIncomingCanMessage() switch (header.StdId) { using namespace bobbycar::can; - case MotorController::Command::Enable: left.enable = *((bool *)buf); break; - case MotorController::Command::Enable: right.enable = *((bool *)buf); break; - case MotorController::Command::InpTgt: timeoutCntLeft = 0; left.rtU.r_inpTgt = *((int16_t*)buf); break; - case MotorController::Command::InpTgt: timeoutCntRight = 0; right.rtU.r_inpTgt = *((int16_t*)buf); break; - case MotorController::Command::CtrlTyp: left.rtP.z_ctrlTypSel = *((uint8_t*)buf); break; - case MotorController::Command::CtrlTyp: right.rtP.z_ctrlTypSel = *((uint8_t*)buf); break; - case MotorController::Command::CtrlMod: left.rtU.z_ctrlModReq = *((uint8_t*)buf); break; - case MotorController::Command::CtrlMod: right.rtU.z_ctrlModReq = *((uint8_t*)buf); break; - case MotorController::Command::IMotMax: left.rtP.i_max = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; - case MotorController::Command::IMotMax: right.rtP.i_max = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; - case MotorController::Command::IDcMax: left.iDcMax = *((uint8_t*)buf); break; - case MotorController::Command::IDcMax: right.iDcMax = *((uint8_t*)buf); break; - case MotorController::Command::NMotMax: left.rtP.n_max = *((uint16_t*)buf) << 4; break; - case MotorController::Command::NMotMax: right.rtP.n_max = *((uint16_t*)buf) << 4; break; - case MotorController::Command::FieldWeakMax: left.rtP.id_fieldWeakMax = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; - case MotorController::Command::FieldWeakMax: right.rtP.id_fieldWeakMax = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; - case MotorController::Command::PhaseAdvMax: left.rtP.a_phaAdvMax = *((uint16_t*)buf) << 4; break; - case MotorController::Command::PhaseAdvMax: right.rtP.a_phaAdvMax = *((uint16_t*)buf) << 4; break; - case MotorController::Command::CruiseCtrlEna: left.rtP.b_cruiseCtrlEna = *((bool*)buf); break; - case MotorController::Command::CruiseCtrlEna: right.rtP.b_cruiseCtrlEna = *((bool*)buf); break; - case MotorController::Command::CruiseMotTgt: left.rtP.n_cruiseMotTgt = *((int16_T*)buf); break; - case MotorController::Command::CruiseMotTgt: right.rtP.n_cruiseMotTgt = *((int16_T*)buf); break; - case MotorController::Command::BuzzerFreq: buzzer.freq = *((uint8_t*)buf); break; - case MotorController::Command::BuzzerFreq: buzzer.freq = *((uint8_t*)buf); break; - case MotorController::Command::BuzzerPattern: buzzer.pattern = *((uint8_t*)buf); break; - case MotorController::Command::BuzzerPattern: buzzer.pattern = *((uint8_t*)buf); break; - case MotorController::Command::Led: break; - case MotorController::Command::Led: break; - case MotorController::Command::Poweroff: break; - case MotorController::Command::Poweroff: break; + case MotorController::Command::Enable: left .enable = *((bool *)buf); break; + case MotorController ::Command::Enable: right.enable = *((bool *)buf); break; + case MotorController::Command::InpTgt: left. rtU.r_inpTgt = *((int16_t*)buf); timeoutCntLeft = 0; break; + case MotorController ::Command::InpTgt: right.rtU.r_inpTgt = *((int16_t*)buf); timeoutCntRight = 0; break; + case MotorController::Command::CtrlTyp: left .rtP.z_ctrlTypSel = *((uint8_t*)buf); break; + case MotorController ::Command::CtrlTyp: right.rtP.z_ctrlTypSel = *((uint8_t*)buf); break; + case MotorController::Command::CtrlMod: left .rtU.z_ctrlModReq = *((uint8_t*)buf); break; + case MotorController ::Command::CtrlMod: right.rtU.z_ctrlModReq = *((uint8_t*)buf); break; + case MotorController::Command::IMotMax: left .rtP.i_max = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; + case MotorController ::Command::IMotMax: right.rtP.i_max = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; + case MotorController::Command::IDcMax: left .iDcMax = *((uint8_t*)buf); break; + case MotorController ::Command::IDcMax: right.iDcMax = *((uint8_t*)buf); break; + case MotorController::Command::NMotMax: left .rtP.n_max = *((uint16_t*)buf) << 4; break; + case MotorController ::Command::NMotMax: right.rtP.n_max = *((uint16_t*)buf) << 4; break; + case MotorController::Command::FieldWeakMax: left .rtP.id_fieldWeakMax = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; + case MotorController ::Command::FieldWeakMax: right.rtP.id_fieldWeakMax = (*((uint8_t*)buf) * A2BIT_CONV) << 4; break; + case MotorController::Command::PhaseAdvMax: left .rtP.a_phaAdvMax = *((uint16_t*)buf) << 4; break; + case MotorController ::Command::PhaseAdvMax: right.rtP.a_phaAdvMax = *((uint16_t*)buf) << 4; break; + case MotorController::Command::CruiseCtrlEna: left .rtP.b_cruiseCtrlEna = *((bool*)buf); break; + case MotorController ::Command::CruiseCtrlEna: right.rtP.b_cruiseCtrlEna = *((bool*)buf); break; + case MotorController::Command::CruiseMotTgt: left .rtP.n_cruiseMotTgt = *((int16_T*)buf); break; + case MotorController ::Command::CruiseMotTgt: right.rtP.n_cruiseMotTgt = *((int16_T*)buf); break; + case MotorController::Command::BuzzerFreq: + case MotorController ::Command::BuzzerFreq: buzzer.freq = *((uint8_t*)buf); break; + case MotorController::Command::BuzzerPattern: + case MotorController ::Command::BuzzerPattern: buzzer.pattern = *((uint8_t*)buf); break; + case MotorController::Command::Led: + case MotorController ::Command::Led: + HAL_GPIO_WritePin(LED_PORT, LED_PIN, *((bool*)buf) ? GPIO_PIN_SET : GPIO_PIN_RESET); + break; + case MotorController::Command::Poweroff: + case MotorController::Command::Poweroff: +#ifdef FEATURE_BUTTON + if (*((bool*)buf)) + poweroff(); +#endif + break; default: #ifndef CAN_LOG_UNKNOWN_ADDR if constexpr (false) @@ -1609,7 +1616,7 @@ void applyIncomingCanMessage() header.StdId&64?'1':'0', header.StdId&32?'1':'0', header.StdId&16?'1':'0', - header.StdId&8?' 1':'0', + header.StdId&8?'1':'0', header.StdId&4?'1':'0', header.StdId&2?'1':'0', @@ -1662,28 +1669,28 @@ void sendCanFeedback() switch (whichToSend++) { using namespace bobbycar::can; - case 0: send(MotorController::Feedback::DcLink, left. rtU.i_DCLink); break; - case 1: send(MotorController::Feedback::DcLink, right.rtU.i_DCLink); break; - case 2: send(MotorController::Feedback::Speed, left. rtY.n_mot); break; - case 3: send(MotorController::Feedback::Speed, right.rtY.n_mot); break; - case 4: send(MotorController::Feedback::Error, left. rtY.z_errCode); break; - case 5: send(MotorController::Feedback::Error, right.rtY.z_errCode); break; - case 6: send(MotorController::Feedback::Angle, left. rtY.a_elecAngle); break; - case 7: send(MotorController::Feedback::Angle, right.rtY.a_elecAngle); break; - case 8: send(MotorController::Feedback::DcPhaA, left. rtY.DC_phaA); break; - case 9: send(MotorController::Feedback::DcPhaA, right.rtY.DC_phaA); break; - case 10: send(MotorController::Feedback::DcPhaB, left. rtY.DC_phaB); break; - case 11: send(MotorController::Feedback::DcPhaB, right.rtY.DC_phaB); break; - case 12: send(MotorController::Feedback::DcPhaC, left. rtY.DC_phaC); break; - case 13: send(MotorController::Feedback::DcPhaC, right.rtY.DC_phaC); break; + case 0: send(MotorController::Feedback::DcLink, left. rtU.i_DCLink); break; + case 1: send(MotorController:: Feedback::DcLink, right.rtU.i_DCLink); break; + case 2: send(MotorController::Feedback::Speed, left. rtY.n_mot); break; + case 3: send(MotorController:: Feedback::Speed, right.rtY.n_mot); break; + case 4: send(MotorController::Feedback::Error, left. rtY.z_errCode); break; + case 5: send(MotorController:: Feedback::Error, right.rtY.z_errCode); break; + case 6: send(MotorController::Feedback::Angle, left. rtY.a_elecAngle); break; + case 7: send(MotorController:: Feedback::Angle, right.rtY.a_elecAngle); break; + case 8: send(MotorController::Feedback::DcPhaA, left. rtY.DC_phaA); break; + case 9: send(MotorController:: Feedback::DcPhaA, right.rtY.DC_phaA); break; + case 10: send(MotorController::Feedback::DcPhaB, left. rtY.DC_phaB); break; + case 11: send(MotorController:: Feedback::DcPhaB, right.rtY.DC_phaB); break; + case 12: send(MotorController::Feedback::DcPhaC, left. rtY.DC_phaC); break; + case 13: send(MotorController:: Feedback::DcPhaC, right.rtY.DC_phaC); break; case 14: send(MotorController::Feedback::Chops, left. chops.exchange(0)); break; - case 15: send(MotorController::Feedback::Chops, right.chops.exchange(0)); break; - case 16: send(MotorController::Feedback::Hall, left.hallBits()); break; - case 17: send(MotorController::Feedback::Hall, right.hallBits()); break; + case 15: send(MotorController:: Feedback::Chops, right.chops.exchange(0)); break; + case 16: send(MotorController::Feedback::Hall, left.hallBits()); break; + case 17: send(MotorController:: Feedback::Hall, right.hallBits()); break; case 18: send(MotorController::Feedback::Voltage, batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); break; - case 19: send(MotorController::Feedback::Voltage, batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); break; - case 20: send(MotorController::Feedback::Temp, board_temp_deg_c); break; - case 21: send(MotorController::Feedback::Temp, board_temp_deg_c); whichToSend = 0; break; + case 19: send(MotorController:: Feedback::Voltage, batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); break; + case 20: send(MotorController::Feedback::Temp, board_temp_deg_c); break; + case 21: send(MotorController:: Feedback::Temp, board_temp_deg_c); whichToSend = 0; break; default: myPrintf("unreachable"); } } -- 2.50.1 From 20e7dce9e9d61d1e3cee99e3a9396b8ccc492133 Mon Sep 17 00:00:00 2001 From: 0xFEEDC0DE64 Date: Sun, 23 May 2021 17:52:35 +0200 Subject: [PATCH 14/15] Cleanups --- bobbycar-protocol | 2 +- main.cpp | 58 +++++++++++++++++++++-------------------------- 2 files changed, 27 insertions(+), 33 deletions(-) diff --git a/bobbycar-protocol b/bobbycar-protocol index 059a98a..99484cc 160000 --- a/bobbycar-protocol +++ b/bobbycar-protocol @@ -1 +1 @@ -Subproject commit 059a98a1a5a6f4817184a3962eb4f46ed2ddf42b +Subproject commit 99484cc91d01b14b4ac77f7559dfcb063b0a4384 diff --git a/main.cpp b/main.cpp index 0845129..fed12a2 100644 --- a/main.cpp +++ b/main.cpp @@ -164,6 +164,7 @@ int8_t dir = 1; int16_t timeoutCntSerial = 0; // Timeout counter for Rx Serial command Command command; +Feedback feedback; #endif #ifdef FEATURE_CAN @@ -288,37 +289,34 @@ int main() HAL_ADC_Start(&hadc1); HAL_ADC_Start(&hadc2); - enum { CurrentMeasAB, CurrentMeasBC, CurrentMeasAC }; + { + constexpr auto doit = [](auto &motor){ + motor.rtP = defaultP; + motor.rtP.b_angleMeasEna = false; + motor.rtP.b_diagEna = DIAG_ENA; + motor.rtP.b_fieldWeakEna = FIELD_WEAK_ENA; + motor.rtP.r_fieldWeakHi = FIELD_WEAK_HI << 4; + motor.rtP.r_fieldWeakLo = FIELD_WEAK_LO << 4; + + motor.rtM.defaultParam = &motor.rtP; + motor.rtM.dwork = &motor.rtDW; + motor.rtM.inputs = &motor.rtU; + motor.rtM.outputs = &motor.rtY; + }; + + doit(left); + doit(right); + + enum { CurrentMeasAB, CurrentMeasBC, CurrentMeasAC }; - left.rtP = defaultP; - left.rtP.b_angleMeasEna = 0; #ifdef PETERS_PLATINE - left.rtP.z_selPhaCurMeasABC = CurrentMeasBC; + left.rtP.z_selPhaCurMeasABC = CurrentMeasBC; #else - left.rtP.z_selPhaCurMeasABC = CurrentMeasAB; + left.rtP.z_selPhaCurMeasABC = CurrentMeasAB; #endif - left.rtP.b_diagEna = DIAG_ENA; - left.rtP.b_fieldWeakEna = FIELD_WEAK_ENA; - left.rtP.r_fieldWeakHi = FIELD_WEAK_HI << 4; - left.rtP.r_fieldWeakLo = FIELD_WEAK_LO << 4; - right.rtP = defaultP; - right.rtP.b_angleMeasEna = 0; - right.rtP.z_selPhaCurMeasABC = CurrentMeasBC; - right.rtP.b_diagEna = DIAG_ENA; - right.rtP.b_fieldWeakEna = FIELD_WEAK_ENA; - right.rtP.r_fieldWeakHi = FIELD_WEAK_HI << 4; - right.rtP.r_fieldWeakLo = FIELD_WEAK_LO << 4; - - left.rtM.defaultParam = &left.rtP; - left.rtM.dwork = &left.rtDW; - left.rtM.inputs = &left.rtU; - left.rtM.outputs = &left.rtY; - - right.rtM.defaultParam = &right.rtP; - right.rtM.dwork = &right.rtDW; - right.rtM.inputs = &right.rtU; - right.rtM.outputs = &right.rtY; + right.rtP.z_selPhaCurMeasABC = CurrentMeasBC; + } applyDefaultSettings(); @@ -1492,8 +1490,6 @@ void sendFeedback() if (UART_DMA_CHANNEL->CNDTR != 0) return; - static Feedback feedback; - feedback.start = Feedback::VALID_HEADER; feedback.left.angle = left.rtY.a_elecAngle; @@ -1508,10 +1504,8 @@ void sendFeedback() feedback.left.current = left.rtU.i_DCLink; feedback.right.current = right.rtU.i_DCLink; - feedback.left.chops = left.chops; - feedback.right.chops = right.chops; - left.chops = 0; - right.chops = 0; + feedback.left.chops = left.chops.exchange(0); + feedback.right.chops = right.chops.exchange(0); feedback.left.hallA = left.rtU.b_hallA; feedback.left.hallB = left.rtU.b_hallB; -- 2.50.1 From cc5071b26bdcd58e36c90a522c81dec34cf9de3b Mon Sep 17 00:00:00 2001 From: 0xFEEDC0DE64 Date: Sun, 23 May 2021 17:54:07 +0200 Subject: [PATCH 15/15] Updated merged protocol --- bobbycar-protocol | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bobbycar-protocol b/bobbycar-protocol index 99484cc..b6f0d6a 160000 --- a/bobbycar-protocol +++ b/bobbycar-protocol @@ -1 +1 @@ -Subproject commit 99484cc91d01b14b4ac77f7559dfcb063b0a4384 +Subproject commit b6f0d6a185eb211382ce068b6b4d695d9a8b41af -- 2.50.1