' Cogs:
'   - main program logic
'   - sensor parsing (IMU, GPS)
'   - IMU serial port
'   - GPS serial port
'   - radio serial port
'   - servo output
'   - FPU

CON
  _CLKMODE = XTAL1 + PLL16X
  _XINFREQ = 5_000_000

  SERVO_PINS = 8
  RADIO_RX_PIN = 12
  RADIO_TX_PIN = 13
  IMU_RX_PIN = 14
  IMU_TX_PIN = 15
  GPS_RX_PIN = 24
  GPS_TX_PIN = 25

  TIMESTEP = 1.0 / 50.0
  MOTION = 60.0 * 60.0
  DRIFT = 1.0 * 1.0
  SLOSH = 1000.0 * 1000.0

  TILT_P = 5
  TILT_I = 0
  TILT_D = 0

  YAW_P = 5
  YAW_I = 0
  YAW_D = 0


OBJ
  flight: "FlightControl"
  gps: "FullDuplexSerial256"
  imu: "FullDuplexSerial256"
  orientation: "Orientation"
  radio: "FullDuplexSerial256"
  sensors: "Sensors"
  servo: "Servo32v3"


VAR
  long gps_data[sensors#GPS_NUM_VARS]
  word imu_data[sensors#IMU_NUM_VARS]
  long orient_data[orientation#NUM_VARS]
  long motor_speeds[4]


PUB main | ch, motor
  dira[SERVO_PINS..SERVO_PINS + 3]~~

  repeat motor from 0 to 3
    servo.set(SERVO_PINS + motor, 1000)
  servo.start

  radio.start(RADIO_RX_PIN, RADIO_TX_PIN, 0, 115200)
  radio.str(string(17, "Hello World", 13, 10))

  sensors.start(GPS_RX_PIN, GPS_TX_PIN, IMU_RX_PIN, IMU_TX_PIN)

  repeat
    repeat motor from 0 to 3
      servo.set(SERVO_PINS + motor, 1000)

    radio.str(string(15, "Command: "))
    case radio.rx
      "f":
        radio.str(string("f..."))
        if radio.rx == "l"
          if radio.rx == "y"
            fly
            next

        radio.str(string("(type 'fly' for flight control)", 13, 10))

      "g":
        connect_gps

      "i":
        connect_imu

      "m":
        radio.str(string("m..."))
        if radio.rx == "o"
          if radio.rx == "t"
            if radio.rx == "o"
              if radio.rx == "r"
                drive_motors
                next

        radio.str(string("(type 'motor' for motor control)", 13, 10))

      "o":
        print_orientation

      "s":
        print_raw_sensors

      other:
        radio.str(string("(unknown)", 13, 10))


PRI fly | collective, pitch, roll, yaw_rate, i, ch
  radio.str(string("Flight control, / to exit", 13, 10))
  orientation.init(@imu_calibration, TIMESTEP, MOTION, DRIFT, SLOSH)
  flight.init_tuning(TILT_P, TILT_I, TILT_D, YAW_P, YAW_I, YAW_D)

  collective := 1000
  pitch := 0
  roll := 0
  yaw_rate := 0
  flight.set_target(collective, pitch, roll, yaw_rate)

  repeat
    repeat
      sensors.get_imu_data(@imu_data)
    until orientation.update(@imu_data)
    orientation.get_data(@orient_data)
    flight.update(@orient_data)

    case ch := radio.rxcheck
      "/":
        radio.str(string(13, 10))
        return

      ",": collective := 1000 #> (collective - 10) <# 2000
      "<": collective := 1000 #> (collective - 100) <# 2000
      ".": collective := 1000 #> (collective + 10) <# 2000
      ">": collective := 1000 #> (collective + 100) <# 2000

      "w": pitch := -200 #> (pitch - 10) <# 200
      "s": pitch := -200 #> (pitch + 10) <# 200
      "a": roll := -200 #> (roll - 10) <# 200
      "d": roll := -200 #> (roll + 10) <# 200
      "q": yaw_rate := -400 #> (yaw_rate - 10) <# 400
      "Q": yaw_rate := -400 #> (yaw_rate - 100) <# 400
      "e": yaw_rate := -400 #> (yaw_rate + 10) <# 400
      "E": yaw_rate := -400 #> (yaw_rate + 100) <# 400

      "z":
        pitch := orient_data[orientation#PITCH_DEGREES_E1]
        roll := orient_data[orientation#ROLL_DEGREES_E1]
        yaw_rate := orient_data[orientation#YAW_RATE_DPS_E1]

      " ":
      -1:
      other:
        radio.str(string("(unknown command)", 13, 10))
        ch := -1

    if ch <> -1
      flight.set_target(collective, pitch, roll, yaw_rate)

    flight.get_speeds(@motor_speeds)
    repeat i from 0 to 3
      if collective =< 1000
        servo.set(SERVO_PINS + i, 1000)
      else
        servo.set(SERVO_PINS + i, 1000 #> motor_speeds[i] <# 1700)

    if ch <> -1
      radio.str(string(13, 10, "Target collective: "))
      radio.dec(collective)
      radio.str(string(" Pitch: "))
      radio.dec(pitch)
      radio.str(string(" Roll: "))
      radio.dec(roll)
      radio.str(string(" Yaw rate: "))
      radio.dec(yaw_rate)

      radio.str(string(13, 10, "Parameters: Tilt "))
      radio.dec(TILT_P)
      radio.str(string(" "))
      radio.dec(TILT_I)
      radio.str(string(" "))
      radio.dec(TILT_D)
      radio.str(string(" Yaw "))
      radio.dec(YAW_P)
      radio.str(string(" "))
      radio.dec(YAW_I)
      radio.str(string(" "))
      radio.dec(YAW_D)

      radio.str(string(13, 10, "Sensor pitch: "))
      radio.dec(orient_data[orientation#PITCH_DEGREES_E1])
      radio.str(string(" Roll: "))
      radio.dec(orient_data[orientation#ROLL_DEGREES_E1])
      radio.str(string(" Yaw rate: "))
      radio.dec(orient_data[orientation#YAW_RATE_DPS_E1])
      radio.str(string(" / Missed: "))
      radio.dec(orientation.get_missed)

      radio.str(string(13, 10, "Motors:"))
      repeat i from 0 to 3
        radio.str(string(" "))
        radio.dec(motor_speeds[i])
      radio.str(string(13, 10))


PRI connect_gps | ch
  sensors.stop
  gps.start(GPS_RX_PIN, GPS_TX_PIN, 0, 57600)
  radio.str(string("Connect to GPS, / to exit", 13, 10))
  repeat
    ch := gps.rxcheck
    if ch <> -1
      radio.tx(ch)
    ch := radio.rxcheck
    if ch <> -1
      gps.tx(ch)
  until ch == "/"
  gps.stop
  sensors.start(GPS_RX_PIN, GPS_TX_PIN, IMU_RX_PIN, IMU_TX_PIN)
  radio.str(string(13, 10))


PRI connect_imu | ch, escape, motor
  sensors.stop
  imu.start(IMU_RX_PIN, IMU_TX_PIN, 0, 115200)
  radio.str(string("Connect to IMU, / to exit", 13, 10))
  escape := false
  repeat
    ch := imu.rxcheck
    if ch <> -1
      radio.tx(ch)

    ch := radio.rxcheck
    if ch <> -1
      if escape
        if ch => "0" and ch =< "9"
          repeat motor from 0 to 3
            servo.set(SERVO_PINS + motor, 1000 + (ch - "0") * 40)
        escape := false
      elseif ch == "\"
        escape := true
      else
        imu.tx(ch)

  until ch == "/"

  imu.stop
  sensors.start(GPS_RX_PIN, GPS_TX_PIN, IMU_RX_PIN, IMU_TX_PIN)
  radio.str(string(13, 10))


PRI drive_motors | speed, motor, last_cnt, last_speed, msec, ch
  radio.str(string("otor control, / to exit", 13, 10))
  speed := 1000
  motor := -1
  last_cnt := cnt
  last_speed := 0
  msec := clkfreq / 1000
  ch := radio.rx
  repeat until ch == "/"
    if ch => "a" and ch =< "d"
      motor := ch - "a"
    elseif ch == "z"
      motor := -1
    elseif ch => "0" and ch =< "9"
      speed := 1000 + (ch - "0") * 100
    elseif ch == "@"
      speed := speed / 100 * 100 + 20
    elseif ch == "^"
      speed := speed / 100 * 100 + 60
    elseif ch == "&"
      speed := speed / 100 * 100 + 70
    elseif ch == "*"
      speed := speed / 100 * 100 + 80
    elseif ch == "("
      speed := speed / 100 * 100 + 90
    elseif ch == ")"
      speed := speed / 100 * 100
    elseif ch => "!" and ch =< ")"
      speed := speed / 100 * 100 + (ch - " ") * 10
    else
      radio.str(string("Unknown motor command "))
      radio.dec(ch)
      radio.str(string(13, 10))
      ch := radio.rx
      next

    if motor == -1
      radio.str(string("All motors"))
      repeat motor from 0 to 3
        servo.set(SERVO_PINS + motor, speed)
      motor := -1
    else
      servo.set(SERVO_PINS + motor, speed)
      radio.str(string("Motor "))
      radio.tx("A" + motor)

    radio.str(string(" duty cycle "))
    radio.dec(speed)
    if last_speed == speed
      radio.str(string("ms - delay "))
      radio.dec((cnt - last_cnt) / msec)
    radio.str(string("ms", 13, 10))

    last_cnt := cnt
    last_speed := speed
    ch := radio.rx

  radio.str(string(13, 10))


PRI print_orientation | ch, updates, last_cnt
  radio.str(string("Orientation filter output, / to exit", 13, 10))
  orientation.init(@imu_calibration, TIMESTEP, MOTION, DRIFT, SLOSH)
  updates := 0
  last_cnt := cnt
  repeat
    repeat
      sensors.get_imu_data(@imu_data)
    until orientation.update(@imu_data)

    updates += 1
    if (cnt - last_cnt) * 10 > clkfreq
      orientation.get_data(@orient_data)
      radio.str(string("Pitch: "))
      radio.dec(orient_data[orientation#PITCH_DEGREES_E1])
      radio.str(string(" Roll: "))
      radio.dec(orient_data[orientation#ROLL_DEGREES_E1])
      radio.str(string(" Yaw rate: "))
      radio.dec(orient_data[orientation#YAW_RATE_DPS_E1])
      radio.str(string(" / Missed: "))
      radio.dec(orientation.get_missed)
      radio.str(string(" in "))
      radio.dec(updates)
      radio.str(string(13, 10))
      last_cnt := cnt

  until radio.rxcheck == "/"
  radio.str(string(13, 10))


PRI print_raw_sensors | i
  sensors.get_gps_data(@gps_data)
  radio.str(string("Sensors", 13, 10, "GPS: "))
  radio.dec(gps_data[sensors#GPS_COUNT])
  radio.str(string(" count, "))
  radio.dec(sensors.get_gps_errors)
  radio.str(string(" errors", 13, 10, "  "))
  repeat i from 1 to sensors#GPS_NUM_VARS - 1
    radio.str(string(" "))
    radio.dec(gps_data[i])
  radio.str(string(13, 10))

  sensors.get_imu_data(@imu_data)
  radio.str(string("IMU: "))
  radio.dec(imu_data[sensors#IMU_COUNT])
  radio.str(string(" count, "))
  radio.dec(sensors.get_imu_errors)
  radio.str(string(" errors", 13, 10, "  "))
  repeat i from 1 to sensors#IMU_NUM_VARS - 1
    radio.str(string(" "))
    radio.dec(imu_data[i])
  radio.str(string(13, 10))


DAT
  imu_calibration long
  long 623.282,644.006,666.39,14.7080946667
  long 546.204,560.614,574.63,11.4182933333
  long 540.712,552.076,564.976,18.5851638889
  long 284.978,538.688,792.174,7.30852544444
  long 227.964,487.202,746.782,9.52011266667
  long 255.034,510.284,767.122,9.313424
  long 474.054,474.532,475.0,1.12955577778
  long 501.0,502.452,504.0,0.879994888889
  long 468.238,471.462,474.402,0.976072111111