tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 0.5
mass = 0.350

# The Stack: First layer computes target speed, second layer computes motor torque
layers = [
    # { type = "Angle", max_angle = 0.78, kp = [4.0, 4.0, 4.0], ki = [0.0, 0.0, 0.0], kd = [0.1, 0.1, 0.1] },
    { type = "Rate", max_rate = 3.14, kp = [
        0.0,
        0.0,
        0.01,
    ], ki = [
        0.0,
        0.0,
        0.0,
    ], kd = [
        0.0,
        0.0,
        0.0,
    ] },
]

#                + (map[0] * setpoint.x) // Roll
#                + (map[1] * setpoint.y) // Yaw
#                + (map[2] * setpoint.z); // Pitch

# /*
#  * Motor position indices
#  *    ^ - Front
#  *    |
#  *    |
#  * 1 --- 0
#  * |     |
#  * |     |
#  * 2 --- 3
#  */
motor_map = [
    [-1.0, 1.0, 1.0],   # Front Right
    [1.0, -1.0, 1.0],   # Front Left
    [1.0, 1.0, -1.0],   # Rear Left
    [-1.0, -1.0, -1.0], # Rear Right
]