feat: report wheel speed and GPIO state periodically
This commit is contained in:
shenlefengnai-rgb
@@ -62,26 +62,29 @@ typedef struct
|
||||
|
||||
#define GPIO_INPUT_MASK 0x7FFFU
|
||||
#define UART_LINE_SIZE 32U
|
||||
#define ADC_REPORT_PERIOD_MS 500U
|
||||
#define PWM_REPORT_PERIOD_MS 20U
|
||||
#define CAN_HEARTBEAT_PERIOD_MS 100U
|
||||
#define DAC_CAN_TIMEOUT_MS 100U
|
||||
#define ADC_REPORT_PERIOD_MS 500U
|
||||
#define PWM_REPORT_PERIOD_MS 20U
|
||||
#define GPIO_REPORT_PERIOD_MS 1000U
|
||||
#define CAN_HEARTBEAT_PERIOD_MS 100U
|
||||
#define DAC_CAN_TIMEOUT_MS 100U
|
||||
#define PWM_TIMEOUT_MS 1000U
|
||||
#define WHEEL_PULSES_PER_REV 49U
|
||||
#define WHEEL_CIRCUMFERENCE_MM 1414U
|
||||
|
||||
#define CAN_HEARTBEAT_STD_ID 0x6A0U
|
||||
#define CAN_ADC_REPORT_STD_ID 0x6A1U
|
||||
#define CAN_PWM_REPORT_STD_ID 0x6A2U
|
||||
#define CAN_DAC_CMD_STD_ID 0x6A3U
|
||||
#define CAN_DAC_ACK_STD_ID 0x6A4U
|
||||
#define CAN_ASSI_LAMP_CMD_STD_ID 0x6A5U
|
||||
#define CAN_GPIO_REPORT_STD_ID 0x6A6U
|
||||
|
||||
#define ASSI_LAMP_FLASH_HALF_PERIOD_MS 250U
|
||||
#define ASSI_STATE_OFF 0U
|
||||
#define ASSI_STATE_READY 1U
|
||||
#define ASSI_STATE_DRIVING 2U
|
||||
#define ASSI_STATE_EMERGENCY 3U
|
||||
#define ASSI_STATE_FINISHED 4U
|
||||
#define CAN_HEARTBEAT_STD_ID 0x6A0U
|
||||
#define CAN_ADC_REPORT_STD_ID 0x6A1U
|
||||
#define CAN_PWM_REPORT_STD_ID 0x6A2U
|
||||
#define CAN_DAC_CMD_STD_ID 0x6A3U
|
||||
#define CAN_DAC_ACK_STD_ID 0x6A4U
|
||||
#define CAN_ASSI_LAMP_CMD_STD_ID 0x6A5U
|
||||
#define CAN_GPIO_REPORT_STD_ID 0x6A6U
|
||||
|
||||
#define ASSI_LAMP_FLASH_HALF_PERIOD_MS 250U
|
||||
#define ASSI_STATE_OFF 0U
|
||||
#define ASSI_STATE_READY 1U
|
||||
#define ASSI_STATE_DRIVING 2U
|
||||
#define ASSI_STATE_EMERGENCY 3U
|
||||
#define ASSI_STATE_FINISHED 4U
|
||||
|
||||
/* USER CODE END PD */
|
||||
|
||||
@@ -100,31 +103,31 @@ I2C_HandleTypeDef hi2c2;
|
||||
|
||||
TIM_HandleTypeDef htim3;
|
||||
TIM_HandleTypeDef htim5;
|
||||
TIM_HandleTypeDef htim9;
|
||||
TIM_HandleTypeDef htim12;
|
||||
|
||||
UART_HandleTypeDef huart1;
|
||||
|
||||
/* USER CODE BEGIN PV */
|
||||
static CaptureChannel capture_channels[4];
|
||||
/* PA0 left-front and PA6 right-front wheel-speed inputs only. */
|
||||
static CaptureChannel capture_channels[2];
|
||||
static uint16_t gpio_last_state;
|
||||
static volatile uint16_t gpio_shadow_state;
|
||||
static volatile uint16_t gpio_report_state;
|
||||
static volatile uint16_t gpio_pending_changed_mask;
|
||||
static uint32_t last_adc_report_ms;
|
||||
static uint32_t last_pwm_report_ms;
|
||||
static uint32_t last_heartbeat_ms;
|
||||
static uint32_t last_can_heartbeat_ms;
|
||||
static uint32_t dac_can_last_command_ms[2];
|
||||
static bool dac_can_watchdog_active[2];
|
||||
static uint32_t last_adc_report_ms;
|
||||
static uint32_t last_pwm_report_ms;
|
||||
static uint32_t last_gpio_report_ms;
|
||||
static uint32_t last_heartbeat_ms;
|
||||
static uint32_t last_can_heartbeat_ms;
|
||||
static uint32_t dac_can_last_command_ms[2];
|
||||
static bool dac_can_watchdog_active[2];
|
||||
static uint16_t can_heartbeat_sequence;
|
||||
static uint32_t adc_filter_history[ADC_FILTER_WINDOW];
|
||||
static uint32_t adc_filter_sum;
|
||||
static uint8_t adc_filter_count;
|
||||
static uint8_t adc_filter_index;
|
||||
static char uart_line[UART_LINE_SIZE];
|
||||
static uint8_t uart_line_len;
|
||||
static uint8_t assi_lamp_state;
|
||||
static uint8_t uart_line_len;
|
||||
static uint8_t assi_lamp_state;
|
||||
|
||||
/* USER CODE END PV */
|
||||
|
||||
@@ -136,8 +139,6 @@ static void MX_CAN2_Init(void);
|
||||
static void MX_I2C2_Init(void);
|
||||
static void MX_TIM3_Init(void);
|
||||
static void MX_TIM5_Init(void);
|
||||
static void MX_TIM9_Init(void);
|
||||
static void MX_TIM12_Init(void);
|
||||
static void MX_USART1_UART_Init(void);
|
||||
static void MX_ADC1_Init(void);
|
||||
/* USER CODE BEGIN PFP */
|
||||
@@ -147,11 +148,11 @@ static void PollCanBus(CAN_HandleTypeDef *hcan, const char *name);
|
||||
static void PollCaptureChannels(void);
|
||||
static void PollGpioChanges(void);
|
||||
static void SendPeriodicReport(void);
|
||||
static void ServiceHeartbeat(void);
|
||||
static void ServiceAssiLamp(void);
|
||||
static void ServiceDacCanWatchdog(void);
|
||||
static void SendCanHeartbeat(void);
|
||||
static void HandleAssiLampSetRequest(const char *source, uint8_t state);
|
||||
static void ServiceHeartbeat(void);
|
||||
static void ServiceAssiLamp(void);
|
||||
static void ServiceDacCanWatchdog(void);
|
||||
static void SendCanHeartbeat(void);
|
||||
static void HandleAssiLampSetRequest(const char *source, uint8_t state);
|
||||
static void HandleTextCommand(const char *source, const char *line);
|
||||
static void HandleDacSetRequest(const char *source, uint8_t dac_id, uint16_t millivolts);
|
||||
static void UartWrite(const char *text);
|
||||
@@ -163,6 +164,7 @@ static const char *DacName(uint8_t dac_id);
|
||||
static uint16_t ReadGpioInputState(void);
|
||||
static void QueueGpioSnapshot(uint16_t state);
|
||||
static uint32_t ReadAdcInputMillivolts(void);
|
||||
static uint32_t WheelSpeedMmPerSecond(uint32_t frequency_hz);
|
||||
static uint32_t GetTimerClockHz(const TIM_HandleTypeDef *htim);
|
||||
static void InitCaptureChannels(void);
|
||||
static void ConfigureCanRuntime(CAN_HandleTypeDef *hcan);
|
||||
@@ -221,8 +223,6 @@ int main(void)
|
||||
MX_I2C2_Init();
|
||||
MX_TIM3_Init();
|
||||
MX_TIM5_Init();
|
||||
MX_TIM9_Init();
|
||||
MX_TIM12_Init();
|
||||
MX_USART1_UART_Init();
|
||||
MX_ADC1_Init();
|
||||
/* USER CODE BEGIN 2 */
|
||||
@@ -238,11 +238,11 @@ int main(void)
|
||||
|
||||
/* USER CODE BEGIN 3 */
|
||||
PollUart();
|
||||
PollCanBus(&hcan1, "CAN1");
|
||||
PollCanBus(&hcan2, "CAN2");
|
||||
ServiceDacCanWatchdog();
|
||||
ServiceAssiLamp();
|
||||
PollCaptureChannels();
|
||||
PollCanBus(&hcan1, "CAN1");
|
||||
PollCanBus(&hcan2, "CAN2");
|
||||
ServiceDacCanWatchdog();
|
||||
ServiceAssiLamp();
|
||||
PollCaptureChannels();
|
||||
PollGpioChanges();
|
||||
SendPeriodicReport();
|
||||
ServiceHeartbeat();
|
||||
@@ -548,88 +548,6 @@ static void MX_TIM5_Init(void)
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief TIM9 Initialization Function
|
||||
* @param None
|
||||
* @retval None
|
||||
*/
|
||||
static void MX_TIM9_Init(void)
|
||||
{
|
||||
|
||||
/* USER CODE BEGIN TIM9_Init 0 */
|
||||
|
||||
/* USER CODE END TIM9_Init 0 */
|
||||
|
||||
TIM_IC_InitTypeDef sConfigIC = {0};
|
||||
|
||||
/* USER CODE BEGIN TIM9_Init 1 */
|
||||
|
||||
/* USER CODE END TIM9_Init 1 */
|
||||
htim9.Instance = TIM9;
|
||||
htim9.Init.Prescaler = 159;
|
||||
htim9.Init.CounterMode = TIM_COUNTERMODE_UP;
|
||||
htim9.Init.Period = 0xFFFF;
|
||||
htim9.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
|
||||
htim9.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
|
||||
if (HAL_TIM_IC_Init(&htim9) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_RISING;
|
||||
sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI;
|
||||
sConfigIC.ICPrescaler = TIM_ICPSC_DIV1;
|
||||
sConfigIC.ICFilter = 0;
|
||||
if (HAL_TIM_IC_ConfigChannel(&htim9, &sConfigIC, TIM_CHANNEL_1) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
/* USER CODE BEGIN TIM9_Init 2 */
|
||||
|
||||
/* USER CODE END TIM9_Init 2 */
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief TIM12 Initialization Function
|
||||
* @param None
|
||||
* @retval None
|
||||
*/
|
||||
static void MX_TIM12_Init(void)
|
||||
{
|
||||
|
||||
/* USER CODE BEGIN TIM12_Init 0 */
|
||||
|
||||
/* USER CODE END TIM12_Init 0 */
|
||||
|
||||
TIM_IC_InitTypeDef sConfigIC = {0};
|
||||
|
||||
/* USER CODE BEGIN TIM12_Init 1 */
|
||||
|
||||
/* USER CODE END TIM12_Init 1 */
|
||||
htim12.Instance = TIM12;
|
||||
htim12.Init.Prescaler = 159;
|
||||
htim12.Init.CounterMode = TIM_COUNTERMODE_UP;
|
||||
htim12.Init.Period = 0xFFFF;
|
||||
htim12.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
|
||||
htim12.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
|
||||
if (HAL_TIM_IC_Init(&htim12) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_RISING;
|
||||
sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI;
|
||||
sConfigIC.ICPrescaler = TIM_ICPSC_DIV1;
|
||||
sConfigIC.ICFilter = 0;
|
||||
if (HAL_TIM_IC_ConfigChannel(&htim12, &sConfigIC, TIM_CHANNEL_1) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
/* USER CODE BEGIN TIM12_Init 2 */
|
||||
|
||||
/* USER CODE END TIM12_Init 2 */
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief USART1 Initialization Function
|
||||
* @param None
|
||||
@@ -685,7 +603,7 @@ static void MX_GPIO_Init(void)
|
||||
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_11, GPIO_PIN_RESET);
|
||||
|
||||
/*Configure GPIO pin Output Level */
|
||||
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_9|GPIO_PIN_10|GPIO_PIN_11, GPIO_PIN_RESET);
|
||||
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_9|GPIO_PIN_10|GPIO_PIN_11, GPIO_PIN_RESET);
|
||||
|
||||
/*Configure GPIO pins : PD8 PD9 PD10 PD11
|
||||
PD12 PD13 PD14 PD0
|
||||
@@ -706,8 +624,8 @@ static void MX_GPIO_Init(void)
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
|
||||
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
|
||||
|
||||
/*Configure GPIO pins : PE0 PE1 PE9 PE10 PE11 */
|
||||
GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_9|GPIO_PIN_10|GPIO_PIN_11;
|
||||
/*Configure GPIO pins : PE0 PE1 PE9 PE10 PE11 */
|
||||
GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_9|GPIO_PIN_10|GPIO_PIN_11;
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
|
||||
GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
|
||||
@@ -765,29 +683,29 @@ static void AppInit(void)
|
||||
gpio_shadow_state = gpio_last_state;
|
||||
gpio_report_state = gpio_last_state;
|
||||
gpio_pending_changed_mask = 0U;
|
||||
last_adc_report_ms = HAL_GetTick();
|
||||
last_pwm_report_ms = last_adc_report_ms;
|
||||
last_heartbeat_ms = last_adc_report_ms;
|
||||
last_can_heartbeat_ms = last_adc_report_ms;
|
||||
dac_can_last_command_ms[0] = 0U;
|
||||
dac_can_last_command_ms[1] = 0U;
|
||||
dac_can_watchdog_active[0] = false;
|
||||
dac_can_watchdog_active[1] = false;
|
||||
last_adc_report_ms = HAL_GetTick();
|
||||
last_pwm_report_ms = last_adc_report_ms;
|
||||
last_gpio_report_ms = last_adc_report_ms;
|
||||
last_heartbeat_ms = last_adc_report_ms;
|
||||
last_can_heartbeat_ms = last_adc_report_ms;
|
||||
dac_can_last_command_ms[0] = 0U;
|
||||
dac_can_last_command_ms[1] = 0U;
|
||||
dac_can_watchdog_active[0] = false;
|
||||
dac_can_watchdog_active[1] = false;
|
||||
can_heartbeat_sequence = 0U;
|
||||
adc_filter_sum = 0U;
|
||||
adc_filter_count = 0U;
|
||||
adc_filter_index = 0U;
|
||||
uart_line_len = 0U;
|
||||
assi_lamp_state = ASSI_STATE_OFF;
|
||||
uart_line_len = 0U;
|
||||
assi_lamp_state = ASSI_STATE_OFF;
|
||||
|
||||
(void)DacSetMillivolts(1U, 0U);
|
||||
(void)DacSetMillivolts(2U, 0U);
|
||||
|
||||
UartWrite("\r\nA11 F407 monitor ready\r\n");
|
||||
UartWrite("UART: DAC1 1.234 or DAC2 2500\r\n");
|
||||
UartWrite("CAN cmd 0x6A3: [dac_id][mV_L][mV_H] or ASCII text\r\n");
|
||||
UartWrite("CAN cmd 0x6A5: [ASSI state 0=OFF,1=READY,2=DRIVING,3=EMERGENCY,4=FINISHED]\r\n");
|
||||
BroadcastGpioReport(0U, gpio_last_state);
|
||||
UartWrite("CAN cmd 0x6A3: [dac_id][mV_L][mV_H] or ASCII text\r\n");
|
||||
UartWrite("CAN cmd 0x6A5: [ASSI state 0=OFF,1=READY,2=DRIVING,3=EMERGENCY,4=FINISHED]\r\n");
|
||||
}
|
||||
|
||||
static void PollUart(void)
|
||||
@@ -836,14 +754,14 @@ static void PollCanBus(CAN_HandleTypeDef *hcan, const char *name)
|
||||
continue;
|
||||
}
|
||||
|
||||
if (rx_header.StdId == CAN_DAC_CMD_STD_ID)
|
||||
if (rx_header.StdId == CAN_DAC_CMD_STD_ID)
|
||||
{
|
||||
if ((rx_header.DLC >= 3U) && (rx_data[0] >= 1U) && (rx_data[0] <= 2U))
|
||||
{
|
||||
uint16_t millivolts = (uint16_t)rx_data[1] | ((uint16_t)rx_data[2] << 8);
|
||||
HandleDacSetRequest(name, rx_data[0], millivolts);
|
||||
dac_can_last_command_ms[rx_data[0] - 1U] = HAL_GetTick();
|
||||
dac_can_watchdog_active[rx_data[0] - 1U] = true;
|
||||
if ((rx_header.DLC >= 3U) && (rx_data[0] >= 1U) && (rx_data[0] <= 2U))
|
||||
{
|
||||
uint16_t millivolts = (uint16_t)rx_data[1] | ((uint16_t)rx_data[2] << 8);
|
||||
HandleDacSetRequest(name, rx_data[0], millivolts);
|
||||
dac_can_last_command_ms[rx_data[0] - 1U] = HAL_GetTick();
|
||||
dac_can_watchdog_active[rx_data[0] - 1U] = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -853,15 +771,15 @@ static void PollCanBus(CAN_HandleTypeDef *hcan, const char *name)
|
||||
memcpy(line, rx_data, length);
|
||||
line[length] = '\0';
|
||||
HandleTextCommand(name, line);
|
||||
}
|
||||
}
|
||||
else if (rx_header.StdId == CAN_ASSI_LAMP_CMD_STD_ID)
|
||||
{
|
||||
if ((rx_header.DLC >= 1U) && (rx_data[0] <= ASSI_STATE_FINISHED))
|
||||
{
|
||||
HandleAssiLampSetRequest(name, rx_data[0]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (rx_header.StdId == CAN_ASSI_LAMP_CMD_STD_ID)
|
||||
{
|
||||
if ((rx_header.DLC >= 1U) && (rx_data[0] <= ASSI_STATE_FINISHED))
|
||||
{
|
||||
HandleAssiLampSetRequest(name, rx_data[0]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -903,62 +821,68 @@ static void PollCaptureChannels(void)
|
||||
|
||||
static void PollGpioChanges(void)
|
||||
{
|
||||
uint32_t now = HAL_GetTick();
|
||||
uint16_t state;
|
||||
uint16_t changed;
|
||||
char line[96];
|
||||
|
||||
__disable_irq();
|
||||
QueueGpioSnapshot(ReadGpioInputState());
|
||||
__enable_irq();
|
||||
|
||||
if ((now - last_gpio_report_ms) < GPIO_REPORT_PERIOD_MS)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
__disable_irq();
|
||||
changed = gpio_pending_changed_mask;
|
||||
state = gpio_report_state;
|
||||
gpio_pending_changed_mask = 0U;
|
||||
__enable_irq();
|
||||
|
||||
if (changed != 0U)
|
||||
{
|
||||
char line[96];
|
||||
|
||||
gpio_last_state = state;
|
||||
snprintf(line,
|
||||
sizeof(line),
|
||||
"GPIO changed mask=0x%04X state=0x%04X\r\n",
|
||||
(unsigned int)changed,
|
||||
(unsigned int)state);
|
||||
UartWrite(line);
|
||||
BroadcastGpioReport(changed, state);
|
||||
}
|
||||
last_gpio_report_ms = now;
|
||||
gpio_last_state = state;
|
||||
snprintf(line,
|
||||
sizeof(line),
|
||||
"GPIO state=0x%04X changed=0x%04X\r\n",
|
||||
(unsigned int)state,
|
||||
(unsigned int)changed);
|
||||
UartWrite(line);
|
||||
BroadcastGpioReport(changed, state);
|
||||
}
|
||||
|
||||
static void SendPeriodicReport(void)
|
||||
{
|
||||
uint32_t now = HAL_GetTick();
|
||||
|
||||
if ((now - last_adc_report_ms) >= ADC_REPORT_PERIOD_MS)
|
||||
{
|
||||
if ((now - last_adc_report_ms) >= ADC_REPORT_PERIOD_MS)
|
||||
{
|
||||
uint32_t adc_mv = ReadAdcInputMillivolts();
|
||||
char line[160];
|
||||
|
||||
last_adc_report_ms = now;
|
||||
last_adc_report_ms = now;
|
||||
|
||||
snprintf(line,
|
||||
sizeof(line),
|
||||
"ADC=%lumV PWM=%lu,%lu,%lu,%luHz\r\n",
|
||||
"ADC=%lumV WheelSpeed=LF:%lu.%03lum/s RF:%lu.%03lum/s\r\n",
|
||||
(unsigned long)adc_mv,
|
||||
(unsigned long)capture_channels[0].frequency_hz,
|
||||
(unsigned long)capture_channels[1].frequency_hz,
|
||||
(unsigned long)capture_channels[2].frequency_hz,
|
||||
(unsigned long)capture_channels[3].frequency_hz);
|
||||
UartWrite(line);
|
||||
BroadcastAdcReport(adc_mv);
|
||||
}
|
||||
|
||||
if ((now - last_pwm_report_ms) >= PWM_REPORT_PERIOD_MS)
|
||||
{
|
||||
last_pwm_report_ms = now;
|
||||
BroadcastPwmReport();
|
||||
}
|
||||
}
|
||||
(unsigned long)(WheelSpeedMmPerSecond(capture_channels[0].frequency_hz) / 1000U),
|
||||
(unsigned long)(WheelSpeedMmPerSecond(capture_channels[0].frequency_hz) % 1000U),
|
||||
(unsigned long)(WheelSpeedMmPerSecond(capture_channels[1].frequency_hz) / 1000U),
|
||||
(unsigned long)(WheelSpeedMmPerSecond(capture_channels[1].frequency_hz) % 1000U));
|
||||
UartWrite(line);
|
||||
BroadcastAdcReport(adc_mv);
|
||||
}
|
||||
|
||||
static void ServiceHeartbeat(void)
|
||||
if ((now - last_pwm_report_ms) >= PWM_REPORT_PERIOD_MS)
|
||||
{
|
||||
last_pwm_report_ms = now;
|
||||
BroadcastPwmReport();
|
||||
}
|
||||
}
|
||||
|
||||
static void ServiceHeartbeat(void)
|
||||
{
|
||||
uint32_t now = HAL_GetTick();
|
||||
|
||||
@@ -967,71 +891,71 @@ static void ServiceHeartbeat(void)
|
||||
last_heartbeat_ms = now;
|
||||
HAL_GPIO_TogglePin(GPIOE, GPIO_PIN_0);
|
||||
}
|
||||
}
|
||||
|
||||
static void ServiceAssiLamp(void)
|
||||
{
|
||||
GPIO_PinState red = GPIO_PIN_RESET;
|
||||
GPIO_PinState green = GPIO_PIN_RESET;
|
||||
GPIO_PinState blue = GPIO_PIN_RESET;
|
||||
bool flash_on = ((HAL_GetTick() / ASSI_LAMP_FLASH_HALF_PERIOD_MS) & 1U) == 0U;
|
||||
|
||||
switch (assi_lamp_state)
|
||||
{
|
||||
case ASSI_STATE_READY:
|
||||
red = GPIO_PIN_SET;
|
||||
green = GPIO_PIN_SET;
|
||||
break;
|
||||
|
||||
case ASSI_STATE_DRIVING:
|
||||
red = flash_on ? GPIO_PIN_SET : GPIO_PIN_RESET;
|
||||
green = flash_on ? GPIO_PIN_SET : GPIO_PIN_RESET;
|
||||
break;
|
||||
|
||||
case ASSI_STATE_EMERGENCY:
|
||||
blue = flash_on ? GPIO_PIN_SET : GPIO_PIN_RESET;
|
||||
break;
|
||||
|
||||
case ASSI_STATE_FINISHED:
|
||||
blue = GPIO_PIN_SET;
|
||||
break;
|
||||
|
||||
case ASSI_STATE_OFF:
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, red);
|
||||
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_10, green);
|
||||
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, blue);
|
||||
}
|
||||
|
||||
static void ServiceDacCanWatchdog(void)
|
||||
{
|
||||
uint32_t now = HAL_GetTick();
|
||||
uint8_t dac_id;
|
||||
|
||||
for (dac_id = 1U; dac_id <= 2U; dac_id++)
|
||||
{
|
||||
uint8_t index = dac_id - 1U;
|
||||
|
||||
if (dac_can_watchdog_active[index] &&
|
||||
((now - dac_can_last_command_ms[index]) >= DAC_CAN_TIMEOUT_MS))
|
||||
{
|
||||
(void)DacSetMillivolts(dac_id, 0U);
|
||||
dac_can_watchdog_active[index] = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void HandleAssiLampSetRequest(const char *source, uint8_t state)
|
||||
{
|
||||
char reply[64];
|
||||
|
||||
assi_lamp_state = state;
|
||||
snprintf(reply, sizeof(reply), "OK %s ASSI state=%u\r\n", source, (unsigned int)state);
|
||||
UartWrite(reply);
|
||||
}
|
||||
}
|
||||
|
||||
static void ServiceAssiLamp(void)
|
||||
{
|
||||
GPIO_PinState red = GPIO_PIN_RESET;
|
||||
GPIO_PinState green = GPIO_PIN_RESET;
|
||||
GPIO_PinState blue = GPIO_PIN_RESET;
|
||||
bool flash_on = ((HAL_GetTick() / ASSI_LAMP_FLASH_HALF_PERIOD_MS) & 1U) == 0U;
|
||||
|
||||
switch (assi_lamp_state)
|
||||
{
|
||||
case ASSI_STATE_READY:
|
||||
red = GPIO_PIN_SET;
|
||||
green = GPIO_PIN_SET;
|
||||
break;
|
||||
|
||||
case ASSI_STATE_DRIVING:
|
||||
red = flash_on ? GPIO_PIN_SET : GPIO_PIN_RESET;
|
||||
green = flash_on ? GPIO_PIN_SET : GPIO_PIN_RESET;
|
||||
break;
|
||||
|
||||
case ASSI_STATE_EMERGENCY:
|
||||
blue = flash_on ? GPIO_PIN_SET : GPIO_PIN_RESET;
|
||||
break;
|
||||
|
||||
case ASSI_STATE_FINISHED:
|
||||
blue = GPIO_PIN_SET;
|
||||
break;
|
||||
|
||||
case ASSI_STATE_OFF:
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, red);
|
||||
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_10, green);
|
||||
HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, blue);
|
||||
}
|
||||
|
||||
static void ServiceDacCanWatchdog(void)
|
||||
{
|
||||
uint32_t now = HAL_GetTick();
|
||||
uint8_t dac_id;
|
||||
|
||||
for (dac_id = 1U; dac_id <= 2U; dac_id++)
|
||||
{
|
||||
uint8_t index = dac_id - 1U;
|
||||
|
||||
if (dac_can_watchdog_active[index] &&
|
||||
((now - dac_can_last_command_ms[index]) >= DAC_CAN_TIMEOUT_MS))
|
||||
{
|
||||
(void)DacSetMillivolts(dac_id, 0U);
|
||||
dac_can_watchdog_active[index] = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void HandleAssiLampSetRequest(const char *source, uint8_t state)
|
||||
{
|
||||
char reply[64];
|
||||
|
||||
assi_lamp_state = state;
|
||||
snprintf(reply, sizeof(reply), "OK %s ASSI state=%u\r\n", source, (unsigned int)state);
|
||||
UartWrite(reply);
|
||||
}
|
||||
|
||||
static void SendCanHeartbeat(void)
|
||||
{
|
||||
@@ -1049,7 +973,7 @@ static void SendCanHeartbeat(void)
|
||||
can_heartbeat_sequence++;
|
||||
gpio_state = ReadGpioInputState();
|
||||
|
||||
/* CAN ID 0x6A0, sent independently on CAN1 and CAN2:
|
||||
/* CAN ID 0x6A0, sent independently on CAN1 and CAN2:
|
||||
* [0] 0xA1 board identifier; [1] firmware protocol version 1;
|
||||
* [2..3] sequence counter, little-endian; [4..7] uptime in ms, little-endian.
|
||||
*/
|
||||
@@ -1338,6 +1262,12 @@ static uint32_t GetTimerClockHz(const TIM_HandleTypeDef *htim)
|
||||
return (pclk * multiplier) / prescaler;
|
||||
}
|
||||
|
||||
static uint32_t WheelSpeedMmPerSecond(uint32_t frequency_hz)
|
||||
{
|
||||
return (uint32_t)(((uint64_t)frequency_hz * WHEEL_CIRCUMFERENCE_MM +
|
||||
(WHEEL_PULSES_PER_REV / 2U)) / WHEEL_PULSES_PER_REV);
|
||||
}
|
||||
|
||||
static void InitCaptureChannels(void)
|
||||
{
|
||||
capture_channels[0].htim = &htim5;
|
||||
@@ -1347,43 +1277,21 @@ static void InitCaptureChannels(void)
|
||||
capture_channels[0].timer_clock_hz = GetTimerClockHz(&htim5);
|
||||
capture_channels[0].last_edge_ms = HAL_GetTick();
|
||||
|
||||
capture_channels[1].htim = &htim9;
|
||||
capture_channels[1].htim = &htim3;
|
||||
capture_channels[1].channel = TIM_CHANNEL_1;
|
||||
capture_channels[1].flag = TIM_FLAG_CC1;
|
||||
capture_channels[1].counter_mask = 0xFFFFU;
|
||||
capture_channels[1].timer_clock_hz = GetTimerClockHz(&htim9);
|
||||
capture_channels[1].timer_clock_hz = GetTimerClockHz(&htim3);
|
||||
capture_channels[1].last_edge_ms = HAL_GetTick();
|
||||
|
||||
capture_channels[2].htim = &htim3;
|
||||
capture_channels[2].channel = TIM_CHANNEL_1;
|
||||
capture_channels[2].flag = TIM_FLAG_CC1;
|
||||
capture_channels[2].counter_mask = 0xFFFFU;
|
||||
capture_channels[2].timer_clock_hz = GetTimerClockHz(&htim3);
|
||||
capture_channels[2].last_edge_ms = HAL_GetTick();
|
||||
|
||||
capture_channels[3].htim = &htim12;
|
||||
capture_channels[3].channel = TIM_CHANNEL_1;
|
||||
capture_channels[3].flag = TIM_FLAG_CC1;
|
||||
capture_channels[3].counter_mask = 0xFFFFU;
|
||||
capture_channels[3].timer_clock_hz = GetTimerClockHz(&htim12);
|
||||
capture_channels[3].last_edge_ms = HAL_GetTick();
|
||||
|
||||
if (HAL_TIM_IC_Start(&htim5, TIM_CHANNEL_1) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
if (HAL_TIM_IC_Start(&htim9, TIM_CHANNEL_1) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
if (HAL_TIM_IC_Start(&htim3, TIM_CHANNEL_1) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
if (HAL_TIM_IC_Start(&htim12, TIM_CHANNEL_1) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
}
|
||||
|
||||
static void ConfigureCanRuntime(CAN_HandleTypeDef *hcan)
|
||||
@@ -1480,14 +1388,15 @@ static void BroadcastAdcReport(uint32_t input_mv)
|
||||
|
||||
static void BroadcastPwmReport(void)
|
||||
{
|
||||
uint8_t payload[8];
|
||||
uint8_t payload[4];
|
||||
size_t i;
|
||||
|
||||
for (i = 0U; i < 4U; i++)
|
||||
for (i = 0U; i < 2U; i++)
|
||||
{
|
||||
uint16_t clamped = (capture_channels[i].frequency_hz > 0xFFFFU) ?
|
||||
uint32_t speed_mm_per_s = WheelSpeedMmPerSecond(capture_channels[i].frequency_hz);
|
||||
uint16_t clamped = (speed_mm_per_s > 0xFFFFU) ?
|
||||
0xFFFFU :
|
||||
(uint16_t)capture_channels[i].frequency_hz;
|
||||
(uint16_t)speed_mm_per_s;
|
||||
payload[i * 2U] = (uint8_t)(clamped & 0xFFU);
|
||||
payload[(i * 2U) + 1U] = (uint8_t)(clamped >> 8);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user