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

read current and next mission item at the same time, issue item commands while processing items, guarding setpoint from non-position items

Browse Source
This commit is contained in:
Andreas Antener
2016-02-08 11:09:10 +01:00
parent 9e0c636b97
commit 93bc018c93
4 changed files with 116 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+47 -12
View File
@@ -67,6 +67,7 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
_waypoint_position_reached(false),
_waypoint_yaw_reached(false),
_time_first_inside_orbit(0),
_action_start(0),
_actuators{},
_actuator_pub(nullptr),
_cmd_pub(nullptr)
@@ -82,6 +83,7 @@ MissionBlock::is_mission_item_reached()
{
/* handle non-navigation or indefinite waypoints */
switch (_mission_item.nav_cmd) {
// XXX: move handling to mission as well
case NAV_CMD_DO_SET_SERVO: {
memset(&actuators, 0, sizeof(actuators));
actuators.control[_mission_item.actuator_num] = 1.0f / 2000 * -_mission_item.actuator_value;
@@ -105,18 +107,13 @@ MissionBlock::is_mission_item_reached()
case vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL: /* fallthrough */
case vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION:
{
/* forward the command to other processes */
warnx("got instantaneous command, forwarding.\n");
struct vehicle_command_s cmd = {};
cmd.command = _mission_item.nav_cmd;
mission_item_to_vehicle_command(&_mission_item, &cmd);
if (_cmd_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd);
// XXX: we should wait on command ACK or status change instead
// currently we just wait so the command be processed
if (hrt_absolute_time() - _action_start > 1000) {
return true;
} else {
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
}
return true;
return false;
}
default:
@@ -238,6 +235,7 @@ MissionBlock::mission_item_to_vehicle_command(const struct mission_item_s *item,
cmd->param5 = item->params[4];
cmd->param6 = item->params[5];
cmd->param7 = item->params[6];
cmd->command = item->nav_cmd;
cmd->target_system = _navigator->get_vstatus()->system_id;
cmd->target_component = _navigator->get_vstatus()->component_id;
@@ -246,9 +244,46 @@ MissionBlock::mission_item_to_vehicle_command(const struct mission_item_s *item,
cmd->confirmation = false;
}
void
MissionBlock::issue_command(const struct mission_item_s *item)
{
if (item_contains_position(item)) {
return;
}
warnx("forwarding command.\n");
struct vehicle_command_s cmd = {};
mission_item_to_vehicle_command(item, &cmd);
_action_start = hrt_absolute_time();
if (_cmd_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd);
} else {
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
}
}
bool
MissionBlock::item_contains_position(const struct mission_item_s *item)
{
// XXX: maybe extend that check onto item properties
if (item->nav_cmd == vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL ||
item->nav_cmd == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) {
return false;
}
return true;
}
void
MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp)
{
/* don't change the setpoint for non-position items */
if (!item_contains_position(item)) {
return;
}
sp->valid = true;
sp->lat = item->lat;
sp->lon = item->lon;
@@ -261,7 +296,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
sp->acceptance_radius = item->acceptance_radius;
switch (item->nav_cmd) {
case NAV_CMD_DO_SET_SERVO:
case NAV_CMD_DO_SET_SERVO: // XXX: should be also return in the beginning for this?
/* Set current position for loitering set point*/
sp->lat = _navigator->get_global_position()->lat;
sp->lon = _navigator->get_global_position()->lon;