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: NAV_CMD_DO_VTOL_TRANSITION: accept once in correct vtol state

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-01-06 18:50:32 +01:00
parent 5579a1d789
commit 2c6b3eeb02
2 changed files with 7 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -112,18 +112,16 @@ MissionBlock::is_mission_item_reached()
case NAV_CMD_DO_VTOL_TRANSITION: case NAV_CMD_DO_VTOL_TRANSITION:
/* if (int(_mission_item.params[0]) == 3) {
* We wait half a second to give the transition command time to propagate. // transition to RW requested, only accept waypoint if vehicle state has changed accordingly
* Then monitor the transition status for completion. return _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
*/
// TODO: check desired transition state achieved and drop _action_start
if (hrt_absolute_time() - _action_start > 500000 &&
!_navigator->get_vstatus()->in_transition_mode) {
_action_start = 0; } else if (int(_mission_item.params[0]) == 4) {
return true; // transition to FW requested, only accept waypoint if vehicle state has changed accordingly
return _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
} else { } else {
// invalid vtol transition request
return false; return false;
} }
@@ -473,7 +471,6 @@ MissionBlock::issue_command(const mission_item_s &item)
_actuator_pub.publish(actuators); _actuator_pub.publish(actuators);
} else { } else {
_action_start = hrt_absolute_time();
// This is to support legacy DO_MOUNT_CONTROL as part of a mission. // This is to support legacy DO_MOUNT_CONTROL as part of a mission.
if (item.nav_cmd == NAV_CMD_DO_MOUNT_CONTROL) { if (item.nav_cmd == NAV_CMD_DO_MOUNT_CONTROL) {

1
src/modules/navigator/mission_block.h
View File

@@ -149,7 +149,6 @@ protected:
bool _waypoint_position_reached{false}; bool _waypoint_position_reached{false};
bool _waypoint_yaw_reached{false}; bool _waypoint_yaw_reached{false};
hrt_abstime _action_start{0};
hrt_abstime _time_wp_reached{0}; hrt_abstime _time_wp_reached{0};
uORB::Publication<actuator_controls_s> _actuator_pub{ORB_ID(actuator_controls_2)}; uORB::Publication<actuator_controls_s> _actuator_pub{ORB_ID(actuator_controls_2)};