/**
* Create a new instance of Accelerometer from an existing AnalogInput.
*
* Make a new instance of accelerometer given an AnalogInput. This is
* particularly useful if the port is going to be read as an analog channel as
* well as through the Accelerometer class.
*
* @param channel The existing AnalogInput object for the analog input the
* accelerometer is connected to
*/
AnalogAccelerometer::AnalogAccelerometer(std::shared_ptr<AnalogInput> channel)
: m_analogInput(channel) {
if (channel == nullptr) {
wpi_setWPIError(NullParameter);
} else {
InitAccelerometer();
}
}
/**
* Create a new instance of Accelerometer from an existing AnalogInput.
*
* Make a new instance of accelerometer given an AnalogInput. This is
* particularly useful if the port is going to be read as an analog channel as
* well as through the Accelerometer class.
*
* @param channel The existing AnalogInput object for the analog input the
* accelerometer is connected to
*/
AnalogAccelerometer::AnalogAccelerometer(AnalogInput* channel)
: m_analogInput(channel, NullDeleter<AnalogInput>()) {
if (channel == nullptr) {
wpi_setWPIError(NullParameter);
} else {
InitAccelerometer();
}
}
void HandleAccelerometer(tMessage *pMsg)
{
if (Control == INIT_MODE) InitAccelerometer();
switch (pMsg->Options)
{
case MSG_OPT_ACCEL_DATA:
AccelerometerSendDataHandler();
break;
case MSG_OPT_ACCEL_ENABLE:
if (!(Control & PC1_OPERATING_MODE)) EnableAccelerometer();
break;
case MSG_OPT_ACCEL_DISABLE:
if (Control & PC1_OPERATING_MODE) DisableAccelerometer();
break;
case MSG_OPT_ACCEL_STREAMING:
ENTER_STANDBY_MODE();
Control &= ~WUF_ENABLE;
Control |= DRDYE_DATA_AVAILABLE;
AccelerometerWrite(KIONIX_CTRL_REG1, &Control, ONE_BYTE);
ENTER_OPERATING_MODE();
break;
case MSG_OPT_ACCEL_WUF:
ENTER_STANDBY_MODE();
Control &= ~DRDYE_DATA_AVAILABLE;
Control |= WUF_ENABLE;
AccelerometerWrite(KIONIX_CTRL_REG1, &Control, ONE_BYTE);
ENTER_OPERATING_MODE();
break;
case MSG_OPT_ACCEL_WUF_THRESHOLD:
ENTER_STANDBY_MODE();
*Data = *pMsg->pBuffer;
AccelerometerWrite(KIONIX_WUF_THRESH, Data, ONE_BYTE);
ENTER_OPERATING_MODE();
break;
case MSG_OPT_ACCEL_G_RANGE:
ENTER_STANDBY_MODE();
Control &= ~ACCEL_RANGE_MASK;
Control |= *pMsg->pBuffer << ACCEL_RANGE_SHFT;
AccelerometerWrite(KIONIX_CTRL_REG1, &Control, ONE_BYTE);
ENTER_OPERATING_MODE();
break;
default:
break;
}
}
void InitSensors(IdSensors Name)
{
switch(Name)
{
case mpu:
InitAccelerometer(Mpu);
break;
default:
break;
}
}
/**
* Create a new instance of Accelerometer from an existing AnalogChannel.
* Make a new instance of accelerometer given an AnalogChannel. This is particularly
* useful if the port is going to be read as an analog channel as well as through
* the Accelerometer class.
*/
Accelerometer::Accelerometer(AnalogChannel *channel)
{
if (channel == NULL)
{
wpi_setWPIError(NullParameter);
}
else
{
m_analogChannel = channel;
InitAccelerometer();
}
m_allocatedChannel = false;
}
/**
* Create a new instance of Accelerometer from an existing AnalogChannel.
* Make a new instance of accelerometer given an AnalogChannel. This is particularly
* useful if the port is going to be read as an analog channel as well as through
* the Accelerometer class.
*/
FilteredAccelerometer::FilteredAccelerometer(AnalogChannel *channel)
{
if (channel == NULL)
{
wpi_fatal(NullParameter);
}
else
{
m_analogChannel = channel;
InitAccelerometer();
}
m_allocatedChannel = false;
}
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)
{
DDRD = 0b01110000;
DDRA = 0b00000001;
//wdt_reset();
//wdt_disable();
char replyDat[2];
i2c_init();
USART roverPort;
USART devicePort;
CommInterface inf;
CommPacket commPkt;
char commData[20];
char commRet;
Timer accelTimer;
Timer dataTimer;
int sensorError;
//PORTA = 1;
//_delay_ms(250);
//PORTA = 0;
//_delay_ms(250);
USART_InitPortStructs();
setDeviceCamera();
USART_Open(&roverPort, 0, USART_BAUD_38400, 20, 32, true);
CommInterfaceInit(&inf, &roverPort);
USART_Open(&devicePort, 1, USART_BAUD_9600, 20, 5, false);
setDeviceServo();
sensorError = InitAccelerometer(&inf);
sensorError = InitBarometer(&inf);
InitServos(&devicePort);
InitCamera(&devicePort);
InitTimers();
//StartTimer(&accelTimer);
//StartTimer(&dataTimer);
setDeviceServo();
PORTA |= 1;
PanTiltSetPosition(TILT_SERVO, TILT_CENTER);
PanTiltSetPosition(PAN_SERVO, PAN_CENTER);
char pingRespond = 0xAA;
commPkt.data = commData;
CommPacket pkt;
char textBuffer[40];
char length;
unsigned char aclBuf[6];
//ServiceBarometer(3);
//Enable Watchdog
//wdt_enable(0b111);
while(1) {
while(CommRXPacketsAvailable(&inf)) {
commRet = CommGetPacket(&inf, &commPkt, 20);
if (!commRet) // didn't get packet returned, even though it said we had one. give up for now
break;
if (commPkt.length==0) { // bad 0-length packet, skip to the next one
continue;
}
// must at least have a length of 1, check the first byte to see what device this is targeted at
if (commPkt.data[0]==SYS_PANTILT) {
PORTA &= ~1;
setDeviceServo();
PanTiltHandlePacket(&commPkt);
PORTA = sensorError;
}
else if (commPkt.data[0]==SYS_CAMERA) {
PORTA &= ~1;
setDeviceCamera();
CameraHandlePacket(&commPkt);
PORTA = sensorError;
}
else if (commPkt.data[0]==SYS_BAROMETER) {
PORTA &= ~1;
BarometerHandlePacket(&commPkt);
PORTA = sensorError;
}
else if (commPkt.data[0]==SYS_ACCEL) {
PORTA &= ~1;
AccelHandlePacket(&commPkt);
PORTA = sensorError;
}
else if (commPkt.data[0]==SYS_PING)
{
pkt.target = 1;
pkt.length = 1;
pkt.data = &pingRespond;
CommSendPacket(&inf,&pkt);
}
}
/*
if (GetSpanUs(&dataTimer) > FROM_uS(1000000))
{
USART_WriteByte(&roverPort,0xFE);
USART_WriteByte(&roverPort,temperatureData>>8);
USART_WriteByte(&roverPort,temperatureData&0xFF);
USART_WriteByte(&roverPort,barometerData>>16);
USART_WriteByte(&roverPort,(barometerData>>8)&0xFF);
USART_WriteByte(&roverPort,barometerData&0xFF);
AccelGetAverage(aclBuf);
length = sprintf(textBuffer,"X: %d \tY: %d \n\r",(aclBuf[0]<<8)|aclBuf[1], (aclBuf[2]<<8)|aclBuf[3]);
USART_Write(&roverPort,textBuffer,length);
StartTimer(&dataTimer);
}
*/
/*
if (GetSpanUs(&accelTimer) > 39)
{
sensorError |= AccelAddDataToBuffer();
StartTimer(&accelTimer);
}
if (BarometerGetState() == 1)
{
sensorError |= ServiceBarometer(3);
}*/
}
}
/**
* Create a new instance of an accelerometer.
*
* The constructor allocates desired analog input.
*
* @param channel The channel number for the analog input the accelerometer is
* connected to
*/
AnalogAccelerometer::AnalogAccelerometer(int32_t channel) {
m_analogInput = std::make_shared<AnalogInput>(channel);
InitAccelerometer();
}
/**
* Create new instance of accelerometer.
*
* Make a new instance of the accelerometer given a module and channel. The constructor allocates
* the desired analog channel from the specified module
*
* @param moduleNumber The analog module (1 or 2).
* @param channel The analog channel (1..8)
*/
Accelerometer::Accelerometer(UINT8 moduleNumber, UINT32 channel)
{
m_analogChannel = new AnalogChannel(moduleNumber, channel);
m_allocatedChannel = true;
InitAccelerometer();
}
/**
* Create a new instance of an accelerometer.
*
* The accelerometer is assumed to be in the first analog module in the given analog channel. The
* constructor allocates desired analog channel.
*/
Accelerometer::Accelerometer(UINT32 channel)
{
m_analogChannel = new AnalogChannel(channel);
m_allocatedChannel = true;
InitAccelerometer();
}
void HandleAccelerometer(tMessage *pMsg)
{
if (Control == INIT_MODE) InitAccelerometer();
switch (pMsg->Options)
{
case MSG_OPT_ACCEL_DATA:
if (Connected(CONN_TYPE_SPP) && QuerySniffState() == Active
#if SUPPORT_BLE
|| Connected(CONN_TYPE_BLE) && CurrentInterval(INTERVAL_STATE) == SHORT
#endif
)
AccelerometerSendDataHandler();
break;
case MSG_OPT_ACCEL_ENABLE:
if (Connected(CONN_TYPE_BLE)) SendMessage(UpdConnParamMsg, SHORT);
else if (Connected(CONN_TYPE_SPP)) SendMessage(SniffControlMsg, MSG_OPT_EXIT_SNIFF);
if (pMsg->Length)
{
ENTER_STANDBY_MODE();
Control &= ~ACCEL_RANGE_MASK;
Control |= (*pMsg->pBuffer & 0x03) << ACCEL_RANGE_SHFT;
AccelerometerWrite(KIONIX_CTRL_REG1, &Control, ONE_BYTE);
}
/* enable accelerometer */
ENTER_OPERATING_MODE();
ACCELEROMETER_INT_ENABLE();
break;
case MSG_OPT_ACCEL_DISABLE:
/* put into low power mode */
ACCELEROMETER_INT_DISABLE();
ENTER_STANDBY_MODE();
if (Connected(CONN_TYPE_BLE)) SendMessage(UpdConnParamMsg, LONG);
break;
case MSG_OPT_ACCEL_STREAMING:
ENTER_STANDBY_MODE();
Control &= ~WUF_ENABLE;
Control |= DRDYE_DATA_AVAILABLE;
AccelerometerWrite(KIONIX_CTRL_REG1, &Control, ONE_BYTE);
ENTER_OPERATING_MODE();
break;
case MSG_OPT_ACCEL_WUF:
ENTER_STANDBY_MODE();
if (pMsg->Length) AccelerometerWrite(KIONIX_WUF_THRESH, pMsg->pBuffer, ONE_BYTE);
Control &= ~DRDYE_DATA_AVAILABLE;
Control |= WUF_ENABLE;
AccelerometerWrite(KIONIX_CTRL_REG1, &Control, ONE_BYTE);
ENTER_OPERATING_MODE();
break;
default:
break;
}
}
/**
* Create new instance of accelerometer.
*
* Make a new instance of the accelerometer given a module and channel. The constructor allocates
* the desired analog channel from the specified module
*/
FilteredAccelerometer::FilteredAccelerometer(UINT32 slot, UINT32 channel)
{
m_analogChannel = new AnalogChannel(slot, channel);
m_allocatedChannel = true;
InitAccelerometer();
}
