diff --git a/conf/messages.xml b/conf/messages.xml
index 1168f54c30..5fa4ecd6ff 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -83,6 +83,7 @@
+
@@ -2737,6 +2738,7 @@
+
diff --git a/sw/airborne/subsystems/nav.c b/sw/airborne/subsystems/nav.c
index 6a0c698f4a..ec21e28d07 100644
--- a/sw/airborne/subsystems/nav.c
+++ b/sw/airborne/subsystems/nav.c
@@ -453,7 +453,7 @@ void nav_periodic_task(void) {
static void send_nav_ref(void) {
DOWNLINK_SEND_NAVIGATION_REF(DefaultChannel, DefaultDevice,
- &nav_utm_east0, &nav_utm_north0, &nav_utm_zone0);
+ &nav_utm_east0, &nav_utm_north0, &nav_utm_zone0, &ground_alt);
}
static void send_nav(void) {
diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml
index 001c53950a..dacb0ad9d4 100644
--- a/sw/ground_segment/cockpit/live.ml
+++ b/sw/ground_segment/cockpit/live.ml
@@ -1288,10 +1288,12 @@ let listen_waypoint_moved = fun () ->
let wp_id = Pprz.int_assoc "wp_id" vs in
let a = fun s -> Pprz.float_assoc s vs in
let geo = { posn_lat = (Deg>>Rad)(a "lat"); posn_long = (Deg>>Rad)(a "long") }
- and altitude = a "alt" in
+ and altitude = a "alt"
+ and ground_alt = a "ground_alt" in
try
let w = ac.fp_group#get_wp wp_id in
+ w#set_ground_alt ground_alt;
w#set ~altitude ~update:true geo
with
Not_found -> () (* Silently ignore unknown waypoints *)
diff --git a/sw/ground_segment/tmtc/aircraft.ml b/sw/ground_segment/tmtc/aircraft.ml
index ec5d564782..334b2961c3 100644
--- a/sw/ground_segment/tmtc/aircraft.ml
+++ b/sw/ground_segment/tmtc/aircraft.ml
@@ -121,6 +121,7 @@ type aircraft = {
mutable climb : float;
mutable nav_ref : nav_ref option;
mutable d_hmsl : float;
+ mutable ground_alt : float; (* ground alt ref if no SRTM data *)
mutable desired_pos : Latlong.geographic;
mutable desired_altitude : float;
mutable desired_course : float;
@@ -169,7 +170,7 @@ let new_aircraft = fun id name fp airframe ->
unix_time = 0.; itow = Int32.of_int 0;
roll = 0.; pitch = 0.;
gspeed=0.; airspeed= -1.; course = 0.; heading = 0.; alt=0.; climb=0.; agl = 0.;
- nav_ref = None; d_hmsl = 0.;
+ nav_ref = None; d_hmsl = 0.; ground_alt = 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;
diff --git a/sw/ground_segment/tmtc/aircraft.mli b/sw/ground_segment/tmtc/aircraft.mli
index 85a7a2ad35..36528997df 100644
--- a/sw/ground_segment/tmtc/aircraft.mli
+++ b/sw/ground_segment/tmtc/aircraft.mli
@@ -87,6 +87,7 @@ type aircraft = {
mutable climb : float;
mutable nav_ref : nav_ref option;
mutable d_hmsl : float; (* difference between geoid and ellipsoid *)
+ mutable ground_alt : float; (* ground alt ref if no SRTM data *)
mutable desired_pos : Latlong.geographic;
mutable desired_altitude : float;
mutable desired_course : float;
diff --git a/sw/ground_segment/tmtc/fw_server.ml b/sw/ground_segment/tmtc/fw_server.ml
index 23bbffb302..610094afc3 100644
--- a/sw/ground_segment/tmtc/fw_server.ml
+++ b/sw/ground_segment/tmtc/fw_server.ml
@@ -113,7 +113,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
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.agl <- a.alt -. (try float (Srtm.of_wgs84 a.pos) with _ -> a.ground_alt);
if a.gspeed > 3. && a.ap_mode = _AUTO2 then
Wind.update ac_name a.gspeed a.course
end
@@ -128,7 +128,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
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.agl <- a.alt -. (try float (Srtm.of_wgs84 a.pos) with _ -> a.ground_alt);
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
@@ -152,12 +152,14 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
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 { 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" });
+ a.ground_alt <- fvalue "ground_alt"
| "NAVIGATION_REF_LLA" ->
let lat = ivalue "lat"
and lon = ivalue "lon" in
let geo = make_geo (float lat /. 1e7) (float lon /. 1e7) in
- a.nav_ref <- Some (Geo geo)
+ a.nav_ref <- Some (Geo geo);
+ a.ground_alt <- fvalue "ground_alt"
| "ATTITUDE" ->
let roll = fvalue "phi"
and pitch = fvalue "theta" in
@@ -316,7 +318,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
a.course <- norm_course (fvalue "course" /. 1e3);
if !heading_from_course then
a.heading <- a.course;
- a.agl <- a.alt -. float (try Srtm.of_wgs84 a.pos with _ -> 0);
+ a.agl <- a.alt -. (try float (Srtm.of_wgs84 a.pos) with _ -> a.ground_alt);
a.bat <- fvalue "vsupply" /. 10.;
a.energy <- ivalue "energy" * 100;
a.throttle <- fvalue "throttle";
diff --git a/sw/ground_segment/tmtc/rotorcraft_server.ml b/sw/ground_segment/tmtc/rotorcraft_server.ml
index dded7be570..07c1a904dd 100644
--- a/sw/ground_segment/tmtc/rotorcraft_server.ml
+++ b/sw/ground_segment/tmtc/rotorcraft_server.ml
@@ -30,8 +30,6 @@ module LL = Latlong
module U = Unix
module Dl_Pprz = Pprz.Messages (struct let name = "datalink" end)
-let nav_ref_alt = ref 0.
-let nav_ref_hmsl = ref 0.
(* FIXME: bound the loop *)
let rec norm_course =
@@ -168,7 +166,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
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_wgs84 a.pos with _ -> 0);
+ a.agl <- a.alt -. (try float (Srtm.of_wgs84 a.pos) with _ -> a.ground_alt);
a.course <- norm_course ((Rad>>Deg) (foi32value "psi" /. angle_frac));
a.heading <- norm_course (foi32value "psi" /. angle_frac);
a.roll <- foi32value "phi" /. angle_frac;
@@ -200,6 +198,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
let nav_ref_ecef = LL.make_ecef [| x; y; z |] in
a.nav_ref <- Some (Ltp nav_ref_ecef);
a.d_hmsl <- hmsl -. alt;
+ a.ground_alt <- hmsl;
| "ROTORCRAFT_NAV_STATUS" ->
a.block_time <- ivalue "block_time";
a.stage_time <- ivalue "stage_time";
diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml
index fe7afc012b..af1b52da70 100644
--- a/sw/ground_segment/tmtc/server.ml
+++ b/sw/ground_segment/tmtc/server.ml
@@ -294,7 +294,8 @@ let send_moved_waypoints = fun a ->
"wp_id", Pprz.Int wp_id;
"long", Pprz.Float ((Rad>>Deg)geo.posn_long);
"lat", Pprz.Float ((Rad>>Deg)geo.posn_lat);
- "alt", Pprz.Float wp.altitude] in
+ "alt", Pprz.Float wp.altitude;
+ "ground_alt", Pprz.Float (try float (Srtm.of_wgs84 geo) with _ -> a.ground_alt)] in
Ground_Pprz.message_send my_id "WAYPOINT_MOVED" vs)
a.waypoints
diff --git a/sw/lib/ocaml/mapWaypoints.ml b/sw/lib/ocaml/mapWaypoints.ml
index bd2f8dd45e..957f02dbf8 100644
--- a/sw/lib/ocaml/mapWaypoints.ml
+++ b/sw/lib/ocaml/mapWaypoints.ml
@@ -74,6 +74,7 @@ object (self)
val label = new CL.widget ~name:name ~color:"white" s 0. wpt_group
val mutable name = name (* FIXME: already in label ! *)
val mutable alt = alt
+ val mutable ground_alt = 0.
val mutable moved = None
val mutable deleted = false
val mutable commit_cb = None
@@ -145,7 +146,7 @@ object (self)
ea#set_adjustment adj;
ea#set_value alt; (* this should be done by set_adjustment but seems to fail on ubuntu 13.10 (at least) *)
- let agl = alt -. float (try Srtm.of_wgs84 initial_wgs84 with _ -> 0) in
+ let agl = alt -. (try float (Srtm.of_wgs84 initial_wgs84) with _ -> ground_alt) in
let agl_lab = GMisc.label ~text:(sprintf " AGL: %4.0fm" agl) ~packing:ha#add () in
let plus10= GButton.button ~label:"+10" ~packing:ha#add () in
let change_alt = fun x ->
@@ -282,6 +283,7 @@ object (self)
if update then updated ()
| (None, false) | (Some _, true) -> ()
| Some _, false -> self#reset_moved ()
+ method set_ground_alt ga = ground_alt <- ga
method delete () =
deleted <- true; (* BOF *)
wpt_group#destroy ()
diff --git a/sw/lib/ocaml/mapWaypoints.mli b/sw/lib/ocaml/mapWaypoints.mli
index 25d9ec5fa5..09c18514f8 100644
--- a/sw/lib/ocaml/mapWaypoints.mli
+++ b/sw/lib/ocaml/mapWaypoints.mli
@@ -52,6 +52,7 @@ class waypoint :
method move : float -> float -> unit
method name : string
method set : ?altitude:float -> ?update:bool -> Latlong.geographic -> unit
+ method set_ground_alt : float -> unit
method set_name : string -> unit
method xy : float * float
method zoom : float -> unit