Commit 7c60cc86 authored by Louckas's avatar Louckas

[FEA]balise: Added support of timeout on captors, each captors is now managed...

[FEA]balise: Added support of timeout on captors, each captors is now managed by a state machine that will disable it if too many timeout happen. This allow the system to dynamically remove faulty captor and keep on working
parent 5d76985f
......@@ -27,30 +27,34 @@ Balise::Balise()
for (uint16_t i = 0; i < NB_OF_CAPTOR_PER_I2C; ++i)
{
if(Enable_Captor[i + 8])
muxMSG[i] = {&muxCh[i], 1, 0xE0, MsgType::Write, nullptr};
if(EnableCaptor[i + 8])
{
mux1MSG[i] = {&mux1Ch[i], 1, 0xE0, MsgType::Write, nullptr};
vl53l1x_i2c1[i] = new VL53l1x(_i2c1);
}
if(Enable_Captor[i + 16])
if(EnableCaptor[i + 16])
{
mux2MSG[i] = {&mux2Ch[i], 1, 0xE0, MsgType::Write, nullptr};
vl53l1x_i2c2[i] = new VL53l1x(_i2c2);
}
if(Enable_Captor[i])
if(EnableCaptor[i])
{
mux3MSG[i] = {&mux3Ch[i], 1, 0xE0, MsgType::Write, nullptr};
vl53l1x_i2c3[i] = new VL53l1x(_i2c3);
}
}
for(uint16_t i = 0; i < TOTAL_NB_CAPTOR; ++i)
{
measuredDistance[i] = 1000;
measuredDistance[i] = 1300;
vl53l1xRangeStatus[i] = 0x4;
vl53l1xStatus[i] = VL53l1x::Received;
vl53l1xDistanceMSGStatus[i] = VL53l1x::Received;
vl53l1xRangeMSGStatus[i] = VL53l1x::Received;
captorState[i] = CaptorStatus::Start;
captorTimeoutCount[i] = 0;
}
_u7GPIO = new MCP23S17(_spi, CP0_GPIO_Port, CP0_Pin);
......@@ -91,53 +95,9 @@ void Balise::launch()
{
ctI2C = 0;
for(uint16_t i = 0; i < NB_OF_CAPTOR_PER_I2C; ++i)
{
if(Enable_Captor[i + 8])
{
if(vl53l1xStatus[8 + i] == VL53l1x::Received)
_i2c1->sendMessage(mux1MSG[i]);
vl53l1xStatus[8 + i] = vl53l1x_i2c1[i]->getDistance(measuredDistance[8 + i]);
vl53l1x_i2c1[i]->getRangeStatus(vl53l1xRangeStatus[8 + i]);
}
if(Enable_Captor[i + 16])
{
if(vl53l1xStatus[16 + i] == VL53l1x::Received)
_i2c2->sendMessage(mux1MSG[i]);
vl53l1xStatus[16 + i] = vl53l1x_i2c2[i]->getDistance(measuredDistance[16 + i]);
vl53l1x_i2c2[i]->getRangeStatus(vl53l1xRangeStatus[16 + i]);
}
if(Enable_Captor[i])
{
if(vl53l1xStatus[0 + i] == VL53l1x::Received)
_i2c3->sendMessage(mux1MSG[i]);
vl53l1xStatus[0 + i] = vl53l1x_i2c3[i]->getDistance(measuredDistance[0 + i]);
vl53l1x_i2c3[i]->getRangeStatus(vl53l1xRangeStatus[0 + i]);
}
}
for(uint16_t i = 0; i < TOTAL_NB_CAPTOR; ++i)
{
if(!((vl53l1xRangeStatus[i] & 0x4) > 0) && Enable_Captor[i])
{
if(measuredDistance[i] < DISTANCE_TO_TRIG)
{
capteurState[i] = true;
}
else
{
capteurState[i] = false;
}
}
else
{
capteurState[i] = false;
}
setGPIOEvitement();
}
getVl53l1Distance();
setGPIOEvitement();
_i2c1->update();
_i2c2->update();
......@@ -164,7 +124,7 @@ void Balise::tick()
void Balise::setGPIOEvitement()
{
if(capteurState[23] || capteurState[0] || capteurState[1])
if(obstacle[23] || obstacle[0] || obstacle[1])
{
HAL_GPIO_WritePin(Evit0_GPIO_Port, Evit0_Pin, GPIO_PIN_SET);
}
......@@ -173,7 +133,7 @@ void Balise::setGPIOEvitement()
HAL_GPIO_WritePin(Evit0_GPIO_Port, Evit0_Pin, GPIO_PIN_RESET);
}
if(capteurState[2] || capteurState[3] || capteurState[4])
if(obstacle[2] || obstacle[3] || obstacle[4])
{
HAL_GPIO_WritePin(Evit1_GPIO_Port, Evit1_Pin, GPIO_PIN_SET);
}
......@@ -182,7 +142,7 @@ void Balise::setGPIOEvitement()
HAL_GPIO_WritePin(Evit1_GPIO_Port, Evit1_Pin, GPIO_PIN_RESET);
}
if(capteurState[5] || capteurState[6] || capteurState[7])
if(obstacle[5] || obstacle[6] || obstacle[7])
{
HAL_GPIO_WritePin(Evit2_GPIO_Port, Evit2_Pin, GPIO_PIN_SET);
}
......@@ -191,7 +151,7 @@ void Balise::setGPIOEvitement()
HAL_GPIO_WritePin(Evit2_GPIO_Port, Evit2_Pin, GPIO_PIN_RESET);
}
if(capteurState[8] || capteurState[9] || capteurState[10])
if(obstacle[8] || obstacle[9] || obstacle[10])
{
HAL_GPIO_WritePin(Evit3_GPIO_Port, Evit3_Pin, GPIO_PIN_SET);
}
......@@ -200,7 +160,7 @@ void Balise::setGPIOEvitement()
HAL_GPIO_WritePin(Evit3_GPIO_Port, Evit3_Pin, GPIO_PIN_RESET);
}
if(capteurState[11] || capteurState[12] || capteurState[13])
if(obstacle[11] || obstacle[12] || obstacle[13])
{
HAL_GPIO_WritePin(Evit4_GPIO_Port, Evit4_Pin, GPIO_PIN_SET);
}
......@@ -209,7 +169,7 @@ void Balise::setGPIOEvitement()
HAL_GPIO_WritePin(Evit4_GPIO_Port, Evit4_Pin, GPIO_PIN_RESET);
}
if(capteurState[14] || capteurState[15] || capteurState[16])
if(obstacle[14] || obstacle[15] || obstacle[16])
{
HAL_GPIO_WritePin(Evit5_GPIO_Port, Evit5_Pin, GPIO_PIN_SET);
}
......@@ -218,7 +178,7 @@ void Balise::setGPIOEvitement()
HAL_GPIO_WritePin(Evit5_GPIO_Port, Evit5_Pin, GPIO_PIN_RESET);
}
if(capteurState[17] || capteurState[18] || capteurState[19])
if(obstacle[17] || obstacle[18] || obstacle[19])
{
HAL_GPIO_WritePin(Evit6_GPIO_Port, Evit6_Pin, GPIO_PIN_SET);
}
......@@ -227,7 +187,7 @@ void Balise::setGPIOEvitement()
HAL_GPIO_WritePin(Evit6_GPIO_Port, Evit6_Pin, GPIO_PIN_RESET);
}
if(capteurState[20] || capteurState[21] || capteurState[22])
if(obstacle[20] || obstacle[21] || obstacle[22])
{
HAL_GPIO_WritePin(Evit7_GPIO_Port, Evit7_Pin, GPIO_PIN_SET);
}
......@@ -241,25 +201,40 @@ void Balise::initDistanceMode()
{
for(uint16_t i = 0; i < NB_OF_CAPTOR_PER_I2C; ++i)
{
if(Enable_Captor[i + 8])
if(EnableCaptor[i + 8])
{
_i2c1->sendMessage(mux1MSG[i]);
_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;
}
if(Enable_Captor[i + 16])
if(EnableCaptor[i + 16])
{
_i2c2->sendMessage(mux1MSG[i]);
_i2c2->sendMessage(muxMSG[i]);
vl53l1x_i2c2[i]->init();
vl53l1x_i2c2[i]->setDistanceMode(VL53l1x::VL53L1DistanceMode::ShortMode);
captorState[i + 16] = CaptorStatus::Ready;
}
else
{
captorState[i + 16] = CaptorStatus::Disabled;
}
if(Enable_Captor[i])
if(EnableCaptor[i])
{
_i2c3->sendMessage(mux1MSG[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;
}
}
}
......@@ -318,14 +293,127 @@ void Balise::blinkLeds()
}
}
void Balise::getVl53l1Distance()
{
for(uint16_t i = 0; i < NB_OF_CAPTOR_PER_I2C; ++i)
{
if(EnableCaptor[i + 8])
{
runAcquisitionStateMachine(_i2c1, vl53l1x_i2c1[i], i, i + 8);
}
if(EnableCaptor[i + 16])
{
runAcquisitionStateMachine(_i2c2, vl53l1x_i2c2[i], i, i + 16);
}
if(EnableCaptor[i])
{
runAcquisitionStateMachine(_i2c3, vl53l1x_i2c3[i], i, i);
}
}
}
void Balise::runAcquisitionStateMachine(I2C *i2c, VL53l1x *vl, uint8_t captorID8, uint8_t captorID24)
{
switch(captorState[captorID24])
{
case CaptorStatus::Ready:
{
i2c->sendMessage(muxMSG[captorID8]);
vl53l1xDistanceMSGStatus[captorID24] = vl->getDistance(measuredDistance[captorID24]);
vl53l1xRangeMSGStatus[captorID24] = vl->getRangeStatus(vl53l1xRangeStatus[captorID24]);
captorState[captorID24] = CaptorStatus::GetDistance;
}
break;
case CaptorStatus::GetDistance:
{
vl53l1xDistanceMSGStatus[captorID24] = vl->getDistance(measuredDistance[captorID24]);
if (vl53l1xDistanceMSGStatus[captorID24] == VL53l1x::VL53L1Status::Received)
captorState[captorID24] = CaptorStatus::GetStatus;
else if(vl53l1xDistanceMSGStatus[captorID24] == VL53l1x::VL53L1Status::Error)
captorState[captorID24] = CaptorStatus::Timeout;
}
break;
case CaptorStatus::GetStatus:
{
vl53l1xRangeMSGStatus[captorID24] = vl->getRangeStatus(vl53l1xRangeStatus[captorID24]);
if (vl53l1xRangeMSGStatus[captorID24] == VL53l1x::VL53L1Status::Received)
captorState[captorID24] = CaptorStatus::AcquisitionDone;
else if(vl53l1xRangeMSGStatus[captorID24] == VL53l1x::VL53L1Status::Error)
captorState[captorID24] = CaptorStatus::Timeout;
}
break;
case CaptorStatus::AcquisitionDone:
{
updateCaptorState(captorID24);
i2c->sendMessage(muxMSG[captorID8]);
vl53l1xDistanceMSGStatus[captorID24] = vl->getDistance(measuredDistance[captorID24]);
vl53l1xRangeMSGStatus[captorID24] = vl->getRangeStatus(vl53l1xRangeStatus[captorID24]);
captorState[captorID24] = CaptorStatus::GetDistance;
}
break;
case CaptorStatus::Timeout:
{
captorTimeoutCount[captorID24]++;
if(captorTimeoutCount[captorID24] > 3)
{
captorState[captorID24] = CaptorStatus::Disabled;
EnableCaptor[captorID24] = false;
}
else
{
i2c->sendMessage(muxMSG[captorID8]);
vl53l1xDistanceMSGStatus[captorID24] = vl->getDistance(measuredDistance[captorID24]);
vl53l1xRangeMSGStatus[captorID24] = vl->getRangeStatus(vl53l1xRangeStatus[captorID24]);
captorState[captorID24] = CaptorStatus::GetDistance;
}
}
break;
default:
break;
}
}
void Balise::updateCaptorState(uint8_t captor)
{
if(!((vl53l1xRangeStatus[captor] & 0x4) > 0) && EnableCaptor[captor])
{
if(measuredDistance[captor] < DISTANCE_TO_TRIG)
{
obstacle[captor] = true;
}
else
{
obstacle[captor] = false;
}
}
else
{
obstacle[captor] = false;
}
}
void Balise::updateLedState()
{
for(uint16_t i = 0; i < TOTAL_NB_CAPTOR; ++i)
{
if(capteurState[i])
if(obstacle[i])
{
ledState[i * 2] = 0x01;
if(capteurState[(i + 1) % 24])
if(obstacle[(i + 1) % 24])
{
ledState[(i * 2) + 1] = 0x01;
}
......
......@@ -19,18 +19,7 @@
#define TOTAL_NB_CAPTOR 24
#define DISTANCE_TO_TRIG 600
constexpr bool Enable_Captor[TOTAL_NB_CAPTOR] =
{
true, true, //Avant
true, true, true, // Avant - droit
true, true, true, // Droite
true, true, true, // Arriere - droit
true, true, true, // Arriere
true, true, true, // Arriere - gauche
true, true, true, // Gauche
true, true, true, // Avant - gauche
true //Avant
};
class Balise {
public:
......@@ -40,6 +29,19 @@ public:
void launch();
void tick();
enum CaptorStatus
{
Start =0,
Init,
SetDistanceMode,
Ready,
GetDistance,
GetStatus,
AcquisitionDone,
Timeout,
Disabled
};
private:
void initDistanceMode();
......@@ -48,35 +50,41 @@ private:
void blinkLeds();
void getVl53l1Distance();
void updateCaptorState(uint8_t captor);
void runAcquisitionStateMachine(I2C *i2c, VL53l1x *vl, uint8_t captorID8, uint8_t captorID24);
void updateLedState();
void setGPIOEvitement();
I2C* _i2c1 {nullptr};
I2C* _i2c2 {nullptr};
I2C* _i2c3 {nullptr};
I2C *_i2c1 {nullptr};
I2C *_i2c2 {nullptr};
I2C *_i2c3 {nullptr};
SPI* _spi {nullptr};
SPI *_spi {nullptr};
MCP23S17* _u7GPIO {nullptr};
MCP23S17* _u8GPIO {nullptr};
MCP23S17* _u9GPIO {nullptr};
MCP23S17 *_u7GPIO {nullptr};
MCP23S17 *_u8GPIO {nullptr};
MCP23S17 *_u9GPIO {nullptr};
VL53l1x* vl53l1x_i2c2[8] {nullptr};
VL53l1x* vl53l1x_i2c1[8] {nullptr};
VL53l1x* vl53l1x_i2c3[8] {nullptr};
VL53l1x *vl53l1x_i2c2[8] {nullptr};
VL53l1x *vl53l1x_i2c1[8] {nullptr};
VL53l1x *vl53l1x_i2c3[8] {nullptr};
uint8_t mux1Ch[NB_OF_CAPTOR_PER_I2C] = {0x08, 0x04, 0x02, 0x01, 0x80, 0x40, 0x20, 0x10};
uint8_t mux2Ch[NB_OF_CAPTOR_PER_I2C] = {0x08, 0x04, 0x02, 0x01, 0x80, 0x40, 0x20, 0x10};
uint8_t mux3Ch[NB_OF_CAPTOR_PER_I2C] = {0x08, 0x04, 0x02, 0x01, 0x80, 0x40, 0x20, 0x10};
uint8_t muxCh[NB_OF_CAPTOR_PER_I2C] = {0x08, 0x04, 0x02, 0x01, 0x80, 0x40, 0x20, 0x10};
i2c_message_t mux1MSG[NB_OF_CAPTOR_PER_I2C];
i2c_message_t mux2MSG[NB_OF_CAPTOR_PER_I2C];
i2c_message_t mux3MSG[NB_OF_CAPTOR_PER_I2C];
i2c_message_t muxMSG[NB_OF_CAPTOR_PER_I2C];
uint16_t measuredDistance[TOTAL_NB_CAPTOR] {0};
uint8_t vl53l1xRangeStatus[TOTAL_NB_CAPTOR] {0};
VL53l1x::VL53L1Status vl53l1xStatus[TOTAL_NB_CAPTOR] { };
VL53l1x::VL53L1Status vl53l1xDistanceMSGStatus[TOTAL_NB_CAPTOR] { };
VL53l1x::VL53L1Status vl53l1xRangeMSGStatus[TOTAL_NB_CAPTOR] { };
Balise::CaptorStatus captorState[TOTAL_NB_CAPTOR] { };
uint8_t captorTimeoutCount[TOTAL_NB_CAPTOR] {0};
uint8_t u7PortA {0};
uint8_t u7PortB {0};
......@@ -88,7 +96,20 @@ private:
uint8_t u9PortB {0xFF};
uint8_t ledState[TOTAL_NB_CAPTOR * 2] {0};
bool capteurState[TOTAL_NB_CAPTOR] {false};
bool obstacle[TOTAL_NB_CAPTOR] {false};
bool EnableCaptor[TOTAL_NB_CAPTOR] =
{
true, true, //Avant
true, true, true, // Avant - droit
true, true, true, // Droite
true, true, true, // Arriere - droit
true, true, true, // Arriere
true, true, true, // Arriere - gauche
true, true, true, // Gauche
true, true, true, // Avant - gauche
true //Avant
};
uint8_t _flagUpdate {0};
uint32_t _globalClock {0};
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment