diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index 52f2d5ad45..1015b6aeb1 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -230,26 +230,6 @@ AP_Proximity::Type AP_Proximity::get_type(uint8_t instance) const return Type::None; } -// return sensor orientation -uint8_t AP_Proximity::get_orientation(uint8_t instance) const -{ - if (!valid_instance(instance)) { - return 0; - } - - return params[instance].orientation.get(); -} - -// return sensor yaw correction -int16_t AP_Proximity::get_yaw_correction(uint8_t instance) const -{ - if (!valid_instance(instance)) { - return 0; - } - - return params[instance].yaw_correction.get(); -} - // return sensor health AP_Proximity::Status AP_Proximity::get_status(uint8_t instance) const { diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index 9dd2948574..a857618754 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -76,9 +76,7 @@ public: // return sensor type of a given instance Type get_type(uint8_t instance) const; - // return sensor orientation and yaw correction - uint8_t get_orientation(uint8_t instance) const; - int16_t get_yaw_correction(uint8_t instance) const; + // return distance filter frequency float get_filter_freq() const { return _filt_freq; } // return sensor health diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.cpp b/libraries/AP_Proximity/AP_Proximity_Backend.cpp index 02bc6103b5..2c758a7a24 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Backend.cpp @@ -47,8 +47,8 @@ void AP_Proximity_Backend::set_status(AP_Proximity::Status status) // correct an angle (in degrees) based on the orientation and yaw correction parameters float AP_Proximity_Backend::correct_angle_for_orientation(float angle_degrees) const { - const float angle_sign = (frontend.get_orientation(state.instance) == 1) ? -1.0f : 1.0f; - return wrap_360(angle_degrees * angle_sign + frontend.get_yaw_correction(state.instance)); + const float angle_sign = (params.orientation == 1) ? -1.0f : 1.0f; + return wrap_360(angle_degrees * angle_sign + params.yaw_correction); } // check if a reading should be ignored because it falls into an ignore area (check_for_ign_area should be sent as false if this check is not needed) diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp index c182b7c2b7..8204a08001 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp @@ -307,8 +307,8 @@ void AP_Proximity_LightWareSF40C::process_message() // process each point const float angle_inc_deg = (1.0f / point_total) * 360.0f; - const float angle_sign = (frontend.get_orientation(state.instance) == 1) ? -1.0f : 1.0f; - const float angle_correction = frontend.get_yaw_correction(state.instance); + const float angle_sign = (params.orientation == 1) ? -1.0f : 1.0f; + const float angle_correction = params.yaw_correction; const uint16_t dist_min_cm = distance_min() * 100; const uint16_t dist_max_cm = distance_max() * 100; diff --git a/libraries/AP_Proximity/AP_Proximity_MAV.cpp b/libraries/AP_Proximity/AP_Proximity_MAV.cpp index a9d2814abf..254f1f36d7 100644 --- a/libraries/AP_Proximity/AP_Proximity_MAV.cpp +++ b/libraries/AP_Proximity/AP_Proximity_MAV.cpp @@ -141,9 +141,9 @@ void AP_Proximity_MAV::handle_obstacle_distance_msg(const mavlink_message_t &msg _last_update_ms = AP_HAL::millis(); // get user configured yaw correction from front end - const float param_yaw_offset = constrain_float(frontend.get_yaw_correction(state.instance), -360.0f, +360.0f); + const float param_yaw_offset = constrain_float(params.yaw_correction, -360.0f, +360.0f); const float yaw_correction = wrap_360(param_yaw_offset + packet.angle_offset); - if (frontend.get_orientation(state.instance) != 0) { + if (params.orientation != 0) { increment *= -1; } diff --git a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp index 6f30df2fd6..63d4c82230 100644 --- a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp +++ b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp @@ -305,8 +305,8 @@ void AP_Proximity_RPLidarA2::parse_response_data() Debug(2, "UART %02x %02x%02x %02x%02x", payload[0], payload[2], payload[1], payload[4], payload[3]); //show HEX values // check if valid SCAN packet: a valid packet starts with startbits which are complementary plus a checkbit in byte+1 if ((payload.sensor_scan.startbit == !payload.sensor_scan.not_startbit) && payload.sensor_scan.checkbit) { - const float angle_sign = (frontend.get_orientation(state.instance) == 1) ? -1.0f : 1.0f; - const float angle_deg = wrap_360(payload.sensor_scan.angle_q6/64.0f * angle_sign + frontend.get_yaw_correction(state.instance)); + const float angle_sign = (params.orientation == 1) ? -1.0f : 1.0f; + const float angle_deg = wrap_360(payload.sensor_scan.angle_q6/64.0f * angle_sign + params.yaw_correction); const float distance_m = (payload.sensor_scan.distance_q2/4000.0f); #if RP_DEBUG_LEVEL >= 2 const uint8_t quality = payload.sensor_scan.quality;