25#include "../Inc/Files/can1_dbc.h"
26#include "../Inc/Scheduler/Scheduler.h"
27#include "../Inc/Sensors/AnalogSensor.h"
28#include "../Inc/Systems/Comms/Can/Can.h"
29#include "../Inc/Systems/Controller/Apps.h"
30#include "../Inc/Systems/Controller/BrakeSystemControl.h"
31#include "../Inc/Systems/Controller/RTD.h"
32#include "../Inc/Systems/Controller/TorqueControl.h"
33#include "../Inc/Systems/External/Inverter.h"
34#include "../Inc/Systems/Monitor/AppsMonitor.h"
35#include "../Inc/Systems/Monitor/BrakePolice.h"
36#include "../Inc/Systems/Monitor/RTDMonitor.h"
37#include "../Inc/Systems/Monitor/TorquePolice.h"
38#include "../Inc/Utils/Constants.h"
41#include "stm32f7xx_hal_adc.h"
90 .name =
"defaultTask",
91 .stack_size = 128 * 4,
92 .priority = (osPriority_t) osPriorityNormal,
182 printf(
"\033[2J\033[1;1H");
183 printf(
"Welcome to VCU!!!\r\n");
185 printf(
"CAN1 init failed\r\n");
206 uint32_t multi_mode = (ADC123_COMMON->CCR & ADC_CCR_MULTI);
207 printf(
"ADC Multi-mode: 0x%08lX\r\n", multi_mode);
213 { .
name =
"VDD", .start_bit = 0, .length = 8, .endian = 0, .isSigned = 0, .scale = 0.1, .offset = 0, .min = 0, .max = 255, .unit =
"voltage", .reciever =
"reciever" },
214 { .name =
"Char 2", .start_bit = 8, .length = 8, .endian = 0, .isSigned = 0, .scale = 1, .offset = 0, .min = 0, .max = 255, .unit =
"unit", .reciever =
"reciever" },
215 { .name =
"Char 3", .start_bit = 16, .length = 8, .endian = 0, .isSigned = 0, .scale = 1, .offset = 0, .min = 0, .max = 255, .unit =
"unit", .reciever =
"reciever" },
216 { .name =
"Char 4", .start_bit = 24, .length = 8, .endian = 0, .isSigned = 0, .scale = 1, .offset = 0, .min = 0, .max = 255, .unit =
"unit", .reciever =
"reciever" },
217 { .name =
"Char 5", .start_bit = 32, .length = 8, .endian = 0, .isSigned = 0, .scale = 1, .offset = 0, .min = 0, .max = 255, .unit =
"unit", .reciever =
"reciever" },
218 { .name =
"Char 6", .start_bit = 40, .length = 8, .endian = 0, .isSigned = 0, .scale = 1, .offset = 0, .min = 0, .max = 255, .unit =
"unit", .reciever =
"reciever" },
219 { .name =
"Char 7", .start_bit = 48, .length = 8, .endian = 0, .isSigned = 0, .scale = 1, .offset = 0, .min = 0, .max = 255, .unit =
"unit", .reciever =
"reciever" },
220 { .name =
"Char 8", .start_bit = 56, .length = 8, .endian = 0, .isSigned = 0, .scale = 1, .offset = 0, .min = 0, .max = 255, .unit =
"unit", .reciever =
"reciever" }
231 uint8_t txdata[8] = {0, 1, 2, 3, 4, 5, 6, 7};
263 initRTD(&rtd, &apps, &bsc, 10, 0, 1);
302 updateables[5] = NULL;
306 printf(
"Starting Vehicle Control Unit with FreeRTOS scheduler...\n");
313 osKernelInitialize();
352 printf(
"ERROR: Scheduler returned unexpectedly!\n");
368 RCC_OscInitTypeDef RCC_OscInitStruct = {0};
369 RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
373 HAL_PWR_EnableBkUpAccess();
377 __HAL_RCC_PWR_CLK_ENABLE();
378 __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE3);
383 RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
384 RCC_OscInitStruct.HSEState = RCC_HSE_BYPASS;
385 RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
386 RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
387 RCC_OscInitStruct.PLL.PLLM = 4;
388 RCC_OscInitStruct.PLL.PLLN = 96;
389 RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV4;
390 RCC_OscInitStruct.PLL.PLLQ = 4;
391 RCC_OscInitStruct.PLL.PLLR = 2;
392 if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
399 RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
400 |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
401 RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
402 RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
403 RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
404 RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
406 if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_1) != HAL_OK)
424 ADC_ChannelConfTypeDef sConfig = {0};
432 hadc1.Instance = ADC1;
433 hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
434 hadc1.Init.Resolution = ADC_RESOLUTION_12B;
435 hadc1.Init.ScanConvMode = ADC_SCAN_ENABLE;
438 hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING;
439 hadc1.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T2_TRGO;
440 hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
441 hadc1.Init.NbrOfConversion = 7;
443 hadc1.Init.EOCSelection = ADC_EOC_SEQ_CONV;
444 if (HAL_ADC_Init(&
hadc1) != HAL_OK)
451 sConfig.Channel = ADC_CHANNEL_0;
452 sConfig.Rank = ADC_REGULAR_RANK_1;
453 sConfig.SamplingTime = ADC_SAMPLETIME_28CYCLES;
454 if (HAL_ADC_ConfigChannel(&
hadc1, &sConfig) != HAL_OK)
461 sConfig.Channel = ADC_CHANNEL_2;
462 sConfig.Rank = ADC_REGULAR_RANK_2;
463 if (HAL_ADC_ConfigChannel(&
hadc1, &sConfig) != HAL_OK)
470 sConfig.Channel = ADC_CHANNEL_6;
471 sConfig.Rank = ADC_REGULAR_RANK_3;
472 if (HAL_ADC_ConfigChannel(&
hadc1, &sConfig) != HAL_OK)
479 sConfig.Channel = ADC_CHANNEL_8;
480 sConfig.Rank = ADC_REGULAR_RANK_4;
481 if (HAL_ADC_ConfigChannel(&
hadc1, &sConfig) != HAL_OK)
488 sConfig.Channel = ADC_CHANNEL_10;
489 sConfig.Rank = ADC_REGULAR_RANK_5;
490 if (HAL_ADC_ConfigChannel(&
hadc1, &sConfig) != HAL_OK)
497 sConfig.Channel = ADC_CHANNEL_12;
498 sConfig.Rank = ADC_REGULAR_RANK_6;
499 if (HAL_ADC_ConfigChannel(&
hadc1, &sConfig) != HAL_OK)
506 sConfig.Rank = ADC_REGULAR_RANK_7;
507 if (HAL_ADC_ConfigChannel(&
hadc1, &sConfig) != HAL_OK)
529 ADC_ChannelConfTypeDef sConfig = {0};
537 hadc2.Instance = ADC2;
538 hadc2.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
539 hadc2.Init.Resolution = ADC_RESOLUTION_12B;
540 hadc2.Init.ScanConvMode = ADC_SCAN_ENABLE;
543 hadc2.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING;
544 hadc2.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T2_TRGO;
545 hadc2.Init.DataAlign = ADC_DATAALIGN_RIGHT;
546 hadc2.Init.NbrOfConversion = 7;
548 hadc2.Init.EOCSelection = ADC_EOC_SEQ_CONV;
549 if (HAL_ADC_Init(&
hadc2) != HAL_OK)
556 sConfig.Channel = ADC_CHANNEL_1;
557 sConfig.Rank = ADC_REGULAR_RANK_1;
558 sConfig.SamplingTime = ADC_SAMPLETIME_28CYCLES;
559 if (HAL_ADC_ConfigChannel(&
hadc2, &sConfig) != HAL_OK)
566 sConfig.Channel = ADC_CHANNEL_3;
567 sConfig.Rank = ADC_REGULAR_RANK_2;
568 if (HAL_ADC_ConfigChannel(&
hadc2, &sConfig) != HAL_OK)
575 sConfig.Channel = ADC_CHANNEL_7;
576 sConfig.Rank = ADC_REGULAR_RANK_3;
577 if (HAL_ADC_ConfigChannel(&
hadc2, &sConfig) != HAL_OK)
584 sConfig.Channel = ADC_CHANNEL_9;
585 sConfig.Rank = ADC_REGULAR_RANK_4;
586 if (HAL_ADC_ConfigChannel(&
hadc2, &sConfig) != HAL_OK)
593 sConfig.Channel = ADC_CHANNEL_11;
594 sConfig.Rank = ADC_REGULAR_RANK_5;
595 if (HAL_ADC_ConfigChannel(&
hadc2, &sConfig) != HAL_OK)
602 sConfig.Channel = ADC_CHANNEL_13;
603 sConfig.Rank = ADC_REGULAR_RANK_6;
604 if (HAL_ADC_ConfigChannel(&
hadc2, &sConfig) != HAL_OK)
611 sConfig.Channel = ADC_CHANNEL_15;
612 sConfig.Rank = ADC_REGULAR_RANK_7;
613 if (HAL_ADC_ConfigChannel(&
hadc2, &sConfig) != HAL_OK)
635 ADC_ChannelConfTypeDef sConfig = {0};
643 hadc3.Instance = ADC3;
644 hadc3.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
645 hadc3.Init.Resolution = ADC_RESOLUTION_12B;
646 hadc3.Init.ScanConvMode = ADC_SCAN_ENABLE;
649 hadc3.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING;
650 hadc3.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T2_TRGO;
651 hadc3.Init.DataAlign = ADC_DATAALIGN_RIGHT;
652 hadc3.Init.NbrOfConversion = 8;
654 hadc3.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
655 if (HAL_ADC_Init(&
hadc3) != HAL_OK)
662 sConfig.Channel = ADC_CHANNEL_4;
663 sConfig.Rank = ADC_REGULAR_RANK_1;
664 sConfig.SamplingTime = ADC_SAMPLETIME_28CYCLES;
665 if (HAL_ADC_ConfigChannel(&
hadc3, &sConfig) != HAL_OK)
672 sConfig.Channel = ADC_CHANNEL_5;
673 sConfig.Rank = ADC_REGULAR_RANK_2;
674 if (HAL_ADC_ConfigChannel(&
hadc3, &sConfig) != HAL_OK)
681 sConfig.Channel = ADC_CHANNEL_6;
682 sConfig.Rank = ADC_REGULAR_RANK_3;
683 if (HAL_ADC_ConfigChannel(&
hadc3, &sConfig) != HAL_OK)
690 sConfig.Channel = ADC_CHANNEL_7;
691 sConfig.Rank = ADC_REGULAR_RANK_4;
692 if (HAL_ADC_ConfigChannel(&
hadc3, &sConfig) != HAL_OK)
699 sConfig.Channel = ADC_CHANNEL_8;
700 sConfig.Rank = ADC_REGULAR_RANK_5;
701 if (HAL_ADC_ConfigChannel(&
hadc3, &sConfig) != HAL_OK)
708 sConfig.Channel = ADC_CHANNEL_9;
709 sConfig.Rank = ADC_REGULAR_RANK_6;
710 if (HAL_ADC_ConfigChannel(&
hadc3, &sConfig) != HAL_OK)
717 sConfig.Channel = ADC_CHANNEL_14;
718 sConfig.Rank = ADC_REGULAR_RANK_7;
719 if (HAL_ADC_ConfigChannel(&
hadc3, &sConfig) != HAL_OK)
726 sConfig.Channel = ADC_CHANNEL_15;
727 sConfig.Rank = ADC_REGULAR_RANK_8;
728 if (HAL_ADC_ConfigChannel(&
hadc3, &sConfig) != HAL_OK)
753 hcan1.Instance = CAN1;
754 hcan1.Init.Prescaler = 3;
755 hcan1.Init.Mode = CAN_MODE_NORMAL;
756 hcan1.Init.SyncJumpWidth = CAN_SJW_1TQ;
757 hcan1.Init.TimeSeg1 = CAN_BS1_13TQ;
758 hcan1.Init.TimeSeg2 = CAN_BS2_2TQ;
765 if (HAL_CAN_Init(&
hcan1) != HAL_OK)
770 CAN_FilterTypeDef sFilterConfig;
771 sFilterConfig.FilterBank = 0;
772 sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
773 sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
774 sFilterConfig.FilterIdHigh = 0x0000;
775 sFilterConfig.FilterIdLow = 0x0000;
776 sFilterConfig.FilterMaskIdHigh = 0x0000;
777 sFilterConfig.FilterMaskIdLow = 0x0000;
778 sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0;
779 sFilterConfig.FilterActivation =
ENABLE;
780 sFilterConfig.SlaveStartFilterBank = 14;
781 HAL_CAN_ConfigFilter(&
hcan1, &sFilterConfig);
784 HAL_CAN_Stop(&
hcan1);
786 if (HAL_CAN_Start(&
hcan1) != HAL_OK) {
787 printf(
"CAN1 Start Error: ErrorCode = 0x%lX\r\n",
hcan1.ErrorCode);
791 if (HAL_CAN_ActivateNotification(&
hcan1, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK) {
792 printf(
"interrupt CAN1 init failed\r\n");
813 hcan2.Instance = CAN2;
814 hcan2.Init.Prescaler = 4;
815 hcan2.Init.Mode = CAN_MODE_NORMAL;
816 hcan2.Init.SyncJumpWidth = CAN_SJW_1TQ;
817 hcan2.Init.TimeSeg1 = CAN_BS1_10TQ;
818 hcan2.Init.TimeSeg2 = CAN_BS2_1TQ;
825 if (HAL_CAN_Init(&
hcan2) != HAL_OK)
830 CAN_FilterTypeDef sFilterConfig;
831 sFilterConfig.FilterBank = 0;
832 sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
833 sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
834 sFilterConfig.FilterIdHigh = 0x0000;
835 sFilterConfig.FilterIdLow = 0x0000;
836 sFilterConfig.FilterMaskIdHigh = 0x0000;
837 sFilterConfig.FilterMaskIdLow = 0x0000;
838 sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0;
839 sFilterConfig.FilterActivation =
ENABLE;
840 sFilterConfig.SlaveStartFilterBank = 14;
841 HAL_CAN_ConfigFilter(&
hcan2, &sFilterConfig);
844 HAL_CAN_Stop(&
hcan2);
846 if (HAL_CAN_Start(&
hcan2) != HAL_OK) {
847 printf(
"CAN2 Start Error: ErrorCode = 0x%lX\r\n",
hcan2.ErrorCode);
851 if (HAL_CAN_ActivateNotification(&
hcan2, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK) {
852 printf(
"interrupt CAN2 init failed\r\n");
873 hcan3.Instance = CAN3;
874 hcan3.Init.Prescaler = 16;
875 hcan3.Init.Mode = CAN_MODE_NORMAL;
876 hcan3.Init.SyncJumpWidth = CAN_SJW_1TQ;
877 hcan3.Init.TimeSeg1 = CAN_BS1_1TQ;
878 hcan3.Init.TimeSeg2 = CAN_BS2_1TQ;
885 if (HAL_CAN_Init(&
hcan3) != HAL_OK)
907 DAC_ChannelConfTypeDef sConfig = {0};
916 if (HAL_DAC_Init(&
hdac) != HAL_OK)
923 sConfig.DAC_Trigger = DAC_TRIGGER_T2_TRGO;
924 sConfig.DAC_OutputBuffer = DAC_OUTPUTBUFFER_ENABLE;
925 if (HAL_DAC_ConfigChannel(&
hdac, &sConfig, DAC_CHANNEL_1) != HAL_OK)
932 if (HAL_DAC_ConfigChannel(&
hdac, &sConfig, DAC_CHANNEL_2) != HAL_OK)
957 hi2c2.Instance = I2C2;
958 hi2c2.Init.Timing = 0x00506682;
959 hi2c2.Init.OwnAddress1 = 0;
960 hi2c2.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
961 hi2c2.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
962 hi2c2.Init.OwnAddress2 = 0;
963 hi2c2.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
964 hi2c2.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
965 hi2c2.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
966 if (HAL_I2C_Init(&
hi2c2) != HAL_OK)
973 if (HAL_I2CEx_ConfigAnalogFilter(&
hi2c2, I2C_ANALOGFILTER_ENABLE) != HAL_OK)
980 if (HAL_I2CEx_ConfigDigitalFilter(&
hi2c2, 0) != HAL_OK)
1005 hi2c4.Instance = I2C4;
1006 hi2c4.Init.Timing = 0x00506682;
1007 hi2c4.Init.OwnAddress1 = 0;
1008 hi2c4.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
1009 hi2c4.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
1010 hi2c4.Init.OwnAddress2 = 0;
1011 hi2c4.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
1012 hi2c4.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
1013 hi2c4.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
1014 if (HAL_I2C_Init(&
hi2c4) != HAL_OK)
1021 if (HAL_I2CEx_ConfigAnalogFilter(&
hi2c4, I2C_ANALOGFILTER_ENABLE) != HAL_OK)
1028 if (HAL_I2CEx_ConfigDigitalFilter(&
hi2c4, 0) != HAL_OK)
1054 hspi4.Instance = SPI4;
1055 hspi4.Init.Mode = SPI_MODE_MASTER;
1056 hspi4.Init.Direction = SPI_DIRECTION_2LINES;
1057 hspi4.Init.DataSize = SPI_DATASIZE_4BIT;
1058 hspi4.Init.CLKPolarity = SPI_POLARITY_LOW;
1059 hspi4.Init.CLKPhase = SPI_PHASE_1EDGE;
1060 hspi4.Init.NSS = SPI_NSS_SOFT;
1061 hspi4.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_2;
1062 hspi4.Init.FirstBit = SPI_FIRSTBIT_MSB;
1063 hspi4.Init.TIMode = SPI_TIMODE_DISABLE;
1064 hspi4.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
1065 hspi4.Init.CRCPolynomial = 7;
1066 hspi4.Init.CRCLength = SPI_CRC_LENGTH_DATASIZE;
1067 hspi4.Init.NSSPMode = SPI_NSS_PULSE_ENABLE;
1068 if (HAL_SPI_Init(&
hspi4) != HAL_OK)
1094 hspi6.Instance = SPI6;
1095 hspi6.Init.Mode = SPI_MODE_MASTER;
1096 hspi6.Init.Direction = SPI_DIRECTION_2LINES;
1097 hspi6.Init.DataSize = SPI_DATASIZE_4BIT;
1098 hspi6.Init.CLKPolarity = SPI_POLARITY_LOW;
1099 hspi6.Init.CLKPhase = SPI_PHASE_1EDGE;
1100 hspi6.Init.NSS = SPI_NSS_SOFT;
1101 hspi6.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_2;
1102 hspi6.Init.FirstBit = SPI_FIRSTBIT_MSB;
1103 hspi6.Init.TIMode = SPI_TIMODE_DISABLE;
1104 hspi6.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
1105 hspi6.Init.CRCPolynomial = 7;
1106 hspi6.Init.CRCLength = SPI_CRC_LENGTH_DATASIZE;
1107 hspi6.Init.NSSPMode = SPI_NSS_PULSE_ENABLE;
1108 if (HAL_SPI_Init(&
hspi6) != HAL_OK)
1130 TIM_ClockConfigTypeDef sClockSourceConfig = {0};
1131 TIM_MasterConfigTypeDef sMasterConfig = {0};
1136 htim1.Instance = TIM1;
1137 htim1.Init.Prescaler = 47999;
1138 htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
1139 htim1.Init.Period = 999;
1140 htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
1141 htim1.Init.RepetitionCounter = 0;
1142 htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
1143 if (HAL_TIM_Base_Init(&
htim1) != HAL_OK)
1147 sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
1148 if (HAL_TIM_ConfigClockSource(&
htim1, &sClockSourceConfig) != HAL_OK)
1152 sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
1153 sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET;
1154 sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
1155 if (HAL_TIMEx_MasterConfigSynchronization(&
htim1, &sMasterConfig) != HAL_OK)
1177 TIM_ClockConfigTypeDef sClockSourceConfig = {0};
1178 TIM_MasterConfigTypeDef sMasterConfig = {0};
1183 htim2.Instance = TIM2;
1184 htim2.Init.Prescaler = 0;
1185 htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
1186 htim2.Init.Period = 479999;
1187 htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
1188 htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
1189 if (HAL_TIM_Base_Init(&
htim2) != HAL_OK)
1193 sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
1194 if (HAL_TIM_ConfigClockSource(&
htim2, &sClockSourceConfig) != HAL_OK)
1198 sMasterConfig.MasterOutputTrigger = TIM_TRGO_UPDATE;
1199 sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
1200 if (HAL_TIMEx_MasterConfigSynchronization(&
htim2, &sMasterConfig) != HAL_OK)
1205 if (HAL_TIM_Base_Start_IT(&
htim2) != HAL_OK) {
1227 huart3.Instance = USART3;
1228 huart3.Init.BaudRate = 115200;
1229 huart3.Init.WordLength = UART_WORDLENGTH_8B;
1230 huart3.Init.StopBits = UART_STOPBITS_1;
1231 huart3.Init.Parity = UART_PARITY_NONE;
1232 huart3.Init.Mode = UART_MODE_TX_RX;
1233 huart3.Init.HwFlowCtl = UART_HWCONTROL_NONE;
1234 huart3.Init.OverSampling = UART_OVERSAMPLING_16;
1235 huart3.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
1236 huart3.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
1237 if (HAL_UART_Init(&
huart3) != HAL_OK)
1254 __HAL_RCC_DMA2_CLK_ENABLE();
1255 __HAL_RCC_DMA1_CLK_ENABLE();
1259 HAL_NVIC_SetPriority(DMA1_Stream5_IRQn, 5, 0);
1260 HAL_NVIC_EnableIRQ(DMA1_Stream5_IRQn);
1262 HAL_NVIC_SetPriority(DMA1_Stream6_IRQn, 5, 0);
1263 HAL_NVIC_EnableIRQ(DMA1_Stream6_IRQn);
1265 HAL_NVIC_SetPriority(DMA2_Stream0_IRQn, 5, 0);
1266 HAL_NVIC_EnableIRQ(DMA2_Stream0_IRQn);
1268 HAL_NVIC_SetPriority(DMA2_Stream1_IRQn, 5, 0);
1269 HAL_NVIC_EnableIRQ(DMA2_Stream1_IRQn);
1271 HAL_NVIC_SetPriority(DMA2_Stream2_IRQn, 5, 0);
1272 HAL_NVIC_EnableIRQ(DMA2_Stream2_IRQn);
1283 GPIO_InitTypeDef GPIO_InitStruct = {0};
1288 __HAL_RCC_GPIOE_CLK_ENABLE();
1289 __HAL_RCC_GPIOC_CLK_ENABLE();
1290 __HAL_RCC_GPIOF_CLK_ENABLE();
1291 __HAL_RCC_GPIOH_CLK_ENABLE();
1292 __HAL_RCC_GPIOA_CLK_ENABLE();
1293 __HAL_RCC_GPIOB_CLK_ENABLE();
1294 __HAL_RCC_GPIOG_CLK_ENABLE();
1295 __HAL_RCC_GPIOD_CLK_ENABLE();
1298 HAL_GPIO_WritePin(GPIOF, GPIO_PIN_12|GPIO_PIN_13, GPIO_PIN_RESET);
1301 HAL_GPIO_WritePin(GPIOE, GPIO_PIN_7|GPIO_PIN_8|GPIO_PIN_9|GPIO_PIN_11
1302 |GPIO_PIN_13, GPIO_PIN_RESET);
1305 HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14|GPIO_PIN_15, GPIO_PIN_RESET);
1308 HAL_GPIO_WritePin(GPIOG, GPIO_PIN_9, GPIO_PIN_RESET);
1315 GPIO_InitStruct.Pin = GPIO_PIN_3|GPIO_PIN_4|GPIO_PIN_10|GPIO_PIN_12
1316 |GPIO_PIN_14|GPIO_PIN_15|GPIO_PIN_0|GPIO_PIN_1;
1317 GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
1318 GPIO_InitStruct.Pull = GPIO_NOPULL;
1319 HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
1323 GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
1324 GPIO_InitStruct.Pull = GPIO_NOPULL;
1328 GPIO_InitStruct.Pin = GPIO_PIN_2|GPIO_PIN_11;
1329 GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
1330 GPIO_InitStruct.Pull = GPIO_NOPULL;
1331 HAL_GPIO_Init(GPIOF, &GPIO_InitStruct);
1335 GPIO_InitStruct.Pin = GPIO_PIN_2|GPIO_PIN_10|GPIO_PIN_11|GPIO_PIN_12
1336 |GPIO_PIN_14|GPIO_PIN_15|GPIO_PIN_4|GPIO_PIN_6;
1337 GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
1338 GPIO_InitStruct.Pull = GPIO_NOPULL;
1339 HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
1342 GPIO_InitStruct.Pin = GPIO_PIN_12|GPIO_PIN_13;
1343 GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
1344 GPIO_InitStruct.Pull = GPIO_NOPULL;
1345 GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
1346 HAL_GPIO_Init(GPIOF, &GPIO_InitStruct);
1351 GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3
1352 |GPIO_PIN_4|GPIO_PIN_5|GPIO_PIN_6|GPIO_PIN_8
1353 |GPIO_PIN_10|GPIO_PIN_11|GPIO_PIN_15;
1354 GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
1355 GPIO_InitStruct.Pull = GPIO_NOPULL;
1356 HAL_GPIO_Init(GPIOG, &GPIO_InitStruct);
1360 GPIO_InitStruct.Pin = GPIO_PIN_7|GPIO_PIN_8|GPIO_PIN_9|GPIO_PIN_11
1362 GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
1363 GPIO_InitStruct.Pull = GPIO_NOPULL;
1364 GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
1365 HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
1370 GPIO_InitStruct.Pin = GPIO_PIN_10|GPIO_PIN_11|GPIO_PIN_12|GPIO_PIN_13
1371 |GPIO_PIN_3|GPIO_PIN_4|GPIO_PIN_5|GPIO_PIN_6
1373 GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
1374 GPIO_InitStruct.Pull = GPIO_NOPULL;
1375 HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
1378 GPIO_InitStruct.Pin = GPIO_PIN_14|GPIO_PIN_15;
1379 GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
1380 GPIO_InitStruct.Pull = GPIO_NOPULL;
1381 GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
1382 HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
1386 GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
1387 GPIO_InitStruct.Pull = GPIO_NOPULL;
1392 GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_7|GPIO_PIN_8|GPIO_PIN_9
1393 |GPIO_PIN_10|GPIO_PIN_11|GPIO_PIN_12;
1394 GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
1395 GPIO_InitStruct.Pull = GPIO_NOPULL;
1396 GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
1397 GPIO_InitStruct.Alternate = GPIO_AF12_SDMMC1;
1398 HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
1401 GPIO_InitStruct.Pin = GPIO_PIN_9|GPIO_PIN_10|GPIO_PIN_11|GPIO_PIN_12;
1402 GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
1403 GPIO_InitStruct.Pull = GPIO_NOPULL;
1404 HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
1407 GPIO_InitStruct.Pin = GPIO_PIN_2;
1408 GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
1409 GPIO_InitStruct.Pull = GPIO_NOPULL;
1410 GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
1411 GPIO_InitStruct.Alternate = GPIO_AF12_SDMMC1;
1412 HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
1415 GPIO_InitStruct.Pin = GPIO_PIN_9;
1416 GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
1417 GPIO_InitStruct.Pull = GPIO_NOPULL;
1418 GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
1419 HAL_GPIO_Init(GPIOG, &GPIO_InitStruct);
1422 GPIO_InitStruct.Pin =
LD2_Pin;
1423 GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
1424 GPIO_InitStruct.Pull = GPIO_NOPULL;
1425 GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
1429 GPIO_InitStruct.Pin = GPIO_PIN_8|GPIO_PIN_9;
1430 GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
1431 GPIO_InitStruct.Pull = GPIO_NOPULL;
1432 GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
1433 GPIO_InitStruct.Alternate = GPIO_AF12_SDMMC1;
1434 HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
1445 HAL_UART_Transmit(&
huart3, (uint8_t *)data, len, HAL_MAX_DELAY);
1454 printf(
"ERROR: FreeRTOS malloc failed!\n");
1464 printf(
"ERROR: Stack overflow in task: %s\n", pcTaskName);
1507 printf(
"Error_Handler\r\n");
1512#ifdef USE_FULL_ASSERT
1520void assert_failed(uint8_t *file, uint32_t line)
void initAppsMonitor(AppsMonitor *am, Apps *apps, int hz)
int startAppsMonitor(AppsMonitor *am)
void initApps(Apps *apps, int hz, int channel1, int channel2)
Initializes the APPs with the given frequency and channel.
int startBrakePolice(BrakePolice *bp)
void initBrakePolice(BrakePolice *bp, BrakeSystemControl *bsc, int hz, int maxTemp)
void initBrakeSystemControl(BrakeSystemControl *bsc, int hz, int maxTemp, int brakeLightActivationPoint, int heavyBrakingActivationPoint, int fbp_channel, int rbp_channel, int temp_channel, int light_port)
Initializes the Braking System with initial settings.
int startBrakeSystemControl(BrakeSystemControl *bsc)
Starts the Brake System Controller.
int init_CANBus(CANBus bus)
Initializes the CANBus struct.
int add_message_lop(CANBus bus, int id, int dlc, int ide, int rtr, const char *name, const char *sender, int signal_count, CAN_Signal_Template *signals)
Adds a message to the CAN message list by passing in the individual message parameters hence "LOP" (L...
int send_CAN_message(CANBus bus, CANProtocol protocol, uint32_t id, uint8_t *data, uint8_t len)
Sends a CAN message.
void print_CAN_Messages_Lists()
Prints the CAN message list.
#define ADC2_CHANNEL_SIZE
#define NUM_DIGITAL_INPUTS
#define NUM_DIGITAL_OUTPUTS
#define ADC1_CHANNEL_SIZE
#define ADC3_CHANNEL_SIZE
void initInverter(Inverter *inverter, TorqueControl *tc, int hz, int maxCurrent, int maxTemp, int maxVoltage)
Initializes the Inverter with initial settings.
void initRTDMonitor(RTDMonitor *rtdm, RTD *rtd, int hz)
Initializes the RTD Monitor with initial settings.
int startRTDMonitor(RTDMonitor *rtdm)
Starts the RTD Monitor.
int startRTD(RTD *rtd)
Starts the RTD Actuator.
void initRTD(RTD *rtd, Apps *apps, BrakeSystemControl *bsc, int hz, int buttonPort, int piezoPort)
Initializes the RTD Actuator with initial settings.
void SchedulerInit(Scheduler *scheduler, Updateable *updatableArray[])
void SchedulerRun(Scheduler *scheduler)
static void MX_CAN3_Init(void)
CAN3 Initialization Function.
DMA_HandleTypeDef hdma_dac1
void Error_Handler(void)
This function is executed in case of error occurrence.
uint32_t adc3_buffer[ADC3_CHANNEL_SIZE]
DMA_HandleTypeDef hdma_adc1
static void MX_USART3_UART_Init(void)
USART3 Initialization Function.
static void MX_TIM1_Init(void)
TIM1 Initialization Function.
const osThreadAttr_t defaultTask_attributes
static void MX_CAN1_Init(void)
CAN1 Initialization Function.
DMA_HandleTypeDef hdma_adc3
uint8_t digital_in_buffer[NUM_DIGITAL_INPUTS]
static void MX_ADC2_Init(void)
ADC2 Initialization Function.
void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName)
static void MX_SPI6_Init(void)
SPI6 Initialization Function.
static void MX_DMA_Init(void)
uint32_t adc2_buffer[ADC2_CHANNEL_SIZE]
osThreadId_t defaultTaskHandle
void SystemClock_Config(void)
System Clock Configuration.
int main(void)
The application entry point.
int _write(int file, char *data, int len)
static void MX_CAN2_Init(void)
CAN2 Initialization Function.
uint32_t dac2_buffer[DAC2_BUFFER_SIZE]
void vApplicationIdleHook(void)
void StartDefaultTask(void *argument)
Function implementing the defaultTask thread.
void vApplicationTickHook(void)
static void MX_ADC1_Init(void)
ADC1 Initialization Function.
uint8_t digital_out_buffer[NUM_DIGITAL_OUTPUTS]
DMA_HandleTypeDef hdma_dac2
UART_HandleTypeDef huart3
void vApplicationMallocFailedHook(void)
DMA_HandleTypeDef hdma_adc2
static void MX_ADC3_Init(void)
ADC3 Initialization Function.
static void MX_SPI4_Init(void)
SPI4 Initialization Function.
static void MX_GPIO_Init(void)
GPIO Initialization Function.
static void MX_DAC_Init(void)
DAC Initialization Function.
static void MX_I2C4_Init(void)
I2C4 Initialization Function.
static void MX_I2C2_Init(void)
I2C2 Initialization Function.
uint32_t adc1_buffer[ADC1_CHANNEL_SIZE]
static void MX_TIM2_Init(void)
TIM2 Initialization Function.
uint32_t dac1_buffer[DAC1_BUFFER_SIZE]
int startTorqueControl(TorqueControl *tc)
Starts the Torque Control Actuator.
void initTorqueControl(TorqueControl *tc, Apps *apps, int hz, float maxTorque)
Initializes the Torque Control Actuator with initial settings.
void initTorquePolice(TorquePolice *tp, TorqueControl *tc, BrakeSystemControl *bsc, RTD *rtd, int hz, float maxTorque)
int startTorquePolice(TorquePolice *tp)
: Header for main.c file. This file contains the common defines of the application.
#define USB_OverCurrent_GPIO_Port
#define USB_OverCurrent_Pin
#define USER_Btn_GPIO_Port
char name[MAX_SIGNAL_NAME_LENGTH]
int(* addMonitor)(struct ControllerSystem *controller, MonitorSystem *monitor)