diff --git a/Core/Src/main.c b/Core/Src/main.c index 824509b..549b3bd 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -74,6 +74,7 @@ const osThreadAttr_t defaultTask_attributes = { /* Private function prototypes -----------------------------------------------*/ void SystemClock_Config(void); static void MX_GPIO_Init(void); +static void MX_DMA_Init(void); static void MX_I2C1_Init(void); static void MX_I2S3_Init(void); static void MX_SPI1_Init(void); @@ -86,6 +87,7 @@ static void MX_TIM4_Init(void); static void MX_USART3_UART_Init(void); void StartDefaultTask(void *argument); + /* USER CODE BEGIN PFP */ /* USER CODE END PFP */ @@ -124,6 +126,7 @@ int main(void) /* Initialize all configured peripherals */ MX_GPIO_Init(); + MX_DMA_Init(); MX_I2C1_Init(); MX_I2S3_Init(); MX_SPI1_Init(); @@ -718,6 +721,22 @@ static void MX_USART3_UART_Init(void) } +/** + * Enable DMA controller clock + */ +static void MX_DMA_Init(void) +{ + + /* DMA controller clock enable */ + __HAL_RCC_DMA2_CLK_ENABLE(); + + /* DMA interrupt init */ + /* DMA2_Stream0_IRQn interrupt configuration */ + HAL_NVIC_SetPriority(DMA2_Stream0_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(DMA2_Stream0_IRQn); + +} + /** * @brief GPIO Initialization Function * @param None diff --git a/KPI_Rover/ADC/drvAdc.c b/KPI_Rover/ADC/drvAdc.c new file mode 100644 index 0000000..0613212 --- /dev/null +++ b/KPI_Rover/ADC/drvAdc.c @@ -0,0 +1,184 @@ +#include "drvAdc.h" +#include "stm32f4xx_hal.h" +#include "cmsis_os.h" +#include + +extern ADC_HandleTypeDef hadc1; + +#define DRV_ADC_FLAG_DMA_COMPLETE 0x01 +#define DRV_ADC_FLAG_TIMER_TRIG 0x02 +#define MEASUREMENT_PERIOD_MS 100 + +typedef enum { ADC_STATE_IDLE, ADC_STATE_BUSY } drv_adc_state_t; + +static drv_adc_state_t current_state = ADC_STATE_IDLE; +static uint8_t active_hw_channels[MAX_HW_ADC_ID]; +static size_t active_channel_count = 0; + +static uint16_t dma_buffer[MAX_HW_ADC_ID * SAMPLES_PER_CYCLE]; + +static osThreadId_t drv_adc_thread_id = NULL; +static osTimerId_t drv_adc_timer_id = NULL; + +typedef struct { + drvAdc_Callback cb; + void* ctx; +} adc_callback_entry_t; +static adc_callback_entry_t callback_table[MAX_HW_ADC_ID]; + + +static uint16_t Filter_Algorithm_36Samples(uint16_t* data, size_t step) { + uint32_t sum = 0; + uint16_t min1 = 0xFFFF, min2 = 0xFFFF; + uint16_t max1 = 0, max2 = 0; + + for (int i = 0; i < SAMPLES_PER_CYCLE; i++) { + uint16_t val = data[i * step]; + sum += val; + + if (val < min1) { + min2 = min1; min1 = val; + } else if (val < min2) { + min2 = val; + } + + if (val > max1) { + max2 = max1; max1 = val; + } else if (val > max2) { + max2 = val; + } + } + + sum -= (min1 + min2 + max1 + max2); + return (uint16_t)(sum >> 5); // / 32 +} + +static void drvAdc_WorkerTask(void *argument) { + for (;;) { + uint32_t flags = osThreadFlagsWait(DRV_ADC_FLAG_TIMER_TRIG | DRV_ADC_FLAG_DMA_COMPLETE, osFlagsWaitAny, osWaitForever); + + switch (current_state) { + + case ADC_STATE_IDLE: + if (flags & DRV_ADC_FLAG_TIMER_TRIG) { + HAL_StatusTypeDef status = HAL_ADC_Start_DMA(&hadc1, + (uint32_t*)dma_buffer, active_channel_count * SAMPLES_PER_CYCLE); + + if (status == HAL_OK) { + current_state = ADC_STATE_BUSY; + } + else { + } + } + break; + + case ADC_STATE_BUSY: + if (flags & DRV_ADC_FLAG_DMA_COMPLETE) { + + HAL_ADC_Stop_DMA(&hadc1); + HAL_ADC_Stop(&hadc1); + + for (size_t i = 0; i < active_channel_count; i++) { + uint8_t hw_ch = active_hw_channels[i]; + + uint16_t filtered = Filter_Algorithm_36Samples(&dma_buffer[i], active_channel_count); + + if (hw_ch < MAX_HW_ADC_ID && callback_table[hw_ch].cb != NULL) { + callback_table[hw_ch].cb(hw_ch, (uint32_t)filtered, callback_table[hw_ch].ctx); + } + } + + current_state = ADC_STATE_IDLE; + } + break; + + default: + HAL_ADC_Stop_DMA(&hadc1); + current_state = ADC_STATE_IDLE; + break; + } + } +} + +static void drvAdc_TimerCallback(void *arg) { + if (drv_adc_thread_id) { + osThreadFlagsSet(drv_adc_thread_id, DRV_ADC_FLAG_TIMER_TRIG); + } +} + +void drvAdc_init(uint32_t enabledChannels_mask) { + active_channel_count = 0; + memset(callback_table, 0, sizeof(callback_table)); + memset(dma_buffer, 0, sizeof(dma_buffer)); + + hadc1.Instance = ADC1; + for (int i = 0; i < MAX_HW_ADC_ID; i++) { + if (enabledChannels_mask & (1U << i)) { + active_channel_count++; + } + } + + hadc1.Init.NbrOfConversion = active_channel_count; + hadc1.Init.ScanConvMode = ENABLE; + hadc1.Init.ContinuousConvMode = ENABLE; + hadc1.Init.DiscontinuousConvMode = DISABLE; + hadc1.Init.DMAContinuousRequests = DISABLE; + HAL_ADC_Init(&hadc1); + + ADC_ChannelConfTypeDef sConfig = {0}; + uint8_t rank_counter = 1; + active_channel_count = 0; + + for (int i = 0; i < MAX_HW_ADC_ID; i++) { + if (enabledChannels_mask & (1U << i)) { + active_hw_channels[active_channel_count++] = i; + sConfig.Channel = i; + sConfig.Rank = rank_counter++; + sConfig.SamplingTime = ADC_SAMPLETIME_480CYCLES; + HAL_ADC_ConfigChannel(&hadc1, &sConfig); + + } + } + + const osThreadAttr_t task_attrs = { + .name = "DrvAdcTask", + .stack_size = 512, + .priority = osPriorityAboveNormal + }; + + drv_adc_thread_id = osThreadNew(drvAdc_WorkerTask, NULL, &task_attrs); + + const osTimerAttr_t timer_attrs = { + .name = "DrvAdcTimer" + }; + + drv_adc_timer_id = osTimerNew(drvAdc_TimerCallback, osTimerPeriodic, NULL, &timer_attrs); +} + +void drvAdc_startMeasurement(void) { + if (drv_adc_timer_id) { + osTimerStart(drv_adc_timer_id, MEASUREMENT_PERIOD_MS); + } +} + +void drvAdc_stopMeasurement(void) { + if (drv_adc_timer_id) { + osTimerStop(drv_adc_timer_id); + } + HAL_ADC_Stop_DMA(&hadc1); +} + +void drvAdc_registerCallback(uint8_t channel, drvAdc_Callback cb, void* ctx) { + if (channel < MAX_HW_ADC_ID) { + callback_table[channel].cb = cb; + callback_table[channel].ctx = ctx; + } +} + +void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc) { + if (hadc->Instance == ADC1) { + if (drv_adc_thread_id) { + osThreadFlagsSet(drv_adc_thread_id, DRV_ADC_FLAG_DMA_COMPLETE); + } + } +} diff --git a/KPI_Rover/ADC/drvAdc.h b/KPI_Rover/ADC/drvAdc.h new file mode 100644 index 0000000..ad6e0da --- /dev/null +++ b/KPI_Rover/ADC/drvAdc.h @@ -0,0 +1,16 @@ +#ifndef DRV_ADC_H +#define DRV_ADC_H + +#include + +#define SAMPLES_PER_CYCLE 36 +#define MAX_HW_ADC_ID 19 + +typedef void (*drvAdc_Callback)(uint8_t channel, uint32_t filteredRaw, void* ctx); + +void drvAdc_init(uint32_t enabledChannels_mask); +void drvAdc_startMeasurement(void); +void drvAdc_stopMeasurement(void); +void drvAdc_registerCallback(uint8_t channel, drvAdc_Callback cb, void* ctx); + +#endif // DRV_ADC_H diff --git a/KPI_Rover/ADC/ulAdc.c b/KPI_Rover/ADC/ulAdc.c new file mode 100644 index 0000000..f70bb25 --- /dev/null +++ b/KPI_Rover/ADC/ulAdc.c @@ -0,0 +1,259 @@ +#include "ulAdc.h" +#include "drvAdc.h" +#include +#include +#include +#include +#include "ulog.h" +#include "Database/ulDatabase.h" + +#define HW_CH_BATTERY 11 +#define HW_CH_TEMP 16 + +#define ADC_MAX_VAL 4095.0f +#define ADC_VREF 3.3f + +// 1. Battery: Voltage Divider /2.V = Raw * (3.3/4095) * 2 +#define BAT_DEFAULT_SCALE ((ADC_VREF / ADC_MAX_VAL) * 2.0f) +#define BAT_DEFAULT_OFFSET 0 + +// 2. Temp: T = (V - V25)/Slope + 25 +// V = Raw * (3.3/4095). +// T = [Raw * (3.3/4095) - V25] / Slope + 25 +// T = Raw * [3.3 / (4095*Slope)] - [V25/Slope - 25] +// T = Scale * (Raw - Offset_Raw) +// if Scale = 3.3 / (4095*Slope) and Offset_Raw represents the zero-crossing. +#define TEMP_V25 0.76f +#define TEMP_SLOPE 0.0025f +#define TEMP_DEFAULT_SCALE (ADC_VREF / (ADC_MAX_VAL * TEMP_SLOPE)) + +#define TEMP_DEFAULT_OFFSET (int32_t)(((TEMP_V25 - (25.0f * TEMP_SLOPE)) * ADC_MAX_VAL) / ADC_VREF) + +#define ADC_CAL_STATUS_OK 0x00 +#define ADC_CAL_STATUS_IN_PROGRESS 0x01 +#define ADC_CAL_STATUS_ERROR 0xFF + +#define LOG_INTERVAL_MS 1000 + +typedef enum { + CAL_STATE_IDLE = 0, + CAL_STATE_WAIT_FOR_SAMPLE +} adc_cal_state_t; + +typedef struct { + uint8_t hw_channel; + + uint16_t db_id_val; + uint16_t db_id_cal_offset; + uint16_t db_id_cal_scale; + + volatile int32_t active_offset; + volatile float active_scale; + + adc_cal_state_t cal_state; + uint8_t pending_point; + float pending_target; + + volatile uint32_t last_raw; + volatile float last_phys; +} adc_channel_ctx_t; + +adc_channel_ctx_t app_adc_ctx[] = { + { + .hw_channel = HW_CH_BATTERY, + .db_id_val = PARAM_BATTERY_VOLTAGE, + .db_id_cal_offset = ADC_CAL_CH_11_OFFSET, + .db_id_cal_scale = ADC_CAL_CH_11_SCALE, + .active_offset = BAT_DEFAULT_OFFSET, + .active_scale = BAT_DEFAULT_SCALE, + .cal_state = CAL_STATE_IDLE + }, + { + .hw_channel = HW_CH_TEMP, + .db_id_val = PARAM_MCU_TEMPERATURE, + .db_id_cal_offset = ADC_CAL_CH_TEMP_OFFSET, + .db_id_cal_scale = ADC_CAL_CH_TEMP_SCALE, + .active_offset = TEMP_DEFAULT_OFFSET, + .active_scale = TEMP_DEFAULT_SCALE, + .cal_state = CAL_STATE_IDLE + } +}; + +#define CTX_COUNT (sizeof(app_adc_ctx)/sizeof(app_adc_ctx[0])) + +static void ulAdc_GetHardcodedDefaults_Bat(float* defScale, int32_t* defOffset) { + const float Vref = 3.3f; + const float MaxAdc = 4095.0f; + const float DivFactor = 2.0f; // (R1+R2)/R2 + + // Val = Scale * (Raw - Offset) + // Offset = 0 + *defOffset = 0; + // Scale = (Vref / MaxAdc) * DivFactor + *defScale = (Vref / MaxAdc) * DivFactor; +} + +static void ulAdc_GetHardcodedDefaults_Temp(float* defScale, int32_t* defOffset) { + const float Vref = 3.3f; + const float MaxAdc = 4095.0f; + const float V25 = 0.76f; + const float Slope = 0.0025f; + + // T = (Raw * (Vref/Max) - V25) / Slope + 25 + // T = (Raw * Vref/Max)/Slope - V25/Slope + 25 + // T = Raw * (Vref/(Max*Slope)) - (V25/Slope - 25) + // T = Scale * (Raw - Offset_Raw) + + float scale_val = Vref / (MaxAdc * Slope); + float offset_val_phys = (V25 / Slope) - 25.0f; + + // Offset_Raw = offset_val_phys / scale_val + + *defScale = scale_val; + *defOffset = (int32_t)(offset_val_phys / scale_val); +} + +float ulAdc_applyCalibration(uint8_t channel, uint32_t rawValue) { + adc_channel_ctx_t* ctx = NULL; + for(size_t i=0; iactive_offset; + return (float)raw_shifted * ctx->active_scale; +} + +void ulAdc_CallbackHandler(uint8_t channel, uint32_t filteredRaw, void* ctx_void) { + adc_channel_ctx_t* ctx = (adc_channel_ctx_t*)ctx_void; + if (!ctx) { + return; + } + ctx->last_raw = filteredRaw; + + switch (ctx->cal_state) { + + case CAL_STATE_IDLE: + { + uint8_t start_cmd = 0; + + if (ulDatabase_getUint8(ADC_CALIBRATION_START, &start_cmd) == 0 && start_cmd == 0xF1) { + uint8_t target_ch = 0; + ulDatabase_getUint8(ADC_CALIBRATION_CHANNEL_ID, &target_ch); + if (target_ch == ctx->hw_channel) { + ulDatabase_getUint8(ADC_CALIBRATION_POINT, &ctx->pending_point); + ulDatabase_getFloat(ADC_CALIBRATION_POINT_VALUE, &ctx->pending_target); + + ctx->cal_state = CAL_STATE_WAIT_FOR_SAMPLE; + } + } + } + break; + + case CAL_STATE_WAIT_FOR_SAMPLE: + { + bool cal_ok = true; + + if (ctx->pending_point == 0) { + //OFFSET CALIBRATION --- + ctx->active_offset = (int32_t)filteredRaw; + ulDatabase_setInt32(ctx->db_id_cal_offset, ctx->active_offset); + } + else if (ctx->pending_point == 1) { + //SCALE CALIBRATION --- + int32_t raw_shifted = (int32_t)filteredRaw - ctx->active_offset; + + if (abs(raw_shifted) > 10) { + ctx->active_scale = ctx->pending_target / (float)raw_shifted; + ulDatabase_setFloat(ctx->db_id_cal_scale, ctx->active_scale); + } else { + cal_ok = false; + } + } + else { + cal_ok = false; + } + + if (cal_ok) { + ulDatabase_setUint8(ADC_CALIBRATION_START, ADC_CAL_STATUS_OK); + } else { + ulDatabase_setUint8(ADC_CALIBRATION_START, ADC_CAL_STATUS_ERROR); + } + ctx->cal_state = CAL_STATE_IDLE; + } + break; + + default: + ctx->cal_state = CAL_STATE_IDLE; + break; + } + + float phys_val = ulAdc_applyCalibration(channel, filteredRaw); + + ctx->last_phys = phys_val; + ulDatabase_setFloat(ctx->db_id_val, phys_val); +} + +void ulAdc_init(void) { + uint32_t adc_mask = 0; + + for (size_t i = 0; i < CTX_COUNT; i++) { + float def_scale; + int32_t def_offset; + + if (app_adc_ctx[i].hw_channel == HW_CH_BATTERY) { + ulAdc_GetHardcodedDefaults_Bat(&def_scale, &def_offset); + } else { + ulAdc_GetHardcodedDefaults_Temp(&def_scale, &def_offset); + } + + adc_channel_ctx_t* ctx = &app_adc_ctx[i]; + adc_mask |= (1U << ctx->hw_channel); + + int32_t stored_offset; + float stored_scale; + + if (ulDatabase_getInt32(ctx->db_id_cal_offset, &stored_offset) == 0) { + ctx->active_offset = stored_offset; + } else { + ctx->active_offset = def_offset; + } + + if (ulDatabase_getFloat(ctx->db_id_cal_scale, &stored_scale) == 0 && + isfinite(stored_scale) && fabsf(stored_scale) > 1e-9f) { + ctx->active_scale = stored_scale; + } else { + ctx->active_scale = def_scale; + } + } + + drvAdc_init(adc_mask); + + for (size_t i = 0; i < CTX_COUNT; i++) { + drvAdc_registerCallback(app_adc_ctx[i].hw_channel, ulAdc_CallbackHandler, (void*)&app_adc_ctx[i]); + } + + drvAdc_startMeasurement(); +} + +void ulAdc_task(void *argument) { + ulAdc_init(); + + for (;;) { + osDelay(LOG_INTERVAL_MS); + + for (int i = 0; i < CTX_COUNT; i++) { + adc_channel_ctx_t* ctx = &app_adc_ctx[i]; + + ULOG_INFO("[ADC CH%d] Raw: %lu | Phys: %.3f", ctx->hw_channel, ctx->last_raw, ctx->last_phys); + osDelay(20); + } + } +} diff --git a/KPI_Rover/ADC/ulAdc.h b/KPI_Rover/ADC/ulAdc.h new file mode 100644 index 0000000..0a4cba6 --- /dev/null +++ b/KPI_Rover/ADC/ulAdc.h @@ -0,0 +1,11 @@ +#ifndef ADC_MANAGER_H +#define ADC_MANAGER_H + +#include + +float ulAdc_applyCalibration(uint8_t channel, uint32_t rawValue); + +void ulAdc_init(void); +void ulAdc_task(void *argument); + +#endif // ADC_MANAGER_H diff --git a/KPI_Rover/Database/ulDatabase.h b/KPI_Rover/Database/ulDatabase.h index 724e9ec..d1f8971 100644 --- a/KPI_Rover/Database/ulDatabase.h +++ b/KPI_Rover/Database/ulDatabase.h @@ -23,7 +23,17 @@ enum ulDatabase_ParamId { MOTOR_RR_RPM, ENCODER_CONTROL_PERIOD_MS, ENCODER_TICKS_PER_REVOLUTION, - PARAM_COUNT + PARAM_COUNT, + ADC_CALIBRATION_START, + ADC_CALIBRATION_POINT, + ADC_CALIBRATION_POINT_VALUE, + ADC_CAL_CH_11_OFFSET, + ADC_CAL_CH_TEMP_OFFSET, + ADC_CAL_CH_11_SCALE, + ADC_CAL_CH_TEMP_SCALE, + ADC_CALIBRATION_CHANNEL_ID, + PARAM_BATTERY_VOLTAGE, + PARAM_MCU_TEMPERATURE }; struct ulDatabase_ParamMetadata { diff --git a/KPI_Rover/KPIRover.c b/KPI_Rover/KPIRover.c index d04d367..9c951d4 100644 --- a/KPI_Rover/KPIRover.c +++ b/KPI_Rover/KPIRover.c @@ -2,6 +2,7 @@ #include "cmsis_os.h" #include "Database/ulDatabase.h" +#include "ADC/ulAdc.h" static struct ulDatabase_ParamMetadata ulDatabase_params[] = { @@ -10,10 +11,28 @@ static struct ulDatabase_ParamMetadata ulDatabase_params[] = { {0, INT32, false, 0}, // MOTOR_RL_RPM, {0, INT32, false, 0}, // MOTOR_RR_RPM, {0, UINT16, true, 5}, // ENCODER_CONTROL_PERIOD_MS, - {0, FLOAT, true, 820.0f} // ENCODER_TICKS_PER_REVOLUTION, + {0, FLOAT, true, 820.0f}, // ENCODER_TICKS_PER_REVOLUTION, + {0, UINT8, false, 0}, // ADC_CALIBRATION_START, + {0, UINT8, false, 0}, // ADC_CALIBRATION_POINT, + {0, FLOAT, false, 0}, // ADC_CALIBRATION_POINT_VALUE, + {0, INT32, true, 0}, // ADC_CAL_CH_11_OFFSET, + {0, INT32, true, 0}, // ADC_CAL_CH_TEMP_OFFSET, + {0, FLOAT, true, 0.0f}, // ADC_CAL_CH_11_SCALE, + {0, FLOAT, true, 0.0f}, // ADC_CAL_CH_TEMP_SCALE, + {0, UINT8, false, 0}, // ADC_CALIBRATION_CHANNEL_ID + {0, FLOAT, false, 0}, // PARAM_BATTERY_VOLTAGE, + {0, FLOAT, false, 0}, // PARAM_MCU_TEMPERATURE }; void KPIRover_Init(void) { ulDatabase_init(ulDatabase_params, sizeof(ulDatabase_params) / sizeof(struct ulDatabase_ParamMetadata)); ulEncoder_Init(); + + // ADC + const osThreadAttr_t adcTask_attributes = { + .name = "adcTask", + .priority = (osPriority_t) osPriorityNormal, + .stack_size = 256 * 4 + }; + (void) osThreadNew(ulAdc_task, NULL, &adcTask_attributes); } diff --git a/ecu_sw.ioc b/ecu_sw.ioc index 99771d1..c3a47b8 100644 --- a/ecu_sw.ioc +++ b/ecu_sw.ioc @@ -120,6 +120,7 @@ Mcu.UserName=STM32F407VGTx MxCube.Version=6.16.0 MxDb.Version=DB.6.0.160 NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:false\:false +NVIC.DMA2_Stream0_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:false\:false NVIC.ForceEnableDMAVector=true NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:false\:false