yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Navigator: execute on reposition commands

Browse Source
This commit is contained in:
Lorenz Meier
2016-04-24 11:54:20 +02:00
parent 711053fd76
commit 2244ff167f
2 changed files with 38 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/navigator/loiter.cpp
View File

@@ -58,7 +58,7 @@ Loiter::Loiter(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_param_min_alt(this, "MIS_LTRMIN_ALT", false)
{
/* load initial params */
// load initial params
updateParams();
}
@@ -77,10 +77,10 @@ Loiter::on_activation()
if (_navigator->get_reposition_triplet()->current.valid) {
reposition();
} else {
/* set current mission item to loiter */
// set current mission item to loiter
set_loiter_item(&_mission_item, _param_min_alt.get());
/* convert mission item to current setpoint */
// convert mission item to current setpoint
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->current.velocity_valid = false;
pos_sp_triplet->previous.valid = false;
@@ -110,6 +110,17 @@ Loiter::reposition()
if (rep->current.valid) {
// set loiter position based on reposition command
// convert mission item to current setpoint
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->current.velocity_valid = false;
memcpy(&pos_sp_triplet->previous, &rep->previous, sizeof(rep->previous));
memcpy(&pos_sp_triplet->current, &rep->current, sizeof(rep->current));
pos_sp_triplet->next.valid = false;
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
_navigator->set_position_setpoint_triplet_updated();
// mark this as done
memset(rep, 0, sizeof(*rep));
}

29
src/modules/navigator/navigator_main.cpp
View File

@@ -409,7 +409,6 @@ Navigator::task_main()
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {
warnx("navigator: got reposition command");
struct position_setpoint_triplet_s *rep = get_reposition_triplet();
@@ -419,10 +418,30 @@ Navigator::task_main()
rep->previous.lon = get_global_position()->lon;
rep->previous.alt = get_global_position()->alt;
rep->current.yaw = cmd.param4;
rep->current.lat = cmd.param5;
rep->current.lon = cmd.param6;
rep->current.alt = cmd.param7;
// Go on and check which changes had been requested
if (isfinite(cmd.param4)) {
rep->current.yaw = cmd.param4;
} else {
rep->current.yaw = NAN;
}
if (isfinite(cmd.param5)) {
rep->current.lat = cmd.param5 / (double)1e7;
} else {
rep->current.lat = get_global_position()->lat;
}
if (isfinite(cmd.param6)) {
rep->current.lon = cmd.param6 / (double)1e7;
} else {
rep->current.lon = get_global_position()->lon;
}
if (isfinite(cmd.param7)) {
rep->current.alt = cmd.param7;
} else {
rep->current.alt = get_global_position()->alt;
}
rep->previous.valid = true;
rep->current.valid = true;