/*
* 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
}
}
