fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 21:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Navigator: differentiate between takeoff and reposition commands, perform calculations for repositions only when armed.

Browse Source
This commit is contained in:
Lorenz Meier
2016-05-10 13:58:42 +02:00
parent befab7303f
commit 8960ab3402
6 changed files with 119 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+23 -3
View File
@@ -129,6 +129,7 @@ Navigator::Navigator() :
_nav_caps{},
_pos_sp_triplet{},
_reposition_triplet{},
_takeoff_triplet{},
_mission_result{},
_att_sp{},
_mission_item_valid(false),
@@ -415,7 +416,7 @@ Navigator::task_main()
struct position_setpoint_triplet_s *rep = get_reposition_triplet();
// store current position as previous position and goal as next
rep->previous.yaw = NAN;
rep->previous.yaw = get_global_position()->yaw;
rep->previous.lat = get_global_position()->lat;
rep->previous.lon = get_global_position()->lon;
rep->previous.alt = get_global_position()->alt;
@@ -448,9 +449,28 @@ Navigator::task_main()
rep->previous.valid = true;
rep->current.valid = true;
rep->next.valid = false;
}
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
struct position_setpoint_triplet_s *rep = get_takeoff_triplet();
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) {
// store current position as previous position and goal as next
rep->previous.yaw = get_global_position()->yaw;
rep->previous.lat = get_global_position()->lat;
rep->previous.lon = get_global_position()->lon;
rep->previous.alt = get_global_position()->alt;
rep->current.loiter_radius = get_loiter_radius();
rep->current.loiter_direction = 1;
rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
rep->current.yaw = cmd.param4;
rep->current.lat = cmd.param5 / (double)1e7;
rep->current.lon = cmd.param6 / (double)1e7;
rep->current.alt = cmd.param7;
rep->previous.valid = true;
rep->current.valid = true;
rep->next.valid = false;
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) {
warnx("navigator: got pause/continue command");
}
}