From 2e1af5905714c170fda361b91ae7620f845cbfb1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 16 Jun 2022 18:11:26 +1000 Subject: [PATCH] AP_Rangefinder: fixed scaling on PWM driver and enable SCALING parameter this fixes a bug introduced here: https://github.com/ArduPilot/ardupilot/pull/18829 and allows the scaling of PWM rangefinders to be adjusted --- libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp index c30c93c418..f8aedca854 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PWM.cpp @@ -29,6 +29,8 @@ AP_RangeFinder_PWM::AP_RangeFinder_PWM(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Backend(_state, _params), estimated_terrain_height(_estimated_terrain_height) { + // this gives one mm per us + params.scaling.set_default(1.0); } /* @@ -47,7 +49,8 @@ bool AP_RangeFinder_PWM::get_reading(float &reading_m) return false; } - reading_m = value_us * 10.0f; // correct for LidarLite. Parameter needed? Converts from decimetres -> m here + // LidarLite uses one mm per us + reading_m = value_us * 0.001 * params.scaling; return true; }