[fixedwing][modules] make nav_cube a module

conf/firmwares/subsystems/fixedwing/navigation_extra.makefile

@@ -11,7 +11,6 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/common_flight_plan.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/traffic_info.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/nav_survey_rectangle.c $(SRC_SUBSYSTEMS)/navigation/nav_line.c
 
-$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/nav_cube.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/discsurvey.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/OSAMNav.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/snav.c

conf/flight_plans/cube.xml

@@ -2,10 +2,6 @@
 
 <flight_plan ALT="130" GROUND_ALT="68" LAT0="52.26483" LON0="9.99394" MAX_DIST_FROM_HOME="1500" NAME="Gross Lobke Cube" QFU="270" SECURITY_HEIGHT="60">
 
-<header>
-#include "subsystems/navigation/nav_cube.h"
-</header>
-
   <waypoints>
     <waypoint name="HOME" x="-30.0" y="13.0"/>
     <waypoint name="CENTER" x="10.0" y="1.0" alt="140.0"/>

conf/modules/nav_cube.xml

@@ -0,0 +1,15 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="nav_cube" dir="nav">
+  <doc>
+    <description>
+      Fixedwing Navigation in a cube towards a center.
+    </description>
+  </doc>
+  <header>
+    <file name="nav_cube.h"/>
+  </header>
+  <makefile>
+    <file name="nav_cube.c"/>
+  </makefile>
+</module>

sw/airborne/modules/nav/nav_cube.c

@@ -20,26 +20,23 @@
  */
 
 /**
- * @file subsystems/navigation/nav_cube.c
+ * @file modules/nav/nav_cube.c
  *
  * Fixedwing Navigation in a cube towards a center.
  *
  */
 
 #include "generated/airframe.h"
-#include "subsystems/navigation/nav_cube.h"
+#include "modules/nav/nav_cube.h"
 #include "subsystems/nav.h"
 #include "generated/flight_plan.h"
 
-int32_t cube_alpha;
+#define MAX_LINES_X 8
-int32_t cube_size_x, cube_size_y, cube_size_z;
+#define STBY_OFFSET 500
-int32_t cube_grid_x, cube_grid_z;
-int32_t cube_offs_x, cube_offs_y, cube_offs_z;
-int32_t cube_nline_x, cube_nline_z;
-int32_t cube_sect=1;
-int32_t cube_nsect_x=1, cube_nsect_z=1;
 
-bool_t nav_cube_init(uint8_t center, uint8_t tb, uint8_t te) {
+struct NavCube nav_cube;
+
+bool_t nav_cube_start(uint8_t center, uint8_t tb, uint8_t te) {
 
   int32_t j, start_bx, start_by, start_bz, start_ex, start_ey, start_ez;
   int32_t bx, by, ex, ey;
@@ -50,52 +47,59 @@ bool_t nav_cube_init(uint8_t center, uint8_t tb, uint8_t te) {
   int32_t cube_line_z_start, cube_line_z_end;
 
   /* sanity checks */
-  if (cube_nsect_x <= 0) cube_nsect_x = 1;
+  if (nav_cube.nsect_x <= 0)
-  if (cube_nsect_z <= 0) cube_nsect_z = 1;
+    nav_cube.nsect_x = 1;
-  if ((cube_sect <= 0) ||
+  if (nav_cube.nsect_z <= 0)
-      (cube_sect > (cube_nsect_x*cube_nsect_z))) cube_sect = 1;
+    nav_cube.nsect_z = 1;
+  if ((nav_cube.sect <= 0) || (nav_cube.sect > (nav_cube.nsect_x*nav_cube.nsect_z)))
+    nav_cube.sect = 1;
 
   /* total number of lines/layers to fly */
-  if (cube_grid_x == 0) cube_nline_x_t = 1;
+  if (nav_cube.grid_x == 0)
-  else cube_nline_x_t = cube_size_x / cube_grid_x + 1;
+    cube_nline_x_t = 1;
-  if (cube_grid_z == 0) cube_nline_z_t = 1;
+  else
-  else cube_nline_z_t = cube_size_z / cube_grid_z + 1;
+    cube_nline_x_t = nav_cube.size.x / nav_cube.grid_x + 1;
+  if (nav_cube.grid_z == 0)
+    cube_nline_z_t = 1;
+  else
+    cube_nline_z_t = nav_cube.size.z / nav_cube.grid_z + 1;
 
   /* position and number of lines in this sector */
-  cube_pos_x = (cube_sect-1) % cube_nsect_x;
+  cube_pos_x = (nav_cube.sect-1) % nav_cube.nsect_x;
-  cube_line_x_start =  (cube_pos_x    * cube_nline_x_t)/cube_nsect_x;
+  cube_line_x_start =  (cube_pos_x    * cube_nline_x_t)/nav_cube.nsect_x;
-  cube_line_x_end   = ((cube_pos_x+1) * cube_nline_x_t)/cube_nsect_x;
+  cube_line_x_end   = ((cube_pos_x+1) * cube_nline_x_t)/nav_cube.nsect_x;
-  if (cube_line_x_end > cube_nline_x_t) cube_line_x_end = cube_nline_x_t;
+  if (cube_line_x_end > cube_nline_x_t)
-  cube_nline_x = cube_line_x_end - cube_line_x_start;
+    cube_line_x_end = cube_nline_x_t;
+  nav_cube.nline_x = cube_line_x_end - cube_line_x_start;
 
   /* do not do more than pre-set number of lines */
-  if (cube_nline_x >= MAX_LINES_X) cube_nline_x = MAX_LINES_X-1;
+  if (nav_cube.nline_x >= MAX_LINES_X) nav_cube.nline_x = MAX_LINES_X-1;
 
   /* position and number of layers in this sector */
-  cube_pos_z = (cube_sect-1) / cube_nsect_x;
+  cube_pos_z = (nav_cube.sect-1) / nav_cube.nsect_x;
-  cube_line_z_start =  (cube_pos_z    * cube_nline_z_t)/cube_nsect_z;
+  cube_line_z_start =  (cube_pos_z    * cube_nline_z_t)/nav_cube.nsect_z;
-  cube_line_z_end   = ((cube_pos_z+1) * cube_nline_z_t)/cube_nsect_z;
+  cube_line_z_end   = ((cube_pos_z+1) * cube_nline_z_t)/nav_cube.nsect_z;
-  cube_nline_z = cube_line_z_end - cube_line_z_start;
+  nav_cube.nline_z = cube_line_z_end - cube_line_z_start;
 
   /* do the costly stuff only once */
-  alpha = ((360. - cube_alpha) / 360.) * 2 * M_PI;
+  alpha = ((360. - nav_cube.alpha) / 360.) * 2 * M_PI;
   cos_alpha = cos(alpha);
   sin_alpha = sin(alpha);
 
   /* calculate lower left start begin/end x coord */
-  start_bx = WaypointX(center) - (((cube_nline_x_t-1) * cube_grid_x)/2)
+  start_bx = WaypointX(center) - (((cube_nline_x_t-1) * nav_cube.grid_x)/2)
-    + cube_offs_x;
+    + nav_cube.offset.x;
   start_ex = start_bx;
 
   /* calculate lower left start end point y coord */
-  start_ey = WaypointY(center) - cube_offs_y;
+  start_ey = WaypointY(center) - nav_cube.offset.y;
 
   /* calculate lower left start begin point y coord */
-  start_by = start_ey - cube_size_y;
+  start_by = start_ey - nav_cube.size.y;
 
   /* calculate lower left start begin/end z coord */
-  start_bz = waypoints[center].a - (((cube_nline_z_t-1) * cube_grid_z)/2)
+  start_bz = waypoints[center].a - (((cube_nline_z_t-1) * nav_cube.grid_z)/2)
-    + (cube_line_z_start*cube_grid_z) + cube_offs_z;
+    + (cube_line_z_start*nav_cube.grid_z) + nav_cube.offset.z;
   start_ez = start_bz;
 
   /* reset all waypoints to the standby position */
@@ -107,12 +111,12 @@ bool_t nav_cube_init(uint8_t center, uint8_t tb, uint8_t te) {
   }
 
   /* set used waypoints */
-  for (j=0; j < cube_nline_x; j++) {
+  for (j=0; j < nav_cube.nline_x; j++) {
     int i = cube_line_x_start+j;
     /* set waypoints and vectorize in regard to center */
-    bx = (start_bx + i*cube_grid_x) - WaypointX(center);
+    bx = (start_bx + i*nav_cube.grid_x) - WaypointX(center);
     by = start_by - WaypointY(center);
-    ex = (start_ex + i*cube_grid_x) - WaypointX(center);
+    ex = (start_ex + i*nav_cube.grid_x) - WaypointX(center);
     ey = start_ey - WaypointY(center);
     /* rotate clockwise with alpha and un-vectorize*/
     waypoints[tb+j].x = bx * cos_alpha - by * sin_alpha + WaypointX(center);
@@ -124,30 +128,34 @@ bool_t nav_cube_init(uint8_t center, uint8_t tb, uint8_t te) {
   }
 
   /* bug in <for from="" to=""> ? */
-  cube_nline_x--;
+  nav_cube.nline_x--;
-  cube_nline_z--;
+  nav_cube.nline_z--;
 
   return FALSE;
 }
 
-bool_t nav_cube(int8_t j, int8_t i,
+bool_t nav_cube_run(int8_t j, int8_t i,
                 uint8_t dest_b, uint8_t dest_e,
                 uint8_t src_b, uint8_t src_e) {
 
-  if (i > cube_nline_x) return FALSE;
+  if (i > nav_cube.nline_x)
-  if (j > cube_nline_z) return FALSE;
+    return FALSE;
+  if (j > nav_cube.nline_z)
+    return FALSE;
 
   waypoints[dest_b].x = waypoints[src_b+i].x;
   waypoints[dest_b].y = waypoints[src_b+i].y;
-  waypoints[dest_b].a = waypoints[src_b+i].a + j*cube_grid_z;
+  waypoints[dest_b].a = waypoints[src_b+i].a + j*nav_cube.grid_z;
   /* always keep at least security altitude */
-  if (waypoints[dest_b].a < (ground_alt+SECURITY_HEIGHT)) waypoints[dest_b].a = ground_alt+SECURITY_HEIGHT;
+  if (waypoints[dest_b].a < (ground_alt+SECURITY_HEIGHT))
+    waypoints[dest_b].a = ground_alt+SECURITY_HEIGHT;
 
   waypoints[dest_e].x = waypoints[src_e+i].x;
   waypoints[dest_e].y = waypoints[src_e+i].y;
-  waypoints[dest_e].a = waypoints[src_e+i].a + j*cube_grid_z;
+  waypoints[dest_e].a = waypoints[src_e+i].a + j*nav_cube.grid_z;
   /* always keep at least security altitude */
-  if (waypoints[dest_e].a < (ground_alt+SECURITY_HEIGHT)) waypoints[dest_e].a = ground_alt+SECURITY_HEIGHT;
+  if (waypoints[dest_e].a < (ground_alt+SECURITY_HEIGHT))
+    waypoints[dest_e].a = ground_alt+SECURITY_HEIGHT;
 
   return FALSE;
 }

sw/airborne/modules/nav/nav_cube.h

@@ -0,0 +1,144 @@
+/*
+ * Copyright (C) 2010  Martin Mueller
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file modules/nav/nav_cube.h
+ *
+ * Fixedwing Navigation in a cube towards a center.
+ *
+ * @verbatim
+ * from above:
+ *
+ * | (offs_x)
+ *
+ * o [x, y]    -
+ * |
+ * | (offs_y)
+ * |
+ * -
+ * ^  ^  ^  ^  ^     |
+ * |  |  |  |  |     |
+ * |  |  |  |  |     | (size_y)
+ * |  |  |  |  |     |
+ * |  |  |  |  |     |
+ * -
+ * |--|
+ * (grid_x)
+ *
+ * |-----------|
+ * (size_x)
+ *
+ *
+ * side view:
+ *
+ * --------->                    -           -
+ * | (grid_z)  |
+ * --------->                    -           |
+ * -           | (size_z)
+ * --------->                    | (offs_z)  |
+ * o [alt]    -           |
+ * --------->                                -
+ *
+ * |----------|--------|
+ * (size_y)  (offs_y)
+ *
+ *
+ * back view, 1x1 sector:
+ *
+ * -------
+ * |       |
+ * |   1   |  ^
+ * |       |  |
+ * s-------   |
+ *
+ * ---->
+ *
+ *
+ * back view, 3x2 sectors:
+ *
+ * ------- ------- -------
+ * |       |       |       |
+ * |   4   |   5   |   6   |  ^
+ * |       |       |       |  |
+ * s-------s-------s-------   |
+ * |       |       |       |
+ * |   1   |   2   |   3   |  ^
+ * |       |       |       |  |
+ * s-------s-------s-------   | (nsect_z)
+ *
+ * ---->   ---->   ---->
+ * (nsect_x)
+ * @endverbatim
+ *
+ */
+
+#ifndef NAV_CUBE_H
+#define NAV_CUBE_H
+
+#include "std.h"
+#include "math/pprz_algebra_int.h"
+
+struct NavCube {
+  /** size of the cube.
+   * x: perpendicular to flight dir,
+   * y: in flight dir,
+   * z: height
+   */
+  struct Int32Vect3 size;
+
+  /** offset to center.
+   * x: horizontal,
+   * y: in direction,
+   * z: vertical
+   */
+  struct Int32Vect3 offset;
+  int32_t alpha;            ///< angle in degrees of flight direction to north, clockwise
+  int32_t grid_x;           ///< grid distance x (horizontal)
+  int32_t grid_z;           ///< grid distance z (vertical)
+  int32_t sect;             ///< sector to fly in (1..[nsect_x*nsect_z])
+  int32_t nsect_x;          ///< number of sectors horizontal
+  int32_t nsect_z;          ///< number of sectors vertical
+  int32_t nline_x;          ///< number of lines x (horizontal)
+  int32_t nline_z;          ///< number of lines z (vertical)
+};
+
+extern struct NavCube nav_cube;
+
+extern bool_t nav_cube_start(uint8_t turb, uint8_t tb, uint8_t te);
+bool_t nav_cube_run(int8_t j, int8_t i,
+                uint8_t dest_b, uint8_t dest_e,
+                uint8_t src_b, uint8_t src_e);
+
+#define nav_cube_SetAlpha(i) { nav_cube.alpha=i; }
+#define nav_cube_SetSect(i)  { nav_cube.sect=i; }
+#define nav_cube_SetGridX(i) { nav_cube.grid_x=i; }
+#define nav_cube_SetGridZ(i) { nav_cube.grid_z=i; }
+#define nav_cube_SetSizeX(i) { nav_cube.size.x=i; }
+#define nav_cube_SetSizeY(i) { nav_cube.size.y=i; }
+#define nav_cube_SetSizeZ(i) { nav_cube.size.z=i; }
+#define nav_cube_SetOffsX(i) { nav_cube.offs.x=i; }
+#define nav_cube_SetOffsY(i) { nav_cube.offs.y=i; }
+#define nav_cube_SetOffsZ(i) { nav_cube.offs.z=i; }
+#define nav_cube_SetNSectX(i) { nav_cube.nsect_x=i; }
+#define nav_cube_SetNSectZ(i) { nav_cube.nsect_z=i; }
+
+
+#endif /* NAV_CUBE_H */

sw/airborne/subsystems/navigation/nav_cube.h

@@ -1,138 +0,0 @@
-/*
- * Copyright (C) 2010  Martin Mueller
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-/**
- * @file subsystems/navigation/nav_cube.h
- *
- * Fixedwing Navigation in a cube towards a center.
- *
- */
-
-#ifndef NAV_CUBE_H
-#define NAV_CUBE_H
-
-#include "std.h"
-
-#define MAX_LINES_X 8
-#define STBY_OFFSET 500
-
-extern bool_t nav_cube_init(uint8_t turb, uint8_t tb, uint8_t te);
-bool_t nav_cube(int8_t j, int8_t i,
-                uint8_t dest_b, uint8_t dest_e,
-                uint8_t src_b, uint8_t src_e);
-
-extern int32_t cube_alpha;     ///< angle of flight direction to north, clockwise
-extern int32_t cube_size_x;    ///< size of the cube x (perpendicular to flight dir)
-extern int32_t cube_size_y;    ///< size of the cube y (in flight dir)
-extern int32_t cube_size_z;    ///< height of the cube z
-extern int32_t cube_grid_x;    ///< grid distance x (horizontal)
-extern int32_t cube_grid_z;    ///< grid distance z (vertical)
-extern int32_t cube_offs_x;    ///< offset to center x (horizontal)
-extern int32_t cube_offs_y;    ///< offset to center y (in direction)
-extern int32_t cube_offs_z;    ///< offset to center z (vertical)
-
-extern int32_t cube_sect;      ///< sector to fly in (1..[nsect_x*nsect_z])
-extern int32_t cube_nsect_x;   ///< number of sectors horizontal
-extern int32_t cube_nsect_z;   ///< number of sectors vertical
-
-extern int32_t cube_nline_x;   ///< number of lines x (horizontal)
-extern int32_t cube_nline_z;   ///< number of lines z (vertical)
-
-#define nav_cube_SetAlpha(i) { cube_alpha=i; }
-#define nav_cube_SetSect(i)  { cube_sect=i; }
-#define nav_cube_SetGridX(i) { cube_grid_x=i; }
-#define nav_cube_SetGridZ(i) { cube_grid_z=i; }
-#define nav_cube_SetSizeX(i) { cube_size_x=i; }
-#define nav_cube_SetSizeY(i) { cube_size_y=i; }
-#define nav_cube_SetSizeZ(i) { cube_size_z=i; }
-#define nav_cube_SetOffsX(i) { cube_offs_x=i; }
-#define nav_cube_SetOffsY(i) { cube_offs_y=i; }
-#define nav_cube_SetOffsZ(i) { cube_offs_z=i; }
-#define nav_cube_SetNSectX(i) { cube_nsect_x=i; }
-#define nav_cube_SetNSectZ(i) { cube_nsect_z=i; }
-
-/*
-
- from above:
-
-         | (offs_x)
-
-         o [x, y]    -
-                     |
-                     | (offs_y)
-                     |
-                     -
-   ^  ^  ^  ^  ^     |
-   |  |  |  |  |     |
-   |  |  |  |  |     | (size_y)
-   |  |  |  |  |     |
-   |  |  |  |  |     |
-                     -
-   |--|
-    (grid_x)
-
-   |-----------|
-     (size_x)
-
-
- side view:
-
-   --------->                    -           -
-                                 | (grid_z)  |
-   --------->                    -           |
-                                 -           | (size_z)
-   --------->                    | (offs_z)  |
-                      o [alt]    -           |
-   --------->                                -
-
-  |----------|--------|
-    (size_y)  (offs_y)
-
-
- back view, 1x1 sector:
-
-   -------
-  |       |
-  |   1   |  ^
-  |       |  |
-  s-------   |
-
-  ---->
-
-
- back view, 3x2 sectors:
-
-   ------- ------- -------
-  |       |       |       |
-  |   4   |   5   |   6   |  ^
-  |       |       |       |  |
-  s-------s-------s-------   |
-  |       |       |       |
-  |   1   |   2   |   3   |  ^
-  |       |       |       |  |
-  s-------s-------s-------   | (nsect_z)
-
-  ---->   ---->   ---->
-   (nsect_x)
-
-*/
-
-#endif /* NAV_CUBE_H */