MBOOL
CamIOPipe::
configPipe(vector<PortInfo const*>const& vInPorts, vector<PortInfo const*>const& vOutPorts)
{
FUNCTION_LOG_START;
MY_LOGD("+ tid(%d), %d in / %d out", gettid(), vInPorts.size(), vOutPorts.size());
MBOOL ret = MTRUE;
//
if (0 == vInPorts.size()
|| 0 == vOutPorts.size()
|| vOutPorts.size() > 2)
{
MY_LOGE("Port config error");
return MFALSE;
}
//
if (EPortType_Sensor != vInPorts.at(0)->type)
{
MY_LOGE("The IN port type should be sensor type");
return MFALSE;
}
//
for (MUINT32 i = 0; i < vOutPorts.size(); i++)
{
if (EPortType_MemoryOut != vOutPorts.at(i)->type)
{
MY_LOGE("The OUT port type should be EPortType_MemoryOut");
return MFALSE;
}
}
// (1). callbacks
mpCamIOPipe->setCallbacks(NULL, NULL, NULL);
// (2). command queue config
ret = ret
&& mpCamIOPipe->sendCommand(NSImageio::NSIspio::EPIPECmd_SET_CQ_CHANNEL,
(MINT32)NSImageio::NSIspio::EPIPE_PASS1_CQ0,
0,
0
)
&& mpCamIOPipe->sendCommand(NSImageio::NSIspio::EPIPECmd_SET_CQ_TRIGGER_MODE,
(MINT32)NSImageio::NSIspio::EPIPE_PASS1_CQ0,
(MINT32)NSImageio::NSIspio::EPIPECQ_TRIGGER_SINGLE_IMMEDIATE,
(MINT32)NSImageio::NSIspio::EPIPECQ_TRIG_BY_START
)
&& mpCamIOPipe->sendCommand(NSImageio::NSIspio::EPIPECmd_SET_CONFIG_STAGE,
(MINT32)NSImageio::NSIspio::eConfigSettingStage_Init,
0,
0
);
if (!ret)
{
MY_LOGE("Cammand queue config fail:%d", mpCamIOPipe->getLastErrorCode());
return ret;
}
//
// (3). In sensor port
vector<NSImageio::NSIspio::PortInfo const*> vCamIOInPorts;
SensorPortInfo const* const pSensorPort = reinterpret_cast<SensorPortInfo const*> (vInPorts.at(0));
::memcpy(&mrSensorPortInfo, const_cast<SensorPortInfo*>(pSensorPort),sizeof(SensorPortInfo));
MUINT32 u4SensorWidth = 0, u4SensorHeight = 0;
MUINT32 u4RawPixelID = 0;
EImageFormat eSensorFmt = eImgFmt_UNKNOWN;
// (3.1) Sensor instance
if (NULL == mpSensorHal)
{
mpSensorHal = SensorHal::createInstance();
if (NULL == mpSensorHal)
{
MY_LOGE("Null sensorHal object");
return MFALSE;
}
}
//
mpSensorHal->sendCommand(static_cast<halSensorDev_e>(pSensorPort->u4DeviceID),
SENSOR_CMD_SET_SENSOR_DEV,
0,
0,
0
);
//
//mpSensorHal->init();
ret = querySensorInfo( pSensorPort->u4DeviceID, pSensorPort->u4Scenario, pSensorPort->u4Bitdepth, eSensorFmt, u4SensorWidth, u4SensorHeight, u4RawPixelID);
MY_LOGD("SensorPortInfo: (u4DeviceID, u4Scenario, bitdepth, fgBypassDelay, fgBypassScenaio, u4RawType) = (%d, %d, %d, %d, %d, %d)",
pSensorPort->u4DeviceID, pSensorPort->u4Scenario, pSensorPort->u4Bitdepth,
pSensorPort->fgBypassDelay, pSensorPort->fgBypassScenaio, pSensorPort->u4RawType);
//
MUINT32 u4SensorStride = u4SensorWidth;
if (eImgFmt_BAYER8 == eSensorFmt || eImgFmt_BAYER10 == eSensorFmt || eImgFmt_BAYER12 == eSensorFmt)
{
u4SensorStride = NSImageio::NSIspio::queryRawStride(eSensorFmt, u4SensorWidth);
}
MY_LOGD("SensorPortInfo: (width, height, format, stride) = (%d, %d, 0x%x, %d, %d, %d)",
u4SensorWidth, u4SensorHeight, eSensorFmt, u4SensorStride);
//
NSImageio::NSIspio::PortInfo tgi;
tgi.eImgFmt = eSensorFmt;
tgi.eRawPxlID = mapRawPixelID(u4RawPixelID);
tgi.u4ImgWidth = u4SensorWidth;
tgi.u4ImgHeight = u4SensorHeight;
tgi.u4Stride[0] = u4SensorStride;
tgi.u4Stride[1] = 0;
tgi.u4Stride[2] = 0;
tgi.type = NSImageio::NSIspio::EPortType_Sensor;
mu4DeviceID = pSensorPort->u4DeviceID;
tgi.index = ((mu4DeviceID == SENSOR_DEV_MAIN)||(mu4DeviceID == SENSOR_DEV_ATV)) ? (NSImageio::NSIspio::EPortIndex_TG1I) : (NSImageio::NSIspio::EPortIndex_TG2I);
tgi.inout = NSImageio::NSIspio::EPortDirection_In;
tgi.u4BufSize = (MUINT32)0;
vCamIOInPorts.push_back(&tgi);
// The raw type, 0: pure raw, 1: pre-process raw
if(pSensorPort->u4RawType == 1)
{
ret = mpCamIOPipe->sendCommand(NSImageio::NSIspio::EPIPECmd_SET_IMGO_RAW_TYPE,
(MINT32)NSImageio::NSIspio::eRawImageType_PreProc,
0,
0
);
}
//
// (4). Out Port
vector<NSImageio::NSIspio::PortInfo const*> vCamIOOutPorts;
NSImageio::NSIspio::PortInfo imgo;
NSImageio::NSIspio::PortInfo img2o;
for (MUINT32 i = 0; i < vOutPorts.size(); i++)
{
MemoryOutPortInfo const* const memOutPort= reinterpret_cast<MemoryOutPortInfo const*> (vOutPorts.at(i));
//
if (0 == memOutPort->index)
{
MY_LOGD("MemoryOutPortInfo1: (fmt, width, height) = (0x%x, %d, %d)", tgi.eImgFmt, tgi.u4ImgWidth, tgi.u4ImgHeight);
MY_LOGD("MemoryOutPortInfo1: stride = (%d, %d, %d)", memOutPort->u4Stride[0], memOutPort->u4Stride[1], memOutPort->u4Stride[2]);
imgo.eImgFmt = tgi.eImgFmt;
imgo.u4ImgWidth = tgi.u4ImgWidth;
imgo.u4ImgHeight = tgi.u4ImgHeight;
// no crop
imgo.crop.y = 0;
imgo.crop.h = imgo.u4ImgHeight;
imgo.type = NSImageio::NSIspio::EPortType_Memory;
imgo.index = NSImageio::NSIspio::EPortIndex_IMGO;
imgo.inout = NSImageio::NSIspio::EPortDirection_Out;
imgo.u4Stride[0] = memOutPort->u4Stride[0];
imgo.u4Stride[1] = memOutPort->u4Stride[1];
imgo.u4Stride[2] = memOutPort->u4Stride[2];
imgo.u4Offset = 0;
vCamIOOutPorts.push_back(&imgo);
}
#warning [TODO] Should check the port config by scenario
else if (1 == memOutPort->index)
{
MY_LOGD("MemoryOutPortInfo2: (fmt, width, height) = (0x%x, %d, %d)", memOutPort->eImgFmt, memOutPort->u4ImgWidth, memOutPort->u4ImgHeight);
MY_LOGD("MemoryOutPortInfo2: stride = (%d, %d, %d)", memOutPort->u4Stride[0], memOutPort->u4Stride[1], memOutPort->u4Stride[2]);
img2o.eImgFmt = memOutPort->eImgFmt;
img2o.u4ImgWidth = memOutPort->u4ImgWidth;
img2o.u4ImgHeight = memOutPort->u4ImgHeight;
img2o.crop.y = 0;
img2o.crop.h = img2o.u4ImgHeight;
img2o.type = NSImageio::NSIspio::EPortType_Memory;
img2o.index = NSImageio::NSIspio::EPortIndex_IMG2O;
img2o.inout = NSImageio::NSIspio::EPortDirection_Out;
img2o.u4Stride[0] = memOutPort->u4Stride[0];
img2o.u4Stride[1] = memOutPort->u4Stride[1];
img2o.u4Stride[2] = memOutPort->u4Stride[2];
vCamIOOutPorts.push_back(&img2o);
mfgIsYUVPortON = MTRUE;
}
}
ret = mpCamIOPipe->configPipe(vCamIOInPorts, vCamIOOutPorts);
//
ret = configSensor(pSensorPort->u4DeviceID, pSensorPort->u4Scenario, u4SensorWidth, u4SensorHeight, pSensorPort->fgBypassDelay, pSensorPort->fgBypassScenaio, MTRUE);
FUNCTION_LOG_END;
return ret;
}
MBOOL
CamIOPipe::
configPipe(vector<PortInfo const*>const& vInPorts, vector<PortInfo const*>const& vOutPorts)
{
//FUNCTION_LOG_START;
MY_LOGD("+ tid(%d), %d in / %d out", gettid(), vInPorts.size(), vOutPorts.size());
MBOOL ret = MTRUE;
//
if (0 == vInPorts.size()
|| 0 == vOutPorts.size()
|| vOutPorts.size() > 2)
{
MY_LOGE("Port config error");
return MFALSE;
}
//
if (EPortType_Sensor != vInPorts.at(0)->type)
{
MY_LOGE("The IN port type should be sensor type");
return MFALSE;
}
//
for (MUINT32 i = 0; i < vOutPorts.size(); i++)
{
if (EPortType_MemoryOut != vOutPorts.at(i)->type)
{
MY_LOGE("The OUT port type should be EPortType_MemoryOut");
return MFALSE;
}
}
// (1). callbacks
mpCamIOPipe->setCallbacks(NULL, NULL, NULL);
// (2). command queue config
ret = ret
&& mpCamIOPipe->sendCommand(NSImageio::NSIspio::EPIPECmd_SET_CQ_CHANNEL,
(MINT32)NSImageio::NSIspio::EPIPE_PASS1_CQ0,
0,
0
)
&& mpCamIOPipe->sendCommand(NSImageio::NSIspio::EPIPECmd_SET_CQ_TRIGGER_MODE,
(MINT32)NSImageio::NSIspio::EPIPE_PASS1_CQ0,
(MINT32)NSImageio::NSIspio::EPIPECQ_TRIGGER_SINGLE_IMMEDIATE,
(MINT32)NSImageio::NSIspio::EPIPECQ_TRIG_BY_START
)
&& mpCamIOPipe->sendCommand(NSImageio::NSIspio::EPIPECmd_SET_CONFIG_STAGE,
(MINT32)NSImageio::NSIspio::eConfigSettingStage_Init,
0,
0
);
if (!ret)
{
MY_LOGE("Cammand queue config fail:%d", mpCamIOPipe->getLastErrorCode());
return ret;
}
//
// (3). In sensor port
vector<NSImageio::NSIspio::PortInfo const*> vCamIOInPorts;
SensorPortInfo const* const pSensorPort = reinterpret_cast<SensorPortInfo const*> (vInPorts.at(0));
::memcpy(&mrSensorPortInfo, const_cast<SensorPortInfo*>(pSensorPort),sizeof(SensorPortInfo));
MUINT32 u4SensorWidth = 0, u4SensorHeight = 0;
MUINT32 u4RawPixelID = 0;
EImageFormat eSensorFmt = eImgFmt_UNKNOWN;
// (3.1) Sensor instance
if (NULL == mpSensorHal)
{
mpSensorHal = SensorHal::createInstance();
if (NULL == mpSensorHal)
{
MY_LOGE("Null sensorHal object");
return MFALSE;
}
}
//
mpSensorHal->sendCommand(static_cast<halSensorDev_e>(pSensorPort->u4DeviceID),
SENSOR_CMD_SET_SENSOR_DEV,
0,
0,
0
);
//
//mpSensorHal->init();
ret = querySensorInfo( pSensorPort->u4DeviceID, pSensorPort->u4Scenario, pSensorPort->u4Bitdepth, eSensorFmt, u4SensorWidth, u4SensorHeight, u4RawPixelID);
MY_LOGD("Sensor: (devID,scen)(%d,%d) (w,h,fmt,bits,pixID)(%d,%d,0x%x,%d,%d) (bpDelay,bpScen,rawType)(%d,%d,%d)",
pSensorPort->u4DeviceID, pSensorPort->u4Scenario,
u4SensorWidth, u4SensorHeight, eSensorFmt, pSensorPort->u4Bitdepth, u4RawPixelID,
pSensorPort->fgBypassDelay, pSensorPort->fgBypassScenaio, pSensorPort->u4RawType);
//
MUINT32 u4SensorStride = u4SensorWidth;
if (eImgFmt_BAYER8 == eSensorFmt || eImgFmt_BAYER10 == eSensorFmt || eImgFmt_BAYER12 == eSensorFmt)
{
u4SensorStride = NSImageio::NSIspio::queryRawStride(eSensorFmt, u4SensorWidth);
}
//MY_LOGD("SensorPortInfo: (width, height, format, stride) = (%d, %d, 0x%x, %d, %d, %d)",
// u4SensorWidth, u4SensorHeight, eSensorFmt, u4SensorStride);
//
NSImageio::NSIspio::PortInfo tgi;
tgi.eImgFmt = eSensorFmt;
tgi.eRawPxlID = mapRawPixelID(u4RawPixelID);
tgi.u4ImgWidth = u4SensorWidth;
tgi.u4ImgHeight = u4SensorHeight;
tgi.u4Stride[0] = u4SensorStride;
tgi.u4Stride[1] = 0;
tgi.u4Stride[2] = 0;
tgi.type = NSImageio::NSIspio::EPortType_Sensor;
mu4DeviceID = pSensorPort->u4DeviceID;
tgi.index = ((mu4DeviceID == SENSOR_DEV_MAIN)||(mu4DeviceID == SENSOR_DEV_ATV)) ? (NSImageio::NSIspio::EPortIndex_TG1I) : (NSImageio::NSIspio::EPortIndex_TG2I);
tgi.inout = NSImageio::NSIspio::EPortDirection_In;
tgi.u4BufSize = (MUINT32)0;
vCamIOInPorts.push_back(&tgi);
// The raw type, 0: pure raw, 1: pre-process raw
if(pSensorPort->u4RawType == 1)
{
ret = mpCamIOPipe->sendCommand(NSImageio::NSIspio::EPIPECmd_SET_IMGO_RAW_TYPE,
(MINT32)NSImageio::NSIspio::eRawImageType_PreProc,
0,
0
);
}
//
// (4). Out Port
vector<NSImageio::NSIspio::PortInfo const*> vCamIOOutPorts;
NSImageio::NSIspio::PortInfo imgo;
NSImageio::NSIspio::PortInfo img2o;
for (MUINT32 i = 0; i < vOutPorts.size(); i++)
{
MemoryOutPortInfo const* const memOutPort= reinterpret_cast<MemoryOutPortInfo const*> (vOutPorts.at(i));
//
if (0 == memOutPort->index)
{
MY_LOGD("Out 0: (fmt,w,h)(0x%x,%d,%d) stride(%d,%d,%d)",
tgi.eImgFmt, tgi.u4ImgWidth, tgi.u4ImgHeight,
memOutPort->u4Stride[0], memOutPort->u4Stride[1], memOutPort->u4Stride[2]);
imgo.eImgFmt = tgi.eImgFmt;
imgo.u4ImgWidth = tgi.u4ImgWidth;
imgo.u4ImgHeight = tgi.u4ImgHeight;
// no crop
imgo.crop.y = 0;
imgo.crop.h = imgo.u4ImgHeight;
imgo.type = NSImageio::NSIspio::EPortType_Memory;
imgo.index = NSImageio::NSIspio::EPortIndex_IMGO;
imgo.inout = NSImageio::NSIspio::EPortDirection_Out;
imgo.u4Stride[0] = memOutPort->u4Stride[0];
imgo.u4Stride[1] = memOutPort->u4Stride[1];
imgo.u4Stride[2] = memOutPort->u4Stride[2];
imgo.u4Offset = 0;
vCamIOOutPorts.push_back(&imgo);
}
#warning [TODO] Should check the port config by scenario
else if (1 == memOutPort->index)
{
MY_LOGD("Out 1: (fmt,w,h)(0x%x,%d,%d) stride(%d,%d,%d)",
memOutPort->eImgFmt, memOutPort->u4ImgWidth, memOutPort->u4ImgHeight,
memOutPort->u4Stride[0], memOutPort->u4Stride[1], memOutPort->u4Stride[2]);
img2o.eImgFmt = memOutPort->eImgFmt;
img2o.u4ImgWidth = memOutPort->u4ImgWidth;
img2o.u4ImgHeight = memOutPort->u4ImgHeight;
img2o.crop.y = 0;
img2o.crop.h = img2o.u4ImgHeight;
img2o.type = NSImageio::NSIspio::EPortType_Memory;
img2o.index = NSImageio::NSIspio::EPortIndex_IMG2O;
img2o.inout = NSImageio::NSIspio::EPortDirection_Out;
img2o.u4Stride[0] = memOutPort->u4Stride[0];
img2o.u4Stride[1] = memOutPort->u4Stride[1];
img2o.u4Stride[2] = memOutPort->u4Stride[2];
vCamIOOutPorts.push_back(&img2o);
mfgIsYUVPortON = MTRUE;
}
}
ret = mpCamIOPipe->configPipe(vCamIOInPorts, vCamIOOutPorts);
//
ret = configSensor(pSensorPort->u4DeviceID, pSensorPort->u4Scenario, u4SensorWidth, u4SensorHeight, pSensorPort->fgBypassDelay, pSensorPort->fgBypassScenaio, MTRUE);
// The raw type, 0: pure raw, 1: pre-process raw 2: sensor test pattern
if(pSensorPort->u4RawType == 2)
{
MINT32 u32Enable = 1;
mpSensorHal->sendCommand(static_cast<halSensorDev_e>(pSensorPort->u4DeviceID),
SENSOR_CMD_SET_TEST_PATTERN_OUTPUT,
(MINT32)&u32Enable,
0,
0);
MY_LOGD("Sensor Test Pattern");
}
// query skip frame to wait for stable
mu4SkipFrame = 0;
if (mrSensorPortInfo.fgBypassDelay == MFALSE)
{
MUINT32 u4Mode = SENSOR_CAPTURE_DELAY;
switch (pSensorPort->u4Scenario)
{
case ACDK_SCENARIO_ID_CAMERA_PREVIEW:
u4Mode = SENSOR_PREVIEW_DELAY;
break;
case ACDK_SCENARIO_ID_CAMERA_CAPTURE_JPEG:
u4Mode = SENSOR_CAPTURE_DELAY;
break;
case ACDK_SCENARIO_ID_VIDEO_PREVIEW:
u4Mode = SENSOR_VIDEO_DELAY;
break;
}
//
mpSensorHal->sendCommand(static_cast<halSensorDev_e>(mrSensorPortInfo.u4DeviceID),
static_cast<int>(SENSOR_CMD_GET_UNSTABLE_DELAY_FRAME_CNT),
reinterpret_cast<int>(&mu4SkipFrame),
reinterpret_cast<int>(&u4Mode));
}
handleNotifyCallback( ECamPipe_NOTIFY_MSG_DROPFRAME, mu4SkipFrame, 0 );
//FUNCTION_LOG_END;
return ret;
}
void spintronicsStateMachine()
{
volatile _Q15 bridgeSample;
volatile _Q15 coilSample;
static unsigned char state = IDLE;
static uint32_t timer;
static uint8_t sensor;
_Q15 cosOmega1T;//fractions of full-scale //aligned to compensate for ADCDAC_GROUP_DELAY
_Q15 cosOmega2T;//fractions of full-scale //aligned to compensate for ADCDAC_GROUP_DELAY
static _Q15 cosOmega1TTimeAligned;//fractions of full-scale
static _Q15 cosOmega2TTimeAligned;//fractions of full-scale
static _Q15 freqT[6];//array contents: 2*f1*t, 2*f2*t, 2*f1*(t + ADCDAC_GROUP_DELAY), 2*f2*(t + ADCDAC_GROUP_DELAY), 2*fdiff*(t + ADCDAC_GROUP_DELAY), 2*fsum*(t + ADCDAC_GROUP_DELAY)
static int64_t cosAccumulator[5];
static int64_t sinAccumulator[5];
static _Q15 phaseAngle[5];//units are radians divided by PI
static _Q15 amplitude[5];//fractions of full-scale
static bool bridgeADCClipFlag;
static bool coilADCClipFlag;
static bool bridgeDigitalClipFlag;
#ifdef SIMULATION_MODE
uint16_t RXBUF2 = rand();//only to give random stimulus during simulation
#endif
if (RXBUF0 == 0x7FFF || RXBUF0 == 0x8000)
{
bridgeADCClipFlag = true;
}
if (RXBUF2 == 0x7FFF || RXBUF2 == 0x8000)
{
coilADCClipFlag = true;
}
bridgeSample = readBridgeSampleAndApplyGain(&bridgeDigitalClipFlag);
coilSample = RXBUF2;
if (GUIRequestingRun == false)
{
state = IDLE;
}
switch (state)
{
case IDLE:
timer = 0;
sensor = 0;
configSensor(sensor);
signalGenerator(RESET, freqT, &cosOmega1T, &cosOmega2T);
shiftRegister(cosOmega1T, cosOmega2T, &cosOmega1TTimeAligned, &cosOmega2TTimeAligned);
cosAccumulator[0] = 0; cosAccumulator[1] = 0; cosAccumulator[2] = 0; cosAccumulator[3] = 0; cosAccumulator[4] = 0;
sinAccumulator[0] = 0; sinAccumulator[1] = 0; sinAccumulator[2] = 0; sinAccumulator[3] = 0; sinAccumulator[4] = 0;
phaseAngle[0] = 0; phaseAngle[1] = 0; phaseAngle[2] = 0; phaseAngle[3] = 0; phaseAngle[4] = 0;
amplitude[0] = 0; amplitude[1] = 0; amplitude[2] = 0; amplitude[3] = 0; amplitude[4] = 0;
bridgeADCClipFlag = false;
coilADCClipFlag = false;
bridgeDigitalClipFlag = false;
#ifdef CS4272_CONTROL_PORT_MODE
state = CALIBRATE_ADC;
enableADCHpf(true);//enable the hpf to calibrate for systematic DC offset
#else
state = START_SIGNAL_GEN;
#endif
break;
#ifdef CS4272_CONTROL_PORT_MODE
case CALIBRATE_ADC:
signalGenerator(RESET, freqT, &cosOmega1T, &cosOmega2T);
shiftRegister(cosOmega1T, cosOmega2T, &cosOmega1TTimeAligned, &cosOmega2TTimeAligned);
++timer;
if (timer == ADC_CALIBRATION_TIME)
{
timer = 0;
state = WAIT_FOR_HPF_DISABLE_MESSAGE_TX;
enableADCHpf(false);//DC calibration complete; disable hpf
}
break;
case WAIT_FOR_HPF_DISABLE_MESSAGE_TX:
signalGenerator(RESET, freqT, &cosOmega1T, &cosOmega2T);
shiftRegister(cosOmega1T, cosOmega2T, &cosOmega1TTimeAligned, &cosOmega2TTimeAligned);
++timer;
if (timer == CS4272_SPI_TX_LATENCY)
{
timer = 0;
state = START_SIGNAL_GEN;
}
break;
#endif
case START_SIGNAL_GEN:
signalGenerator(RUN, freqT, &cosOmega1T, &cosOmega2T);
shiftRegister(cosOmega1T, cosOmega2T, &cosOmega1TTimeAligned, &cosOmega2TTimeAligned);
++timer;
if (timer == SETUP_TIME)
{
timer = 0;
state = MEASURE_PHASE;
}
break;
case MEASURE_PHASE:
measurePhase(bridgeSample, coilSample, freqT, cosOmega1TTimeAligned, cosOmega2TTimeAligned, cosAccumulator, sinAccumulator);
signalGenerator(RUN, freqT, &cosOmega1T, &cosOmega2T);
shiftRegister(cosOmega1T, cosOmega2T, &cosOmega1TTimeAligned, &cosOmega2TTimeAligned);
++timer;
if (timer == phaseMeasurementTime)
{
timer = 0;
state = CALCULATE_PHASE;
}
break;
case CALCULATE_PHASE:
{
static uint8_t shiftAmt[5];
if (timer < 5)
{
calculateShiftAmount(timer, cosAccumulator, sinAccumulator, shiftAmt);
}
else
{
calculatePhase(timer - 5, cosAccumulator, sinAccumulator, phaseAngle, shiftAmt);
}
signalGenerator(RUN, freqT, &cosOmega1T, &cosOmega2T);
shiftRegister(cosOmega1T, cosOmega2T, &cosOmega1TTimeAligned, &cosOmega2TTimeAligned);
++timer;
if (timer == 10)
{
cosAccumulator[0] = 0; cosAccumulator[1] = 0; cosAccumulator[2] = 0; cosAccumulator[3] = 0; cosAccumulator[4] = 0;
sinAccumulator[0] = 0; sinAccumulator[1] = 0; sinAccumulator[2] = 0; sinAccumulator[3] = 0; sinAccumulator[4] = 0;
timer = 0;
state = MEASURE_AMPLITUDE;
}
break;
}
case MEASURE_AMPLITUDE:
measureAmplitude(bridgeSample, coilSample, freqT, cosAccumulator, phaseAngle);
signalGenerator(RUN, freqT, &cosOmega1T, &cosOmega2T);
shiftRegister(cosOmega1T, cosOmega2T, &cosOmega1TTimeAligned, &cosOmega2TTimeAligned);
++timer;
if (timer == amplitudeMeasurementTime)
{
timer = 0;
state = CALCULATE_AMPLITUDE;
}
break;
case CALCULATE_AMPLITUDE:
calculateAmplitude(timer, cosAccumulator, amplitude);
signalGenerator(RUN, freqT, &cosOmega1T, &cosOmega2T);
shiftRegister(cosOmega1T, cosOmega2T, &cosOmega1TTimeAligned, &cosOmega2TTimeAligned);
++timer;
if (timer == 5)
{
uint8_t index;
uint8_t txSensor;//temporary variable to store the value for transmission
_Q15 txPhaseAngle[5];//temporary array to store values for transmission
_Q15 txAmplitude[5];//temporary array to store values for transmission
bool txBridgeADCClipFlag;//temporary variable to store the flag for transmission
bool txCoilADCClipFlag;//temporary variable to store the flag for transmission
bool txBridgeDigitalClipFlag;//temporary variable to store the flag for transmission
//store variables for transmission
txSensor = sensor;
for (index = 0; index < 5; ++index)
{
txPhaseAngle[index] = phaseAngle[index];
txAmplitude[index] = amplitude[index];
}
txBridgeADCClipFlag = bridgeADCClipFlag;
txCoilADCClipFlag = coilADCClipFlag;
txBridgeDigitalClipFlag = bridgeDigitalClipFlag;
//clear static variables for next iteration of the state machine
cosAccumulator[0] = 0; cosAccumulator[1] = 0; cosAccumulator[2] = 0; cosAccumulator[3] = 0; cosAccumulator[4] = 0;
timer = 0;
bridgeADCClipFlag = false;
coilADCClipFlag = false;
bridgeDigitalClipFlag = false;
state = START_SIGNAL_GEN;
++sensor;
if (sensor == NUMBER_OF_SENSORS)
{
sensor = 0;
}
configSensor(sensor);
//transmit results
transmitResults(txSensor, txPhaseAngle, txAmplitude, txBridgeADCClipFlag, txCoilADCClipFlag, txBridgeDigitalClipFlag);//don't do this until the state machine is ready for the next measurment period; transmitting results takes more than one sample period.
}
break;
default:
timer = 0;
sensor = 0;
configSensor(sensor);
signalGenerator(RESET, freqT, &cosOmega1T, &cosOmega2T);
shiftRegister(cosOmega1T, cosOmega2T, &cosOmega1TTimeAligned, &cosOmega2TTimeAligned);
cosAccumulator[0] = 0; cosAccumulator[1] = 0; cosAccumulator[2] = 0; cosAccumulator[3] = 0; cosAccumulator[4] = 0;
sinAccumulator[0] = 0; sinAccumulator[1] = 0; sinAccumulator[2] = 0; sinAccumulator[3] = 0; sinAccumulator[4] = 0;
phaseAngle[0] = 0; phaseAngle[1] = 0; phaseAngle[2] = 0; phaseAngle[3] = 0; phaseAngle[4] = 0;
amplitude[0] = 0; amplitude[1] = 0; amplitude[2] = 0; amplitude[3] = 0; amplitude[4] = 0;
break;
}
}
