From 8bfb1d24450792ce3d8f4c378cfa3e5f86b81fe6 Mon Sep 17 00:00:00 2001
From: Raouf <raoufbadjadja@gmail.com>
Date: Thu, 6 Sep 2018 19:17:25 +0900
Subject: [PATCH] AP_Proximity: add support for OBSTACLE_DISTANCE message

---
 libraries/AP_Proximity/AP_Proximity_MAV.cpp | 94 ++++++++++++++++++---
 1 file changed, 80 insertions(+), 14 deletions(-)

diff --git a/libraries/AP_Proximity/AP_Proximity_MAV.cpp b/libraries/AP_Proximity/AP_Proximity_MAV.cpp
index 5a547d766b..b9a22dd3ee 100644
--- a/libraries/AP_Proximity/AP_Proximity_MAV.cpp
+++ b/libraries/AP_Proximity/AP_Proximity_MAV.cpp
@@ -57,24 +57,90 @@ bool AP_Proximity_MAV::get_upward_distance(float &distance) const
 // handle mavlink DISTANCE_SENSOR messages
 void AP_Proximity_MAV::handle_msg(mavlink_message_t *msg)
 {
-    mavlink_distance_sensor_t packet;
-    mavlink_msg_distance_sensor_decode(msg, &packet);
+    if (msg->msgid == MAVLINK_MSG_ID_DISTANCE_SENSOR) {
+        mavlink_distance_sensor_t packet;
+        mavlink_msg_distance_sensor_decode(msg, &packet);
 
-    // store distance to appropriate sector based on orientation field
-    if (packet.orientation <= MAV_SENSOR_ROTATION_YAW_315) {
-        uint8_t sector = packet.orientation;
-        _angle[sector] = sector * 45;
-        _distance[sector] = packet.current_distance / 100.0f;
+        // store distance to appropriate sector based on orientation field
+        if (packet.orientation <= MAV_SENSOR_ROTATION_YAW_315) {
+            uint8_t sector = packet.orientation;
+            _angle[sector] = sector * 45;
+            _distance[sector] = packet.current_distance / 100.0f;
+            _distance_min = packet.min_distance / 100.0f;
+            _distance_max = packet.max_distance / 100.0f;
+            _distance_valid[sector] = (_distance[sector] >= _distance_min) && (_distance[sector] <= _distance_max);
+            _last_update_ms = AP_HAL::millis();
+            update_boundary_for_sector(sector);
+        }
+
+        // store upward distance
+        if (packet.orientation == MAV_SENSOR_ROTATION_PITCH_90) {
+            _distance_upward = packet.current_distance / 100.0f;
+            _last_upward_update_ms = AP_HAL::millis();
+        }
+        return;
+    }
+
+    if (msg->msgid == MAVLINK_MSG_ID_OBSTACLE_DISTANCE) {
+        mavlink_obstacle_distance_t packet;
+        mavlink_msg_obstacle_distance_decode(msg, &packet);
+
+        // check increment (message's sector width)
+        uint8_t increment = packet.increment;
+        if (packet.increment == 0) {
+            return;
+        }
+
+        const float MAX_DISTANCE = 9999.0f;
+        const uint8_t total_distances = MIN(360.0f / increment, 72);
+
+        // set distance min and max
         _distance_min = packet.min_distance / 100.0f;
         _distance_max = packet.max_distance / 100.0f;
-        _distance_valid[sector] = (_distance[sector] >= _distance_min) && (_distance[sector] <= _distance_max);
         _last_update_ms = AP_HAL::millis();
-        update_boundary_for_sector(sector);
-    }
 
-    // store upward distance
-    if (packet.orientation == MAV_SENSOR_ROTATION_PITCH_90) {
-        _distance_upward = packet.current_distance / 100.0f;
-        _last_upward_update_ms = AP_HAL::millis();
+        // get user configured yaw correction from front end
+        const float yaw_correction = constrain_float(frontend.get_yaw_correction(state.instance), -360.0f, +360.0f);
+        float dir_correction;
+        if (frontend.get_orientation(state.instance) == 0) {
+            dir_correction = 1.0f;
+        } else {
+            dir_correction = -1.0f;
+        }
+
+        // initialise updated array and proximity sector angles (to closest object) and distances
+        bool sector_updated[_num_sectors];
+        float sector_width_half[_num_sectors];
+        for (uint8_t i = 0; i < _num_sectors; i++) {
+            sector_updated[i] = false;
+            sector_width_half[i] = _sector_width_deg[i] * 0.5f;
+            _angle[i] = _sector_middle_deg[i];
+            _distance[i] = MAX_DISTANCE;
+        }
+
+        // iterate over message's sectors
+        for (uint8_t j = 0; j < total_distances; j++) {
+            const float packet_distance_m = packet.distances[j] * 0.01f;
+            const float mid_angle = wrap_360(j * increment * dir_correction + yaw_correction);
+
+            // iterate over proximity sectors
+            for (uint8_t i = 0; i < _num_sectors; i++) {
+                float angle_diff = fabsf(wrap_180(_sector_middle_deg[i] - mid_angle));
+                // update distance array sector with shortest distance from message
+                if ((angle_diff <= sector_width_half[i]) && (packet_distance_m < _distance[i])) {
+                    _distance[i] = packet_distance_m;
+                    _angle[i] = mid_angle;
+                    sector_updated[i] = true;
+                }
+            }
+        }
+
+        // update proximity sectors validity and boundary point
+        for (uint8_t i = 0; i < _num_sectors; i++) {
+            _distance_valid[i] = (_distance[i] >= _distance_min) && (_distance[i] <= _distance_max);
+            if (sector_updated[i]) {
+                update_boundary_for_sector(i);
+            }
+        }
     }
 }