diff --git a/libraries/AP_ADSB/AP_ADSB.cpp b/libraries/AP_ADSB/AP_ADSB.cpp index 222e4e25fb..f04117d274 100644 --- a/libraries/AP_ADSB/AP_ADSB.cpp +++ b/libraries/AP_ADSB/AP_ADSB.cpp @@ -20,8 +20,8 @@ https://en.wikipedia.org/wiki/Automatic_dependent_surveillance_%E2%80%93_broadcast */ -#include #include "AP_ADSB.h" + #if HAL_ADSB_ENABLED #include #include // for sprintf @@ -35,18 +35,23 @@ #define VEHICLE_TIMEOUT_MS 5000 // if no updates in this time, drop it from the list -#define ADSB_VEHICLE_LIST_SIZE_DEFAULT 25 -#define ADSB_VEHICLE_LIST_SIZE_MAX 100 + #define ADSB_CHAN_TIMEOUT_MS 15000 #define ADSB_SQUAWK_OCTAL_DEFAULT 1200 #define ADSB_BITBASK_RF_CAPABILITIES_UAT_IN (1 << 0) #define ADSB_BITBASK_RF_CAPABILITIES_1090ES_IN (1 << 1) -#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) - #define ADSB_LIST_RADIUS_DEFAULT 10000 // in meters -#else // APM_BUILD_TYPE(APM_BUILD_ArduCopter), Rover, Boat - #define ADSB_LIST_RADIUS_DEFAULT 2000 // in meters +#ifndef ADSB_VEHICLE_LIST_SIZE_DEFAULT + #define ADSB_VEHICLE_LIST_SIZE_DEFAULT 25 +#endif + +#ifndef ADSB_LIST_RADIUS_DEFAULT + #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) + #define ADSB_LIST_RADIUS_DEFAULT 10000 // in meters + #else + #define ADSB_LIST_RADIUS_DEFAULT 2000 // in meters + #endif #endif extern const AP_HAL::HAL& hal; @@ -69,6 +74,7 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = { // @Description: ADSB list size of nearest vehicles. Longer lists take longer to refresh with lower SRx_ADSB values. // @Range: 1 100 // @User: Advanced + // @RebootRequired: True AP_GROUPINFO("LIST_MAX", 2, AP_ADSB, in_state.list_size_param, ADSB_VEHICLE_LIST_SIZE_DEFAULT), @@ -106,19 +112,19 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = { // @Description: GPS antenna lateral offset. This describes the physical location offest from center of the GPS antenna on the aircraft. // @Values: 0:NoData,1:Left2m,2:Left4m,3:Left6m,4:Center,5:Right2m,6:Right4m,7:Right6m // @User: Advanced - AP_GROUPINFO("OFFSET_LAT", 7, AP_ADSB, out_state.cfg.gpsLatOffset, UAVIONIX_ADSB_OUT_CFG_GPS_OFFSET_LAT_RIGHT_0M), + AP_GROUPINFO("OFFSET_LAT", 7, AP_ADSB, out_state.cfg.gpsOffsetLat, UAVIONIX_ADSB_OUT_CFG_GPS_OFFSET_LAT_RIGHT_0M), // @Param: OFFSET_LON // @DisplayName: GPS antenna longitudinal offset // @Description: GPS antenna longitudinal offset. This is usually set to 1, Applied By Sensor // @Values: 0:NO_DATA,1:AppliedBySensor // @User: Advanced - AP_GROUPINFO("OFFSET_LON", 8, AP_ADSB, out_state.cfg.gpsLonOffset, UAVIONIX_ADSB_OUT_CFG_GPS_OFFSET_LON_APPLIED_BY_SENSOR), + AP_GROUPINFO("OFFSET_LON", 8, AP_ADSB, out_state.cfg.gpsOffsetLon, UAVIONIX_ADSB_OUT_CFG_GPS_OFFSET_LON_APPLIED_BY_SENSOR), // @Param: RF_SELECT // @DisplayName: Transceiver RF selection - // @Description: Transceiver RF selection for Rx enable and/or Tx enable. This only effects devices that can Tx and Rx. Rx-only devices override this to always be Rx-only. - // @Values: 0:Disabled,1:Rx-Only,2:Tx-Only,3:Rx and Tx Enabled + // @Description: Transceiver RF selection for Rx enable and/or Tx enable. This only effects devices that can Tx and/or Rx. Rx-only devices should override this to always be Rx-only. + // @Bitmask: 0:Rx,1:Tx // @User: Advanced AP_GROUPINFO("RF_SELECT", 9, AP_ADSB, out_state.cfg.rfSelect, UAVIONIX_ADSB_OUT_RF_SELECT_RX_ENABLED), @@ -166,10 +172,16 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = { AP_ADSB::AP_ADSB() { AP_Param::setup_object_defaults(this, var_info); +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL if (_singleton != nullptr) { AP_HAL::panic("AP_ADSB must be singleton"); } +#endif _singleton = this; + +#ifdef ADSB_STATIC_CALLSIGN + strncpy(&out_state.cfg.callsign, ADSB_STATIC_CALLSIGN, sizeof(out_state.cfg.callsign)); +#endif } /* @@ -177,28 +189,20 @@ AP_ADSB::AP_ADSB() */ void AP_ADSB::init(void) { - // in_state - in_state.vehicle_count = 0; if (in_state.vehicle_list == nullptr) { - if (in_state.list_size_param != constrain_int16(in_state.list_size_param, 1, ADSB_VEHICLE_LIST_SIZE_MAX)) { - in_state.list_size_param.set_and_notify(ADSB_VEHICLE_LIST_SIZE_DEFAULT); - in_state.list_size_param.save(); - } - in_state.list_size = in_state.list_size_param; - in_state.vehicle_list = new adsb_vehicle_t[in_state.list_size]; + // sanity check param + in_state.list_size_param = constrain_int16(in_state.list_size_param, 1, INT16_MAX); + + in_state.vehicle_list = new adsb_vehicle_t[in_state.list_size_param]; if (in_state.vehicle_list == nullptr) { - // dynamic RAM allocation of _vehicle_list[] failed, disable gracefully - hal.console->printf("Unable to initialize ADS-B vehicle list\n"); - _enabled.set_and_notify(0); + // dynamic RAM allocation of in_state.vehicle_list[] failed + _init_failed = true; // this keeps us from constantly trying to init forever in main update + gcs().send_text(MAV_SEVERITY_INFO, "ADSB: Unable to initialize ADSB vehicle list"); + return; } + in_state.list_size_allocated = in_state.list_size_param; } - - furthest_vehicle_distance = 0; - furthest_vehicle_index = 0; - - // out_state - set_callsign("PING1234", false); } /* @@ -206,13 +210,27 @@ void AP_ADSB::init(void) */ void AP_ADSB::deinit(void) { - in_state.vehicle_count = 0; if (in_state.vehicle_list != nullptr) { delete [] in_state.vehicle_list; in_state.vehicle_list = nullptr; } } +bool AP_ADSB::check_startup() +{ + if (!_enabled || _init_failed) { + if (in_state.vehicle_list != nullptr) { + deinit(); + } + // nothing to do + return false; + } + if (in_state.vehicle_list == nullptr) { + init(); + } + return in_state.vehicle_list != nullptr; +} + bool AP_ADSB::is_valid_callsign(uint16_t octal) { // treat "octal" as decimal and test if any decimal digit is > 7 @@ -235,27 +253,16 @@ bool AP_ADSB::is_valid_callsign(uint16_t octal) */ void AP_ADSB::update(void) { - // update _my_loc - if (!AP::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(); + if (!check_startup()) { return; } const uint32_t now = AP_HAL::millis(); + if (!AP::ahrs().get_position(_my_loc)) { + _my_loc.zero(); + } + // check current list for vehicles that time out uint16_t index = 0; while (index < in_state.vehicle_count) { @@ -269,17 +276,6 @@ void AP_ADSB::update(void) } } - if (_my_loc.is_zero()) { - // if we don't have a GPS lock then there's nothing else to do - return; - } - - if (out_state.chan < 0) { - // if there's no transceiver detected then do not set ICAO and do not service the transceiver - return; - } - - if (out_state.cfg.squawk_octal_param != out_state.cfg.squawk_octal) { // param changed, check that it's a valid octal if (!is_valid_callsign(out_state.cfg.squawk_octal_param)) { @@ -295,8 +291,8 @@ void AP_ADSB::update(void) // 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) { + } else if ((out_state.cfg.rfSelect & UAVIONIX_ADSB_OUT_RF_SELECT_TX_ENABLED) && + (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) { @@ -305,19 +301,31 @@ void AP_ADSB::update(void) 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); + +#ifndef ADSB_STATIC_CALLSIGN + if (!out_state.cfg.was_set_externally) { + set_callsign("PING", true); + } +#endif gcs().send_text(MAV_SEVERITY_INFO, "ADSB: Using ICAO_id %d and Callsign %s", (int)out_state.cfg.ICAO_id, out_state.cfg.callsign); out_state.last_config_ms = 0; // send now } + update_uavionix(); +} + +void AP_ADSB::update_uavionix() +{ + const uint32_t now = AP_HAL::millis(); + // send static configuration data to transceiver, every 5s if (out_state.chan_last_ms > 0 && 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; gcs().send_text(MAV_SEVERITY_ERROR, "ADSB: Transceiver heartbeat timed out"); - } else if (out_state.chan < MAVLINK_COMM_NUM_BUFFERS) { + } else if (out_state.chan >= 0 && !_my_loc.is_zero() && out_state.chan < MAVLINK_COMM_NUM_BUFFERS) { const 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; @@ -352,8 +360,8 @@ void AP_ADSB::determine_furthest_aircraft(void) } } // for index - furthest_vehicle_index = max_distance_index; - furthest_vehicle_distance = max_distance; + in_state.furthest_vehicle_index = max_distance_index; + in_state.furthest_vehicle_distance = max_distance; } /* @@ -382,9 +390,9 @@ void AP_ADSB::delete_vehicle(const uint16_t index) } // if the vehicle is the furthest, invalidate it. It has been bumped - if (index == furthest_vehicle_index && furthest_vehicle_distance > 0) { - furthest_vehicle_distance = 0; - furthest_vehicle_index = 0; + if (index == in_state.furthest_vehicle_index && in_state.furthest_vehicle_distance > 0) { + in_state.furthest_vehicle_distance = 0; + in_state.furthest_vehicle_index = 0; } if (index != (in_state.vehicle_count-1)) { in_state.vehicle_list[index] = in_state.vehicle_list[in_state.vehicle_count-1]; @@ -416,12 +424,11 @@ bool AP_ADSB::find_index(const adsb_vehicle_t &vehicle, uint16_t *index) const */ void AP_ADSB::handle_adsb_vehicle(const adsb_vehicle_t &vehicle) { - if (in_state.vehicle_list == nullptr) { - // We are only null when disabled. Updating is inhibited. + if (!check_startup()) { return; } - uint16_t index = in_state.list_size + 1; // initialize with invalid index + uint16_t index = in_state.list_size_allocated + 1; // initialize with invalid index const Location vehicle_loc = AP_ADSB::get_location(vehicle); const bool my_loc_is_zero = _my_loc.is_zero(); const float my_loc_distance_to_vehicle = _my_loc.get_distance(vehicle_loc); @@ -453,7 +460,7 @@ void AP_ADSB::handle_adsb_vehicle(const adsb_vehicle_t &vehicle) // found, update it set_vehicle(index, vehicle); - } else if (in_state.vehicle_count < in_state.list_size) { + } else if (in_state.vehicle_count < in_state.list_size_allocated) { // not found and there's room, add it to the end of the list set_vehicle(in_state.vehicle_count, vehicle); @@ -464,23 +471,23 @@ void AP_ADSB::handle_adsb_vehicle(const adsb_vehicle_t &vehicle) if (my_loc_is_zero) { // nothing else to do - furthest_vehicle_distance = 0; - furthest_vehicle_index = 0; + in_state.furthest_vehicle_distance = 0; + in_state.furthest_vehicle_index = 0; } else { - if (furthest_vehicle_distance <= 0) { + if (in_state.furthest_vehicle_distance <= 0) { // ensure this is populated determine_furthest_aircraft(); } - if (my_loc_distance_to_vehicle < furthest_vehicle_distance) { // is closer than the furthest + if (my_loc_distance_to_vehicle < in_state.furthest_vehicle_distance) { // is closer than the furthest // replace with the furthest vehicle - set_vehicle(furthest_vehicle_index, vehicle); + set_vehicle(in_state.furthest_vehicle_index, vehicle); - // furthest_vehicle_index is now invalid because the vehicle was overwritten, need + // in_state.furthest_vehicle_index is now invalid because the vehicle was overwritten, need // to run determine_furthest_aircraft() to determine a new one next time - furthest_vehicle_distance = 0; - furthest_vehicle_index = 0; + in_state.furthest_vehicle_distance = 0; + in_state.furthest_vehicle_index = 0; } } } // if buffer full @@ -496,25 +503,12 @@ void AP_ADSB::handle_adsb_vehicle(const adsb_vehicle_t &vehicle) } } -/* - * Update the vehicle list. If the vehicle is already in the - * list then it will update it, otherwise it will be added. - */ -void AP_ADSB::handle_vehicle(const mavlink_message_t &packet) -{ - adsb_vehicle_t vehicle {}; - const uint32_t now = AP_HAL::millis(); - mavlink_msg_adsb_vehicle_decode(&packet, &vehicle.info); - vehicle.last_update_ms = now - (vehicle.info.tslc * 1000); - handle_adsb_vehicle(vehicle); -} - /* * Copy a vehicle's data into the list */ void AP_ADSB::set_vehicle(const uint16_t index, const adsb_vehicle_t &vehicle) { - if (index >= in_state.list_size) { + if (index >= in_state.list_size_allocated) { // out of range return; } @@ -525,7 +519,7 @@ void AP_ADSB::set_vehicle(const uint16_t index, const adsb_vehicle_t &vehicle) void AP_ADSB::send_adsb_vehicle(const mavlink_channel_t chan) { - if (in_state.vehicle_list == nullptr || in_state.vehicle_count == 0) { + if (!check_startup() || in_state.vehicle_count == 0) { return; } @@ -565,7 +559,7 @@ void AP_ADSB::send_adsb_vehicle(const mavlink_channel_t chan) } -void AP_ADSB::send_dynamic_out(const mavlink_channel_t chan) +void AP_ADSB::send_dynamic_out(const mavlink_channel_t chan) const { const AP_GPS &gps = AP::gps(); const Vector3f &gps_velocity = gps.velocity(); @@ -600,7 +594,7 @@ void AP_ADSB::send_dynamic_out(const mavlink_channel_t chan) } uint16_t state = 0; - if (out_state._is_in_auto_mode) { + if (out_state.is_in_auto_mode) { state |= UAVIONIX_ADSB_OUT_DYNAMIC_STATE_AUTOPILOT_ENABLED; } if (!out_state.is_flying) { @@ -646,13 +640,13 @@ void AP_ADSB::send_dynamic_out(const mavlink_channel_t chan) * This function will override the MSB byte of the 24bit ICAO address. To ensure an invalid >24bit ICAO is never broadcasted, * this function is used to create the encoded verison without ever writing to the actual ICAO number. It's created on-demand */ -uint32_t AP_ADSB::get_encoded_icao(void) +uint32_t AP_ADSB::encode_icao(const uint32_t icao_id) const { // utilize the upper unused 8bits of the icao with special flags. // This encoding is required for uAvionix devices that break the MAVLink spec. // ensure the user assignable icao is 24 bits - uint32_t encoded_icao = (uint32_t)out_state.cfg.ICAO_id & 0x00FFFFFF; + uint32_t encoded_icao = icao_id & 0x00FFFFFF; encoded_icao &= ~0x20000000; // useGnssAltitude should always be FALSE encoded_icao |= 0x10000000; // csidLogic should always be TRUE @@ -729,11 +723,8 @@ uint8_t AP_ADSB::get_encoded_callsign_null_char() * This allows a GCS to send cfg info through the autopilot to the ADSB hardware. * This is done indirectly by reading and storing the packet and then another mechanism sends it out periodically */ -void AP_ADSB::handle_out_cfg(const mavlink_message_t &msg) +void AP_ADSB::handle_out_cfg(const mavlink_uavionix_adsb_out_cfg_t &packet) { - mavlink_uavionix_adsb_out_cfg_t packet {}; - mavlink_msg_uavionix_adsb_out_cfg_decode(&msg, &packet); - out_state.cfg.was_set_externally = true; out_state.cfg.ICAO_id = packet.ICAO; @@ -744,8 +735,8 @@ void AP_ADSB::handle_out_cfg(const mavlink_message_t &msg) out_state.cfg.emitterType = packet.emitterType; out_state.cfg.lengthWidth = packet.aircraftSize; - out_state.cfg.gpsLatOffset = packet.gpsOffsetLat; - out_state.cfg.gpsLonOffset = packet.gpsOffsetLon; + out_state.cfg.gpsOffsetLat = packet.gpsOffsetLat; + out_state.cfg.gpsOffsetLon = packet.gpsOffsetLon; out_state.cfg.rfSelect = packet.rfSelect; out_state.cfg.stall_speed_cm = packet.stallSpeed; @@ -777,7 +768,7 @@ void AP_ADSB::send_configure(const mavlink_channel_t chan) icao = out_state.cfg.ICAO_id; } else { callsign[MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG_FIELD_CALLSIGN_LEN-1] = (int8_t)get_encoded_callsign_null_char(); - icao = get_encoded_icao(); + icao = encode_icao((uint32_t)out_state.cfg.ICAO_id); } mavlink_msg_uavionix_adsb_out_cfg_send( @@ -786,8 +777,8 @@ void AP_ADSB::send_configure(const mavlink_channel_t chan) (const char*)callsign, (uint8_t)out_state.cfg.emitterType, (uint8_t)out_state.cfg.lengthWidth, - (uint8_t)out_state.cfg.gpsLatOffset, - (uint8_t)out_state.cfg.gpsLonOffset, + (uint8_t)out_state.cfg.gpsOffsetLat, + (uint8_t)out_state.cfg.gpsOffsetLon, out_state.cfg.stall_speed_cm, (uint8_t)out_state.cfg.rfSelect); } @@ -796,12 +787,8 @@ void AP_ADSB::send_configure(const mavlink_channel_t chan) * this is a message from the transceiver reporting it's health. Using this packet * we determine which channel is on so we don't have to send out_state to all channels */ - -void AP_ADSB::handle_transceiver_report(const mavlink_channel_t chan, const mavlink_message_t &msg) +void AP_ADSB::handle_transceiver_report(const mavlink_channel_t chan, const mavlink_uavionix_adsb_transceiver_health_report_t &packet) { - mavlink_uavionix_adsb_transceiver_health_report_t packet {}; - mavlink_msg_uavionix_adsb_transceiver_health_report_decode(&msg, &packet); - if (out_state.chan != chan) { gcs().send_text(MAV_SEVERITY_DEBUG, "ADSB: Found transceiver on channel %d", chan); } @@ -814,12 +801,10 @@ void AP_ADSB::handle_transceiver_report(const mavlink_channel_t chan, const mavl /* @brief Generates pseudorandom ICAO from gps time, lat, and lon. Reference: DO282B, 2.2.4.5.1.3.2 - - Note gps.time is the number of seconds since UTC midnight */ -uint32_t AP_ADSB::genICAO(const Location &loc) +uint32_t AP_ADSB::genICAO(const Location &loc) const { - // gps_time is not seconds since UTC midnight, but it is an equivalent random number + // gps_time is used as a pseudo-random number instead of seconds since UTC midnight // TODO: use UTC time instead of GPS time const AP_GPS &gps = AP::gps(); const uint64_t gps_time = gps.time_epoch_usec(); @@ -882,31 +867,41 @@ void AP_ADSB::set_callsign(const char* str, const bool append_icao) void AP_ADSB::push_sample(const adsb_vehicle_t &vehicle) { - samples.push(vehicle); + _samples.push(vehicle); } bool AP_ADSB::next_sample(adsb_vehicle_t &vehicle) { - return samples.pop(vehicle); + return _samples.pop(vehicle); } void AP_ADSB::handle_message(const mavlink_channel_t chan, const mavlink_message_t &msg) { switch (msg.msgid) { - case MAVLINK_MSG_ID_ADSB_VEHICLE: - handle_vehicle(msg); + case MAVLINK_MSG_ID_ADSB_VEHICLE: { + adsb_vehicle_t vehicle {}; + mavlink_msg_adsb_vehicle_decode(&msg, &vehicle.info); + vehicle.last_update_ms = AP_HAL::millis() - uint32_t(vehicle.info.tslc * 1000U); + handle_adsb_vehicle(vehicle); break; - case MAVLINK_MSG_ID_UAVIONIX_ADSB_TRANSCEIVER_HEALTH_REPORT: - handle_transceiver_report(chan, msg); + } + + case MAVLINK_MSG_ID_UAVIONIX_ADSB_TRANSCEIVER_HEALTH_REPORT: { + mavlink_uavionix_adsb_transceiver_health_report_t packet {}; + mavlink_msg_uavionix_adsb_transceiver_health_report_decode(&msg, &packet); + handle_transceiver_report(chan, packet); break; + } - case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CFG: - handle_out_cfg(msg); + case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CFG: { + mavlink_uavionix_adsb_out_cfg_t packet {}; + mavlink_msg_uavionix_adsb_out_cfg_decode(&msg, &packet); + handle_out_cfg(packet); break; + } case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_DYNAMIC: // unhandled, this is an outbound packet only - default: break; } @@ -932,7 +927,7 @@ bool AP_ADSB::get_vehicle_by_ICAO(const uint32_t icao, adsb_vehicle_t &vehicle) /* * Write vehicle to log */ -void AP_ADSB::write_log(const adsb_vehicle_t &vehicle) +void AP_ADSB::write_log(const adsb_vehicle_t &vehicle) const { switch (_log) { case logging::SPECIAL_ONLY: @@ -969,4 +964,3 @@ AP_ADSB *AP::ADSB() return AP_ADSB::get_singleton(); } #endif // HAL_ADSB_ENABLED - diff --git a/libraries/AP_ADSB/AP_ADSB.h b/libraries/AP_ADSB/AP_ADSB.h index 5aa3d9f15c..2863a47f20 100644 --- a/libraries/AP_ADSB/AP_ADSB.h +++ b/libraries/AP_ADSB/AP_ADSB.h @@ -23,16 +23,16 @@ #include #include -#include -#include -#include -#include #ifndef HAL_ADSB_ENABLED #define HAL_ADSB_ENABLED !HAL_MINIMIZE_FEATURES && BOARD_FLASH_SIZE > 1024 #endif #if HAL_ADSB_ENABLED +#include +#include +#include +#include class AP_ADSB { public: @@ -43,6 +43,11 @@ public: AP_ADSB(const AP_ADSB &other) = delete; AP_ADSB &operator=(const AP_ADSB&) = delete; + // get singleton instance + static AP_ADSB *get_singleton(void) { + return _singleton; + } + struct adsb_vehicle_t { mavlink_adsb_vehicle_t info; // the whole mavlink struct with all the juicy details. sizeof() == 38 uint32_t last_update_ms; // last time this was refreshed, allows timeouts @@ -54,20 +59,27 @@ public: // periodic task that maintains vehicle_list void update(void); - uint16_t get_vehicle_count() { return in_state.vehicle_count; } - // send ADSB_VEHICLE mavlink message, usually as a StreamRate void send_adsb_vehicle(mavlink_channel_t chan); - void set_stall_speed_cm(const uint16_t stall_speed_cm) { out_state.cfg.stall_speed_cm = stall_speed_cm; } - void set_max_speed(int16_t max_speed) { - if (!out_state.cfg.was_set_externally) { - // convert m/s to knots - out_state.cfg.maxAircraftSpeed_knots = (float)max_speed * 1.94384f; + bool set_stall_speed_cm(const uint16_t stall_speed_cm) { + if (out_state.cfg.was_set_externally) { + return false; } + out_state.cfg.stall_speed_cm = stall_speed_cm; + return true; } - void set_is_auto_mode(const bool is_in_auto_mode) { out_state._is_in_auto_mode = is_in_auto_mode; } + bool set_max_speed(int16_t max_speed) { + if (out_state.cfg.was_set_externally) { + return false; + } + // convert m/s to knots + out_state.cfg.maxAircraftSpeed_knots = (float)max_speed * M_PER_SEC_TO_KNOTS; + return true; + } + + void set_is_auto_mode(const bool is_in_auto_mode) { out_state.is_in_auto_mode = is_in_auto_mode; } void set_is_flying(const bool is_flying) { out_state.is_flying = is_flying; } UAVIONIX_ADSB_RF_HEALTH get_transceiver_status(void) { return out_state.status; } @@ -78,9 +90,18 @@ public: bool enabled() const { return _enabled; } + + bool init_failed() const { + return _init_failed; + } + + bool healthy() { + return check_startup(); + } + bool next_sample(adsb_vehicle_t &obstacle); - // handle a adsb_vehicle_t from an external source (used for UAVCAN) + // handle a adsb_vehicle_t from an external source void handle_adsb_vehicle(const adsb_vehicle_t &vehicle); // mavlink message handler @@ -96,20 +117,16 @@ public: // confirm a value is a valid callsign static bool is_valid_callsign(uint16_t octal) WARN_IF_UNUSED; - // get singleton instance - static AP_ADSB *get_singleton(void) { - return _singleton; - } - private: static AP_ADSB *_singleton; - // initialize _vehicle_list + // initialize vehicle_list void init(); - - // free _vehicle_list void deinit(); + // check to see if we are initialized (and possibly do initialization) + bool check_startup(); + // compares current vector against vehicle_list to detect threats void determine_furthest_aircraft(void); @@ -122,48 +139,50 @@ private: void set_vehicle(const uint16_t index, const adsb_vehicle_t &vehicle); // Generates pseudorandom ICAO from gps time, lat, and lon - uint32_t genICAO(const Location &loc); + uint32_t genICAO(const Location &loc) const; // set callsign: 8char string (plus null termination) then optionally append last 4 digits of icao void set_callsign(const char* str, const bool append_icao); // send static and dynamic data to ADSB transceiver void send_configure(const mavlink_channel_t chan); - void send_dynamic_out(const mavlink_channel_t chan); + void send_dynamic_out(const mavlink_channel_t chan) const; // special helpers for uAvionix workarounds - uint32_t get_encoded_icao(void); + uint32_t encode_icao(const uint32_t icao_id) const; uint8_t get_encoded_callsign_null_char(void); - // add or update vehicle_list from inbound mavlink msg - void handle_vehicle(const mavlink_message_t &msg); + // configure ADSB-out transceivers + void handle_out_cfg(const mavlink_uavionix_adsb_out_cfg_t &packet); - // handle ADS-B transceiver report for ping2020 - void handle_transceiver_report(mavlink_channel_t chan, const mavlink_message_t &msg); - - void handle_out_cfg(const mavlink_message_t &msg); + // mavlink handler + void handle_transceiver_report(const mavlink_channel_t chan, const mavlink_uavionix_adsb_transceiver_health_report_t &packet); AP_Int8 _enabled; Location _my_loc; + bool _init_failed; // ADSB-IN state. Maintains list of external vehicles struct { // list management AP_Int16 list_size_param; - uint16_t list_size = 1; // start with tiny list, then change to param-defined size. This ensures it doesn't fail on start - adsb_vehicle_t *vehicle_list = nullptr; + uint16_t list_size_allocated; + adsb_vehicle_t *vehicle_list; uint16_t vehicle_count; AP_Int32 list_radius; AP_Int16 list_altitude; + // index of and distance to furthest vehicle in list + uint16_t furthest_vehicle_index; + float furthest_vehicle_distance; + // streamrate stuff uint32_t send_start_ms[MAVLINK_COMM_NUM_BUFFERS]; uint16_t send_index[MAVLINK_COMM_NUM_BUFFERS]; } in_state; - // ADSB-OUT state. Maintains export data struct { uint32_t last_config_ms; // send once every 10s @@ -172,7 +191,7 @@ private: uint32_t chan_last_ms; UAVIONIX_ADSB_RF_HEALTH status; // transceiver status bool is_flying; - bool _is_in_auto_mode; + bool is_in_auto_mode; // ADSB-OUT configuration struct { @@ -182,8 +201,8 @@ private: char callsign[MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG_FIELD_CALLSIGN_LEN]; //Vehicle identifier (8 characters, null terminated, valid characters are A-Z, 0-9, " " only). AP_Int8 emitterType; AP_Int8 lengthWidth; // Aircraft length and width encoding (table 2-35 of DO-282B) - AP_Int8 gpsLatOffset; - AP_Int8 gpsLonOffset; + AP_Int8 gpsOffsetLat; + AP_Int8 gpsOffsetLon; uint16_t stall_speed_cm; AP_Int8 rfSelect; AP_Int16 squawk_octal_param; @@ -196,22 +215,17 @@ private: } out_state; - // index of and distance to furthest vehicle in list - uint16_t furthest_vehicle_index; - float furthest_vehicle_distance; - - // special ICAO of interest that ignored filters when != 0 AP_Int32 _special_ICAO_target; - static const uint8_t max_samples = 30; - ObjectBuffer samples{max_samples}; + static const uint8_t _max_samples = 30; + ObjectBuffer _samples{_max_samples}; void push_sample(const adsb_vehicle_t &vehicle); // logging AP_Int8 _log; - void write_log(const adsb_vehicle_t &vehicle); + void write_log(const adsb_vehicle_t &vehicle) const; enum logging { NONE = 0, SPECIAL_ONLY = 1, @@ -219,6 +233,8 @@ private: }; + void update_uavionix(); + }; namespace AP {