[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [5073] Change internal representation of positions f
|
From: |
Gautier Hattenberger |
|
Subject: |
[paparazzi-commits] [5073] Change internal representation of positions from utm to lat/ lon wgs84 |
|
Date: |
Mon, 19 Jul 2010 15:57:54 +0000 |
Revision: 5073
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5073
Author: gautier
Date: 2010-07-19 15:57:53 +0000 (Mon, 19 Jul 2010)
Log Message:
-----------
Change internal representation of positions from utm to lat/lon wgs84
Handle properly different navigation references between fw and quadrotors
Modified Paths:
--------------
paparazzi3/trunk/sw/ground_segment/tmtc/aircraft.ml
paparazzi3/trunk/sw/ground_segment/tmtc/aircraft.mli
paparazzi3/trunk/sw/ground_segment/tmtc/airprox.ml
paparazzi3/trunk/sw/ground_segment/tmtc/booz_server.ml
paparazzi3/trunk/sw/ground_segment/tmtc/fw_server.ml
paparazzi3/trunk/sw/ground_segment/tmtc/kml.ml
paparazzi3/trunk/sw/ground_segment/tmtc/server.ml
Modified: paparazzi3/trunk/sw/ground_segment/tmtc/aircraft.ml
===================================================================
--- paparazzi3/trunk/sw/ground_segment/tmtc/aircraft.ml 2010-07-19 01:03:52 UTC
(rev 5072)
+++ paparazzi3/trunk/sw/ground_segment/tmtc/aircraft.ml 2010-07-19 15:57:53 UTC
(rev 5073)
@@ -22,20 +22,14 @@
*
*)
+open Latlong
+
type ac_cam = {
mutable phi : float; (* Rad, right = >0 *)
mutable theta : float; (* Rad, front = >0 *)
mutable target : (float * float) (* meter*meter relative *)
}
-type inflight_calib = {
- mutable if_mode : int; (* DOWN|OFF|UP *)
- mutable if_val1 : float;
- mutable if_val2 : float
- }
-
-type contrast_status = string (** DEFAULT|WAITING|SET *)
-
type rc_status = string (** OK, LOST, REALLY_LOST *)
type rc_mode = string (** MANUAL, AUTO, FAILSAFE *)
type fbw = {
@@ -66,34 +60,58 @@
}
type horiz_mode =
- Circle of Latlong.utm * int
- | Segment of Latlong.utm * Latlong.utm
+ Circle of Latlong.geographic * int
+ | Segment of Latlong.geographic * Latlong.geographic
| UnknownHorizMode
-type waypoint = { altitude : float; wp_utm : Latlong.utm }
+type nav_ref =
+ Geo of Latlong.geographic
+ | Utm of Latlong.utm
+ | Ltp of Latlong.ecef
+let add_pos_to_nav_ref = fun nav_ref ?(z = 0.) (x, y) ->
+ let rec lat_of_xy = fun lat last geo (_x, _y) n e ->
+ if n > 0 && abs_float (lat -. last) > e then
+ lat_of_xy (asin ( (_y +. (sin geo.posn_lat)*.(cos lat)*.(cos (asin (_x
/. cos lat)))) /. cos geo.posn_lat)) lat geo (_x, _y) (n-1) e
+ else
+ lat
+ in
+ match nav_ref with
+ Geo geo ->
+ let m_to_rad = 0.0005399568034557235 *. 0.00029088820866572159 in
+ let lat = lat_of_xy (geo.posn_lat +. asin (y*.m_to_rad)) 0. geo
(x*.m_to_rad, y *.m_to_rad) 10 1.e-7 in
+ Latlong.make_geo lat (geo.posn_long +. asin (x*.m_to_rad /. cos lat))
+ | Utm utm ->
+ Latlong.of_utm Latlong.WGS84 (Latlong.utm_add utm (x, y))
+ | Ltp ecef ->
+ let ned = Latlong.make_ned [| y; x; 0. |] in (* FIXME z=0 *)
+ let (geo, _) = Latlong.geo_of_ecef Latlong.WGS84 (Latlong.ecef_of_ned
ecef ned) in
+ geo
+
+type waypoint = { altitude : float; wp_geo : Latlong.geographic }
+
type aircraft = {
id : string;
name : string;
flight_plan : Xml.xml;
airframe : Xml.xml;
- mutable pos : Latlong.utm;
+ mutable pos : Latlong.geographic;
mutable unix_time : float;
mutable itow : int32; (* ms *)
mutable roll : float;
mutable pitch : float;
mutable heading : float; (* rad, CW 0=N *)
- mutable nav_ref : Latlong.utm option;
- mutable desired_east : float;
- mutable desired_north : float;
- mutable desired_altitude : float;
- mutable desired_course : float;
- mutable desired_climb : float;
mutable gspeed : float; (* m/s *)
mutable course : float; (* rad *)
mutable alt : float;
mutable agl : float;
mutable climb : float;
+ mutable nav_ref : nav_ref option;
+ mutable d_hmsl : float;
+ mutable desired_pos : Latlong.geographic;
+ mutable desired_altitude : float;
+ mutable desired_course : float;
+ mutable desired_climb : float;
mutable cur_block : int;
mutable cur_stage : int;
mutable throttle : float;
@@ -112,7 +130,6 @@
cam : ac_cam;
mutable gps_mode : int;
mutable gps_Pacc : int;
- inflight_calib : inflight_calib;
fbw : fbw;
svinfo : svinfo array;
waypoints : (int, waypoint) Hashtbl.t;
@@ -123,7 +140,7 @@
dl_setting_values : float array;
mutable nb_dl_setting_values : int;
mutable survey : (Latlong.geographic * Latlong.geographic) option;
- mutable last_bat_msg_date : float;
+ mutable last_msg_date : float;
mutable time_since_last_survey_msg : float;
mutable dist_to_wp : float
}
@@ -132,27 +149,27 @@
let new_aircraft = fun id name fp airframe ->
let svsinfo_init = Array.init gps_nb_channels (fun _ -> svinfo_init ()) in
- { id = id ; name = name; roll = 0.; pitch = 0.; desired_east = 0.;
desired_north = 0.; flight_plan = fp; airframe = airframe; dist_to_wp = 0.;
- desired_course = 0.;
- gspeed=0.; course = 0.; heading = 0.; alt=0.; climb=0.; cur_block=0;
cur_stage=0;
- throttle = 0.; throttle_accu = 0.; rpm = 0.; temp = 0.; bat = 42.; amp =
0.; energy = 0; ap_mode= -1; agl = 0.;
+ { id = id ; name = name; flight_plan = fp; airframe = airframe;
+ pos = { Latlong.posn_lat = 0.; posn_long = 0. };
+ unix_time = 0.; itow = Int32.of_int 0;
+ roll = 0.; pitch = 0.;
+ gspeed=0.; course = 0.; heading = 0.; alt=0.; climb=0.; agl = 0.;
+ nav_ref = None; d_hmsl = 0.;
+ desired_pos = { Latlong.posn_lat = 0.; posn_long = 0. };
+ desired_course = 0.; desired_altitude = 0.; desired_climb = 0.;
+ cur_block=0; cur_stage=0;
+ flight_time = 0; stage_time = 0; block_time = 0;
+ throttle = 0.; throttle_accu = 0.; rpm = 0.; temp = 0.; bat = 42.; amp =
0.; energy = 0; ap_mode= -1;
+ kill_mode = false;
gaz_mode= -1; lateral_mode= -1;
gps_mode =0; gps_Pacc = 0; periodic_callbacks = [];
- desired_altitude = 0.;
- desired_climb = 0.;
- pos = { Latlong.utm_x = 0.; utm_y = 0.; utm_zone = 0 };
- unix_time = 0.; itow = Int32.of_int 0;
- nav_ref = None;
cam = { phi = 0.; theta = 0. ; target=(0.,0.)};
- inflight_calib = { if_mode = 1 ; if_val1 = 0.; if_val2 = 0.};
- kill_mode = false;
fbw = { rc_status = "???"; rc_mode = "???" };
svinfo = svsinfo_init;
dl_setting_values = Array.create max_nb_dl_setting_values 42.;
nb_dl_setting_values = 0;
- flight_time = 0; stage_time = 0; block_time = 0;
horiz_mode = UnknownHorizMode;
horizontal_mode = 0;
- waypoints = Hashtbl.create 3; survey = None; last_bat_msg_date = 0.;
+ waypoints = Hashtbl.create 3; survey = None; last_msg_date = 0.;
dist_to_wp = 0.;
time_since_last_survey_msg = 1729.
}
Modified: paparazzi3/trunk/sw/ground_segment/tmtc/aircraft.mli
===================================================================
--- paparazzi3/trunk/sw/ground_segment/tmtc/aircraft.mli 2010-07-19
01:03:52 UTC (rev 5072)
+++ paparazzi3/trunk/sw/ground_segment/tmtc/aircraft.mli 2010-07-19
15:57:53 UTC (rev 5073)
@@ -30,13 +30,6 @@
mutable target : (float * float) (* meter*meter relative *)
}
-type inflight_calib = {
- mutable if_mode : int;
- mutable if_val1 : float;
- mutable if_val2 : float;
-}
-type contrast_status = string
-
type rc_status = string
type rc_mode = string
type fbw = { mutable rc_status : rc_status; mutable rc_mode : rc_mode; }
@@ -52,34 +45,41 @@
}
val svinfo_init : unit -> svinfo
type horiz_mode =
- Circle of Latlong.utm * int
- | Segment of Latlong.utm * Latlong.utm
+ Circle of Latlong.geographic * int
+ | Segment of Latlong.geographic * Latlong.geographic
| UnknownHorizMode
-type waypoint = { altitude : float; wp_utm : Latlong.utm }
+type nav_ref =
+ Geo of Latlong.geographic
+ | Utm of Latlong.utm
+ | Ltp of Latlong.ecef
+val add_pos_to_nav_ref : nav_ref -> ?z:float -> (float * float) ->
Latlong.geographic
+
+type waypoint = { altitude : float; wp_geo : Latlong.geographic }
+
type aircraft = {
id : string;
name : string;
flight_plan : Xml.xml;
airframe : Xml.xml;
- mutable pos : Latlong.utm;
+ mutable pos : Latlong.geographic;
mutable unix_time : float;
mutable itow : int32;
mutable roll : float;
mutable pitch : float;
mutable heading : float; (* rad *)
- mutable nav_ref : Latlong.utm option;
- mutable desired_east : float;
- mutable desired_north : float;
- mutable desired_altitude : float;
- mutable desired_course : float;
- mutable desired_climb : float;
mutable gspeed : float; (* m/s *)
mutable course : float; (* rad *)
mutable alt : float;
mutable agl : float; (* m *)
mutable climb : float;
+ mutable nav_ref : nav_ref option;
+ mutable d_hmsl : float; (* difference between geoid and ellipsoid *)
+ mutable desired_pos : Latlong.geographic;
+ mutable desired_altitude : float;
+ mutable desired_course : float;
+ mutable desired_climb : float;
mutable cur_block : int;
mutable cur_stage : int;
mutable throttle : float;
@@ -98,7 +98,6 @@
cam : ac_cam;
mutable gps_mode : int;
mutable gps_Pacc : int;
- inflight_calib : inflight_calib;
fbw : fbw;
svinfo : svinfo array;
waypoints : (int, waypoint) Hashtbl.t;
@@ -109,7 +108,7 @@
dl_setting_values : float array;
mutable nb_dl_setting_values : int;
mutable survey : (Latlong.geographic * Latlong.geographic) option;
- mutable last_bat_msg_date : float;
+ mutable last_msg_date : float;
mutable time_since_last_survey_msg : float;
mutable dist_to_wp : float
}
Modified: paparazzi3/trunk/sw/ground_segment/tmtc/airprox.ml
===================================================================
--- paparazzi3/trunk/sw/ground_segment/tmtc/airprox.ml 2010-07-19 01:03:52 UTC
(rev 5072)
+++ paparazzi3/trunk/sw/ground_segment/tmtc/airprox.ml 2010-07-19 15:57:53 UTC
(rev 5073)
@@ -39,8 +39,10 @@
(** and if the horizontal distance is less than 100 meters (5s between
*)
(** 2 aircraft at 10m/s
*)
let airprox = fun aircraft1 aircraft2 ->
- let x1 = aircraft1.pos.utm_x and x2 = aircraft2.pos.utm_x and
- y1 = aircraft1.pos.utm_y and y2 = aircraft2.pos.utm_y and
+ let p1 = Latlong.utm_of Latlong.WGS84 aircraft1.pos in
+ let p2 = Latlong.utm_of Latlong.WGS84 aircraft2.pos in
+ let x1 = p1.utm_x and x2 = p2.utm_x and
+ y1 = p1.utm_y and y2 = p2.utm_y and
z1 = aircraft1.alt and z2 = aircraft2.alt in
let alt_difference = abs_float (z1 -. z2) and
dist = distance (x1, y1) (x2, y2) in
@@ -50,8 +52,10 @@
(** level is warning if the distance between both aircraft is increasing
*)
(** level is crictical otherwise
*)
let airprox_level = fun aircraft1 aircraft2 ->
- let x1 = aircraft1.pos.utm_x and x2 = aircraft2.pos.utm_x and
- y1 = aircraft1.pos.utm_y and y2 = aircraft2.pos.utm_y in
+ let p1 = Latlong.utm_of Latlong.WGS84 aircraft1.pos in
+ let p2 = Latlong.utm_of Latlong.WGS84 aircraft2.pos in
+ let x1 = p1.utm_x and x2 = p2.utm_x and
+ y1 = p1.utm_y and y2 = p2.utm_y in
let d0 = distance (x1, y1) (x2, y2) in
let course1 = aircraft1.course and course2 = aircraft2.course and
speed1 = aircraft1.gspeed and speed2 = aircraft2.gspeed in
Modified: paparazzi3/trunk/sw/ground_segment/tmtc/booz_server.ml
===================================================================
--- paparazzi3/trunk/sw/ground_segment/tmtc/booz_server.ml 2010-07-19
01:03:52 UTC (rev 5072)
+++ paparazzi3/trunk/sw/ground_segment/tmtc/booz_server.ml 2010-07-19
15:57:53 UTC (rev 5073)
@@ -32,7 +32,6 @@
module U = Unix
module Dl_Pprz = Pprz.Messages (struct let name = "datalink" end)
-let nav_ref_ecef = ref (LL.make_ecef [|0.;0.;0.|])
let nav_ref_alt = ref 0.
let nav_ref_hmsl = ref 0.
@@ -89,7 +88,7 @@
let update_waypoint = fun ac wp_id p alt ->
- let new_wp = { altitude = alt; wp_utm = p } in
+ let new_wp = { altitude = alt; wp_geo = p } in
try
let prev_wp = Hashtbl.find ac.waypoints wp_id in
if new_wp <> prev_wp then
@@ -117,10 +116,20 @@
let angle_frac = 2. ** 12.
let gps_frac = 1e7
-let utm_hmsl_of_ltp = fun ned ->
- let (geo, alt) = LL.geo_of_ecef LL.WGS84 (LL.ecef_of_ned !nav_ref_ecef ned)
in
- (LL.utm_of LL.WGS84 geo, alt +. !nav_ref_hmsl -. !nav_ref_alt)
+let geo_hmsl_of_ltp = fun ned nav_ref d_hmsl ->
+ match nav_ref with
+ | Ltp nav_ref_ecef ->
+ let (geo, alt) = LL.geo_of_ecef LL.WGS84 (LL.ecef_of_ned nav_ref_ecef ned)
in
+ (geo, alt +. d_hmsl)
+ | _ -> (LL.make_geo 0. 0., 0.)
+let hmsl_of_ref = fun nav_ref d_hmsl ->
+ match nav_ref with
+ | Ltp nav_ref_ecef ->
+ let (_, alt) = LL.geo_of_ecef LL.WGS84 nav_ref_ecef in
+ alt +. d_hmsl
+ | _ -> 0.
+
let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
let value = fun x -> try Pprz.assoc x values with Not_found -> failwith
(sprintf "Error: field '%s' not found\n" x) in
@@ -136,7 +145,7 @@
(*and i32value = fun x -> i32value (value x)*)
and foi32value = fun x -> foi32value (value x) in
if not (msg.Pprz.name = "DOWNLINK_STATUS") then
- a.last_bat_msg_date <- U.gettimeofday ();
+ a.last_msg_date <- U.gettimeofday ();
match msg.Pprz.name with
"BOOZ2_FP" ->
begin match a.nav_ref with
@@ -145,12 +154,14 @@
let north = foi32value "north" /. pos_frac
and east = foi32value "east" /. pos_frac
and up = foi32value "up" /. pos_frac in
- let (utm, h) = utm_hmsl_of_ltp (LL.make_ned [| north; east; -. up
|]) in
- a.pos <- utm;
+ let (geo, h) = geo_hmsl_of_ltp (LL.make_ned [| north; east; -. up
|]) nav_ref a.d_hmsl in
+ a.pos <- geo;
a.alt <- h;
- a.desired_east <- foi32value "carrot_east" /. pos_frac;
- a.desired_north <- foi32value "carrot_north" /. pos_frac;
- a.desired_altitude <- (foi32value "carrot_up" /. pos_frac) +.
!nav_ref_hmsl;
+ let desired_east = foi32value "carrot_east" /. pos_frac
+ and desired_north = foi32value "carrot_north" /. pos_frac
+ and desired_alt = foi32value "carrot_up" /. pos_frac in
+ a.desired_pos <- Aircraft.add_pos_to_nav_ref nav_ref ~z:desired_alt
(desired_east, desired_north);
+ a.desired_altitude <- desired_alt +. (hmsl_of_ref nav_ref a.d_hmsl);
a.desired_course <- foi32value "carrot_psi" /. angle_frac
(* a.desired_climb <- ?? *)
end;
@@ -158,7 +169,7 @@
and vnorth = foi32value "vnorth" /. speed_frac in
a.gspeed <- sqrt(vnorth*.vnorth +. veast*.veast);
a.climb <- foi32value "vup" /. speed_frac;
- a.agl <- a.alt -. float (try Srtm.of_utm a.pos with _ -> 0);
+ a.agl <- a.alt -. float (try Srtm.of_wgs84 a.pos with _ -> 0);
a.course <- norm_course ((Rad>>Deg) (foi32value "psi" /. angle_frac));
a.heading <- norm_course (foi32value "psi" /. angle_frac);
a.roll <- foi32value "phi" /. angle_frac;
@@ -175,19 +186,15 @@
a.ap_mode <- check_index (get_pprz_mode (ivalue "ap_mode"))
ap_modes "BOOZ_AP_MODE";
a.kill_mode <- ivalue "ap_motors_on" == 0;
a.bat <- fvalue "vsupply" /. 10.;
- | "BOOZ2_GPS" ->
- a.gps_Pacc <- ivalue "Pacc"
| "BOOZ2_INS_REF" ->
let x = foi32value "ecef_x0" /. 100.
and y = foi32value "ecef_y0" /. 100.
and z = foi32value "ecef_z0" /. 100.
and alt = foi32value "alt0" /. 100.
and hmsl = foi32value "hmsl0" /. 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);
- nav_ref_alt := alt;
- nav_ref_hmsl := hmsl;
+ let nav_ref_ecef = LL.make_ecef [| x; y; z |] in
+ a.nav_ref <- Some (Ltp nav_ref_ecef);
+ a.d_hmsl <- hmsl -. alt;
| "BOOZ2_NAV_STATUS" ->
a.block_time <- ivalue "block_time";
a.stage_time <- ivalue "stage_time";
@@ -202,8 +209,8 @@
let east = foi32value "east" /. pos_frac
and north = foi32value "north" /. pos_frac
and up = foi32value "up" /. pos_frac in
- let (utm, h) = utm_hmsl_of_ltp (LL.make_ned [| north; east; -. up
|]) in
- update_waypoint a (ivalue "wp_id") utm h;
+ let (geo, h) = geo_hmsl_of_ltp (LL.make_ned [| north; east; -. up
|]) nav_ref a.d_hmsl in
+ update_waypoint a (ivalue "wp_id") geo h;
| None -> (); (** Can't use this message *)
end
| _ -> ()
Modified: paparazzi3/trunk/sw/ground_segment/tmtc/fw_server.ml
===================================================================
--- paparazzi3/trunk/sw/ground_segment/tmtc/fw_server.ml 2010-07-19
01:03:52 UTC (rev 5072)
+++ paparazzi3/trunk/sw/ground_segment/tmtc/fw_server.ml 2010-07-19
15:57:53 UTC (rev 5073)
@@ -74,7 +74,7 @@
let update_waypoint = fun ac wp_id p alt ->
- let new_wp = { altitude = alt; wp_utm = p } in
+ let new_wp = { altitude = alt; wp_geo = p } in
try
let prev_wp = Hashtbl.find ac.waypoints wp_id in
if new_wp <> prev_wp then
@@ -83,7 +83,10 @@
Not_found ->
Hashtbl.add ac.waypoints wp_id new_wp
+
+
let heading_from_course = ref false
+
let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
let value = fun x -> try Pprz.assoc x values with Not_found -> failwith
(sprintf "Error: field '%s' not found\n" x) in
@@ -97,22 +100,38 @@
| _ -> f
and ivalue = fun x -> ivalue (value x) in
if not (msg.Pprz.name = "DOWNLINK_STATUS") then
- a.last_bat_msg_date <- U.gettimeofday ();
+ a.last_msg_date <- U.gettimeofday ();
match msg.Pprz.name with
"GPS" ->
- a.pos <- { LL.utm_x = fvalue "utm_east" /. 100.;
- utm_y = fvalue "utm_north" /. 100.;
- utm_zone = ivalue "utm_zone" };
+ let p = { LL.utm_x = fvalue "utm_east" /. 100.;
+ utm_y = fvalue "utm_north" /. 100.;
+ utm_zone = ivalue "utm_zone" } in
+ a.pos <- LL.of_utm WGS84 p;
a.unix_time <- LL.unix_time_of_tow (truncate (fvalue "itow" /. 1000.));
a.itow <- Int32.of_float (fvalue "itow");
a.gspeed <- fvalue "speed" /. 100.;
a.course <- norm_course ((Deg>>Rad)(fvalue "course" /. 10.));
if !heading_from_course then
- a.heading <- a.course;
- a.agl <- a.alt -. float (try Srtm.of_utm a.pos with _ -> 0);
+ a.heading <- a.course;
+ a.agl <- a.alt -. float (try Srtm.of_wgs84 a.pos with _ -> 0);
a.gps_mode <- check_index (ivalue "mode") gps_modes "GPS_MODE";
if a.gspeed > 3. && a.ap_mode = _AUTO2 then
- Wind.update ac_name a.gspeed a.course
+ Wind.update ac_name a.gspeed a.course
+ | "GPS_LLA" ->
+ let lat = ivalue "lat"
+ and lon = ivalue "lon" in
+ let geo = make_geo_deg (float lat /. 1e7) (float lon /. 1e7) in
+ a.pos <- geo;
+ a.unix_time <- LL.unix_time_of_tow (truncate (fvalue "itow" /. 1000.));
+ a.itow <- Int32.of_float (fvalue "itow");
+ a.gspeed <- fvalue "speed" /. 100.;
+ a.course <- norm_course ((Deg>>Rad)(fvalue "course" /. 10.));
+ if !heading_from_course then
+ a.heading <- a.course;
+ a.agl <- a.alt -. float (try Srtm.of_wgs84 a.pos with _ -> 0);
+ a.gps_mode <- check_index (ivalue "mode") gps_modes "GPS_MODE";
+ if a.gspeed > 3. && a.ap_mode = _AUTO2 then
+ Wind.update ac_name a.gspeed a.course
| "GPS_SOL" ->
a.gps_Pacc <- ivalue "Pacc"
| "ESTIMATOR" ->
@@ -120,24 +139,37 @@
a.climb <- fvalue "z_dot"
| "DESIRED" ->
(* Trying to be compatible with old logs ... *)
- a.desired_east <- (try fvalue "x" with _ -> fvalue "desired_x");
- a.desired_north <- (try fvalue "y" with _ -> fvalue "desired_y");
+ begin match a.nav_ref with
+ Some nav_ref ->
+ let x = (try fvalue "x" with _ -> fvalue "desired_x")
+ and y = (try fvalue "y" with _ -> fvalue "desired_y") in
+ (*let target_utm = LL.utm_add (LL.utm_of nav_ref) (x, y) in
+ let target_geo = LL.of_utm WGS84 target_utm in
+ a.desired_pos <- target_geo;*)
+ a.desired_pos <- Aircraft.add_pos_to_nav_ref nav_ref (x, y);
+ | None -> ()
+ end;
a.desired_altitude <- (try fvalue "altitude" with _ -> fvalue
"desired_altitude");
a.desired_climb <- (try fvalue "climb" with _ -> fvalue "desired_climb");
begin try a.desired_course <- norm_course (fvalue "course") with
_ -> () end
| "NAVIGATION_REF" ->
- a.nav_ref <- Some { utm_x = fvalue "utm_east"; utm_y = fvalue
"utm_north"; utm_zone = ivalue "utm_zone" }
+ a.nav_ref <- Some (Utm { utm_x = fvalue "utm_east"; utm_y = fvalue
"utm_north"; utm_zone = ivalue "utm_zone" })
+ | "NAVIGATION_REF_LLA" ->
+ let lat = ivalue "lat"
+ and lon = ivalue "lon" in
+ let geo = make_geo_deg (float lat /. 1e7) (float lon /. 1e7) in
+ a.nav_ref <- Some (Geo geo)
| "ATTITUDE" ->
let roll = fvalue "phi"
and pitch = fvalue "theta" in
if (List.assoc "phi" msg.Pprz.fields).Pprz._type = Pprz.Scalar "int16"
then begin (* Compatibility with old message in degrees *)
- a.roll <- roll /. 180. *. pi;
- a.pitch <- pitch /. 180. *. pi;
- heading_from_course := true; (* Awfull hack to get heading from GPS *)
+ a.roll <- roll /. 180. *. pi;
+ a.pitch <- pitch /. 180. *. pi;
+ heading_from_course := true; (* Awfull hack to get heading from GPS *)
end else begin
- a.roll <- roll;
- a.pitch <- pitch;
- a.heading <- norm_course (fvalue "psi")
+ a.roll <- roll;
+ a.pitch <- pitch;
+ a.heading <- norm_course (fvalue "psi")
end
| "NAVIGATION" ->
a.cur_block <- ivalue "cur_block";
@@ -162,17 +194,17 @@
let mcu1_status = ivalue "mcu1_status" in
(** c.f. link_autopilot.h *)
a.fbw.rc_status <-
- if mcu1_status land 0b1 > 0
- then "OK"
- else if mcu1_status land 0b10 > 0
- then "NONE"
- else "LOST";
- a.fbw.rc_mode <-
- if mcu1_status land 0b1000 > 0
- then "FAILSAFE"
- else if mcu1_status land 0b100 > 0
- then "AUTO"
- else "MANUAL";
+ if mcu1_status land 0b1 > 0
+ then "OK"
+ else if mcu1_status land 0b10 > 0
+ then "NONE"
+ else "LOST";
+ a.fbw.rc_mode <-
+ if mcu1_status land 0b1000 > 0
+ then "FAILSAFE"
+ else if mcu1_status land 0b100 > 0
+ then "AUTO"
+ else "MANUAL";
| "CAM" ->
a.cam.phi <- (Deg>>Rad) (fvalue "phi");
a.cam.theta <- (Deg>>Rad) (fvalue "theta");
@@ -181,41 +213,41 @@
let i = ivalue "chn" in
assert(i < Array.length a.svinfo);
a.svinfo.(i) <- {
- svid = ivalue "SVID";
- flags = ivalue "Flags";
- qi = ivalue "QI";
- cno = ivalue "CNO";
- elev = ivalue "Elev";
- azim = ivalue "Azim";
- age = 0
+ svid = ivalue "SVID";
+ flags = ivalue "Flags";
+ qi = ivalue "QI";
+ cno = ivalue "CNO";
+ elev = ivalue "Elev";
+ azim = ivalue "Azim";
+ age = 0
}
| "CIRCLE" ->
begin
- match a.nav_ref, a.horizontal_mode with
- Some nav_ref, 2 -> (** FIXME *)
- a.horiz_mode <- Circle (LL.utm_add nav_ref (fvalue "center_east",
fvalue "center_north"), truncate (fvalue "radius"));
- if !Kml.enabled then Kml.update_horiz_mode a
- | _ -> ()
+ match a.nav_ref, a.horizontal_mode with
+ Some nav_ref, 2 -> (** FIXME *)
+ a.horiz_mode <- Circle (Aircraft.add_pos_to_nav_ref nav_ref (fvalue
"center_east", fvalue "center_north"), truncate (fvalue "radius"));
+ if !Kml.enabled then Kml.update_horiz_mode a
+ | _ -> ()
end
| "SEGMENT" ->
begin
- match a.nav_ref, a.horizontal_mode with
- Some nav_ref, 1 -> (** FIXME *)
- let p1 = LL.utm_add nav_ref (fvalue "segment_east_1", fvalue
"segment_north_1")
- and p2 = LL.utm_add nav_ref (fvalue "segment_east_2", fvalue
"segment_north_2") in
- a.horiz_mode <- Segment (p1, p2);
- if !Kml.enabled then Kml.update_horiz_mode a
- | _ -> ()
+ match a.nav_ref, a.horizontal_mode with
+ Some nav_ref, 1 -> (** FIXME *)
+ let p1 = Aircraft.add_pos_to_nav_ref nav_ref (fvalue
"segment_east_1", fvalue "segment_north_1")
+ and p2 = Aircraft.add_pos_to_nav_ref nav_ref (fvalue
"segment_east_2", fvalue "segment_north_2") in
+ a.horiz_mode <- Segment (p1, p2);
+ if !Kml.enabled then Kml.update_horiz_mode a
+ | _ -> ()
end
| "SURVEY" ->
begin
- a.time_since_last_survey_msg <- 0.;
- match a.nav_ref with
- Some nav_ref ->
- let p1 = LL.utm_add nav_ref (fvalue "west", fvalue "south")
- and p2 = LL.utm_add nav_ref (fvalue "east", fvalue "north") in
- a.survey <- Some (LL.of_utm WGS84 p1, LL.of_utm WGS84 p2)
- | None -> ()
+ a.time_since_last_survey_msg <- 0.;
+ match a.nav_ref with
+ Some nav_ref ->
+ let p1 = Aircraft.add_pos_to_nav_ref nav_ref (fvalue "west",
fvalue "south")
+ and p2 = Aircraft.add_pos_to_nav_ref nav_ref (fvalue "east",
fvalue "north") in
+ a.survey <- Some (p1, p2)
+ | None -> ()
end
| "CALIBRATION" ->
a.throttle_accu <- fvalue "climb_sum_err"
@@ -228,15 +260,21 @@
failwith "Too much dl_setting values !!!"
| "WP_MOVED" ->
begin
- match a.nav_ref with
- Some nav_ref ->
- let utm_zone = try ivalue "utm_zone" with _ -> nav_ref.utm_zone in
- let p = { LL.utm_x = fvalue "utm_east";
- utm_y = fvalue "utm_north";
- utm_zone = utm_zone } in
- update_waypoint a (ivalue "wp_id") p (fvalue "alt")
- | None -> () (** Can't use this message *)
+ match a.nav_ref with
+ Some Utm nav_ref ->
+ let utm_zone = try ivalue "utm_zone" with _ -> nav_ref.utm_zone in
+ let p = { LL.utm_x = fvalue "utm_east";
+ utm_y = fvalue "utm_north";
+ utm_zone = utm_zone } in
+ update_waypoint a (ivalue "wp_id") (LL.of_utm WGS84 p) (fvalue
"alt")
+ | _ -> () (** Can't use this message *)
end
+ | "WP_MOVED_LLA" ->
+ let lat = ivalue "lat"
+ and lon = ivalue "lon"
+ and alt = ivalue "alt" in
+ let geo = make_geo_deg (float lat /. 1e7) (float lon /. 1e7) in
+ update_waypoint a (ivalue "wp_id") geo (float alt /. 100.)
| "FORMATION_SLOT_TM" ->
Dl_Pprz.message_send "ground_dl" "FORMATION_SLOT" values
| "FORMATION_STATUS_TM" ->
Modified: paparazzi3/trunk/sw/ground_segment/tmtc/kml.ml
===================================================================
--- paparazzi3/trunk/sw/ground_segment/tmtc/kml.ml 2010-07-19 01:03:52 UTC
(rev 5072)
+++ paparazzi3/trunk/sw/ground_segment/tmtc/kml.ml 2010-07-19 15:57:53 UTC
(rev 5073)
@@ -86,10 +86,10 @@
stylemap "msn_ac_icon" "ac_style" "ac_style";
stylemap "msn_wp_icon" "wp_style" "wp_style"]
-let circle = fun utm0 radius alt ->
+let circle = fun geo radius alt ->
let degree_point = fun d ->
let a = (Deg>>Rad) (float d) in
- let utm = utm_add utm0 (cos a *. radius, sin a *. radius) in
+ let utm = utm_add (utm_of WGS84 geo) (cos a *. radius, sin a *. radius) in
let wgs84 = of_utm WGS84 utm in
coordinates wgs84 alt in
let points = Array.init 360 degree_point in
@@ -147,7 +147,7 @@
data "latitude" (sprintf "%f" ((Rad>>Deg)wgs84.posn_lat));
data "range" (ExtXml.attrib fp "max_dist_from_home")] in
kml
- [el "Document" [] (data "open"
"1"::lookat::style@(horiz_mode::ring_around_home utm0 fp::ac::waypoints))]
+ [el "Document" [] (data "open"
"1"::lookat::style@(horiz_mode::ring_around_home geo0 fp::ac::waypoints))]
let aircraft = fun ac url_flight_plan url_changes ->
let dyn_links =
@@ -212,7 +212,7 @@
if !l <> !last_state then begin
last_state := !l;
let url_flight_plan = sprintf "http://%s:%d/var/%s/flight_plan.kml";
!hostname !port ac.name in
- let changes = List.map (fun (wp_id, wp) -> change_waypoint ac.name
wp_id(of_utm WGS84 wp.wp_utm) wp.altitude) !l in
+ let changes = List.map (fun (wp_id, wp) -> change_waypoint ac.name wp_id
wp.wp_geo wp.altitude) !l in
let kml_update = link_update url_flight_plan changes in
print_xml ac.name "wp_changes.kml" kml_update
end
@@ -227,7 +227,7 @@
let alt = ac.desired_altitude in
match ac.horiz_mode with
Segment (p1, p2) ->
- let coordinates = String.concat " " (List.map (fun p -> coordinates
(of_utm WGS84 p) alt) [p1; p2]) in
+ let coordinates = String.concat " " (List.map (fun p -> coordinates p
alt) [p1; p2]) in
let kml_changes = update_linear_ring url_flight_plan "horiz_mode"
coordinates in
print_xml ac.name "route_changes.kml" kml_changes
| Circle (p, r) ->
@@ -245,8 +245,7 @@
let block = ExtXml.child blocks (string_of_int ac.cur_block) in
let block_name = ExtXml.attrib block "name" in
let description = sprintf "<b><font color=\"green\">%s</font></b>:
%s\nBat: <b><font color=\"red\">%.1fV</font></b>\nAGL: %.0fm\nSpeed: %.1fm/s"
ap_modes.(ac.ap_mode) block_name ac.bat ac.agl ac.gspeed in
- let wgs84 = of_utm WGS84 ac.pos in
- let change = change_placemark ~description ac.name wgs84 ac.alt in
+ let change = change_placemark ~description ac.name ac.pos ac.alt in
let kml_changes = link_update url_flight_plan [change] in
print_xml ac.name "ac_changes.kml" kml_changes
with
Modified: paparazzi3/trunk/sw/ground_segment/tmtc/server.ml
===================================================================
--- paparazzi3/trunk/sw/ground_segment/tmtc/server.ml 2010-07-19 01:03:52 UTC
(rev 5072)
+++ paparazzi3/trunk/sw/ground_segment/tmtc/server.ml 2010-07-19 15:57:53 UTC
(rev 5073)
@@ -151,10 +151,9 @@
let alpha = -. a.course in
let east = dx *. cos alpha -. dy *. sin alpha
and north = dx *. sin alpha +. dy *. cos alpha in
- let utm = LL.utm_add a.pos (east, north) in
+ let utm = LL.utm_add (LL.utm_of WGS84 a.pos) (east, north) in
let wgs84 = LL.of_utm WGS84 utm in
- let utm_target = LL.utm_add nav_ref a.cam.target in
- let twgs84 = LL.of_utm WGS84 utm_target in
+ let twgs84 = Aircraft.add_pos_to_nav_ref nav_ref a.cam.target in
let values = ["ac_id", Pprz.String a.id;
"cam_lat", Pprz.Float ((Rad>>Deg)wgs84.posn_lat);
"cam_long", Pprz.Float ((Rad>>Deg)wgs84.posn_long);
@@ -205,21 +204,18 @@
let send_horiz_status = fun a ->
match a.horiz_mode with
- Circle (utm, r) ->
- let wgs84 = LL.of_utm WGS84 utm in
+ Circle (geo, r) ->
let vs = [ "ac_id", Pprz.String a.id;
- "circle_lat", Pprz.Float ((Rad>>Deg)wgs84.posn_lat);
- "circle_long", Pprz.Float ((Rad>>Deg)wgs84.posn_long);
- "radius", Pprz.Int r ] in
+ "circle_lat", Pprz.Float ((Rad>>Deg)geo.posn_lat);
+ "circle_long", Pprz.Float ((Rad>>Deg)geo.posn_long);
+ "radius", Pprz.Int r ] in
Ground_Pprz.message_send my_id "CIRCLE_STATUS" vs
- | Segment (u1, u2) ->
- let geo1 = LL.of_utm WGS84 u1 in
- let geo2 = LL.of_utm WGS84 u2 in
+ | Segment (geo1, geo2) ->
let vs = [ "ac_id", Pprz.String a.id;
- "segment1_lat", Pprz.Float ((Rad>>Deg)geo1.posn_lat);
- "segment1_long", Pprz.Float ((Rad>>Deg)geo1.posn_long);
- "segment2_lat", Pprz.Float ((Rad>>Deg)geo2.posn_lat);
- "segment2_long", Pprz.Float ((Rad>>Deg)geo2.posn_long) ] in
+ "segment1_lat", Pprz.Float ((Rad>>Deg)geo1.posn_lat);
+ "segment1_long", Pprz.Float ((Rad>>Deg)geo1.posn_long);
+ "segment2_lat", Pprz.Float ((Rad>>Deg)geo2.posn_lat);
+ "segment2_long", Pprz.Float ((Rad>>Deg)geo2.posn_long) ] in
Ground_Pprz.message_send my_id "SEGMENT_STATUS" vs
| UnknownHorizMode -> ()
@@ -256,7 +252,7 @@
try
let vs =
["ac_id", Pprz.String id;
- "time_since_last_bat_msg", Pprz.Float (U.gettimeofday () -.
a.last_bat_msg_date)] in
+ "time_since_last_bat_msg", Pprz.Float (U.gettimeofday () -.
a.last_msg_date)] in
Ground_Pprz.message_send my_id "TELEMETRY_STATUS" vs
with
_exc -> ()
@@ -264,7 +260,7 @@
let send_moved_waypoints = fun a ->
Hashtbl.iter
(fun wp_id wp ->
- let geo = LL.of_utm WGS84 wp.wp_utm in
+ let geo = wp.wp_geo in
let vs =
["ac_id", Pprz.String a.id;
"wp_id", Pprz.Int wp_id;
@@ -282,7 +278,7 @@
try
let a = Hashtbl.find aircrafts ac in
let f = fun x -> Pprz.Float x in
- let wgs84 = try LL.of_utm WGS84 a.pos with _ -> LL.make_geo 0. 0. in
+ let wgs84 = try a.pos with _ -> LL.make_geo 0. 0. in
let values = ["ac_id", Pprz.String ac;
"roll", f (Geometry_2d.rad2deg a.roll);
"pitch", f (Geometry_2d.rad2deg a.pitch);
@@ -302,9 +298,10 @@
if Hashtbl.length aircrafts > 1 then
begin
let cm_of_m = fun f -> Pprz.Int (truncate (100. *. f)) in
+ let pos = LL.utm_of WGS84 a.pos in
let ac_info = ["ac_id", Pprz.String ac;
- "utm_east", cm_of_m a.pos.utm_x;
- "utm_north", cm_of_m a.pos.utm_y;
+ "utm_east", cm_of_m pos.utm_x;
+ "utm_north", cm_of_m pos.utm_y;
"course", Pprz.Int (truncate (10. *. (Geometry_2d.rad2deg
a.course)));
"alt", cm_of_m a.alt;
"speed", cm_of_m a.gspeed;
@@ -319,15 +316,13 @@
begin
match a.nav_ref with
Some nav_ref ->
- let target_utm = LL.utm_add nav_ref (a.desired_east, a.desired_north)
in
- let target_wgs84 = LL.of_utm WGS84 target_utm in
let values = ["ac_id", Pprz.String ac;
"cur_block", Pprz.Int a.cur_block;
"cur_stage", Pprz.Int a.cur_stage;
"stage_time", Pprz.Int a.stage_time;
"block_time", Pprz.Int a.block_time;
- "target_lat", f ((Rad>>Deg)target_wgs84.posn_lat);
- "target_long", f ((Rad>>Deg)target_wgs84.posn_long);
+ "target_lat", f ((Rad>>Deg)a.desired_pos.posn_lat);
+ "target_long", f ((Rad>>Deg)a.desired_pos.posn_long);
"target_alt", Pprz.Float a.desired_altitude;
"target_climb", Pprz.Float a.desired_climb;
"target_course", Pprz.Float
((Rad>>Deg)a.desired_course);
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [5073] Change internal representation of positions from utm to lat/ lon wgs84,
Gautier Hattenberger <=