Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 09:36:19 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

add itow in AC_INFO messages

Browse Source
This commit is contained in:
Gautier Hattenberger
2008-04-01 12:05:41 +00:00
parent 0e32ad6a68
commit 772213a01f
8 changed files with 34 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/messages.xml
+14
View File
@@ -663,6 +663,19 @@
<field name="accel" type="float" unit="m/s/s"/>
</message>
<message name="CROSS_TRACK_ERROR" ID="80">
<field name="cross_track_error" type="float" unit="m"/>
<field name="cte_int" type="float"/>
</message>
<message name="ESTIMATOR_COMP_FILTER" ID="81">
<field name="gyro_hp_last_out" type="float"/>
<field name="gyro_int" type="float"/>
<field name="gps_lp_last_out" type="float"/>
<field name="ir_lp_last_out" type="float"/>
</message>
<message name="ENOSE_STATUS" ID="250">
<field name="val1" type="uint16"/>
<field name="val2" type="uint16"/>
@@ -843,6 +856,7 @@
<field name="climb" type="float" unit="m/s"/>
<field name="agl" type="float" unit="m"/>
<field name="unix_time" type="float" unit="s (Unix time)"/>
<field name="itow" type="uint32" unit="ms"/>
</message>
<message name="AP_STATUS" ID="12">
sw/airborne/datalink.c
+2 -1
View File
@@ -65,7 +65,8 @@ void dl_parse_msg(void) {
float a = MOfCm(DL_ACINFO_alt(dl_buffer));
float c = RadOfDeg(((float)DL_ACINFO_course(dl_buffer))/ 10.);
float s = MOfCm(DL_ACINFO_speed(dl_buffer));
SetAcInfo(id, ux, uy, c, a, s);
uint32_t t = DL_ACINFO_itow(dl_buffer);
SetAcInfo(id, ux, uy, c, a, s, t);
} else
#endif
#ifdef NAV
sw/airborne/sim/sim_ap.c
+4 -4
View File
@@ -104,17 +104,17 @@ value get_commands(value val_commands) {
return Val_int(commands[COMMAND_THROTTLE]);
}
value set_ac_info_native(value ac_id __attribute__ ((unused)), value ux __attribute__ ((unused)), value uy __attribute__ ((unused)), value course __attribute__ ((unused)), value alt __attribute__ ((unused)), value gspeed __attribute__ ((unused))) {
value set_ac_info_native(value ac_id __attribute__ ((unused)), value ux __attribute__ ((unused)), value uy __attribute__ ((unused)), value course __attribute__ ((unused)), value alt __attribute__ ((unused)), value gspeed __attribute__ ((unused)), value itow __attribute__ ((unused))) {
#ifdef TRAFFIC_INFO
SetAcInfo(Int_val(ac_id), Double_val(ux), Double_val(uy),
Double_val(course), Double_val(alt), Double_val(gspeed));
Double_val(course), Double_val(alt), Double_val(gspeed), Int32_val(itow));
#endif
return Val_unit;
}
value set_ac_info(value * argv, int argn) {
assert (argn == 6);
return set_ac_info_native(argv[0], argv[1], argv[2], argv[3],argv[4], argv[5]);
assert (argn == 7);
return set_ac_info_native(argv[0], argv[1], argv[2], argv[3],argv[4], argv[5], argv[6]);
}
sw/airborne/traffic_info.h
+3 -2
View File
@@ -37,21 +37,22 @@ struct ac_info_ {
float course; /* rad (CW) */
float alt; /* m */
float gspeed; /* m/s */
uint32_t itow; /* ms */
};
extern struct ac_info_ the_acs[NB_ACS];
#define SetAcInfo(_id, _utm_x /*m*/, _utm_y /*m*/, _course/*rad(CW)*/, _alt/*m*/,_gspeed/*m/s*/) { \
#define SetAcInfo(_id, _utm_x /*m*/, _utm_y /*m*/, _course/*rad(CW)*/, _alt/*m*/,_gspeed/*m/s*/, _itow) { \
if (_id < NB_ACS) { \
the_acs[_id].east = _utm_x - nav_utm_east0; \
the_acs[_id].north = _utm_y - nav_utm_north0; \
the_acs[_id].course = _course; \
the_acs[_id].alt = _alt; \
the_acs[_id].gspeed = _gspeed; \
the_acs[_id].itow = (uint32_t)_itow; \
} \
}
struct ac_info_ *
get_ac_info(uint8_t id);
sw/ground_segment/tmtc/aircraft.ml
+2 -1
View File
@@ -79,6 +79,7 @@ type aircraft = {
airframe : Xml.xml;
mutable pos : Latlong.utm;
mutable unix_time : float;
mutable itow : int32; (* ms *)
mutable roll : float;
mutable pitch : float;
mutable nav_ref : Latlong.utm option;
@@ -139,7 +140,7 @@ let new_aircraft = fun id name fp airframe ->
desired_altitude = 0.;
desired_climb = 0.;
pos = { Latlong.utm_x = 0.; utm_y = 0.; utm_zone = 0 };
unix_time = 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.};
sw/ground_segment/tmtc/aircraft.mli
+1
View File
@@ -65,6 +65,7 @@ type aircraft = {
airframe : Xml.xml;
mutable pos : Latlong.utm;
mutable unix_time : float;
mutable itow : int32;
mutable roll : float;
mutable pitch : float;
mutable nav_ref : Latlong.utm option;
sw/ground_segment/tmtc/link.ml
+5 -2
View File
@@ -314,18 +314,21 @@ let get_fp = fun device _sender vs ->
Debug.trace 'b' (sprintf "ACINFO %d for %d" ac_id dest_id);
let ac_device = airborne_device dest_id airframes device.transport in
let f = fun a -> Pprz.float_assoc a vs in
let i32 = fun a -> Pprz.int32_assoc a vs in
let lat = (Deg>>Rad) (f "lat")
and long = (Deg>>Rad) (f "long")
and course = f "course"
and alt = f "alt"
and gspeed = f "speed" in
and gspeed = f "speed"
and itow = i32 "itow" in
let utm = Latlong.utm_of WGS84 {posn_lat=lat; posn_long=long} in
let vs = ["ac_id", Pprz.Int ac_id;
"utm_east", cm_of_m utm.utm_x;
"utm_north", cm_of_m utm.utm_y;
"course", Pprz.Int (truncate (10. *. course));
"alt", cm_of_m alt;
"speed", cm_of_m gspeed] in
"speed", cm_of_m gspeed;
"itow", Pprz.Int32 itow] in
let msg_id, _ = Dl_Pprz.message_of_name "ACINFO" in
let s = Dl_Pprz.payload_of_values msg_id my_id vs in
send dest_id device ac_device s Low
sw/ground_segment/tmtc/server.ml
+3 -1
View File
@@ -244,9 +244,10 @@ let log_and_parse = fun logging ac_name (a:Aircraft.aircraft) msg values ->
utm_y = fvalue "utm_north" /. 100.;
utm_zone = ivalue "utm_zone" };
a.unix_time <- Latlong.unix_time_of_tow (truncate (fvalue "itow" /. 1000.));
a.itow <- Int32.of_float (fvalue "itow");
(*Printf.fprintf stderr "itow %lu %ld\n" a.itow a.itow;*)
a.gspeed <- fvalue "speed" /. 100.;
a.course <- norm_course ((Deg>>Rad)(fvalue "course" /. 10.));
a.agl <- a.alt -. float (try Srtm.of_utm a.pos with _ -> 0);
a.gps_mode <- check_index (ivalue "mode") gps_modes "GPS_MODE";
if a.gspeed > 3. && a.ap_mode = _AUTO2 then
@@ -553,6 +554,7 @@ let send_aircraft_msg = fun ac ->
"lat", f ((Rad>>Deg)wgs84.posn_lat);
"long", f ((Rad>>Deg) wgs84.posn_long);
"unix_time", f a.unix_time;
"itow", Pprz.Int32 a.itow;
"speed", f a.gspeed;
"course", f (Geometry_2d.rad2deg a.course);
"alt", f a.alt;