Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-23 21:36:28 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

[agl] use ground_alt from autopilot if no SRTM data available

Browse Source
This commit is contained in:
Gautier Hattenberger
2014-05-11 00:00:05 +02:00
parent 6fba4e4e39
commit ce3c21c428
10 changed files with 24 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/messages.xml
+2
View File
@@ -83,6 +83,7 @@
<field name="utm_east" type="int32" unit="m"/>
<field name="utm_north" type="int32" unit="m"/>
<field name="utm_zone" type="uint8"/>
<field name="ground_alt" type="float" unit="m"/>
</message>
<message name="NAVIGATION" id="10">
@@ -2737,6 +2738,7 @@
<field name="lat" type="float" unit="deg"></field>
<field name="long" type="float" unit="deg"></field>
<field name="alt" type="float" unit="m"></field>
<field name="ground_alt" type="float" unit="m"/>
</message>
<message name="SURVEY_STATUS" id="31">
sw/airborne/subsystems/nav.c
+1 -1
View File
@@ -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) {
sw/ground_segment/cockpit/live.ml
+3 -1
View File
@@ -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 *)
sw/ground_segment/tmtc/aircraft.ml
+2 -1
View File
@@ -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;
sw/ground_segment/tmtc/aircraft.mli
+1
View File
@@ -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;
sw/ground_segment/tmtc/fw_server.ml
+7 -5
View File
@@ -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";
sw/ground_segment/tmtc/rotorcraft_server.ml
+2 -3
View File
@@ -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";
sw/ground_segment/tmtc/server.ml
+2 -1
View File
@@ -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
sw/lib/ocaml/mapWaypoints.ml
+3 -1
View File
@@ -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 ()
sw/lib/ocaml/mapWaypoints.mli
+1
View File
@@ -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