fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mission_block: handle SET_ROI_LOCATION with absolute altitude correctly (#19258)

Browse Source
This commit is contained in:
Matthias Grob 2022-03-03 13:25:00 +01:00 committed by GitHub
parent 49df00c319
commit 666cf2326d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 8 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -538,16 +538,18 @@ MissionBlock::issue_command(const mission_item_s &item)
vcmd.param2 = item.params[1];
vcmd.param3 = item.params[2];
vcmd.param4 = item.params[3];
vcmd.param5 = static_cast<double>(item.params[4]);
vcmd.param6 = static_cast<double>(item.params[5]);
vcmd.param7 = item.params[6];
if (item.nav_cmd == NAV_CMD_DO_SET_ROI_LOCATION && item.altitude_is_relative) {
if (item.nav_cmd == NAV_CMD_DO_SET_ROI_LOCATION) {
// We need to send out the ROI location that was parsed potentially with double precision to lat/lon because mission item parameters 5 and 6 only have float precision
vcmd.param5 = item.lat;
vcmd.param6 = item.lon;
vcmd.param7 = item.altitude + _navigator->get_home_position()->alt;
} else {
vcmd.param5 = (double)item.params[4];
vcmd.param6 = (double)item.params[5];
vcmd.param7 = item.params[6];
if (item.altitude_is_relative) {
vcmd.param7 = item.altitude + _navigator->get_home_position()->alt;
}
}
_navigator->publish_vehicle_cmd(&vcmd);