CON
  #0
  COUNT
  PITCH_DEGREES_E1
  ROLL_DEGREES_E1
  YAW_RATE_DPS_E1
  NUM_VARS

  GYRO_DPS = 3.3 / 1024.0 / 0.002


VAR
  long ax_0, ay_0, az_0
  long ax_g, ay_g, az_g
  long pitch_0, roll_0, yaw_0, gyro_dpts
  long yaw_rate, last_count, missed


OBJ
  m: "DynamicMathLib"
  s: "Sensors"

  pitch: "KalmanFilter"
  roll: "KalmanFilter"


PUB init(calibration, timestep_sec, motion_dps, drift_dps2, slosh_deg) | v
  ax_0 := cal_mid(calibration, s#IMU_ACCEL_X)
  ay_0 := cal_mid(calibration, s#IMU_ACCEL_Y)
  az_0 := cal_mid(calibration, s#IMU_ACCEL_Z)

  ax_g := cal_range(calibration, s#IMU_ACCEL_X)
  ay_g := cal_range(calibration, s#IMU_ACCEL_Y)
  az_g := cal_range(calibration, s#IMU_ACCEL_Z)

  pitch_0 := cal_mid(calibration, s#IMU_PITCH)
  roll_0 := cal_mid(calibration, s#IMU_ROLL)
  yaw_0 := cal_mid(calibration, s#IMU_YAW)
  gyro_dpts := m.FMul(GYRO_DPS, timestep_sec)

  yaw_rate := 0
  last_count := -1
  missed := 0

  motion_dps := m.FMul(motion_dps, timestep_sec)
  drift_dps2 := m.FMul(drift_dps2, m.FSqr(timestep_sec))

  v := m.Degrees(m.FDiv(cal_var(calibration, s#IMU_ACCEL_Y), ay_g))
  pitch.init(motion_dps, drift_dps2, m.FAdd(slosh_deg, v))

  v := m.Degrees(m.FDiv(cal_var(calibration, s#IMU_ACCEL_X), ax_g))
  roll.init(motion_dps, drift_dps2, m.FAdd(slosh_deg, v))


PUB update(imu_data) | c, x, y, z, p, r
  c := word[imu_data][s#IMU_COUNT]
  if c == last_count
    return false
  if last_count <> -1
    missed += (c - last_count - 1) & $FFFF

  x := m.FDiv(m.FSub(m.FFloat(word[imu_data][s#IMU_ACCEL_X]), ax_0), ax_g)
  y := m.FDiv(m.FSub(m.FFloat(word[imu_data][s#IMU_ACCEL_Y]), ay_0), ay_g)
  z := m.FDiv(m.FSub(m.FFloat(word[imu_data][s#IMU_ACCEL_Z]), az_0), az_g)
  p := m.FMul(m.FSub(pitch_0, m.FFloat(word[imu_data][s#IMU_PITCH])), gyro_dpts)
  r := m.FMul(m.FSub(roll_0, m.FFloat(word[imu_data][s#IMU_ROLL])), gyro_dpts)
  yaw_rate := m.FMul(m.FSub(yaw_0, m.FFloat(word[imu_data][s#IMU_YAW])), GYRO_DPS)

  pitch.update(m.ATan2D(y, z), p)
  roll.update(m.ATan2D(x, z), r)
  last_count := c
  return true


PUB get_data(array)
  long[array][COUNT] := last_count
  long[array][PITCH_DEGREES_E1] := m.FRound(m.FMul(pitch.get_value, 10.0))
  long[array][ROLL_DEGREES_E1] := m.FRound(m.FMul(roll.get_value, 10.0))
  long[array][YAW_RATE_DPS_E1] := m.FRound(m.FMul(yaw_rate, 10.0))


PUB get_missed
  return missed


PRI cal_range(calibration, axis) | min_val, max_val
  min_val := long[calibration][axis * 4 - 4]
  max_val := long[calibration][axis * 4 - 2]
  return m.FMul(m.FSub(max_val, min_val), 0.5)


PRI cal_mid(calibration, axis)
  return long[calibration][axis * 4 - 3]


PRI cal_var(calibration, axis)
  return long[calibration][axis * 4 - 1]