AP_Radar: Fix array indexing, avoiding 0x800 internal error if all 6 …

…peers are visible
rmaia3d · Feb 17, 2024 · 05810ef · 05810ef
1 parent 49803a2
commit 05810ef
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 5 deletions.
diff --git a/libraries/AP_Radar/AP_Radar.cpp b/libraries/AP_Radar/AP_Radar.cpp
@@ -64,7 +64,7 @@ void AP_Radar::update(void)
     }
 
     // only healthy if the data is less than 3s old
-    _flags.healthy = (AP_HAL::millis() - _last_update_ms < RADAR_PEER_FRESH_TIME_MS);
+    _flags.healthy = ((AP_HAL::millis() - _last_update_ms) < RADAR_PEER_FRESH_TIME_MS);
 }
 
 void AP_Radar::handle_msg(const mavlink_message_t &msg)
@@ -114,9 +114,11 @@ uint8_t AP_Radar::get_next_healthy_peer(uint8_t current_id)
 {
         // Try all available slots
 	for (uint8_t i = 1; i < RADAR_MAX_PEERS; i++) {
-            uint8_t next_peer = (current_id + i) % RADAR_MAX_PEERS;
-            if (get_peer_healthy(next_peer)) {
-                return next_peer;
+        uint8_t next_id = current_id + i;
+        uint8_t next_peer = next_id >= RADAR_MAX_PEERS ? next_id - RADAR_MAX_PEERS : next_id;
+        if (get_peer_healthy(next_peer))
+        {
+            return next_peer;
             }
         }
         return current_id;

diff --git a/libraries/AP_Radar/AP_Radar_MSP.cpp b/libraries/AP_Radar/AP_Radar_MSP.cpp
@@ -56,7 +56,7 @@ void AP_Radar_MSP::update(void)
 void AP_Radar_MSP::handle_msp(const MSP::msp_radar_pos_message_t &pkt)
 {
     // record peer state
-    uint8_t id = pkt.radar_no;
+    uint8_t id = pkt.radar_no - 1U;
     peers[id].radar_no = pkt.radar_no;
     peers[id].state = pkt.state;
     peers[id].location = Location(pkt.lat, pkt.lon, pkt.alt, Location::AltFrame::ABSOLUTE);