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

mc_pos_control: change use avoidance to handle data loss during mission 

There were accidents because when missions lead through an obstacle
and it should be avoided but the setpoints from the external obstacle
avoidance module are suddenly missing the mission is continued into the
obstacle which results in a crash.
This commit is contained in:
Matthias Grob 2019-01-13 13:36:03 +01:00
parent a563092742
commit 7b9cfac767
1 changed files with 16 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -1211,12 +1211,22 @@ MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_setpoin
bool
MulticopterPositionControl::use_obstacle_avoidance()
{
/* check that external obstacle avoidance is sending data and that the first point is valid */
return (MPC_OBS_AVOID.get()
&& (hrt_elapsed_time((hrt_abstime *)&_traj_wp_avoidance.timestamp) < TRAJECTORY_STREAM_TIMEOUT_US)
&& (_traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid == true)
&& ((_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) ||
(_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL)));
if (MPC_OBS_AVOID.get()) {
const bool avoidance_data_timeout = hrt_elapsed_time((hrt_abstime *)&_traj_wp_avoidance.timestamp) > TRAJECTORY_STREAM_TIMEOUT_US;
const bool avoidance_point_valid = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid == true;
const bool in_mission = _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
const bool in_rtl = _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
// switch to hold mode to stop when we loose external avoidance data during a mission
if (avoidance_data_timeout && in_mission) {
send_vehicle_cmd_do(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
}
if ((in_mission || in_rtl) && !avoidance_data_timeout && avoidance_point_valid) {
return true;
}
}
return false;
}
void