#ifndef FMW_MESSAGE_H
#define FMW_MESSAGE_H
+// TODO(lb): fill with sensible values
+#define FMW_MIN_ACCEPTABLE_TICKS_PER_REVOLUTION 1
+#define FMW_MAX_ACCEPTABLE_TICKS_PER_REVOLUTION 10000
+
+// TODO(lb): fill with sensible values
+#define FMW_MIN_ACCEPTABLE_PID_PROPORTIONAL -1.f
+#define FMW_MAX_ACCEPTABLE_PID_PROPORTIONAL 1.f
+
+// TODO(lb): fill with sensible values
+#define FMW_MIN_ACCEPTABLE_PID_INTEGRAL -1.f
+#define FMW_MAX_ACCEPTABLE_PID_INTEGRAL 1.f
+
+// TODO(lb): fill with sensible values
+#define FMW_MIN_ACCEPTABLE_PID_DERIVATIVE -1.f
+#define FMW_MAX_ACCEPTABLE_PID_DERIVATIVE 1.f
+
+// TODO(lb): fill with sensible values
+#define FMW_MIN_ACCEPTABLE_LED_VOLTAGE 0.f
+#define FMW_MAX_ACCEPTABLE_LED_VOLTAGE 1.f
+
+// TODO(lb): fill with sensible values
+#define FMW_MIN_ACCEPTABLE_LED_UPDATE_PERIOD 1
+#define FMW_MAX_ACCEPTABLE_LED_UPDATE_PERIOD 10000
+
typedef union {
struct {
float proportional;
float values[3];
} FMW_PidConstants;
-typedef uint16_t MessageStatusCode;
+typedef uint16_t FMW_MessageStatusCode;
enum {
- MessageStatusCode_Waiting4Config = 0,
- MessageStatusCode_Running = 1,
- MessageStatusCode_Error_Config = 2,
- MessageStatusCode_Error_Velocity = 3,
- MessageStatusCode_Fault_HBridge = 4,
+ FMW_MessageStatusCode_None,
+
+ FMW_MessageStatusCode_Waiting4Config = 0,
+ FMW_MessageStatusCode_Running = 1,
+ FMW_MessageStatusCode_Error_Config = 2,
+ FMW_MessageStatusCode_Error_Velocity = 3,
+ FMW_MessageStatusCode_Fault_HBridge = 4,
+
+ FMW_MessageStatusCode_COUNT,
};
typedef uint8_t FMW_State;
enum {
+ FMW_State_None,
+
FMW_State_Init,
FMW_State_Error,
FMW_State_Running,
+
+ FMW_State_COUNT,
};
typedef uint8_t FMW_MessageType;
enum {
- FMW_MessageType_Error,
+ FMW_MessageType_None,
+ FMW_MessageType_Error,
FMW_MessageType_Run,
FMW_MessageType_Config_Robot,
FMW_MessageType_Config_PID,
FMW_MessageType_Config_LED,
-
FMW_MessageType_Status,
FMW_MessageType_Velocity,
FMW_MessageType_COUNT,
};
-typedef uint8_t FMW_Error;
-enum {
- FMW_Error_Unknown = 0,
+#define FMW_RESULT_VARIANTS(X) \
+ X(FMW_Result_Ok) \
+ X(FMW_Result_Error_NilMessage) \
+ X(FMW_Result_Error_UART_Crc) \
+ X(FMW_Result_Error_UART_NilHandle) \
+ X(FMW_Result_Error_UART_NegativeTimeout) \
+ X(FMW_Result_Error_UART_ReceiveTimeoutElapsed) \
+ X(FMW_Result_Error_MessageHandler_InvalidState) \
+ X(FMW_Result_Error_MessageHandler_Init_NonPositiveBaseline) \
+ X(FMW_Result_Error_MessageHandler_Init_NonPositiveWheelCircumference) \
+ X(FMW_Result_Error_MessageHandler_Init_InvalidTicksPerRevolution) \
+ X(FMW_Result_Error_MessageHandler_Init_InvalidPIDProportionalConstant) \
+ X(FMW_Result_Error_MessageHandler_Init_InvalidPIDIntegralConstant) \
+ X(FMW_Result_Error_MessageHandler_Init_InvalidPIDDerivativeConstant) \
+ X(FMW_Result_Error_MessageHandler_Init_InvalidLEDVoltage) \
+ X(FMW_Result_Error_Command_NotRecognized) \
+ X(FMW_Result_Error_Command_NotAvailable) \
+ X(FMW_Result_COUNT)
- FMW_Error_UART_Crc,
- FMW_Error_UART_ReceiveTimeoutElapsed,
-
- FMW_Error_Command_NotRecognized,
- FMW_Error_Command_NotAvailable,
+typedef uint8_t FMW_Result;
+enum {
+#define X(Variant) Variant,
+ FMW_RESULT_VARIANTS(X)
+#undef X
};
#pragma pack(push, 1)
union {
struct {
- FMW_Error reason;
+ const char *filename;
int32_t line;
int32_t filename_size;
- const char *filename;
+ FMW_Result reason;
} error;
struct {
float baseline;
- float left_wheel_circumference;
- uint32_t left_ticks_per_revolution;
- float right_wheel_circumference;
- uint32_t right_ticks_per_revolution;
+ float wheel_circumference_left;
+ float wheel_circumference_right;
+ uint32_t ticks_per_revolution_left;
+ uint32_t ticks_per_revolution_right;
} config_robot;
struct {
FMW_PidConstants left;
} config_led;
struct {
- MessageStatusCode status_code;
+ FMW_MessageStatusCode status_code;
uint16_t delta_millis;
int32_t left_ticks;
int32_t right_ticks;
static void MX_TIM6_Init(void);\r
static void MX_CRC_Init(void);\r
/* USER CODE BEGIN PFP */\r
-__attribute__((noreturn)) void start(void);\r
+\r
/* USER CODE END PFP */\r
\r
/* Private user code ---------------------------------------------------------*/\r
/* USER CODE END 0 */\r
\r
/**\r
- * @brief The application entry point.\r
- * @retval int\r
- */\r
+ * @brief The application entry point.\r
+ * @retval int\r
+ */\r
int main(void)\r
{\r
\r
}\r
\r
/**\r
- * @brief System Clock Configuration\r
- * @retval None\r
- */\r
+ * @brief System Clock Configuration\r
+ * @retval None\r
+ */\r
void SystemClock_Config(void)\r
{\r
RCC_OscInitTypeDef RCC_OscInitStruct = {0};\r
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};\r
\r
/** Configure LSE Drive Capability\r
- */\r
+ */\r
HAL_PWR_EnableBkUpAccess();\r
\r
/** Configure the main internal regulator output voltage\r
- */\r
+ */\r
__HAL_RCC_PWR_CLK_ENABLE();\r
__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE3);\r
\r
/** Initializes the RCC Oscillators according to the specified parameters\r
- * in the RCC_OscInitTypeDef structure.\r
- */\r
+ * in the RCC_OscInitTypeDef structure.\r
+ */\r
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;\r
RCC_OscInitStruct.HSEState = RCC_HSE_BYPASS;\r
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;\r
RCC_OscInitStruct.PLL.PLLQ = 4;\r
RCC_OscInitStruct.PLL.PLLR = 2;\r
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)\r
- {\r
- Error_Handler();\r
- }\r
+ {\r
+ Error_Handler();\r
+ }\r
\r
/** Activate the Over-Drive mode\r
- */\r
+ */\r
if (HAL_PWREx_EnableOverDrive() != HAL_OK)\r
- {\r
- Error_Handler();\r
- }\r
+ {\r
+ Error_Handler();\r
+ }\r
\r
/** Initializes the CPU, AHB and APB buses clocks\r
- */\r
+ */\r
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK\r
- |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;\r
+ |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;\r
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;\r
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;\r
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;\r
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;\r
\r
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_3) != HAL_OK)\r
- {\r
- Error_Handler();\r
- }\r
+ {\r
+ Error_Handler();\r
+ }\r
}\r
\r
/**\r
- * @brief ADC1 Initialization Function\r
- * @param None\r
- * @retval None\r
- */\r
+ * @brief ADC1 Initialization Function\r
+ * @param None\r
+ * @retval None\r
+ */\r
static void MX_ADC1_Init(void)\r
{\r
\r
/* USER CODE END ADC1_Init 1 */\r
\r
/** Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)\r
- */\r
+ */\r
hadc1.Instance = ADC1;\r
hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;\r
hadc1.Init.Resolution = ADC_RESOLUTION_12B;\r
hadc1.Init.DMAContinuousRequests = DISABLE;\r
hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV;\r
if (HAL_ADC_Init(&hadc1) != HAL_OK)\r
- {\r
- Error_Handler();\r
- }\r
+ {\r
+ Error_Handler();\r
+ }\r
\r
/** Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time.\r
- */\r
+ */\r
sConfig.Channel = ADC_CHANNEL_2;\r
sConfig.Rank = ADC_REGULAR_RANK_1;\r
sConfig.SamplingTime = ADC_SAMPLETIME_84CYCLES;\r
if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)\r
- {\r
- Error_Handler();\r
- }\r
+ {\r
+ Error_Handler();\r
+ }\r
/* USER CODE BEGIN ADC1_Init 2 */\r
\r
/* USER CODE END ADC1_Init 2 */\r
}\r
\r
/**\r
- * @brief CRC Initialization Function\r
- * @param None\r
- * @retval None\r
- */\r
+ * @brief CRC Initialization Function\r
+ * @param None\r
+ * @retval None\r
+ */\r
static void MX_CRC_Init(void)\r
{\r
\r
hcrc.Init.OutputDataInversionMode = CRC_OUTPUTDATA_INVERSION_DISABLE;\r
hcrc.InputDataFormat = CRC_INPUTDATA_FORMAT_BYTES;\r
if (HAL_CRC_Init(&hcrc) != HAL_OK)\r
- {\r
- Error_Handler();\r
- }\r
+ {\r
+ Error_Handler();\r
+ }\r
/* USER CODE BEGIN CRC_Init 2 */\r
\r
/* USER CODE END CRC_Init 2 */\r
}\r
\r
/**\r
- * @brief TIM1 Initialization Function\r
- * @param None\r
- * @retval None\r
- */\r
+ * @brief TIM1 Initialization Function\r
+ * @param None\r
+ * @retval None\r
+ */\r
static void MX_TIM1_Init(void)\r
{\r
\r
htim1.Init.RepetitionCounter = 0;\r
htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;\r
if (HAL_TIM_PWM_Init(&htim1) != HAL_OK)\r
- {\r
- Error_Handler();\r
- }\r
+ {\r
+ Error_Handler();\r
+ }\r
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;\r
sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET;\r
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;\r
if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK)\r
- {\r
- Error_Handler();\r
- }\r
+ {\r
+ Error_Handler();\r
+ }\r
sConfigOC.OCMode = TIM_OCMODE_PWM1;\r
sConfigOC.Pulse = 0;\r
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;\r
sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET;\r
sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;\r
if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)\r
- {\r
- Error_Handler();\r
- }\r
+ {\r
+ Error_Handler();\r
+ }\r
if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)\r
- {\r
- Error_Handler();\r
- }\r
+ {\r
+ Error_Handler();\r
+ }\r
sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE;\r
sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE;\r
sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF;\r
sBreakDeadTimeConfig.Break2Filter = 0;\r
sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE;\r
if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK)\r
- {\r
- Error_Handler();\r
- }\r
+ {\r
+ Error_Handler();\r
+ }\r
/* USER CODE BEGIN TIM1_Init 2 */\r
\r
/* USER CODE END TIM1_Init 2 */\r
}\r
\r
/**\r
- * @brief TIM2 Initialization Function\r
- * @param None\r
- * @retval None\r
- */\r
+ * @brief TIM2 Initialization Function\r
+ * @param None\r
+ * @retval None\r
+ */\r
static void MX_TIM2_Init(void)\r
{\r
\r
sConfig.IC2Prescaler = TIM_ICPSC_DIV1;\r
sConfig.IC2Filter = 0;\r
if (HAL_TIM_Encoder_Init(&htim2, &sConfig) != HAL_OK)\r
- {\r
- Error_Handler();\r
- }\r
+ {\r
+ Error_Handler();\r
+ }\r
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;\r
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;\r
if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)\r
- {\r
- Error_Handler();\r
- }\r
+ {\r
+ Error_Handler();\r
+ }\r
/* USER CODE BEGIN TIM2_Init 2 */\r
\r
/* USER CODE END TIM2_Init 2 */\r
}\r
\r
/**\r
- * @brief TIM3 Initialization Function\r
- * @param None\r
- * @retval None\r
- */\r
+ * @brief TIM3 Initialization Function\r
+ * @param None\r
+ * @retval None\r
+ */\r
static void MX_TIM3_Init(void)\r
{\r
\r
sConfig.IC2Prescaler = TIM_ICPSC_DIV1;\r
sConfig.IC2Filter = 0;\r
if (HAL_TIM_Encoder_Init(&htim3, &sConfig) != HAL_OK)\r
- {\r
- Error_Handler();\r
- }\r
+ {\r
+ Error_Handler();\r
+ }\r
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;\r
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;\r
if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)\r
- {\r
- Error_Handler();\r
- }\r
+ {\r
+ Error_Handler();\r
+ }\r
/* USER CODE BEGIN TIM3_Init 2 */\r
\r
/* USER CODE END TIM3_Init 2 */\r
}\r
\r
/**\r
- * @brief TIM4 Initialization Function\r
- * @param None\r
- * @retval None\r
- */\r
+ * @brief TIM4 Initialization Function\r
+ * @param None\r
+ * @retval None\r
+ */\r
static void MX_TIM4_Init(void)\r
{\r
\r
htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;\r
htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;\r
if (HAL_TIM_PWM_Init(&htim4) != HAL_OK)\r
- {\r
- Error_Handler();\r
- }\r
+ {\r
+ Error_Handler();\r
+ }\r
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;\r
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;\r
if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK)\r
- {\r
- Error_Handler();\r
- }\r
+ {\r
+ Error_Handler();\r
+ }\r
sConfigOC.OCMode = TIM_OCMODE_PWM1;\r
sConfigOC.Pulse = 0;\r
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;\r
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;\r
if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)\r
- {\r
- Error_Handler();\r
- }\r
+ {\r
+ Error_Handler();\r
+ }\r
if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_4) != HAL_OK)\r
- {\r
- Error_Handler();\r
- }\r
+ {\r
+ Error_Handler();\r
+ }\r
/* USER CODE BEGIN TIM4_Init 2 */\r
\r
/* USER CODE END TIM4_Init 2 */\r
}\r
\r
/**\r
- * @brief TIM6 Initialization Function\r
- * @param None\r
- * @retval None\r
- */\r
+ * @brief TIM6 Initialization Function\r
+ * @param None\r
+ * @retval None\r
+ */\r
static void MX_TIM6_Init(void)\r
{\r
\r
htim6.Init.Period = 65535;\r
htim6.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;\r
if (HAL_TIM_Base_Init(&htim6) != HAL_OK)\r
- {\r
- Error_Handler();\r
- }\r
+ {\r
+ Error_Handler();\r
+ }\r
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;\r
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;\r
if (HAL_TIMEx_MasterConfigSynchronization(&htim6, &sMasterConfig) != HAL_OK)\r
- {\r
- Error_Handler();\r
- }\r
+ {\r
+ Error_Handler();\r
+ }\r
/* USER CODE BEGIN TIM6_Init 2 */\r
\r
/* USER CODE END TIM6_Init 2 */\r
}\r
\r
/**\r
- * @brief USART3 Initialization Function\r
- * @param None\r
- * @retval None\r
- */\r
+ * @brief USART3 Initialization Function\r
+ * @param None\r
+ * @retval None\r
+ */\r
static void MX_USART3_UART_Init(void)\r
{\r
\r
huart3.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;\r
huart3.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;\r
if (HAL_UART_Init(&huart3) != HAL_OK)\r
- {\r
- Error_Handler();\r
- }\r
+ {\r
+ Error_Handler();\r
+ }\r
/* USER CODE BEGIN USART3_Init 2 */\r
\r
/* USER CODE END USART3_Init 2 */\r
}\r
\r
/**\r
- * @brief GPIO Initialization Function\r
- * @param None\r
- * @retval None\r
- */\r
+ * @brief GPIO Initialization Function\r
+ * @param None\r
+ * @retval None\r
+ */\r
static void MX_GPIO_Init(void)\r
{\r
GPIO_InitTypeDef GPIO_InitStruct = {0};\r
}\r
\r
/* USER CODE BEGIN 4 */\r
-__attribute__((noreturn)) void start(void) {\r
+void start(void) {\r
+ FMW_Message msg = {0};\r
do {\r
- fmw_message_handle(FMW_State_Init, &huart3, 180 * 1000);\r
- } while(fmw_state != FMW_State_Running);\r
+ FMW_Result res = fmw_message_receive_uart(&huart3, 180 * 1000, &msg);\r
+ if (res != FMW_Result_Ok) {\r
+ FMW_RESULT_LOG_UART(&huart3, res);\r
+ continue;\r
+ }\r
+\r
+ res = fmw_message_handle(FMW_State_Init, &msg);\r
+ if (res != FMW_Result_Ok) {\r
+ FMW_RESULT_LOG_UART(&huart3, res);\r
+ continue;\r
+ }\r
+ } while(fmw_state == FMW_State_Init);\r
\r
fmw_encoder_init(encoders.values);\r
fmw_motor_init(motors.values);\r
}\r
}\r
\r
-void fmw_message_handle(FMW_State state, UART_HandleTypeDef *huart, int32_t wait_ms) {\r
- FMW_Message msg = {0};\r
- HAL_StatusTypeDef uart_packet_status = HAL_UART_Receive(huart, (uint8_t*)&msg, sizeof(msg), wait_ms);\r
- FMW_REPORT_UNLESS(uart_packet_status == HAL_OK, FMW_Error_UART_ReceiveTimeoutElapsed);\r
- FMW_REPORT_UNLESS(msg.header.type < FMW_MessageType_COUNT, FMW_Error_Command_NotRecognized);\r
+FMW_Result fmw_message_receive_uart(UART_HandleTypeDef *huart, int32_t wait_ms, FMW_Message *msg) {\r
+ if (!(wait_ms >= 0)) { return FMW_Result_Error_UART_NegativeTimeout; }\r
+\r
+ memset(msg, 0, sizeof *msg);\r
+\r
+ HAL_StatusTypeDef uart_packet_status = HAL_UART_Receive(huart, (uint8_t*)msg, sizeof(msg), wait_ms);\r
+ if (!(uart_packet_status == HAL_OK)) { return FMW_Result_Error_UART_ReceiveTimeoutElapsed; }\r
+ if (!(msg->header.type > FMW_MessageType_None && msg->header.type < FMW_MessageType_COUNT)) {\r
+ return FMW_Result_Error_Command_NotRecognized;\r
+ }\r
+ return FMW_Result_Ok;\r
+}\r
\r
- uint32_t crc_received = msg.header.crc;\r
- msg.header.crc = 0;\r
+FMW_Result fmw_message_handle(FMW_State state, const FMW_Message *msg) {\r
+ if (!(state > FMW_State_None && state < FMW_State_COUNT)) { return FMW_Result_Error_MessageHandler_InvalidState; }\r
+\r
+ // NOTE(lb): the `msg->header.crc != -1` checks are just because i haven't\r
+ // implemented CRC into the program that sends these messages.\r
switch (state) {\r
case FMW_State_Init: {\r
- switch (msg.header.type) {\r
+ switch (msg->header.type) {\r
case FMW_MessageType_Run: {\r
- uint32_t crc_computed = HAL_CRC_Calculate(&hcrc, (uint32_t*)&msg, sizeof msg.header);\r
- FMW_REPORT_UNLESS(crc_received == -1 || crc_computed == crc_received, FMW_Error_UART_Crc);\r
+ if (msg->header.crc != -1) {\r
+ uint32_t crc_computed = HAL_CRC_Calculate(&hcrc, (uint32_t*)msg, sizeof msg->header.type);\r
+ if (!(crc_computed == msg->header.crc)) { return FMW_Result_Error_UART_Crc; }\r
+ }\r
fmw_state = FMW_State_Running;\r
} break;\r
case FMW_MessageType_Config_Robot: {\r
- uint32_t crc_computed = HAL_CRC_Calculate(&hcrc, (uint32_t*)&msg, sizeof msg.header.type + sizeof msg.config_robot);\r
- FMW_REPORT_UNLESS(crc_received == -1 || crc_computed == crc_received, FMW_Error_UART_Crc);\r
-\r
- odometry.baseline = msg.config_robot.baseline;\r
- encoders.left.wheel_circumference = msg.config_robot.left_wheel_circumference;\r
- encoders.left.ticks_per_revolution = msg.config_robot.left_ticks_per_revolution;\r
- encoders.right.wheel_circumference = msg.config_robot.right_wheel_circumference;\r
- encoders.right.ticks_per_revolution = msg.config_robot.right_ticks_per_revolution;\r
+ if (msg->header.crc != -1) {\r
+ uint32_t crc_computed = HAL_CRC_Calculate(&hcrc, (uint32_t*)msg, sizeof msg->header.type + sizeof msg->config_robot);\r
+ if (!(crc_computed == msg->header.crc)) { return FMW_Result_Error_UART_Crc; }\r
+ }\r
+\r
+ if (!(msg->config_robot.baseline > 0.f)) {\r
+ return FMW_Result_Error_MessageHandler_Init_NonPositiveBaseline;\r
+ }\r
+ if (!(msg->config_robot.wheel_circumference_left > 0.f ||\r
+ msg->config_robot.wheel_circumference_right > 0.f)) {\r
+ return FMW_Result_Error_MessageHandler_Init_NonPositiveWheelCircumference;\r
+ }\r
+ if (!(msg->config_robot.ticks_per_revolution_left >= FMW_MIN_ACCEPTABLE_TICKS_PER_REVOLUTION &&\r
+ msg->config_robot.ticks_per_revolution_left < FMW_MAX_ACCEPTABLE_TICKS_PER_REVOLUTION)) {\r
+ return FMW_Result_Error_MessageHandler_Init_InvalidTicksPerRevolution;\r
+ }\r
+ if (!(msg->config_robot.ticks_per_revolution_right >= FMW_MIN_ACCEPTABLE_TICKS_PER_REVOLUTION &&\r
+ msg->config_robot.ticks_per_revolution_right < FMW_MAX_ACCEPTABLE_TICKS_PER_REVOLUTION)) {\r
+ return FMW_Result_Error_MessageHandler_Init_InvalidTicksPerRevolution;\r
+ }\r
+\r
+ odometry.baseline = msg->config_robot.baseline;\r
+ encoders.left.wheel_circumference = msg->config_robot.wheel_circumference_left;\r
+ encoders.left.ticks_per_revolution = msg->config_robot.ticks_per_revolution_left;\r
+ encoders.right.wheel_circumference = msg->config_robot.wheel_circumference_right;\r
+ encoders.right.ticks_per_revolution = msg->config_robot.ticks_per_revolution_right;\r
} break;\r
case FMW_MessageType_Config_PID: {\r
- uint32_t crc_computed = HAL_CRC_Calculate(&hcrc, (uint32_t*)&msg, sizeof msg.header.type + sizeof msg.config_pid);\r
- FMW_REPORT_UNLESS(crc_received == -1 || crc_computed == crc_received, FMW_Error_UART_Crc);\r
+ if (msg->header.crc != -1) {\r
+ uint32_t crc_computed = HAL_CRC_Calculate(&hcrc, (uint32_t*)msg, sizeof msg->header.type + sizeof msg->config_pid);\r
+ if (!(crc_computed == msg->header.crc)) { return FMW_Result_Error_UART_Crc; }\r
+ }\r
\r
- memcpy(&pid_left.ks, &msg.config_pid.left, sizeof pid_left.ks);\r
- memcpy(&pid_right.ks, &msg.config_pid.right, sizeof pid_right.ks);\r
- memcpy(&pid_cross.ks, &msg.config_pid.cross, sizeof pid_cross.ks);\r
+ if (!(msg->config_pid.left.proportional >= FMW_MIN_ACCEPTABLE_PID_PROPORTIONAL &&\r
+ msg->config_pid.left.proportional < FMW_MAX_ACCEPTABLE_PID_PROPORTIONAL)) {\r
+ return FMW_Result_Error_MessageHandler_Init_InvalidPIDProportionalConstant;\r
+ }\r
+ if (!(msg->config_pid.cross.proportional >= FMW_MIN_ACCEPTABLE_PID_PROPORTIONAL &&\r
+ msg->config_pid.cross.proportional < FMW_MAX_ACCEPTABLE_PID_PROPORTIONAL)) {\r
+ return FMW_Result_Error_MessageHandler_Init_InvalidPIDProportionalConstant;\r
+ }\r
+ if (!(msg->config_pid.right.proportional >= FMW_MIN_ACCEPTABLE_PID_PROPORTIONAL &&\r
+ msg->config_pid.right.proportional < FMW_MAX_ACCEPTABLE_PID_PROPORTIONAL)) {\r
+ return FMW_Result_Error_MessageHandler_Init_InvalidPIDProportionalConstant;\r
+ }\r
+\r
+ if (!(msg->config_pid.left.integral >= FMW_MIN_ACCEPTABLE_PID_INTEGRAL &&\r
+ msg->config_pid.left.integral < FMW_MAX_ACCEPTABLE_PID_INTEGRAL)) {\r
+ return FMW_Result_Error_MessageHandler_Init_InvalidPIDIntegralConstant;\r
+ }\r
+ if (!(msg->config_pid.cross.integral >= FMW_MIN_ACCEPTABLE_PID_INTEGRAL &&\r
+ msg->config_pid.cross.integral < FMW_MAX_ACCEPTABLE_PID_INTEGRAL)) {\r
+ return FMW_Result_Error_MessageHandler_Init_InvalidPIDIntegralConstant;\r
+ }\r
+ if (!(msg->config_pid.right.integral >= FMW_MIN_ACCEPTABLE_PID_INTEGRAL &&\r
+ msg->config_pid.right.integral < FMW_MAX_ACCEPTABLE_PID_INTEGRAL)) {\r
+ return FMW_Result_Error_MessageHandler_Init_InvalidPIDIntegralConstant;\r
+ }\r
+\r
+ if (!(msg->config_pid.left.derivative >= FMW_MIN_ACCEPTABLE_PID_DERIVATIVE &&\r
+ msg->config_pid.left.derivative < FMW_MAX_ACCEPTABLE_PID_DERIVATIVE)) {\r
+ return FMW_Result_Error_MessageHandler_Init_InvalidPIDDerivativeConstant;\r
+ }\r
+ if (!(msg->config_pid.cross.derivative >= FMW_MIN_ACCEPTABLE_PID_DERIVATIVE &&\r
+ msg->config_pid.cross.derivative < FMW_MAX_ACCEPTABLE_PID_DERIVATIVE)) {\r
+ return FMW_Result_Error_MessageHandler_Init_InvalidPIDDerivativeConstant;\r
+ }\r
+ if (!(msg->config_pid.right.derivative >= FMW_MIN_ACCEPTABLE_PID_DERIVATIVE &&\r
+ msg->config_pid.right.derivative < FMW_MAX_ACCEPTABLE_PID_DERIVATIVE)) {\r
+ return FMW_Result_Error_MessageHandler_Init_InvalidPIDDerivativeConstant;\r
+ }\r
+\r
+ memcpy(&pid_left.ks, &msg->config_pid.left, sizeof pid_left.ks);\r
+ memcpy(&pid_right.ks, &msg->config_pid.right, sizeof pid_right.ks);\r
+ memcpy(&pid_cross.ks, &msg->config_pid.cross, sizeof pid_cross.ks);\r
} break;\r
case FMW_MessageType_Config_LED: {\r
- uint32_t crc_computed = HAL_CRC_Calculate(&hcrc, (uint32_t*)&msg, sizeof msg.header.type + sizeof msg.config_led);\r
- FMW_REPORT_UNLESS(crc_received == -1 || crc_computed == crc_received, FMW_Error_UART_Crc);\r
+ if (msg->header.crc != -1) {\r
+ uint32_t crc_computed = HAL_CRC_Calculate(&hcrc, (uint32_t*)msg, sizeof msg->header.type + sizeof msg->config_led);\r
+ if (!(crc_computed == msg->header.crc)) { return FMW_Result_Error_UART_Crc; }\r
+ }\r
\r
- pled.voltage_red = msg.config_led.voltage_red;\r
- pled.voltage_orange = msg.config_led.voltage_orange;\r
- pled.voltage_hysteresis = msg.config_led.voltage_hysteresis;\r
- led_update_period = msg.config_led.update_period;\r
+ if (!(msg->config_led.voltage_red >= FMW_MIN_ACCEPTABLE_LED_VOLTAGE &&\r
+ msg->config_led.voltage_red < FMW_MAX_ACCEPTABLE_LED_VOLTAGE)) {\r
+ return FMW_Result_Error_MessageHandler_Init_InvalidLEDVoltage;\r
+ }\r
+ if (!(msg->config_led.voltage_orange >= FMW_MIN_ACCEPTABLE_LED_VOLTAGE &&\r
+ msg->config_led.voltage_orange < FMW_MAX_ACCEPTABLE_LED_VOLTAGE)) {\r
+ return FMW_Result_Error_MessageHandler_Init_InvalidLEDVoltage;\r
+ }\r
+ if (!(msg->config_led.voltage_hysteresis >= FMW_MIN_ACCEPTABLE_LED_VOLTAGE &&\r
+ msg->config_led.voltage_hysteresis < FMW_MAX_ACCEPTABLE_LED_VOLTAGE)) {\r
+ return FMW_Result_Error_MessageHandler_Init_InvalidLEDVoltage;\r
+ }\r
+ if (!(msg->config_led.update_period >= FMW_MIN_ACCEPTABLE_LED_UPDATE_PERIOD &&\r
+ msg->config_led.update_period < FMW_MAX_ACCEPTABLE_LED_UPDATE_PERIOD)) {\r
+ return FMW_Result_Error_MessageHandler_Init_InvalidLEDVoltage;\r
+ }\r
+\r
+ pled.voltage_red = msg->config_led.voltage_red;\r
+ pled.voltage_orange = msg->config_led.voltage_orange;\r
+ pled.voltage_hysteresis = msg->config_led.voltage_hysteresis;\r
+ led_update_period = msg->config_led.update_period;\r
} break;\r
}\r
} break;\r
}\r
+\r
+ return FMW_Result_Ok;\r
}\r
\r
-void fmw_report_handler(FMW_Error error_code, const char *filename, int32_t filename_length, int32_t line) {\r
- // TODO(lb): send error message via UART,\r
- // go back to some sort of "initialization" state\r
- fmw_motor_brake(&motors.left);\r
- fmw_motor_brake(&motors.right);\r
- fmw_state = FMW_State_Error;\r
- for (;;) {}\r
+int32_t fmw_result_format(char buffer[], size_t buffer_size,\r
+ const char *filename, int16_t filename_length,\r
+ int32_t line, FMW_Result result) {\r
+ static const char* const variant_strings[] = {\r
+ #define X(Variant) #Variant,\r
+ FMW_RESULT_VARIANTS(X)\r
+ #undef X\r
+ };\r
+\r
+ return snprintf(buffer, buffer_size,\r
+ "[%.*s:%ld] failed with result: %s\r\n",\r
+ filename_length, filename, line,\r
+ variant_strings[result]);\r
+}\r
+\r
+void fmw_result_log_uart(UART_HandleTypeDef *huart, FMW_Result result,\r
+ const char *filename, int16_t filename_length,\r
+ int32_t line) {\r
+ if (fmw_state != FMW_State_Init) {\r
+ fmw_motor_brake(&motors.left);\r
+ fmw_motor_brake(&motors.right);\r
+ }\r
+\r
+ char buff[512] = {0};\r
+ int32_t length = fmw_result_format(buff, sizeof buff, filename, filename_length, line, result);\r
+ HAL_UART_Transmit(huart, (uint8_t*)buff, length, HAL_MAX_DELAY);\r
}\r
\r
// TODO(lb): move to fmw. maybe create a FMW_Buzzer?\r
/* USER CODE END 4 */\r
\r
/**\r
- * @brief This function is executed in case of error occurrence.\r
- * @retval None\r
- */\r
+ * @brief This function is executed in case of error occurrence.\r
+ * @retval None\r
+ */\r
void Error_Handler(void)\r
{\r
/* USER CODE BEGIN Error_Handler_Debug */\r
}\r
#ifdef USE_FULL_ASSERT\r
/**\r
- * @brief Reports the name of the source file and the source line number\r
- * where the assert_param error has occurred.\r
- * @param file: pointer to the source file name\r
- * @param line: assert_param error line source number\r
- * @retval None\r
- */\r
+ * @brief Reports the name of the source file and the source line number\r
+ * where the assert_param error has occurred.\r
+ * @param file: pointer to the source file name\r
+ * @param line: assert_param error line source number\r
+ * @retval None\r
+ */\r
void assert_failed(uint8_t *file, uint32_t line)\r
{\r
/* USER CODE BEGIN 6 */\r
projects / pioneer-stm32 / commitdiff