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

CollisionPrevention: add failsafe for stale distance data

This commit is contained in:
Martina Rivizzigno 2019-06-25 09:42:17 +02:00 committed by Beat Küng
parent 0ee770e853
commit 8637a9255e
2 changed files with 37 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
src/lib/CollisionPrevention/CollisionPrevention.cpp
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);
}
}

7
src/lib/CollisionPrevention/CollisionPrevention.hpp
View File

@ -48,11 +48,13 @@
#include <uORB/topics/obstacle_distance.h>
#include <uORB/topics/collision_constraints.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_command.h>
#include <mathlib/mathlib.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <systemlib/mavlink_log.h>
#include <commander/px4_custom_mode.h>
class CollisionPrevention : public ModuleParams
{
@ -73,15 +75,13 @@ private:
orb_advert_t _constraints_pub{nullptr}; /**< constraints publication */
orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */
orb_advert_t _obstacle_distance_pub{nullptr}; /**< obstacle_distance publication */
orb_advert_t _pub_vehicle_command{nullptr}; /**< vehicle command do publication */
uORB::SubscriptionData<obstacle_distance_s> _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */
uORB::Subscription _sub_distance_sensor[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}}; /**< distance data received from onboard rangefinders */
uORB::SubscriptionData<vehicle_attitude_s> _sub_vehicle_attitude{ORB_ID(vehicle_attitude)};
static constexpr uint64_t RANGE_STREAM_TIMEOUT_US{500000};
static constexpr uint64_t MESSAGE_THROTTLE_US{5000000};
hrt_abstime _last_message{0};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_COL_PREV_D>) _param_mpc_col_prev_d, /**< collision prevention keep minimum distance */
@ -125,5 +125,6 @@ private:
void updateOffboardObstacleDistance(obstacle_distance_s &obstacle);
void updateDistanceSensor(obstacle_distance_s &obstacle);
void _publishVehicleCmdDoLoiter();
};