------ArduSlave.h-----
#pragma once

// Includes
#include "CModule.h"
#include "CPin.h"
#include "SysConfig.h"

class ArduSlave : public CModule
{
public:
    void Initialize();
    void Update( CCommand& commandIn );
};



-------ArduSlave.cpp------

#include "SysConfig.h"
#if(HASArduSlave)
#include "NDataManager.h"
#include "CPin.h"
#include "ArduSlave.h"

#include "NConfigManager.h"

namespace {

void ArduSlave::Initialize()
{
    /* Serial.begin(9600); */
}

void ArduSlave::Update( CCommand& commandIn )
{
    Serial.print(F("ArduSlaveTEST:"));
    Serial.println(';');
}

}  //end namespace

#endif


-----OpenROV2x.ino-------
// Includes
#include "NArduinoManager.h"
#include "NVehicleManager.h"
#include "NDataManager.h"
#include "NCommManager.h"
#include "NModuleManager.h"
#include "ArduSlave.h"

// TODO: Verify the data type of "command". I don't believe that it should be a string, so this is just a place-holder
//  for right now. Once you determine the proper type, come back and change it here. In fact compilation will likely fail
//  because you're passing a String into your Update() function, and it's expecting a CCommand type--as specified in
//  the CCommand.h header file.
const String command = "test";

void setup()
{
    // Initialize main subsystems
    NArduinoManager::Initialize();
    NCommManager::Initialize();
    NVehicleManager::Initialize();
    NModuleManager::Initialize();
    NDataManager::Initialize();
    ArduSlave::Initialize();

    ArduSlave::Update(command);

    // Set timer 5 divisor to 8 for PWM frequency of 3921.16Hz (D44, D45, D46)
    TCCR5B = ( TCCR5B & B11111000 ) | B00000010;

    // Boot complete
    Serial.println( F( "boot:1;" ) );
}

void loop()
{
    // Reset the watchdog timer
    wdt_reset();

    // Attempt to read a current command off of the command line
    NCommManager::GetCurrentCommand();

    // Handle any config change requests
    NVehicleManager::HandleMessages( NCommManager::m_currentCommand );

    // Handle update loops for each module
    NModuleManager::HandleModuleUpdates( NCommManager::m_currentCommand );

    // Handle update loops that send data back to the beaglebone
    NDataManager::HandleOutputLoops();
}

-------SysModules.h-----
#pragma once
#include "SysConfig.h"

#include "Plugins.h"
#include "PinDefinitions.h"

// ---------------------------------------
// Conditional module definitions
// -------------------------------
// Once these objects are instantiated, they will register themselves as modules in the module manager.
// They can also be accessed individually as namespace members.
// ---------------------------------------

#if( HASOROV_CONTROLLERBOARD_25 )
#include "CControllerBoard.h"
CControllerBoard mcontrollerBoard;
#endif

#if(HASSTD_LIGHTS)
#include "CLights.h"
CLights mlights( PIN_STANDARD_LIGHTS );
#endif

#if(HASEXT_LIGHTS )
#include "CExternalLights.h"
CExternalLights melights( PIN_PWM_3 );
#endif

#if(HASSTD_CALIBRATIONLASERS)
#include "CCalibrationLaser.h"
CCalibrationLaser mcalibrationLaser( PIN_LASERS );
#endif

#if(THRUSTERCONFIGURATION != THRUSTER_CONFIG_NONE )
#include "CThrusters.h"
CThrusters mthrusters;
#endif

#if(HASSTD_AUTOPILOT)
#include "CAutopilot.h"
CAutopilot mautopilot;
#endif

#if(HASCAMERASERVO)
#include "CCameraServo.h"
CCameraServo mcameraServo;
#endif

#if(HASALT_SERVO)
#include "CAltServo.h"
CAltServo altservo1;
#endif

#if(DEADMANSWITCHON)
#include "CDeadManSwitch.h"
CDeadManSwitch mdeadManSwitch;
#endif

// IMU1
#if( HASMPU9150 )
#include "CMPU9150.h"
CMPU9150 mmpu9150( mpu9150::EAddress::ADDRESS_B );
#endif

#if(HASMS5803_14BA)
#include "CMS580314BA.h"
CMS580314BA m_ms5803( &I2C0, ms5803_14ba::EAddress::ADDRESS_A );
#endif

// IMU2
#if(HASBNO055)
#include "CBNO055.h"
CBNO055 mbno055( &I2C0, bno055::EAddress::ADDRESS_A );
#endif

#if(HASMS5837_30BA)_
#include "CMS583730BA.h"
CMS583730BA m_ms5837( &I2C0, ms5837_30ba::EAddress::ADDRESS_A );
#endif

#if(HASArduSlave)
#include "ArduSlave.h"
ArduSlave marduslave;
#endif

-----SysConfig.h-----
#pragma once

#include "PinDefinitions.h"
#include "CompileOptions.h"

// TODO: The MCU ARCH and Board Type should eventually be passed in solely from the build script.

// ---------------------------------------------------------
// MCU Architecture Selection
// ---------------------------------------------------------

#ifndef ARDUINOARCH_AVR
_   #error "This sketch is only supported on for ARDUINO_ARCH_AVR!"
#endif

// Configuration
#define HASOROV_CONTROLLERBOARD_25 (1)
#define HASSTD_LIGHTS (1)
#define HASSTD_CALIBRATIONLASERS (1)
#define HASCAMERASERVO (1)
#define HASSTD_AUTOPILOT (1)
#define DEADMANSWITCHON (0)

// Thrusters configurations (These appear depricated and replaced by the THRUSTERCONFIGURATION below)
#define THRUSTERCONFIG_NONE (0)
#define THRUSTERCONFIG_2X1 (1)
#define THRUSTERCONFIG_2Xv2 (2)
#define THRUSTERCONFIG_v2X1Xv2 (3)

// Selected Thruster Configuration
#define THRUSTERCONFIGURATION THRUSTER_CONFIG_2X1
//#define THRUSTERCONFIGURATION THRUSTER_CONFIG_2Xv2
//#define THRUSTERCONFIGURATION THRUSTER_CONFIG_v2X1Xv2

// ---------------------------------------------------------
// After Market Modules
// ---------------------------------------------------------

// External Lights
#define HASEXT_LIGHTS (1)

// AltServo
#define HASALT_SERVO (0)

// MS580314BA Depth Sensor
#define HASMS5803_14BA (1)

// MS583730BA Depth Sensor
#define HASMS5837_30BA (1)

// MPU9150
#define HASMPU9150 (1)

// BNO055 IMU
#define HASBNO055 (1)

#define HASArduSlave (1)