Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-02-05 10:41:00 +08:00
Code Issues Actions 2 Packages Projects Releases Wiki Activity

[nav] takeoff and land module (#3010)

Browse Source 
* [nav] takeoff and land module

Functions for takeoff and landing (fixedwing and rotorcraft), can be
called from the flight plan or from the mission module.
Two flight plan examples are provided.
Various options are possible, see module description and header file.

* [sim] ins_sim module is only doing INS, not AHRS

add AHRS requirement for sim target, if needed provided by ahrs_sim
This commit is contained in:
Gautier Hattenberger
2023-03-16 21:45:21 +01:00
committed by GitHub
parent 02df6da2d7
commit 1d7b6dff57
15 changed files with 1158 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

65
conf/flight_plans/fixedwing_demo_takeoff_land.xml Normal file
View File

@@ -0,0 +1,65 @@
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
<flight_plan alt="260" ground_alt="185" lat0="43.46223" lon0="1.27289" max_dist_from_home="1500" name="Fixedwing demo takeoff and land" security_height="25">
<header/>
<waypoints>
<waypoint name="HOME" x="0" y="0"/>
<waypoint name="STDBY" x="49.5" y="100.1"/>
<waypoint alt="215.0" name="AF" x="177.4" y="45.1"/>
<waypoint alt="185.0" name="TD" x="28.8" y="57.0"/>
<waypoint name="CLIMB" x="-114.5" y="162.3"/>
</waypoints>
<variables>
<variable var="land_dir" init="45." min="0." max="360." step="0.1"/>
<variable var="land_distance" init="200." min="0." max="300." step="0.1"/>
</variables>
<modules>
<module name="nav" type="takeoff_and_landing"/>
</modules>
<blocks>
<block name="Wait GPS">
<set value="1" var="autopilot.kill_throttle"/>
<while cond="!GpsFixValid()"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 1)"/>
<call_once fun="NavSetGroundReferenceHere()"/>
</block>
<block name="Holding point">
<call_once fun="autopilot_set_kill_throttle(true)"/>
<attitude roll="0" throttle="0" vmode="throttle"/>
</block>
<block name="Takeoff (WP)" strip_button="Takeoff (WP)" group="takeoff">
<set value="0" var="autopilot.flight_time"/>
<call fun="nav_takeoff_from_wp(WP_CLIMB)"/>
<deroute block="Standby"/>
</block>
<block name="Takeoff (Here)" strip_button="Takeoff (Here)" group="takeoff">
<set value="0" var="autopilot.flight_time"/>
<call fun="nav_takeoff_from_here()"/>
<deroute block="Standby"/>
</block>
<block key="Ctrl+a" name="Standby" strip_button="Standby" strip_icon="home.png" group="home">
<circle radius="nav_radius" wp="STDBY"/>
</block>
<block name="Land (WP)" strip_button="Land (WP)" group="land">
<call fun="nav_land_at_wp(WP_TD, WP_TD, nav_radius)"/>
<deroute block="Landed"/>
</block>
<block name="Land (WP-glide)" strip_button="Land (WP-glide)" group="land">
<call fun="nav_land_at_wp(WP_TD, WP_AF, nav_radius)"/>
<deroute block="Landed"/>
</block>
<block name="Land (Here)" strip_button="Land (Here)" group="land">
<call fun="nav_land_here(0., nav_radius)"/>
<deroute block="Landed"/>
</block>
<block name="Land (Loc)" strip_button="Land (Loc)" group="land">
<call fun="nav_land_at_loc(0., 43.4625, 1.27329, land_dir, land_distance, nav_radius)"/>
<deroute block="Landed"/>
</block>
<block name="Landed">
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
</block>
</blocks>
</flight_plan>

64
conf/flight_plans/rotorcraft_demo_takeoff_land.xml Normal file
View File

@@ -0,0 +1,64 @@
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
<flight_plan alt="157" ground_alt="147" lat0="43 33 50.83" lon0="1 28 52.61" max_dist_from_home="150" name="Rotorcraft demo takeoff and land" security_height="1">
<header/>
<waypoints>
<waypoint name="HOME" x="0.0" y="0.0"/>
<waypoint name="CLIMB" x="0.0" y="5.0"/>
<waypoint name="STDBY" x="-2.0" y="-5.0"/>
<waypoint name="P1" x="3.6" y="-13.9"/>
<waypoint name="TD" x="5.6" y="-10.9" height="2."/>
</waypoints>
<variables>
<variable var="land_dir" init="45." min="0." max="360." step="0.1"/>
<variable var="land_distance" init="10." min="0." max="20." step="0.1"/>
</variables>
<modules>
<module name="nav" type="takeoff_and_landing"/>
</modules>
<blocks>
<block name="Wait GPS">
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid()"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 1)"/>
<call_once fun="NavSetGroundReferenceHere()"/>
<!--<call_once fun="NavSetAltitudeReferenceHere()"/>-->
</block>
<block name="Holding point">
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
</block>
<block name="Takeoff (WP)" strip_button="Takeoff (WP)" group="takeoff">
<call fun="nav_takeoff_from_wp(WP_CLIMB)"/>
<deroute block="Standby"/>
</block>
<block name="Takeoff (Here)" strip_button="Takeoff (Here)" group="takeoff">
<call fun="nav_takeoff_from_here()"/>
<deroute block="Standby"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<stay wp="STDBY"/>
</block>
<block name="Land (WP)" strip_button="Land (WP)" group="land">
<call fun="nav_land_at_wp(WP_TD, WP_TD, 0.)"/>
<deroute block="Landed"/>
</block>
<block name="Land (WP-glide)" strip_button="Land (WP-glide)" group="land">
<call fun="nav_land_at_wp(WP_TD, WP_STDBY, 0.)"/>
<deroute block="Landed"/>
</block>
<block name="Land (Here)" strip_button="Land (Here)" group="land">
<call fun="nav_land_here(0., 0.)"/>
<deroute block="Landed"/>
</block>
<block name="Land (Loc)" strip_button="Land (Loc)" group="land">
<call fun="nav_land_at_loc(0., waypoint_get_lat_deg(WP_P1), waypoint_get_lon_deg(WP_P1), land_dir, land_distance, 0.)"/>
<deroute block="Landed"/>
</block>
<block name="Landed">
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
</block>
</blocks>
</flight_plan>

9
conf/modules/ins_sim.xml
View File

@@ -3,23 +3,18 @@
<module name="ins_sim" dir="ins" task="estimation">
<doc>
<description>
Simulated AHRS and INS.
Simulated INS.
</description>
</doc>
<dep>
<depends>@imu,@gps</depends>
<provides>ins,ahrs</provides>
<provides>ins</provides>
</dep>
<header>
<file name="ins_gps_passthrough.h"/>
</header>
<init fun="ins_gps_passthrough_init()"/>
<makefile target="sim">
<file name="ahrs.c" dir="modules/ahrs"/>
<file name="ahrs_sim.c" dir="modules/ahrs"/>
<define name="USE_AHRS"/>
<define name="AHRS_TYPE_H" value="modules/ahrs/ahrs_sim.h" type="string"/>
<file name="ins.c"/>
<file name="ins_gps_passthrough_utm.c"/>
<define name="INS_TYPE_H" value="modules/ins/ins_gps_passthrough.h" type="string"/>

48
conf/modules/nav_takeoff_and_landing.xml Normal file
View File

@@ -0,0 +1,48 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="nav_takeoff_and_landing" dir="nav" task="control">
<doc>
<description>
Navigation routines for takeoff and landing
Basic procedures for rotorcraft and fixed-wing:
- Takeoff rotorcraft: from current location, can move a WP to takeoff point or not
- Takeoff fixedwing: in direction of a WP, flight plan QFU, or specified location
- Landing rotorcraft: land at current location, to a specified WP or location, with a glide or not
- Landing fixedwing: land with a glide between two WPs or from a direction/distance, or land with a circle
Functions can be called from flight plan or from the mission module if loaded.
</description>
<section name="NAV_TAKEOFF" prefix="NAV_TAKEOFF_">
<define name="CLIMB_SPEED" value="1.0" description="Takeoff climb speed (rotorcraft, default: NAV_CLIMB_VSPEED)"/>
<define name="PITCH" value="15." unit="deg" description="Takeoff pitch angle (fixedwing)"/>
<define name="THROTTLE" value="1.0" unit="[0-1]" description="Takeoff throttle (fixedwing, normalized [0-1])"/>
<define name="HEIGHT" value="2./20." unit="m" description="Takeoff end height (default: rotorcraft=2., fixedwing=20."/>
<define name="DIST" value="200." unit="m" description="Takeoff end distance (fixedwing)"/>
<define name="AUTO_LAUNCH" value="FALSE|TRUE" description="Set 'launch' setting automatically (fixedwing, default: TRUE)"/>
</section>
<section name="NAV_LANDING" prefix="NAV_LANDING_">
<define name="DESCEND_SPEED" value="-1." description="Landing descent speed (default: rotorcraft=NAV_DESCEND_VSPEED, fixedwing=-1.0)"/>
<define name="AF_HEIGHT" value="5./30." description="Landing start of descent height (default: rotorcraft=5., fixedwing=30.)"/>
<define name="FLARE_HEIGHT" value="2./10." description="Landing flare height (default: rotorcraft=2., fixedwing=10.)"/>
</section>
</doc>
<dep>
<depends>@navigation</depends>
<recommends>@mission</recommends>
</dep>
<header>
<file name="nav_takeoff_and_landing.h"/>
</header>
<init fun="nav_takeoff_and_landing_init()"/>
<periodic fun="nav_takeoff_and_landing_periodic()" freq="1."/>
<makefile firmware="fixedwing">
<file name="nav_takeoff_and_landing_fw.c"/>
<test firmware="fixedwing">
<define name="USE_MISSION"/>
</test>
</makefile>
<makefile firmware="rotorcraft">
<file name="nav_takeoff_and_landing_rotorcraft.c"/>
<test firmware="rotorcraft">
<define name="USE_MISSION"/>
</test>
</makefile>
</module>

2
conf/modules/targets/sim.xml
View File

@@ -11,7 +11,7 @@
</description>
</doc>
<dep>
<depends>system_core,electrical,settings,actuators_dummy,@gps,@ins</depends>
<depends>system_core,electrical,settings,actuators_dummy,@gps,@ins,@ahrs</depends>
<suggests>gps_sim,imu_sim,ins_sim,ahrs_sim,baro_sim,telemetry_sim</suggests>
</dep>
<makefile target="sim">

8
sw/airborne/firmwares/fixedwing/nav.c
View File

@@ -157,14 +157,16 @@ void nav_circle_XY(float x, float y, float radius)
void nav_glide(uint8_t start_wp, uint8_t wp)
{
float start_alt = waypoints[start_wp].a;
float diff_alt = waypoints[wp].a - start_alt;
nav_glide_alt(waypoints[start_wp].a, waypoints[wp].a);
}
void nav_glide_alt(float start_alt, float final_alt) {
float diff_alt = final_alt - start_alt;
float alt = start_alt + nav_leg_progress * diff_alt;
float pre_climb = stateGetHorizontalSpeedNorm_f() * diff_alt / nav_leg_length;
NavVerticalAltitudeMode(alt, pre_climb);
}
#define MAX_DIST_CARROT 250.
#define MIN_HEIGHT_CARROT 50.
#define MAX_HEIGHT_CARROT 150.

6
sw/airborne/firmwares/fixedwing/nav.h
View File

@@ -98,6 +98,11 @@ extern void fly_to_xy(float x, float y);
fly_to_xy(waypoints[_wp].x, waypoints[_wp].y); \
}
#define NavGotoPoint(_point) { \
horizontal_mode = HORIZONTAL_MODE_WAYPOINT; \
fly_to_xy(_point.x, _point.y); \
}
extern void nav_eight_init(void);
extern void nav_eight(uint8_t, uint8_t, float);
@@ -140,6 +145,7 @@ extern void nav_follow(uint8_t _ac_id, float _distance, float _height);
#define NavFollow(_ac_id, _distance, _height) nav_follow(_ac_id, _distance, _height)
extern void nav_glide(uint8_t start_wp, uint8_t wp);
extern void nav_glide_alt(float start_alt, float final_alt);
#define NavGlide(_start_wp, _wp) nav_glide(_start_wp, _wp)
#define NavCircleWaypoint(wp, radius) \

35
sw/airborne/firmwares/rotorcraft/navigation.c
View File

@@ -55,6 +55,16 @@ const float max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME;
float flight_altitude;
/** Empty navigation functions set at init
*/
static void empty_stage_init(void) {}
static void empty_goto(struct EnuCoor_f *wp UNUSED) {}
static void empty_route(struct EnuCoor_f *wp_start UNUSED, struct EnuCoor_f *wp_end UNUSED) {}
static bool empty_approaching(struct EnuCoor_f *wp_to UNUSED, struct EnuCoor_f *wp_from UNUSED, float approaching_time UNUSED) { return true; }
static void empty_circle(struct EnuCoor_f *wp_center UNUSED, float radius UNUSED) {}
static void empty_oval_init(void) {}
static void empty_oval(struct EnuCoor_f *wp1 UNUSED, struct EnuCoor_f *wp2 UNUSED, float radius UNUSED) {};
static inline void nav_set_altitude(void);
void nav_init(void)
@@ -94,13 +104,13 @@ void nav_init(void)
nav.climb_vspeed = NAV_CLIMB_VSPEED;
nav.descend_vspeed = NAV_DESCEND_VSPEED;
nav.nav_stage_init = NULL;
nav.nav_goto = NULL;
nav.nav_route = NULL;
nav.nav_approaching = NULL;
nav.nav_circle = NULL;
nav.nav_oval_init = NULL;
nav.nav_oval = NULL;
nav.nav_stage_init = empty_stage_init;
nav.nav_goto = empty_goto;
nav.nav_route = empty_route;
nav.nav_approaching = empty_approaching;
nav.nav_circle = empty_circle;
nav.nav_oval_init = empty_oval_init;
nav.nav_oval = empty_oval;
// generated init function
auto_nav_init();
@@ -270,6 +280,17 @@ bool nav_is_in_flight(void)
return autopilot_in_flight();
}
void nav_glide_points(struct EnuCoor_f *start_point, struct EnuCoor_f *end_point)
{
struct FloatVect2 wp_diff, pos_diff;
VECT2_DIFF(wp_diff, *end_point, *start_point);
VECT2_DIFF(pos_diff, *stateGetPositionEnu_f(), *start_point);
float length2 = Max(float_vect2_norm2(&wp_diff), 0.1f);
float progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / length2;
float alt = start_point->z + (end_point->z - start_point->z) * progress;
NavVerticalAltitudeMode(alt, 0);
}
/** Home mode navigation */
void nav_home(void)
{

67
sw/airborne/firmwares/rotorcraft/navigation.h
View File

@@ -211,6 +211,8 @@ extern void nav_periodic_task(void);
extern bool nav_is_in_flight(void);
extern void nav_glide_points(struct EnuCoor_f *start_point, struct EnuCoor_f *end_point);
/** heading utility functions */
extern void nav_set_heading_rad(float rad);
extern void nav_set_heading_deg(float deg);
@@ -299,44 +301,47 @@ bool nav_check_wp_time(struct EnuCoor_f *wp, uint16_t stay_time);
/*********** Navigation to waypoint *************************************/
static inline void NavGotoWaypoint(uint8_t wp)
{
if (nav.nav_goto) {
nav.nav_goto(&waypoints[wp].enu_f);
struct EnuCoor_f * _wp = waypoint_get_enu_f(wp);
if (_wp != NULL) {
nav.nav_goto(_wp);
}
}
/*********** Navigation along a line *************************************/
static inline void NavSegment(uint8_t wp_start, uint8_t wp_end)
{
if (nav.nav_route) {
nav.nav_route(&waypoints[wp_start].enu_f, &waypoints[wp_end].enu_f);
struct EnuCoor_f * _wp_start = waypoint_get_enu_f(wp_start);
struct EnuCoor_f * _wp_end = waypoint_get_enu_f(wp_end);
if (_wp_start != NULL && _wp_end != NULL) {
nav.nav_route(_wp_start, _wp_end);
}
}
static inline bool NavApproaching(uint8_t wp, float approaching_time)
{
if (nav.nav_approaching) {
return nav.nav_approaching(&waypoints[wp].enu_f, NULL, approaching_time);
}
else {
return true;
struct EnuCoor_f * _wp = waypoint_get_enu_f(wp);
if (_wp != NULL) {
return nav.nav_approaching(_wp, NULL, approaching_time);
}
return true; // not valid, end now
}
static inline bool NavApproachingFrom(uint8_t to, uint8_t from, float approaching_time)
{
if (nav.nav_approaching) {
return nav.nav_approaching(&waypoints[to].enu_f, &waypoints[from].enu_f, approaching_time);
}
else {
return true;
struct EnuCoor_f * _to = waypoint_get_enu_f(to);
struct EnuCoor_f * _from = waypoint_get_enu_f(from);
if (_to != NULL && _from != NULL) {
return nav.nav_approaching(_to, _from, approaching_time);
}
return true; // not valid, end now
}
/*********** Navigation on a circle **************************************/
static inline void NavCircleWaypoint(uint8_t wp_center, float radius)
{
if (nav.nav_circle) {
nav.nav_circle(&waypoints[wp_center].enu_f, radius);
struct EnuCoor_f * _wp_center = waypoint_get_enu_f(wp_center);
if (_wp_center != NULL) {
nav.nav_circle(_wp_center, radius);
}
}
@@ -344,33 +349,29 @@ static inline void NavCircleWaypoint(uint8_t wp_center, float radius)
/*********** Navigation along an oval *************************************/
static inline void nav_oval_init(void)
{
if (nav.nav_oval_init) {
nav.nav_oval_init();
}
nav.nav_oval_init();
}
static inline void Oval(uint8_t wp1, uint8_t wp2, float radius)
{
if (nav.nav_oval) {
nav.nav_oval(&waypoints[wp1].enu_f, &waypoints[wp2].enu_f, radius);
struct EnuCoor_f * _wp1 = waypoint_get_enu_f(wp1);
struct EnuCoor_f * _wp2 = waypoint_get_enu_f(wp2);
if (_wp1 != NULL && _wp2 != NULL) {
nav.nav_oval(_wp1, _wp2, radius);
}
}
/** Nav glide routine */
static inline void NavGlide(uint8_t start_wp, uint8_t wp)
{
struct FloatVect2 wp_diff, pos_diff;
VECT2_DIFF(wp_diff, waypoints[wp].enu_f, waypoints[start_wp].enu_f);
VECT2_DIFF(pos_diff, *stateGetPositionEnu_f(), waypoints[start_wp].enu_f);
float start_alt = waypoint_get_alt(start_wp);
float diff_alt = waypoint_get_alt(wp) - start_alt;
float length2 = Max(float_vect2_norm2(&wp_diff), 0.1f);
float progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / length2;
float alt = start_alt + diff_alt * progress;
NavVerticalAltitudeMode(alt, 0);
}
static inline void NavGlide(uint8_t wp_start, uint8_t wp_end)
{
struct EnuCoor_f * _wp_start = waypoint_get_enu_f(wp_start);
struct EnuCoor_f * _wp_end = waypoint_get_enu_f(wp_end);
if (_wp_start != NULL && _wp_end != NULL) {
nav_glide_points(_wp_start, _wp_end);
}
}
/***********************************************************

6
sw/airborne/modules/nav/nav_rotorcraft_base.c
View File

@@ -100,10 +100,12 @@ static bool nav_approaching(struct EnuCoor_f *wp, struct EnuCoor_f *from, float
/* if coming from a valid waypoint */
if (from != NULL) {
/* return TRUE if normal line at the end of the segment is crossed */
/* return TRUE if normal line at the end of the segment is crossed
* if 'from' and 'to' WP are the same, diff is zero and function should
* return false (only distance to waypoint will be considered) */
struct FloatVect2 from_diff;
VECT2_DIFF(from_diff, *wp, *from);
return (diff.x * from_diff.x + diff.y * from_diff.y < 0.f);
return (diff.x * from_diff.x + diff.y * from_diff.y < -0.01f);
}
return false;

150
sw/airborne/modules/nav/nav_takeoff_and_landing.h Normal file
View File

@@ -0,0 +1,150 @@
/*
* Copyright (C) 2023 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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_takeoff_and_landing.h"
* @author Gautier Hattenberger <gautier.hattenberger@enac.fr>
* Navigation routines for takeoff and landing
* Basic procedures for rotorcraft and fixed-wing
*/
#ifndef NAV_TAKEOFF_AND_LANDING_H
#define NAV_TAKEOFF_AND_LANDING_H
#include "std.h"
#include "math/pprz_geodetic_float.h"
enum nav_takeoff_status {
NAV_TAKEOFF_INIT,
NAV_TAKEOFF_START_MOTOR,
NAV_TAKEOFF_CLIMB,
NAV_TAKEOFF_DONE
};
enum nav_landing_status {
NAV_LANDING_INIT,
NAV_LANDING_REACH_AF,
NAV_LANDING_DESCENT,
NAV_LANDING_FLARE,
NAV_LANDING_DONE
};
/** Structure for takeoff
*/
struct nav_takeoff {
enum nav_takeoff_status status; ///< current step
struct EnuCoor_f climb_pos; ///< climb point
struct EnuCoor_f start_pos; ///< start position
uint8_t climb_id; ///< climb waypoint id
bool timeout; ///< true if status should be set to init
};
/** Structure for landing
*/
struct nav_landing {
enum nav_landing_status status; ///< current step
struct EnuCoor_f td_pos; ///< touch down point
struct EnuCoor_f af_pos; ///< start of descent point
float radius; ///< circle radius for fixedwing landing
uint8_t td_id; ///< touch down wp id
uint8_t af_id; ///< start of descent wp id
bool timeout; ///< true if status should be set to init
};
/** Takeoff direction in range [0-360] (deg)
* set to flight plan QFU by default
*/
extern float nav_takeoff_direction;
/** Init function
*/
extern void nav_takeoff_and_landing_init(void);
/** Periodic timeout check function
*/
extern void nav_takeoff_and_landing_periodic(void);
/** Takeoff from a waypoint
*
* - fixedwing: set the climb direction
* - rotorcraft: set the wp at current location and climb verticaly
*
* @param[in] wp_id waypoint ID
* @return true until procedure is completed
*/
extern bool nav_takeoff_from_wp(uint8_t wp_id);
/** Takeoff from lat long location
*
* - fixedwing: set the climb direction
* - rotorcraft: start from current location and climb verticaly (lat, lon have no effect)
*
* @param[in] lat takeoff latitude (deg)
* @param[in] lon takeoff longitude (deg)
* @return true until procedure is completed
*/
extern bool nav_takeoff_from_loc(float lat, float lon);
/** Takeoff from current location
*
* - fixedwing: climb direction is specified with setting
* - rotorcraft: start from current location and climb verticaly
*
* @return true until procedure is completed
*/
extern bool nav_takeoff_from_here(void);
/** Land at waypoint location
*
* set touch down (TD) and start of descent (AF)
* - rotorcraft: if TD and AF id are the same, vertical landing
*
* @param[in] td_id waypoint ID for touch down point
* @param[in] af_if waypoint ID for start of descent
* @param[in] radius circle radius for final turn (if positive: turn right, if negative: turn left) (m)
* @return true until procedure is completed
*/
extern bool nav_land_at_wp(uint8_t td_id, uint8_t af_id, float radius);
/** Land at lat long location
*
* @param[in] td_alt touch down altitude above ground ref (m)
* @param[in] lat landing latitude (deg)
* @param[in] lon landing longitude (deg)
* @param[in] dir direction approach direction ([0-360], deg)
* @param[in] dist distance to start descent (m)
* @param[in] radius circle radius for final turn (if positive: turn right, if negative: turn left) (m)
* @return true until procedure is completed
*/
extern bool nav_land_at_loc(float td_alt, float lat, float lon, float dir, float dist, float radius);
/** Land at current location
*
* emergency landing
* - fixedwing: circle down around current location
* - rotorcraft: vertical landing at current location
*
* @param[in] td_alt touch down altitude above ground ref (m)
* @param[in] radius circle radius for final turn (if positive: turn right, if negative: turn left) (m)
* @return true until procedure is completed
*/
extern bool nav_land_here(float td_alt, float radius);
#endif // NAV_TAKEOFF_AND_LANDING_H

376
sw/airborne/modules/nav/nav_takeoff_and_landing_fw.c Normal file
View File

@@ -0,0 +1,376 @@
/*
* Copyright (C) 2023 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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_takeoff_and_landing_fw.c"
* @author Gautier Hattenberger <gautier.hattenberger@enac.fr>
* Navigation routines for takeoff and landing
* Basic procedures for fixed-wing
*/
#include "modules/nav/nav_takeoff_and_landing.h"
#include "math/pprz_geodetic_float.h"
#include "generated/flight_plan.h"
#include "firmwares/fixedwing/nav.h"
#include "firmwares/fixedwing/guidance/guidance_v.h"
#include "autopilot.h"
#ifndef NAV_TAKEOFF_PITCH
#define NAV_TAKEOFF_PITCH 15.f
#endif
#ifndef NAV_TAKEOFF_THROTTLE
#define NAV_TAKEOFF_THROTTLE 1.f
#endif
#ifndef NAV_TAKEOFF_HEIGHT
#define NAV_TAKEOFF_HEIGHT 20.f
#endif
#ifndef NAV_TAKEOFF_DIST
#define NAV_TAKEOFF_DIST 200.f
#endif
#ifndef NAV_TAKEOFF_AUTO_LAUNCH
#define NAV_TAKEOFF_AUTO_LAUNCH true
#endif
#ifndef NAV_LANDING_DESCEND_SPEED
#define NAV_LANDING_DESCEND_SPEED -1.f
#endif
#ifndef NAV_LANDING_AF_HEIGHT
#define NAV_LANDING_AF_HEIGHT 30.f
#endif
#ifndef NAV_LANDING_FLARE_HEIGHT
#define NAV_LANDING_FLARE_HEIGHT 10.f
#endif
static struct nav_takeoff takeoff;
static struct nav_landing landing;
static bool nav_takeoff_run(void);
static bool nav_land_run(void);
float nav_takeoff_direction;
#if USE_MISSION
#include "modules/mission/mission_common.h"
static bool nav_takeoff_mission(uint8_t nb UNUSED, float *params UNUSED, enum MissionRunFlag flag)
{
if (flag == MissionInit) {
takeoff.status = NAV_TAKEOFF_INIT;
return nav_takeoff_from_here();
}
else if (flag == MissionRun) {
return nav_takeoff_run();
}
return false; // not a valid case
}
static bool nav_land_mission(uint8_t nb, float *params, enum MissionRunFlag flag)
{
if (flag == MissionInit && nb == 2) {
float td_alt = params[0];
float radius = params[1];
landing.status = NAV_LANDING_INIT;
return nav_land_here(td_alt, radius);
} else if (flag == MissionInit && nb == 3) {
uint8_t td_id = (uint8_t)(params[0]);
uint8_t af_id = (uint8_t)(params[1]);
float radius = params[2];
return nav_land_at_wp(td_id, af_id, radius);
} else if (flag == MissionInit && nb == 6) {
float td_alt = params[0];
float lat = params[1];
float lon = params[2];
float dir = params[3];
float dist = params[4];
float radius = params[5];
return nav_land_at_loc(td_alt, lat, lon, dir, dist, radius);
} else if (flag == MissionRun) {
return nav_land_run();
}
return false; // not a valid case
}
#endif
void nav_takeoff_and_landing_init(void)
{
takeoff.status = NAV_TAKEOFF_INIT;
takeoff.climb_id = 0;
takeoff.timeout = false;
landing.status = NAV_LANDING_INIT;
landing.td_id = 0;
landing.af_id = 0;
landing.timeout = false;
nav_takeoff_direction = QFU;
#if USE_MISSION
mission_register(nav_takeoff_mission, "TKOFF");
mission_register(nav_land_mission, "LAND");
#endif
}
void nav_takeoff_and_landing_periodic(void)
{
if (takeoff.timeout) {
takeoff.status = NAV_TAKEOFF_INIT;
}
if (landing.timeout) {
landing.status = NAV_LANDING_INIT;
}
// reset flag to true, set to false if run function is called during period
takeoff.timeout = true;
landing.timeout = true;
}
static bool nav_takeoff_run(void) {
static int start_motor_counter = 0;
switch (takeoff.status) {
case NAV_TAKEOFF_INIT:
takeoff.status = NAV_TAKEOFF_START_MOTOR;
start_motor_counter = 0;
break;
case NAV_TAKEOFF_START_MOTOR:
autopilot_set_motors_on(true);
start_motor_counter++;
if (start_motor_counter == NAVIGATION_FREQUENCY) {
start_motor_counter = 0;
if (autopilot_get_motors_on()) {
#if NAV_TAKEOFF_AUTO_LAUNCH
autopilot.launch = true;
#endif
takeoff.status = NAV_TAKEOFF_CLIMB; // next step after 1s
} else {
takeoff.status = NAV_TAKEOFF_DONE; // failed to start motors, end now
}
}
break;
case NAV_TAKEOFF_CLIMB:
// climb towards point at specified pitch and throttle
NavGotoPoint(takeoff.climb_pos);
NavVerticalAutoThrottleMode(RadOfDeg(NAV_TAKEOFF_PITCH));
NavVerticalThrottleMode(TRIM_UPPRZ(MAX_PPRZ*NAV_TAKEOFF_THROTTLE));
if (nav_approaching_xy(takeoff.climb_pos.x, takeoff.climb_pos.y, takeoff.start_pos.x, takeoff.start_pos.y, CARROT)
|| (stateGetPositionEnu_f()->z > takeoff.start_pos.z + NAV_TAKEOFF_HEIGHT)) {
// end when climb point or target alt is reached
takeoff.status = NAV_TAKEOFF_DONE;
}
break;
default:
takeoff.status = NAV_TAKEOFF_INIT;
return false;
}
takeoff.timeout = false;
return true;
}
bool nav_takeoff_from_wp(uint8_t wp_id)
{
if (takeoff.status == NAV_TAKEOFF_INIT) {
takeoff.climb_id = wp_id;
takeoff.climb_pos.x = WaypointX(wp_id);
takeoff.climb_pos.y = WaypointY(wp_id);
takeoff.climb_pos.z = WaypointAlt(wp_id) - GetAltRef();
takeoff.start_pos = *stateGetPositionEnu_f();
}
return nav_takeoff_run();
}
bool nav_takeoff_from_loc(float lat, float lon)
{
if (takeoff.status == NAV_TAKEOFF_INIT) {
struct LlaCoor_f lla = { RadOfDeg(lat), RadOfDeg(lon), stateGetPositionLla_f()->alt + NAV_TAKEOFF_HEIGHT };
struct UtmCoor_f utm;
utm_of_lla_f(&utm, &lla);
ENU_OF_UTM_DIFF(takeoff.climb_pos, utm, state.utm_origin_f);
}
return nav_takeoff_from_here();
}
bool nav_takeoff_from_here(void)
{
if (takeoff.status == NAV_TAKEOFF_INIT) {
takeoff.climb_id = 0;
takeoff.start_pos = *stateGetPositionEnu_f();
takeoff.climb_pos = takeoff.start_pos;
takeoff.climb_pos.x = takeoff.start_pos.x + NAV_TAKEOFF_DIST * sinf(RadOfDeg(nav_takeoff_direction));
takeoff.climb_pos.y = takeoff.start_pos.y + NAV_TAKEOFF_DIST * cosf(RadOfDeg(nav_takeoff_direction));
takeoff.climb_pos.z = takeoff.start_pos.z + NAV_TAKEOFF_HEIGHT;
}
return nav_takeoff_run();
}
static bool nav_land_run(void)
{
static float baseleg_out_qdr = 0.f;
float qdr = 0.f;
// test if glide is needed or not
float dx = landing.td_pos.x - landing.af_pos.x;
float dy = landing.td_pos.y - landing.af_pos.y;
float dist = sqrtf(dx*dx + dy*dy);
bool glide = false;
float x_unit = 0.f;
float y_unit = 0.f;
if (dist > 1.f) {
glide = true;
x_unit = dx / dist;
y_unit = dy / dist;
}
switch (landing.status) {
case NAV_LANDING_INIT:
if (glide) {
// compute baseleg circle center, store in dummy WP 0
waypoints[0].x = landing.af_pos.x + y_unit * landing.radius;
waypoints[0].y = landing.af_pos.y - x_unit * landing.radius;
waypoints[0].a = landing.af_pos.z + GetAltRef();
baseleg_out_qdr = M_PI - atan2f(-y_unit, -x_unit);
if (landing.radius < 0.f) {
baseleg_out_qdr += M_PI;
}
} else {
// turn around AF point
waypoints[0].x = landing.af_pos.x;
waypoints[0].y = landing.af_pos.y;
waypoints[0].a = landing.af_pos.z + GetAltRef();
baseleg_out_qdr = 0.f;
}
landing.status = NAV_LANDING_REACH_AF;
break;
case NAV_LANDING_REACH_AF:
NavVerticalAutoThrottleMode(0.f);
NavVerticalAltitudeMode(waypoints[0].a, 0.f);
NavCircleWaypoint(0, landing.radius);
if (NavCircleCount() > 0.25 && (fabsf(GetPosAlt() - WaypointAlt(0)) < 10.f)) {
if (glide) {
// if final glide, direction should be correct
qdr = M_PI_2 - nav_circle_trigo_qdr;
NormCourseRad(qdr);
if (CloseRadAngles(baseleg_out_qdr, qdr)) {
landing.status = NAV_LANDING_DESCENT;
}
} else {
// no final glide, start from correct altitude
landing.status = NAV_LANDING_DESCENT;
}
}
break;
case NAV_LANDING_DESCENT:
if (glide) {
nav_route_xy(landing.af_pos.x, landing.af_pos.y, landing.td_pos.x, landing.td_pos.y);
NavVerticalAutoThrottleMode(0.f);
nav_glide_alt(landing.af_pos.z + GetAltRef(), landing.td_pos.z + GetAltRef());
if (nav_approaching_xy(landing.td_pos.x, landing.td_pos.y, landing.af_pos.x, landing.af_pos.y, CARROT)
|| (GetPosAlt() < landing.td_pos.z + NAV_LANDING_FLARE_HEIGHT + GetAltRef())) {
landing.status = NAV_LANDING_FLARE;
}
} else {
NavCircleWaypoint(0, landing.radius);
NavVerticalAutoThrottleMode(0.f);
NavVerticalClimbMode(NAV_LANDING_DESCEND_SPEED);
if (GetPosAlt() < landing.td_pos.z + NAV_LANDING_FLARE_HEIGHT + GetAltRef()) {
landing.status = NAV_LANDING_FLARE;
}
}
break;
case NAV_LANDING_FLARE:
NavVerticalAutoThrottleMode(0.f);
NavVerticalThrottleMode(0.f);
if (glide) {
nav_route_xy(landing.af_pos.x, landing.af_pos.y, landing.td_pos.x, landing.td_pos.y);
if (nav_approaching_xy(landing.td_pos.x, landing.td_pos.y, landing.af_pos.x, landing.af_pos.y, 0.f)) {
landing.status = NAV_LANDING_DONE;
}
} else {
NavCircleWaypoint(0, landing.radius);
// stay on circle, no end
}
break;
case NAV_LANDING_DONE:
default:
landing.status = NAV_LANDING_INIT;
return false;
}
landing.timeout = false;
return true;
}
bool nav_land_at_wp(uint8_t td_id, uint8_t af_id, float radius)
{
if (landing.status == NAV_LANDING_INIT) {
landing.td_id = td_id;
landing.af_id = af_id;
landing.td_pos.x = WaypointX(td_id);
landing.td_pos.y = WaypointY(td_id);
landing.td_pos.z = WaypointAlt(td_id) - GetAltRef();
landing.af_pos.x = WaypointX(af_id);
landing.af_pos.y = WaypointY(af_id);
landing.af_pos.z = landing.td_pos.z + NAV_LANDING_AF_HEIGHT;
landing.radius = radius;
}
return nav_land_run();
}
bool nav_land_at_loc(float td_alt, float lat, float lon, float dir, float dist, float radius)
{
if (landing.status == NAV_LANDING_INIT) {
landing.td_id = 0;
landing.af_id = 0;
struct LlaCoor_f lla = { RadOfDeg(lat), RadOfDeg(lon), GetAltRef() + td_alt };
struct UtmCoor_f utm;
utm_of_lla_f(&utm, &lla);
ENU_OF_UTM_DIFF(landing.td_pos, utm, state.utm_origin_f);
landing.af_pos.x = landing.td_pos.x + dist * sinf(RadOfDeg(dir));
landing.af_pos.y = landing.td_pos.y + dist * cosf(RadOfDeg(dir));
landing.af_pos.z = landing.td_pos.z + NAV_LANDING_AF_HEIGHT;
landing.radius = radius;
}
return nav_land_run();
}
bool nav_land_here(float td_alt, float radius)
{
if (landing.status == NAV_LANDING_INIT) {
landing.td_id = 0;
landing.af_id = 0;
landing.td_pos = *stateGetPositionEnu_f();
landing.td_pos.z = td_alt;
landing.af_pos = landing.td_pos;
landing.af_pos.z = landing.td_pos.z + NAV_LANDING_AF_HEIGHT;
landing.radius = radius;
}
return nav_land_run();
}

294
sw/airborne/modules/nav/nav_takeoff_and_landing_rotorcraft.c Normal file
View File

@@ -0,0 +1,294 @@
/*
* Copyright (C) 2023 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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_takeoff_and_landing_rotorcraft.c"
* @author Gautier Hattenberger <gautier.hattenberger@enac.fr>
* Navigation routines for takeoff and landing
* Basic procedures for rotorcraft
*/
#include "modules/nav/nav_takeoff_and_landing.h"
#include "math/pprz_geodetic_float.h"
#include "generated/flight_plan.h"
#include "firmwares/rotorcraft/navigation.h"
#include "modules/nav/waypoints.h"
#include "autopilot.h"
#ifndef NAV_TAKEOFF_CLIMB_SPEED
#define NAV_TAKEOFF_CLIMB_SPEED NAV_CLIMB_VSPEED
#endif
#ifndef NAV_TAKEOFF_HEIGHT
#define NAV_TAKEOFF_HEIGHT 2.f
#endif
#ifndef NAV_LANDING_DESCEND_SPEED
#define NAV_LANDING_DESCEND_SPEED NAV_DESCEND_VSPEED
#endif
#ifndef NAV_LANDING_AF_HEIGHT
#define NAV_LANDING_AF_HEIGHT 5.f
#endif
#ifndef NAV_LANDING_FLARE_HEIGHT
#define NAV_LANDING_FLARE_HEIGHT 2.f
#endif
static struct nav_takeoff takeoff;
static struct nav_landing landing;
static bool nav_takeoff_run(void);
static bool nav_land_run(void);
float nav_takeoff_direction;
#if USE_MISSION
#include "modules/mission/mission_common.h"
static bool nav_takeoff_mission(uint8_t nb UNUSED, float *params UNUSED, enum MissionRunFlag flag)
{
if (flag == MissionInit) {
takeoff.status = NAV_TAKEOFF_INIT;
return nav_takeoff_from_here();
}
else if (flag == MissionRun) {
return nav_takeoff_run();
}
return false; // not a valid case
}
static bool nav_land_mission(uint8_t nb, float *params, enum MissionRunFlag flag)
{
if (flag == MissionInit && nb == 1) {
float td_alt = params[0];
landing.status = NAV_LANDING_INIT;
return nav_land_here(td_alt, 0.f);
} else if (flag == MissionInit && nb == 2) {
uint8_t td_id = (uint8_t)(params[0]);
uint8_t af_id = (uint8_t)(params[1]);
return nav_land_at_wp(td_id, af_id, 0.f);
} else if (flag == MissionInit && nb == 5) {
float td_alt = params[0];
float lat = params[1];
float lon = params[2];
float dir = params[3];
float dist = params[4];
return nav_land_at_loc(td_alt, lat, lon, dir, dist, 0.f);
} else if (flag == MissionRun) {
return nav_land_run();
}
return false; // not a valid case
}
#endif
void nav_takeoff_and_landing_init(void)
{
takeoff.status = NAV_TAKEOFF_INIT;
takeoff.climb_id = 0;
takeoff.timeout = false;
landing.status = NAV_LANDING_INIT;
landing.radius = 0.f; // no effect
landing.td_id = 0;
landing.af_id = 0;
landing.timeout = false;
nav_takeoff_direction = QFU;
#if USE_MISSION
mission_register(nav_takeoff_mission, "TKOFF");
mission_register(nav_land_mission, "LAND");
#endif
}
void nav_takeoff_and_landing_periodic(void)
{
if (takeoff.timeout) {
takeoff.status = NAV_TAKEOFF_INIT;
}
if (landing.timeout) {
landing.status = NAV_LANDING_INIT;
}
// reset flag to true, set to false if run function is called during period
takeoff.timeout = true;
landing.timeout = true;
}
static bool nav_takeoff_run(void) {
static int start_motor_counter = 0;
switch (takeoff.status) {
case NAV_TAKEOFF_INIT:
takeoff.status = NAV_TAKEOFF_START_MOTOR;
start_motor_counter = 0;
break;
case NAV_TAKEOFF_START_MOTOR:
NavResurrect();
start_motor_counter++;
if (start_motor_counter == NAVIGATION_FREQUENCY) {
start_motor_counter = 0;
if (autopilot_get_motors_on()) {
takeoff.status = NAV_TAKEOFF_CLIMB; // next step after 1s
} else {
takeoff.status = NAV_TAKEOFF_DONE; // failed to start motors, end now
}
}
break;
case NAV_TAKEOFF_CLIMB:
// call vertical climb from nav/guidance
NavGotoWaypoint(takeoff.climb_id);
NavVerticalClimbMode(NAV_TAKEOFF_CLIMB_SPEED);
if (stateGetPositionEnu_f()->z - takeoff.start_pos.z > NAV_TAKEOFF_HEIGHT) {
// end when takeoff height is reached
takeoff.status = NAV_TAKEOFF_DONE;
}
break;
case NAV_TAKEOFF_DONE:
default:
takeoff.status = NAV_TAKEOFF_INIT;
return false;
}
takeoff.timeout = false;
return true;
}
bool nav_takeoff_from_wp(uint8_t wp_id)
{
if (takeoff.status == NAV_TAKEOFF_INIT) {
takeoff.climb_id = wp_id;
waypoint_set_here_2d(wp_id);
takeoff.climb_pos = *waypoint_get_enu_f(wp_id);
takeoff.start_pos = *stateGetPositionEnu_f();
}
return nav_takeoff_run();
}
bool nav_takeoff_from_loc(float lat UNUSED, float lon UNUSED)
{
return nav_takeoff_from_here();
}
bool nav_takeoff_from_here(void)
{
if (takeoff.status == NAV_TAKEOFF_INIT) {
takeoff.climb_id = 0; // use dummy hidden WP
takeoff.start_pos = *stateGetPositionEnu_f();
takeoff.climb_pos = takeoff.start_pos;
waypoint_set_enu(0, &takeoff.climb_pos);
}
return nav_takeoff_run();
}
static bool nav_land_run(void)
{
float dx, dy, dist2;
switch (landing.status) {
case NAV_LANDING_INIT:
landing.status = NAV_LANDING_REACH_AF;
break;
case NAV_LANDING_REACH_AF:
nav.nav_goto(&landing.af_pos);
NavVerticalAltitudeMode(landing.af_pos.z, 0.f);
if (nav.nav_approaching(&landing.af_pos, NULL, 0.f)) {
landing.status = NAV_LANDING_DESCENT;
}
break;
case NAV_LANDING_DESCENT:
// test if glide is needed or not
dx = landing.af_pos.x - landing.td_pos.x;
dy = landing.af_pos.y - landing.td_pos.y;
dist2 = dx*dx + dy*dy;
if (dist2 > 1.f) {
nav.nav_route(&landing.af_pos, &landing.td_pos);
nav_glide_points(&landing.af_pos, &landing.td_pos);
} else {
nav.nav_goto(&landing.td_pos);
NavVerticalClimbMode(NAV_LANDING_DESCEND_SPEED);
}
if (nav.nav_approaching(&landing.td_pos, &landing.af_pos, 0.f)
|| (stateGetPositionEnu_f()->z < landing.td_pos.z)) {
landing.status = NAV_LANDING_FLARE;
}
break;
case NAV_LANDING_FLARE:
nav.nav_goto(&landing.td_pos);
NavVerticalClimbMode(NAV_LANDING_DESCEND_SPEED);
if (!nav_is_in_flight()) {
landing.status = NAV_LANDING_DONE;
}
break;
case NAV_LANDING_DONE:
default:
NavKillThrottle();
landing.status = NAV_LANDING_INIT;
return false;
}
landing.timeout = false;
return true;
}
bool nav_land_at_wp(uint8_t td_id, uint8_t af_id, float radius UNUSED)
{
if (landing.status == NAV_LANDING_INIT) {
landing.td_id = td_id;
landing.af_id = af_id;
landing.td_pos = *waypoint_get_enu_f(td_id);
landing.td_pos.z = landing.td_pos.z + NAV_LANDING_FLARE_HEIGHT;
landing.af_pos = *waypoint_get_enu_f(af_id);
landing.af_pos.z = landing.td_pos.z + NAV_LANDING_AF_HEIGHT;
}
return nav_land_run();
}
bool nav_land_at_loc(float td_alt, float lat, float lon, float dir, float dist, float radius UNUSED)
{
if (landing.status == NAV_LANDING_INIT) {
landing.td_id = 0;
landing.af_id = 0;
struct LlaCoor_f lla = { RadOfDeg(lat), RadOfDeg(lon), state.ned_origin_f.lla.alt + td_alt };
enu_of_lla_point_f(&landing.td_pos, &state.ned_origin_f, &lla);
landing.td_pos.z = landing.td_pos.z + NAV_LANDING_FLARE_HEIGHT;
landing.af_pos.x = landing.td_pos.x + dist * sinf(RadOfDeg(dir));
landing.af_pos.y = landing.td_pos.y + dist * cosf(RadOfDeg(dir));
landing.af_pos.z = landing.td_pos.z + NAV_LANDING_AF_HEIGHT;
}
return nav_land_run();
}
bool nav_land_here(float td_alt, float radius UNUSED)
{
if (landing.status == NAV_LANDING_INIT) {
landing.td_id = 0;
landing.af_id = 0;
landing.td_pos = *stateGetPositionEnu_f();
landing.td_pos.z = td_alt + NAV_LANDING_FLARE_HEIGHT;
landing.af_pos = landing.td_pos;
landing.af_pos.z = landing.td_pos.z + NAV_LANDING_AF_HEIGHT;
}
return nav_land_run();
}

62
sw/airborne/modules/nav/waypoints.c
View File

@@ -118,6 +118,42 @@ float waypoint_get_alt(uint8_t wp_id)
return 0.f;
}
float waypoint_get_lat_deg(uint8_t wp_id)
{
if (wp_id < nb_waypoint) {
if (!waypoint_is_global(wp_id) && !bit_is_set(waypoints[wp_id].flags, WP_FLAG_LLA_I)) {
waypoint_globalize(wp_id);
}
return DEG_OF_EM7DEG(waypoints[wp_id].lla.lat);
}
else {
return 0.f;
}
}
float waypoint_get_lat_rad(uint8_t wp_id)
{
return RadOfDeg(waypoint_get_lat_deg(wp_id));
}
float waypoint_get_lon_deg(uint8_t wp_id)
{
if (wp_id < nb_waypoint) {
if (!waypoint_is_global(wp_id) && !bit_is_set(waypoints[wp_id].flags, WP_FLAG_LLA_I)) {
waypoint_globalize(wp_id);
}
return DEG_OF_EM7DEG(waypoints[wp_id].lla.lon);
}
else {
return 0.f;
}
}
float waypoint_get_lon_rad(uint8_t wp_id)
{
return RadOfDeg(waypoint_get_lon_deg(wp_id));
}
void waypoint_set_enu_i(uint8_t wp_id, struct EnuCoor_i *enu)
{
if (wp_id < nb_waypoint) {
@@ -340,6 +376,32 @@ struct LlaCoor_i *waypoint_get_lla(uint8_t wp_id)
}
}
struct EnuCoor_f *waypoint_get_enu_f(uint8_t wp_id)
{
if (wp_id < nb_waypoint) {
if (waypoint_is_global(wp_id) && !bit_is_set(waypoints[wp_id].flags, WP_FLAG_ENU_F)) {
waypoint_localize(wp_id);
}
return &waypoints[wp_id].enu_f;
}
else {
return NULL;
}
}
struct EnuCoor_i *waypoint_get_enu_i(uint8_t wp_id)
{
if (wp_id < nb_waypoint) {
if (waypoint_is_global(wp_id) && !bit_is_set(waypoints[wp_id].flags, WP_FLAG_ENU_I)) {
waypoint_localize(wp_id);
}
return &waypoints[wp_id].enu_i;
}
else {
return NULL;
}
}
void waypoint_copy(uint8_t wp_dest, uint8_t wp_src)
{
if (wp_dest < nb_waypoint && wp_src < nb_waypoint) {

19
sw/airborne/modules/nav/waypoints.h
View File

@@ -72,6 +72,14 @@ extern float waypoint_get_x(uint8_t wp_id);
extern float waypoint_get_y(uint8_t wp_id);
/** Get altitude of waypoint in meters (above reference) */
extern float waypoint_get_alt(uint8_t wp_id);
/** Get latitude of waypoint in deg */
extern float waypoint_get_lat_deg(uint8_t wp_id);
/** Get latitude of waypoint in rad */
extern float waypoint_get_lat_rad(uint8_t wp_id);
/** Get longitude of waypoint in deg */
extern float waypoint_get_lon_deg(uint8_t wp_id);
/** Get longitude of waypoint in rad */
extern float waypoint_get_lon_rad(uint8_t wp_id);
/** Get LLA coordinates of waypoint.
* If the waypoint does not have its global coordinates set,
@@ -82,6 +90,17 @@ extern float waypoint_get_alt(uint8_t wp_id);
*/
extern struct LlaCoor_i *waypoint_get_lla(uint8_t wp_id);
/** Get ENU coordinates (float)
* @param wp_id waypoint id
* @return pointer to waypoint ENU (float) coordinates, NULL if invalid
*/
extern struct EnuCoor_f *waypoint_get_enu_f(uint8_t wp_id);
/** Get ENU coordinates (integer)
* @param wp_id waypoint id
* @return pointer to waypoint ENU (integer) coordinates, NULL if invalid
*/
extern struct EnuCoor_i *waypoint_get_enu_i(uint8_t wp_id);
/*
* Set waypoint coordinates.