Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-08 02:15:53 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

server update for booz messages

Browse Source
This commit is contained in:
Gautier Hattenberger
2009-05-10 19:51:15 +00:00
parent bd4ff3dee9
commit 7aa7310a5e
2 changed files with 13 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/ground_segment/tmtc/booz_server.ml
+12 -15
View File
@@ -136,28 +136,28 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
begin match a.nav_ref with
None -> (); (* No nav_ref yet *)
| Some nav_ref ->
(*a.pos <- LL.utm_add nav_ref (foi32value "east" /. pos_frac,
* foi32value "north" /. pos_frac)*)
let x = foi32value "north" /. pos_frac
and y = foi32value "east" /. pos_frac
and z = -. foi32value "up" /. pos_frac in
let (geo, _) = LL.geo_of_ecef LL.WGS84 (LL.ecef_of_ned !nav_ref_ecef (LL.make_ned [| x; y; z |])) in
a.pos <- LL.utm_of LL.WGS84 geo
and z = foi32value "up" /. pos_frac in
let (geo, _) = LL.geo_of_ecef LL.WGS84 (LL.ecef_of_ned !nav_ref_ecef (LL.make_ned [| x; y; -. z |])) in
let (_, ref_alt) = LL.geo_of_ecef LL.WGS84 !nav_ref_ecef in
a.pos <- LL.utm_of LL.WGS84 geo;
a.alt <- z +. ref_alt;
a.desired_east <- foi32value "carrot_east" /. pos_frac;
a.desired_north <- foi32value "carrot_north" /. pos_frac;
a.desired_altitude <- (foi32value "carrot_up" /. pos_frac) +. ref_alt;
a.desired_course <- foi32value "carrot_psi" /. angle_frac
(* a.desired_climb <- ?? *)
end;
a.alt <- foi32value "up" /. pos_frac;
let veast = foi32value "veast" /. speed_frac /. 10000.
and vnorth = foi32value "vnorth" /. speed_frac /. 10000. in
a.gspeed <- sqrt(vnorth*.vnorth +. veast*.veast);
a.climb <- foi32value "vup" /. speed_frac /. 10000.;
a.agl <- a.alt -. float (try Srtm.of_utm a.pos with _ -> 0);
a.course <- norm_course (foi32value "psi" /. angle_frac);
a.course <- norm_course ((Rad>>Deg) (foi32value "psi" /. angle_frac));
a.heading <- norm_course (foi32value "psi" /. angle_frac);
a.roll <- foi32value "phi" /. angle_frac;
a.pitch <- foi32value "theta" /. angle_frac;
a.desired_east <- foi32value "carrot_east" /. pos_frac;
a.desired_north <- foi32value "carrot_north" /. pos_frac;
a.desired_altitude <- foi32value "carrot_up" /. pos_frac;
a.desired_course <- foi32value "carrot_psi" /. angle_frac;
(* a.desired_climb <- ?? *)
a.throttle <- foi32value "thrust" /. 2.; (* thrust / 200 * 100 *)
(*a.unix_time <- LL.unix_time_of_tow (truncate (fvalue "itow" /. 1000.));
a.itow <- Int32.of_float (fvalue "itow");
@@ -171,15 +171,12 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
a.kill_mode <- ivalue "ap_motors_on" == 0;
a.bat <- fvalue "vsupply" /. 10.;
| "BOOZ2_NAV_REF" ->
(*let lat = (Rad>>Deg) (foi32value "lat" /. gps_frac)
and lon = (Rad>>Deg) (foi32value "lon" /. gps_frac) in*)
let x = (foi32value "x" /. 100.)
and y = (foi32value "y" /. 100.)
and z = (foi32value "z" /. 100.) in
nav_ref_ecef := LL.make_ecef [| x; y; z |];
let (geo, _) = LL.geo_of_ecef LL.WGS84 !nav_ref_ecef in
a.nav_ref <- Some (LL.utm_of LL.WGS84 geo);
(*a.nav_ref <- Some (LL.utm_of LL.WGS84 (LL.make_geo_deg lat lon));*)
| "BOOZ2_NAV_STATUS" ->
a.block_time <- ivalue "block_time";
a.stage_time <- ivalue "stage_time";
sw/ground_segment/tmtc/server_globals.ml
+1 -1
View File
@@ -9,7 +9,7 @@ let gaz_modes = [|"MANUAL";"GAZ";"CLIMB";"ALT"|]
let lat_modes = [|"MANUAL";"ROLL_RATE";"ROLL";"COURSE"|]
let gps_modes = [|"NOFIX";"DRO";"2D";"3D";"GPSDRO"|]
let gps_hybrid_modes = [|"OFF";"ON"|]
let horiz_modes = [|"WAYPOINT";"ROUTE";"CIRCLE"|]
let horiz_modes = [|"WAYPOINT";"ROUTE";"CIRCLE";"ATTITUDE"|]
let string_of_values = fun values ->
String.concat " " (List.map (fun (_, v) -> Pprz.string_of_value v) values)