AP_RangeFinder: add sensor type to backend

libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp

@@ -40,7 +40,7 @@ volatile struct range *rangerpru;
    already know that we should setup the rangefinder
 */
 AP_RangeFinder_BBB_PRU::AP_RangeFinder_BBB_PRU(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
-    AP_RangeFinder_Backend(_ranger, instance, _state)
+    AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_ULTRASOUND)
 {
 }
 

libraries/AP_RangeFinder/AP_RangeFinder_Bebop.cpp

@@ -64,7 +64,7 @@ static const uint16_t waveform_mode1[32] = {
 
 AP_RangeFinder_Bebop::AP_RangeFinder_Bebop(RangeFinder &_ranger,
         uint8_t instance, RangeFinder::RangeFinder_State &_state) :
-    AP_RangeFinder_Backend(_ranger, instance, _state),
+    AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_ULTRASOUND),
     _thread(new Linux::Thread(FUNCTOR_BIND_MEMBER(&AP_RangeFinder_Bebop::_loop, void)))
 {
     _init();

libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.cpp

@@ -27,7 +27,7 @@ extern const AP_HAL::HAL& hal;
 AP_RangeFinder_LeddarOne::AP_RangeFinder_LeddarOne(RangeFinder &_ranger, uint8_t instance,
                                                                RangeFinder::RangeFinder_State &_state,
                                                                AP_SerialManager &serial_manager) :
-    AP_RangeFinder_Backend(_ranger, instance, _state)
+    AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_LASER)
 {
     uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0);
     if (uart != nullptr) {

libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp

@@ -27,7 +27,7 @@ extern const AP_HAL::HAL& hal;
    already know that we should setup the rangefinder
 */
 AP_RangeFinder_LightWareI2C::AP_RangeFinder_LightWareI2C(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
-    : AP_RangeFinder_Backend(_ranger, instance, _state)
+    : AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_LASER)
     , _dev(std::move(dev)) {}
 
 /*

libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp

@@ -28,7 +28,7 @@ extern const AP_HAL::HAL& hal;
 AP_RangeFinder_LightWareSerial::AP_RangeFinder_LightWareSerial(RangeFinder &_ranger, uint8_t instance,
                                                                RangeFinder::RangeFinder_State &_state,
                                                                AP_SerialManager &serial_manager) :
-    AP_RangeFinder_Backend(_ranger, instance, _state)
+    AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_LASER)
 {
     uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0);
     if (uart != nullptr) {

libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp

@@ -26,7 +26,7 @@ extern const AP_HAL::HAL& hal;
    already know that we should setup the rangefinder
 */
 AP_RangeFinder_MAVLink::AP_RangeFinder_MAVLink(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
-    AP_RangeFinder_Backend(_ranger, instance, _state)
+    AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_UNKNOWN)
 {
     last_update_ms = AP_HAL::millis();
     distance_cm = 0;

libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp

@@ -36,7 +36,7 @@ extern const AP_HAL::HAL& hal;
    already know that we should setup the rangefinder
 */
 AP_RangeFinder_MaxsonarI2CXL::AP_RangeFinder_MaxsonarI2CXL(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state)
-    : AP_RangeFinder_Backend(_ranger, instance, _state)
+    : AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_ULTRASOUND)
     , _dev(hal.i2c_mgr->get_device(1, AP_RANGE_FINDER_MAXSONARI2CXL_DEFAULT_ADDR))
 {
 }

libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp

@@ -32,7 +32,7 @@ extern const AP_HAL::HAL& hal;
 AP_RangeFinder_MaxsonarSerialLV::AP_RangeFinder_MaxsonarSerialLV(RangeFinder &_ranger, uint8_t instance,
                                                                  RangeFinder::RangeFinder_State &_state,
                                                                  AP_SerialManager &serial_manager) :
-    AP_RangeFinder_Backend(_ranger, instance, _state)
+    AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_ULTRASOUND)
 {
     uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar, 0);
     if (uart != nullptr) {

libraries/AP_RangeFinder/AP_RangeFinder_PX4_PWM.cpp

@@ -44,7 +44,7 @@ extern "C" {
    already know that we should setup the rangefinder
 */
 AP_RangeFinder_PX4_PWM::AP_RangeFinder_PX4_PWM(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
-	AP_RangeFinder_Backend(_ranger, instance, _state),
+	AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_UNKNOWN),
     _last_timestamp(0),
     _last_pulse_time_ms(0),
     _disable_time_ms(0),

libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp

@@ -47,7 +47,7 @@ extern const AP_HAL::HAL& hal;
 AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(uint8_t bus, RangeFinder &_ranger, uint8_t instance,
                                                              RangeFinder::RangeFinder_State &_state,
                                                              RangeFinder::RangeFinder_Type _rftype)
-    : AP_RangeFinder_Backend(_ranger, instance, _state)
+    : AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_LASER)
     , _dev(hal.i2c_mgr->get_device(bus, LL40LS_ADDR))
     , rftype(_rftype)
 {

libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp

@@ -32,7 +32,7 @@ extern const AP_HAL::HAL& hal;
    already know that we should setup the rangefinder
 */
 AP_RangeFinder_analog::AP_RangeFinder_analog(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
-    AP_RangeFinder_Backend(_ranger, instance, _state)
+    AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_UNKNOWN)
 {
     source = hal.analogin->channel(ranger._pin[instance]);
     if (source == nullptr) {

libraries/AP_RangeFinder/AP_RangeFinder_trone.cpp

@@ -36,7 +36,7 @@ extern const AP_HAL::HAL& hal;
    already know that we should setup the rangefinder
 */
 AP_RangeFinder_trone::AP_RangeFinder_trone(uint8_t bus, RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state)
-    : AP_RangeFinder_Backend(_ranger, instance, _state)
+    : AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_LASER)
     , dev(hal.i2c_mgr->get_device(bus, TRONE_I2C_ADDR))
 {
 }

libraries/AP_RangeFinder/AP_RangeFinder_uLanding.cpp

@@ -28,7 +28,7 @@ extern const AP_HAL::HAL& hal;
 AP_RangeFinder_uLanding::AP_RangeFinder_uLanding(RangeFinder &_ranger, uint8_t instance,
                                                              RangeFinder::RangeFinder_State &_state,
                                                              AP_SerialManager &serial_manager) :
-    AP_RangeFinder_Backend(_ranger, instance, _state)
+    AP_RangeFinder_Backend(_ranger, instance, _state, MAV_DISTANCE_SENSOR_RADAR)
 {
     uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Aerotenna_uLanding, 0);
     if (uart != nullptr) {

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -881,3 +881,24 @@ const Vector3f &RangeFinder::get_pos_offset_orient(enum Rotation orientation) co
     }
     return pos_offset_zero;
 }
+
+MAV_DISTANCE_SENSOR RangeFinder::get_sensor_type(uint8_t instance) const {
+    // sanity check instance
+    if (instance >= RANGEFINDER_MAX_INSTANCES) {
+        return MAV_DISTANCE_SENSOR_UNKNOWN;
+    }
+
+    if (drivers[instance] == nullptr || _type[instance] == RangeFinder_TYPE_NONE) {
+        return MAV_DISTANCE_SENSOR_UNKNOWN;
+    }
+    return drivers[instance]->get_sensor_type();
+}
+
+MAV_DISTANCE_SENSOR RangeFinder::get_sensor_type_orient(enum Rotation orientation) const
+{
+    uint8_t i;
+    if (find_instance(orientation, i)) {
+        return get_sensor_type(i);
+    }
+    return MAV_DISTANCE_SENSOR_UNKNOWN;
+}

libraries/AP_RangeFinder/RangeFinder.h

@@ -155,6 +155,10 @@ public:
     }
     int16_t ground_clearance_cm_orient(enum Rotation orientation) const;
 
+    MAV_DISTANCE_SENSOR get_sensor_type(uint8_t instance) const;
+
+    MAV_DISTANCE_SENSOR get_sensor_type_orient(enum Rotation orientation) const;
+
     // query status
     RangeFinder_Status status(uint8_t instance) const;
     RangeFinder_Status status_orient(enum Rotation orientation) const;

libraries/AP_RangeFinder/RangeFinder_Backend.cpp

@@ -24,9 +24,10 @@ extern const AP_HAL::HAL& hal;
   base class constructor. 
   This incorporates initialisation as well.
 */
-AP_RangeFinder_Backend::AP_RangeFinder_Backend(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) :
+AP_RangeFinder_Backend::AP_RangeFinder_Backend(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state, MAV_DISTANCE_SENSOR _sensor_type) :
         ranger(_ranger),
-        state(_state) 
+        state(_state),
+        sensor_type(_sensor_type)
 {
     _sem = hal.util->new_semaphore();    
 }

libraries/AP_RangeFinder/RangeFinder_Backend.h

@@ -22,7 +22,7 @@ class AP_RangeFinder_Backend
 {
 public:
     // constructor. This incorporates initialisation as well.
-	AP_RangeFinder_Backend(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state);
+	AP_RangeFinder_Backend(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state, MAV_DISTANCE_SENSOR _sensor_type);
 
     // we declare a virtual destructor so that RangeFinder drivers can
     // override with a custom destructor if need be
@@ -36,6 +36,10 @@ public:
         return ranger._powersave_range > 0 && ranger.estimated_terrain_height > ranger._powersave_range;
     }
 
+    MAV_DISTANCE_SENSOR get_sensor_type() const {
+        return sensor_type;
+    }
+
     virtual void handle_msg(mavlink_message_t *msg) { return; }
 
 protected:
@@ -48,6 +52,7 @@ protected:
 
     RangeFinder &ranger;
     RangeFinder::RangeFinder_State &state;
+    MAV_DISTANCE_SENSOR sensor_type;
 
     // semaphore for access to shared frontend data
     AP_HAL::Semaphore *_sem;