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

ROI - move handling to navigator (#7939)

Browse Source
This commit is contained in:
Daniel Agar
2017-09-26 12:25:02 -04:00
committed by GitHub
parent e72769c924
commit 3c18be387c
24 changed files with 301 additions and 291 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+17 -20
View File
@@ -145,13 +145,12 @@ void
Mission::on_inactivation()
{
// Disable camera trigger
vehicle_command_s cmd{};
vehicle_command_s cmd = {};
cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL;
// Pause trigger
cmd.param1 = -1.0f;
cmd.param3 = 1.0f;
cmd.timestamp = hrt_absolute_time();
_navigator->publish_vehicle_cmd(cmd);
_navigator->publish_vehicle_cmd(&cmd);
}
void
@@ -160,13 +159,12 @@ Mission::on_activation()
set_mission_items();
// unpause triggering if it was paused
vehicle_command_s cmd{};
vehicle_command_s cmd = {};
cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL;
// unpause trigger
cmd.param1 = -1.0f;
cmd.param3 = 0.0f;
cmd.timestamp = hrt_absolute_time();
_navigator->publish_vehicle_cmd(cmd);
_navigator->publish_vehicle_cmd(&cmd);
}
void
@@ -510,7 +508,7 @@ Mission::set_mission_items()
/* update position setpoint triplet */
pos_sp_triplet->previous.valid = false;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
/* reuse setpoint for LOITER only if it's not IDLE */
@@ -542,7 +540,7 @@ Mission::set_mission_items()
/*********************************** handle mission item *********************************************/
/* handle position mission items */
if (item_contains_position(&_mission_item)) {
if (item_contains_position(_mission_item)) {
/* force vtol land */
if (_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get() == 1
@@ -766,7 +764,7 @@ Mission::set_mission_items()
set_align_mission_item(&_mission_item, &mission_item_next_position);
/* set position setpoint to target during the transition */
mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->current);
mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->current);
}
/* yaw is aligned now */
@@ -808,10 +806,10 @@ Mission::set_mission_items()
/*********************************** set setpoints and check next *********************************************/
/* set current position setpoint from mission item (is protected against non-position items) */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
/* issue command if ready (will do nothing for position mission items) */
issue_command(&_mission_item);
issue_command(_mission_item);
/* set current work item type */
_work_item_type = new_work_item_type;
@@ -834,7 +832,7 @@ Mission::set_mission_items()
/* try to process next mission item */
if (has_next_position_item) {
/* got next mission item, update setpoint triplet */
mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->next);
mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->next);
} else {
/* next mission item is not available */
@@ -1025,10 +1023,10 @@ Mission::heading_sp_update()
point_to_latlon[1] = _navigator->get_home_position()->lon;
} else if (_param_yawmode.get() == MISSION_YAWMODE_TO_ROI
&& _navigator->get_vroi()->mode == vehicle_roi_s::VEHICLE_ROI_LOCATION) {
&& _navigator->get_vroi().mode == vehicle_roi_s::ROI_LOCATION) {
/* target location is ROI */
point_to_latlon[0] = _navigator->get_vroi()->lat;
point_to_latlon[1] = _navigator->get_vroi()->lon;
point_to_latlon[0] = _navigator->get_vroi().lat;
point_to_latlon[1] = _navigator->get_vroi().lon;
} else {
/* target location is next (current) waypoint */
@@ -1185,7 +1183,7 @@ Mission::do_abort_landing()
_mission_item.origin = ORIGIN_ONBOARD;
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
_navigator->set_position_setpoint_triplet_updated();
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "Holding at %dm above landing.",
@@ -1204,8 +1202,7 @@ Mission::do_abort_landing()
// send reposition cmd to get out of mission
vehicle_command_s vcmd = {};
mission_item_to_vehicle_command(&_mission_item, &vcmd);
vcmd.timestamp = hrt_absolute_time();
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION;
vcmd.param1 = -1;
vcmd.param2 = 1;
@@ -1213,7 +1210,7 @@ Mission::do_abort_landing()
vcmd.param6 = _mission_item.lon;
vcmd.param7 = alt_sp;
_navigator->publish_vehicle_cmd(vcmd);
_navigator->publish_vehicle_cmd(&vcmd);
}
bool
@@ -1230,7 +1227,7 @@ Mission::prepare_mission_items(bool onboard, struct mission_item_s *mission_item
/* trying to find next position mission item */
while (read_mission_item(onboard, offset, next_position_mission_item)) {
if (item_contains_position(next_position_mission_item)) {
if (item_contains_position(*next_position_mission_item)) {
*has_next_position_item = true;
break;
}