fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-29 12:33:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

CollisionPrevention: add failsafe for stale distance data

Browse Source
This commit is contained in:
Martina Rivizzigno
2019-07-15 10:58:00 +02:00
committed by Beat Küng
parent 0ee770e853
commit 8637a9255e
2 changed files with 37 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/CollisionPrevention/CollisionPrevention.cpp
+33 -3
View File
@@ -265,9 +265,14 @@ void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint,
}
}
} else if (_last_message + MESSAGE_THROTTLE_US < hrt_absolute_time()) {
mavlink_log_critical(&_mavlink_log_pub, "No range data received");
_last_message = hrt_absolute_time();
} else {
// if distance data are stale, switch to Loiter and disable Collision Prevention
// such that it is still possible to fly in Position Control Mode
_publishVehicleCmdDoLoiter();
mavlink_log_critical(&_mavlink_log_pub, "No range data received, loitering.");
float col_prev_d = -1.f;
param_set(param_find("MPC_COL_PREV_D"), &col_prev_d);
mavlink_log_critical(&_mavlink_log_pub, "Collision Prevention disabled.");
}
}
@@ -292,3 +297,28 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa
publishConstrainedSetpoint(original_setpoint, new_setpoint);
original_setpoint = new_setpoint;
}
void CollisionPrevention::_publishVehicleCmdDoLoiter()
{
vehicle_command_s command{};
command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
command.param1 = (float)1; // base mode
command.param3 = (float)0; // sub mode
command.target_system = 1;
command.target_component = 1;
command.source_system = 1;
command.source_component = 1;
command.confirmation = false;
command.from_external = false;
command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO;
command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
// publish the vehicle command
if (_pub_vehicle_command == nullptr) {
_pub_vehicle_command = orb_advertise_queue(ORB_ID(vehicle_command), &command,
vehicle_command_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command), _pub_vehicle_command, &command);
}
}