dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

update loitering coordinates when 183 mission item is lastone

This commit is contained in:
fpvaspassion 2015-08-13 21:39:50 +03:00 committed by Lorenz Meier
parent 60be6a577f
commit 158c260c43
1 changed files with 11 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/navigator/mission_block.cpp
View File

@ -75,10 +75,13 @@ MissionBlock::is_mission_item_reached()
{
if (_mission_item.nav_cmd == NAV_CMD_DO_SET_SERVO) {
/* Request for new PWM value for selected channel */
_actuators.control[_mission_item.actuator_num] = 1.0f / 2000 * -_mission_item.actuator_value;
_actuators.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators);
warnx("Set servo cmd executed");
/* Warn about action */
warnx("Set servo cmd executed");
return true;
}
@ -204,6 +207,13 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
sp->pitch_min = item->pitch_min;
switch (item->nav_cmd) {
case NAV_CMD_DO_SET_SERVO:
/* Set current position for loitering set point*/
sp->lat = _navigator->get_global_position()->lat;
sp->lon = _navigator->get_global_position()->lon;
sp->alt = _navigator->get_global_position()->alt;
sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
break;
case NAV_CMD_IDLE:
sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE;
break;