dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

navigator mission block fix get_time_inside() and cleanup helpers

This commit is contained in:
Daniel Agar 2019-02-05 11:08:38 -05:00
parent 3665bc59b8
commit 83e76ece1c
2 changed files with 12 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/navigator/mission_block.cpp
View File

@ -477,10 +477,14 @@ MissionBlock::issue_command(const mission_item_s &item)
} }
float 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) { if ((item.nav_cmd == NAV_CMD_WAYPOINT && _navigator->get_vstatus()->is_rotary_wing) ||
return item.time_inside; 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; return 0.0f;
@ -510,7 +514,7 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
sp->lat = item.lat; sp->lat = item.lat;
sp->lon = item.lon; 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 = item.yaw;
sp->yaw_valid = PX4_ISFINITE(item.yaw); sp->yaw_valid = PX4_ISFINITE(item.yaw);
sp->loiter_radius = (fabsf(item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item.loiter_radius) : 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 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) { if (mission_item.altitude_is_relative) {
return mission_item.altitude + _navigator->get_home_position()->alt; return mission_item.altitude + _navigator->get_home_position()->alt;

6
src/modules/navigator/mission_block.h
View File

@ -115,13 +115,13 @@ protected:
/** /**
* General function used to adjust the mission item based on vehicle specific limitations * 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); 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{}; mission_item_s _mission_item{};