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
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;

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
*/
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{};