log odid information

uci-overRID · Mar 14, 2024 · 9b3cac7 · 9b3cac7
1 parent a309f22
commit 9b3cac7
Showing 1 changed file with 6 additions and 0 deletions.
diff --git a/libraries/AP_Avoidance/AP_Avoidance.cpp b/libraries/AP_Avoidance/AP_Avoidance.cpp
@@ -9,6 +9,7 @@ extern const AP_HAL::HAL& hal;
 #include <GCS_MAVLink/GCS.h>
 #include <AP_Vehicle/AP_Vehicle.h>
 #include <AP_Vehicle/AP_Vehicle_Type.h>
+#include <AP_Logger/AP_Logger.h>
 
 #define AVOIDANCE_DEBUGGING 0
 
@@ -621,9 +622,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);
+    AP::logger().Write("ODID", "TimeUS,Threat_level", "Qd", AP_HAL::micros64(), new_threat_level);
+
 
     if (threat != nullptr) {
         GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Closest apprach xy: %f,z: %f", threat->closest_approach_xy, threat->closest_approach_z);
+        float closest_approach_xy = threat->closest_approach_xy;
+        float closest_approach_z = threat->closest_approach_z;
+        AP::logger().Write("ODID", "TimeUS,xy,z", "Qff", AP_HAL::micros64(), closest_approach_xy, closest_approach_z);
         new_threat_level = threat->threat_level;
         if (new_threat_level == MAV_COLLISION_THREAT_LEVEL_HIGH) {
             action = (MAV_COLLISION_ACTION)_fail_action.get();