Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 12:23:23 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

[fixewing][modules] convert nav_spiral to module

Browse Source
This commit is contained in:
Felix Ruess
2013-08-21 19:52:38 +02:00
parent e5d0a102d7
commit cff3bb7d45
8 changed files with 250 additions and 237 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/examples/Twinstar_energyadaptive.xml
-1
View File
@@ -164,7 +164,6 @@ twog_1.0 + aspirin + ETS baro + ETS speed
<define name="NO_XBEE_API_INIT" value="TRUE"/>
<define name="TRIGGER_DELAY" value="1."/>
<define name="DEFAULT_CIRCLE_RADIUS" value="70."/>
<define name="MIN_CIRCLE_RADIUS" value="35.0"/>
</section>
<section name="GLS_APPROACH" prefix="APP_">
conf/airframes/flixr_discovery.xml
-3
View File
@@ -227,9 +227,6 @@
<define name="DEFAULT_CIRCLE_RADIUS" value="50.0"/>
<!-- only for spiral -->
<define name="MIN_CIRCLE_RADIUS" value="35.0"/>
<!-- The Glide definitions are used for calculating the touch down point during auto landing -->
<!--
-->
conf/firmwares/subsystems/fixedwing/navigation_extra.makefile
-1
View File
@@ -14,7 +14,6 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/nav_survey_rectangle.c $(SRC_SUBS
$(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/spiral.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/poly_survey_adv.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/gls.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/border_line.c
conf/modules/nav_spiral.xml
+20
View File
@@ -0,0 +1,20 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="nav_spiral" dir="nav">
<doc>
<description>
Fixedwing navigation in a spiral/helix.
creating a helix:
- start radius to end radius, increasing after reaching alphamax
- Alphamax is calculated from given segments
- IMPORTANT: numer of segments has to be larger than 2!
</description>
<define name="NAV_SPIRAL_MIN_CIRCLE_RADIUS" value="120" description="minium circle radius in meters"/>
</doc>
<header>
<file name="nav_spiral.h"/>
</header>
<makefile>
<file name="nav_spiral.c"/>
</makefile>
</module>
sw/airborne/modules/nav/nav_spiral.c
+172
View File
@@ -0,0 +1,172 @@
/*
* Copyright (C) 2011-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_spiral.c
*
* Fixedwing navigation in a spiral/helix.
*
* creating a helix:
* - start radius to end radius, increasing after reaching alphamax
* - Alphamax is calculated from given segments
* - IMPORTANT: numer of segments has to be larger than 2!
*/
#include "modules/nav/nav_spiral.h"
#include "subsystems/nav.h"
#include "state.h"
#include "autopilot.h"
#include "generated/flight_plan.h"
#ifdef DIGITAL_CAM
#include "modules/digital_cam/dc.h"
#endif
#ifndef NAV_SPIRAL_MIN_CIRCLE_RADIUS
#define NAV_SPIRAL_MIN_CIRCLE_RADIUS 60
#endif
struct NavSpiral nav_spiral;
bool_t nav_spiral_start(uint8_t center_wp, uint8_t edge_wp, float radius_start, float radius_inc, float segments)
{
VECT2_COPY(nav_spiral.center, waypoints[center_wp]); // center of the helix
nav_spiral.center.z = waypoints[center_wp].a;
nav_spiral.radius_start = radius_start; // start radius of the helix
nav_spiral.segments = segments;
nav_spiral.radius_min = NAV_SPIRAL_MIN_CIRCLE_RADIUS;
if (nav_spiral.radius_start < nav_spiral.radius_min)
nav_spiral.radius_start = nav_spiral.radius_min;
nav_spiral.radius_increment = radius_inc; // multiplier for increasing the spiral
struct FloatVect2 edge;
VECT2_DIFF(edge, waypoints[edge_wp], nav_spiral.center);
FLOAT_VECT2_NORM(nav_spiral.radius, edge);
// get a copy of the current position
struct EnuCoor_f pos_enu;
memcpy(&pos_enu, stateGetPositionEnu_f(), sizeof(struct EnuCoor_f));
VECT3_DIFF(nav_spiral.trans_current, pos_enu, nav_spiral.center);
nav_spiral.dist_from_center = FLOAT_VECT3_NORM(nav_spiral.trans_current);
// nav_spiral.alpha_limit denotes angle, where the radius will be increased
nav_spiral.alpha_limit = 2*M_PI / nav_spiral.segments;
//current position
nav_spiral.fly_from.x = stateGetPositionEnu_f()->x;
nav_spiral.fly_from.y = stateGetPositionEnu_f()->y;
if(nav_spiral.dist_from_center > nav_spiral.radius)
nav_spiral.status = Outside;
return FALSE;
}
bool_t nav_spiral_run(void)
{
struct EnuCoor_f pos_enu;
memcpy(&pos_enu, stateGetPositionEnu_f(), sizeof(struct EnuCoor_f));
VECT2_DIFF(nav_spiral.trans_current, pos_enu, nav_spiral.center);
nav_spiral.dist_from_center = FLOAT_VECT3_NORM(nav_spiral.trans_current);
float DistanceStartEstim;
float CircleAlpha;
switch(nav_spiral.status)
{
case Outside:
//flys until center of the helix is reached an start helix
nav_route_xy(nav_spiral.fly_from.x, nav_spiral.fly_from.y, nav_spiral.center.x, nav_spiral.center.y);
// center reached?
if (nav_approaching_xy(nav_spiral.center.x, nav_spiral.center.y, nav_spiral.fly_from.x, nav_spiral.fly_from.y, 0)) {
// nadir image
#ifdef DIGITAL_CAM
dc_send_command(DC_SHOOT);
#endif
nav_spiral.status = StartCircle;
}
break;
case StartCircle:
// Starts helix
// storage of current coordinates
// calculation needed, State switch to Circle
nav_circle_XY(nav_spiral.center.y, nav_spiral.center.y, nav_spiral.radius_start);
if(nav_spiral.dist_from_center >= nav_spiral.radius_start){
VECT2_COPY(nav_spiral.last_circle, pos_enu);
nav_spiral.status = Circle;
// Start helix
#ifdef DIGITAL_CAM
dc_Circle(360/nav_spiral.segments);
#endif
}
break;
case Circle: {
nav_circle_XY(nav_spiral.center.x, nav_spiral.center.y, nav_spiral.radius_start);
// Trigonometrische Berechnung des bereits geflogenen Winkels alpha
// equation:
// alpha = 2 * asin ( |Starting position angular - current positon| / (2* nav_spiral.radius_start)
// if alphamax already reached, increase radius.
//DistanceStartEstim = |Starting position angular - current positon|
struct FloatVect2 pos_diff;
VECT2_DIFF(pos_diff, nav_spiral.last_circle, pos_enu);
FLOAT_VECT2_NORM(DistanceStartEstim, pos_diff);
CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * nav_spiral.radius_start)));
if (CircleAlpha >= nav_spiral.alpha_limit) {
VECT2_COPY(nav_spiral.last_circle, pos_enu);
nav_spiral.status = IncSpiral;
}
break;
}
case IncSpiral:
// increasing circle radius as long as it is smaller than max helix radius
if(nav_spiral.radius_start + nav_spiral.radius_increment < nav_spiral.radius)
{
nav_spiral.radius_start = nav_spiral.radius_start + nav_spiral.radius_increment;
#ifdef DIGITAL_CAM
if (dc_cam_tracing) {
// calculating Cam angle for camera alignment
nav_spiral.trans_current.z = pos_enu.z - nav_spiral.center.z;
dc_cam_angle = atan(nav_spiral.radius_start/nav_spiral.trans_current.z) * 180 / M_PI;
}
#endif
}
else {
nav_spiral.radius_start = nav_spiral.radius;
#ifdef DIGITAL_CAM
// Stopps DC
dc_stop();
#endif
}
nav_spiral.status = Circle;
break;
default:
break;
}
NavVerticalAutoThrottleMode(0.); /* No pitch */
NavVerticalAltitudeMode(nav_spiral.center.z, 0.); /* No preclimb */
return TRUE;
}
sw/airborne/modules/nav/nav_spiral.h
+58
View File
@@ -0,0 +1,58 @@
/*
* Copyright (C) 2011-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_spiral.h
*
* Fixedwing navigation in a spiral/helix.
*
*/
#ifndef NAV_SPIRAL_H
#define NAV_SPIRAL_H
#include "std.h"
#include "math/pprz_algebra_float.h"
enum SpiralStatus { Outside, StartCircle, Circle, IncSpiral };
struct NavSpiral {
struct FloatVect3 trans_current;
struct FloatVect2 fly_from;
struct FloatVect2 last_circle;
struct FloatVect3 center;
float dist_from_center;
float alpha_limit;
float segments;
float radius;
float radius_min;
float radius_start;
float radius_increment;
enum SpiralStatus status;
};
extern struct NavSpiral nav_spiral;
extern bool_t nav_spiral_run(void);
extern bool_t nav_spiral_start(uint8_t center_wp, uint8_t edge_wp, float radius_start,
float radius_inc, float segments);
#endif // NAV_SPIRAL_H
sw/airborne/subsystems/navigation/spiral.c
-193
View File
@@ -1,193 +0,0 @@
/*
* Copyright (C) 2011 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 subsystems/navigation/spiral.c
*
* Fixedwing navigation in a spiral/helix from Uni Stuttgart.
*
* creating a helix:
* - start radius to end radius, increasing after reaching alphamax
* - Alphamax is calculated from given segments
* - IMPORTANT: numer of segments has to be larger than 2!
*/
#include "subsystems/navigation/spiral.h"
#include "subsystems/nav.h"
#include "state.h"
#include "autopilot.h"
#include "generated/flight_plan.h"
#ifdef DIGITAL_CAM
#include "modules/digital_cam/dc.h"
#endif
enum SpiralStatus { Outside, StartCircle, Circle, IncSpiral };
static enum SpiralStatus CSpiralStatus;
// static float SpiralTheta;
// static float Fly2X;
// static float Fly2Y;
static float FlyFromX;
static float FlyFromY;
static float TransCurrentX;
static float TransCurrentY;
static float TransCurrentZ;
static float EdgeCurrentX;
static float EdgeCurrentY;
static float LastCircleX;
static float LastCircleY;
static float DistanceFromCenter;
static float Spiralradius;
static uint8_t Center;
static uint8_t Edge;
static float SRad;
static float IRad;
static float Alphalimit;
static float Segmente;
static float ZPoint;
static float nav_radius_min;
#ifndef MIN_CIRCLE_RADIUS
#define MIN_CIRCLE_RADIUS 120
#endif
bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float IncRad, float Segments, float ZKoord)
{
Center = CenterWP; // center of the helix
Edge = EdgeWP; // edge point on the maximaum radius
SRad = StartRad; // start radius of the helix
Segmente = Segments;
ZPoint = ZKoord;
nav_radius_min = MIN_CIRCLE_RADIUS;
if (SRad < nav_radius_min) SRad = nav_radius_min;
IRad = IncRad; // multiplier for increasing the spiral
EdgeCurrentX = WaypointX(Edge) - WaypointX(Center);
EdgeCurrentY = WaypointY(Edge) - WaypointY(Center);
Spiralradius = sqrt(EdgeCurrentX*EdgeCurrentX+EdgeCurrentY*EdgeCurrentY);
TransCurrentX = stateGetPositionEnu_f()->x - WaypointX(Center);
TransCurrentY = stateGetPositionEnu_f()->y - WaypointY(Center);
TransCurrentZ = stateGetPositionEnu_f()->z - ZPoint;
DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY);
// SpiralTheta = atan2(TransCurrentY,TransCurrentX);
// Fly2X = Spiralradius*cos(SpiralTheta+M_PI)+WaypointX(Center);
// Fly2Y = Spiralradius*sin(SpiralTheta+M_PI)+WaypointY(Center);
// Alphalimit denotes angle, where the radius will be increased
Alphalimit = 2*M_PI / Segments;
//current position
FlyFromX = stateGetPositionEnu_f()->x;
FlyFromY = stateGetPositionEnu_f()->y;
if(DistanceFromCenter > Spiralradius)
CSpiralStatus = Outside;
return FALSE;
}
bool_t SpiralNav(void)
{
TransCurrentX = stateGetPositionEnu_f()->x - WaypointX(Center);
TransCurrentY = stateGetPositionEnu_f()->y - WaypointY(Center);
DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY);
float DistanceStartEstim;
float CircleAlpha;
switch(CSpiralStatus)
{
case Outside:
//flys until center of the helix is reached an start helix
nav_route_xy(FlyFromX,FlyFromY,WaypointX(Center), WaypointY(Center));
// center reached?
if (nav_approaching_xy(WaypointX(Center), WaypointY(Center), FlyFromX, FlyFromY, 0)) {
// nadir image
#ifdef DIGITAL_CAM
dc_send_command(DC_SHOOT);
#endif
CSpiralStatus = StartCircle;
}
break;
case StartCircle:
// Starts helix
// storage of current coordinates
// calculation needed, State switch to Circle
nav_circle_XY(WaypointX(Center), WaypointY(Center), SRad);
if(DistanceFromCenter >= SRad){
LastCircleX = stateGetPositionEnu_f()->x;
LastCircleY = stateGetPositionEnu_f()->y;
CSpiralStatus = Circle;
// Start helix
#ifdef DIGITAL_CAM
dc_Circle(360/Segmente);
#endif
}
break;
case Circle: {
nav_circle_XY(WaypointX(Center), WaypointY(Center), SRad);
// Trigonometrische Berechnung des bereits geflogenen Winkels alpha
// equation:
// alpha = 2 * asin ( |Starting position angular - current positon| / (2* SRad)
// if alphamax already reached, increase radius.
//DistanceStartEstim = |Starting position angular - current positon|
DistanceStartEstim = sqrt (((LastCircleX-stateGetPositionEnu_f()->x)*(LastCircleX-stateGetPositionEnu_f()->x))
+ ((LastCircleY-stateGetPositionEnu_f()->y)*(LastCircleY-stateGetPositionEnu_f()->y)));
CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * SRad)));
if (CircleAlpha >= Alphalimit) {
LastCircleX = stateGetPositionEnu_f()->x;
LastCircleY = stateGetPositionEnu_f()->y;
CSpiralStatus = IncSpiral;
}
break;
}
case IncSpiral:
// increasing circle radius as long as it is smaller than max helix radius
if(SRad + IRad < Spiralradius)
{
SRad = SRad + IRad;
#ifdef DIGITAL_CAM
if (dc_cam_tracing) {
// calculating Cam angle for camera alignment
TransCurrentZ = stateGetPositionEnu_f()->z - ZPoint;
dc_cam_angle = atan(SRad/TransCurrentZ) * 180 / M_PI;
}
#endif
}
else {
SRad = Spiralradius;
#ifdef DIGITAL_CAM
// Stopps DC
dc_stop();
#endif
}
CSpiralStatus = Circle;
break;
default:
break;
}
return TRUE;
}
sw/airborne/subsystems/navigation/spiral.h
-39
View File
@@ -1,39 +0,0 @@
/*
* Copyright (C) 2011 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 subsystems/navigation/spiral.h
*
* Fixedwing navigation in a spiral/helix from Uni Stuttgart.
*
*/
#ifndef SPIRAL_H
#define SPIRAL_H
#include "std.h"
extern bool_t SpiralNav(void);
extern bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float IncRad,
float Segments, float ZKoord );
#endif // SPIRAL_H