Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

OA Failsafe improvement #11187

Merged
merged 3 commits into from
Jan 16, 2019
Merged

OA Failsafe improvement #11187

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
9d909f8
mc_pos_control: refactor OA setpoints out of PositionControl calls
MaEtUgR Jan 13, 2019
2efc050
mc_pos_control: add Loiter/Hold mode to possible failsafe commands
MaEtUgR Jan 13, 2019
56161a4
mc_pos_control: change use avoidance to handle data loss during mission
MaEtUgR Jan 13, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
35 changes: 25 additions & 10 deletions src/modules/mc_pos_control/mc_pos_control_main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -750,15 +750,15 @@ MulticopterPositionControl::run()
limit_altitude(setpoint);
}

// Update states, setpoints and constraints.
_control.updateConstraints(constraints);
_control.updateState(_states);

// adjust setpoints based on avoidance
if (use_obstacle_avoidance()) {
execute_avoidance_waypoint(setpoint);
}

// Update states, setpoints and constraints.
_control.updateConstraints(constraints);
_control.updateState(_states);

// update position controller setpoints
if (!_control.updateSetpoint(setpoint)) {
warn_rate_limited("Position-Control Setpoint-Update failed");
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -1306,6 +1316,11 @@ void MulticopterPositionControl::send_vehicle_cmd_do(uint8_t nav_state)
command.param2 = (float)PX4_CUSTOM_MAIN_MODE_ALTCTL;
break;

case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO;
command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
break;

default: //vehicle_status_s::NAVIGATION_STATE_POSCTL
command.param2 = (float)PX4_CUSTOM_MAIN_MODE_POSCTL;
break;
Expand Down