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(); }