void AP_Avoidance::handle_msg(const mavlink_message_t &msg)
{
if (!check_startup()) {
// avoidance is not active / allocated
return;
}
if (msg.msgid != MAVLINK_MSG_ID_GLOBAL_POSITION_INT) {
// we only take position from GLOBAL_POSITION_INT
return;
}
if (msg.sysid == mavlink_system.sysid) {
// we do not obstruct ourselves....
return;
}
// inform AP_Avoidance we have a new player
mavlink_global_position_int_t packet;
mavlink_msg_global_position_int_decode(&msg, &packet);
Location loc;
loc.lat = packet.lat;
loc.lng = packet.lon;
loc.alt = packet.alt / 10; // mm -> cm
loc.flags.relative_alt = false;
Vector3f vel = Vector3f(packet.vx/100.0f, // cm to m
packet.vy/100.0f,
packet.vz/100.0f);
add_obstacle(AP_HAL::millis(),
MAV_COLLISION_SRC_ADSB,
msg.sysid,
loc,
vel);
}
// vel is north/east/down!
void AP_Avoidance::add_obstacle(const uint32_t obstacle_timestamp_ms,
const MAV_COLLISION_SRC src,
const uint32_t src_id,
const Location &loc,
const Vector3f &vel_ned)
{
if (! check_startup()) {
return;
}
uint32_t oldest_timestamp = std::numeric_limits<uint32_t>::max();
uint8_t oldest_index = 255; // avoid compiler warning with initialisation
int16_t index = -1;
uint8_t i;
for (i=0; i<_obstacle_count; i++) {
if (_obstacles[i].src_id == src_id &&
_obstacles[i].src == src) {
// pre-existing obstacle found; we will update its information
index = i;
break;
}
if (_obstacles[i].timestamp_ms < oldest_timestamp) {
oldest_timestamp = _obstacles[i].timestamp_ms;
oldest_index = i;
}
}
WITH_SEMAPHORE(_rsem);
if (index == -1) {
// existing obstacle not found. See if we can store it anyway:
if (i <_obstacles_allocated) {
// have room to store more vehicles...
index = _obstacle_count++;
} else if (oldest_timestamp < obstacle_timestamp_ms) {
// replace this very old entry with this new data
index = oldest_index;
} else {
// no room for this (old?!) data
return;
}
_obstacles[index].src = src;
_obstacles[index].src_id = src_id;
}
_obstacles[index]._location = loc;
_obstacles[index]._velocity = vel_ned;
_obstacles[index].timestamp_ms = obstacle_timestamp_ms;
}
void AP_Avoidance::update()
{
if (!check_startup()) {
return;
}
if (_adsb.enabled()) {
get_adsb_samples();
}
check_for_threats();
// notify GCS of most serious thread
handle_threat_gcs_notify(most_serious_threat());
// avoid object (if necessary)
handle_avoidance_local(most_serious_threat());
}
static void handler_Switch3(s32 data) { check_startup(); }
