1083 lines
36 KiB
C
1083 lines
36 KiB
C
/*******************************************************************************
|
|
* @file full_agc.c
|
|
* @brief Automatic Gain Control (AGC) for NIRS LED/PD measurement system
|
|
* @author Charles KWON <charleskwon@medithings.co.kr>
|
|
* @version V2.0.0
|
|
* @date 2025-12-31
|
|
*
|
|
* Copyright (c) 2025 Medithings Inc.
|
|
* All rights reserved.
|
|
*
|
|
* This module implements automatic gain calibration for the NIRS system.
|
|
* It measures photodetector response for each LED and calculates optimal
|
|
* DAC values for MCP4725 to normalize signal amplitudes across all channels.
|
|
*
|
|
* AGC Process Overview:
|
|
* 1. Sequentially activate each LED-PD pair
|
|
* 2. Sample ADC with gain switch toggling (high/low mode)
|
|
* 3. Calculate voltage difference and derive DAC calibration value
|
|
* 4. Store results in EEPROM for persistent calibration
|
|
*
|
|
* Hardware Dependencies:
|
|
* - MCP4725 DAC for gain control
|
|
* - SAADC for photodetector voltage measurement
|
|
* - FSA5157P6X analog switch for signal routing
|
|
******************************************************************************/
|
|
|
|
#include "sdk_common.h"
|
|
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <math.h>
|
|
#include "nrf.h"
|
|
#include "boards.h"
|
|
#include "app_error.h"
|
|
#include "nrf_drv_saadc.h"
|
|
#include "nrfx_gpiote.h"
|
|
#include "nrf_drv_ppi.h"
|
|
#include "app_timer.h"
|
|
#include "nrf_drv_timer.h"
|
|
#include "nrf_delay.h"
|
|
#include "measurements.h"
|
|
#include "main_timer.h"
|
|
#include "battery_saadc.h"
|
|
#include "main.h"
|
|
#include "full_agc.h"
|
|
#include "mcp4725_i2c.h"
|
|
//#include "ad5272_i2c.h"
|
|
#include "ble_nus.h"
|
|
#include "cat_interface.h"
|
|
#include <cmd_parse.h>
|
|
#include "debug_print.h"
|
|
#include "i2c_manager.h"
|
|
#include "storage/dr_mem.h"
|
|
|
|
/*============================================================================*/
|
|
/* ADC Configuration Constants - Charles KWON */
|
|
/*============================================================================*/
|
|
|
|
#if FEATURE_AGC_FLOAT
|
|
/** @brief Reference voltage in millivolts for ADC conversion (float mode) */
|
|
#define FULL_AGC_REF_VOLTAGE_IN_MILLIVOLTS 600.0f
|
|
|
|
/** @brief ADC prescaling compensation factor (VDD/3 input scaling) */
|
|
#define FULL_AGC_PRE_SCALING_COMPENSATION 6.0f
|
|
|
|
/** @brief Maximum digital value for 10-bit ADC resolution */
|
|
#define FULL_AGC_ADC_RES_10BITS 1024.0f
|
|
#else
|
|
/** @brief Reference voltage in millivolts for ADC conversion (integer mode) */
|
|
#define FULL_AGC_REF_VOLTAGE_IN_MILLIVOLTS 600
|
|
|
|
/** @brief ADC prescaling compensation factor (VDD/3 input scaling) */
|
|
#define FULL_AGC_PRE_SCALING_COMPENSATION 6
|
|
|
|
/** @brief Maximum digital value for 10-bit ADC resolution */
|
|
#define FULL_AGC_ADC_RES_10BITS 1024
|
|
#endif
|
|
|
|
/** @brief Total number of LEDs in the system */
|
|
#define LED_NUM 24
|
|
|
|
/**
|
|
* @brief Convert ADC value to millivolts
|
|
* - Charles KWON
|
|
*
|
|
* @param ADC_VALUE Raw ADC reading
|
|
* @return Voltage in millivolts
|
|
*/
|
|
#define FULL_AGC_VOUT_IN_MILLI_VOLTS(ADC_VALUE) \
|
|
((((ADC_VALUE) * FULL_AGC_REF_VOLTAGE_IN_MILLIVOLTS) / FULL_AGC_ADC_RES_10BITS) * FULL_AGC_PRE_SCALING_COMPENSATION)
|
|
|
|
/*============================================================================*/
|
|
/* DAC Range Limits - Charles KWON */
|
|
/*============================================================================*/
|
|
|
|
/** @brief Minimum DAC value for MCP4725 (prevents under-amplification) */
|
|
#define DAC_MIN 125
|
|
|
|
/** @brief Maximum DAC value for MCP4725 (prevents saturation) */
|
|
#define DAC_MAX 1365
|
|
|
|
/*============================================================================*/
|
|
/* External Variables - Charles KWON */
|
|
/*============================================================================*/
|
|
|
|
/** @brief DAC values for each LED-PD pair stored in EEPROM */
|
|
extern uint16_t led_pd_dac_v[LED_NUM];
|
|
|
|
/** @brief Flag indicating command processing in progress */
|
|
extern volatile bool processing;
|
|
|
|
/*============================================================================*/
|
|
/* LED/PD Configuration Arrays - Charles KWON */
|
|
/*============================================================================*/
|
|
|
|
/** @brief Working list of LEDs for current AGC scan */
|
|
uint8_t full_agc_LED_list[48];
|
|
|
|
/** @brief Working list of PDs for current AGC scan */
|
|
uint8_t full_agc_PD_list[1];
|
|
|
|
/**
|
|
* @brief LED indices for AGC Part A (LEDs 0-23, PD0)
|
|
* - Charles KWON
|
|
*/
|
|
uint8_t full_agc_LED_a[24] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23};
|
|
|
|
/** @brief PD index for AGC Part A */
|
|
uint8_t full_agc_PD_a[1] = {0};
|
|
|
|
/**
|
|
* @brief LED indices for AGC Part B (LEDs 24-47, PD1)
|
|
* - Charles KWON
|
|
*/
|
|
const uint8_t full_agc_LED_b[24] = {24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47};
|
|
|
|
/** @brief PD index for AGC Part B */
|
|
uint8_t full_agc_PD_b[1] = {1};
|
|
|
|
/*============================================================================*/
|
|
/* State Machine Variables - Charles KWON */
|
|
/*============================================================================*/
|
|
|
|
/** @brief Number of LEDs to process in current scan */
|
|
static int8_t LED_NO = 24;
|
|
|
|
/** @brief Current PD index (-1 = not started) */
|
|
static int8_t pd_no = -1;
|
|
|
|
/** @brief Current LED index (-1 = not started) */
|
|
static int8_t led_no = -1;
|
|
|
|
/*============================================================================*/
|
|
/* ADC Buffer Configuration - Charles KWON */
|
|
/*============================================================================*/
|
|
|
|
/** @brief Number of samples per ADC conversion (averaging) */
|
|
#define SAMPLES_IN_BUFFER 4
|
|
|
|
/** @brief Double-buffered ADC sample storage */
|
|
static nrf_saadc_value_t full_agc_buf[2][SAMPLES_IN_BUFFER];
|
|
|
|
/*============================================================================*/
|
|
/* AGC State Flags - Charles KWON */
|
|
/*============================================================================*/
|
|
|
|
/** @brief AGC Part A (PD0, LEDs 0-23) in progress */
|
|
bool full_agc_a_start = false;
|
|
|
|
/** @brief AGC Part B (PD1, LEDs 24-47) in progress */
|
|
bool full_agc_b_start = false;
|
|
|
|
/** @brief AGC Part C (reserved for future use) in progress */
|
|
bool full_agc_c_start = false;
|
|
|
|
/** @brief Full AGC sequence completed successfully */
|
|
bool full_agc_completed = false;
|
|
|
|
/*============================================================================*/
|
|
/* Timer Definitions - Charles KWON */
|
|
/*============================================================================*/
|
|
|
|
/** @brief Timer for AGC measurement loop */
|
|
APP_TIMER_DEF(m_full_agc_loop_timer_id);
|
|
|
|
/** @brief AGC loop interval in milliseconds */
|
|
#define FULL_AGC_LOOP_INTERVAL 2
|
|
|
|
/** @brief Timer for AGC data transmission */
|
|
APP_TIMER_DEF(m_full_agc_send_timer_id);
|
|
|
|
/** @brief AGC send interval (feature-dependent) */
|
|
#if FEATURE_DELAY
|
|
#define FULL_AGC_SEND_INTERVAL 500
|
|
#else
|
|
#define FULL_AGC_SEND_INTERVAL 100
|
|
#endif
|
|
|
|
/*============================================================================*/
|
|
/* Measurement Variables - Charles KWON */
|
|
/*============================================================================*/
|
|
|
|
#if FEATURE_AGC_FLOAT
|
|
/** @brief High-mode ADC reading in millivolts */
|
|
static double agc_read_h = 0;
|
|
|
|
/** @brief Low-mode ADC reading in millivolts */
|
|
static double agc_read_l = 0;
|
|
|
|
/** @brief Reference voltage difference */
|
|
static double agc_read_ref = 0;
|
|
|
|
/** @brief Current ADC reading in millivolts */
|
|
static double m_full_agc_in_milli_volts = 0.0f;
|
|
#else
|
|
/** @brief High-mode ADC reading in millivolts */
|
|
static uint16_t agc_read_h = 0;
|
|
|
|
/** @brief Low-mode ADC reading in millivolts */
|
|
static uint16_t agc_read_l = 0;
|
|
|
|
/** @brief Reference voltage difference */
|
|
static uint16_t agc_read_ref = 0;
|
|
|
|
/** @brief Current ADC reading in millivolts */
|
|
static uint16_t m_full_agc_in_milli_volts = 0;
|
|
#endif
|
|
|
|
/** @brief Sample skip counter (skip first few unstable samples) */
|
|
static uint16_t cnt_js = 0;
|
|
|
|
/** @brief Current gain mode (true=high, false=low) */
|
|
static bool agc_mode = false;
|
|
|
|
/** @brief Buffer for voltage difference analysis */
|
|
static uint16_t array_buff[48] = {0,};
|
|
|
|
/** @brief Current index in array_buff */
|
|
static uint16_t idx = 0;
|
|
|
|
/** @brief Order counter for BLE transmission sequence */
|
|
static uint8_t order_cnt = 0;
|
|
|
|
/*============================================================================*/
|
|
/* AGC Result Storage Arrays - Charles KWON */
|
|
/*============================================================================*/
|
|
|
|
/**
|
|
* @brief AGC read values for all LED-PD pairs
|
|
* - Charles KWON
|
|
*
|
|
* Stores calculated DAC values after AGC calibration
|
|
*/
|
|
uint16_t agc_read_v[LED_NUM] = {1000, 1000, 1000, 1000, 1000, 1000,
|
|
1000, 1000, 1000, 1000, 1000, 1000,
|
|
1000, 1000, 1000, 1000, 1000, 1000,
|
|
1000, 1000, 1000, 1000, 1000, 1000};
|
|
|
|
/** @brief AGC Part A, PD0 results */
|
|
uint16_t agc_A_0[LED_NUM] = {1000, 1000, 1000, 1000, 1000, 1000,
|
|
1000, 1000, 1000, 1000, 1000, 1000,
|
|
1000, 1000, 1000, 1000, 1000, 1000,
|
|
1000, 1000, 1000, 1000, 1000, 1000};
|
|
|
|
/** @brief AGC Part A, PD1 results */
|
|
uint16_t agc_A_1[LED_NUM] = {1000, 1000, 1000, 1000, 1000, 1000,
|
|
1000, 1000, 1000, 1000, 1000, 1000,
|
|
1000, 1000, 1000, 1000, 1000, 1000,
|
|
1000, 1000, 1000, 1000, 1000, 1000};
|
|
|
|
/** @brief AGC Part A, MOD1 results */
|
|
uint16_t agc_A_2[LED_NUM] = {1000, 1000, 1000, 1000, 1000, 1000,
|
|
1000, 1000, 1000, 1000, 1000, 1000,
|
|
1000, 1000, 1000, 1000, 1000, 1000,
|
|
1000, 1000, 1000, 1000, 1000, 1000};
|
|
|
|
/** @brief AGC Part B, PD0 results */
|
|
uint16_t agc_B_0[LED_NUM] = {1000, 1000, 1000, 1000, 1000, 1000,
|
|
1000, 1000, 1000, 1000, 1000, 1000,
|
|
1000, 1000, 1000, 1000, 1000, 1000,
|
|
1000, 1000, 1000, 1000, 1000, 1000};
|
|
|
|
/** @brief AGC Part B, PD1 results */
|
|
uint16_t agc_B_1[LED_NUM] = {1000, 1000, 1000, 1000, 1000, 1000,
|
|
1000, 1000, 1000, 1000, 1000, 1000,
|
|
1000, 1000, 1000, 1000, 1000, 1000,
|
|
1000, 1000, 1000, 1000, 1000, 1000};
|
|
|
|
/** @brief AGC Part B, MOD1 results */
|
|
uint16_t agc_B_2[LED_NUM] = {1000, 1000, 1000, 1000, 1000, 1000,
|
|
1000, 1000, 1000, 1000, 1000, 1000,
|
|
1000, 1000, 1000, 1000, 1000, 1000,
|
|
1000, 1000, 1000, 1000, 1000, 1000};
|
|
|
|
/*============================================================================*/
|
|
/* Communication Buffers - Charles KWON */
|
|
/*============================================================================*/
|
|
|
|
/** @brief Command source type */
|
|
extern which_cmd_t cmd_type_t;
|
|
|
|
/** @brief AGC ASCII transmission buffer */
|
|
char agc_tx_buffer[BLE_NUS_MAX_DATA_LEN];
|
|
|
|
/** @brief AGC response buffer */
|
|
char agc_tx_buffer_r[BLE_NUS_MAX_DATA_LEN];
|
|
|
|
/** @brief Flag to control BLE send sequence order */
|
|
bool agc_ble_send_order = false;
|
|
|
|
/** @brief BLE connection state */
|
|
extern volatile bool ble_connection_st;
|
|
|
|
/** @brief AGC binary transmission buffer */
|
|
uint8_t agc_bin_buffer[BLE_NUS_MAX_DATA_LEN];
|
|
|
|
/*============================================================================*/
|
|
/* ADC Interrupt Handler - Charles KWON */
|
|
/*============================================================================*/
|
|
|
|
/**
|
|
* @brief SAADC event handler for AGC voltage measurement
|
|
* - Charles KWON
|
|
*
|
|
* Processes ADC conversion results from photodetector.
|
|
* Averages multiple samples for noise reduction and converts
|
|
* to millivolts for DAC calculation.
|
|
*
|
|
* @param[in] p_event Pointer to SAADC event structure
|
|
*/
|
|
static void full_agc_voltage_handler(nrf_drv_saadc_evt_t const * p_event)
|
|
{
|
|
uint16_t min_data = 0;
|
|
|
|
if (p_event->type == NRF_DRV_SAADC_EVT_DONE)
|
|
{
|
|
uint32_t err_code;
|
|
|
|
err_code = nrf_drv_saadc_buffer_convert(p_event->data.done.p_buffer, SAMPLES_IN_BUFFER);
|
|
APP_ERROR_CHECK(err_code);
|
|
|
|
/* Sum all samples for averaging */
|
|
for (uint8_t i = 0; i < SAMPLES_IN_BUFFER; i++) {
|
|
min_data += p_event->data.done.p_buffer[i];
|
|
}
|
|
|
|
/* Convert averaged ADC value to millivolts */
|
|
m_full_agc_in_milli_volts = (uint16_t)(FULL_AGC_VOUT_IN_MILLI_VOLTS(min_data) * 0.25);
|
|
}
|
|
}
|
|
|
|
/*============================================================================*/
|
|
/* AGC State Machine - Charles KWON */
|
|
/*============================================================================*/
|
|
|
|
/**
|
|
* @brief Main AGC measurement state machine
|
|
* - Charles KWON
|
|
*
|
|
* Implements the core AGC algorithm:
|
|
* 1. Activate LED-PD pair
|
|
* 2. Toggle gain switch and sample ADC in both modes
|
|
* 3. Calculate voltage difference
|
|
* 4. Derive DAC value: Dn = (Vout/Vref) x 4096
|
|
* 5. Store result and advance to next LED-PD pair
|
|
*
|
|
* Uses delay_ticks_counter for non-blocking LED settling time.
|
|
*/
|
|
static void full_agc_measurement(void)
|
|
{
|
|
static bool mode_behind = false;
|
|
static bool sampling_behind = false;
|
|
static bool agc_processing_behind = false;
|
|
|
|
static double Vref = 3.3f; /* Reference voltage = Vdd */
|
|
static double dac_value_h = 0.0f;
|
|
static double dac_value_l = 0.0f;
|
|
#if FEATURE_DETAIL_VALUE_AGC
|
|
static double avg_num = 0.0f;
|
|
#endif
|
|
|
|
static uint16_t dac_reg_h = 0;
|
|
static uint16_t dac_reg_l = 0;
|
|
static uint16_t max_num = 0, zero_count = 0, sum_num = 0;
|
|
static uint16_t diff_volt = 0;
|
|
|
|
/* Non-blocking delay counter for LED settling */
|
|
static int delay_ticks_counter = 0;
|
|
|
|
/* Skip processing during delay period */
|
|
if (delay_ticks_counter > 0)
|
|
{
|
|
delay_ticks_counter--;
|
|
return;
|
|
}
|
|
|
|
/*------------------------------------------------------------------------*/
|
|
/* Initial LED/PD activation */
|
|
/*------------------------------------------------------------------------*/
|
|
if (led_no == -1) {
|
|
led_no = 0;
|
|
pd_no = 0;
|
|
|
|
led_on(full_agc_LED_list[led_no]);
|
|
pd_on(full_agc_PD_list[pd_no]);
|
|
|
|
DBG_PRINTF("LED-1\r\n");
|
|
delay_ticks_counter = 100;
|
|
DBG_PRINTF("Finished part A. Pausing for ~108ms before part B...\r\n");
|
|
|
|
} else if ((led_no >= 0) && (led_no <= 47)) {
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
/* Step 1: Toggle gain mode */
|
|
/*--------------------------------------------------------------------*/
|
|
if (mode_behind == false) {
|
|
mode_behind = true;
|
|
|
|
if (agc_mode == false) {
|
|
agc_mode = true;
|
|
} else if (agc_mode == true) {
|
|
agc_mode = false;
|
|
}
|
|
|
|
#if FEATURE_DETAIL_VALUE_AGC
|
|
DBG_PRINTF("agc_mode [%d]\r\n", agc_mode);
|
|
#endif
|
|
AGC_GAIN_SW(agc_mode);
|
|
|
|
return;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
/* Step 2: Trigger ADC sampling */
|
|
/*--------------------------------------------------------------------*/
|
|
if (sampling_behind == false) {
|
|
sampling_behind = true;
|
|
|
|
ret_code_t err_code = nrf_drv_saadc_sample();
|
|
cnt_js++;
|
|
APP_ERROR_CHECK(err_code);
|
|
|
|
return;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
/* Step 3: Process ADC results and calculate DAC value */
|
|
/*--------------------------------------------------------------------*/
|
|
if (agc_processing_behind == false) {
|
|
if (agc_mode == true) {
|
|
/* High-mode measurement */
|
|
if (m_full_agc_in_milli_volts == 0) {
|
|
#if FEATURE_DETAIL_VALUE_AGC
|
|
DBG_PRINTF("<1>ReSampling +++++ mode = %d, led_no = %d(%d), pd_no = %d(%d)\r\n",
|
|
agc_mode, led_no, full_agc_LED_list[led_no], pd_no, full_agc_PD_list[pd_no]);
|
|
#endif
|
|
agc_mode = false;
|
|
mode_behind = false;
|
|
sampling_behind = false;
|
|
return;
|
|
}
|
|
|
|
agc_read_h = m_full_agc_in_milli_volts;
|
|
m_full_agc_in_milli_volts = 0.0f;
|
|
|
|
/* Calculate DAC value: Dn = (Vout/Vref) x 4096 */
|
|
dac_value_h = (((agc_read_h / Vref) * 4096.0f) / 1000.0f);
|
|
dac_reg_h = (uint16_t)(dac_value_h + 0.5f); /* Round to nearest integer */
|
|
|
|
#if FEATURE_FOR_SCOPE
|
|
nrf_gpio_pin_set(ADC_CLK_18);
|
|
nrf_gpio_pin_clear(ADC_CLK_18);
|
|
nrf_gpio_pin_set(ADC_CLK_18);
|
|
nrf_gpio_pin_clear(ADC_CLK_18);
|
|
#endif
|
|
|
|
led_pd_dac_v[full_agc_LED_list[led_no]] = dac_reg_h;
|
|
|
|
#if FEATURE_DETAIL_VALUE_AGC
|
|
#if FEATURE_AGC_FLOAT
|
|
DBG_PRINTF("mode = %d, led_no = %d(%d), pd_no = %d(%d), \t%lf(mV)->DAC %d\r\n",
|
|
agc_mode, led_no, full_agc_LED_list[led_no], pd_no, full_agc_PD_list[pd_no],
|
|
agc_read_h, dac_reg_h);
|
|
#else
|
|
DBG_PRINTF("mode = %d, led_no = %d(%d), pd_no = %d(%d), \t%d(mV)->DAC %d\r\n",
|
|
agc_mode, led_no, full_agc_LED_list[led_no], pd_no, full_agc_PD_list[pd_no],
|
|
agc_read_h, dac_reg_h);
|
|
#endif
|
|
#endif
|
|
|
|
sw_i2c_init_once();
|
|
pd_gain_set(full_agc_LED_list[led_no]);
|
|
|
|
mode_behind = false;
|
|
sampling_behind = false;
|
|
|
|
} else if (agc_mode == false) {
|
|
/* Low-mode measurement */
|
|
if (m_full_agc_in_milli_volts == 0) {
|
|
#if FEATURE_DETAIL_VALUE_AGC
|
|
DBG_PRINTF("<2>ReSampling +++++ mode = %d, led_no = %d(%d), pd_no = %d(%d)\r\n",
|
|
agc_mode, led_no, full_agc_LED_list[led_no], pd_no, full_agc_PD_list[pd_no]);
|
|
#endif
|
|
agc_mode = true;
|
|
mode_behind = false;
|
|
sampling_behind = false;
|
|
return;
|
|
}
|
|
|
|
agc_read_l = m_full_agc_in_milli_volts;
|
|
m_full_agc_in_milli_volts = 0.0f;
|
|
|
|
/* Calculate DAC value for low mode */
|
|
dac_value_l = (((agc_read_l / Vref) * 4096.0f) / 1000.0f);
|
|
dac_reg_l = (uint16_t)(dac_value_l + 0.5f);
|
|
|
|
#if FEATURE_FOR_SCOPE
|
|
nrf_gpio_pin_set(ADC_CLK_18);
|
|
nrf_gpio_pin_clear(ADC_CLK_18);
|
|
#endif
|
|
|
|
agc_read_v[full_agc_LED_list[led_no]] = dac_reg_l;
|
|
|
|
/* Calculate voltage difference between high and low modes */
|
|
agc_read_ref = fabsf(agc_read_h - agc_read_l);
|
|
|
|
/* Store difference for analysis */
|
|
diff_volt = (uint16_t)(agc_read_ref + 0.5f);
|
|
array_buff[idx] = diff_volt;
|
|
idx++;
|
|
|
|
#if FEATURE_DETAIL_VALUE_AGC
|
|
#if FEATURE_AGC_FLOAT
|
|
DBG_PRINTF("mode = %d, led_no = %d(%d), pd_no = %d(%d), \t%f(mV), \t\t\tDiff = %d\r\n\r\n",
|
|
agc_mode, led_no, full_agc_LED_list[led_no], pd_no, full_agc_PD_list[pd_no],
|
|
agc_read_l, diff_volt);
|
|
#else
|
|
DBG_PRINTF("mode = %d, led_no = %d(%d), pd_no = %d(%d), \t%d(mV), \t\t\tDiff = %d\r\n\r\n",
|
|
agc_mode, led_no, full_agc_LED_list[led_no], pd_no, full_agc_PD_list[pd_no],
|
|
agc_read_l, diff_volt);
|
|
#endif
|
|
#endif
|
|
agc_processing_behind = true;
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
/* Step 4: Advance to next LED-PD pair */
|
|
/*--------------------------------------------------------------------*/
|
|
mode_behind = false;
|
|
sampling_behind = false;
|
|
agc_processing_behind = false;
|
|
|
|
agc_mode = false;
|
|
dac_value_h = 0.0f;
|
|
dac_reg_h = 0;
|
|
diff_volt = 0;
|
|
|
|
if (pd_no < PD_NO - 1) {
|
|
/* Next PD for same LED */
|
|
pd_no++;
|
|
pd_on(full_agc_PD_list[pd_no]);
|
|
}
|
|
else if (pd_no >= PD_NO - 1) {
|
|
pd_no = 0;
|
|
|
|
if (led_no < LED_NO - 1) {
|
|
/* Next LED */
|
|
led_no++;
|
|
led_on(full_agc_LED_list[led_no]);
|
|
pd_on(full_agc_PD_list[pd_no]);
|
|
|
|
/* Add delay at LED package boundaries */
|
|
if (full_agc_LED_list[led_no] % 6 == 0) {
|
|
delay_ticks_counter = 5;
|
|
DBG_PRINTF("LED_PACKEAGE\r\n");
|
|
}
|
|
DBG_PRINTF("LED+1\r\n");
|
|
} else if (led_no >= LED_NO - 1) {
|
|
/*------------------------------------------------------------*/
|
|
/* All LEDs processed - transition to next phase */
|
|
/*------------------------------------------------------------*/
|
|
pd_no = -1;
|
|
led_no = -1;
|
|
|
|
#if FEATURE_DETAIL_VALUE_AGC
|
|
DBG_PRINTF("\r\nLED-PD Gain Array =\r\n");
|
|
for (uint16_t i = 0; i < PD_NUM; i++) {
|
|
for (uint16_t j = 0; j < LED_NUM; j++) {
|
|
DBG_PRINTF("%d,\t", led_pd_dac_v[i][j]);
|
|
}
|
|
DBG_PRINTF("\r\n");
|
|
}
|
|
DBG_PRINTF("\r\n");
|
|
#endif
|
|
if (ble_connection_st == 0) {
|
|
full_agc_send_timer_stop();
|
|
full_agc_end();
|
|
DBG_PRINTF("Full AGC STOP 1\r\n");
|
|
}
|
|
else if (full_agc_a_start == true) {
|
|
/* Part A complete, start Part B */
|
|
full_agc_a_start = false;
|
|
full_agc_b_start = true;
|
|
full_agc_start();
|
|
} else if (full_agc_b_start == true) {
|
|
/* Part B complete, finalize AGC */
|
|
full_agc_b_start = false;
|
|
|
|
full_agc_completed = true;
|
|
full_agc_end();
|
|
|
|
#if FEATURE_DETAIL_VALUE_AGC
|
|
DBG_PRINTF("Full AGC, Completed!\r\n");
|
|
#endif
|
|
|
|
/* Analyze voltage difference statistics */
|
|
for (uint16_t i = 0; i < idx; i++) {
|
|
#if FEATURE_DETAIL_VALUE_AGC
|
|
if (i % 25 == 0) DBG_PRINTF("\r\n");
|
|
DBG_PRINTF("%d, \t", array_buff[i]);
|
|
#endif
|
|
sum_num += array_buff[i];
|
|
if (array_buff[i] == 0) zero_count++;
|
|
if (max_num < array_buff[i]) {
|
|
max_num = array_buff[i];
|
|
}
|
|
}
|
|
|
|
#if FEATURE_DETAIL_VALUE_AGC
|
|
avg_num = (double)sum_num / idx;
|
|
DBG_PRINTF("\r\n\r\n");
|
|
DBG_PRINTF("Max_number = %d\r\n", max_num);
|
|
DBG_PRINTF("Avg_number = %.2f\r\n", avg_num);
|
|
DBG_PRINTF("Zero_count = %d\r\n", zero_count);
|
|
DBG_PRINTF("Zero_Percentage = %.2f%%\r\n", (double)zero_count / idx * 100);
|
|
DBG_PRINTF("\r\n");
|
|
#endif
|
|
|
|
/* Clear analysis buffer */
|
|
for (uint16_t i = 0; i < 48; i++) {
|
|
array_buff[i] = 0;
|
|
}
|
|
#if FEATURE_DETAIL_VALUE_AGC
|
|
avg_num = 0.0f;
|
|
#endif
|
|
max_num = 0;
|
|
zero_count = 0;
|
|
sum_num = 0;
|
|
idx = 0;
|
|
|
|
#if FEATURE_DETAIL_VALUE_AGC
|
|
DBG_PRINTF("agc_read_v = \r\n");
|
|
for (uint16_t i = 0; i < PD_NUM; i++) {
|
|
for (uint16_t j = 0; j < LED_NUM; j++) {
|
|
DBG_PRINTF("%d, \t", agc_read_v[i][j]);
|
|
}
|
|
DBG_PRINTF("\r\n");
|
|
}
|
|
DBG_PRINTF("\r\n");
|
|
#endif
|
|
|
|
/*--------------------------------------------------------*/
|
|
/* Send AGC results via UART or BLE */
|
|
/*--------------------------------------------------------*/
|
|
if (cmd_type_t == CMD_UART) {
|
|
DBG_PRINTF("Tagc, ");
|
|
for (uint16_t j = 0; j < LED_NUM; j++) {
|
|
DBG_PRINTF("%d, ", led_pd_dac_v[j]);
|
|
}
|
|
DBG_PRINTF("\r\n");
|
|
} else if (cmd_type_t == CMD_BLE) {
|
|
battery_timer_stop();
|
|
|
|
/* Copy results to transmission arrays */
|
|
for (uint16_t j = 0; j < LED_NO; j++) {
|
|
agc_A_0[j] = led_pd_dac_v[j];
|
|
}
|
|
for (uint16_t j = 0; j < LED_NO; j++) {
|
|
agc_A_1[j] = led_pd_dac_v[j + LED_NO];
|
|
}
|
|
|
|
/* Format and send binary data */
|
|
format_data(agc_bin_buffer, "rag:", led_pd_dac_v, 96);
|
|
binary_tx_handler(agc_bin_buffer, 50);
|
|
|
|
for (uint16_t i = 0; i < 50; i++) {
|
|
DBG_PRINTF("%02X ", agc_bin_buffer[i]);
|
|
}
|
|
DBG_PRINTF("\r\n");
|
|
|
|
DBG_PRINTF("Tagc,");
|
|
for (uint16_t j = 0; j < LED_NUM; j++) {
|
|
DBG_PRINTF("%d, ", led_pd_dac_v[j]);
|
|
}
|
|
|
|
/* Save AGC results to W25Q32 */
|
|
dr_memWrite("agc_gain", led_pd_dac_v, 96);
|
|
nrf_delay_ms(10);
|
|
DBG_PRINTF("AGC_Gain Write!\r\n");
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/*============================================================================*/
|
|
/* AGC Control Functions - Charles KWON */
|
|
/*============================================================================*/
|
|
|
|
/**
|
|
* @brief Initialize and start AGC sequence
|
|
* - Charles KWON
|
|
*
|
|
* Configures LED and PD lists based on current phase (A or B).
|
|
* Part A: LEDs 0-23 with PD0
|
|
* Part B: LEDs 24-47 with PD1
|
|
*/
|
|
void full_agc_start(void)
|
|
{
|
|
cnt_js = 0;
|
|
|
|
if (full_agc_a_start == true) {
|
|
#if FEATURE_DETAIL_VALUE_AGC
|
|
DBG_PRINTF("\r\n===== Full_AGC_A start ====================\r\n");
|
|
DBG_PRINTF("LED : ");
|
|
#endif
|
|
|
|
full_agc_PD_list[0] = full_agc_PD_a[0];
|
|
|
|
for (uint8_t i = 0; i < LED_NO; i++) {
|
|
full_agc_LED_list[i] = full_agc_LED_a[i];
|
|
}
|
|
|
|
#if FEATURE_DETAIL_VALUE_AGC
|
|
DBG_PRINTF("\r\n");
|
|
DBG_PRINTF("PD : ");
|
|
DBG_PRINTF("%d ", full_agc_PD_list[i]);
|
|
DBG_PRINTF("\r\n\r\n");
|
|
#endif
|
|
}
|
|
else if (full_agc_b_start == true) {
|
|
#if FEATURE_DETAIL_VALUE_AGC
|
|
DBG_PRINTF("\r\n===== Full_AGC_B start ====================\r\n");
|
|
DBG_PRINTF("LED : ");
|
|
#endif
|
|
|
|
for (uint8_t i = 0; i < LED_NO; i++) {
|
|
full_agc_LED_list[i] = full_agc_LED_b[i];
|
|
#if FEATURE_DETAIL_VALUE_AGC
|
|
DBG_PRINTF("%d ", full_agc_LED_list[i]);
|
|
#endif
|
|
}
|
|
|
|
#if FEATURE_DETAIL_VALUE_AGC
|
|
DBG_PRINTF("\r\n");
|
|
DBG_PRINTF("PD : ");
|
|
#endif
|
|
|
|
full_agc_PD_list[0] = full_agc_PD_b[0];
|
|
|
|
#if FEATURE_DETAIL_VALUE_AGC
|
|
DBG_PRINTF("\r\n\r\n");
|
|
#endif
|
|
}
|
|
|
|
pd_no = -1;
|
|
led_no = -1;
|
|
}
|
|
|
|
/**
|
|
* @brief Terminate AGC sequence and cleanup
|
|
* - Charles KWON
|
|
*
|
|
* Stops timers, uninitializes ADC, restores I2C mode,
|
|
* turns off all LEDs/PDs, and clears processing flag.
|
|
*/
|
|
void full_agc_end(void)
|
|
{
|
|
hw_i2c_init_once();
|
|
full_agc_timer_stop();
|
|
full_agc_uninit();
|
|
|
|
battery_timer_start();
|
|
|
|
led_off(99); /* Turn off all LEDs */
|
|
pd_off(99); /* Turn off all PDs */
|
|
|
|
DBG_PRINTF("end_process\r\n\r\n");
|
|
processing = false;
|
|
}
|
|
|
|
/*============================================================================*/
|
|
/* SAADC Configuration - Charles KWON */
|
|
/*============================================================================*/
|
|
|
|
/**
|
|
* @brief Initialize SAADC for AGC voltage measurement
|
|
* - Charles KWON
|
|
*
|
|
* Configures single-ended ADC on AIN6 (FSA5157P6X output)
|
|
* with double-buffering for continuous sampling.
|
|
*/
|
|
void full_agc_adc_init(void)
|
|
{
|
|
ret_code_t err_code = nrf_drv_saadc_init(NULL, full_agc_voltage_handler);
|
|
APP_ERROR_CHECK(err_code);
|
|
|
|
nrf_saadc_channel_config_t config =
|
|
NRF_DRV_SAADC_DEFAULT_CHANNEL_CONFIG_SE(NRF_SAADC_INPUT_AIN6);
|
|
|
|
err_code = nrf_drv_saadc_channel_init(0, &config);
|
|
APP_ERROR_CHECK(err_code);
|
|
|
|
err_code = nrf_drv_saadc_buffer_convert(full_agc_buf[0], SAMPLES_IN_BUFFER);
|
|
APP_ERROR_CHECK(err_code);
|
|
|
|
err_code = nrf_drv_saadc_buffer_convert(full_agc_buf[1], SAMPLES_IN_BUFFER);
|
|
APP_ERROR_CHECK(err_code);
|
|
}
|
|
|
|
/**
|
|
* @brief Uninitialize SAADC
|
|
* - Charles KWON
|
|
*/
|
|
void full_agc_uninit(void)
|
|
{
|
|
nrf_drv_saadc_uninit();
|
|
nrf_drv_saadc_channel_uninit(0);
|
|
}
|
|
|
|
/*============================================================================*/
|
|
/* Timer Callbacks - Charles KWON */
|
|
/*============================================================================*/
|
|
|
|
/**
|
|
* @brief AGC loop timer callback
|
|
* - Charles KWON
|
|
*
|
|
* Called periodically to drive the AGC state machine.
|
|
*
|
|
* @param[in] p_context Unused timer context
|
|
*/
|
|
void full_agc_loop(void * p_context)
|
|
{
|
|
UNUSED_PARAMETER(p_context);
|
|
full_agc_measurement();
|
|
}
|
|
|
|
/*============================================================================*/
|
|
/* AGC Loop Timer Control - Charles KWON */
|
|
/*============================================================================*/
|
|
|
|
/**
|
|
* @brief Start AGC measurement loop timer
|
|
* - Charles KWON
|
|
*/
|
|
void full_agc_timer_start(void)
|
|
{
|
|
APP_ERROR_CHECK(app_timer_start(m_full_agc_loop_timer_id,
|
|
APP_TIMER_TICKS(FULL_AGC_LOOP_INTERVAL), NULL));
|
|
}
|
|
|
|
/**
|
|
* @brief Stop AGC measurement loop timer
|
|
* - Charles KWON
|
|
*/
|
|
void full_agc_timer_stop(void)
|
|
{
|
|
APP_ERROR_CHECK(app_timer_stop(m_full_agc_loop_timer_id));
|
|
}
|
|
|
|
/**
|
|
* @brief Initialize AGC loop timer
|
|
* - Charles KWON
|
|
*/
|
|
void full_agc_timer_init(void)
|
|
{
|
|
APP_ERROR_CHECK(app_timer_create(&m_full_agc_loop_timer_id,
|
|
APP_TIMER_MODE_REPEATED, full_agc_loop));
|
|
}
|
|
|
|
/*============================================================================*/
|
|
/* AGC Measurement Entry Point - Charles KWON */
|
|
/*============================================================================*/
|
|
|
|
/**
|
|
* @brief Start full AGC measurement sequence
|
|
* - Charles KWON
|
|
*
|
|
* Entry point for AGC calibration. Initializes state,
|
|
* stops battery monitoring, and begins Part A measurement.
|
|
*/
|
|
void full_agc_mesurement_start(void)
|
|
{
|
|
agc_ble_send_order = false;
|
|
full_agc_completed = false;
|
|
battery_timer_stop();
|
|
order_cnt = 0;
|
|
|
|
full_agc_a_start = true;
|
|
|
|
for (uint8_t i = 0; i < LED_NO; i++) {
|
|
DBG_PRINTF("%d ", full_agc_LED_list[i]);
|
|
}
|
|
DBG_PRINTF("led/r/n ");
|
|
|
|
full_agc_start();
|
|
nrf_delay_ms(5);
|
|
full_agc_adc_init();
|
|
nrf_delay_ms(5);
|
|
full_agc_timer_start();
|
|
}
|
|
|
|
/*============================================================================*/
|
|
/* AGC Data Transmission - Charles KWON */
|
|
/*============================================================================*/
|
|
|
|
/**
|
|
* @brief AGC data send loop timer callback
|
|
* - Charles KWON
|
|
*
|
|
* Handles sequenced transmission of AGC results over BLE.
|
|
* Transmits data in chunks to avoid BLE packet size limits.
|
|
*
|
|
* @param[in] p_context Unused timer context
|
|
*/
|
|
void full_agc_send_loop(void * p_context)
|
|
{
|
|
UNUSED_PARAMETER(p_context);
|
|
|
|
static bool send_step = false;
|
|
|
|
full_agc_send_timer_stop();
|
|
|
|
if (send_step == false) {
|
|
/* Data preparation phase */
|
|
if (order_cnt >= 6) {
|
|
order_cnt = 0;
|
|
send_step = true;
|
|
} else if (order_cnt < 6) {
|
|
switch (order_cnt) {
|
|
case 0:
|
|
for (uint16_t j = 0; j < LED_NO; j++) {
|
|
agc_A_0[j] = led_pd_dac_v[j];
|
|
agc_B_0[j] = agc_read_v[j];
|
|
}
|
|
break;
|
|
|
|
case 1:
|
|
for (uint16_t j = 0; j < LED_NO; j++) {
|
|
agc_A_1[j] = led_pd_dac_v[LED_NO + j];
|
|
agc_B_1[j] = agc_read_v[LED_NO + j];
|
|
}
|
|
break;
|
|
|
|
case 2:
|
|
case 3:
|
|
case 4:
|
|
case 5:
|
|
break;
|
|
|
|
default:
|
|
DBG_PRINTF("ERR!!!, Out of range <1>\r\n");
|
|
break;
|
|
}
|
|
order_cnt++;
|
|
}
|
|
} else if (send_step == true) {
|
|
/* Data transmission phase */
|
|
if (order_cnt >= 6) {
|
|
if (ble_connection_st == 1) {
|
|
battery_timer_start();
|
|
}
|
|
|
|
order_cnt = 0;
|
|
send_step = false;
|
|
return;
|
|
} else if (order_cnt < 3) {
|
|
if (agc_ble_send_order == false) {
|
|
switch (order_cnt) {
|
|
case 0:
|
|
{
|
|
int offset = sprintf(agc_tx_buffer, "Tagc-A0");
|
|
for (uint8_t j = 0; j < LED_NO; j++) {
|
|
offset += sprintf(agc_tx_buffer + offset, ",\t%d", agc_A_0[j]);
|
|
}
|
|
sprintf(agc_tx_buffer + offset, "\r\n");
|
|
}
|
|
break;
|
|
|
|
case 1:
|
|
break;
|
|
|
|
case 2:
|
|
{
|
|
int offset = sprintf(agc_tx_buffer, "Tagc-A1");
|
|
for (uint8_t j = 0; j < LED_NO; j++) {
|
|
offset += sprintf(agc_tx_buffer + offset, ",\t%d", agc_A_1[j]);
|
|
}
|
|
sprintf(agc_tx_buffer + offset, "\r\n");
|
|
}
|
|
break;
|
|
|
|
case 3:
|
|
case 4:
|
|
case 5:
|
|
break;
|
|
|
|
default:
|
|
DBG_PRINTF("ERR!!!, Out of range <2>\r\n");
|
|
break;
|
|
}
|
|
|
|
if (ble_connection_st == 0) {
|
|
full_agc_end();
|
|
full_agc_send_timer_stop();
|
|
DBG_PRINTF("Full AGC STOP 2\r\n");
|
|
} else {
|
|
data_tx_handler(agc_tx_buffer);
|
|
order_cnt++;
|
|
if (order_cnt == 10) {
|
|
agc_ble_send_order = true;
|
|
order_cnt = 0;
|
|
}
|
|
}
|
|
} else if (agc_ble_send_order == true) {
|
|
if (ble_connection_st == 0) {
|
|
full_agc_end();
|
|
full_agc_send_timer_stop();
|
|
} else {
|
|
order_cnt++;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (ble_connection_st == 0) {
|
|
order_cnt = 0;
|
|
full_agc_send_timer_stop();
|
|
} else {
|
|
full_agc_send_timer_start();
|
|
}
|
|
}
|
|
|
|
/*============================================================================*/
|
|
/* AGC Send Timer Control - Charles KWON */
|
|
/*============================================================================*/
|
|
|
|
/**
|
|
* @brief Start AGC data transmission timer
|
|
* - Charles KWON
|
|
*/
|
|
void full_agc_send_timer_start(void)
|
|
{
|
|
APP_ERROR_CHECK(app_timer_start(m_full_agc_send_timer_id,
|
|
APP_TIMER_TICKS(FULL_AGC_SEND_INTERVAL), NULL));
|
|
}
|
|
|
|
/**
|
|
* @brief Stop AGC data transmission timer
|
|
* - Charles KWON
|
|
*/
|
|
void full_agc_send_timer_stop(void)
|
|
{
|
|
APP_ERROR_CHECK(app_timer_stop(m_full_agc_send_timer_id));
|
|
}
|
|
|
|
/**
|
|
* @brief Initialize AGC send timer
|
|
* - Charles KWON
|
|
*/
|
|
void full_agc_send_timer_init(void)
|
|
{
|
|
APP_ERROR_CHECK(app_timer_create(&m_full_agc_send_timer_id,
|
|
APP_TIMER_MODE_SINGLE_SHOT, full_agc_send_loop));
|
|
}
|