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 30 Packages Projects Releases Wiki Activity

[nav] add an option for takeoff height (#3462)

Browse Source 
Add a parameter to the takeoff function to specify the height at which the takeoff is considered finished.
A negative value tells that the default value (configurable from airframe file) should be used.
Update demo FP
This commit is contained in:
Gautier Hattenberger
2025-06-04 22:41:41 +02:00
committed by GitHub
parent 12586a6811
commit 06a0542090
5 changed files with 53 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/flight_plans/fixedwing_demo_takeoff_land.xml
+1 -1
View File
@@ -36,7 +36,7 @@
</block>
<block name="Takeoff (Here)" strip_button="Takeoff (Here)" group="takeoff">
<set value="0" var="autopilot.flight_time"/>
<call fun="nav_takeoff_from_here()"/>
<call fun="nav_takeoff_from_here(-1.)"/>
<deroute block="Standby"/>
</block>
<block key="Ctrl+a" name="Standby" strip_button="Standby" strip_icon="home.png" group="home">
conf/flight_plans/rotorcraft_demo_takeoff_land.xml
+1 -1
View File
@@ -35,7 +35,7 @@
<deroute block="Standby"/>
</block>
<block name="Takeoff (Here)" strip_button="Takeoff (Here)" group="takeoff">
<call fun="nav_takeoff_from_here()"/>
<call fun="nav_takeoff_from_here(-1.)"/>
<deroute block="Standby"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
sw/airborne/modules/nav/nav_takeoff_and_landing.h
+6 -3
View File
@@ -86,9 +86,10 @@ extern void nav_takeoff_and_landing_periodic(void);
* - rotorcraft: set the wp at current location and climb verticaly
*
* @param[in] wp_id waypoint ID
* @param[in] height height in meters for ending takeoff procedure (<0 to use default value)
* @return true until procedure is completed
*/
extern bool nav_takeoff_from_wp(uint8_t wp_id);
extern bool nav_takeoff_from_wp(uint8_t wp_id, float height);
/** Takeoff from lat long location
*
@@ -97,18 +98,20 @@ extern bool nav_takeoff_from_wp(uint8_t wp_id);
*
* @param[in] lat takeoff latitude (deg)
* @param[in] lon takeoff longitude (deg)
* @param[in] height height in meters for ending takeoff procedure (<0 to use default value)
* @return true until procedure is completed
*/
extern bool nav_takeoff_from_loc(float lat, float lon);
extern bool nav_takeoff_from_loc(float lat, float lon, float height);
/** Takeoff from current location
*
* - fixedwing: climb direction is specified with setting
* - rotorcraft: start from current location and climb verticaly
*
* @param[in] height height in meters for ending takeoff procedure (<0 to use default value)
* @return true until procedure is completed
*/
extern bool nav_takeoff_from_here(void);
extern bool nav_takeoff_from_here(float height);
/** Land at waypoint location
*
sw/airborne/modules/nav/nav_takeoff_and_landing_fw.c
+27 -12
View File
@@ -76,9 +76,10 @@ float nav_takeoff_direction;
static bool nav_takeoff_mission(uint8_t nb UNUSED, float *params UNUSED, enum MissionRunFlag flag)
{
if (flag == MissionInit) {
if (flag == MissionInit && nb == 1) {
float height = params[0];
takeoff.status = NAV_TAKEOFF_INIT;
return nav_takeoff_from_here();
return nav_takeoff_from_here(height);
}
else if (flag == MissionRun) {
return nav_takeoff_run();
@@ -175,7 +176,7 @@ static bool nav_takeoff_run(void) {
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)) {
|| (stateGetPositionEnu_f()->z - takeoff.start_pos.z > takeoff.climb_pos.z)) {
// end when climb point or target alt is reached
takeoff.status = NAV_TAKEOFF_DONE;
}
@@ -188,39 +189,53 @@ static bool nav_takeoff_run(void) {
return true;
}
bool nav_takeoff_from_wp(uint8_t wp_id)
bool nav_takeoff_from_wp(uint8_t wp_id, float height)
{
if (takeoff.status == NAV_TAKEOFF_INIT) {
takeoff.start_pos = *stateGetPositionEnu_f();
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();
if (height < 0.f) {
takeoff.climb_pos.z = takeoff.start_pos.z + NAV_TAKEOFF_HEIGHT;
} else {
takeoff.climb_pos.z = takeoff.start_pos.z + height;
}
}
return nav_takeoff_run();
}
bool nav_takeoff_from_loc(float lat, float lon)
bool nav_takeoff_from_loc(float lat, float lon, float height)
{
if (takeoff.status == NAV_TAKEOFF_INIT) {
struct LlaCoor_f lla = { RadOfDeg(lat), RadOfDeg(lon), stateGetPositionLla_f()->alt + NAV_TAKEOFF_HEIGHT };
takeoff.start_pos = *stateGetPositionEnu_f();
float alt = stateGetPositionLla_f()->alt;
if (height < 0.f) {
alt += NAV_TAKEOFF_HEIGHT;
} else {
alt += height;
}
struct LlaCoor_f lla = { RadOfDeg(lat), RadOfDeg(lon), alt };
struct UtmCoor_f utm;
utm_of_lla_f(&utm, &lla);
ENU_OF_UTM_DIFF(takeoff.climb_pos, utm, *stateGetUtmOrigin_f());
}
return nav_takeoff_from_here();
return nav_takeoff_run();
}
bool nav_takeoff_from_here(void)
bool nav_takeoff_from_here(float height)
{
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;
if (height < 0.f) {
takeoff.climb_pos.z = takeoff.start_pos.z + NAV_TAKEOFF_HEIGHT;
} else {
takeoff.climb_pos.z = takeoff.start_pos.z + height;
}
}
return nav_takeoff_run();
sw/airborne/modules/nav/nav_takeoff_and_landing_rotorcraft.c
+18 -7
View File
@@ -64,9 +64,10 @@ float nav_takeoff_direction;
static bool nav_takeoff_mission(uint8_t nb UNUSED, float *params UNUSED, enum MissionRunFlag flag)
{
if (flag == MissionInit) {
if (flag == MissionInit && nb == 1) {
float height = params[0];
takeoff.status = NAV_TAKEOFF_INIT;
return nav_takeoff_from_here();
return nav_takeoff_from_here(height);
}
else if (flag == MissionRun) {
return nav_takeoff_run();
@@ -157,7 +158,7 @@ static bool nav_takeoff_run(void) {
autopilot_set_in_flight(true);
NavGotoWaypoint(takeoff.climb_id);
NavVerticalClimbMode(NAV_TAKEOFF_CLIMB_SPEED);
if (stateGetPositionEnu_f()->z - takeoff.start_pos.z > NAV_TAKEOFF_HEIGHT) {
if (stateGetPositionEnu_f()->z - takeoff.start_pos.z > takeoff.climb_pos.z) {
// end when takeoff height is reached
takeoff.status = NAV_TAKEOFF_DONE;
}
@@ -171,29 +172,39 @@ static bool nav_takeoff_run(void) {
return true;
}
bool nav_takeoff_from_wp(uint8_t wp_id)
bool nav_takeoff_from_wp(uint8_t wp_id, float height)
{
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();
if (height < 0.f) {
takeoff.climb_pos.z = takeoff.start_pos.z + NAV_TAKEOFF_HEIGHT; // default value
} else {
takeoff.climb_pos.z = takeoff.start_pos.z + height;
}
}
return nav_takeoff_run();
}
bool nav_takeoff_from_loc(float lat UNUSED, float lon UNUSED)
bool nav_takeoff_from_loc(float lat UNUSED, float lon UNUSED, float height)
{
return nav_takeoff_from_here();
return nav_takeoff_from_here(height);
}
bool nav_takeoff_from_here(void)
bool nav_takeoff_from_here(float height)
{
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;
if (height < 0.f) {
takeoff.climb_pos.z += NAV_TAKEOFF_HEIGHT; // default value
} else {
takeoff.climb_pos.z += height;
}
waypoint_set_enu(0, &takeoff.climb_pos);
}