/****************************************************************************************************
* @fn I2CCommTask
* This tasks primary goal is to serialize the communication request (sensor results) going
* over I2C
*
* @param none
*
* @return none
*
***************************************************************************************************/
ASF_TASK void I2CCommTask( ASF_TASK_ARG )
{
MessageBuffer *rcvMsg = NULLP;
/* I2C Slave mode initialization */
I2C_Slave_init();
/* Init register area for slave */
SH_Slave_init();
while(1)
{
ASFReceiveMessage(I2CSLAVE_COMM_TASK_ID, &rcvMsg );
switch (rcvMsg->msgId)
{
case MSG_ACC_DATA:
SendSensorData(AP_PSENSOR_ACCELEROMETER_RAW, &rcvMsg->msg.msgAccelData);
break;
case MSG_MAG_DATA:
SendSensorData(AP_PSENSOR_MAGNETIC_FIELD_RAW, &rcvMsg->msg.msgMagData);
break;
case MSG_GYRO_DATA:
SendSensorData(AP_PSENSOR_GYROSCOPE_RAW, &rcvMsg->msg.msgGyroData);
break;
default:
D1_printf("I2C:!!!UNHANDLED MESSAGE:%d!!!\r\n", rcvMsg->msgId);
break;
}
}
}
void loop() {
if(g_bConnected) {
g_servo.loop();
if(0<Serial.available()) {
auto readcmd = ReadCommand();
if(readcmd.m_bValid) {
HandleCommand(readcmd.m_cmd);
if(!g_bConnected) return;
}
} else if(c_nTIMETODISCONNECT < millis()-g_nLastCommand) {
OnDisconnection();
return;
} else if(c_nTIMETOSTOP < millis()-g_nLastCommand) {
InternalHandleCommand(SRobotCommand::stop());
}
for(unsigned int i=0; i<countof(g_amotors); ++i) {
g_amotors[i].ComputePID(g_apid[i]);
}
SendSensorData();
} else {
auto readcmd = ReadCommand();
if(readcmd.m_bValid && readcmd.m_cmd.m_ecmd==ecmdCONNECT) {
HandleCommand(readcmd.m_cmd);
}
}
}
// This function sends data that has been requested.
void SendPendingMKSerial(void)
{
// Handle only one request at a time.
if (tx_request_)
{
// A one-time request has higher priority than a periodic "stream" of data.
if (tx_request_ & MK_TX_VERSION) SendVersion();
}
else if (mk_stream_ && TimestampInPast(stream_timer_))
{
// A data stream is active and it is time for another transmission.
switch (mk_stream_)
{
case MK_STREAM_CONTROL:
SendControlData();
break;
case MK_STREAM_KALMAN:
SendKalmanData();
break;
case MK_STREAM_MOTOR_SETPOINTS:
SendMotorSetpoints();
break;
case MK_STREAM_SENSORS:
SendSensorData();
break;
default:
break;
}
stream_timer_ += stream_period_;
// Prevent timer rollover for small periods.
if (TimestampInPast(stream_timer_)) stream_timer_ = GetTimestamp();
// Disable the stream if no request has been renewing received in a while.
if (TimestampInPast(stream_timeout_)) mk_stream_ = MK_STREAM_NONE;
}
}
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);
}
}
