From a5263570f865c60bfe10b0d4d2f55bb43d70a0d1 Mon Sep 17 00:00:00 2001 From: agressiva Date: Mon, 4 May 2015 11:52:25 -0300 Subject: [PATCH] [modules] add rotorcraft polygon survey --- conf/modules/nav_survey_poly_rotorcraft.xml | 32 ++ .../modules/nav/nav_survey_poly_rotorcraft.c | 540 ++++++++++++++++++ .../modules/nav/nav_survey_poly_rotorcraft.h | 62 ++ 3 files changed, 634 insertions(+) create mode 100644 conf/modules/nav_survey_poly_rotorcraft.xml create mode 100644 sw/airborne/modules/nav/nav_survey_poly_rotorcraft.c create mode 100644 sw/airborne/modules/nav/nav_survey_poly_rotorcraft.h diff --git a/conf/modules/nav_survey_poly_rotorcraft.xml b/conf/modules/nav_survey_poly_rotorcraft.xml new file mode 100644 index 00000000000..afd1a909570 --- /dev/null +++ b/conf/modules/nav_survey_poly_rotorcraft.xml @@ -0,0 +1,32 @@ + + + + + +Polygon survey from OSAM. +You can use: +* nav_survey_poly_osam_setup function for static definitions or +* nav_survey_poly_osam_setup_towards for a more flexible setup + + + + + + + + + + + + + + + + +
+ +
+ + + + diff --git a/sw/airborne/modules/nav/nav_survey_poly_rotorcraft.c b/sw/airborne/modules/nav/nav_survey_poly_rotorcraft.c new file mode 100644 index 00000000000..ca18804d9e3 --- /dev/null +++ b/sw/airborne/modules/nav/nav_survey_poly_rotorcraft.c @@ -0,0 +1,540 @@ +/* + * Copyright (C) 2008-2014 The Paparazzi Team + * + * 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_survey_poly_rotorcraft.c + * + */ + +/* +#include +#include "mcu_periph/uart.h" +#include "messages.h" +#include "subsystems/datalink/downlink.h" +*/ + + + +#include "modules/nav/nav_survey_poly_rotorcraft.h" + +#include "firmwares/rotorcraft/navigation.h" +#include "state.h" +#include "autopilot.h" +#include "generated/flight_plan.h" + +#ifdef DIGITAL_CAM +#include "modules/digital_cam/dc.h" +#endif + +#ifndef POLYSURVEY_DEFAULT_SIZE +#define POLYSURVEY_DEFAULT_SIZE 10 +#endif + +#ifndef POLYSURVEY_DEFAULT_DISTANCE +#define POLYSURVEY_DEFAULT_DISTANCE 25 +#endif + +/// if 0 default to half sweep +#ifndef POLYSURVEY_ENTRY_DISTANCE +#define POLYSURVEY_ENTRY_DISTANCE 0 +#endif + +/// maximum number of polygon corners +#ifndef POLYSURVEY_MAX_POLYGONSIZE +#define POLYSURVEY_MAX_POLYGONSIZE 20 +#endif + +uint8_t Poly_Size = POLYSURVEY_DEFAULT_SIZE; +float Poly_Distance = POLYSURVEY_DEFAULT_DISTANCE; + + +bool_t nav_survey_poly_setup_towards(uint8_t FirstWP, uint8_t Size, float Sweep, int SecondWP) +{ + float dx = waypoints[SecondWP].enu_f.x - waypoints[FirstWP].enu_f.x; + float dy = waypoints[SecondWP].enu_f.y - waypoints[FirstWP].enu_f.y; + if (dx == 0.0f) { dx = 0.000000001; } + float ang = atan(dy / dx); + ang = DegOfRad(ang); + + //if values passed, use it. + if (Size == 0) {Size = Poly_Size;} + if (Sweep == 0) {Sweep = Poly_Distance;} + return nav_survey_poly_setup(FirstWP, Size, Sweep, ang); +} + +struct Point2D {float x; float y;}; +struct Line {float m; float b; float x;}; + +static void TranslateAndRotateFromWorld(struct EnuCoor_f *p, float Zrot, float transX, float transY); +static void RotateAndTranslateToWorld(struct EnuCoor_f *p, float Zrot, float transX, float transY); +static void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2); +static float EvaluateLineForX(float y, struct Line L); + +#define MaxPolygonSize POLYSURVEY_MAX_POLYGONSIZE +#define MaxFloat 1000000000 +#define MinFloat -1000000000 + +#ifndef LINE_START_FUNCTION +#define LINE_START_FUNCTION {} +#endif +#ifndef LINE_STOP_FUNCTION +#define LINE_STOP_FUNCTION {} +#endif + +/************** Polygon Survey **********************************************/ + +/** This routine will cover the enitre area of any Polygon defined in the flightplan which is a convex polygon. + */ + +enum SurveyStatus { Init, Entry, Sweep, Turn }; +static enum SurveyStatus CSurveyStatus; +static struct Point2D SmallestCorner; +static struct Line Edges[MaxPolygonSize]; +static float EdgeMaxY[MaxPolygonSize]; +static float EdgeMinY[MaxPolygonSize]; +static float SurveyTheta; +static float dSweep; +static struct EnuCoor_f SurveyToWP; +static struct EnuCoor_f SurveyFromWP; +static struct EnuCoor_f SurveyEntry; + +//static struct EnuCoor_f survey_from, survey_to; +static struct EnuCoor_i survey_from_i, survey_to_i; + +static uint8_t SurveyEntryWP; +static uint8_t SurveySize; +//static float SurveyCircleQdr; +static float MaxY; +uint16_t PolySurveySweepNum; +uint16_t PolySurveySweepBackNum; +float EntryRadius; + +//========================================================================================================================= +bool_t nav_survey_poly_setup(uint8_t EntryWP, uint8_t Size, float sw, float Orientation) +{ + SmallestCorner.x = 0; + SmallestCorner.y = 0; + int i = 0; + float ys = 0; + static struct EnuCoor_f EntryPoint; + float LeftYInt; + float RightYInt; + float temp; + float XIntercept1 = 0; + float XIntercept2 = 0; + float entry_distance; + + float PolySurveyEntryDistance = POLYSURVEY_ENTRY_DISTANCE; + + if (PolySurveyEntryDistance == 0) { + entry_distance = sw / 2; + } else { + entry_distance = PolySurveyEntryDistance; + } + + SurveyTheta = RadOfDeg(Orientation); + PolySurveySweepNum = 0; + PolySurveySweepBackNum = 0; + + SurveyEntryWP = EntryWP; + SurveySize = Size; + + struct EnuCoor_f Corners[MaxPolygonSize]; + + CSurveyStatus = Init; + + if (Size == 0) { + return TRUE; + } + + //Don't initialize if Polygon is too big or if the orientation is not between 0 and 90 + if (Size <= MaxPolygonSize && Orientation >= -90 && Orientation <= 90) { + //Initialize Corners + for (i = 0; i < Size; i++) { + Corners[i].x = waypoints[i + EntryWP].enu_f.x; + Corners[i].y = waypoints[i + EntryWP].enu_f.y; + } + + //Rotate Corners so sweeps are parellel with x axis + for (i = 0; i < Size; i++) { + TranslateAndRotateFromWorld(&Corners[i], SurveyTheta, 0, 0); + } + + //Find min x and min y + SmallestCorner.y = Corners[0].y; + SmallestCorner.x = Corners[0].x; + for (i = 1; i < Size; i++) { + if (Corners[i].y < SmallestCorner.y) { + SmallestCorner.y = Corners[i].y; + } + + if (Corners[i].x < SmallestCorner.x) { + SmallestCorner.x = Corners[i].x; + } + } + + //Translate Corners all exist in quad #1 + for (i = 0; i < Size; i++) { + TranslateAndRotateFromWorld(&Corners[i], 0, SmallestCorner.x, SmallestCorner.y); + } + + //Rotate and Translate Entry Point + EntryPoint.x = Corners[0].x; + EntryPoint.y = Corners[0].y; + + //Find max y + MaxY = Corners[0].y; + for (i = 1; i < Size; i++) { + if (Corners[i].y > MaxY) { + MaxY = Corners[i].y; + } + } + + //Find polygon edges + for (i = 0; i < Size; i++) { + if (i == 0) + if (Corners[Size - 1].x == Corners[i].x) { //Don't divide by zero! + Edges[i].m = MaxFloat; + } else { + Edges[i].m = ((Corners[Size - 1].y - Corners[i].y) / (Corners[Size - 1].x - Corners[i].x)); + } + else if (Corners[i].x == Corners[i - 1].x) { + Edges[i].m = MaxFloat; + } else { + Edges[i].m = ((Corners[i].y - Corners[i - 1].y) / (Corners[i].x - Corners[i - 1].x)); + } + + //Edges[i].m = MaxFloat; + Edges[i].b = (Corners[i].y - (Corners[i].x * Edges[i].m)); + } + + //Find Min and Max y for each line + FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[0], Edges[1]); + FindInterceptOfTwoLines(&temp, &RightYInt, Edges[0], Edges[Size - 1]); + + if (LeftYInt > RightYInt) { + EdgeMaxY[0] = LeftYInt; + EdgeMinY[0] = RightYInt; + } else { + EdgeMaxY[0] = RightYInt; + EdgeMinY[0] = LeftYInt; + } + + for (i = 1; i < Size - 1; i++) { + FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[i], Edges[i + 1]); + FindInterceptOfTwoLines(&temp, &RightYInt, Edges[i], Edges[i - 1]); + + if (LeftYInt > RightYInt) { + EdgeMaxY[i] = LeftYInt; + EdgeMinY[i] = RightYInt; + } else { + EdgeMaxY[i] = RightYInt; + EdgeMinY[i] = LeftYInt; + } + } + + FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[Size - 1], Edges[0]); + FindInterceptOfTwoLines(&temp, &RightYInt, Edges[Size - 1], Edges[Size - 2]); + + if (LeftYInt > RightYInt) { + EdgeMaxY[Size - 1] = LeftYInt; + EdgeMinY[Size - 1] = RightYInt; + } else { + EdgeMaxY[Size - 1] = RightYInt; + EdgeMinY[Size - 1] = LeftYInt; + } + + //Find amount to increment by every sweep + if (EntryPoint.y >= MaxY / 2) { + entry_distance = -entry_distance; + dSweep = -sw; + } else { + dSweep = sw; + } + + //Find y value of the first sweep + ys = EntryPoint.y + entry_distance; + + //Find the edges which intercet the sweep line first + for (i = 0; i < SurveySize; i++) { + if (EdgeMinY[i] <= ys && EdgeMaxY[i] > ys) { + XIntercept2 = XIntercept1; + XIntercept1 = EvaluateLineForX(ys, Edges[i]); + } + } + + //Find point to come from and point to go to + if (fabs(EntryPoint.x - XIntercept2) <= fabs(EntryPoint.x - XIntercept1)) { + SurveyToWP.x = XIntercept1; + SurveyToWP.y = ys; + + SurveyFromWP.x = XIntercept2; + SurveyFromWP.y = ys; + } else { + SurveyToWP.x = XIntercept2; + SurveyToWP.y = ys; + + SurveyFromWP.x = XIntercept1; + SurveyFromWP.y = ys; + } + + //Find the entry point + SurveyEntry.x = SurveyFromWP.x; + SurveyEntry.y = EntryPoint.y + entry_distance; + + //Go into entry state + CSurveyStatus = Entry; + + LINE_STOP_FUNCTION; + NavVerticalAltitudeMode(waypoints[SurveyEntryWP].enu_f.z, 0.); + nav_set_heading_deg(-Orientation + 90.); + + } + + return FALSE; +} + +//========================================================================================================================= +bool_t nav_survey_poly_run(void) +{ + + struct EnuCoor_f C; + struct EnuCoor_f ToP; + struct EnuCoor_f FromP; + float ys = 0; + static struct EnuCoor_f LastPoint; + int i; + bool_t LastHalfSweep; + static bool_t HalfSweep = FALSE; + float XIntercept1 = 0; + float XIntercept2 = 0; + float DInt1 = 0; + float DInt2 = 0; + + switch (CSurveyStatus) { + case Entry: + C = SurveyEntry; + RotateAndTranslateToWorld(&C, 0, SmallestCorner.x, SmallestCorner.y); + RotateAndTranslateToWorld(&C, SurveyTheta, 0, 0); + + ENU_BFP_OF_REAL(survey_from_i, C); + horizontal_mode = HORIZONTAL_MODE_ROUTE; + VECT3_COPY(navigation_target, survey_from_i); + + if (((nav_approaching_from(&survey_from_i, NULL, 0)) + && (fabsf(stateGetPositionEnu_f()->z - waypoints[SurveyEntryWP].enu_f.z)) < 1.)) { + CSurveyStatus = Sweep; + nav_init_stage(); + LINE_START_FUNCTION; + } + break; + case Sweep: + LastHalfSweep = HalfSweep; + ToP = SurveyToWP; + FromP = SurveyFromWP; + + //Rotate and Translate Line points into real world + RotateAndTranslateToWorld(&ToP, 0, SmallestCorner.x, SmallestCorner.y); + RotateAndTranslateToWorld(&ToP, SurveyTheta, 0, 0); + + RotateAndTranslateToWorld(&FromP, 0, SmallestCorner.x, SmallestCorner.y); + RotateAndTranslateToWorld(&FromP, SurveyTheta, 0, 0); + + //follow the line + ENU_BFP_OF_REAL(survey_to_i, ToP); + ENU_BFP_OF_REAL(survey_from_i, FromP); + + horizontal_mode = HORIZONTAL_MODE_ROUTE; + nav_route(&survey_from_i, &survey_to_i); + + if (nav_approaching_from(&survey_to_i, NULL, 0)) { + LastPoint = SurveyToWP; + +#ifdef DIGITAL_CAM + float line_length = fabsf((fabsf(FromP.x) - fabsf(ToP.x))); + double inteiro; + double fract = modf(line_length / dc_distance_interval, &inteiro); + if (fract > .5) { + //if last shot is more than shot_distance/2 from the corner then take a picture in the corner before go to the next sweep + dc_send_command(DC_SHOOT); + } +#endif + + //fprintf(stderr,"Lastpoint:%f <= %f\n",( LastPoint.y + dSweep ), 0. ); + + + if (LastPoint.y + dSweep >= MaxY || LastPoint.y + dSweep <= 0) { //Your out of the Polygon so Sweep Back or Half Sweep + //fprintf(stderr,"nao cabe interiro\n"); + + if ((LastPoint.y + (dSweep / 2)) <= MaxY || LastPoint.y + (dSweep / 2) >= 0) { //Sweep back + //fprintf(stderr,"nao cabe meio\n"); + dSweep = -dSweep; + } else { + //fprintf(stderr,"cabe meio\n"); + } + + if (LastHalfSweep) { + HalfSweep = FALSE; + ys = LastPoint.y + (dSweep); + } else { + HalfSweep = TRUE; + ys = LastPoint.y + (dSweep / 2); + } + + } else { // Normal sweep + //fprintf(stderr,"cabe interiro\n"); + + //Find y value of the first sweep + HalfSweep = FALSE; + ys = LastPoint.y + dSweep; + } + + //Find the edges which intercet the sweep line first + for (i = 0; i < SurveySize; i++) { + if (EdgeMinY[i] < ys && EdgeMaxY[i] >= ys) { + XIntercept2 = XIntercept1; + XIntercept1 = EvaluateLineForX(ys, Edges[i]); + } + } + + //Find point to come from and point to go to + DInt1 = XIntercept1 - LastPoint.x; + DInt2 = XIntercept2 - LastPoint.x; + + if (DInt1 * DInt2 >= 0) { + if (fabs(DInt2) <= fabs(DInt1)) { + SurveyToWP.x = XIntercept1; + SurveyToWP.y = ys; + + SurveyFromWP.x = XIntercept2; + SurveyFromWP.y = ys; + } else { + SurveyToWP.x = XIntercept2; + SurveyToWP.y = ys; + + SurveyFromWP.x = XIntercept1; + SurveyFromWP.y = ys; + } + } else { + if ((SurveyToWP.x - SurveyFromWP.x) > 0 && DInt2 > 0) { + SurveyToWP.x = XIntercept1; + SurveyToWP.y = ys; + + SurveyFromWP.x = XIntercept2; + SurveyFromWP.y = ys; + } else if ((SurveyToWP.x - SurveyFromWP.x) < 0 && DInt2 < 0) { + SurveyToWP.x = XIntercept1; + SurveyToWP.y = ys; + + SurveyFromWP.x = XIntercept2; + SurveyFromWP.y = ys; + } else { + SurveyToWP.x = XIntercept2; + SurveyToWP.y = ys; + + SurveyFromWP.x = XIntercept1; + SurveyFromWP.y = ys; + } + } + + //Go into Turn state + CSurveyStatus = Turn; + nav_init_stage(); + LINE_STOP_FUNCTION; + + PolySurveySweepNum++; + } + + break; + case Turn: + FromP = LastPoint; + ToP = SurveyFromWP; + + //Rotate and Translate Line points into real world + RotateAndTranslateToWorld(&ToP, 0, SmallestCorner.x, SmallestCorner.y); + RotateAndTranslateToWorld(&ToP, SurveyTheta, 0, 0); + + RotateAndTranslateToWorld(&FromP, 0, SmallestCorner.x, SmallestCorner.y); + RotateAndTranslateToWorld(&FromP, SurveyTheta, 0, 0); + + //follow the line + ENU_BFP_OF_REAL(survey_to_i, ToP); + ENU_BFP_OF_REAL(survey_from_i, FromP); + + horizontal_mode = HORIZONTAL_MODE_ROUTE; + nav_route(&survey_from_i, &survey_to_i); + + if (nav_approaching_from(&survey_to_i, NULL, 0)) { + CSurveyStatus = Sweep; + nav_init_stage(); + LINE_START_FUNCTION; + } + + break; + case Init: + return FALSE; + default: + return FALSE; + } + + return TRUE; + +} + +//============================================================================================================================================ +/* + Translates point so (transX, transY) are (0,0) then rotates the point around z by Zrot +*/ +void TranslateAndRotateFromWorld(struct EnuCoor_f *p, float Zrot, float transX, float transY) +{ + float temp; + + p->x = p->x - transX; + p->y = p->y - transY; + + temp = p->x; + p->x = p->x * cosf(Zrot) + p->y * sinf(Zrot); + p->y = -temp * sinf(Zrot) + p->y * cosf(Zrot); +} + +/// Rotates point round z by -Zrot then translates so (0,0) becomes (transX,transY) +void RotateAndTranslateToWorld(struct EnuCoor_f *p, float Zrot, float transX, float transY) +{ + float temp = p->x; + + p->x = p->x * cosf(Zrot) - p->y * sinf(Zrot); + p->y = temp * sinf(Zrot) + p->y * cosf(Zrot); + + p->x = p->x + transX; + p->y = p->y + transY; +} + +void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2) +{ + *x = ((L2.b - L1.b) / (L1.m - L2.m)); + *y = L1.m * (*x) + L1.b; +} + + +float EvaluateLineForX(float y, struct Line L) +{ + return ((y - L.b) / L.m); +} diff --git a/sw/airborne/modules/nav/nav_survey_poly_rotorcraft.h b/sw/airborne/modules/nav/nav_survey_poly_rotorcraft.h new file mode 100644 index 00000000000..769b81d6631 --- /dev/null +++ b/sw/airborne/modules/nav/nav_survey_poly_rotorcraft.h @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2008-2014 The Paparazzi Team + * + * 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_survey_poly_rotorcraft.h + * + */ + +#ifndef NAV_SURVEY_POLY_OSAM_H +#define NAV_SURVEY_POLY_OSAM_H + +#include "std.h" + +extern uint8_t Poly_Size; +extern float Poly_Distance; +extern uint16_t PolySurveySweepNum; +extern uint16_t PolySurveySweepBackNum; + +/** + * Setup polygon survey. + * @param FirstWP first waypoint/corner of the polygon + * @param Size number of waypoints/corners used to define the polygon + * @param Sweep distance between scan lines + * @param Orientation angle of scan lines in degrees (CCW, east) + */ +extern bool_t nav_survey_poly_setup(uint8_t FirstWP, uint8_t Size, float Sweep, float Orientation); + +/** + * Setup "dynamic" polygon survey with sweep orientation towards a waypoint. + * Computes the sweep orientation angle from the line FirstWP-SecondWP. + * If you pass zero for Size and/or Sweep it will use the global Poly_Size and + * Poly_Sweep variables respectively (which can be changed via telemetry/settings). + * @param FirstWp first waypoint/corner of the polygon + * @param Size number of waypoints/corners used to define the polygon, + * if zero uses Poly_Size + * @param Sweep distance between scan lines, if zero uses Poly_Sweep + * @param SecondWp second waypoint towards which the sweep orientation is computed + */ +extern bool_t nav_survey_poly_setup_towards(uint8_t FirstWP, uint8_t Size, float Sweep, int SecondWP); + +/** Run polygon survey */ +extern bool_t nav_survey_poly_run(void); + +#endif