...
 
Commits (2)
......@@ -77,12 +77,12 @@ void Balise::launch()
uint16_t ctSPI = 0;
uint16_t ctI2C = 0;
initDistanceMode();
initGPIODir();
blinkLeds();
initCaptors();
while(1)
{
if(_flagUpdate > 0)
......@@ -197,44 +197,61 @@ void Balise::setGPIOEvitement()
}
}
void Balise::initDistanceMode()
void Balise::initCaptors()
{
for(uint16_t i = 0; i < NB_OF_CAPTOR_PER_I2C; ++i)
{
if(EnableCaptor[i + 8])
{
_i2c1->sendMessage(muxMSG[i]);
vl53l1x_i2c1[i]->init();
vl53l1x_i2c1[i]->setDistanceMode(VL53l1x::VL53L1DistanceMode::ShortMode);
captorState[i + 8] = CaptorStatus::Ready;
}
else
{
captorState[i + 8] = CaptorStatus::Disabled;
}
bool initNotDone = true;
if(EnableCaptor[i + 16])
{
_i2c2->sendMessage(muxMSG[i]);
vl53l1x_i2c2[i]->init();
vl53l1x_i2c2[i]->setDistanceMode(VL53l1x::VL53L1DistanceMode::ShortMode);
captorState[i + 16] = CaptorStatus::Ready;
}
else
while(initNotDone)
{
if(_flagUpdate > 0)
{
captorState[i + 16] = CaptorStatus::Disabled;
}
_flagUpdate = 0;
initNotDone = false;
if(EnableCaptor[i])
{
_i2c3->sendMessage(muxMSG[i]);
vl53l1x_i2c3[i]->init();
vl53l1x_i2c3[i]->setDistanceMode(VL53l1x::VL53L1DistanceMode::ShortMode);
captorState[i] = CaptorStatus::Ready;
}
else
{
captorState[i] = CaptorStatus::Disabled;
for(uint16_t i = 0; i < NB_OF_CAPTOR_PER_I2C; ++i)
{
if((captorState[i + 8] != CaptorStatus::Ready) && (captorState[i + 8] != CaptorStatus::Disabled))
{
initNotDone = true;
if(EnableCaptor[i + 8])
{
runAcquisitionStateMachine(_i2c1, vl53l1x_i2c1[i], i, i + 8);
}
else
{
captorState[i + 8] = CaptorStatus::Disabled;
}
}
if((captorState[i + 16] != CaptorStatus::Ready) && (captorState[i + 16] != CaptorStatus::Disabled))
{
initNotDone = true;
if(EnableCaptor[i + 16])
{
runAcquisitionStateMachine(_i2c2, vl53l1x_i2c2[i], i, i + 16);
}
else
{
captorState[i + 16] = CaptorStatus::Disabled;
}
}
if((captorState[i] != CaptorStatus::Ready) && (captorState[i] != CaptorStatus::Disabled))
{
initNotDone = true;
if(EnableCaptor[i])
{
runAcquisitionStateMachine(_i2c3, vl53l1x_i2c3[i], i, i);
}
else
{
captorState[i] = CaptorStatus::Disabled;
}
}
}
_i2c1->update();
_i2c2->update();
_i2c3->update();
}
}
}
......@@ -318,6 +335,84 @@ void Balise::runAcquisitionStateMachine(I2C *i2c, VL53l1x *vl, uint8_t captorID8
{
switch(captorState[captorID24])
{
case CaptorStatus::Start:
{
captorState[captorID24] = CaptorStatus::Init;
}
break;
case CaptorStatus::Init:
{
i2c->sendMessage(muxMSG[captorID8]);
vl->init();
vl->setDistanceMode(VL53l1x::VL53L1DistanceMode::ShortMode);
captorState[captorID24] = CaptorStatus::WaitInit;
}
break;
case CaptorStatus::WaitInit:
{
VL53l1x::VL53L1Status status = vl->checkInit();
if (status == VL53l1x::VL53L1Status::Sent)
captorState[captorID24] = CaptorStatus::WaitDistanceMode;
else if (status == VL53l1x::VL53L1Status::Error)
captorState[captorID24] = CaptorStatus::InitTimeout;
}
break;
case CaptorStatus::InitTimeout:
{
captorTimeoutCount[captorID24]++;
if(captorTimeoutCount[captorID24] > 3)
{
captorState[captorID24] = CaptorStatus::Disabled;
EnableCaptor[captorID24] = false;
}
else
{
i2c->sendMessage(muxMSG[captorID8]);
vl->init();
captorState[captorID24] = CaptorStatus::WaitInit;
}
}
break;
case CaptorStatus::WaitDistanceMode:
{
VL53l1x::VL53L1Status status = vl->checkSetDistanceMode();
if (status == VL53l1x::VL53L1Status::Sent)
captorState[captorID24] = CaptorStatus::Ready;
else if (status == VL53l1x::VL53L1Status::Error)
captorState[captorID24] = CaptorStatus::DistanceTimeout;
}
break;
case CaptorStatus::DistanceTimeout:
{
captorTimeoutCount[captorID24]++;
if(captorTimeoutCount[captorID24] > 3)
{
captorState[captorID24] = CaptorStatus::Disabled;
EnableCaptor[captorID24] = false;
}
else
{
i2c->sendMessage(muxMSG[captorID8]);
vl->setDistanceMode(VL53l1x::VL53L1DistanceMode::ShortMode);
captorState[captorID24] = CaptorStatus::WaitDistanceMode;
}
}
break;
case CaptorStatus::Ready:
{
i2c->sendMessage(muxMSG[captorID8]);
......
......@@ -33,7 +33,10 @@ public:
{
Start =0,
Init,
SetDistanceMode,
WaitInit,
WaitDistanceMode,
InitTimeout,
DistanceTimeout,
Ready,
GetDistance,
GetStatus,
......@@ -44,7 +47,7 @@ public:
private:
void initDistanceMode();
void initCaptors();
void initGPIODir();
......