static void CheckEnvironment()
{
GetSensorState(&SensorState);
UpdateSafety(&SensorState, &SafetyState);
#if 0
GetImuState(&ImuState);
#endif
GetOdomState(&OdomState);
// Note: There may be a time when the Imu heading will be used, i.e., assigned to the gyro_heading field of OdomState
// That could be done here. First, get ImuState, then OdomState and then assign.
// Check the safety state to ensure the robot doesn't run into anything
// Note: We don't check angular velocity because being able to turn in place is a valuable method getting out of a tough spot.
if ( (commanded_linear_velocity > 0 && !SafetyState.safe_to_proceed) || (commanded_linear_velocity < 0 && !SafetyState.safe_to_recede))
{
commanded_linear_velocity = 0.0;
commanded_angular_velocity = 0.0;
}
// Don't let the robot move unchecked. At the maximum the robot can move for MAX_NO_SERIAL_COMMS_TIME seconds
// If the robot is moving (non-zero speed on left or right wheels) and there has not been serial communication for
// MAX_NO_SERIAL_COMMS_TIME then zero out the velocities
if ( OpState.moving &&
((millis() - OpState.serial_timeout) > MAX_NO_SERIAL_COMMS_TIME) )
{
commanded_linear_velocity = 0.0;
commanded_angular_velocity = 0.0;
}
}
static void SendSensorBoolData(ASensorType_t sensorType, MsgSensorBoolData *pMsg)
{
Buffer_t *pHifPacket;
uint8_t *pPayload;
UncalibratedFixP_t UnCalFixPData;
/* Create packet and place in queue */
pHifPacket = (Buffer_t *)PacketMem;
pPayload = M_GetBufferDataStart(pHifPacket);
#if 0 // Result to Android is now obsoleted in new OSP API
sensorType = ResultToAndroidTypeMap(sensorId);
#endif
//QUOC: Why need to check sensorType == 12?
if (sensorType == 12) __ASM volatile("BKPT #01");
// Do not send data if host did not activate this sensor type
if (GetSensorState(sensorType) == 0)
return;
SensorHubAssertInt(); /* Assert interrupt to host */
/* Process sensor and format into packet */
switch (sensorType) {
case SENSOR_STEP_DETECTOR:
case SENSOR_SIGNIFICANT_MOTION:
UnCalFixPData.TimeStamp.TS64 = pMsg->timeStamp;
UnCalFixPData.Axis[0] = pMsg->active;
UnCalFixPData.Axis[1] = 0;
UnCalFixPData.Axis[2] = 0;
UnCalFixPData.Offset[0] = 0;
UnCalFixPData.Offset[1] = 0;
UnCalFixPData.Offset[2] = 0;
pHifPacket->Header.Length = FormatUncalibratedPktFixP(pPayload,
&UnCalFixPData, META_DATA_UNUSED, sensorType);
break;
default:
return;
}
QueueOverFlow = FastData_add(&FDC, pHifPacket);
}
/***************************************************************************
* @fn SendSensorData
* Sends 3-axis sensor data over the I2C slave interface
* Enqueues data only.
*
***************************************************************************/
static void SendSensorData(ASensorType_t sensorType, MsgSensorData *pMsg)
{
Buffer_t *pHifPacket;
uint8_t *pPayload;
UncalibratedFixP_t UnCalFixPData;
CalibratedFixP_t CalFixPData;
QuaternionFixP_t QuatFixPData;
/* Create packet and place in queue */
pHifPacket = (Buffer_t *)PacketMem;
pPayload = M_GetBufferDataStart(pHifPacket);
// sensorType = ResultToAndroidTypeMap(sensorId);
// if (sensorType == 12) __ASM volatile("BKPT #01");
if (GetSensorState(sensorType) == 0)
return;
SensorHubAssertInt(); /* Assert interrupt to host */
/* Process sensor and format into packet */
switch (sensorType) {
case AP_PSENSOR_ACCELEROMETER_UNCALIBRATED:
case SENSOR_MAGNETIC_FIELD_UNCALIBRATED:
case SENSOR_GYROSCOPE_UNCALIBRATED:
case SENSOR_PRESSURE:
case SENSOR_STEP_COUNTER:
UnCalFixPData.TimeStamp.TS64 = pMsg->timeStamp;
UnCalFixPData.Axis[0] = pMsg->X;
UnCalFixPData.Axis[1] = pMsg->Y;
UnCalFixPData.Axis[2] = pMsg->Z;
UnCalFixPData.Offset[0] = 0;
UnCalFixPData.Offset[1] = 0;
UnCalFixPData.Offset[2] = 0;
pHifPacket->Header.Length = FormatUncalibratedPktFixP(pPayload,
&UnCalFixPData, META_DATA_UNUSED, sensorType);
break;
case SENSOR_ACCELEROMETER:
case SENSOR_MAGNETIC_FIELD:
case SENSOR_GYROSCOPE:
CalFixPData.TimeStamp.TS64 = pMsg->timeStamp;
CalFixPData.Axis[0] = pMsg->X;
CalFixPData.Axis[1] = pMsg->Y;
CalFixPData.Axis[2] = pMsg->Z;
pHifPacket->Header.Length = FormatCalibratedPktFixP(pPayload,
&CalFixPData, sensorType);
break;
case SENSOR_ROTATION_VECTOR:
case SENSOR_GEOMAGNETIC_ROTATION_VECTOR:
case SENSOR_GAME_ROTATION_VECTOR:
QuatFixPData.TimeStamp.TS64 = pMsg->timeStamp;
QuatFixPData.Quat[0] = pMsg->W;
QuatFixPData.Quat[1] = pMsg->X;
QuatFixPData.Quat[2] = pMsg->Y;
QuatFixPData.Quat[3] = pMsg->Z;
pHifPacket->Header.Length = FormatQuaternionPktFixP(pPayload,
&QuatFixPData, sensorType);
break;
case SENSOR_ORIENTATION:
case SENSOR_GRAVITY:
case SENSOR_LINEAR_ACCELERATION:
CalFixPData.TimeStamp.TS64 = pMsg->timeStamp;
CalFixPData.Axis[0] = pMsg->X;
CalFixPData.Axis[1] = pMsg->Y;
CalFixPData.Axis[2] = pMsg->Z;
pHifPacket->Header.Length = FormatCalibratedPktFixP(pPayload,
&CalFixPData, sensorType);
break;
default:
return;
}
/* Enqueue packet in HIF queue */
/* status = EnQueue(_HiFQueue, pHifPacket); */
QueueOverFlow = FastData_add(&FDC, pHifPacket);
}