Line data    Source code

       1             : #include "Sub.h"
       2             : 
       3             : // return barometric altitude in centimeters
       4       54502 : void Sub::read_barometer()
       5             : {
       6       54502 :     barometer.update();
       7             :     // If we are reading a positive altitude, the sensor needs calibration
       8             :     // Even a few meters above the water we should have no significant depth reading
       9       54502 :     if(barometer.get_altitude() > 0) {
      10          40 :         barometer.update_calibration();
      11             :     }
      12             : 
      13       54502 :     if (ap.depth_sensor_present) {
      14       54502 :         sensor_health.depth = barometer.healthy(depth_sensor_idx);
      15             :     }
      16       54502 : }
      17             : 
      18          82 : void Sub::init_rangefinder()
      19             : {
      20             : #if AP_RANGEFINDER_ENABLED
      21          82 :     rangefinder.set_log_rfnd_bit(MASK_LOG_CTUN);
      22          82 :     rangefinder.init(ROTATION_PITCH_270);
      23          82 :     rangefinder_state.alt_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ);
      24          82 :     rangefinder_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_270);
      25             : #endif
      26          82 : }
      27             : 
      28             : // return rangefinder altitude in centimeters
      29      109016 : void Sub::read_rangefinder()
      30             : {
      31             : #if AP_RANGEFINDER_ENABLED
      32      109016 :     rangefinder.update();
      33             : 
      34             :     // signal quality ranges from 0 (worst) to 100 (perfect), -1 means n/a
      35      109016 :     int8_t signal_quality_pct = rangefinder.signal_quality_pct_orient(ROTATION_PITCH_270);
      36             : 
      37      109016 :     rangefinder_state.alt_healthy =
      38      147665 :             (rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::Good) &&
      39      164373 :             (rangefinder.range_valid_count_orient(ROTATION_PITCH_270) >= RANGEFINDER_HEALTH_MAX) &&
      40       16708 :             (signal_quality_pct == -1 || signal_quality_pct >= g.rangefinder_signal_min);
      41             : 
      42      109016 :     float temp_alt_m = rangefinder.distance_orient(ROTATION_PITCH_270);
      43             : 
      44             : #if RANGEFINDER_TILT_CORRECTION
      45             :     // correct alt for angle of the rangefinder
      46             :     temp_alt_m = temp_alt_m * MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z);
      47             : #endif
      48             : 
      49      109016 :     rangefinder_state.alt = temp_alt_m;
      50      109016 :     rangefinder_state.inertial_alt_cm = pos_control.get_pos_estimate_U_m() * 100.0f;
      51      109016 :     rangefinder_state.min = rangefinder.min_distance_orient(ROTATION_PITCH_270);
      52      109016 :     rangefinder_state.max = rangefinder.max_distance_orient(ROTATION_PITCH_270);
      53             : 
      54             :     // calculate rangefinder_terrain_offset_cm
      55      109016 :     if (rangefinder_state.alt_healthy) {
      56       38473 :         uint32_t now = AP_HAL::millis();
      57       38473 :         if (now - rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS) {
      58             :             // reset filter if we haven't used it within the last second
      59          12 :             rangefinder_state.alt_filt.reset(rangefinder_state.alt);
      60             :         } else {
      61       38461 :             rangefinder_state.alt_filt.apply(rangefinder_state.alt, 0.05f);
      62             :         }
      63       38473 :         rangefinder_state.last_healthy_ms = now;
      64       38473 :         rangefinder_state.rangefinder_terrain_offset_cm =
      65       38473 :             sub.rangefinder_state.inertial_alt_cm - (sub.rangefinder_state.alt_filt.get() * 100);
      66             :     }
      67             : 
      68             :     // send rangefinder altitude and health to waypoint navigation library
      69      109016 :     wp_nav.set_rangefinder_terrain_U_cm(
      70      109016 :             rangefinder_state.enabled,
      71      109016 :             rangefinder_state.alt_healthy,
      72             :             rangefinder_state.rangefinder_terrain_offset_cm);
      73      109016 :     circle_nav.set_rangefinder_terrain_U_cm(
      74      109016 :             rangefinder_state.enabled && wp_nav.rangefinder_used(),
      75      109016 :             rangefinder_state.alt_healthy,
      76             :             rangefinder_state.rangefinder_terrain_offset_cm);
      77             : #endif  // AP_RANGEFINDER_ENABLED
      78      109016 : }
      79             : 
      80             : // return true if rangefinder_alt can be used
      81       84440 : bool Sub::rangefinder_alt_ok() const
      82             : {
      83       84440 :     uint32_t now = AP_HAL::millis();
      84       84440 :     return (rangefinder_state.enabled && rangefinder_state.alt_healthy && now - rangefinder_state.last_healthy_ms < RANGEFINDER_TIMEOUT_MS);
      85             : }