void HandleMessages( CCommand& commandIn )
{
if( commandIn.Equals( "reportSetting" ) )
{
Serial.print( F( "*settings:" ) );
Serial.print( F( "smoothingIncriment|" ) );
Serial.print( String( NConfigManager::m_throttleSmoothingIncrement ) + "," );
Serial.print( F( "deadZone_min|" ) );
Serial.print( String( NConfigManager::m_deadZoneMin ) + "," );
Serial.print( F( "deadZone_max|" ) );
Serial.print( String( NConfigManager::m_deadZoneMax ) + ";" );
Serial.print( F( "water_type" ) );
Serial.println( String( NConfigManager::m_waterType ) + ";" );
}
else if( commandIn.Equals( "rcap" ) ) //report capabilities
{
Serial.print( F( "CAPA:" ) );
Serial.print( m_capabilityBitmask );
Serial.print( ';' );
NArduinoManager::ScanI2C();
}
else if( commandIn.Equals( "updateSetting" ) )
{
//TODO: Need to update the motors with new deadZone setting. Probably move
//deadzone to the thruster resposibilitiy
NConfigManager::m_throttleSmoothingIncrement = commandIn.m_arguments[1];
NConfigManager::m_deadZoneMin = commandIn.m_arguments[2];
NConfigManager::m_deadZoneMax = commandIn.m_arguments[3];
NConfigManager::m_waterType = commandIn.m_arguments[4];
}
}
void CExternalLights::Update( CCommand& commandIn )
{
if( commandIn.Equals( "eligt" ) )
{
float percentValue = ( float )commandIn.m_arguments[1] / 100.0f;
int value = (int)( 255.0f * percentValue );
elight.Write( value );
Serial.print( F( "LIGTE:" ) );
Serial.print( value );
Serial.print( ';' );
Serial.print( F( "LIGPE:" ) );
Serial.print( percentValue );
Serial.println( ';' );
}
}
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();
}
}
}
void CDeadManSwitch::Update( CCommand& command )
{
// Check for messages
if( NCommManager::m_isCommandAvailable )
{
if( command.Equals( "ping" ) )
{
deadmanSwitchTimer.Reset();
if( _deadmanSwitchEnabled )
{
int argsToSend[] = {0};
command.PushCommand( "start", argsToSend );
_deadmanSwitchEnabled = false;
}
Serial.print( F( "pong:" ) );
Serial.print( command.m_arguments[0] );
Serial.print( "," );
Serial.print( millis() );
Serial.print( ";" );
}
else if( command.Equals( "dms" ) )
{
if( command.m_arguments[0] == 0 )
{
_deadmanSwitchArmed = false;
}
else
{
_deadmanSwitchArmed = true;
}
}
}
//Auto arm the deadman switch after a reasonable boot time
if( !_deadmanSwitchArmed && ( millis() > DEADMAN_SWITCH_DELAY_TO_ARM_MS ) )
{
_deadmanSwitchArmed = true;
}
if( ( deadmanSwitchTimer.HasElapsed( 2000 ) ) && _deadmanSwitchArmed && ( _deadmanSwitchEnabled == false ) )
{
int argsToSend[] = {0}; //include number of parms as fist parm
command.PushCommand( "deptloff", argsToSend );
command.PushCommand( "headloff", argsToSend );
command.PushCommand( "stop", argsToSend );
_deadmanSwitchEnabled = true;
}
if( _deadmanSwitchEnabled && blinklightTimer.HasElapsed( 500 ) )
{
int argsToSend[] = {1, 50};
if( blinkstate )
{
argsToSend[1] = 0;
}
command.PushCommand( "ligt", argsToSend );
blinkstate = !blinkstate;
}
}
void CThrusters::Update( CCommand& command )
{
if( command.Equals( "mtrmod1" ) )
{
port_motor.m_positiveModifier = command.m_arguments[1] / 100;
vertical_motor.m_positiveModifier = command.m_arguments[2] / 100;
starboard_motor.m_positiveModifier = command.m_arguments[3] / 100;
}
if( command.Equals( "mtrmod2" ) )
{
port_motor.m_negativeModifier = command.m_arguments[1] / 100;
vertical_motor.m_negativeModifier = command.m_arguments[2] / 100;
starboard_motor.m_negativeModifier = command.m_arguments[3] / 100;
}
if( command.Equals( "rmtrmod" ) )
{
Serial.print( F( "mtrmod:" ) );
Serial.print( port_motor.m_positiveModifier );
Serial.print( "," );
Serial.print( vertical_motor.m_positiveModifier );
Serial.print( "," );
Serial.print( starboard_motor.m_positiveModifier );
Serial.print( "," );
Serial.print( port_motor.m_negativeModifier );
Serial.print( "," );
Serial.print( vertical_motor.m_negativeModifier );
Serial.print( "," );
Serial.print( starboard_motor.m_negativeModifier );
Serial.println( ";" );
}
if( command.Equals( "go" ) )
{
//ignore corrupt data
if( command.m_arguments[1] > 999 && command.m_arguments[2] > 999 && command.m_arguments[3] > 999 && command.m_arguments[1] < 2001 && command.m_arguments[2] < 2001 && command.m_arguments[3] < 2001 )
{
p = command.m_arguments[1];
v = command.m_arguments[2];
s = command.m_arguments[3];
if( command.m_arguments[4] == 1 )
{
bypasssmoothing = true;
}
}
}
if( command.Equals( "port" ) )
{
//ignore corrupt data
if( command.m_arguments[1] > 999 && command.m_arguments[1] < 2001 )
{
p = command.m_arguments[1];
if( command.m_arguments[2] == 1 )
{
bypasssmoothing = true;
}
}
}
if( command.Equals( "vertical" ) )
{
//ignore corrupt data
if( command.m_arguments[1] > 999 && command.m_arguments[1] < 2001 )
{
v = command.m_arguments[1];
if( command.m_arguments[2] == 1 )
{
bypasssmoothing = true;
}
}
}
if( command.Equals( "starboard" ) )
{
//ignore corrupt data
if( command.m_arguments[1] > 999 && command.m_arguments[1] < 2001 )
{
s = command.m_arguments[1];
if( command.m_arguments[2] == 1 )
{
bypasssmoothing = true;
}
}
}
if( command.Equals( "thro" ) || command.Equals( "yaw" ) )
{
if( command.Equals( "thro" ) )
{
if( command.m_arguments[1] >= -100 && command.m_arguments[1] <= 100 )
{
trg_throttle = command.m_arguments[1] / 100.0;
}
}
if( command.Equals( "yaw" ) )
{
//ignore corrupt data
if( command.m_arguments[1] >= -100 && command.m_arguments[1] <= 100 ) //percent of max turn
{
trg_yaw = command.m_arguments[1] / 100.0;
}
}
// The code below spreads the throttle spectrum over the possible range
// of the motor. Not sure this belongs here or should be placed with
// deadzon calculation in the motor code.
if( trg_throttle >= 0 )
{
p = 1500 + ( 500.0 / abs( port_motor.m_positiveModifier ) ) * trg_throttle;
s = p;
}
else
{
p = 1500 + ( 500.0 / abs( port_motor.m_negativeModifier ) ) * trg_throttle;
s = p;
}
trg_motor_power = s;
int turn = trg_yaw * 250; //max range due to reverse range
if( trg_throttle >= 0 )
{
int offset = ( abs( turn ) + trg_motor_power ) - 2000;
if( offset < 0 )
{
offset = 0;
}
p = trg_motor_power + turn - offset;
s = trg_motor_power - turn - offset;
}
else
{
int offset = 1000 - ( trg_motor_power - abs( turn ) );
if( offset < 0 )
{
offset = 0;
}
p = trg_motor_power + turn + offset;
s = trg_motor_power - turn + offset;
}
}
if( command.Equals( "lift" ) )
{
if( command.m_arguments[1] >= -100 && command.m_arguments[1] <= 100 )
{
trg_lift = command.m_arguments[1] / 100.0;
v = 1500 + 500 * trg_lift;
}
}
#ifdef ESCPOWER_PIN
else if( command.Equals( "escp" ) )
{
escpower.Write( command.m_arguments[1] ); //Turn on the ESCs
Serial.print( F( "log:escpower=" ) );
Serial.print( command.m_arguments[1] );
Serial.println( ';' );
}
#endif
else if( command.Equals( "start" ) )
{
port_motor.Activate();
vertical_motor.Activate();
starboard_motor.Activate();
}
else if( command.Equals( "stop" ) )
{
p = MOTOR_TARGET_NEUTRAL_US;
v = MOTOR_TARGET_NEUTRAL_US;
s = MOTOR_TARGET_NEUTRAL_US;
// Not sure why the reset does not re-attach the servo.
//port_motor.stop();
//vertical_motor.stop();
//starboard_motor.stop();
}
#ifdef ESCPOWER_PIN
else if( ( command.Equals( "mcal" ) ) && ( canPowerESCs ) )
{
Serial.println( F( "log:Motor Callibration Staring;" ) );
//Experimental. Add calibration code here
Serial.println( F( "log:Motor Callibration Complete;" ) );
}
#endif
//to reduce AMP spikes, smooth large power adjustments out. This incirmentally adjusts the motors and servo
//to their new positions in increments. The incriment should eventually be adjustable from the cockpit so that
//the pilot could have more aggressive response profiles for the ROV.
if( controltime.HasElapsed( 50 ) )
{
if( p != new_p || v != new_v || s != new_s )
{
new_p = p;
new_v = v;
new_s = s;
// Check to see if any motors are non-neutral to signal system that at least one motor is running
if( p != MOTOR_TARGET_NEUTRAL_US || v != MOTOR_TARGET_NEUTRAL_US || s != MOTOR_TARGET_NEUTRAL_US )
{
NDataManager::m_thrusterData.MotorsActive = true;
}
else
{
NDataManager::m_thrusterData.MotorsActive = false;
}
Serial.print( F( "motors:" ) );
Serial.print( port_motor.SetMotorTarget( new_p ) );
Serial.print( ',' );
Serial.print( vertical_motor.SetMotorTarget( new_v ) );
Serial.print( ',' );
Serial.print( starboard_motor.SetMotorTarget( new_s ) );
Serial.println( ';' );
}
}
NDataManager::m_navData.FTHR = map( ( new_p + new_s ) / 2, 1000, 2000, -100, 100 );
//The output from the motors is unique to the thruster configuration
if( thrusterOutput.HasElapsed( 1000 ) )
{
Serial.print( F( "mtarg:" ) );
Serial.print( p );
Serial.print( ',' );
Serial.print( v );
Serial.print( ',' );
Serial.print( s );
Serial.println( ';' );
NDataManager::m_thrusterData.MATC = port_motor.IsActive() || port_motor.IsActive() || port_motor.IsActive();
Serial.print( F( "mtrmod:" ) );
Serial.print( port_motor.m_positiveModifier );
Serial.print( "," );
Serial.print( vertical_motor.m_positiveModifier );
Serial.print( "," );
Serial.print( starboard_motor.m_positiveModifier );
Serial.print( "," );
Serial.print( port_motor.m_negativeModifier );
Serial.print( "," );
Serial.print( vertical_motor.m_negativeModifier );
Serial.print( "," );
Serial.print( starboard_motor.m_negativeModifier );
Serial.println( ";" );
}
}
