Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 13:55:40 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

[nav] split osam nav module

Browse Source
This commit is contained in:
Gautier Hattenberger
2013-11-04 18:58:56 +01:00
committed by Felix Ruess
parent 64d518d785
commit 15b1240008
19 changed files with 1506 additions and 1318 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/flight_plans/nav_modules.xml
+4 -10
View File
@@ -73,6 +73,10 @@
<survey_rectangle grid="150" wp1="S1" wp2="S2"/>
</block>
<!--===================== Nav modules example blocks =================================-->
<block name="Bungee take-off">
<call fun="bungee_takeoff_start(WP_HOME)"/>
<call fun="bungee_takeoff_run()"/>
</block>
<block name="Poly Survey S1-S2" strip_button="Poly Survey">
<call fun="poly_survey_adv_start(WP_S1,5,180,30,10,50,100)"/>
<call fun="poly_survey_adv_run()"/>
@@ -91,20 +95,10 @@
<go from="1" hmode="route" wp="2"/>
<deroute block="Standby"/>
</block>
<!-- ===================== OSAM Nav modules example blocks =========================-->
<block name="Flower" strip_button="Flower (wp 1-2)">
<call fun="flower_start(WP_1, WP_2)"/>
<call fun="flower_run()"/>
</block>
<block name="Bungee take-off">
<call fun="bungee_takeoff_start(WP_HOME)"/>
<call fun="bungee_takeoff_run()"/>
</block>
<block name="Skid Landing">
<call fun="skid_landing_start(WP_AF,WP_TD,nav_radius)"/>
<call fun="skid_landing_run()"/>
</block>
<block name="Poly survey">
<call fun="osam_poly_survey_start(WP_S1, 5, 50, 45)"/>
<call fun="osam_poly_survey_run()"/>
conf/modules/nav_bungee_takeoff.xml
+15
View File
@@ -0,0 +1,15 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="nav_bungee_takeoff" dir="nav">
<doc>
<description>
Takeoff procedure using a bungee
</description>
</doc>
<header>
<file name="nav_bungee_takeoff.h"/>
</header>
<makefile>
<file name="nav_bungee_takeoff.c"/>
</makefile>
</module>
conf/modules/nav_flower.xml
+15
View File
@@ -0,0 +1,15 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="nav_flower" dir="nav">
<doc>
<description>
Make a flower navigation pattern
</description>
</doc>
<header>
<file name="nav_flower.h"/>
</header>
<makefile>
<file name="nav_flower.c"/>
</makefile>
</module>
conf/modules/nav_line_osam.xml
+15
View File
@@ -0,0 +1,15 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="nav_line_osam" dir="nav">
<doc>
<description>
Fly along a line
</description>
</doc>
<header>
<file name="nav_line_osam.h"/>
</header>
<makefile>
<file name="nav_line_osam.c"/>
</makefile>
</module>
conf/modules/nav_osam.xml
-20
View File
@@ -1,20 +0,0 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="nav_osam" dir="nav">
<doc>
<description>
OSAM collection of advanced navigation routines.
-Flower
-Bungee take-off
-Polygon survey
-Vertical raster
-Skid landing
</description>
</doc>
<header>
<file name="nav_osam.h"/>
</header>
<makefile>
<file name="nav_osam.c"/>
</makefile>
</module>
conf/modules/nav_poly_survey_osam.xml
+15
View File
@@ -0,0 +1,15 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="nav_poly_survey_osam" dir="nav">
<doc>
<description>
Polygon survey from OSAM
</description>
</doc>
<header>
<file name="nav_poly_survey_osam.h"/>
</header>
<makefile>
<file name="nav_poly_survey_osam.c"/>
</makefile>
</module>
conf/modules/nav_vertical_raster.xml
+15
View File
@@ -0,0 +1,15 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="nav_vertical_raster" dir="nav">
<doc>
<description>
Copy of nav line with altitude changes at every sweep
</description>
</doc>
<header>
<file name="nav_vertical_raster.h"/>
</header>
<makefile>
<file name="nav_vertical_raster.c"/>
</makefile>
</module>
sw/airborne/modules/nav/nav_bungee_takeoff.c
+225
View File
@@ -0,0 +1,225 @@
/*
* Copyright (C) 2008-2013 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_bungee_takeoff.c
*
* from OSAM advanced navigation routines
*
*/
#include "modules/nav/nav_bungee_takeoff.h"
#include "subsystems/nav.h"
#include "state.h"
#include "autopilot.h"
#include "generated/flight_plan.h"
/************** Bungee Takeoff **********************************************/
/** Takeoff functions for bungee takeoff.
Run initialize function when the plane is on the bungee, the bungee is fully extended and you are ready to
launch the plane. After initialized, the plane will follow a line drawn by the position of the plane on initialization and the
position of the bungee (given in the arguments). Once the plane crosses the throttle line, which is perpendicular to the line the plane is following,
and intersects the position of the bungee (plus or minus a fixed distance (TakeOff_Distance in airframe file) from the bungee just in case the bungee doesn't release directly above the bungee) the prop will come on. The plane will then continue to follow the line until it has reached a specific
height (defined in as Takeoff_Height in airframe file) above the bungee waypoint and speed (defined as Takeoff_Speed in the airframe file).
@verbatim
<section name="Takeoff" prefix="Takeoff_">
<define name="Height" value="30" unit="m"/>
<define name="Speed" value="15" unit="m/s"/>
<define name="Distance" value="10" unit="m"/>
<define name="MinSpeed" value="5" unit="m/s"/>
</section>
@endverbatim
*/
#ifndef Takeoff_Distance
#define Takeoff_Distance 10
#endif
#ifndef Takeoff_Height
#define Takeoff_Height 30
#endif
#ifndef Takeoff_Speed
#define Takeoff_Speed 15
#endif
#ifndef Takeoff_MinSpeed
#define Takeoff_MinSpeed 5
#endif
enum TakeoffStatus { Launch, Throttle, Finished };
static enum TakeoffStatus CTakeoffStatus;
static float throttlePx;
static float throttlePy;
static float initialx;
static float initialy;
static float ThrottleSlope;
static bool_t AboveLine;
static float BungeeAlt;
static float TDistance;
static uint8_t BungeeWaypoint;
bool_t bungee_takeoff_start(uint8_t BungeeWP)
{
float ThrottleB;
initialx = stateGetPositionEnu_f()->x;
initialy = stateGetPositionEnu_f()->y;
BungeeWaypoint = BungeeWP;
//Takeoff_Distance can only be positive
TDistance = fabs(Takeoff_Distance);
//Translate initial position so that the position of the bungee is (0,0)
float Currentx = initialx-(WaypointX(BungeeWaypoint));
float Currenty = initialy-(WaypointY(BungeeWaypoint));
//Record bungee alt (which should be the ground alt at that point)
BungeeAlt = waypoints[BungeeWaypoint].a;
//Find Launch line slope and Throttle line slope
float MLaunch = Currenty/Currentx;
//Find Throttle Point (the point where the throttle line and launch line intersect)
if(Currentx < 0)
throttlePx = TDistance/sqrt(MLaunch*MLaunch+1);
else
throttlePx = -(TDistance/sqrt(MLaunch*MLaunch+1));
if(Currenty < 0)
throttlePy = sqrt((TDistance*TDistance)-(throttlePx*throttlePx));
else
throttlePy = -sqrt((TDistance*TDistance)-(throttlePx*throttlePx));
//Find ThrottleLine
ThrottleSlope = tan(atan2(Currenty,Currentx)+(3.14/2));
ThrottleB = (throttlePy - (ThrottleSlope*throttlePx));
//Determine whether the UAV is below or above the throttle line
if(Currenty > ((ThrottleSlope*Currentx)+ThrottleB))
AboveLine = TRUE;
else
AboveLine = FALSE;
//Enable Launch Status and turn kill throttle on
CTakeoffStatus = Launch;
kill_throttle = 1;
//Translate the throttle point back
throttlePx = throttlePx+(WaypointX(BungeeWP));
throttlePy = throttlePy+(WaypointY(BungeeWP));
return FALSE;
}
bool_t bungee_takeoff_run(void)
{
//Translate current position so Throttle point is (0,0)
float Currentx = stateGetPositionEnu_f()->x-throttlePx;
float Currenty = stateGetPositionEnu_f()->y-throttlePy;
bool_t CurrentAboveLine;
float ThrottleB;
switch(CTakeoffStatus)
{
case Launch:
//Follow Launch Line
NavVerticalAutoThrottleMode(0);
NavVerticalAltitudeMode(BungeeAlt+Takeoff_Height, 0.);
nav_route_xy(initialx,initialy,throttlePx,throttlePy);
kill_throttle = 1;
//recalculate lines if below min speed
if((*stateGetHorizontalSpeedNorm_f()) < Takeoff_MinSpeed)
{
initialx = stateGetPositionEnu_f()->x;
initialy = stateGetPositionEnu_f()->y;
//Translate initial position so that the position of the bungee is (0,0)
Currentx = initialx-(WaypointX(BungeeWaypoint));
Currenty = initialy-(WaypointY(BungeeWaypoint));
//Find Launch line slope
float MLaunch = Currenty/Currentx;
//Find Throttle Point (the point where the throttle line and launch line intersect)
if(Currentx < 0)
throttlePx = TDistance/sqrt(MLaunch*MLaunch+1);
else
throttlePx = -(TDistance/sqrt(MLaunch*MLaunch+1));
if(Currenty < 0)
throttlePy = sqrt((TDistance*TDistance)-(throttlePx*throttlePx));
else
throttlePy = -sqrt((TDistance*TDistance)-(throttlePx*throttlePx));
//Find ThrottleLine
ThrottleSlope = tan(atan2(Currenty,Currentx)+(3.14/2));
ThrottleB = (throttlePy - (ThrottleSlope*throttlePx));
//Determine whether the UAV is below or above the throttle line
if(Currenty > ((ThrottleSlope*Currentx)+ThrottleB))
AboveLine = TRUE;
else
AboveLine = FALSE;
//Translate the throttle point back
throttlePx = throttlePx+(WaypointX(BungeeWaypoint));
throttlePy = throttlePy+(WaypointY(BungeeWaypoint));
}
//Find out if the UAV is currently above the line
if(Currenty > (ThrottleSlope*Currentx))
CurrentAboveLine = TRUE;
else
CurrentAboveLine = FALSE;
//Find out if UAV has crossed the line
if(AboveLine != CurrentAboveLine && (*stateGetHorizontalSpeedNorm_f()) > Takeoff_MinSpeed)
{
CTakeoffStatus = Throttle;
kill_throttle = 0;
nav_init_stage();
}
break;
case Throttle:
//Follow Launch Line
NavVerticalAutoThrottleMode(AGR_CLIMB_PITCH);
NavVerticalThrottleMode(9600*(1));
nav_route_xy(initialx,initialy,throttlePx,throttlePy);
kill_throttle = 0;
if((stateGetPositionEnu_f()->z > BungeeAlt+Takeoff_Height-10) && ((*stateGetHorizontalSpeedNorm_f()) > Takeoff_Speed))
{
CTakeoffStatus = Finished;
return FALSE;
}
else
{
return TRUE;
}
break;
default:
break;
}
return TRUE;
}
sw/airborne/modules/nav/nav_bungee_takeoff.h
+37
View File
@@ -0,0 +1,37 @@
/*
* Copyright (C) 2008-2013 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_bungee_takeoff.h
*
* from OSAM advanced navigation routines
*
*/
#ifndef NAV_BUNGEE_TAKEOFF_H
#define NAV_BUNGEE_TAKEOFF_H
#include "std.h"
extern bool_t bungee_takeoff_start(uint8_t BungeeWP);
extern bool_t bungee_takeoff_run(void);
#endif
sw/airborne/modules/nav/nav_flower.c
+155
View File
@@ -0,0 +1,155 @@
/*
* Copyright (C) 2008-2013 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_flower.c
*
* from OSAM advanced navigation routines
*/
#include "modules/nav/nav_flower.h"
#include "subsystems/nav.h"
#include "state.h"
#include "autopilot.h"
#include "generated/flight_plan.h"
/************** Flower Navigation **********************************************/
/** Makes a flower pattern.
CenterWP is the center of the flower. The Navigation Height is taken from this waypoint.
EdgeWP defines the radius of the flower (distance from CenterWP to EdgeWP)
*/
enum FlowerStatus { Outside, FlowerLine, Circle };
static enum FlowerStatus CFlowerStatus;
static float CircleX;
static float CircleY;
static float Fly2X;
static float Fly2Y;
static float FlyFromX;
static float FlyFromY;
static float TransCurrentX;
static float TransCurrentY;
static float EdgeCurrentX;
static float EdgeCurrentY;
static float DistanceFromCenter;
static float FlowerTheta;
static float Flowerradius;
static uint8_t Center;
static uint8_t Edge;
bool_t flower_start(uint8_t CenterWP, uint8_t EdgeWP)
{
Center = CenterWP;
Edge = EdgeWP;
EdgeCurrentX = WaypointX(Edge) - WaypointX(Center);
EdgeCurrentY = WaypointY(Edge) - WaypointY(Center);
Flowerradius = sqrtf(EdgeCurrentX*EdgeCurrentX+EdgeCurrentY*EdgeCurrentY);
TransCurrentX = stateGetPositionEnu_f()->x - WaypointX(Center);
TransCurrentY = stateGetPositionEnu_f()->y - WaypointY(Center);
DistanceFromCenter = sqrtf(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY);
FlowerTheta = atan2f(TransCurrentY,TransCurrentX);
Fly2X = Flowerradius*cosf(FlowerTheta+3.14)+WaypointX(Center);
Fly2Y = Flowerradius*sinf(FlowerTheta+3.14)+WaypointY(Center);
FlyFromX = stateGetPositionEnu_f()->x;
FlyFromY = stateGetPositionEnu_f()->y;
if(DistanceFromCenter > Flowerradius)
CFlowerStatus = Outside;
else
CFlowerStatus = FlowerLine;
CircleX = 0;
CircleY = 0;
return FALSE;
}
bool_t flower_run(void)
{
TransCurrentX = stateGetPositionEnu_f()->x - WaypointX(Center);
TransCurrentY = stateGetPositionEnu_f()->y - WaypointY(Center);
DistanceFromCenter = sqrtf(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY);
bool_t InCircle = TRUE;
float CircleTheta;
if(DistanceFromCenter > Flowerradius)
InCircle = FALSE;
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(waypoints[Center].a, 0.);
switch(CFlowerStatus)
{
case Outside:
nav_route_xy(FlyFromX,FlyFromY,Fly2X,Fly2Y);
if(InCircle)
{
CFlowerStatus = FlowerLine;
FlowerTheta = atan2f(TransCurrentY,TransCurrentX);
Fly2X = Flowerradius*cosf(FlowerTheta+3.14)+WaypointX(Center);
Fly2Y = Flowerradius*sinf(FlowerTheta+3.14)+WaypointY(Center);
FlyFromX = stateGetPositionEnu_f()->x;
FlyFromY = stateGetPositionEnu_f()->y;
nav_init_stage();
}
break;
case FlowerLine:
nav_route_xy(FlyFromX,FlyFromY,Fly2X,Fly2Y);
if(!InCircle)
{
CFlowerStatus = Circle;
CircleTheta = nav_radius/Flowerradius;
CircleX = Flowerradius*cosf(FlowerTheta+3.14-CircleTheta)+WaypointX(Center);
CircleY = Flowerradius*sinf(FlowerTheta+3.14-CircleTheta)+WaypointY(Center);
nav_init_stage();
}
break;
case Circle:
nav_circle_XY(CircleX, CircleY, nav_radius);
if(InCircle)
{
EdgeCurrentX = WaypointX(Edge) - WaypointX(Center);
EdgeCurrentY = WaypointY(Edge) - WaypointY(Center);
Flowerradius = sqrtf(EdgeCurrentX*EdgeCurrentX+EdgeCurrentY*EdgeCurrentY);
if(DistanceFromCenter > Flowerradius)
CFlowerStatus = Outside;
else
CFlowerStatus = FlowerLine;
FlowerTheta = atan2f(TransCurrentY,TransCurrentX);
Fly2X = Flowerradius*cosf(FlowerTheta+3.14)+WaypointX(Center);
Fly2Y = Flowerradius*sinf(FlowerTheta+3.14)+WaypointY(Center);
FlyFromX = stateGetPositionEnu_f()->x;
FlyFromY = stateGetPositionEnu_f()->y;
nav_init_stage();
}
break;
default:
break;
}
return TRUE;
}
sw/airborne/modules/nav/nav_flower.h
+35
View File
@@ -0,0 +1,35 @@
/*
* Copyright (C) 2008-2013 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_flower.h
*
*/
#ifndef NAV_FLOWER_H
#define NAV_FLOWER_H
#include "std.h"
extern bool_t flower_run(void);
extern bool_t flower_start(uint8_t CenterWP, uint8_t EdgeWP);
#endif
sw/airborne/modules/nav/nav_line_osam.c
+203
View File
@@ -0,0 +1,203 @@
/*
* Copyright (C) 2008-2012 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_osam.c
*
* Flight line from OSAM advanced navigation routines
*
* TODO compare with normal flight line
*/
#include "modules/nav/nav_line_osam.h"
#include "subsystems/nav.h"
#include "state.h"
#include "autopilot.h"
#include "generated/flight_plan.h"
struct Point2D {float x; float y;};
enum FLStatus { FLInitialize, FLCircleS, FLLine, FLFinished };
static enum FLStatus CFLStatus = FLInitialize;
static struct Point2D FLCircle;
static struct Point2D FLFROMWP;
static struct Point2D FLTOWP;
static float FLQDR;
static float FLRadius;
/*
Translates point so (transX, transY) are (0,0) then rotates the point around z by Zrot
*/
void TranslateAndRotateFromWorld(struct Point2D *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);
}
bool_t osam_flight_line_run(uint8_t From_WP, uint8_t To_WP, float radius, float Space_Before, float Space_After)
{
struct Point2D V;
struct Point2D P;
float dv;
switch(CFLStatus)
{
case FLInitialize:
//Translate WPs so From_WP is origin
V.x = WaypointX(To_WP) - WaypointX(From_WP);
V.y = WaypointY(To_WP) - WaypointY(From_WP);
//Record Aircraft Position
P.x = stateGetPositionEnu_f()->x;
P.y = stateGetPositionEnu_f()->y;
//Rotate Aircraft Position so V is aligned with x axis
TranslateAndRotateFromWorld(&P, atan2f(V.y,V.x), WaypointX(From_WP), WaypointY(From_WP));
//Find which side of the flight line the aircraft is on
if(P.y > 0)
FLRadius = -radius;
else
FLRadius = radius;
//Find unit vector of V
dv = sqrtf(V.x*V.x+V.y*V.y);
V.x = V.x / dv;
V.y = V.y / dv;
//Find begin and end points of flight line
FLFROMWP.x = -V.x*Space_Before;
FLFROMWP.y = -V.y*Space_Before;
FLTOWP.x = V.x*(dv+Space_After);
FLTOWP.y = V.y*(dv+Space_After);
//Find center of circle
FLCircle.x = FLFROMWP.x + V.y * FLRadius;
FLCircle.y = FLFROMWP.y - V.x * FLRadius;
//Find the angle to exit the circle
FLQDR = atan2f(FLFROMWP.x-FLCircle.x, FLFROMWP.y-FLCircle.y);
//Translate back
FLFROMWP.x = FLFROMWP.x + WaypointX(From_WP);
FLFROMWP.y = FLFROMWP.y + WaypointY(From_WP);
FLTOWP.x = FLTOWP.x + WaypointX(From_WP);
FLTOWP.y = FLTOWP.y + WaypointY(From_WP);
FLCircle.x = FLCircle.x + WaypointX(From_WP);
FLCircle.y = FLCircle.y + WaypointY(From_WP);
CFLStatus = FLCircleS;
nav_init_stage();
break;
case FLCircleS:
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(waypoints[From_WP].a, 0);
nav_circle_XY(FLCircle.x, FLCircle.y, FLRadius);
if(NavCircleCount() > 0.2 && NavQdrCloseTo(DegOfRad(FLQDR)))
{
CFLStatus = FLLine;
LINE_START_FUNCTION;
nav_init_stage();
}
break;
case FLLine:
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(waypoints[From_WP].a, 0);
nav_route_xy(FLFROMWP.x,FLFROMWP.y,FLTOWP.x,FLTOWP.y);
if(nav_approaching_xy(FLTOWP.x,FLTOWP.y,FLFROMWP.x,FLFROMWP.y, 0))
{
CFLStatus = FLFinished;
LINE_STOP_FUNCTION;
nav_init_stage();
}
break;
case FLFinished:
CFLStatus = FLInitialize;
nav_init_stage();
return FALSE;
break;
default:
break;
}
return TRUE;
}
static uint8_t FLBlockCount = 0;
bool_t osam_flight_line_block_run(uint8_t First_WP, uint8_t Last_WP, float radius, float Space_Before, float Space_After)
{
if(First_WP < Last_WP)
{
osam_flight_line_run(First_WP+FLBlockCount, First_WP+FLBlockCount+1, radius, Space_Before, Space_After);
if(CFLStatus == FLInitialize)
{
FLBlockCount++;
if(First_WP+FLBlockCount >= Last_WP)
{
FLBlockCount = 0;
return FALSE;
}
}
}
else
{
osam_flight_line_run(First_WP-FLBlockCount, First_WP-FLBlockCount-1, radius, Space_Before, Space_After);
if(CFLStatus == FLInitialize)
{
FLBlockCount++;
if(First_WP-FLBlockCount <= Last_WP)
{
FLBlockCount = 0;
return FALSE;
}
}
}
return TRUE;
}
sw/airborne/modules/nav/nav_line_osam.h
+35
View File
@@ -0,0 +1,35 @@
/*
* Copyright (C) 2008-2013 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_line_osam.h
*
*/
#ifndef NAV_LINE_OSAM_H
#define NAV_LINE_OSAM_H
#include "std.h"
extern bool_t osam_flight_line_run(uint8_t From_WP, uint8_t To_WP, float radius, float Space_Before, float Space_After);
extern bool_t osam_flight_line_block_run(uint8_t First_WP, uint8_t Last_WP, float radius, float Space_Before, float Space_After);
#endif
sw/airborne/modules/nav/nav_osam.c
-1220
View File
File diff suppressed because it is too large Load Diff
sw/airborne/modules/nav/nav_osam.h
-68
View File
@@ -1,68 +0,0 @@
/*
* Copyright (C) 2008-2012 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_osam.h
*
*/
#ifndef OSAMNav_H
#define OSAMNav_H
#include "std.h"
struct Point2D {float x; float y;};
struct Line {float m;float b;float x;};
extern bool_t flower_run(void);
extern bool_t flower_start(uint8_t CenterWP, uint8_t EdgeWP);
extern bool_t bungee_takeoff_start(uint8_t BungeeWP);
extern bool_t bungee_takeoff_run(void);
extern bool_t skid_landing_start(uint8_t AFWP, uint8_t TDWP, float radius);
extern bool_t skid_landing_run(void);
#define PolygonSize 10
#define MaxFloat 1000000000
#define MinFloat -1000000000
extern bool_t osam_poly_survey_start(uint8_t FirstWP, uint8_t Size, float Sweep, float Orientation);
extern bool_t osam_poly_survey_run(void);
extern uint16_t PolySurveySweepNum;
extern uint16_t PolySurveySweepBackNum;
extern bool_t vertical_raster_start( void );
extern bool_t vertical_raster_run(uint8_t wp1, uint8_t wp2, float radius, float AltSweep);
extern bool_t osam_flight_line_run(uint8_t From_WP, uint8_t To_WP, float radius, float Space_Before, float Space_After);
extern bool_t osam_flight_line_block_run(uint8_t First_WP, uint8_t Last_WP, float radius, float Space_Before, float Space_After);
void TranslateAndRotateFromWorld(struct Point2D *p, float Zrot, float transX, float transY);
void RotateAndTranslateToWorld(struct Point2D *p, float Zrot, float transX, float transY);
void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2);
float EvaluateLineForY(float x, struct Line L);
float EvaluateLineForX(float y, struct Line L);
float DistanceEquation(struct Point2D p1,struct Point2D p2);
#endif
sw/airborne/modules/nav/nav_poly_survey_osam.c
+513
View File
File diff suppressed because it is too large Load Diff
sw/airborne/modules/nav/nav_poly_survey_osam.h
+37
View File
@@ -0,0 +1,37 @@
/*
* Copyright (C) 2008-2013 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_poly_survey_osam.h
*
*/
#ifndef NAV_POLY_SURVEY_OSAM_H
#define NAV_POLY_SURVEY_OSAM_H
#include "std.h"
extern bool_t osam_poly_survey_start(uint8_t FirstWP, uint8_t Size, float Sweep, float Orientation);
extern bool_t osam_poly_survey_run(void);
extern uint16_t PolySurveySweepNum;
extern uint16_t PolySurveySweepBackNum;
#endif
sw/airborne/modules/nav/nav_vertical_raster.c
+152
View File
@@ -0,0 +1,152 @@
/*
* Copyright (C) 2008-2013 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_vertical_raster.c
*
* from OSAM advanced navigation routines
*/
#include "modules/nav/nav_vertical_raster.h"
#include "subsystems/nav.h"
#include "state.h"
#include "autopilot.h"
#include "generated/flight_plan.h"
/************** Vertical Raster **********************************************/
/** Copy of nav line. The only difference is it changes altitude every sweep, but doesn't come out of circle until
it reaches altitude.
*/
enum line_status { LR12, LQC21, LTC2, LQC22, LR21, LQC12, LTC1, LQC11 };
static enum line_status line_status;
bool_t vertical_raster_start( void ) {
line_status = LR12;
return FALSE;
}
bool_t vertical_raster_run(uint8_t l1, uint8_t l2, float radius, float AltSweep) {
radius = fabs(radius);
float alt = waypoints[l1].a;
waypoints[l2].a = alt;
float l2_l1_x = WaypointX(l1) - WaypointX(l2);
float l2_l1_y = WaypointY(l1) - WaypointY(l2);
float d = sqrt(l2_l1_x*l2_l1_x+l2_l1_y*l2_l1_y);
/* Unit vector from l1 to l2 */
float u_x = l2_l1_x / d;
float u_y = l2_l1_y / d;
/* The half circle centers and the other leg */
struct point l2_c1 = { WaypointX(l1) + radius * u_y,
WaypointY(l1) + radius * -u_x,
alt };
struct point l2_c2 = { WaypointX(l1) + 1.732*radius * u_x,
WaypointY(l1) + 1.732*radius * u_y,
alt };
struct point l2_c3 = { WaypointX(l1) + radius * -u_y,
WaypointY(l1) + radius * u_x,
alt };
struct point l1_c1 = { WaypointX(l2) + radius * -u_y,
WaypointY(l2) + radius * u_x,
alt };
struct point l1_c2 = { WaypointX(l2) +1.732*radius * -u_x,
WaypointY(l2) + 1.732*radius * -u_y,
alt };
struct point l1_c3 = { WaypointX(l2) + radius * u_y,
WaypointY(l2) + radius * -u_x,
alt };
float qdr_out_2_1 = M_PI/3. - atan2(u_y, u_x);
float qdr_out_2_2 = -M_PI/3. - atan2(u_y, u_x);
float qdr_out_2_3 = M_PI - atan2(u_y, u_x);
/* Vertical target */
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(WaypointAlt(l1), 0.);
switch (line_status) {
case LR12: /* From wp l2 to wp l1 */
NavSegment(l2, l1);
if (NavApproachingFrom(l1, l2, CARROT)) {
line_status = LQC21;
waypoints[l1].a = waypoints[l1].a+AltSweep;
nav_init_stage();
}
break;
case LQC21:
nav_circle_XY(l2_c1.x, l2_c1.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_1)-10)) {
line_status = LTC2;
nav_init_stage();
}
break;
case LTC2:
nav_circle_XY(l2_c2.x, l2_c2.y, -radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)+10) && stateGetPositionEnu_f()->z >= (waypoints[l1].a-10)) {
line_status = LQC22;
nav_init_stage();
}
break;
case LQC22:
nav_circle_XY(l2_c3.x, l2_c3.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_3)-10)) {
line_status = LR21;
nav_init_stage();
}
break;
case LR21: /* From wp l1 to wp l2 */
NavSegment(l1, l2);
if (NavApproachingFrom(l2, l1, CARROT)) {
line_status = LQC12;
waypoints[l1].a = waypoints[l1].a+AltSweep;
nav_init_stage();
}
break;
case LQC12:
nav_circle_XY(l1_c1.x, l1_c1.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_1 + M_PI)-10)) {
line_status = LTC1;
nav_init_stage();
}
break;
case LTC1:
nav_circle_XY(l1_c2.x, l1_c2.y, -radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_2 + M_PI)+10) && stateGetPositionEnu_f()->z >= (waypoints[l1].a-5)) {
line_status = LQC11;
nav_init_stage();
}
break;
case LQC11:
nav_circle_XY(l1_c3.x, l1_c3.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_3 + M_PI)-10)) {
line_status = LR12;
nav_init_stage();
}
default:
break;
}
return TRUE; /* This pattern never ends */
}
sw/airborne/modules/nav/nav_vertical_raster.h
+35
View File
@@ -0,0 +1,35 @@
/*
* Copyright (C) 2008-2013 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_vertical_raster.h
*
*/
#ifndef NAV_VERTICAL_RASTER_H
#define NAV_VERTICAL_RASTER_H
#include "std.h"
extern bool_t vertical_raster_start( void );
extern bool_t vertical_raster_run(uint8_t wp1, uint8_t wp2, float radius, float AltSweep);
#endif