Sub: use only downward facing rangefinder

ArduSub/GCS_Mavlink.cpp

@@ -224,10 +224,10 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan)
 #endif
 
 #if RANGEFINDER_ENABLED == ENABLED
-    if (rangefinder.num_sensors() > 0) {
+    if (rangefinder_state.enabled) {
         control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
-        if (rangefinder.has_data()) {
+        if (rangefinder.has_data_orient(ROTATION_PITCH_270)) {
             control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
         }
     }
@@ -379,13 +379,13 @@ void NOINLINE Sub::send_current_waypoint(mavlink_channel_t chan)
 void NOINLINE Sub::send_rangefinder(mavlink_channel_t chan)
 {
     // exit immediately if rangefinder is disabled
-    if (!rangefinder.has_data()) {
+    if (!rangefinder.has_data_orient(ROTATION_PITCH_270)) {
         return;
     }
     mavlink_msg_rangefinder_send(
         chan,
-        rangefinder.distance_cm() * 0.01f,
-        rangefinder.voltage_mv() * 0.001f);
+        rangefinder.distance_cm_orient(ROTATION_PITCH_270) * 0.01f,
+        rangefinder.voltage_mv_orient(ROTATION_PITCH_270) * 0.001f);
 }
 #endif
 

ArduSub/Sub.h

@@ -176,7 +176,7 @@ private:
     Compass compass;
     AP_InertialSensor ins;
 
-    RangeFinder rangefinder {serial_manager};
+    RangeFinder rangefinder {serial_manager, ROTATION_PITCH_270};
     struct {
         bool enabled:1;
         bool alt_healthy:1; // true if we can trust the altitude from the rangefinder

ArduSub/control_auto.cpp

@@ -665,7 +665,7 @@ float Sub::get_auto_heading(void)
 bool Sub::auto_terrain_recover_start()
 {
     // Check rangefinder status to see if recovery is possible
-    switch (rangefinder.status()) {
+    switch (rangefinder.status_orient(ROTATION_PITCH_270)) {
 
     case RangeFinder::RangeFinder_OutOfRangeLow:
     case RangeFinder::RangeFinder_OutOfRangeHigh:
@@ -720,7 +720,7 @@ void Sub::auto_terrain_recover_run()
         return;
     }
 
-    switch (rangefinder.status()) {
+    switch (rangefinder.status_orient(ROTATION_PITCH_270)) {
 
     case RangeFinder::RangeFinder_OutOfRangeLow:
         target_climb_rate = wp_nav.get_speed_up();

ArduSub/sensors.cpp

@@ -25,7 +25,7 @@ void Sub::init_rangefinder(void)
 #if RANGEFINDER_ENABLED == ENABLED
     rangefinder.init();
     rangefinder_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ);
-    rangefinder_state.enabled = (rangefinder.num_sensors() >= 1);
+    rangefinder_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_270);
 #endif
 }
 
@@ -35,9 +35,9 @@ void Sub::read_rangefinder(void)
 #if RANGEFINDER_ENABLED == ENABLED
     rangefinder.update();
 
-    rangefinder_state.alt_healthy = ((rangefinder.status() == RangeFinder::RangeFinder_Good) && (rangefinder.range_valid_count() >= RANGEFINDER_HEALTH_MAX));
+    rangefinder_state.alt_healthy = ((rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::RangeFinder_Good) && (rangefinder.range_valid_count_orient(ROTATION_PITCH_270) >= RANGEFINDER_HEALTH_MAX));
 
-    int16_t temp_alt = rangefinder.distance_cm();
+    int16_t temp_alt = rangefinder.distance_cm_orient(ROTATION_PITCH_270);
 
 #if RANGEFINDER_TILT_CORRECTION == ENABLED
     // correct alt for angle of the rangefinder

ArduSub/switches.cpp

@@ -209,7 +209,7 @@ void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
     case AUXSW_RANGEFINDER:
         // enable or disable the sonar
 #if RANGEFINDER_ENABLED == ENABLED
-        if ((ch_flag == AUX_SWITCH_HIGH) && (rangefinder.num_sensors() >= 1)) {
+        if ((ch_flag == AUX_SWITCH_HIGH) && rangefinder.has_orientation(ROTATION_PITCH_270)) {
             rangefinder_state.enabled = true;
         } else {
             rangefinder_state.enabled = false;

ArduSub/test.cpp

@@ -253,9 +253,12 @@ int8_t Sub::test_rangefinder(uint8_t argc, const Menu::arg *argv)
         hal.scheduler->delay(100);
         rangefinder.update();
 
-        cliSerial->printf("Primary: status %d distance_cm %d \n", (int)rangefinder.status(), rangefinder.distance_cm());
-        cliSerial->printf("All: device_0 type %d status %d distance_cm %d, device_1 type %d status %d distance_cm %d\n",
-                          (int)rangefinder._type[0], (int)rangefinder.status(0), rangefinder.distance_cm(0), (int)rangefinder._type[1], (int)rangefinder.status(1), rangefinder.distance_cm(1));
+        for (uint8_t i=0; i<rangefinder.num_sensors(); i++) {
+            cliSerial->printf("Dev%d: status %d distance_cm %d\n",
+                    (int)i,
+                    (int)rangefinder.status(i),
+                    (int)rangefinder.distance_cm(i));
+        }
 
         if (cliSerial->available() > 0) {
             return (0);