Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

[nav] implement survey for hybrids

Browse Source 
Survey based on the OSAM algo for rotorcraft. Instead of going from one
sweep line to the next with a line, a circle aligns the aircraft with
the next line.
Circles radius at the start of lines can be either automatic (sweep distance),
fixed or direct line (equivalent to original rotorcraft).

The code itself is reorganized to use a global structure instead of many
variables.
This commit is contained in:
Gautier Hattenberger
2023-04-04 20:00:40 +02:00
parent 07a43ca4c4
commit dbed41784e
3 changed files with 775 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/nav_survey_hybrid.xml
+51
View File
@@ -0,0 +1,51 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="nav_survey_hybrid" dir="nav" task="control">
<doc>
<description>
Polygon survey for hybrid aircraft.
Based on poly_osam algorithm.
Compatible with generic rotorcraft.
Support mission mode with custom elements:
- points in local NED: SRVHL orientation sweep_distance radius height p1x p1y p2x p2y p3x p3y [p4x p4y]
- points in global LLA: SRVHG orientation sweep_distance radius height p1lat p1lon p2lat p2lon p3lat p3lon [p4lat p4lon]
- orientation is in degrees
- sweep_distance in meters
- radius can be: negative, automatically set to sweep/2; zero, not turning on circles; positive, fixed radius
- height in meters above reference point
- the polygon in mission mode can have either 3 or 4 points.
</description>
<section name="SURVEY_HYBRID" prefix="SURVEY_HYBRID_">
<define name="MAX_POLYGON_SIZE" value="20" description="max waypoints usable in polygon survey"/>
<define name="HALF_SWEEP_ENABLED" value="TRUE|FALSE" description="interleave sweep lines when sweeping back"/>
<define name="APPROACHING_TIME" value="3." description="end of segment anticipation time"/>
<define name="MAX_SWEEP" value="0" description="max number of sweep lines (0 for unlimited)"/>
<define name="MAX_SWEEP_BACK" value="0" description="max number of sweep back, e.g. changing direction (0 for unlimited)"/>
<define name="ENTRY_DISTANCE" value="10." description="distance from entry point (default: half sweep distance)"/>
</section>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="Survey Hybrid">
<dl_setting min="0" max="1" step="1" var="survey_hybrid.half_sweep_enabled" type="uint8" shortname="half_sweep" values="FALSE|TRUE"/>
<dl_setting min="0" max="50" step="1" var="survey_hybrid.sweep_nb_max" type="uint16" shortname="nb_max"/>
<dl_setting min="0" max="50" step="1" var="survey_hybrid.sweep_back_nb_max" type="uint16" shortname="back_nb_max"/>
</dl_settings>
</dl_settings>
</settings>
<dep>
<depends>@navigation</depends>
<recommends>@mission,@digital_cam</recommends>
</dep>
<header>
<file name="nav_survey_hybrid.h"/>
</header>
<makefile target="ap|nps">
<file name="nav_survey_hybrid.c"/>
<test firmware="rotorcraft">
<define name="USE_MISSION"/>
</test>
</makefile>
</module>
sw/airborne/modules/nav/nav_survey_hybrid.c
+654
View File
File diff suppressed because it is too large Load Diff
sw/airborne/modules/nav/nav_survey_hybrid.h
+70
View File
@@ -0,0 +1,70 @@
/*
* Copyright (C) 2023 Gautier Hattenberger <gautier.hattenberger@enac.fr>
* Based on OSAM poly survey
*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/nav/nav_survey_hybrid.h
*
*/
#ifndef NAV_SURVEY_HYBRID_H
#define NAV_SURVEY_HYBRID_H
#include "std.h"
struct SurveyHybrid {
uint16_t sweep_nb_max;
uint16_t sweep_back_nb_max;
uint16_t sweep_nb;
uint16_t sweep_back_nb;
bool half_sweep_enabled;
};
extern struct SurveyHybrid survey_hybrid;
/** Init function
*/
extern void nav_survey_hybrid_init(void);
/**
* Setup polygon survey.
* @param start_wp first waypoint/corner of the polygon
* @param orientation angle of scan lines in degrees (CCW, east)
* @param size number of waypoints/corners used to define the polygon
* @param sweep distance between scan lines
* @param radius turn radius (<0: automatic, radius = sweep/2; 0: no turns, use straight lines only; >0: fixed radius)
*/
extern void nav_survey_hybrid_setup_orientation(uint8_t start_wp, float orientation, uint8_t size, float sweep, float radius);
/**
* Setup "dynamic" polygon survey with sweep orientation towards a waypoint.
* Computes the sweep orientation angle from the line first-second WP.
* @param start_wp first waypoint/corner of the polygon
* @param second_wp second waypoint towards which the sweep orientation is computed
* @param size number of waypoints/corners used to define the polygon
* @param sweep distance between scan lines, if zero uses Poly_Distance
* @param radius turn radius (<0: automatic, radius = sweep/2; 0: no turns, use straight lines only; >0: fixed radius)
*/
extern void nav_survey_hybrid_setup_towards(uint8_t start_wp, uint8_t second_wp, uint8_t size, float sweep, float radius);
/** Run polygon hybrid survey */
extern bool nav_survey_hybrid_run(void);
#endif