base.yml

EventHub:
    type: EventHub

# Queues events from background threads so they can be executed in python
QueuedEventHub:
    depends_on: ["EventHub"]
    type: QueuedEventHub

# Exectures Motions
MotionManager:
    type: MotionManager
    depends_on: ["Vehicle", "Controller", "QueuedEventHub", "EventHub"]

# Handles Timers
TimerManager:
    type: TimerManager
    depends_on: ["EventHub"]

# Object that creates the vehicle interface
Vehicle:
    # The full name of the class to instantiate in this case its a wrapper
    # for the base C++ vehicle
    type: Vehicle
    # Time between updates in ms
    update_interval: 25
    priority: high

    depends_on: ["EventHub", "Logging"
]

    MagBoomName: MagBoom

    # The list of devices to create for the vehicle
    Devices:
        # NOTE: All current numbers here are BS and need to updated
        VelocitySensor:
            depends_on: ["SensorBoard"]
            type: VisionVelocitySensor
        SensorBoard:
            type: SensorBoard
            update_interval: 25
            priority: high
            depthCalibSlope: 35.5
            depthCalibIntercept: 1182
            # Position is for Tortuga 2 measured in inches as (-11, 1.5, 3)
            depthSensorLocation: [-0.916666667, 0.125, 0.25]
            servo1FirePosition: 4000 # Range 256-4352
            servo2FirePosition: 256 # Range 256-4352
        MarkerDropper:
            type: SBMarkerDropper
            depends_on: ["SensorBoard"]
        TorpedoLauncher:
            type: SBTorpedoLauncher
            depends_on: ['SensorBoard']
        Sonar:
            depends_on: ['SensorBoard']
            type: SBSonar
        Battery1:
            depends_on: ['SensorBoard']
            type: SBPowerSource
            id: 0

        Battery2:
            depends_on: ['SensorBoard']
            type: SBPowerSource
            id: 1

        Batter3:
            depends_on: ['SensorBoard']
            type: SBPowerSource
            id: 2

        Battery4:
            depends_on: ['SensorBoard']
            type: SBPowerSource
            id: 3

        Shore:
            depends_on: ['SensorBoard']
            type: SBPowerSource
            id: 4

        Temp1:
            depends_on: ['SensorBoard']
            type: SBTempSensor
            id: 0

        Temp2:
            depends_on: ['SensorBoard']
            type: SBTempSensor
            id: 5

        Temp3:
            depends_on: ['SensorBoard']
            type: SBTempSensor
            id: 6

        # This entry creates a thruster
        StarboardThruster:
            depends_on: ['SensorBoard']
            # serial: 147
            type: SBThruster
            update_interval: 100

            # String representing the address of the thruster
            address: 1
            # Number determined by testing
            calibration_factor: 25
            # Number used to flip thruster forces (if thruster is reversed
                
            # on the vehicle)
            direction: 1
            # Distance from center of rotation
            offset: 0.19685
        PortThruster:
            depends_on: ['SensorBoard']
            # serial: 149
            type: SBThruster
            address: 0
            calibration_factor: 25
            direction: 1
            offset: 0.19685

        ForeThruster:
            depends_on: ['SensorBoard']
            type: SBThruster
            # serial: 146
            address: 3
            calibration_factor: 25
            direction: -1
            offset: 0.2413

        AftThruster:
            depends_on: ['SensorBoard']
            # Serial 148
            type: SBThruster
            address: 5
            calibration_factor: 25
            direction: -1
            offset: 0.381

        TopThruster:
            depends_on: ['SensorBoard']
            # serial: 149
            type: SBThruster
            address: 2
            calibration_factor: 25
            direction: 1
            offset: 0.19685

        BottomThruster:
            depends_on: ['SensorBoard']
            type: SBThruster
            address: 4
            calibration_factor: 25
            direction: -1
            offset: 0.19685
            
        MagBoom:
            type: IMU
            num: 1
            devfile: /dev/magboom
            update_interval: 5
            priority: high

            #imuToVehicleRotMatrix: [[ 0.9140, -0.4048, -0.0273],
            #                        [-0.4057, -0.9123, -0.0554],
            #                        [-0.0024,  0.0617, -0.9981]] 

            #imuToVehicleRotMatrix: [[0.6022, -0.7980, 0.0253],
            #                        [-0.7975, -0.5997, 0.0655],
            #                        [-0.0371, -0.0597, -0.9975]]

            imuToVehicleRotMatrix: [[-0.4351, -0.9000, -0.0268],
                                    [-0.9004, 0.4346, 0.0216],
                                    [-0.0077, 0.0335, -0.9994]]

            #magXBias: -0.0702
            #magYBias:  0.0056
            #magZBias:  0.0124
            #magXBias: -0.0735
            #magYBias: 0.0056
            #magZBias: -0.0109
            # Tortuga 3
            magXBias: -0.0135
            magYBias: 0.0147
            magZBias: -0.0495
            
            #gyroXBias: 0.0013
            #gyroYBias: 0.0499
            #gyroZBias: -0.0501

            # Tortuga 3
            gyroXBias: 0.0087
            gyroYBias: 0.0201
            gyroZBias: -0.0084

            localMagneticPitch: 60

        IMU:
            type: IMU
            num: 0
            update_interval: 5
            priority: high
            # These are in VEHICLE FRAME
            # Pre-thruster change out
            #magXBias: 0.158325
            #magYBias: 0.229488
            #magZBias: -0.270406
            # Post Thruster change out, and with 6 thrusters
            #magXBias: 0.243511
            #magYBias: 0.260058
            #magZBias: -0.211263
            # New Electronics
            #magXBias: 0.243998
            #magYBias: 0.269084
            #magZBias: -0.221182
            # Tortuga 2 first dive
            #magXBias: 0.251043
            #magYBias: 0.196805
            #magZBias: -0.171669
            # Tortuga 2 With Everthing
            #magXBias: 0.25861
            #magYBias: 0.252917
            #magZBias: -0.183411
            # Tortuga 2 Redo
            #magXBias: 0.2010
            #magYBias: 0.2160
            #magZBias: -0.1899
            # Tortuga 2.5
            #magXBias: -0.1698
            #magYBias: -0.1691
            #magZBias: -0.2362
            # Tortuga 3
            magXBias: 0.0798
            magYBias: 0.0384
            magZBias: 0.5876 

            # Tortuga 2.5
            #gyroXBias: 0.0216
            #gyroYBias: -0.0148
            #gyroZBias: -0.0242
            # Tortuga 3
            gyroXBias: 0.0174
            gyroYBias: -0.0136
            gyroZBias: -0.0304

            # Tortuga 2.5
            #imuToVehicleRotMatrix: [[-0.8794, 0.0060, -0.4761],
            #                        [0.4759, 0.0424, -0.8785],
            #                        [0.0149, -0.9991, -0.0401]]
            # Tortuga 3
            imuToVehicleRotMatrix: [[0.1806, 0.0378, 0.9828],
                                    [0.9833, -0.0313, -0.1795],
                                    [0.0240, 0.9988, -0.0428]]
            

            # Set me to zero to ignore mag.
            #magCorruptThresh: 0.0
            localMagneticPitch: 60
RemoteController:
    type: RemoteController
    depends_on: ["Controller"]
    update_interval: -1
    yawGain: 40

Logging:
    type: Logging
    #update_interval: -1
    Appenders:
        ThrusterLog:
            type: File
            fileName: thrusters.log
            Layout:
                type: Pattern
                pattern: "%m %r%n"
        PowerLog:
            type: File
            fileName: power.log
            Layout:
                type: Pattern
                pattern: "%m %r%n"
        ControlLog:
            type: File
            fileName: control.log
            Layout:
                type: Pattern
                pattern: "%m %r%n"
                    
        IMULog:
            type: File
            fileName: imu.log
            Layout:
                type: Pattern
                pattern: "%m %r%n"
                    
        Console:
            type: Console
            Layout:
                type: Pattern
                pattern: "%c %m %r%n"
    Categories:
        Thruster:
            appenders: ['ThrusterLog'] #,'Console']
        Power:
            appenders: ['PowerLog'] #,'Console']
        Controller:
            appenders: ['ControlLog'] #'Console']
        IMU:
            appenders: ['IMULog'] #'Console']

Controller:
    depends_on: ["Vehicle", "EventHub"]
    type: BWPDController
    update_interval: 40
    priority: high
    holdCurrentHeading: 1 # Set to 0 to always point north
    holdCurrentDepth: 1
    initializationPause: 2
    orientationThreshold: 0.05

    adaptCtrlRotK: 5
    adaptCtrlRotGamma: 1


    angularPGain: 15
    angularDGain: 2
    speedPGain: 8
    sidewaysSpeedPGain: 8
    # Should not be negative
    depthPGain: 20
    velocityPGain: 0.1
    inertia: [[0.201, 0, 0],
              [0, 4, 0],
              [0, 0, 1.288]]

    desiredSpeed: 0
    desiredDepth: 0.2
    desiredQuaternion: [0, 0, 0, 1]

    # Picks kind of depth controller
    # 1 = P control (Normal)
    # 4 = Dis. Obs. w/Integral Aug.
    # 5 = PID
    depthControlType: 5

    # Discrete Observer Controller w/Integral Aug. (#4)
    depthOCIA4: [0, 12217, -469, 0,
                 0, 0, 0, 0,
                 0, -9, 0, 0,
                 0, 2932, 49, 1]
    depthOCIB4: [-12221, 1, 9, -2937]
    depthOCIC4: [0, 0, 0, 1]

    # Settings for PID Depth Controller (#5)
    depthKp: 10.0 #12.0
    depthKi: 1.0
    depthKd: 1.0 #3.0