Merge remote-tracking branch 'paparazzi/master' into wind_info

conf/modules/cv_opencvdemo.xml

@@ -7,6 +7,8 @@
     After this is done the folder sw/ext/opencv_bebop/install has a opencv.xml file.
     The LDFLAGS in this file should be the same as in this conf file.
     </description>
+
+    <define name="OPENCVDEMO_CAMERA" value="front_camera|bottom_camera" description="Video device to use"/>
   </doc>
   <header>
     <file name="cv_opencvdemo.h"/>

conf/modules/cv_qrcode.xml

@@ -7,7 +7,9 @@
 
       A telemetry message with the code content is sent when a QR code is detected when qrscan is called.
     </description>
-  </doc>
+    <define name="QRCODE_CAMERA" value="front_camera|bottom_camera" description="The V4L2 camera device that is used for searching a QR code"/>
+    <define name="QRCODE_DRAW_RECTANGLE" value="TRUE|FALSE" description="Whether or not to draw a rectangle around a found QR code"/>
+ </doc>
 
   <depends>video_thread</depends>
 

sw/airborne/modules/computer_vision/cv_opencvdemo.c

@@ -29,8 +29,8 @@
 
 
 // Function
-int opencv_func(struct image_t* img);
-int opencv_func(struct image_t* img)
+struct image_t* opencv_func(struct image_t* img);
+struct image_t* opencv_func(struct image_t* img)
 {
 
   if (img->type == IMAGE_YUV422)
@@ -41,11 +41,11 @@ int opencv_func(struct image_t* img)
 
 // opencv_example(NULL, 10,10);
 
-  return FALSE;
+  return NULL;
 }
 
 void opencvdemo_init(void)
 {
-  cv_add(opencv_func);
+  cv_add_to_device(&OPENCVDEMO_CAMERA, opencv_func);
 }
 

sw/airborne/modules/computer_vision/opencv_example.cpp

@@ -40,18 +40,20 @@ int opencv_example(char *img, int width, int height)
   Mat M(height, width, CV_8UC2, img);
   Mat image;
   // If you want a color image, uncomment this line
-  // cvtColor(M, image, CV_YUV2RGB_Y422);
+   cvtColor(M, image, CV_YUV2BGR_Y422);
   // For a grayscale image, use this one
-  cvtColor(M, image, CV_YUV2GRAY_Y422);
+//  cvtColor(M, image, CV_YUV2GRAY_Y422);
 
   // Blur it, because we can
   blur(image, image, Size(5, 5));
 
   // Canny edges, only works with grayscale image
-  int edgeThresh = 35;
-  Canny(image, image, edgeThresh, edgeThresh * 3);
+//  int edgeThresh = 35;
+//  Canny(image, image, edgeThresh, edgeThresh * 3);
 
   // Convert back to YUV422, and put it in place of the original image
-  grayscale_opencv_to_yuv422(image, img, width, height);
+//  grayscale_opencv_to_yuv422(image, img, width, height);
+  color_opencv_to_yuv422(image, img, width, height);
+
   return 0;
 }

sw/airborne/modules/computer_vision/opencv_image_functions.cpp

@@ -36,17 +36,18 @@ using namespace cv;
 
 void color_opencv_to_yuv422(Mat image, char *img, int width, int height)
 {
+  // Convert to YUV color space
+  cvtColor(image, image, COLOR_BGR2YUV);
 
-//Turn the opencv RGB colored image back in a YUV colored image for the drone
   for (int row = 0; row < height; row++) {
     for (int col = 0; col < width; col++) {
-      cv::Vec3b pixelHere = image.at<cv::Vec3b>(row, col);
-      img[(row * width + col) * 2 + 1] = 0.299 * pixelHere[0] + 0.587 * pixelHere[1] + 0.114 * pixelHere[2];
-      if (col % 2 == 0) { // U
-        img[(row * width + col) * 2] = 0.492 * (pixelHere[2] - img[(row * width + col) * 2 + 1] + 127);
-      } else { // V
-        img[(row * width + col) * 2] = 0.877 * (pixelHere[0] - img[(row * width + col) * 2 + 1] + 127);
-      }
+      // Extract pixel color from image
+      cv::Vec3b &c = image.at<cv::Vec3b>(row, col);
+
+      // Set image buffer values
+      int i = row * width + col;
+      img[2 * i + 1] = c[0]; // y;
+      img[2 * i] = col % 2 ? c[1] : c[2]; // u or v
     }
   }
 }

sw/airborne/modules/computer_vision/qrcode/qr_code.c

@@ -29,10 +29,15 @@
 #include "zbar.h"
 #include <stdio.h>
 
+#ifndef QRCODE_DRAW_RECTANGLE
+#define QRCODE_DRAW_RECTANGLE FALSE
+#endif
+bool drawRectangleAroundQRCode = QRCODE_DRAW_RECTANGLE;
+
 void qrcode_init(void)
 {
-	// Add qrscan to the list of image processing tasks in video_thread
-  cv_add(qrscan);
+  // Add qrscan to the list of image processing tasks in video_thread
+  cv_add_to_device(&QRCODE_CAMERA, qrscan);
 }
 
 // Telemetry
@@ -41,7 +46,7 @@ void qrcode_init(void)
 
 zbar_image_scanner_t *scanner = 0;
 
-struct image_t* qrscan(struct image_t *img)
+struct image_t *qrscan(struct image_t *img)
 {
   int i, j;
 
@@ -86,9 +91,28 @@ struct image_t* qrscan(struct image_t *img)
     // do something useful with results
     zbar_symbol_type_t typ = zbar_symbol_get_type(symbol);
     char *data = (char *)zbar_symbol_get_data(symbol);
-    printf("decoded %s symbol \"%s\"\n",
-           zbar_get_symbol_name(typ), data);
+    printf("decoded %s symbol \"%s\" at %d %d\n",
+           zbar_get_symbol_name(typ), data, zbar_symbol_get_loc_x(symbol, 0), zbar_symbol_get_loc_y(symbol, 0));
+
+    if (drawRectangleAroundQRCode) {
+      uint8_t cornerIndex;
+      struct point_t from;
+      struct point_t to;
+
+      for (cornerIndex = 0; cornerIndex < 4; cornerIndex++) {
+        // Determine where to draw from and to
+        uint8_t nextCorner = (cornerIndex + 1) % 4;
+        from.x = zbar_symbol_get_loc_x(symbol, cornerIndex);
+        from.y = zbar_symbol_get_loc_y(symbol, cornerIndex);
 
+        to.x = zbar_symbol_get_loc_x(symbol, nextCorner);
+        to.y = zbar_symbol_get_loc_y(symbol, nextCorner);
+
+        // Draw a line between these two corners
+        image_draw_line(img, &from, &to);
+      }
+
+    }
     // TODO: not allowed to access telemetry from vision thread
 #if DOWNLINK
     DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, strlen(data), data);
@@ -99,5 +123,5 @@ struct image_t* qrscan(struct image_t *img)
   zbar_image_destroy(image);
   //zbar_image_scanner_destroy(scanner);
 
-  return NULL; // QRCode is not returning a new image.
+  return img;
 }

sw/airborne/modules/computer_vision/qrcode/qr_code.h

@@ -33,8 +33,9 @@
 
 #include "../lib/vision/image.h"
 
+extern bool drawRectangleAroundQRCode;
 extern void qrcode_init(void);
-extern bool qrscan(struct image_t *img);
+extern struct image_t *qrscan(struct image_t *img);
 
 
 #endif

sw/airborne/subsystems/ins/hf_float.c

@@ -193,10 +193,10 @@ static int b2_hff_rb_n; ///< ringbuffer fill count
 
 
 /** by how many steps the estimated GPS validity point in time differed from GPS_LAG_N */
-static int lag_counter_err;
+static int16_t lag_counter_err;
 
 /** counts down the propagation steps until the filter state is saved again */
-static int save_counter;
+static int16_t save_counter;
 static int past_save_counter;
 #define SAVE_NOW 0
 #define SAVING -1

sw/airborne/subsystems/ins/hf_float.h

@@ -46,7 +46,7 @@ struct HfilterFloat {
   float ydotdot;
   float xP[HFF_STATE_SIZE][HFF_STATE_SIZE];
   float yP[HFF_STATE_SIZE][HFF_STATE_SIZE];
-  uint8_t lag_counter;
+  uint16_t lag_counter;
   bool rollback;
 };
 

sw/airborne/subsystems/navigation/waypoints.c

@@ -282,7 +282,7 @@ void waypoints_localize_all(void)
 struct LlaCoor_i *waypoint_get_lla(uint8_t wp_id)
 {
   if (wp_id < nb_waypoint) {
-    if (!bit_is_set(waypoints[wp_id].flags, WP_FLAG_LLA_I)) {
+    if (!waypoint_is_global(wp_id) && !bit_is_set(waypoints[wp_id].flags, WP_FLAG_LLA_I)) {
       waypoint_globalize(wp_id);
     }
     return &waypoints[wp_id].lla;

sw/ground_segment/cockpit/gcs.ml

@@ -26,6 +26,7 @@ module G = MapCanvas
 open Printf
 open Latlong
 
+
 let locale = GtkMain.Main.init ~setlocale:false ()
 
 let soi = string_of_int
@@ -377,7 +378,7 @@ let options =
     "-auto_hide_fp", Arg.Unit (fun () -> Live.auto_hide_fp true; hide_fp := true), "Automatically hide flight plans of unselected aircraft";
     "-timestamp", Arg.Set timestamp, "Bind on timestampped telemetry messages";
     "-ac_ids", Arg.String (fun s -> Live.filter_ac_ids s), "comma separated list of AC IDs to show in GCS";
-    "-no_confirm_kill", Arg.Unit (fun () -> confirm_kill := false), "Disable kill confirmation from strip button"; 
+    "-no_confirm_kill", Arg.Unit (fun () -> confirm_kill := false), "Disable kill confirmation from strip button";
   ]
 
 
@@ -498,7 +499,7 @@ let resize = fun (widget:GObj.widget) orientation size ->
 
 let rec pack_widgets = fun orientation xml widgets packing ->
   let size = try Some (ExtXml.int_attrib xml "size") with _ -> None in
-  match String.lowercase (Xml.tag xml) with
+  match Compat.bytes_lowercase (Xml.tag xml) with
       "widget" ->
         let name = ExtXml.attrib xml "name" in
         let widget =
@@ -525,7 +526,7 @@ and pack_list = fun resize orientation xmls widgets packing ->
 
 let rec find_widget_children = fun name xml ->
   let xmls = Xml.children xml in
-  match String.lowercase (Xml.tag xml) with
+  match Compat.bytes_lowercase (Xml.tag xml) with
       "widget" when ExtXml.attrib xml "name" = name -> xmls
     | "rows" | "columns" ->
       let rec loop = function
@@ -539,7 +540,7 @@ let rec find_widget_children = fun name xml ->
 
 let rec replace_widget_children = fun name children xml ->
   let xmls = Xml.children xml
-  and tag = String.lowercase (Xml.tag xml) in
+  and tag = Compat.bytes_lowercase (Xml.tag xml) in
   match tag with
       "widget" ->
         Xml.Element("widget",
@@ -561,7 +562,7 @@ let rec update_widget_size = fun orientation widgets xml ->
     if orientation = `HORIZONTAL then rect.Gtk.width else rect.Gtk.height
   in
   let xmls = Xml.children xml
-  and tag = String.lowercase (Xml.tag xml) in
+  and tag = Compat.bytes_lowercase (Xml.tag xml) in
   match tag with
       "widget" ->
         let name = ExtXml.attrib xml "name" in

sw/ground_segment/cockpit/live.ml

@@ -27,6 +27,7 @@ open Latlong
 module LL = Latlong
 open Printf
 
+
 module Tele_Pprz = PprzLink.Messages(struct let name = "telemetry" end)
 module Ground_Pprz = PprzLink.Messages(struct let name = "ground" end)
 module Alert_Pprz = PprzLink.Messages(struct let name = "alert" end)
@@ -167,7 +168,7 @@ let select_ac = fun acs_notebook ac_id ->
     if !active_ac <> "" then begin
       let ac' = find_ac !active_ac in
       ac'.strip#hide_buttons ();
-      ac'.notebook_label#set_width_chars (String.length ac'.notebook_label#text);
+      ac'.notebook_label#set_width_chars (Compat.bytes_length ac'.notebook_label#text);
       if !_auto_hide_fp then hide_fp ac'
     end;
 
@@ -339,12 +340,12 @@ let mark = fun (geomap:G.widget) ac_id track plugin_frame ->
 let attributes_pretty_printer = fun attribs ->
   (* Remove the optional attributes *)
   let valid = fun a ->
-    let a = String.lowercase a in
+    let a = Compat.bytes_lowercase a in
     a <> "no" && a <> "strip_icon" && a <> "strip_button" && a <> "pre_call"
     && a <> "post_call" && a <> "key" && a <> "group" in
 
   let sprint_opt = fun b s ->
-    if String.length b > 0 then
+    if Compat.bytes_length b > 0 then
       sprintf " %s%s%s" s b s
     else
       ""
@@ -631,7 +632,7 @@ let create_ac = fun ?(confirm_kill=true) alert (geomap:G.widget) (acs_notebook:G
   let settings_file = Http.file_of_url settings_url in
   let settings_xml =
     try
-      if String.compare "replay" settings_file <> 0 then
+      if Compat.bytes_compare "replay" settings_file <> 0 then
         ExtXml.parse_file ~noprovedtd:true settings_file
       else
         Xml.Element("empty", [], [])
@@ -732,7 +733,7 @@ let create_ac = fun ?(confirm_kill=true) alert (geomap:G.widget) (acs_notebook:G
 
         (* only horizontal wind and airspeed are updated, so bitmask is 0b0000101 = 5 *)
         let msg_items = ["WIND_INFO"; ac_id; "5"; wind_east; wind_north; "0.0"; airspeed] in
-        let value = String.concat ";" msg_items in
+        let value = Compat.concat ";" msg_items in
         let vs = ["ac_id", PprzLink.String ac_id; "message", PprzLink.String value] in
         Ground_Pprz.message_send "dl" "RAW_DATALINK" vs;
       with

sw/ground_segment/cockpit/page_settings.ml

@@ -24,6 +24,7 @@
 
 open Printf
 
+
 let (//) = Filename.concat
 
 
@@ -90,7 +91,7 @@ let one_setting = fun (i:int) (do_change:int -> float -> unit) ac_id packing dl_
       1.
   in
   (* get number of digits after decimal dot *)
-  let digits = try String.length (ExtXml.attrib dl_setting "step") - String.index (ExtXml.attrib dl_setting "step") '.' - 1 with _ -> 0 in
+  let digits = try Compat.bytes_length (ExtXml.attrib dl_setting "step") - Compat.bytes_index (ExtXml.attrib dl_setting "step") '.' - 1 with _ -> 0 in
   let page_incr = step_incr
   and page_size = step_incr
   and show_auto = try ExtXml.attrib dl_setting "auto" = "true" with _ -> false in
@@ -248,7 +249,7 @@ let one_setting = fun (i:int) (do_change:int -> float -> unit) ac_id packing dl_
 
   (** Insert the related buttons in the strip and prepare the papgets DnD *)
   List.iter (fun x ->
-    match String.lowercase (Xml.tag x) with
+    match Compat.bytes_lowercase (Xml.tag x) with
         "strip_button" ->
           let label = ExtXml.attrib x "name"
           and sp_value = ExtXml.float_attrib x "value"
@@ -292,7 +293,7 @@ let same_tag_for_all = function
   | x::xs ->
     let tag_first = Xml.tag x in
     List.iter (fun y -> assert(ExtXml.tag_is y tag_first)) xs;
-    String.lowercase tag_first
+    Compat.bytes_lowercase tag_first
 
 
 (** Build the tree of settings *)
@@ -365,4 +366,3 @@ object (self)
     let settings = Array.fold_right (fun setting r -> try (setting#index, setting#xml, setting#last_known_value)::r with _ -> r) variables [] in
     SaveSettings.popup airframe_filename (Array.of_list settings) do_change
 end
-

sw/ground_segment/cockpit/saveSettings.ml

@@ -22,6 +22,7 @@
  *
  *)
 
+
 module U = Unix
 
 (** How to have them local ? *)
@@ -198,7 +199,7 @@ let fill_data = fun (model:GTree.tree_store) settings airframe_xml ->
 
   (* Warning if needed *)
   if !not_in_airframe_file <> [] then begin
-    GToolbox.message_box ~title:"Warning" (Printf.sprintf "Some parameters not writable in the airframe file:\n\n%s" (String.concat "\n" !not_in_airframe_file));
+    GToolbox.message_box ~title:"Warning" (Printf.sprintf "Some parameters not writable in the airframe file:\n\n%s" (Compat.bytes_concat "\n" !not_in_airframe_file));
   end
 
 

sw/ground_segment/cockpit/sectors.ml

@@ -2,11 +2,12 @@
 
 open Printf
 
+
 let (//) = Filename.concat
 
 let rec display = fun (geomap:MapCanvas.widget) r ->
 
-  match String.lowercase (Xml.tag r) with
+  match Compat.bytes_lowercase (Xml.tag r) with
       "disc" ->
         let rad = float_of_string (ExtXml.attrib r "radius")
         and geo = Latlong.of_string (ExtXml.attrib (ExtXml.child r "point") "pos") in

sw/ground_segment/cockpit/strip.ml

@@ -23,6 +23,7 @@
  *)
 
 open Printf
+
 module LL=Latlong
 
 let (//) = Filename.concat
@@ -327,7 +328,7 @@ object
 
     (* add a button widget in a vertical box if it belongs to a group (create new group if needed) *)
   method add_widget ?(group="") w =
-    let (vbox, pack) = match String.length group with
+    let (vbox, pack) = match Compat.bytes_length group with
         0 -> (GPack.vbox ~show:true (), true)
       | _ -> try (Hashtbl.find button_tbl group, false) with
           Not_found ->