OBJ
  m: "DynamicMathLib"
  orientation: "Orientation"


VAR
  long tuning_tilt_p, tuning_tilt_i, tuning_tilt_d
  long tuning_yaw_p, tuning_yaw_i, tuning_yaw_d

  long target_collective, target_pitch, target_roll, target_yaw_rate

  long pitch_value, pitch_delta, pitch_integral
  long roll_value, roll_delta, roll_integral
  long yaw_value, yaw_delta, yaw_integral


PUB init_tuning(tilt_p, tilt_i, tilt_d, yaw_p, yaw_i, yaw_d)
  tuning_tilt_p := tilt_p
  tuning_tilt_i := tilt_i
  tuning_tilt_d := tilt_d
  tuning_yaw_p := yaw_p
  tuning_yaw_i := yaw_i
  tuning_yaw_d := yaw_d

  pitch_value := pitch_delta := pitch_integral := 0
  roll_value := roll_delta := roll_integral := 0
  yaw_value := yaw_delta := yaw_integral := 0


PUB set_target(collective, pitch, roll, yaw_rate)
  target_collective := collective
  target_pitch := pitch
  target_roll := roll
  target_yaw_rate := yaw_rate


PUB update(orient_data) | x
  x := long[orient_data][orientation#PITCH_DEGREES_E1]
  pitch_delta := x - pitch_value
  pitch_value := x
  pitch_integral += x - target_pitch

  x := long[orient_data][orientation#ROLL_DEGREES_E1]
  roll_delta := x - roll_value
  roll_value := x
  roll_integral += x - target_roll

  x := long[orient_data][orientation#YAW_RATE_DPS_E1]
  yaw_delta := x - yaw_value
  yaw_value := x
  yaw_integral += x - target_yaw_rate


PUB get_speeds(motor_array) | p, r, y
  p := tuning_tilt_p * (target_pitch - pitch_value) / 10
  p -= tuning_tilt_i * pitch_integral
  p -= tuning_tilt_d * pitch_delta

  r := tuning_tilt_p * (target_roll - roll_value) / 10
  r -= tuning_tilt_i * roll_integral
  r -= tuning_tilt_d * roll_delta

  y := tuning_yaw_p * (target_yaw_rate - yaw_value) / 100
  y -= tuning_yaw_i * yaw_integral
  y -= tuning_yaw_d * yaw_delta

  long[motor_array][0] := target_collective + p + y
  long[motor_array][1] := target_collective - r - y
  long[motor_array][2] := target_collective - p + y
  long[motor_array][3] := target_collective + r - y