*** empty log message ***

sw/airborne/autopilot/estimator.c

@@ -30,6 +30,7 @@
 #include "pid.h"
 #include "infrared.h"
 #include "autopilot.h"
+#include "flight_plan.h"
 
 
 /* position in meters */
@@ -63,6 +64,25 @@ float estimator_hspeed_dir;
 
 float estimator_rad_of_ir, estimator_ir, estimator_rad;
 
+
+/* array of horizon angles computed for a given height (at flight_plan.h generation) */
+
+int8_t angles[NB_HEIGHTS] = HEIGHTS;
+
+
+/* (aircraft axis, ir axis) angle */
+
+#define aircraft_ir_angle ( M_PI / 2)
+
+
+
+#define height_index_coeff (NB_HEIGHTS / ( 2 * M_PI ))
+
+#define NORM_RAD_ANGLE2(x) { \
+    while (x > 2 * M_PI) x -= 2 * M_PI; \
+    while (x < 0 ) x += 2 * M_PI; \
+  }
+
 #define EstimatorSetPos(x, y, z) { estimator_x = x; estimator_y = y; estimator_z = z; }
 #define EstimatorSetAtt(phi, psi, theta) { estimator_phi = phi; estimator_psi = psi; estimator_theta = theta; }
 
@@ -113,7 +133,33 @@ void estimator_init( void ) {
 void estimator_update_state_infrared( void ) {
   float rad_of_ir = (ir_estim_mode == IR_ESTIM_MODE_ON && EstimatorIrGainIsCorrect()) ? 
     estimator_rad_of_ir : ir_rad_of_ir;
-  estimator_phi  = rad_of_ir * ir_roll;
+
+  /* phi correction because of the relief effect on ir data */
+
+  /*** int8_t index_left, index_right;
+
+  float angle =  gps_fcourse - aircraft_ir_angle;
+
+  NORM_RAD_ANGLE2(angle);
+
+  index_left = angle * height_index_coeff ;
+
+  angle =  gps_fcourse + aircraft_ir_angle;
+
+  NORM_RAD_ANGLE2(angle);
+
+  index_right = angle * height_index_coeff ;
+
+  int8_t angle_left = angles[index_left]; 
+
+  int8_t angle_right = angles[index_right];
+
+
+  float correction =  angle_left - angle_right;
+
+  printf(" degres_left %d angle_left %d degres_right %d angle_right %d correction %.2f \n", (index_left*15), angle_left, (index_right*15), angle_right, correction); ***/
+   
+  estimator_phi  = rad_of_ir * ir_roll /*** + RadOfDeg( correction ) ***/;
 
   estimator_theta = rad_of_ir * ir_pitch;
 }

sw/lib/ocaml/srtm.ml

@@ -116,7 +116,6 @@ let horizon_slope = fun geo r psi alpha d ->
 	let s_utm = utm_q +. !dj *. cos( heading_psi_2alpha +. alpha_i) in
 	let t_utm = utm_p +. !dj *. sin( heading_psi_2alpha +. alpha_i) in
 	let h = of_utm { utm_zone = z; utm_x = s_utm; utm_y = t_utm} in
-	Printf.fprintf stderr "psi=%.0f: %.0f %.0f %d\n%!" psi s_utm t_utm h;
 	begin
 	  begin 
 	    let slope =  float_of_int (h-r ) /. !dj in
@@ -127,7 +126,7 @@ let horizon_slope = fun geo r psi alpha d ->
 	dj := !dj +. step_d;
       done;
      (* Printf.printf "alpha_i %.2f max_slope %.2f \n" alpha_i !max_slope; *)
-      calc_horizon (sum_cos_alpha_i_slope_alpha_i +. !max_slope *. (cos alpha_i)) (sum_cos_alpha_i +. (cos alpha_i)) (alpha_i +. step_alpha)
+      calc_horizon (sum_cos_alpha_i_slope_alpha_i +. !max_slope *. (cos (alpha_i -. alpha))) (sum_cos_alpha_i +. (cos (alpha_i -. alpha))) (alpha_i +. step_alpha)
     end
   in 
   begin

sw/simulator/sim.ml

@@ -157,8 +157,8 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
 		      let altitude = (int_of_float gps_state.Gps.alt) in
 		      let horizon_right = Srtm.horizon_slope gps_state.Gps.wgs84 altitude (gps_state.Gps.course +. Latlong.pi /. 2.)  half_aperture horizon_distance in
 		      let horizon_left = Srtm.horizon_slope gps_state.Gps.wgs84 altitude (gps_state.Gps.course -. Latlong.pi /. 2.) half_aperture horizon_distance in 
-		  (***) Printf.printf "IR: %f-%f\n%!" horizon_right horizon_left;
-		  (***) Printf.printf "alt: %d\n%!" altitude;
+		      (***) Printf.printf "IR: %f-%f\n%!" horizon_right horizon_left;
+		      (***) Printf.printf "alt: %d\n%!" altitude;
 		      horizon_right -. horizon_left
 		    else
 		      0. in

sw/tools/gen_flight_plan.ml

@@ -493,7 +493,7 @@ let print_heights = fun wgs84 alt ->
   Srtm.add_path (Env.paparazzi_home ^ "/data/srtm");
   Xml2h.define "NB_HEIGHTS" (string_of_int nb_heights);
   Xml2h.define "HEIGHTS" "{ /* Degrees from default alt*/\\";
-  let a = pi /. float nb_heights in
+  let a =2. *. pi /. float nb_heights in
   for i = 0 to nb_heights - 1 do
     let psi = float i *. a in
     let horizon = Srtm.horizon_slope wgs84 alt psi  half_aperture horizon_distance in