diff --git a/conf/firmwares/subsystems/fixedwing/navigation_extra.makefile b/conf/firmwares/subsystems/fixedwing/navigation_extra.makefile index c9d42eda7b..91828035b2 100644 --- a/conf/firmwares/subsystems/fixedwing/navigation_extra.makefile +++ b/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 diff --git a/conf/flight_plans/cube.xml b/conf/flight_plans/cube.xml index 93754e3b33..aa4487f645 100644 --- a/conf/flight_plans/cube.xml +++ b/conf/flight_plans/cube.xml @@ -2,10 +2,6 @@ -
-#include "subsystems/navigation/nav_cube.h" -
- diff --git a/conf/modules/nav_cube.xml b/conf/modules/nav_cube.xml new file mode 100644 index 0000000000..587aff3514 --- /dev/null +++ b/conf/modules/nav_cube.xml @@ -0,0 +1,15 @@ + + + + + + Fixedwing Navigation in a cube towards a center. + + +
+ +
+ + + + diff --git a/sw/airborne/subsystems/navigation/nav_cube.c b/sw/airborne/modules/nav/nav_cube.c similarity index 57% rename from sw/airborne/subsystems/navigation/nav_cube.c rename to sw/airborne/modules/nav/nav_cube.c index 574a4348f0..4ae06714fe 100644 --- a/sw/airborne/subsystems/navigation/nav_cube.c +++ b/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; -int32_t cube_size_x, cube_size_y, cube_size_z; -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; +#define MAX_LINES_X 8 +#define STBY_OFFSET 500 -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 (cube_nsect_z <= 0) cube_nsect_z = 1; - if ((cube_sect <= 0) || - (cube_sect > (cube_nsect_x*cube_nsect_z))) cube_sect = 1; + if (nav_cube.nsect_x <= 0) + nav_cube.nsect_x = 1; + if (nav_cube.nsect_z <= 0) + 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; - else cube_nline_x_t = cube_size_x / cube_grid_x + 1; - if (cube_grid_z == 0) cube_nline_z_t = 1; - else cube_nline_z_t = cube_size_z / cube_grid_z + 1; + if (nav_cube.grid_x == 0) + cube_nline_x_t = 1; + else + 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_line_x_start = (cube_pos_x * cube_nline_x_t)/cube_nsect_x; - cube_line_x_end = ((cube_pos_x+1) * cube_nline_x_t)/cube_nsect_x; - if (cube_line_x_end > cube_nline_x_t) cube_line_x_end = cube_nline_x_t; - cube_nline_x = cube_line_x_end - cube_line_x_start; + cube_pos_x = (nav_cube.sect-1) % nav_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)/nav_cube.nsect_x; + if (cube_line_x_end > cube_nline_x_t) + 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_line_z_start = (cube_pos_z * cube_nline_z_t)/cube_nsect_z; - cube_line_z_end = ((cube_pos_z+1) * cube_nline_z_t)/cube_nsect_z; - cube_nline_z = cube_line_z_end - cube_line_z_start; + cube_pos_z = (nav_cube.sect-1) / nav_cube.nsect_x; + 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)/nav_cube.nsect_z; + 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) - + cube_offs_x; + start_bx = WaypointX(center) - (((cube_nline_x_t-1) * nav_cube.grid_x)/2) + + 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) - + (cube_line_z_start*cube_grid_z) + cube_offs_z; + start_bz = waypoints[center].a - (((cube_nline_z_t-1) * nav_cube.grid_z)/2) + + (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 ? */ - cube_nline_x--; - cube_nline_z--; + nav_cube.nline_x--; + 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 (j > cube_nline_z) return FALSE; + if (i > nav_cube.nline_x) + 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; } diff --git a/sw/airborne/modules/nav/nav_cube.h b/sw/airborne/modules/nav/nav_cube.h new file mode 100644 index 0000000000..04aba18acb --- /dev/null +++ b/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 */ diff --git a/sw/airborne/subsystems/navigation/nav_cube.h b/sw/airborne/subsystems/navigation/nav_cube.h deleted file mode 100644 index 32766e8e8f..0000000000 --- a/sw/airborne/subsystems/navigation/nav_cube.h +++ /dev/null @@ -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 */