# ここから、初期化
# 僕の大好きなArduinoでいうところのsetupにあたる

# GPS未受信時の時刻初期値
#TinyFeather::notify_gps_time(1741, 0) 
#TinyFeather::simulate_1pps

servo_out = [0] * 8 # サーボPWM出力は8ch分、単位は[ms]で1500msで中立

def rad2deg(rad) 
  rad / Math::PI * 180
end

f = TinyFeather::IO::file("user_log.csv") # ログファイルの準備

# ここまで、初期化

# 次に誘導制御の1ループを定義
# 僕の大好きなArduinoでいうところのloopにあたる
TinyFeather::guidance_control = lambda{|gc_info|
  # gc_infoから誘導制御に必要な情報を取得できる
  # 例えば航法系のインスタンス変数として
  #  itow  : GPS(週)時刻
  #  lat   : 経度[rad]
  #  lng   : 経度[rad]
  #  h     : 高度[m]
  #  vn    : 北方向速度[m/s]
  #  ve    : 東方向速度[m/s]
  #  vd    : 下方向速度[m/s]
  #  psi   : 真方位角[rad]
  #  phi   : ロール角[rad]
  #  theta : ピッチ角[rad]
  #  accel : 機体軸加速度[m/s^2] => [x, y, z]
  #  omega : 機体軸角速度[rad/s] => [x, y, z]
  
  # 適当な制御則
  servo_out[0] = (rad2deg(gc_info.phi) * 10).to_i + 1500 # ロール角に対するP制御(ゲインx10)
  servo_out[1] = (rad2deg(gc_info.theta) * 10).to_i + 1500 # ピッチ角に対するP制御(ゲインx10)
  
  # 指令値に対する制限
  servo_out.collect!{|v|
    if v > 2200 then
      2200 # 2200msが最大
    elsif v < 800 then
      800 #800msが最小
    else
      v
    end
  }
  
  # 指令値を書き出し
  TinyFeather::servo_write(servo_out)
  
  # ログをとる
  f.write "#{gc_info.itow},#{servo_out[0]},#{servo_out[1]}\n"
}