horiz mode

sw/airborne/autopilot/main.c

@@ -224,7 +224,7 @@ uint8_t ac_ident = AC_ID;
 /** \def EventPos(_cpt, _channel, _event)
  *  @@@@@ A FIXER @@@@@
  */
-#define PERIODIC_SEND_PPRZ_MODE() DOWNLINK_SEND_PPRZ_MODE(&pprz_mode, &vertical_mode, &lateral_mode, &inflight_calib_mode, &mcu1_status, &ir_estim_mode);
+#define PERIODIC_SEND_PPRZ_MODE() DOWNLINK_SEND_PPRZ_MODE(&pprz_mode, &vertical_mode, &lateral_mode, &horizontal_mode, &inflight_calib_mode, &mcu1_status, &ir_estim_mode);
 #define PERIODIC_SEND_DESIRED() DOWNLINK_SEND_DESIRED(&desired_roll, &desired_pitch, &desired_x, &desired_y, &desired_altitude, &desired_climb);
 
 #define PERIODIC_SEND_NAVIGATION_REF()  DOWNLINK_SEND_NAVIGATION_REF(&nav_utm_east0, &nav_utm_north0, &nav_utm_zone0);

sw/airborne/autopilot/nav.c

@@ -59,6 +59,7 @@ bool_t in_circle = FALSE;
 bool_t in_segment = FALSE;
 int16_t circle_x, circle_y, circle_radius;
 int16_t segment_x_1, segment_y_1, segment_x_2, segment_y_2;
+uint8_t horizontal_mode;
 
 #define RcRoll(travel) (from_fbw.channels[RADIO_ROLL]* (float)travel /(float)MAX_PPRZ)
 
@@ -116,6 +117,7 @@ static float qdr;
 	else new_circle = FALSE; \
 	circle_count = fabs(sum_alpha) / (2*M_PI); \
   float alpha_carrot = alpha + CARROT / -radius * estimator_hspeed_mod; \
+  horizontal_mode = HORIZONTAL_MODE_CIRCLE; \
   fly_to_xy(x+cos(alpha_carrot)*fabs(radius), \
 						y+sin(alpha_carrot)*fabs(radius)); \
   qdr = DegOfRad(M_PI/2 - alpha_carrot); \
@@ -228,11 +230,7 @@ static inline void fly_to_xy(float x, float y) {
  *  \brief Just call \a fly_to_xy with x and y of current waypoint.
  */
 static void fly_to(uint8_t wp) { 
-	in_segment = TRUE;
+  horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
-	segment_x_1 = estimator_x;
-	segment_y_1 = estimator_y;
-	segment_x_2 = waypoints[wp].x;
-	segment_y_2 = waypoints[wp].y;
   fly_to_xy(waypoints[wp].x, waypoints[wp].y);
 }
 
@@ -259,6 +257,14 @@ static void route_to(uint8_t _last_wp, uint8_t wp) {
 	segment_y_1 = last_wp_y;
 	segment_x_2 = waypoints[wp].x;
 	segment_y_2 = waypoints[wp].y;
+
+  in_segment = TRUE;
+  segment_x_1 = last_wp_x;
+  segment_y_1 = last_wp_y;
+  segment_x_2 = waypoints[wp].x;
+  segment_y_2 = waypoints[wp].y;
+  horizontal_mode = HORIZONTAL_MODE_ROUTE;
+
   fly_to_xy(last_wp_x + alpha*leg_x, last_wp_y + alpha*leg_y);
 }
 

sw/airborne/autopilot/nav.h

@@ -66,6 +66,12 @@ extern bool_t in_segment;
 extern int16_t circle_x, circle_y, circle_radius;
 extern int16_t segment_x_1, segment_y_1, segment_x_2, segment_y_2;
 
+extern uint8_t horizontal_mode;
+
+#define HORIZONTAL_MODE_WAYPOINT 0
+#define HORIZONTAL_MODE_ROUTE 1
+#define HORIZONTAL_MODE_CIRCLE 2
+
 
 void nav_update(void);
 void nav_home(void);

sw/lib/ocaml/mapTrack.ml

@@ -52,11 +52,9 @@ let cam_field_half_width = ref 0.0
 let cam_field_half_height_1 = ref 0.0
 let cam_field_half_height_2 = ref 0.0
 let cam_heading = ref 0.0
-let angle_of_view = ref 0.0
-let oblic_distance = ref 0.0
 let max_cam_half_height = 10000.0
 let max_oblic_distance = 10000.0
-let min_distance = 0.1
+let min_distance = 10.
 let min_height = 0.1
 
 let half_pi = m_pi /. 2.0
@@ -161,24 +159,21 @@ class track = fun ?(name="coucou") ?(size = 50) ?(color="red") (geomap:MapCanvas
 	    cam_heading := norm_angle_360 ( rad2deg (asin (cross_product vect_north cam_vect_normalized)))
 	      else cam_heading := norm_angle_360 ( rad2deg (m_pi -. asin (cross_product vect_north cam_vect_normalized)))
 	  else cam_heading := last_heading;
-	  if last_height < min_height then 
+	  let (angle_of_view, oblic_distance) = 
-	    begin
+	    if last_height < min_height then 
-	      angle_of_view := half_pi;
+	      (half_pi, max_oblic_distance)
-	      oblic_distance := max_oblic_distance
+	    else
-	    end
+	      let oav = atan ( d /. last_height) in
-	  else 
+	      (oav, last_height /. (cos oav))
-	    begin
+	  in
-	      angle_of_view := (atan ( d /. last_height) );
+	  let alpha_1 = angle_of_view +. cam_half_aperture in
-	      oblic_distance := last_height /. (cos !angle_of_view)
+	  let alpha_2 = angle_of_view -. cam_half_aperture in
-	    end;
-	  let alpha_1 = !angle_of_view +. cam_half_aperture in
-	  let alpha_2 = !angle_of_view -. cam_half_aperture in
 	  begin 
 	    if alpha_1 < half_pi then
 	      cam_field_half_height_1 := (tan alpha_1) *. last_height -. d
 	    else cam_field_half_height_1 := max_cam_half_height;
-	    cam_field_half_height_2 := d -. (tan ( !angle_of_view -. cam_half_aperture)) *. last_height;
+	    cam_field_half_height_2 := d -. (tan ( angle_of_view -. cam_half_aperture)) *. last_height;
-	    cam_field_half_width := ( tan (cam_half_aperture) ) *. !oblic_distance;
+	    cam_field_half_width := ( tan (cam_half_aperture) ) *. oblic_distance;
 	      
 (***	  Printf.printf "dist %.2f aoview %.2f oblic_distance %.2f cfh1 %.2f cfh2 %.2f cfhw %.2f last_xw %.2f last_yw %.2f cam_heading %.2f \n " d !angle_of_view !oblic_distance !cam_field_half_height_1 !cam_field_half_height_2 !cam_field_half_width last_xw last_yw !cam_heading; 
    flush stdout;   ***)
@@ -199,6 +194,7 @@ class track = fun ?(name="coucou") ?(size = 50) ?(color="red") (geomap:MapCanvas
     method resize =  fun new_size ->
       let a = Array.create new_size empty in
       let size =  Array.length segments in
+
       let m = min new_size size in
       let j = ref ((top - m + size) mod size) in
       for i = 0 to m - 1 do