296 lines
9.4 KiB
C
296 lines
9.4 KiB
C
/**
|
||
* @copyright 2015 Indie Semiconductor.
|
||
*
|
||
* This file is proprietary to Indie Semiconductor.
|
||
* All rights reserved. Reproduction or distribution, in whole
|
||
* or in part, is forbidden except by express written permission
|
||
* of Indie Semiconductor.
|
||
*
|
||
* @file measureTask.c
|
||
* @Author: Jack.Pan
|
||
* @E-mail:jack.pan@indiemicro.com
|
||
* @Date: 2020/09/10
|
||
*/
|
||
#include <crc32.h>
|
||
#include <measureTask.h>
|
||
#include <motorControlTask.h>
|
||
#include <gtimer_device.h>
|
||
|
||
#define AVERAGE_MEASURE_POS (4U)
|
||
#define AVERAGE_MEASURE_GAIN (1U << AVERAGE_MEASURE_POS)
|
||
|
||
#define MEASURE_GAIN_POS 12U
|
||
#define MEASURE_GAIN 4096 /*(1UL << MEASURE_GAIN_POS)*/
|
||
|
||
#define MEASURE_TEMP_GAIN_POS 14U
|
||
#define MEASURE_TEMP_GAIN 7208 /*16384*0.43994*/
|
||
|
||
|
||
#define RAW_PN_VOLT_SAMPLE_NUM_POS 4U
|
||
#define RAW_PN_VOLT_SAMPLE_NUM (1U << RAW_PN_VOLT_SAMPLE_NUM_POS)
|
||
|
||
#if RAW_PN_VOLT_SAMPLE_NUM_POS <=0U
|
||
#error RAW_PN_VOLT_SAMPLE_NUM_POS >= 1U
|
||
#endif
|
||
|
||
typedef struct{
|
||
int32_t coefficient;
|
||
int32_t offset;
|
||
}coeffParam_t;
|
||
|
||
typedef struct{
|
||
uint32_t currCode;
|
||
uint16_t vampbuff[255];//vampbuff[24][10];行：一个周期的步数 列：（步与步之间的时间 ）÷（PWM周期）
|
||
uint16_t count;
|
||
coeffParam_t coeff;
|
||
int16_t target;
|
||
}AdcDatabuff_t;
|
||
|
||
|
||
typedef struct{
|
||
AdcDatabuff_t vBatt;
|
||
AdcDatabuff_t vChipTemp;
|
||
AdcDatabuff_t vAmp;
|
||
AdcDatabuff_t vPB3;
|
||
AdcDatabuff_t vPC4;
|
||
coeffParam_t tChip;
|
||
int16_t chipTemperature;
|
||
}AdcResult_t;
|
||
|
||
|
||
typedef struct{
|
||
uint8_t CoilAOpencnt;
|
||
uint8_t CoilAShortcnt;
|
||
uint8_t CoilBOpencnt;
|
||
uint8_t CoilBShortcnt;
|
||
uint8_t MotorOCcnt;
|
||
}ErrorCnt_t;
|
||
|
||
|
||
static TaskState_t adcTaskState = TASK_STATE_INIT;
|
||
static AdcResult_t adcResult;
|
||
|
||
static AdcGeneralTriggerSRC_t measSyncMode = ADC_TRIG_SRC_SOFT_INPUT;
|
||
static ADCMeasureParam_t measItem;
|
||
static volatile uint32_t measStart = 0U;
|
||
static volatile uint32_t updateSystemInfoStart = 0U;
|
||
static volatile uint32_t updatarow = 0U;
|
||
void add_data_to_vampbuff(uint32_t *original, uint16_t newData, uint16_t *bufferIndex,uint16_t *buff);
|
||
void add_data_to_buff(uint32_t *original, uint16_t newData, uint16_t *bufferIndex,uint16_t *buff);
|
||
void measureDoneISR(ADCMeasureParam_t item, uint16_t *const result);
|
||
void measureParamInit(void);
|
||
|
||
void updateTimerExpired(SoftTimer_t *timer);
|
||
|
||
static volatile uint32_t adcConvertDone = 0U;
|
||
static volatile uint16_t measGeneralAdcCode[4];
|
||
static uint16_t vAmpbuff[24];
|
||
static uint8_t SamplesNum = 0U;
|
||
static uint8_t updateSystemInfocnt = 0U;
|
||
void add_data_to_vampbuff(uint32_t *original, uint16_t newData, uint16_t *bufferIndex,uint16_t *buff)
|
||
{
|
||
*original = newData;
|
||
uint16_t row= 0U;
|
||
|
||
if(measItem.item == ADC_MEASURE_ITEM_VAMP){
|
||
|
||
SamplesNum = Motor_GetOnestep_csa_adcnt();//（步与步之间的时间 ）÷（PWM周期）
|
||
row = Motor_GetMotorStep();
|
||
|
||
if (*original == INVALID_PARAM){
|
||
for (uint8_t i = 0U; i < SamplesNum; i++){
|
||
buff[(row*SamplesNum)+i] = newData;
|
||
}
|
||
}else{
|
||
buff[(*bufferIndex) + (row*SamplesNum)] = newData;
|
||
}
|
||
(*bufferIndex) ++;
|
||
if ((*bufferIndex) >= SamplesNum){
|
||
(*bufferIndex) = 0U;
|
||
updatarow = row;
|
||
updateSystemInfoStart = 1U;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
void measureDoneISR(ADCMeasureParam_t param, uint16_t *const result)
|
||
{
|
||
measGeneralAdcCode[0] = (result[0]>=0x800U)? 0U: result[0];
|
||
measGeneralAdcCode[1] = (result[1]>=0x800U)? 0U: result[1];
|
||
measGeneralAdcCode[2] = (result[2]>=0x800U)? 0U: result[2];
|
||
measGeneralAdcCode[3] = (result[3]>=0x800U)? 0U: result[3];
|
||
measItem = ADC_GetadcMeasParamm();
|
||
|
||
adcConvertDone = 1U;
|
||
measStart = 1U;
|
||
TM_PostTask(TASK_ID_ADC_MEASURE);
|
||
}
|
||
|
||
void measureParamStart(void)
|
||
{
|
||
switch(measItem.item){
|
||
case ADC_MEASURE_ITEM_VBAT_VTEMP:
|
||
// add_data_to_buff(&adcResult.vBatt.currCode, measGeneralAdcCode[1], &adcResult.vBatt.count,adcResult.vBatt.buff);
|
||
// add_data_to_buff(&adcResult.vChipTemp.currCode, measGeneralAdcCode[2], &adcResult.vChipTemp.count,adcResult.vChipTemp.buff);
|
||
break;
|
||
case ADC_MEASURE_ITEM_VAMP:
|
||
add_data_to_vampbuff(&adcResult.vAmp.currCode, measGeneralAdcCode[0], &adcResult.vAmp.count,adcResult.vAmp.vampbuff);
|
||
add_data_to_vampbuff(&adcResult.vBatt.currCode, measGeneralAdcCode[1], &adcResult.vBatt.count,adcResult.vBatt.vampbuff);
|
||
add_data_to_vampbuff(&adcResult.vPB3.currCode, measGeneralAdcCode[2], &adcResult.vPB3.count,adcResult.vPB3.vampbuff);
|
||
add_data_to_vampbuff(&adcResult.vPC4.currCode, measGeneralAdcCode[3], &adcResult.vPC4.count,adcResult.vPC4.vampbuff);
|
||
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
|
||
int16_t get_volt(AdcDatabuff_t *const param)
|
||
{
|
||
int16_t volt = (int16_t)INVALID_VOLT;
|
||
int32_t sVolt;
|
||
uint32_t uVolt;
|
||
uint32_t sum = 0;
|
||
|
||
if (param->currCode != INVALID_PARAM){
|
||
if(measItem.item == ADC_MEASURE_ITEM_VBAT_VTEMP){
|
||
|
||
|
||
}else if(measItem.item == ADC_MEASURE_ITEM_VAMP){ //求一步以内的母线电流、VBAT、PB3、PC4
|
||
sum = 0;
|
||
SamplesNum = Motor_GetOnestep_csa_adcnt();//（步与步之间的时间 ）÷（PWM周期）
|
||
for (uint8_t i = 0U; i < SamplesNum; i++){
|
||
sum += param->vampbuff[updatarow*SamplesNum + i];
|
||
}
|
||
sVolt = param->coeff.coefficient * (int32_t)sum/SamplesNum + param->coeff.offset;
|
||
uVolt = (uint32_t)sVolt;
|
||
uVolt = uVolt/4096;
|
||
volt = (int16_t)uVolt;
|
||
}
|
||
}
|
||
return volt;
|
||
}
|
||
|
||
int16_t get_chip_temperature(int16_t vTemp)
|
||
{
|
||
int32_t stemp;
|
||
uint32_t uTemp;
|
||
uint8_t negative = 0U;
|
||
stemp = adcResult.tChip.coefficient*vTemp + adcResult.tChip.offset;
|
||
if (stemp < 0){
|
||
stemp = -stemp;
|
||
negative = 1U;
|
||
}
|
||
uTemp = (uint32_t)stemp;
|
||
uTemp = uTemp >> MEASURE_TEMP_GAIN_POS;
|
||
stemp = (int32_t)uTemp;
|
||
if (negative != 0U){
|
||
stemp = -stemp;
|
||
}
|
||
return (int16_t)stemp;
|
||
}
|
||
|
||
void updateSystemInfo(void)
|
||
{
|
||
/* calculate chip temperature */
|
||
adcResult.vChipTemp.target = get_volt(&adcResult.vChipTemp);
|
||
adcResult.chipTemperature = get_chip_temperature(adcResult.vChipTemp.target);
|
||
adcResult.vBatt.target = get_volt(&adcResult.vBatt);
|
||
adcResult.vPB3.target = get_volt(&adcResult.vPB3);
|
||
adcResult.vPC4.target = get_volt(&adcResult.vPC4);
|
||
adcResult.vAmp.target = get_volt(&adcResult.vAmp);
|
||
vAmpbuff[Motor_GetMotorStep()] = adcResult.vAmp.target;
|
||
|
||
// TM_PostTask(TASK_ID_SAFETY_MONITOR);
|
||
updateSystemInfocnt++;
|
||
if(updateSystemInfocnt > 23U){
|
||
updateSystemInfocnt = 0;
|
||
TM_PostTask(TASK_ID_SAFETY_MONITOR);
|
||
}
|
||
}
|
||
void MES_TaskHandler(void)
|
||
{
|
||
switch(adcTaskState){
|
||
case TASK_STATE_INIT:
|
||
measureParamInit();
|
||
ADC_GeneralInit();/* init ADC */
|
||
ADC_RegisterIRQ(measureDoneISR);
|
||
adcTaskState = TASK_STATE_ACTIVE;
|
||
break;
|
||
case TASK_STATE_ACTIVE:
|
||
/* Start parameters measuring */
|
||
|
||
if (updateSystemInfoStart == 1U){
|
||
updateSystemInfo();
|
||
updateSystemInfoStart = 0U;
|
||
}
|
||
|
||
if (measStart != 0U){
|
||
measStart = 0U;
|
||
measureParamStart();
|
||
}
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
|
||
|
||
void measureParamInit(void)
|
||
{
|
||
uint16_t adc0V5 = 426;//HWCFG_SFRS->ADC_0V5_CODE;//816
|
||
uint16_t adc1V0 = 853;//HWCFG_SFRS->ADC_1V0_CODE;//1652
|
||
|
||
uint16_t adc0V2 = 1773;//HWCFG_SFRS->ADC_1V0_CODE;//1652
|
||
uint16_t adc0V0 = 975;//5gain 31/32
|
||
|
||
uint16_t batt13V5 = 802;//
|
||
uint16_t batt8V0 = 468;//
|
||
|
||
adcResult.vChipTemp.coeff.coefficient = (1000 -500)*MEASURE_GAIN/((int32_t)adc1V0 - (int32_t)adc0V5);
|
||
adcResult.vChipTemp.coeff.offset = 500*MEASURE_GAIN - adcResult.vChipTemp.coeff.coefficient*(int32_t)adc0V5;
|
||
adcResult.vChipTemp.currCode = INVALID_PARAM;
|
||
/* v = a*t + b; a = 2.273, t = v/a - b/a; gain = 1/a, offset = - b/a*/
|
||
adcResult.tChip.coefficient = MEASURE_TEMP_GAIN;
|
||
adcResult.tChip.offset = 25*(int32_t)(1UL << MEASURE_TEMP_GAIN_POS) - MEASURE_TEMP_GAIN*(int32_t)HWCFG_SFRS->TSENSOR_mV_25C;
|
||
/*vAmp 5gain 31/32 200mv 20mv*/
|
||
adcResult.vAmp.coeff.coefficient = (200 -0)*MEASURE_GAIN/((int32_t)adc0V2 - (int32_t)adc0V0);
|
||
adcResult.vAmp.coeff.offset = 0*MEASURE_GAIN - adcResult.vAmp.coeff.coefficient*(int32_t)adc0V0;
|
||
adcResult.vAmp.currCode = INVALID_PARAM;
|
||
/* battery volt*/
|
||
adcResult.vBatt.coeff.coefficient = (int32_t)(13500 - 8000)*MEASURE_GAIN/((int32_t)batt13V5 - (int32_t)batt8V0);
|
||
adcResult.vBatt.coeff.offset = (int32_t)(8000*MEASURE_GAIN) - adcResult.vBatt.coeff.coefficient*(int32_t)batt8V0;
|
||
adcResult.vBatt.currCode = INVALID_PARAM;
|
||
/* PB3 volt*/
|
||
adcResult.vPB3.coeff.coefficient = (int32_t)(1000 - 500)*MEASURE_GAIN/((int32_t)adc1V0 - (int32_t)adc0V5);
|
||
adcResult.vPB3.coeff.offset = (int32_t)(500*MEASURE_GAIN) - adcResult.vPB3.coeff.coefficient*(int32_t)adc0V5;
|
||
adcResult.vPB3.currCode = INVALID_PARAM;
|
||
/* PC4 volt*/
|
||
adcResult.vPC4.coeff.coefficient = (int32_t)(1000 - 500)*MEASURE_GAIN*32/((int32_t)adc1V0 - (int32_t)adc0V5)/22;
|
||
adcResult.vPC4.coeff.offset = (int32_t)(500*MEASURE_GAIN) - adcResult.vPC4.coeff.coefficient*(int32_t)adc0V5;
|
||
adcResult.vPC4.currCode = INVALID_PARAM;
|
||
}
|
||
|
||
int16_t Get_vAmp(void){
|
||
return adcResult.vAmp.target;
|
||
}
|
||
|
||
uint16_t *MEAGet_vAmpbuff(void){
|
||
|
||
return vAmpbuff;
|
||
|
||
}
|
||
uint16_t MEA_GetVbat(void)
|
||
{
|
||
return adcResult.vBatt.target;
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|