AP_RangeFinder: Hexsoon_CAN debug

rmackay9 · Dec 20, 2024 · 294f880 · 294f880
1 parent 3faea52
commit 294f880
Show file tree

Hide file tree

Showing 2 changed files with 42 additions and 2 deletions.
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Hexsoon_CAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Hexsoon_CAN.cpp
@@ -6,6 +6,7 @@
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_HAL/AP_HAL.h>
 #include <GCS_MAVLink/GCS.h>
+#include <AP_HAL/utility/sparse-endian.h>
 
 // update the state of the sensor
 void AP_RangeFinder_Hexsoon_CAN::update(void)
@@ -82,7 +83,7 @@ bool AP_RangeFinder_Hexsoon_CAN::handle_frame(AP_HAL::CANFrame &frame)
             // calculate distance
             const float dist_vert = (object_info.dist_vert * 0.2) - 500;
             const float dist_horiz = (object_info.dist_horiz * 0.2) - 204.6;
-            const float dist_m = sqrtf(sq(dist_vert) + sq(dist_horiz));
+            const float dist_m = sqrtF(sq(dist_vert) + sq(dist_horiz));
             accumulate_distance_m(dist_m);
             GCS_SEND_TEXT(MAV_SEVERITY_INFO, "HexCAN dist:%4.2f", (double)dist_m);
             break;
@@ -96,4 +97,38 @@ bool AP_RangeFinder_Hexsoon_CAN::handle_frame(AP_HAL::CANFrame &frame)
     return true;
 }
 
+// debug test method
+void AP_RangeFinder_Hexsoon_CAN::test_method()
+{
+    // distance and relative velocity information for a single tracked object
+    // copy frame data to structure to ease decoding
+    ObjectGeneralInfo object_info;
+    uint8_t fake_frame[8] = {0x57, 0x4E, 0xC4, 0x0C, 0x7F, 0x60, 0x18, 0x80};
+    memcpy(&object_info, fake_frame, sizeof(object_info));
+
+    // calculate distance
+    const int16_t dist_vert = be16toh(object_info.dist_vert);//(int16_t(be16toh(object_info.dist_vert)) * 0.2) - 500;
+    const float dist_horiz = (object_info.dist_horiz * 0.2) - 204.6;
+    const float dist_m = sqrtF(sq(dist_vert) + sq(dist_horiz));
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "HexCAN dist:%4.2f", (double)dist_m);
+
+    // calculate speed
+    const float speed_vert = object_info.velocity_vert * 0.25 - 128;
+    const float speed_horiz = object_info.velocity_horiz * 0.25 - 64;    
+
+    // check signal strength
+    const float receiver_strength = (object_info.rcs * 0.5) - 64;
+
+    const uint8_t sector = object_info.sector_num;
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "HexCAN targ:%u sect:%u dV:%x dH:%4.1f sV:%4.1f sH:%4.1f dyn:%u rcs:%4.2f",
+                                    (unsigned)object_info.object_id,
+                                    (unsigned)sector,
+                                    (int)dist_vert,
+                                    (double)dist_horiz,
+                                    (double)speed_vert,
+                                    (double)speed_horiz,
+                                    (unsigned)object_info.dynamic_properties,
+                                    (double)receiver_strength);    
+}
+
 #endif  // AP_RANGEFINDER_HEXSOON_CAN_ENABLED
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Hexsoon_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_Hexsoon_CAN.h
@@ -3,6 +3,7 @@
 
 #if AP_RANGEFINDER_HEXSOON_CAN_ENABLED
 #include "AP_RangeFinder_Backend_CAN.h"
+#include <AP_HAL/utility/sparse-endian.h>
 
 class AP_RangeFinder_Hexsoon_CAN : public AP_RangeFinder_Backend_CAN {
 public:
@@ -16,6 +17,9 @@ class AP_RangeFinder_Hexsoon_CAN : public AP_RangeFinder_Backend_CAN {
     // handler for incoming frames, returns true if packet consumed
     bool handle_frame(AP_HAL::CANFrame &frame) override;
 
+    // debug test method
+    static void test_method();
+
     static const struct AP_Param::GroupInfo var_info[];
 
 private:
@@ -32,7 +36,8 @@ class AP_RangeFinder_Hexsoon_CAN : public AP_RangeFinder_Backend_CAN {
     // object general info message structure
     struct PACKED ObjectGeneralInfo {
         uint8_t object_id;
-        int16_t dist_vert:13;          // distance from center in vertical plane in meters (can be negative)
+        //int16_t dist_vert:13;          // distance from center in vertical plane in meters (can be negative)
+        be16_t dist_vert:13;          // distance from center in vertical plane in meters (can be negative)
         int16_t dist_horiz:11;         // distance from center in horizontal plane in meters (can be negative)
         int16_t velocity_vert:10;
         int16_t velocity_horiz:9;