After hardware test

.gitmodules

@@ -9,7 +9,7 @@
 	url = https://github.com/google/benchmark.git
 [submodule "modules/mavlink"]
 	path = modules/mavlink
-	url = https://github.com/ArduPilot/mavlink
+	url = https://github.com/uci-overRID/mavlink
 [submodule "gtest"]
 	path = modules/gtest
 	url = https://github.com/ArduPilot/googletest

libraries/AP_Avoidance/AP_Avoidance.cpp

@@ -258,14 +258,14 @@ void AP_Avoidance::add_obstacle(const uint32_t obstacle_timestamp_ms,
     return add_obstacle(obstacle_timestamp_ms, src, src_id, loc, vel);
 }
 
-bool mac_eq(uint8_t a[6], uint8_t b[6]) {
-    return a[0] == b[0] &&
-           a[1] == b[1] &&
-           a[2] == b[2] &&
-           a[3] == b[3] &&
-           a[4] == b[4] &&
-           a[5] == b[5];
-}
+// bool mac_eq(uint8_t a[6], uint8_t b[6]) {
+//     return a[0] == b[0] &&
+//            a[1] == b[1] &&
+//            a[2] == b[2] &&
+//            a[3] == b[3] &&
+//            a[4] == b[4] &&
+//            a[5] == b[5];
+// }
 
 void AP_Avoidance::add_obstacle(uint32_t obstacle_timestamp_ms,
                       const MAV_COLLISION_SRC src,
@@ -620,11 +620,14 @@ void AP_Avoidance::handle_avoidance_local(AP_Avoidance::Obstacle *threat)
 {
     MAV_COLLISION_THREAT_LEVEL new_threat_level = MAV_COLLISION_THREAT_LEVEL_NONE;
     MAV_COLLISION_ACTION action = MAV_COLLISION_ACTION_NONE;
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Threat level: %d", new_threat_level);
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Closest apprach xy: %f,z: %f", threat->closest_approach_xy, threat->closest_approach_z);
 
     if (threat != nullptr) {
         new_threat_level = threat->threat_level;
         if (new_threat_level == MAV_COLLISION_THREAT_LEVEL_HIGH) {
             action = (MAV_COLLISION_ACTION)_fail_action.get();
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Taking fail action");
             Location my_loc;
             if (action != MAV_COLLISION_ACTION_NONE && _fail_altitude_minimum > 0 &&
                 AP::ahrs().get_location(my_loc) && ((my_loc.alt*0.01f) < _fail_altitude_minimum)) {
@@ -771,4 +774,4 @@ AP_Avoidance *ap_avoidance()
 
 }
 
-#endif // HAL_ADSB_ENABLED
+#endif // HAL_ADSB_ENABLED

libraries/AP_HAL_ChibiOS/hwdef/MatekF405-TE/hwdef.dat

@@ -191,4 +191,4 @@ define HAL_BUTTON_ENABLED 0
 define AP_NOTIFY_OREOLED_ENABLED 0
 define HAL_PICCOLO_CAN_ENABLE 0
 define AP_BATTERY_SYNTHETIC_CURRENT_ENABLED 0
-define HAL_ADSB_ENABLED 1
+define HAL_ADSB_ENABLED 1

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -4025,10 +4025,10 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg)
     case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE:
     case MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION:
         if(AP::vehicle()->odidscanner.message_from_rx(chan)) {
-            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Detected msg from odid-rx");
+            // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Detected msg from odid-rx");
             AP::vehicle()->odidscanner.handle_msg(msg);
         } else {
-            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Detected msg from odid-tx");
+            // GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Detected msg from odid-tx");
             AP::opendroneid().handle_msg(chan, msg);
         }
         break;