From f5a8565cc936179df946af8479f1a5fabeeac886 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 21 Aug 2013 20:58:44 +0200 Subject: [PATCH] [fixedwing] convert disc_survey to module --- .../fixedwing/navigation_extra.makefile | 1 - conf/modules/nav_disc_survey.xml | 15 +++ sw/airborne/math/pprz_algebra_float.h | 1 + sw/airborne/modules/nav/nav_disc_survey.c | 122 ++++++++++++++++++ .../nav/nav_disc_survey.h} | 12 +- .../subsystems/navigation/discsurvey.c | 111 ---------------- sw/include/std.h | 3 - 7 files changed, 144 insertions(+), 121 deletions(-) create mode 100644 conf/modules/nav_disc_survey.xml create mode 100644 sw/airborne/modules/nav/nav_disc_survey.c rename sw/airborne/{subsystems/navigation/discsurvey.h => modules/nav/nav_disc_survey.h} (78%) delete mode 100644 sw/airborne/subsystems/navigation/discsurvey.c diff --git a/conf/firmwares/subsystems/fixedwing/navigation_extra.makefile b/conf/firmwares/subsystems/fixedwing/navigation_extra.makefile index e4e33e11232..cbd3236864b 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/discsurvey.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/OSAMNav.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/snav.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/poly_survey_adv.c diff --git a/conf/modules/nav_disc_survey.xml b/conf/modules/nav_disc_survey.xml new file mode 100644 index 00000000000..9964d80ea77 --- /dev/null +++ b/conf/modules/nav_disc_survey.xml @@ -0,0 +1,15 @@ + + + + + + Disc Survey. + + +
+ +
+ + + + diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index 19bab93efa0..89f4cb95023 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.h @@ -138,6 +138,7 @@ struct FloatRates { FLOAT_VECT2_SMUL(_v, _v, 1./n); \ } +#define FLOAT_VECT2_DOT_PRODUCT(_v1, _v2) ((_v1).x*(_v2).x + (_v1).y*(_v2).y) /* * Dimension 3 Vectors diff --git a/sw/airborne/modules/nav/nav_disc_survey.c b/sw/airborne/modules/nav/nav_disc_survey.c new file mode 100644 index 00000000000..5291e544128 --- /dev/null +++ b/sw/airborne/modules/nav/nav_disc_survey.c @@ -0,0 +1,122 @@ +/* + * Copyright (C) 2003-2005 Pascal Brisset, Antoine Drouin + * + * 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_disc_survey.c + * + */ + +#include "modules/nav/nav_disc_survey.h" + +#include "generated/airframe.h" +#include "state.h" +#include "std.h" +#include "subsystems/nav.h" +#include "generated/flight_plan.h" +#include "math/pprz_algebra_float.h" + +enum DiscSurveyStatus { UTURN, SEGMENT, DOWNWIND }; + +struct DiscSurvey { + enum DiscSurveyStatus status; + int8_t sign; + struct FloatVect2 c; + struct FloatVect2 c1; + struct FloatVect2 c2; +}; + +static struct DiscSurvey disc_survey; + + +bool_t nav_disc_survey_start( float grid ) { + nav_survey_shift = grid; + disc_survey.status = DOWNWIND; + disc_survey.sign = 1; + disc_survey.c1.x = stateGetPositionEnu_f()->x; + disc_survey.c1.y = stateGetPositionEnu_f()->y; + return FALSE; +} + +bool_t nav_disc_survey_run( uint8_t center_wp, float radius) { + struct FloatVect2* wind = stateGetHorizontalWindspeed_f(); + float wind_dir = atan2(wind->x, wind->y) + M_PI; + + /** Not null even if wind_east=wind_north=0 */ + struct FloatVect2 upwind; + upwind.x = cos(wind_dir); + upwind.y = sin(wind_dir); + + float grid = nav_survey_shift / 2; + + switch (disc_survey.status) { + case UTURN: + nav_circle_XY(disc_survey.c.x, disc_survey.c.y, grid*disc_survey.sign); + if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) { + disc_survey.c1.x = stateGetPositionEnu_f()->x; + disc_survey.c1.y = stateGetPositionEnu_f()->y; + + struct FloatVect2 dist; + VECT2_DIFF(dist, disc_survey.c1, waypoints[center_wp]); + float d = FLOAT_VECT2_DOT_PRODUCT(upwind, dist); + if (d > radius) { + disc_survey.status = DOWNWIND; + } else { + float w = sqrtf(radius*radius - d*d) - 1.5*grid; + + struct FloatVect2 crosswind; + crosswind.x = -upwind.y; + crosswind.y = upwind.x; + + disc_survey.c2.x = waypoints[center_wp].x + d*upwind.x - w*disc_survey.sign*crosswind.x; + disc_survey.c2.y = waypoints[center_wp].y + d*upwind.y - w*disc_survey.sign*crosswind.y; + + disc_survey.status = SEGMENT; + } + nav_init_stage(); + } + break; + + case DOWNWIND: + disc_survey.c2.x = waypoints[center_wp].x - upwind.x * radius; + disc_survey.c2.y = waypoints[center_wp].y - upwind.y * radius; + disc_survey.status = SEGMENT; + /* No break; */ + + case SEGMENT: + nav_route_xy(disc_survey.c1.x, disc_survey.c1.y, disc_survey.c2.x, disc_survey.c2.y); + if (nav_approaching_xy(disc_survey.c2.x, disc_survey.c2.y, disc_survey.c1.x, disc_survey.c1.y, CARROT)) { + disc_survey.c.x = disc_survey.c2.x + grid*upwind.x; + disc_survey.c.y = disc_survey.c2.y + grid*upwind.y; + + disc_survey.sign = -disc_survey.sign; + disc_survey.status = UTURN; + nav_init_stage(); + } + break; + default: + break; + } + + NavVerticalAutoThrottleMode(0.); /* No pitch */ + NavVerticalAltitudeMode(WaypointAlt(center_wp), 0.); /* No preclimb */ + + return TRUE; +} diff --git a/sw/airborne/subsystems/navigation/discsurvey.h b/sw/airborne/modules/nav/nav_disc_survey.h similarity index 78% rename from sw/airborne/subsystems/navigation/discsurvey.h rename to sw/airborne/modules/nav/nav_disc_survey.h index 0bc5cdb32ea..7101399e86b 100644 --- a/sw/airborne/subsystems/navigation/discsurvey.h +++ b/sw/airborne/modules/nav/nav_disc_survey.h @@ -20,16 +20,16 @@ */ /** - * @file subsystems/navigation/discsurvey.h + * @file modules/nav/nav_disc_survey.h * */ -#ifndef DISCSURVEY_H -#define DISCSURVEY_H +#ifndef NAV_DISC_SURVEY_H +#define NAV_DISC_SURVEY_H #include "std.h" -extern bool_t disc_survey_init( float grid ); -extern bool_t disc_survey(uint8_t c, float radius); +extern bool_t nav_disc_survey_start( float grid ); +extern bool_t nav_disc_survey_run(uint8_t c, float radius); -#endif /* DISCSURVEY_H */ +#endif /* NAV_DISC_SURVEY_H */ diff --git a/sw/airborne/subsystems/navigation/discsurvey.c b/sw/airborne/subsystems/navigation/discsurvey.c deleted file mode 100644 index 5da1126ad1b..00000000000 --- a/sw/airborne/subsystems/navigation/discsurvey.c +++ /dev/null @@ -1,111 +0,0 @@ -/* - * Copyright (C) 2003-2005 Pascal Brisset, Antoine Drouin - * - * 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/discsurvey.c - * - */ - -#include "subsystems/navigation/discsurvey.h" - -#include "generated/airframe.h" -#include "state.h" -#include "std.h" -#include "subsystems/nav.h" -#include "generated/flight_plan.h" - -enum status { UTURN, SEGMENT, DOWNWIND }; -static enum status status; -static int8_t sign; -static struct point c; -static struct point c1; -static struct point c2; - -bool_t disc_survey_init( float grid ) { - nav_survey_shift = grid; - status = DOWNWIND; - sign = 1; - c1.x = stateGetPositionEnu_f()->x; - c1.y = stateGetPositionEnu_f()->y; - return FALSE; -} - -bool_t disc_survey( uint8_t center, float radius) { - struct FloatVect2* wind = stateGetHorizontalWindspeed_f(); - float wind_dir = atan2(wind->x, wind->y) + M_PI; - - /** Not null even if wind_east=wind_north=0 */ - float upwind_x = cos(wind_dir); - float upwind_y = sin(wind_dir); - - float grid = nav_survey_shift / 2; - - switch (status) { - case UTURN: - nav_circle_XY(c.x, c.y, grid*sign); - if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) { - c1.x = stateGetPositionEnu_f()->x; - c1.y = stateGetPositionEnu_f()->y; - - float d = ScalarProduct(upwind_x, upwind_y, stateGetPositionEnu_f()->x-WaypointX(center), stateGetPositionEnu_f()->y-WaypointY(center)); - if (d > radius) { - status = DOWNWIND; - } else { - float w = sqrt(radius*radius - d*d) - 1.5*grid; - - float crosswind_x = - upwind_y; - float crosswind_y = upwind_x; - - c2.x = WaypointX(center)+d*upwind_x-w*sign*crosswind_x; - c2.y = WaypointY(center)+d*upwind_y-w*sign*crosswind_y; - - status = SEGMENT; - } - nav_init_stage(); - } - break; - - case DOWNWIND: - c2.x = WaypointX(center) - upwind_x * radius; - c2.y = WaypointY(center) - upwind_y * radius; - status = SEGMENT; - /* No break; */ - - case SEGMENT: - nav_route_xy(c1.x, c1.y, c2.x, c2.y); - if (nav_approaching_xy(c2.x, c2.y, c1.x, c1.y, CARROT)) { - c.x = c2.x + grid*upwind_x; - c.y = c2.y + grid*upwind_y; - - sign = -sign; - status = UTURN; - nav_init_stage(); - } - break; - default: - break; - } - - NavVerticalAutoThrottleMode(0.); /* No pitch */ - NavVerticalAltitudeMode(WaypointAlt(center), 0.); /* No preclimb */ - - return TRUE; -} diff --git a/sw/include/std.h b/sw/include/std.h index e164ea57263..f75c36c063e 100644 --- a/sw/include/std.h +++ b/sw/include/std.h @@ -137,9 +137,6 @@ typedef uint8_t unit_t; #define Blend(a, b, rho) (((rho)*(a))+(1-(rho))*(b)) -#define ScalarProduct(x1,y1,x2,y2) ((x1)*(x2)+(y1)*(y2)) - - #define RunOnceEvery(_prescaler, _code) { \ static uint16_t prescaler = 0; \ prescaler++; \