int32_t ReadSensors(AxisSensors Axiss)
{
static int32_t readValue=0;
switch(Axiss)
{
case xAxis:
readValue=ReadAccelerometer(XAxis);
break;
case yAxis:
readValue=ReadAccelerometer(YAxis);
break;
case zAxis:
readValue=ReadAccelerometer(ZAxis);
default:
break;
}
return readValue;
}
/****************************************************************************
Function:
void __attribute__((interrupt, shadow, auto_psv)) _T4Interrupt(void)
Description:
This function updates the tick count and calls ReadAccelerometer()
to monitor the accelerometer tri-axial outputs.
Precondition:
Timer 4 and the Timer 4 interrupt must be enabled in order for
this function to execute. AccelerometerInit() must be called before
Timer 4 and the Timer 4 interrupt are enabled.
Parameters:
None
Returns:
None
Remarks:
None
***************************************************************************/
void __attribute__((interrupt, shadow, auto_psv)) _T1Interrupt(void)
{
// Clear flag
IFS0bits.T1IF = 0;
tick++;
// Read accelerometer data
ReadAccelerometer();
}
/*!
Calls tests in order:
1. Test 1
2. ReadAccelerometer
3. Test 2
There were various test cases which we did in the demo. We simply commented out
each test case instead of deleting them.
*/
int viterbi_update(float* data, int data_len)
{
int EstimatedStates[data_len+1];
int nObs;
int Observations[data_len];
// printf("Accelerometer\n");
// printf("=============\n");
ReadAccelerometer(data, data_len, Observations, &nObs);
// printf("ESTIMATED STATES TEST 1\n");
// printf("=======================\n");
Viterbi_C(Observations, nObs, EstimatedStates, &hmm);
return 0;
}
void DemoAccelerometer()
{
float roll, pitch;
/* while user button is not pressed */
while (KeyPressCount()==0)
{
ReadAccelerometer(AccBuffer);
ComputePitchAndRollFromAcc( AccBuffer[0], AccBuffer[1], AccBuffer[2], &pitch, &roll );
printf("A: %7.2f %7.2f %7.2f PR=%7.2f,%7.2f\r\n", AccBuffer[0], AccBuffer[1], AccBuffer[2], pitch, roll );
SetLed(ALL|OFF);
if ((fabs(roll) <= 70.0f) && (fabs(pitch) <= 70.0f))
{
if( pitch > 25 )
SetLed(L1);
if( pitch < -25 )
SetLed(L5);
if( roll > 25 )
SetLed(L3);
if( roll < -25 )
SetLed(L7);
if( pitch > 25 && roll > 25 )
SetLed(L2);
if( pitch < -25 && roll > 25 )
SetLed(L8);
if( pitch < -25 && roll < -25 )
SetLed(L6);
if( pitch < -25 && roll > 25 )
SetLed(L4);
}
else
{
SetLed(ALL|TOGGLE);
/* Delay 50ms */
Delay(5);
}
}
}
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);
}
}
void DemoMagnetometer()
{
__IO float HeadingValue = 0.0f;
int led=L1;
/* Waiting User Button is pressed */
while (KeyPressCount()==0)
{
/* Wait for data ready */
Delay(5);
/* Read Compass data */
ReadMagnetometer(MagBuffer);
ReadAccelerometer(AccBuffer);
ComputePitchAndRoll();
/* tilt correction */
fTiltedX = MagBuffer[0]*fCosPitch+MagBuffer[2]*fSinPitch;
fTiltedY = MagBuffer[0]*fSinRoll*fSinPitch+MagBuffer[1]*fCosRoll-MagBuffer[1]*fSinRoll*fCosPitch;
HeadingValue = (float) ((atan2f((float)fTiltedY,(float)fTiltedX))*180)/M_PI;
if (HeadingValue < 0)
{
HeadingValue = HeadingValue + 360;
}
if ((RollAng <= 40.0f) && (PitchAng <= 40.0f))
{
if (((HeadingValue < 25.0f)&&(HeadingValue >= 0.0f))||((HeadingValue >=340.0f)&&(HeadingValue <= 360.0f)))
{
led=LED10;
}
else if ((HeadingValue <70.0f)&&(HeadingValue >= 25.0f))
{
led=LED9;
}
else if ((HeadingValue < 115.0f)&&(HeadingValue >= 70.0f))
{
led=LED7;
}
else if ((HeadingValue <160.0f)&&(HeadingValue >= 115.0f))
{
led=LED5;
}
else if ((HeadingValue <205.0f)&&(HeadingValue >= 160.0f))
{
led=LED3;
}
else if ((HeadingValue <250.0f)&&(HeadingValue >= 205.0f))
{
led=LED4;
}
else if ((HeadingValue < 295.0f)&&(HeadingValue >= 250.0f))
{
led=LED6;
}
else if ((HeadingValue < 340.0f)&&(HeadingValue >= 295.0f))
{
led=LED8;
}
SetLed(ALL|OFF);
SetLed(led);
}
else
{
SetLed(ALL|TOGGLE);
/* Delay 50ms */
Delay(5);
}
}
}
