diff --git a/conf/airframes/drops.xml b/conf/airframes/drops.xml index c4e3a3b4f6..0c87d7b3a1 100644 --- a/conf/airframes/drops.xml +++ b/conf/airframes/drops.xml @@ -14,7 +14,7 @@ - + @@ -63,7 +63,7 @@ - +
@@ -107,7 +107,9 @@
+ + @@ -150,20 +152,20 @@
- + - - + + - - + + @@ -206,11 +208,6 @@
- -
- -
- CONFIG = \"tiny_2_1.h\" diff --git a/conf/airframes/lila.xml b/conf/airframes/lila.xml index dc80ec42bd..d95e818453 100644 --- a/conf/airframes/lila.xml +++ b/conf/airframes/lila.xml @@ -97,7 +97,9 @@
+ + @@ -145,16 +147,17 @@ - - + - + + + +
@@ -194,12 +197,7 @@
- -
- -
- - + CONFIG = \"tiny_2_1.h\" include $(PAPARAZZI_SRC)/conf/autopilot/tiny.makefile @@ -241,7 +239,7 @@ ap.CFLAGS += -DINFRARED -DALT_KALMAN -DWIND_INFO -DWIND_INFO_RET ap.srcs += infrared.c estimator.c ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM -ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c +ap.srcs += nav.c fw_h_ctl_a.c fw_v_ctl.c ap.srcs += nav_line.c ap.srcs += nav_survey_rectangle.c @@ -249,9 +247,14 @@ ap.srcs += nav_survey_rectangle.c ap.srcs += snav.c # Config for SITL simulation -include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile +sim.ARCHDIR = $(ARCHI) +sim.ARCH = sitl +sim.TARGET = autopilot +sim.TARGETDIR = autopilot +sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -DINFRARED -DNAV -DLED -DWIND_INFO +sim.srcs = latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl_a.c fw_v_ctl.c nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c + sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN sim.srcs += nav_line.c nav_survey_rectangle.c - diff --git a/conf/telemetry/fw_h_ctl_a.xml b/conf/telemetry/fw_h_ctl_a.xml index 9a33c7e86c..c0c56f9cb2 100644 --- a/conf/telemetry/fw_h_ctl_a.xml +++ b/conf/telemetry/fw_h_ctl_a.xml @@ -4,14 +4,13 @@ - - + + - + - @@ -19,7 +18,7 @@ - + diff --git a/sw/airborne/fw_h_ctl_a.c b/sw/airborne/fw_h_ctl_a.c index 2f9a988495..3839b16cd1 100644 --- a/sw/airborne/fw_h_ctl_a.c +++ b/sw/airborne/fw_h_ctl_a.c @@ -132,6 +132,10 @@ void h_ctl_init( void ) { h_ctl_roll_attitude_gain = H_CTL_ROLL_ATTITUDE_GAIN; h_ctl_roll_rate_gain = H_CTL_ROLL_RATE_GAIN; + h_ctl_roll_igain = H_CTL_ROLL_IGAIN; + h_ctl_roll_sum_err = 0; + h_ctl_roll_Kff = H_CTL_ROLL_KFF; + #ifdef AGR_CLIMB nav_ratio=0; #endif @@ -184,8 +188,20 @@ void h_ctl_course_loop ( void ) { BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); } +static float airspeed_ratio2; + void h_ctl_attitude_loop ( void ) { if (!h_ctl_disabled) { + float throttle_diff = v_ctl_throttle_setpoint / (float)MAX_PPRZ - v_ctl_auto_throttle_nominal_cruise_throttle; + float airspeed = NOMINAL_AIRSPEED; /* Estimated from the throttle */ + if (throttle_diff > 0) + airspeed += throttle_diff / (V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE - v_ctl_auto_throttle_nominal_cruise_throttle) * (MAXIMUM_AIRSPEED - NOMINAL_AIRSPEED); + else + airspeed += throttle_diff / (v_ctl_auto_throttle_nominal_cruise_throttle - V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE) * (NOMINAL_AIRSPEED - MINIMUM_AIRSPEED); + + float airspeed_ratio = airspeed / NOMINAL_AIRSPEED; + Bound(airspeed_ratio, 0.5, 2.); + airspeed_ratio2 = airspeed_ratio*airspeed_ratio; h_ctl_roll_loop(); h_ctl_pitch_loop(); } @@ -213,6 +229,8 @@ inline static void h_ctl_roll_loop( void ) { - h_ctl_roll_igain * h_ctl_roll_sum_err + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle; + // cmd /= airspeed_ratio2; + h_ctl_aileron_setpoint = TRIM_PPRZ(cmd); } @@ -262,7 +280,8 @@ inline static void h_ctl_pitch_loop( void ) { float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref_pitch_rate; last_err = err; float cmd = h_ctl_pitch_pgain * err + h_ctl_pitch_dgain * d_err; + + cmd /= airspeed_ratio2; + h_ctl_elevator_setpoint = TRIM_PPRZ(cmd); } - - diff --git a/sw/lib/ocaml/latlong.ml b/sw/lib/ocaml/latlong.ml index f4d8ec8237..de61a40e13 100644 --- a/sw/lib/ocaml/latlong.ml +++ b/sw/lib/ocaml/latlong.ml @@ -485,8 +485,9 @@ let bearing = fun geo1 geo2 -> ((Rad>>Deg)(atan2 dx dy), sqrt(dx*.dx+.dy*.dy)) -let leap_seconds = 14 (* http://www.leapsecond.com/java/gpsclock.htm *) +let leap_seconds = 15 (* http://www.leapsecond.com/java/gpsclock.htm *) +let gps_epoch = 315964800. (* In seconds, in the unix reference *) let gps_tow_of_utc = fun ?wday hour min sec -> let wday = @@ -499,9 +500,18 @@ let get_gps_tow = fun () -> let utc = Unix.gmtime (Unix.gettimeofday ()) in gps_tow_of_utc ~wday:utc.Unix.tm_wday utc.Unix.tm_hour utc.Unix.tm_min utc.Unix.tm_sec -let unix_time_of_tow = fun tow -> - let host_tow = get_gps_tow () in - Unix.gettimeofday () +. float (tow - host_tow) + +let unix_time_of_tow = fun ?week tow -> + match week with + None -> + let host_tow = get_gps_tow () + and unix_now = Unix.gettimeofday () in + unix_now +. float (tow - host_tow) + | Some w -> + gps_epoch + +. float w *. 60. *. 60. *. 24. *. 7. + +. float (tow - leap_seconds) + type coordinates_kind = diff --git a/sw/lib/ocaml/latlong.mli b/sw/lib/ocaml/latlong.mli index a65da5bf11..d8832ed522 100644 --- a/sw/lib/ocaml/latlong.mli +++ b/sw/lib/ocaml/latlong.mli @@ -161,7 +161,8 @@ val gps_tow_of_utc : ?wday:int -> int -> int -> int -> int val get_gps_tow : unit -> int (** Returns the current GPS time of week in seconds *) -val unix_time_of_tow : int -> float +val unix_time_of_tow : ?week:int -> int -> float +(** If week if not supplied, current one is assumed *) type coordinates_kind = WGS84_dec diff --git a/sw/logalizer/Makefile b/sw/logalizer/Makefile index 6d3c4041db..3d65ed1f59 100644 --- a/sw/logalizer/Makefile +++ b/sw/logalizer/Makefile @@ -27,7 +27,7 @@ OCAMLC = ocamlc OCAMLOPT = ocamlopt INCLUDES= -I +xml-light -I +lablgtk2 -I ../lib/ocaml -all: play plotter plot +all: play plotter plot sd2log play : log_file.ml play_core.ml play.ml @echo OL $@ @@ -45,6 +45,10 @@ plot : log_file.cmx gtk_export.cmx export.cmx plot.cmx @echo OL $@ $(Q)$(OCAMLOPT) $(INCLUDES) -o $@ unix.cmxa str.cmxa xml-light.cmxa glibivy-ocaml.cmxa lablgtk.cmxa lib-pprz.cmxa lablglade.cmxa gtkInit.cmx $^ +sd2log : sd2log.cmo + @echo OL $@ + $(Q)$(OCAMLC) $(INCLUDES) -custom -o $@ unix.cma str.cma xml-light.cma glibivy-ocaml.cma lib-pprz.cma $^ + # Target for bytecode executable (if ocamlopt is not available) # plot : log_file.cmo gtk_export.cmo export.cmo plot.cmo # @echo OL $@ diff --git a/sw/logalizer/sd2log.ml b/sw/logalizer/sd2log.ml new file mode 100644 index 0000000000..07dcd7622c --- /dev/null +++ b/sw/logalizer/sd2log.ml @@ -0,0 +1,82 @@ +open Printf +module U = Unix +let (//) = Filename.concat +let logs_path = Env.paparazzi_home // "var" // "logs" + + +module Tm_Pprz = Pprz.Messages (struct let name = "telemetry" end) + +module Parser = Serial.Transport(Logpprz.Transport) + +let run_command = fun com -> + if Sys.command com <> 0 then begin + fprintf stderr "Command '%s' failed\n" com; + exit 1; + end + + + +let convert_file = fun file -> + let tmp_file = Filename.temp_file "tlm_from_sd" "data" in + + let f_in = open_in file + and f_out = open_out tmp_file in + + let start_unix_time = ref None in + + let use_payload = fun payload -> + let log_msg = Logpprz.parse payload in + let (msg_id, ac_id, vs) = + Tm_Pprz.values_of_payload log_msg.Logpprz.pprz_data in + let msg_descr = Tm_Pprz.message_of_id msg_id in + let timestamp = Int32.to_float log_msg.Logpprz.timestamp /. 1e4 in + fprintf f_out "%.3f %d %s\n" timestamp ac_id (Tm_Pprz.string_of_message msg_descr vs); + + (** Looking for a date from a GPS message *) + if !start_unix_time = None + && msg_descr.Pprz.name = "GPS" + && Pprz.int_assoc "mode" vs = 3 then + let itow = Pprz.int_assoc "itow" vs / 1000 + and week = Pprz.int_assoc "week" vs in + printf "itow=%d week=%d\n%!" itow week; + let unix_time = Latlong.unix_time_of_tow ~week itow in + start_unix_time := Some (unix_time -. timestamp) + in + + let parser = Parser.parse use_payload in + let Serial.Closure reader = Serial.input parser in + + try + while true do + reader (U.descr_of_in_channel f_in) + done + with + End_of_file -> + close_in f_in; + close_out f_out; + + (* Rename the file according to the GPS time *) + let start_time = + match !start_unix_time with + None -> U.gettimeofday () (* Not found, use now *) + | Some u -> u in + + let d = U.localtime start_time in + let basename = sprintf "%02d_%02d_%02d__%02d_%02d_%02d_SD" (d.U.tm_year mod 100) (d.U.tm_mon+1) (d.U.tm_mday) (d.U.tm_hour) (d.U.tm_min) (d.U.tm_sec) in + let data_name = sprintf "%s.data" basename in + + (** Move the produced .data file *) + let com = sprintf "mv %s %s" tmp_file (logs_path // data_name) in + run_command com; + + (** Save the original binary file *) + let com = sprintf "mv %s %s" tmp_file (logs_path // data_name) in + run_command com + +let () = + if Array.length Sys.argv = 2 then + convert_file Sys.argv.(1) + else begin + fprintf stderr "Usage: %s \n" Sys.argv.(0); + exit 1; + end