void SensorsTask(void)
{
// Array storing the status of each physical sensor.
// If a sensor fails to initialize, or fails 3 sensor reads in a row,
// it will be disabled here until the next system reboot
// 0 = tmp0
// 1 = tmp1
// 2 = tmp3
// 3 = humidity / air temp
// 4 = pressure
// 5 = accelerometer
uint8_t enabledSensors[7] = {3, 3, 3, 3, 3, 3, 3};
uint8_t i;
I2C_Status retVal = I2C_OK;
INFO("(SENSORS_TASK) I2C Sensor failed to initialize\r\n");
for(i = 0; i < 3; i++)
{
// If the temperature sensor initialized, set its enabled value to 3 (so it has 3 chances to respond to a read request)
// Else, disable the sensor
if(InitTempSensor(i) != I2C_OK)
{
I2C_Reset(SLB_I2C);
enabledSensors[i] = 0;
WARN("(SENSORS_TASK) I2C Sensor failed to initialize\r\n");
}
}
if(InitHumiditySensor() != I2C_OK)
{
I2C_Reset(ALB_I2C);
enabledSensors[3] = 0;
WARN("(SENSORS_TASK) Humidity sensor failed to initialize\r\n");
}
if(InitPressureSensor() != I2C_OK)
{
I2C_Reset(ALB_I2C);
enabledSensors[4] = 0;
WARN("(SENSORS_TASK) Pressure sensor failed to initialize\r\n");
}
if(InitAccelerometer() != I2C_OK)
{
I2C_Reset(ALB_I2C);
enabledSensors[5] = 0;
WARN("(SENSORS_TASK) Accelerometer failed to initialize\r\n");
}
// Let other tasks in the system warmup before entering the sensor polling loop
osDelay(2000);
// TODO: re-initialize the sensors once a day to check for failures and for sensors that have come back online
// TODO: report to the base station when a sensor fails
while(1)
{
for(i = 0; i < 3; i++)
{
if(enabledSensors[i] > 0)
{
switch(i)
{
case 0:
retVal = ReadTempSensor(0, &sensorData.temp0);
break;
case 1:
retVal = ReadTempSensor(1, &sensorData.temp1);
break;
case 2:
retVal = ReadTempSensor(2, &sensorData.temp2);
break;
default:
retVal = ReadTempSensor(0, &sensorData.temp0);
break;
}
// If the sensor read failed, indicate that the sensor has one less chance to respond correctly before being disabled
if(retVal != I2C_OK)
{
I2C_Reset(SLB_I2C);
enabledSensors[i]--;
WARN("(SENSORS_TASK) Temp sensor read failed\r\n");
}
// The sensor is still alive! Restore it to a full 3 chances to respond
else if(enabledSensors[i] != 3)
{
enabledSensors[i] = 3;
DEBUG("(SENSORS_TASK) Temp sensor connection restored\r\n");
}
}
}
if(enabledSensors[3] > 0)
{
do {
if(ReadHumiditySensor(&sensorData.humid) != I2C_OK)
{
I2C_Reset(ALB_I2C);
enabledSensors[3]--;
WARN("(SENSORS_TASK) Humidity sensor read failed\r\n");
break;
}
else if(enabledSensors[3] != 3)
{
enabledSensors[3] = 3;
DEBUG("(SENSORS_TASK) Humidity sensor connection restored\r\n");
}
if(ReadAirTempSensor(&sensorData.tempAir) != I2C_OK)
{
I2C_Reset(ALB_I2C);
enabledSensors[3]--;
WARN("(SENSORS_TASK) Air temp sensor read failed\r\n");
}
else if(enabledSensors[3] != 3)
{
enabledSensors[3] = 3;
DEBUG("(SENSORS_TASK) Air temp sensor connection restored\r\n");
}
}
while(0);
}
if(enabledSensors[4] > 0)
{
if(ReadPressureSensor(&sensorData.alt) != I2C_OK)
{
I2C_Reset(ALB_I2C);
enabledSensors[4]--;
WARN("(SENSORS_TASK) Altimeter sensor read failed\r\n");
}
}
if(enabledSensors[5] > 0)
{
uint16_t x, y, z;
if(ReadAccelerometer(&x, &y, &z) != I2C_OK)
{
I2C_Reset(ALB_I2C);
enabledSensors[5]--;
WARN("(SENSORS_TASK) Accelerometer sensor read failed\r\n");
}
DEBUG("X: %d, Y: %d, Z: %d", x, y, z);
}
ReadSoilMoisture(&sensorData.moist0, &sensorData.moist1, &sensorData.moist2);
// Send sensor Data to the base station
SendSensorData();
osDelay(pollingRate);
}
}
int main(void)
{
int LogOn = 0;
int ComOn = 0;
int dT;
int16_t data[32];
uint32_t x;
UINT cnt;
InitSystemTick();
InitGPIO();
InitTimer();
STM_EVAL_LEDOn(LED4);
InitUART(115200);
InitPressureSensor();
InitFlowMeter();
InitAccAndMag();
InitGyro();
STM_EVAL_LEDOn(LED7); // zapnem LED7 - zelena
Delta_us();
while(1)
{
// sample period is time elapsed since previous sampling of sensors
sampleSensors(a,m,g);
// update timebase for next time
dT = Delta_us();
samplePeriod = 0.000001f * dT;
// convert gyro deg/s to rad/s
imuDegToRadV3(g);
// update AHRS
MadgwickFullAHRSUpdate(g, a, m, samplePeriod, quaternion);
if (LogOn) {
sprintf(text, "%5d%6d%6d%6d%7d%7d%7d%5d%5d%5d\r\n",
dT,
aRawData[0], aRawData[1], aRawData[2],
gRawData[0], gRawData[1], gRawData[2],
mRawData[0], mRawData[1], mRawData[2]);
f_write(&File, text, strlen(text), &cnt);
}
if(STM_EVAL_PBGetState(BUTTON_USER)) { // zmena rezimu vystupu
if (LogOn) {
// Stop logdata
LogOn = 0;
STM_EVAL_LEDOff(LED5); // zapnem LED7 - zelena
if (f_close(&File) == 0)
xprintf("Stop login data.\n\r");
else
xprintf("Error write datafile.\n\r");
} else {
// Start logdata
if (newFile() == 0)
xprintf("Start login data.\n\r");
else
xprintf("Error create datafile.\n\r");
STM_EVAL_LEDOn(LED5); // zapnem LED7 - zelena
LogOn = 1;
}
}
if (ComOn) {
data[0] = dT;
data[1] = aRawData[0];
data[2] = aRawData[1];
data[3] = aRawData[2];
data[4] = gRawData[0];
data[5] = gRawData[1];
data[6] = gRawData[2];
data[7] = mRawData[0];
data[8] = mRawData[1];
data[9] = mRawData[2];
data[10] = 0x8080;
SendDataUART((uint8_t*) data, 22);
}
if (IsReceiveUART()) {
if (ReadByteUART() == 's') {
STM_EVAL_LEDOn(LED10); // zapnem LED7 - zelena
ComOn = 1;
} else {
STM_EVAL_LEDOff(LED10); // zapnem LED7 - zelena
ComOn = 0;
}
}
/*
if (x = GetFlowMeter()) {
printf("Flow: %d\n\r", x);
}
*/
}
}
