AP_Proximity: Add OBSTACLE_DISTANCE_3D mavlink message

libraries/AP_Proximity/AP_Proximity_MAV.cpp

@@ -21,7 +21,7 @@
 extern const AP_HAL::HAL& hal;
 
 #define PROXIMITY_MAV_TIMEOUT_MS    500 // distance messages must arrive within this many milliseconds
-#define PROXIMITY_3D_MSG_TIMEOUT_MS  50  // mini-fence will be cleared if OBSTACLE_DISTANCE_3D message does not arrive within this many milliseconds
+#define PROXIMITY_3D_MSG_TIMEOUT_MS  50  // boundary will be reset if OBSTACLE_DISTANCE_3D message does not arrive within this many milliseconds
 // update the state of the sensor
 void AP_Proximity_MAV::update(void)
 {
@@ -180,4 +180,60 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
             }
         }
     }
+
+    if (msg.msgid == MAVLINK_MSG_ID_OBSTACLE_DISTANCE_3D) {
+        mavlink_obstacle_distance_3d_t packet;
+        mavlink_msg_obstacle_distance_3d_decode(&msg, &packet);
+
+        const uint32_t previous_sys_time = _last_update_ms;
+        _last_update_ms = AP_HAL::millis();
+        // time_diff will check if the new message arrived significantly later than the last message
+        const uint32_t time_diff = _last_update_ms - previous_sys_time;
+
+        const uint32_t previous_msg_timestamp = _last_3d_msg_update_ms;
+        _last_3d_msg_update_ms = packet.time_boot_ms;
+        bool clear_fence = false;
+        
+        // we will add on to the last fence if the time stamp is the same
+        // provided we got the new obstacle in less than PROXIMITY_3D_MSG_TIMEOUT_MS  
+        if ((previous_msg_timestamp != _last_3d_msg_update_ms) || (time_diff > PROXIMITY_3D_MSG_TIMEOUT_MS)) {
+            clear_fence = true;
+        }
+
+        _distance_min = packet.min_distance;
+        _distance_max = packet.max_distance;
+        
+        Vector3f current_pos;
+        Matrix3f body_to_ned;
+        const bool database_ready = database_prepare_for_push(current_pos, body_to_ned);
+
+        if (clear_fence) {
+            // cleared fence back to defaults since we have a new timestamp
+            boundary.reset_all_sectors_and_stacks(); 
+        }
+
+        const Vector3f obstacle(packet.x, packet.y, packet.z);
+        if (obstacle.length() < _distance_min || obstacle.length() > _distance_max || obstacle.is_zero()) {
+            // message isn't healthy
+            return;
+        }
+        // extract yaw and pitch from Obstacle Vector
+        const float yaw = wrap_360(degrees(atan2f(obstacle.y, obstacle.x)));
+        const float pitch = wrap_180(degrees(M_PI_2 - atan2f(norm(obstacle.x, obstacle.y), obstacle.z))); 
+
+        // allot them correct stack and sector based on calculated pitch and yaw
+        const boundary_location bnd_loc = boundary.get_sector(yaw, pitch);
+
+        if (boundary.get_distance(bnd_loc) < obstacle.length()) {
+            // we already have a shorter distance in this stack and sector
+            return;
+        }
+
+        boundary.set_attributes(bnd_loc, yaw, pitch, obstacle.length());
+        boundary.update_boundary(bnd_loc);
+
+        if (database_ready) {
+            database_push(yaw, obstacle.length(),_last_update_ms, current_pos, body_to_ned, pitch);
+        }
+    }
 }

libraries/AP_Proximity/AP_Proximity_MAV.h

@@ -20,16 +20,17 @@ public:
     // get distance upwards in meters. returns true on success
     bool get_upward_distance(float &distance) const override;
 
-    // handle mavlink DISTANCE_SENSOR messages
+    // handle mavlink messages
     void handle_msg(const mavlink_message_t &msg) override;
 
 private:
     // horizontal distance support
-    uint32_t _last_update_ms;   // system time of last DISTANCE_SENSOR message received
+    uint32_t _last_update_ms;   // system time of last mavlink message received
+    uint32_t _last_3d_msg_update_ms;   // last stored OBSTACLE_DISTANCE_3D message timestamp
     float _distance_max;        // max range of sensor in meters
     float _distance_min;        // min range of sensor in meters
 
     // upward distance support
-    uint32_t _last_upward_update_ms;    // system time of last update distance
+    uint32_t _last_upward_update_ms;    // system time of last update of upward distance
     float _distance_upward;             // upward distance in meters
 };