conf/airframes/drops.xml

@@ -14,7 +14,7 @@
   <servos>
     <servo name="MOTOR_LEFT" no="0" min="1000" neutral="1000" max="2000"/>
     <servo name="AILERON_LEFT" no="1" min="1900" neutral="1500" max="1100"/>
-    <servo name="ELEVATOR" no="2" min="1900" neutral="1360" max="1100"/>
+    <servo name="ELEVATOR" no="2" min="1900" neutral="1500" max="1100"/>
     <servo name="RUDDER" no="3" min="1100" neutral="1500" max="1900"/>
     <servo name="AILERON_RIGHT" no="4" min="1900" neutral="1500" max="1100"/>
     <servo name="HATCH" no="5" min="1860" neutral="1860" max="1022"/>
@@ -63,7 +63,7 @@
     <define name="IR_TOP" value="ADC_0"/>
     <define name="IR_NB_SAMPLES" value="16"/>
     <define name="GYRO_ROLL" value="ADC_3"/>
-    <define name="GYRO_NB_SAMPLES" value="16"/> 
+    <define name="GYRO_NB_SAMPLES" value="32"/> 
   </section>
 
   <section name="INFRARED" prefix="IR_">
@@ -107,7 +107,9 @@
   </section>
  
   <section name="MISC">
+    <define name="MINIMUM_AIRSPEED" value="10." unit="m/s"/>
     <define name="NOMINAL_AIRSPEED" value="12." unit="m/s"/>
+    <define name="MAXIMUM_AIRSPEED" value="18." unit="m/s"/>
     <define name="CARROT" value="5." unit="s"/>
     <define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
     <define name="CONTROL_RATE" value="60" unit="Hz"/>
@@ -150,20 +152,20 @@
   </section>
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
-    <define name="COURSE_PGAIN" value="-0.929000020027"/>
+    <define name="COURSE_PGAIN" value="-0.739000022411"/>
     <define name="ROLL_MAX_SETPOINT" value="0.60" unit="radians"/>
     <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
     <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
 
     <define name="ROLL_PGAIN" value="6000."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
-    <define name="PITCH_PGAIN" value="-7000."/>
-    <define name="PITCH_DGAIN" value="1.5"/>
+    <define name="PITCH_PGAIN" value="-13135.5927734"/>
+    <define name="PITCH_DGAIN" value="-1694.91503906"/>
 
     <define name="ELEVATOR_OF_ROLL" value="1520.73706055"/>
 
-    <define name="ROLL_ATTITUDE_GAIN" value="-7500"/>
-    <define name="ROLL_RATE_GAIN" value="-1500"/>
+    <define name="ROLL_ATTITUDE_GAIN" value="-5847.45800781"/>
+    <define name="ROLL_RATE_GAIN" value="-2923.72900391"/>
 
     <define name="ROLL_KFF" value="-500"/>
     <define name="ROLL_IGAIN" value="-500"/>
@@ -206,11 +208,6 @@
     <define name="DEVICE_TYPE" value="XBEE"/>
     <define name="DEVICE_ADDRESS" value="...."/>
   </section>
- 
- <section name="SIMU">
-    <define name="YAW_RESPONSE_FACTOR" value="0.5"/>
- </section>
-
  <makefile>
 CONFIG = \"tiny_2_1.h\"
 

conf/airframes/lila.xml

@@ -97,7 +97,9 @@
   </section>
  
   <section name="MISC">
+    <define name="MINIMUM_AIRSPEED" value="10." unit="m/s"/>
     <define name="NOMINAL_AIRSPEED" value="20." unit="m/s"/>
+    <define name="MAXIMUM_AIRSPEED" value="40." unit="m/s"/>
     <define name="CARROT" value="5." unit="s"/>
     <define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
     <define name="CONTROL_RATE" value="60" unit="Hz"/>
@@ -145,16 +147,17 @@
     <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
     <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
 
-    <define name="ROLL_PGAIN" value="13000."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
     <define name="PITCH_PGAIN" value="-2500."/>
-    <define name="PITCH_DGAIN" value="0.2"/>
+    <define name="PITCH_DGAIN" value="-1000"/>
 
     <define name="ELEVATOR_OF_ROLL" value="1100"/>
 
-    <!--define name="ROLL_ATTITUDE_GAIN" value="-7500"/>
-    <define name="ROLL_RATE_GAIN" value="-1500"/-->
+    <define name="ROLL_ATTITUDE_GAIN" value="-7500"/>
+    <!--define name="ROLL_RATE_GAIN" value="-1500"/-->
 
+    <define name="ROLL_KFF" value="-500"/>
+    <define name="ROLL_IGAIN" value="-500"/>
   </section>
 
   <section name="NAV">
@@ -194,12 +197,7 @@
     <define name="DEVICE_TYPE" value="XBEE"/>
     <define name="DEVICE_ADDRESS" value="...."/>
   </section>
- 
- <section name="SIMU">
-    <define name="YAW_RESPONSE_FACTOR" value="0.5"/>
- </section>
-
- <makefile>
+  <makefile>
 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
-
   </makefile>
 </airframe>

conf/telemetry/fw_h_ctl_a.xml

@@ -4,14 +4,13 @@
   <process name="Ap">
     <mode name="default">
       <message name="ALIVE"          period="5"/>
-      <message name="ATTITUDE"       period="0.1"/>
-      <message name="H_CTL_A"        period="0.1"/>
+      <message name="ATTITUDE"       period="0.2"/>
+      <message name="H_CTL_A"        period="0.2"/>
       <message name="ESTIMATOR"      period="0.5"/>
       <message name="WP_MOVED"       period="0.5"/>
       <message name="CIRCLE"         period="1.05"/>
-      <message name="DESIRED"        period="0.1"/>
+      <message name="DESIRED"        period="0.2"/>
       <message name="BAT"            period="1.1"/>
-      <message name="BARO_MS5534A"   period="1.0"/>
       <message name="SEGMENT"        period="1.2"/>
       <message name="CALIBRATION"    period="2.1"/>
       <message name="NAVIGATION_REF" period="9."/>
@@ -19,7 +18,7 @@
       <message name="DOWNLINK"       period="5.1"/>
       <message name="DL_VALUE"       period="1.5"/>
       <message name="IR_SENSORS"     period="1.2"/>
-      <message name="GYRO_RATES"     period="1.1"/>
+      <message name="GYRO_RATES"     period="0.05"/>
       <message name="SURVEY"         period="2.1"/>
       <message name="GPS_SOL"        period="2.0"/>
     </mode>

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);
 }
-
-

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 = 

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

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 $@

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 <telemetry airborne file>\n" Sys.argv.(0);
+    exit 1;
+  end