/* * periodic update to handle vehicle timeouts and trigger collision detection */ void AP_ADSB::update(void) { if (!_enabled) { if (_vehicle_list != NULL) { deinit(); } // nothing to do return; } else if (_vehicle_list == NULL) { init(); return; } uint16_t index = 0; while (index < _vehicle_count) { // check list and drop stale vehicles. When disabled, the list will get flushed if (AP_HAL::millis() - _vehicle_list[index].last_update_ms > VEHICLE_TIMEOUT_MS) { // don't increment index, we want to check this same index again because the contents changed // also, if we're disabled then clear the list delete_vehicle(index); } else { index++; } } perform_threat_detection(); //hal.console->printf("ADSB: cnt %u, lowT %.0f, highT %.0f\r", _vehicle_count, _lowest_threat_distance, _highest_threat_distance); }
/* * periodic update to handle vehicle timeouts and trigger collision detection */ void AP_ADSB::update(void) { // update _my_loc if (!_ahrs.get_position(_my_loc)) { _my_loc.zero(); } if (!_enabled) { if (in_state.vehicle_list != nullptr) { deinit(); } // nothing to do return; } else if (in_state.vehicle_list == nullptr) { init(); return; } else if (in_state.list_size != in_state.list_size_param) { deinit(); return; } uint32_t now = AP_HAL::millis(); // check current list for vehicles that time out uint16_t index = 0; while (index < in_state.vehicle_count) { // check list and drop stale vehicles. When disabled, the list will get flushed if (now - in_state.vehicle_list[index].last_update_ms > VEHICLE_TIMEOUT_MS) { // don't increment index, we want to check this same index again because the contents changed // also, if we're disabled then clear the list delete_vehicle(index); } else { index++; } } // ----------------------- if (_my_loc.is_zero()) { // if we don't have a GPS lock then there's nothing else to do return; } // ----------------------- perform_threat_detection(); // ensure it's positive 24bit but allow -1 if (out_state.cfg.ICAO_id_param <= -1 || out_state.cfg.ICAO_id_param > 0x00FFFFFF) { // icao param of -1 means static information is not sent, transceiver is assumed pre-programmed. // reset timer constantly so it never reaches 10s so it never sends out_state.last_config_ms = now; } else if (out_state.cfg.ICAO_id == 0 || out_state.cfg.ICAO_id_param_prev != out_state.cfg.ICAO_id_param) { // if param changed then regenerate. This allows the param to be changed back to zero to trigger a re-generate if (out_state.cfg.ICAO_id_param == 0) { out_state.cfg.ICAO_id = genICAO(_my_loc); } else { out_state.cfg.ICAO_id = out_state.cfg.ICAO_id_param; } out_state.cfg.ICAO_id_param_prev = out_state.cfg.ICAO_id_param; set_callsign("PING", true); GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "ADSB: Using ICAO_id %d and Callsign %s", out_state.cfg.ICAO_id, out_state.cfg.callsign); out_state.last_config_ms = 0; // send now } // send static configuration data to transceiver, every 10s if (out_state.chan >= 0 && out_state.chan < MAVLINK_COMM_NUM_BUFFERS) { if (now - out_state.chan_last_ms > ADSB_CHAN_TIMEOUT_MS) { // haven't gotten a heartbeat health status packet in a while, assume hardware failure // TODO: reset out_state.chan out_state.chan = -1; } else { mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0 + out_state.chan); if (now - out_state.last_config_ms >= 5000 && HAVE_PAYLOAD_SPACE(chan, UAVIONIX_ADSB_OUT_CFG)) { out_state.last_config_ms = now; send_configure(chan); } // last_config_ms // send dynamic data to transceiver at 5Hz if (now - out_state.last_report_ms >= 200 && HAVE_PAYLOAD_SPACE(chan, UAVIONIX_ADSB_OUT_DYNAMIC)) { out_state.last_report_ms = now; send_dynamic_out(chan); } // last_report_ms } // chan_last_ms } }