From 4801a4f4736dfccb00176b93c942a7caf7530b3d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 15 Jan 2020 15:02:36 +1100 Subject: [PATCH 01/10] DataFlash: added ADSB logging structures --- libraries/DataFlash/LogStructure.h | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/libraries/DataFlash/LogStructure.h b/libraries/DataFlash/LogStructure.h index 06a8d04e4e..ded4231565 100644 --- a/libraries/DataFlash/LogStructure.h +++ b/libraries/DataFlash/LogStructure.h @@ -692,6 +692,19 @@ struct PACKED log_Current_Cells { uint16_t cell_voltages[10]; }; +struct PACKED log_ADSB { + LOG_PACKET_HEADER; + uint64_t time_us; + uint32_t ICAO_address; + int32_t lat; + int32_t lng; + int32_t alt; + uint16_t heading; + uint16_t hor_velocity; + int16_t ver_velocity; + uint16_t squawk; +}; + struct PACKED log_Compass { LOG_PACKET_HEADER; uint64_t time_us; @@ -1408,6 +1421,8 @@ Format characters in the format string for binary log messages "RATE", "Qffffffffffff", "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut", "skk-kk-kk-oo-", "F?????????BB-" }, \ { LOG_RALLY_MSG, sizeof(log_Rally), \ "RALY", "QBBLLh", "TimeUS,Tot,Seq,Lat,Lng,Alt", "s--DUm", "F--GGB" }, \ + { LOG_ADSB_MSG, sizeof(log_ADSB), \ + "ADSB", "QIiiiHHhH", "TimeUS,ICAO_address,Lat,Lng,Alt,Heading,Hor_vel,Ver_vel,Squark", "s-DUmhnn-", "F-GGCBCC-" }, \ { LOG_VISUALODOM_MSG, sizeof(log_VisualOdom), \ "VISO", "Qffffffff", "TimeUS,dt,AngDX,AngDY,AngDZ,PosDX,PosDY,PosDZ,conf", "ssrrrmmm-", "FF000000-" } @@ -1561,6 +1576,7 @@ enum LogMessages : uint8_t { LOG_ISBD_MSG, LOG_ASP2_MSG, LOG_PERFORMANCE_MSG, + LOG_ADSB_MSG, _LOG_LAST_MSG_ }; From 944036e0ba897ac2e1eca90973140aede58b3ffd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 15 Jan 2020 15:02:44 +1100 Subject: [PATCH 02/10] AP_ADSB: added ADSB logging --- libraries/AP_ADSB/AP_ADSB.cpp | 186 +++++++++++++++++++++++++++------- libraries/AP_ADSB/AP_ADSB.h | 41 ++++++-- 2 files changed, 181 insertions(+), 46 deletions(-) diff --git a/libraries/AP_ADSB/AP_ADSB.cpp b/libraries/AP_ADSB/AP_ADSB.cpp index 0bdcc261ad..4b31f647c3 100644 --- a/libraries/AP_ADSB/AP_ADSB.cpp +++ b/libraries/AP_ADSB/AP_ADSB.cpp @@ -27,6 +27,11 @@ #include #include #include +#include +#include +#include +#include + #define VEHICLE_TIMEOUT_MS 5000 // if no updates in this time, drop it from the list #define ADSB_VEHICLE_LIST_SIZE_DEFAULT 25 @@ -68,9 +73,10 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = { // @Param: LIST_RADIUS // @DisplayName: ADSB vehicle list radius filter - // @Description: ADSB vehicle list radius filter. Vehicles detected outside this radius will be completely ignored. They will not show up in the SRx_ADSB stream to the GCS and will not be considered in any avoidance calculations. - // @Range: 1 100000 + // @Description: ADSB vehicle list radius filter. Vehicles detected outside this radius will be completely ignored. They will not show up in the SRx_ADSB stream to the GCS and will not be considered in any avoidance calculations. A value of 0 will disable this filter. + // @Range: 0 100000 // @User: Advanced + // @Units: m AP_GROUPINFO("LIST_RADIUS", 3, AP_ADSB, in_state.list_radius, ADSB_LIST_RADIUS_DEFAULT), // @Param: ICAO_ID @@ -118,6 +124,7 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = { // @Param: SQUAWK // @DisplayName: Squawk code // @Description: VFR squawk (Mode 3/A) code is a pre-programmed default code when the pilot is flying VFR and not in contact with ATC. In the USA, the VFR squawk code is octal 1200 (hex 0x280, decimal 640) and in most parts of Europe the VFR squawk code is octal 7000. If an invalid octal number is set then it will be reset to 1200. + // @Range: 0 7777 // @Units: octal // @User: Advanced AP_GROUPINFO("SQUAWK", 10, AP_ADSB, out_state.cfg.squawk_octal_param, ADSB_SQUAWK_OCTAL_DEFAULT), @@ -130,6 +137,26 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = { // @User: Advanced AP_GROUPINFO("RF_CAPABLE", 11, AP_ADSB, out_state.cfg.rf_capable, 0), + // @Param: LIST_ALT + // @DisplayName: ADSB vehicle list altitude filter + // @Description: ADSB vehicle list altitude filter. Vehicles detected above this altitude will be completely ignored. They will not show up in the SRx_ADSB stream to the GCS and will not be considered in any avoidance calculations. A value of 0 will disable this filter. + // @Range: 0 32767 + // @User: Advanced + // @Units: m + AP_GROUPINFO("LIST_ALT", 12, AP_ADSB, in_state.list_altitude, 0), + + // @Param: ICAO_SPECL + // @DisplayName: ICAO_ID of special vehicle + // @Description: ICAO_ID of special vehicle that ignores ADSB_LIST_RADIUS and ADSB_LIST_ALT. The vehicle is always tracked. Use 0 to disable. + // @User: Advanced + AP_GROUPINFO("ICAO_SPECL", 13, AP_ADSB, _special_ICAO_target, 0), + + // @Param: LOG + // @DisplayName: ADS-B logging + // @Description: 0: no logging, 1: log only special ID, 2:log all + // @Values: 0:no logging,1:log only special ID,2:log all + // @User: Advanced + AP_GROUPINFO("LOG", 14, AP_ADSB, _log, 1), AP_GROUPEND }; @@ -185,6 +212,23 @@ void AP_ADSB::deinit(void) } } +bool AP_ADSB::is_valid_callsign(uint16_t octal) +{ + // treat "octal" as decimal and test if any decimal digit is > 7 + if (octal > 7777) { + return false; + } + + while (octal != 0) { + if (octal % 10 > 7) { + return false; + } + octal /= 10; + } + + return true; +} + /* * periodic update to handle vehicle timeouts and trigger collision detection */ @@ -209,7 +253,7 @@ void AP_ADSB::update(void) return; } - uint32_t now = AP_HAL::millis(); + const uint32_t now = AP_HAL::millis(); // check current list for vehicles that time out uint16_t index = 0; @@ -237,7 +281,7 @@ void AP_ADSB::update(void) if (out_state.cfg.squawk_octal_param != out_state.cfg.squawk_octal) { // param changed, check that it's a valid octal - if (!is_valid_octal(out_state.cfg.squawk_octal_param)) { + if (!is_valid_callsign(out_state.cfg.squawk_octal_param)) { // invalid, reset it to default out_state.cfg.squawk_octal_param = ADSB_SQUAWK_OCTAL_DEFAULT; } @@ -261,7 +305,7 @@ void AP_ADSB::update(void) } out_state.cfg.ICAO_id_param_prev = out_state.cfg.ICAO_id_param; set_callsign("PING", true); - gcs().send_text(MAV_SEVERITY_INFO, "ADSB: Using ICAO_id %d and Callsign %s", out_state.cfg.ICAO_id, out_state.cfg.callsign); + 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 } @@ -273,7 +317,7 @@ void AP_ADSB::update(void) out_state.chan = -1; gcs().send_text(MAV_SEVERITY_ERROR, "ADSB: Transceiver heartbeat timed out"); } else if (out_state.chan < MAVLINK_COMM_NUM_BUFFERS) { - mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0 + out_state.chan); + 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; send_configure(chan); @@ -297,7 +341,10 @@ void AP_ADSB::determine_furthest_aircraft(void) uint16_t max_distance_index = 0; for (uint16_t index = 0; index < in_state.vehicle_count; index++) { - float distance = _my_loc.get_distance(get_location(in_state.vehicle_list[index])); + if (is_special_vehicle(in_state.vehicle_list[index].info.ICAO_address)) { + continue; + } + const float distance = _my_loc.get_distance(get_location(in_state.vehicle_list[index])); if (max_distance < distance || index == 0) { max_distance = distance; max_distance_index = index; @@ -311,9 +358,9 @@ void AP_ADSB::determine_furthest_aircraft(void) /* * Convert/Extract a Location from a vehicle */ -Location_Class AP_ADSB::get_location(const adsb_vehicle_t &vehicle) const +Location AP_ADSB::get_location(const adsb_vehicle_t &vehicle) const { - Location_Class loc = Location_Class( + const Location_Class loc = Location_Class( vehicle.info.lat, vehicle.info.lon, vehicle.info.altitude * 0.1f, @@ -328,19 +375,22 @@ Location_Class AP_ADSB::get_location(const adsb_vehicle_t &vehicle) const */ void AP_ADSB::delete_vehicle(const uint16_t index) { - if (index < in_state.vehicle_count) { - // 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.vehicle_count-1)) { - in_state.vehicle_list[index] = in_state.vehicle_list[in_state.vehicle_count-1]; - } - // TODO: is memset needed? When we decrement the index we essentially forget about it - memset(&in_state.vehicle_list[in_state.vehicle_count-1], 0, sizeof(adsb_vehicle_t)); - in_state.vehicle_count--; + if (index >= in_state.vehicle_count) { + // index out of range + return; } + + // 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.vehicle_count-1)) { + in_state.vehicle_list[index] = in_state.vehicle_list[in_state.vehicle_count-1]; + } + // TODO: is memset needed? When we decrement the index we essentially forget about it + memset(&in_state.vehicle_list[in_state.vehicle_count-1], 0, sizeof(adsb_vehicle_t)); + in_state.vehicle_count--; } /* @@ -374,7 +424,9 @@ void AP_ADSB::handle_adsb_vehicle(const adsb_vehicle_t &vehicle) Location_Class 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); - const bool out_of_range = in_state.list_radius > 0 && !my_loc_is_zero && my_loc_distance_to_vehicle > in_state.list_radius; + const bool is_special = is_special_vehicle(vehicle.info.ICAO_address); + const bool out_of_range = in_state.list_radius > 0 && !my_loc_is_zero && my_loc_distance_to_vehicle > in_state.list_radius && !is_special; + const bool out_of_range_alt = in_state.list_altitude > 0 && !my_loc_is_zero && abs(vehicle_loc.alt - _my_loc.alt) > in_state.list_altitude*100 && !is_special; const bool is_tracked_in_list = find_index(vehicle, &index); const uint32_t now = AP_HAL::millis(); @@ -383,6 +435,7 @@ void AP_ADSB::handle_adsb_vehicle(const adsb_vehicle_t &vehicle) if (vehicle_loc.is_zero() || out_of_range || + out_of_range_alt || detected_ourself || (vehicle.info.ICAO_address > 0x00FFFFFF) || // ICAO address is 24bits, so ignore higher values. !(vehicle.info.flags & required_flags_position) || @@ -446,11 +499,11 @@ 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) +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); + mavlink_msg_adsb_vehicle_decode(&packet, &vehicle.info); vehicle.last_update_ms = now - (vehicle.info.tslc * 1000); handle_adsb_vehicle(vehicle); } @@ -460,9 +513,13 @@ void AP_ADSB::handle_vehicle(const mavlink_message_t *packet) */ void AP_ADSB::set_vehicle(const uint16_t index, const adsb_vehicle_t &vehicle) { - if (index < in_state.list_size) { - in_state.vehicle_list[index] = vehicle; + if (index >= in_state.list_size) { + // out of range + return; } + in_state.vehicle_list[index] = vehicle; + + write_log(vehicle); } void AP_ADSB::send_adsb_vehicle(const mavlink_channel_t chan) @@ -659,7 +716,7 @@ uint8_t AP_ADSB::get_encoded_callsign_null_char() // using the above logic, we must always assign the squawk. once we get configured // externally then get_encoded_callsign_null_char() stops getting called - snprintf(out_state.cfg.callsign, 5, "%04d", unsigned(out_state.cfg.squawk_octal)); + snprintf(out_state.cfg.callsign, 5, "%04d", unsigned(out_state.cfg.squawk_octal) & 0x1FFF); memset(&out_state.cfg.callsign[4], 0, 5); // clear remaining 5 chars encoded_null |= 0x40; @@ -671,10 +728,10 @@ 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_message_t &msg) { mavlink_uavionix_adsb_out_cfg_t packet {}; - mavlink_msg_uavionix_adsb_out_cfg_decode(msg, &packet); + mavlink_msg_uavionix_adsb_out_cfg_decode(&msg, &packet); out_state.cfg.was_set_externally = true; @@ -692,9 +749,9 @@ void AP_ADSB::handle_out_cfg(const mavlink_message_t* msg) out_state.cfg.stall_speed_cm = packet.stallSpeed; // guard against string with non-null end char - char c = out_state.cfg.callsign[MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG_FIELD_CALLSIGN_LEN-1]; + const char c = out_state.cfg.callsign[MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG_FIELD_CALLSIGN_LEN-1]; out_state.cfg.callsign[MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG_FIELD_CALLSIGN_LEN-1] = 0; - gcs().send_text(MAV_SEVERITY_INFO, "ADSB: Using ICAO_id %d and Callsign %s", out_state.cfg.ICAO_id, out_state.cfg.callsign); + 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.cfg.callsign[MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG_FIELD_CALLSIGN_LEN-1] = c; // send now @@ -709,7 +766,7 @@ void AP_ADSB::send_configure(const mavlink_channel_t chan) // MAVLink spec says the 9 byte callsign field is 8 byte string with 9th byte as null. // Here we temporarily set some flags in that null char to signify the callsign // may be a flightplanID instead - int8_t callsign[MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG_FIELD_CALLSIGN_LEN]; + int8_t callsign[sizeof(out_state.cfg.callsign)]; uint32_t icao; memcpy(callsign, out_state.cfg.callsign, sizeof(out_state.cfg.callsign)); @@ -739,10 +796,10 @@ void AP_ADSB::send_configure(const mavlink_channel_t chan) * 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_message_t &msg) { mavlink_uavionix_adsb_transceiver_health_report_t packet {}; - mavlink_msg_uavionix_adsb_transceiver_health_report_decode(msg, &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); @@ -759,7 +816,7 @@ void AP_ADSB::handle_transceiver_report(const mavlink_channel_t chan, const mavl Note gps.time is the number of seconds since UTC midnight */ -uint32_t AP_ADSB::genICAO(const Location_Class &loc) +uint32_t AP_ADSB::genICAO(const Location &loc) { // gps_time is not seconds since UTC midnight, but it is an equivalent random number // TODO: use UTC time instead of GPS time @@ -767,7 +824,7 @@ uint32_t AP_ADSB::genICAO(const Location_Class &loc) const uint64_t gps_time = gps.time_epoch_usec(); uint32_t timeSum = 0; - uint32_t M3 = 4096 * (loc.lat & 0x00000FFF) + (loc.lng & 0x00000FFF); + const uint32_t M3 = 4096 * (loc.lat & 0x00000FFF) + (loc.lng & 0x00000FFF); for (uint8_t i=0; i<24; i++) { timeSum += (((gps_time & 0x00FFFFFF)>> i) & 0x00000001); @@ -832,9 +889,9 @@ bool AP_ADSB::next_sample(adsb_vehicle_t &vehicle) return samples.pop_front(vehicle); } -void AP_ADSB::handle_message(const mavlink_channel_t chan, const mavlink_message_t* msg) +void AP_ADSB::handle_message(const mavlink_channel_t chan, const mavlink_message_t &msg) { - switch (msg->msgid) { + switch (msg.msgid) { case MAVLINK_MSG_ID_ADSB_VEHICLE: handle_vehicle(msg); break; @@ -854,7 +911,60 @@ void AP_ADSB::handle_message(const mavlink_channel_t chan, const mavlink_message } +// If that ICAO is found in the database then return true with a fully populated vehicle +bool AP_ADSB::get_vehicle_by_ICAO(const uint32_t icao, adsb_vehicle_t &vehicle) const +{ + adsb_vehicle_t temp_vehicle; + temp_vehicle.info.ICAO_address = icao; + + uint16_t i; + if (find_index(temp_vehicle, &i)) { + // vehicle is tracked in list. + // we must memcpy it because the database may reorganize itself and we don't + // want the reference to magically start pointing at a different vehicle + memcpy(&vehicle, &in_state.vehicle_list[i], sizeof(adsb_vehicle_t)); + return true; + } + return false; +} + +/* + * Write vehicle to log + */ +void AP_ADSB::write_log(const adsb_vehicle_t &vehicle) +{ + switch (_log) { + case logging::SPECIAL_ONLY: + if (!is_special_vehicle(vehicle.info.ICAO_address)) { + return; + } + break; + + case logging::ALL: + break; + + case logging::NONE: + default: + return; + } + + struct log_ADSB pkt = { + LOG_PACKET_HEADER_INIT(LOG_ADSB_MSG), + time_us : AP_HAL::micros64(), + ICAO_address : vehicle.info.ICAO_address, + lat : vehicle.info.lat, + lng : vehicle.info.lon, + alt : vehicle.info.altitude, + heading : vehicle.info.heading, + hor_velocity : vehicle.info.hor_velocity, + ver_velocity : vehicle.info.ver_velocity, + squawk : vehicle.info.squawk, + }; + DataFlash_Class::instance()->WriteBlock(&pkt, sizeof(pkt)); +} + AP_ADSB *AP::ADSB() { return AP_ADSB::get_singleton(); } + diff --git a/libraries/AP_ADSB/AP_ADSB.h b/libraries/AP_ADSB/AP_ADSB.h index 50de6ac9f4..c754ed2fe3 100644 --- a/libraries/AP_ADSB/AP_ADSB.h +++ b/libraries/AP_ADSB/AP_ADSB.h @@ -21,12 +21,11 @@ Tom Pittenger, November 2015 */ +#include #include #include #include #include -#include - #include class AP_ADSB { @@ -68,7 +67,7 @@ class AP_ADSB { UAVIONIX_ADSB_RF_HEALTH get_transceiver_status(void) { return out_state.status; } // extract a location out of a vehicle item - Location_Class get_location(const adsb_vehicle_t &vehicle) const; + Location get_location(const adsb_vehicle_t &vehicle) const; bool enabled() const { return _enabled; @@ -79,7 +78,17 @@ class AP_ADSB { void handle_adsb_vehicle(const adsb_vehicle_t &vehicle); // mavlink message handler - void handle_message(const mavlink_channel_t chan, const mavlink_message_t* msg); + void handle_message(const mavlink_channel_t chan, const mavlink_message_t &msg); + + // when true, a vehicle with that ICAO was found in database and the vehicle is populated. + bool get_vehicle_by_ICAO(const uint32_t icao, adsb_vehicle_t &vehicle) const; + + uint32_t get_special_ICAO_target() const { return (uint32_t)_special_ICAO_target; }; + void set_special_ICAO_target(const uint32_t new_icao_target) { _special_ICAO_target = (int32_t)new_icao_target; }; + bool is_special_vehicle(uint32_t icao) const { return _special_ICAO_target != 0 && (_special_ICAO_target == (int32_t)icao); } + + // 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) { @@ -107,7 +116,7 @@ class AP_ADSB { 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_Class &loc); + uint32_t genICAO(const Location &loc); // 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); @@ -121,12 +130,12 @@ class AP_ADSB { 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); + void handle_vehicle(const mavlink_message_t &msg); // handle ADS-B transceiver report for ping2020 - void handle_transceiver_report(mavlink_channel_t chan, const mavlink_message_t* msg); + void handle_transceiver_report(mavlink_channel_t chan, const mavlink_message_t &msg); - void handle_out_cfg(const mavlink_message_t* msg); + void handle_out_cfg(const mavlink_message_t &msg); AP_Int8 _enabled; @@ -141,6 +150,7 @@ class AP_ADSB { adsb_vehicle_t *vehicle_list = nullptr; uint16_t vehicle_count; AP_Int32 list_radius; + AP_Int16 list_altitude; // streamrate stuff uint32_t send_start_ms[MAVLINK_COMM_NUM_BUFFERS]; @@ -184,10 +194,25 @@ class AP_ADSB { 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; AP_Buffer samples; void push_sample(const adsb_vehicle_t &vehicle); + + // logging + AP_Int8 _log; + void write_log(const adsb_vehicle_t &vehicle); + enum logging { + NONE = 0, + SPECIAL_ONLY = 1, + ALL = 2 + }; + + }; namespace AP { From 4b3c9afd57d433a4228f7a5b3f6dd40cb8096dc6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 15 Jan 2020 15:03:08 +1100 Subject: [PATCH 03/10] Copter: fixed call to adsb handle_message --- ArduCopter/GCS_Mavlink.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 64e1cf4a45..80fb0c51ba 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1576,7 +1576,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_DYNAMIC: case MAVLINK_MSG_ID_UAVIONIX_ADSB_TRANSCEIVER_HEALTH_REPORT: #if ADSB_ENABLED == ENABLED - copter.adsb.handle_message(chan, msg); + copter.adsb.handle_message(chan, *msg); #endif break; From b117fd69210afd0762e28152813f667c110e70ad Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 15 Jan 2020 15:03:28 +1100 Subject: [PATCH 04/10] Copter: fixed missing break found by warning when adding ADSB logging --- ArduCopter/events.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 5a0eb3a947..4df1082c5f 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -83,6 +83,7 @@ void Copter::handle_battery_failsafe(const char *type_str, const int8_t action) #else init_disarm_motors(); #endif + break; case Failsafe_Action_LandIfManual: if (!flightmode->is_autopilot()) { set_mode_land_with_pause(MODE_REASON_BATTERY_FAILSAFE); From a5985a69089b12d6776d920cc1d1042f82bf8b0f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 15 Jan 2020 15:07:13 +1100 Subject: [PATCH 05/10] HAL_ChibiOS: enable logging of all ADSB contacts --- libraries/AP_HAL_ChibiOS/hwdef/MttrCubeBlack/defaults.parm | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeBlack/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeBlack/defaults.parm index 2b8634de7a..ab70c79ced 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeBlack/defaults.parm +++ b/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeBlack/defaults.parm @@ -253,4 +253,5 @@ TOFF_POS_CHANGE 2 WP_NAVALT_MAX 3 ESC_CALIBRATION 9 ADSB_ENABLE 1 +ADSB_LOG 2 ADSB_LIST_RADIUS 10000 From 41d37545ff7afb9859fcdcf089918a024e9351cb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 15 Jan 2020 16:08:14 +1100 Subject: [PATCH 06/10] Copter: fixed ADSB avoidance the matternet code has a check that prevents changing to GUIDED mode when armed. This check prevents ADSB avoidance working as the AVOID_ADSB mode is a derived class from GUIDED. --- ArduCopter/mode.h | 2 +- ArduCopter/mode_avoid_adsb.cpp | 11 +++++++++-- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 39c451e943..44b4f601ca 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -816,10 +816,10 @@ class ModeGuided : public Mode { uint32_t wp_distance() const override; int32_t wp_bearing() const override; float crosstrack_error() const override; + void pos_control_start(); private: - void pos_control_start(); void vel_control_start(); void posvel_control_start(); void takeoff_run(); diff --git a/ArduCopter/mode_avoid_adsb.cpp b/ArduCopter/mode_avoid_adsb.cpp index dd4e80741b..372e201a67 100644 --- a/ArduCopter/mode_avoid_adsb.cpp +++ b/ArduCopter/mode_avoid_adsb.cpp @@ -12,8 +12,15 @@ // initialise avoid_adsb controller bool Copter::ModeAvoidADSB::init(const bool ignore_checks) { - // re-use guided mode - return Copter::ModeGuided::init(ignore_checks); + if (copter.position_ok() || ignore_checks) { + // initialise yaw + auto_yaw.set_mode_to_default(false); + // start in position control mode + pos_control_start(); + return true; + }else{ + return false; + } } bool Copter::ModeAvoidADSB::set_velocity(const Vector3f& velocity_neu) From 0815ed3822ec10e52295ab08f26913c773d8871e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 15 Jan 2020 16:08:40 +1100 Subject: [PATCH 07/10] AP_Avoidance: fixed param doc generation for avoidance --- libraries/AP_Avoidance/AP_Avoidance.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/libraries/AP_Avoidance/AP_Avoidance.cpp b/libraries/AP_Avoidance/AP_Avoidance.cpp index 32871b43f0..c0c2435b80 100644 --- a/libraries/AP_Avoidance/AP_Avoidance.cpp +++ b/libraries/AP_Avoidance/AP_Avoidance.cpp @@ -47,7 +47,6 @@ const AP_Param::GroupInfo AP_Avoidance::var_info[] = { // @Param: F_ACTION // @DisplayName: Collision Avoidance Behavior // @Description: Specifies aircraft behaviour when a collision is imminent - // The following values should come from the mavlink COLLISION_ACTION enum // @Values: 0:None,1:Report,2:Climb Or Descend,3:Move Horizontally,4:Move Perpendicularly in 3D,5:RTL,6:Hover // @User: Advanced AP_GROUPINFO("F_ACTION", 2, AP_Avoidance, _fail_action, AP_AVOIDANCE_FAIL_ACTION_DEFAULT), @@ -55,7 +54,6 @@ const AP_Param::GroupInfo AP_Avoidance::var_info[] = { // @Param: W_ACTION // @DisplayName: Collision Avoidance Behavior - Warn // @Description: Specifies aircraft behaviour when a collision may occur - // The following values should come from the mavlink COLLISION_ACTION enum // @Values: 0:None,1:Report // @User: Advanced AP_GROUPINFO("W_ACTION", 3, AP_Avoidance, _warn_action, MAV_COLLISION_ACTION_REPORT), From 68a9871fcc9d59ae862b746969bcf8114c505346 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 17 Jan 2020 18:25:55 +1100 Subject: [PATCH 08/10] Tools: fixed CI compiler for Copter 4.0 --- Tools/scripts/configure-ci.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/scripts/configure-ci.sh b/Tools/scripts/configure-ci.sh index b721a3c19f..073826bb87 100755 --- a/Tools/scripts/configure-ci.sh +++ b/Tools/scripts/configure-ci.sh @@ -6,7 +6,7 @@ set -ex # Disable ccache for the configure phase, it's not worth it export CCACHE_DISABLE="true" -ARM_ROOT="gcc-arm-none-eabi-4_9-2015q3" +ARM_ROOT="gcc-arm-none-eabi-6-2017-q2-update" ARM_TARBALL="$ARM_ROOT-20150921-linux.tar.bz2" RPI_ROOT="master" From 5b5b755f27896407addabc0581b08b8e9a29fbcb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 18 Jan 2020 12:35:08 +1100 Subject: [PATCH 09/10] Tools: fixed compiler path --- Tools/scripts/configure-ci.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/scripts/configure-ci.sh b/Tools/scripts/configure-ci.sh index 073826bb87..59ea935f5d 100755 --- a/Tools/scripts/configure-ci.sh +++ b/Tools/scripts/configure-ci.sh @@ -7,7 +7,7 @@ set -ex export CCACHE_DISABLE="true" ARM_ROOT="gcc-arm-none-eabi-6-2017-q2-update" -ARM_TARBALL="$ARM_ROOT-20150921-linux.tar.bz2" +ARM_TARBALL="$ARM_ROOT-linux.tar.bz2" RPI_ROOT="master" RPI_TARBALL="$RPI_ROOT.tar.gz" From f1a9916ff295d211c88f85d2530370cd6a09d838 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 14 Dec 2019 20:01:14 +0000 Subject: [PATCH 10/10] Tools: align ci gcc with environment gcc v6 on arm --- Tools/scripts/configure-ci.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/scripts/configure-ci.sh b/Tools/scripts/configure-ci.sh index 59ea935f5d..672540432d 100755 --- a/Tools/scripts/configure-ci.sh +++ b/Tools/scripts/configure-ci.sh @@ -73,7 +73,7 @@ ln -s ~/opt/$CCACHE_ROOT/ccache ~/ccache/clang exportline="export PATH=$HOME/ccache" exportline="${exportline}:$HOME/bin" exportline="${exportline}:$HOME/.local/bin" -exportline="${exportline}:$HOME/opt/gcc-arm-none-eabi-4_9-2015q3/bin" +exportline="${exportline}:$HOME/opt/gcc-arm-none-eabi-6-2017-q2-update/bin" exportline="${exportline}:$HOME/opt/tools-master/arm-bcm2708/gcc-linaro-arm-linux-gnueabihf-raspbian-x64/bin" exportline="${exportline}:$HOME/opt/$CCACHE_ROOT" exportline="${exportline}:\$PATH"