From 83e76ece1cdbc27f6972c528690b27c6b67d4790 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 5 Feb 2019 11:08:38 -0500
Subject: [PATCH] navigator mission block fix get_time_inside() and cleanup
helpers
---
src/modules/navigator/mission_block.cpp | 14 +++++++++-----
src/modules/navigator/mission_block.h | 6 +++---
2 files changed, 12 insertions(+), 8 deletions(-)
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index acedbcd6f8..ff5eb2d351 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -477,10 +477,14 @@ MissionBlock::issue_command(const mission_item_s &item)
}
float
-MissionBlock::get_time_inside(const struct mission_item_s &item)
+MissionBlock::get_time_inside(const mission_item_s &item) const
{
- if (item.nav_cmd != NAV_CMD_TAKEOFF) {
- return item.time_inside;
+ if ((item.nav_cmd == NAV_CMD_WAYPOINT && _navigator->get_vstatus()->is_rotary_wing) ||
+ item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+ item.nav_cmd == NAV_CMD_DELAY) {
+
+ // a negative time inside would be invalid
+ return math::max(item.time_inside, 0.0f);
}
return 0.0f;
@@ -510,7 +514,7 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
sp->lat = item.lat;
sp->lon = item.lon;
- sp->alt = item.altitude_is_relative ? item.altitude + _navigator->get_home_position()->alt : item.altitude;
+ sp->alt = get_absolute_altitude_for_item(item);
sp->yaw = item.yaw;
sp->yaw_valid = PX4_ISFINITE(item.yaw);
sp->loiter_radius = (fabsf(item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item.loiter_radius) :
@@ -736,7 +740,7 @@ MissionBlock::mission_apply_limitation(mission_item_s &item)
}
float
-MissionBlock::get_absolute_altitude_for_item(struct mission_item_s &mission_item) const
+MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item) const
{
if (mission_item.altitude_is_relative) {
return mission_item.altitude + _navigator->get_home_position()->alt;
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
index 052b805986..9561b85ffc 100644
--- a/src/modules/navigator/mission_block.h
+++ b/src/modules/navigator/mission_block.h
@@ -115,13 +115,13 @@ protected:
/**
* General function used to adjust the mission item based on vehicle specific limitations
*/
- void mission_apply_limitation(mission_item_s &item);
+ void mission_apply_limitation(mission_item_s &item);
void issue_command(const mission_item_s &item);
- float get_time_inside(const struct mission_item_s &item);
+ float get_time_inside(const mission_item_s &item) const ;
- float get_absolute_altitude_for_item(struct mission_item_s &mission_item) const;
+ float get_absolute_altitude_for_item(const mission_item_s &mission_item) const;
mission_item_s _mission_item{};
--
GitLab