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; 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); 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) diff --git a/Tools/scripts/configure-ci.sh b/Tools/scripts/configure-ci.sh index b721a3c19f..672540432d 100755 --- a/Tools/scripts/configure-ci.sh +++ b/Tools/scripts/configure-ci.sh @@ -6,8 +6,8 @@ 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_TARBALL="$ARM_ROOT-20150921-linux.tar.bz2" +ARM_ROOT="gcc-arm-none-eabi-6-2017-q2-update" +ARM_TARBALL="$ARM_ROOT-linux.tar.bz2" RPI_ROOT="master" RPI_TARBALL="$RPI_ROOT.tar.gz" @@ -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" 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 { 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), 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 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_ };