hitl.ml

(*
 *  $Id$
 *
 * Hardware In The Loop
 *
 * Copyright (C) 2004 Pascal Brisset, Antoine Drouin
 *
 * This file is part of paparazzi.
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, write to
 * the Free Software Foundation, 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 *
 *)

open Printf
open Stdlib
module LL = Latlong
open LL

module TelePprz = Pprz.Messages(struct let name = "telemetry" end)
module DatalinkPprz = Pprz.Messages(struct let name = "datalink" end)
module GroundPprz = Pprz.Messages(struct let name = "ground" end)

module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct
  let my_id = ref (-1)

  let init = fun id (_:GPack.box) ->
    my_id := id

  let boot = fun time_scale -> ()

  let scale = fun value s -> truncate (value *. s)

  open Gps

  let array_of_string = fun s ->
    Array.init (String.length s) (fun i -> Pprz.Int (Char.code s.[i]))

  let send_ubx_message = fun class_name msg_name vs ->
    let class_id, msg_id, msg_spec = Ubx.message class_name msg_name in
    let ubx_payload = Ubx.ubx_payload msg_spec vs in
    Debug.call 'h' (fun f -> fprintf f "ubx_payload: %d:%s\n" msg_id (Debug.xprint ubx_payload));
    let a = array_of_string ubx_payload in
    let s = Pprz.string_of_value (Pprz.Array a) in
    let vs = ["ac_id",Pprz.Int !my_id;
	      "class", Pprz.Int class_id;
	      "id", Pprz.Int msg_id;
	      "ubx_payload", Pprz.String s] in

    try
      DatalinkPprz.message_send "hitl" "HITL_UBX" vs
    with
      exc -> prerr_endline (Printexc.to_string exc)


  let gps = fun gps ->
    let utm = LL.utm_of LL.WGS84 gps.wgs84
    and lon = (Rad>>Deg)gps.wgs84.posn_long