void CBNO055::Initialize()
{
// Reset timers
bno055_sample_timer.Reset();
report_timer.Reset();
imuTimer.Reset();
InitializeSensor();
}
void BrightnessSensor::Initialize(unsigned int number)
{
this->number = number;
InitializeSensor
( userEeprom.getUInt8(0x4511 + number)
, COM_OBJ_BRIGHTNESS_EAST + objNoOffset
, 0x4514 + number
, userEeprom.getUInt8(0x4517 + number)
);
threshold[0].upperLimit = userEeprom.getUInt8(0x451D + number) * 1000;
threshold[0].upperLimitTime = timeConversionMinSec(0x452C + number);
threshold[0].lowerLimit = userEeprom.getUInt8(0x4520 + number) * 1000;
threshold[0].lowerLimitTime = timeConversionMinSec(0x452F + number);
if (userEeprom.getUInt8(0x4523))
{
threshold[0].changeLowerLimit = COM_OBJ_THRESHOLD_1_EAST_LOWER + 17 * number;
threshold[0].changeUpperLimit = COM_OBJ_THRESHOLD_1_EAST_UPPER + 17 * number;
}
threshold[0].sendLimitExceeded = userEeprom.getUInt8(0x4526 + number);
threshold[0].sendLowerDeviation = userEeprom.getUInt8(0x4529 + number);
threshold[0].cycleTime = timeConversionMinSec(0x4594 + number);
if (userEeprom.getUInt8(0x4532))
threshold[0].blockObjNo = COM_OBJ_BRIGHTNESS_EAST_BLOCK + 17 * number;
threshold[1].upperLimit = userEeprom.getUInt8(0x4538 + number) * 1000;
threshold[1].upperLimitTime = timeConversionMinSec(0x4544 + number);
threshold[1].lowerLimit = userEeprom.getUInt8(0x453B + number) * 1000;
threshold[1].lowerLimitTime = timeConversionMinSec(0x4547 + number);
threshold[1].sendLimitExceeded = userEeprom.getUInt8(0x453E + number);
threshold[1].sendLowerDeviation = userEeprom.getUInt8(0x4541 + number);
threshold[1].cycleTime = timeConversionMinSec(0x4597 + number);
if (userEeprom.getUInt8(0x451A))
// check if threshold 1 is active
threshold[0].objNumber = COM_OBJ_THRESHOLD_1_EAST + 17 * number;
if (userEeprom.getUInt8(0x4535))
// check if threshold 2 is active
threshold[1].objNumber = COM_OBJ_THRESHOLD_2_EAST + 17 * number;
}
void CBNO055::Update( CCommand& commandIn )
{
if( commandIn.Equals( "imumode" ) )
{
if( initalized && commandIn.m_arguments[ 0 ] != 0 )
{
if( commandIn.m_arguments[ 1 ] == 0 )
{
// Turn off override
inFusionMode = true;
bno.EnterNDOFMode();
}
if( commandIn.m_arguments[ 1 ] == 12 )
{
// Override to NDOF
inFusionMode = true;
bno.EnterNDOFMode();
}
if( commandIn.m_arguments[ 1 ] == 8 )
{
// Override to IMU mode
inFusionMode = false;
bno.EnterIMUMode();
}
}
else
{
Serial.println( "log:Can't enter override, IMU is not initialized yet!;" );
}
}
// 1000 / 21
if( bno055_sample_timer.HasElapsed( 47 ) )
{
if( !initalized )
{
// Attempt every 10 secs
if( report_timer.HasElapsed( 10000 ) )
{
// Attempt to initialize the chip again
InitializeSensor();
}
return;
}
// System status checks
if( report_timer.HasElapsed( 1000 ) )
{
// System calibration
if( bno.GetCalibration() )
{
Serial.print( "BNO055.CALIB_MAG:" );
Serial.print( bno.m_magCal );
Serial.println( ';' );
Serial.print( "BNO055.CALIB_ACC:" );
Serial.print( bno.m_accelCal );
Serial.println( ';' );
Serial.print( "BNO055.CALIB_GYR:" );
Serial.print( bno.m_gyroCal );
Serial.println( ';' );
Serial.print( "BNO055.CALIB_SYS:" );
Serial.print( bno.m_systemCal );
Serial.println( ';' );
// Get offsets
bno.GetGyroOffsets();
bno.GetAccelerometerOffsets();
bno.GetMagnetometerOffsets();
}
else
{
Serial.print( "BNO055.CALIB_MAG:" );
Serial.print( "N/A" );
Serial.println( ';' );
Serial.print( "BNO055.CALIB_ACC:" );
Serial.print( "N/A" );
Serial.println( ';' );
Serial.print( "BNO055.CALIB_GYR:" );
Serial.print( "N/A" );
Serial.println( ';' );
Serial.print( "BNO055.CALIB_SYS:" );
Serial.print( "N/A" );
Serial.println( ';' );
}
// Operating mode
if( bno.GetOperatingMode() )
{
Serial.print( "BNO055.MODE:" );
Serial.print( bno.m_operatingMode );
Serial.println( ';' );
}
else
{
Serial.print( "BNO055.MODE:" );
Serial.print( "N/A" );
Serial.println( ';' );
}
// System status
if( bno.GetSystemStatus() )
{
Serial.print( "BNO055_STATUS:" );
Serial.print( bno.m_systemStatus, HEX );
Serial.println( ";" );
}
else
{
Serial.print( "BNO055_STATUS:" );
Serial.print( "N/A" );
Serial.println( ";" );
}
// System Error
if( bno.GetSystemError() )
{
Serial.print( "BNO055_ERROR_FLAG:" );
Serial.print( bno.m_systemError );
Serial.println( ";" );
}
else
{
Serial.print( "BNO055_ERROR_FLAG:" );
Serial.print( "N/A" );
Serial.println( ";" );
}
}
// Get orientation data
if( bno.GetVector( CAdaBNO055::VECTOR_EULER, euler ) )
{
// Throw out exactly zero heading values that are all zeroes - necessary when switching modes
if( euler.x() != 0.0f )
{
// These may need adjusting
NDataManager::m_navData.YAW = euler.x();
NDataManager::m_navData.HDGD = euler.x();
}
NDataManager::m_navData.PITC = euler.z();
NDataManager::m_navData.ROLL = -euler.y();
}
}
}
