From e7dd21a0bb299d4fb965a87039fb30c46d574bf7 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 9 Feb 2017 19:26:57 +0900
Subject: [PATCH] AP_RangeFinder: remove primary, add orientation

---
 libraries/AP_RangeFinder/RangeFinder.cpp | 154 ++++++++++++++++++++---
 libraries/AP_RangeFinder/RangeFinder.h   |  55 ++++----
 2 files changed, 162 insertions(+), 47 deletions(-)

diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp
index 39fb66cc63..8c5ef28a6f 100644
--- a/libraries/AP_RangeFinder/RangeFinder.cpp
+++ b/libraries/AP_RangeFinder/RangeFinder.cpp
@@ -152,6 +152,13 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("_POS", 49, RangeFinder, _pos_offset[0], 0.0f),
 
+    // @Param: _ORIENT
+    // @DisplayName: Rangefinder orientation
+    // @Description: Orientation of rangefinder
+    // @Values: 0:Forward, 1:Forward-Right, 2:Right, 3:Back-Right, 4:Back, 5:Back-Left, 6:Left, 7:Forward-Left, 24:Up, 25:Down
+    // @User: Advanced
+    AP_GROUPINFO("_ORIENT", 53, RangeFinder, _orientation[0], ROTATION_PITCH_270),
+
 #if RANGEFINDER_MAX_INSTANCES > 1
     // @Param: 2_TYPE
     // @DisplayName: Second Rangefinder type
@@ -264,6 +271,12 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("2_POS", 50, RangeFinder, _pos_offset[1], 0.0f),
 
+    // @Param: 2_ORIENT
+    // @DisplayName: Rangefinder 2 orientation
+    // @Description: Orientation of 2nd rangefinder
+    // @Values: 0:Forward, 1:Forward-Right, 2:Right, 3:Back-Right, 4:Back, 5:Back-Left, 6:Left, 7:Forward-Left, 24:Up, 25:Down
+    // @User: Advanced
+    AP_GROUPINFO("2_ORIENT", 54, RangeFinder, _orientation[1], ROTATION_PITCH_270),
 #endif
 
 #if RANGEFINDER_MAX_INSTANCES > 2
@@ -379,6 +392,12 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("3_POS", 51, RangeFinder, _pos_offset[2], 0.0f),
 
+    // @Param: 3_ORIENT
+    // @DisplayName: Rangefinder 3 orientation
+    // @Description: Orientation of 3rd rangefinder
+    // @Values: 0:Forward, 1:Forward-Right, 2:Right, 3:Back-Right, 4:Back, 5:Back-Left, 6:Left, 7:Forward-Left, 24:Up, 25:Down
+    // @User: Advanced
+    AP_GROUPINFO("3_ORIENT", 55, RangeFinder, _orientation[2], ROTATION_PITCH_270),
 #endif
 
 #if RANGEFINDER_MAX_INSTANCES > 3
@@ -493,19 +512,30 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
     // @Units: m
     // @User: Advanced
     AP_GROUPINFO("4_POS", 52, RangeFinder, _pos_offset[3], 0.0f),
+
+    // @Param: 4_ORIENT
+    // @DisplayName: Rangefinder 4 orientation
+    // @Description: Orientation of 4th range finder
+    // @Values: 0:Forward, 1:Forward-Right, 2:Right, 3:Back-Right, 4:Back, 5:Back-Left, 6:Left, 7:Forward-Left, 24:Up, 25:Down
+    // @User: Advanced
+    AP_GROUPINFO("4_ORIENT", 56, RangeFinder, _orientation[3], ROTATION_PITCH_270),
 #endif
     
     AP_GROUPEND
 };
 
-RangeFinder::RangeFinder(AP_SerialManager &_serial_manager) :
-    primary_instance(0),
+RangeFinder::RangeFinder(AP_SerialManager &_serial_manager, enum Rotation orientation_default) :
     num_instances(0),
     estimated_terrain_height(0),
     serial_manager(_serial_manager)
 {
     AP_Param::setup_object_defaults(this, var_info);
 
+    // set orientation defaults
+    for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
+        _orientation[i].set_default(orientation_default);
+    }
+
     // init state and drivers
     memset(state,0,sizeof(state));
     memset(drivers,0,sizeof(drivers));
@@ -558,13 +588,6 @@ void RangeFinder::update(void)
             update_pre_arm_check(i);
         }
     }
-
-    // work out primary instance - first sensor returning good data
-    for (int8_t i=num_instances-1; i>=0; i--) {
-        if (drivers[i] != nullptr && (state[i].status == RangeFinder_Good)) {
-            primary_instance = i;
-        }
-    }
 }
 
 bool RangeFinder::_add_backend(AP_RangeFinder_Backend *backend)
@@ -688,13 +711,87 @@ RangeFinder::RangeFinder_Status RangeFinder::status(uint8_t instance) const
     return state[instance].status;
 }
 
-void RangeFinder::handle_msg(mavlink_message_t *msg) {
-  uint8_t i;
-  for (i=0; i<num_instances; i++) {
-      if ((drivers[i] != nullptr) && (_type[i] != RangeFinder_TYPE_NONE)) {
+RangeFinder::RangeFinder_Status RangeFinder::status_orient(enum Rotation orientation) const
+{
+    uint8_t i;
+    if (find_instance(orientation, i)) {
+        return status(i);
+    }
+    return RangeFinder_NotConnected;
+}
+
+void RangeFinder::handle_msg(mavlink_message_t *msg)
+{
+    uint8_t i;
+    for (i=0; i<num_instances; i++) {
+        if ((drivers[i] != nullptr) && (_type[i] != RangeFinder_TYPE_NONE)) {
           drivers[i]->handle_msg(msg);
-      }
-  }
+        }
+    }
+}
+
+// return true if we have a range finder with the specified orientation
+bool RangeFinder::has_orientation(enum Rotation orientation) const
+{
+    uint8_t i;
+    return find_instance(orientation, i);
+}
+
+// find first range finder instance with the specified orientation
+bool RangeFinder::find_instance(enum Rotation orientation, uint8_t &instance) const
+{
+    for (uint8_t i=0; i<num_instances; i++) {
+        if (_orientation[i] == orientation) {
+            instance = i;
+            return true;
+        }
+    }
+    return false;
+}
+
+uint16_t RangeFinder::distance_cm_orient(enum Rotation orientation) const
+{
+    uint8_t i;
+    if (find_instance(orientation, i)) {
+        return distance_cm(i);
+    }
+    return 0;
+}
+
+uint16_t RangeFinder::voltage_mv_orient(enum Rotation orientation) const
+{
+    uint8_t i;
+    if (find_instance(orientation, i)) {
+        return voltage_mv(i);
+    }
+    return 0;
+}
+
+int16_t RangeFinder::max_distance_cm_orient(enum Rotation orientation) const
+{
+    uint8_t i;
+    if (find_instance(orientation, i)) {
+        return max_distance_cm(i);
+    }
+    return 0;
+}
+
+int16_t RangeFinder::min_distance_cm_orient(enum Rotation orientation) const
+{
+    uint8_t i;
+    if (find_instance(orientation, i)) {
+        return min_distance_cm(i);
+    }
+    return 0;
+}
+
+int16_t RangeFinder::ground_clearance_cm_orient(enum Rotation orientation) const
+{
+    uint8_t i;
+    if (find_instance(orientation, i)) {
+        return ground_clearance_cm(i);
+    }
+    return 0;
 }
 
 // true if sensor is returning data
@@ -707,6 +804,24 @@ bool RangeFinder::has_data(uint8_t instance) const
     return ((state[instance].status != RangeFinder_NotConnected) && (state[instance].status != RangeFinder_NoData));
 }
 
+bool RangeFinder::has_data_orient(enum Rotation orientation) const
+{
+    uint8_t i;
+    if (find_instance(orientation, i)) {
+        return has_data(i);
+    }
+    return false;
+}
+
+uint8_t RangeFinder::range_valid_count_orient(enum Rotation orientation) const
+{
+    uint8_t i;
+    if (find_instance(orientation, i)) {
+        return range_valid_count(i);
+    }
+    return 0;
+}
+
 /*
   returns true if pre-arm checks have passed for all range finders
   these checks involve the user lifting or rotating the vehicle so that sensor readings between
@@ -748,3 +863,12 @@ void RangeFinder::update_pre_arm_check(uint8_t instance)
         state[instance].pre_arm_check = true;
     }
 }
+
+const Vector3f &RangeFinder::get_pos_offset_orient(enum Rotation orientation) const
+{
+    uint8_t i=0;
+    if (find_instance(orientation, i)) {
+        return get_pos_offset(i);
+    }
+    return pos_offset_zero;
+}
diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h
index ea2556e25f..45a2c361a3 100644
--- a/libraries/AP_RangeFinder/RangeFinder.h
+++ b/libraries/AP_RangeFinder/RangeFinder.h
@@ -33,7 +33,7 @@ class RangeFinder
 public:
     friend class AP_RangeFinder_Backend;
 
-    RangeFinder(AP_SerialManager &_serial_manager);
+    RangeFinder(AP_SerialManager &_serial_manager, enum Rotation orientation_default);
 
     // RangeFinder driver types
     enum RangeFinder_Type {
@@ -96,6 +96,7 @@ public:
     AP_Int8  _address[RANGEFINDER_MAX_INSTANCES];
     AP_Int16 _powersave_range;
     AP_Vector3f _pos_offset[RANGEFINDER_MAX_INSTANCES]; // position offset in body frame
+    AP_Int8  _orientation[RANGEFINDER_MAX_INSTANCES];
 
     static const struct AP_Param::GroupInfo var_info[];
     
@@ -116,63 +117,55 @@ public:
 
 #define _RangeFinder_STATE(instance) state[instance]
 
-    uint8_t get_primary(void) const {
-        return primary_instance;
+    // return true if we have a range finder with the specified orientation
+    bool has_orientation(enum Rotation orientation) const;
+
+    // find first range finder instance with the specified orientation
+    bool find_instance(enum Rotation orientation, uint8_t &instance) const;
+
+    // get orientation of a given range finder
+    enum Rotation get_orientation(uint8_t instance) const {
+        return (instance<num_instances? (enum Rotation)_orientation[instance].get() : ROTATION_NONE);
     }
-    
+
     uint16_t distance_cm(uint8_t instance) const {
         return (instance<num_instances? _RangeFinder_STATE(instance).distance_cm : 0);
     }
-    uint16_t distance_cm() const {
-        return distance_cm(primary_instance);
-    }
+    uint16_t distance_cm_orient(enum Rotation orientation) const;
 
     uint16_t voltage_mv(uint8_t instance) const {
         return _RangeFinder_STATE(instance).voltage_mv;
     }
-    uint16_t voltage_mv() const {
-        return voltage_mv(primary_instance);
-    }
+    uint16_t voltage_mv_orient(enum Rotation orientation) const;
 
     int16_t max_distance_cm(uint8_t instance) const {
         return _max_distance_cm[instance];
     }
-    int16_t max_distance_cm() const {
-        return max_distance_cm(primary_instance);
-    }
+    int16_t max_distance_cm_orient(enum Rotation orientation) const;
 
     int16_t min_distance_cm(uint8_t instance) const {
         return _min_distance_cm[instance];
     }
-    int16_t min_distance_cm() const {
-        return min_distance_cm(primary_instance);
-    }
+    int16_t min_distance_cm_orient(enum Rotation orientation) const;
+
     int16_t ground_clearance_cm(uint8_t instance) const {
         return _ground_clearance_cm[instance];
     }
-    int16_t ground_clearance_cm() const {
-        return _ground_clearance_cm[primary_instance];
-    }
+    int16_t ground_clearance_cm_orient(enum Rotation orientation) const;
 
     // query status
     RangeFinder_Status status(uint8_t instance) const;
-    RangeFinder_Status status(void) const {
-        return status(primary_instance);
-    }
+    RangeFinder_Status status_orient(enum Rotation orientation) const;
 
     // true if sensor is returning data
     bool has_data(uint8_t instance) const;
-    bool has_data() const {
-        return has_data(primary_instance);
-    }
+    bool has_data_orient(enum Rotation orientation) const;
 
     // returns count of consecutive good readings
-    uint8_t range_valid_count() const {
-        return range_valid_count(primary_instance);
-    }
     uint8_t range_valid_count(uint8_t instance) const {
         return _RangeFinder_STATE(instance).range_valid_count;
     }
+    uint8_t range_valid_count_orient(enum Rotation orientation) const;
 
     /*
       set an externally estimated terrain height. Used to enable power
@@ -193,17 +186,15 @@ public:
     const Vector3f &get_pos_offset(uint8_t instance) const {
         return _pos_offset[instance];
     }
-    const Vector3f &get_pos_offset(void) const {
-        return _pos_offset[primary_instance];
-    }
+    const Vector3f &get_pos_offset_orient(enum Rotation orientation) const;
 
 private:
     RangeFinder_State state[RANGEFINDER_MAX_INSTANCES];
     AP_RangeFinder_Backend *drivers[RANGEFINDER_MAX_INSTANCES];
-    uint8_t primary_instance:3;
     uint8_t num_instances:3;
     float estimated_terrain_height;
     AP_SerialManager &serial_manager;
+    Vector3f pos_offset_zero;   // allows returning position offsets of zero for invalid requests
 
     void detect_instance(uint8_t instance);
     void update_instance(uint8_t instance);