Copter: send upward DISTANCE_SENSOR message to GCS

Yukong · Apr 20, 2017 · 3348ab4 · 3348ab4
1 parent 496e8f3
commit 3348ab4
Showing 1 changed file with 21 additions and 6 deletions.
diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp
@@ -234,15 +234,13 @@ void NOINLINE Copter::send_proximity(mavlink_channel_t chan, uint16_t count_max)
         return;
     }
 
-    // send at most 8 distances
-    if (count_max > 8) {
-        count_max = 8;
-    }
+    bool send_upwards = true;
 
-    // send known distances
+    // send horizontal distances
     AP_Proximity::Proximity_Distance_Array dist_array;
-        for (uint8_t i=0; i<count_max; i++) {
+    uint8_t horiz_count = MIN(count_max, 8);    // send at most 8 horizontal distances
     if (g2.proximity.get_horizontal_distances(dist_array)) {
+        for (uint8_t i=0; i<horiz_count; i++) {
             mavlink_msg_distance_sensor_send(
                 chan,
                 AP_HAL::millis(),               //  time since system boot
@@ -254,6 +252,23 @@ void NOINLINE Copter::send_proximity(mavlink_channel_t chan, uint16_t count_max)
                 dist_array.orientation[i],      //  direction the sensor faces from MAV_SENSOR_ORIENTATION enum
                 1);                             // Measurement covariance in centimeters, 0 for unknown / invalid readings
         }
+        // check if we still have room to send upwards distance
+        send_upwards = (count_max > 8);
+    }
+
+    // send upward distance
+    float dist_up;
+    if (send_upwards && g2.proximity.get_upward_distance(dist_up)) {
+        mavlink_msg_distance_sensor_send(
+            chan,
+            AP_HAL::millis(),               //  time since system boot
+            (uint16_t)(g2.proximity.distance_min() * 100),  // minimum distance the sensor can measure in centimeters
+            (uint16_t)(g2.proximity.distance_max() * 100),  // maximum distance the sensor can measure in centimeters
+            (uint16_t)(dist_up * 100),      // current distance reading
+            MAV_DISTANCE_SENSOR_LASER,      // type from MAV_DISTANCE_SENSOR enum
+            0,                              // onboard ID of the sensor
+            MAV_SENSOR_ROTATION_PITCH_90,   // direction upwards
+            1);                             // Measurement covariance in centimeters, 0 for unknown / invalid readings
     }
 #endif
 }