fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 14: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/navigator_main.cpp
+119 -45
View File
@@ -64,6 +64,7 @@
#include <uORB/topics/home_position.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/uORB.h>
@@ -213,12 +214,6 @@ Navigator::params_update()
}
}
void
Navigator::vehicle_roi_update()
{
orb_copy(ORB_ID(vehicle_roi), _vehicle_roi_sub, &_vroi);
}
void
Navigator::task_main_trampoline(int argc, char *argv[])
{
@@ -252,7 +247,6 @@ Navigator::task_main()
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
_vehicle_roi_sub = orb_subscribe(ORB_ID(vehicle_roi));
/* copy all topics first time */
vehicle_status_update();
@@ -264,7 +258,6 @@ Navigator::task_main()
home_position_update(true);
fw_pos_ctrl_status_update(true);
params_update();
vehicle_roi_update();
/* wakeup source(s) */
px4_pollfd_struct_t fds[1] = {};
@@ -365,13 +358,6 @@ Navigator::task_main()
home_position_update();
}
/* ROI updated */
orb_check(_vehicle_roi_sub, &updated);
if (updated) {
vehicle_roi_update();
}
/* vehicle_command updated */
orb_check(_vehicle_command_sub, &updated);
@@ -379,7 +365,13 @@ Navigator::task_main()
vehicle_command_s cmd;
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) {
// DO_GO_AROUND is currently handled by the position controller (unacknowledged)
// TODO: move DO_GO_AROUND handling to navigator
publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {
struct position_setpoint_triplet_s *rep = get_reposition_triplet();
struct position_setpoint_triplet_s *curr = get_position_setpoint_triplet();
@@ -435,6 +427,8 @@ Navigator::task_main()
rep->current.valid = true;
rep->next.valid = false;
// CMD_DO_REPOSITION is acknowledged by commander
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
struct position_setpoint_triplet_s *rep = get_takeoff_triplet();
@@ -472,6 +466,8 @@ Navigator::task_main()
rep->current.valid = true;
rep->next.valid = false;
// CMD_NAV_TAKEOFF is acknowledged by commander
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_LAND_START) {
/* find NAV_CMD_DO_LAND_START in the mission and
@@ -480,30 +476,17 @@ Navigator::task_main()
int land_start = _mission.find_offboard_land_start();
if (land_start != -1) {
struct vehicle_command_s vcmd = {
.timestamp = hrt_absolute_time(),
.param5 = 0.0f,
.param6 = 0.0f,
.param1 = (float)land_start,
.param2 = 0.0f,
.param3 = 0.0f,
.param4 = 0.0f,
.param7 = 0.0f,
.command = vehicle_command_s::VEHICLE_CMD_MISSION_START,
.target_system = (uint8_t)get_vstatus()->system_id,
.target_component = (uint8_t)get_vstatus()->component_id,
.source_system = (uint8_t)get_vstatus()->system_id,
.source_component = (uint8_t)get_vstatus()->component_id
};
publish_vehicle_cmd(vcmd);
vehicle_command_s vcmd = {};
vcmd.param1 = land_start;
publish_vehicle_cmd(&vcmd);
} else {
PX4_WARN("planned landing not available");
}
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) {
publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) {
if (get_mission_result()->valid &&
PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0) && (cmd.param1 < _mission_result.seq_total)) {
@@ -526,6 +509,51 @@ Navigator::task_main()
set_cruising_throttle();
}
}
// TODO: handle responses for supported DO_CHANGE_SPEED options?
publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI) {
_vroi = {};
_vroi.mode = cmd.param1;
switch (_vroi.mode) {
case vehicle_command_s::VEHICLE_ROI_NONE:
break;
case vehicle_command_s::VEHICLE_ROI_WPNEXT:
// TODO: implement point toward next MISSION
break;
case vehicle_command_s::VEHICLE_ROI_WPINDEX:
_vroi.mission_seq = cmd.param2;
break;
case vehicle_command_s::VEHICLE_ROI_LOCATION:
_vroi.lat = cmd.param5;
_vroi.lon = cmd.param6;
_vroi.alt = cmd.param7;
break;
case vehicle_command_s::VEHICLE_ROI_TARGET:
_vroi.target_seq = cmd.param2;
break;
default:
_vroi.mode = vehicle_command_s::VEHICLE_ROI_NONE;
break;
}
_vroi.timestamp = hrt_absolute_time();
if (_vehicle_roi_pub != nullptr) {
orb_publish(ORB_ID(vehicle_roi), _vehicle_roi_pub, &_vroi);
} else {
_vehicle_roi_pub = orb_advertise(ORB_ID(vehicle_roi), &_vroi);
}
publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
}
}
@@ -979,17 +1007,6 @@ Navigator::publish_geofence_result()
}
}
void
Navigator::publish_vehicle_cmd(const struct vehicle_command_s &vcmd)
{
if (_vehicle_cmd_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, &vcmd);
} else {
_vehicle_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
}
}
void
Navigator::set_mission_failure(const char *reason)
{
@@ -999,3 +1016,60 @@ Navigator::set_mission_failure(const char *reason)
mavlink_log_critical(&_mavlink_log_pub, "%s", reason);
}
}
void
Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd)
{
vcmd->timestamp = hrt_absolute_time();
vcmd->source_system = _vstatus.system_id;
vcmd->source_component = _vstatus.component_id;
vcmd->target_system = _vstatus.system_id;
vcmd->confirmation = false;
vcmd->from_external = false;
// The camera commands are not processed on the autopilot but will be
// sent to the mavlink links to other components.
switch (vcmd->command) {
case NAV_CMD_IMAGE_START_CAPTURE:
case NAV_CMD_IMAGE_STOP_CAPTURE:
case NAV_CMD_VIDEO_START_CAPTURE:
case NAV_CMD_VIDEO_STOP_CAPTURE:
vcmd->target_component = 100; // MAV_COMP_ID_CAMERA
break;
default:
vcmd->target_component = _vstatus.component_id;
break;
}
if (_vehicle_cmd_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, vcmd);
} else {
_vehicle_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
}
}
void
Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result)
{
vehicle_command_ack_s command_ack = {};
command_ack.timestamp = hrt_absolute_time();
command_ack.command = cmd.command;
command_ack.target_system = cmd.source_system;
command_ack.target_component = cmd.source_component;
command_ack.from_external = cmd.from_external;
command_ack.result = result;
command_ack.result_param1 = 0;
command_ack.result_param2 = 0;
if (_vehicle_cmd_ack_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command_ack), _vehicle_cmd_ack_pub, &command_ack);
} else {
_vehicle_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
}
}