From 1c1caa937450ca63ea79f4e7740c63ffe2df7c0d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 7 Sep 2020 16:23:44 +0900 Subject: [PATCH] AP_Winch: correct Daiwa line lengtha and speed scaling --- libraries/AP_Winch/AP_Winch_Daiwa.cpp | 2 +- libraries/AP_Winch/AP_Winch_Daiwa.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Winch/AP_Winch_Daiwa.cpp b/libraries/AP_Winch/AP_Winch_Daiwa.cpp index 6af69c2564..ab810c997d 100644 --- a/libraries/AP_Winch/AP_Winch_Daiwa.cpp +++ b/libraries/AP_Winch/AP_Winch_Daiwa.cpp @@ -60,7 +60,7 @@ void AP_Winch_Daiwa::send_status(const GCS_MAVLINK &channel) } // convert speed percentage to absolute speed - const float speed_ms = fabsf(config.rate_max) * (float)latest.speed_pct; + const float speed_ms = fabsf(config.rate_max) * (float)latest.speed_pct * 0.01f; // send status mavlink_msg_winch_status_send( diff --git a/libraries/AP_Winch/AP_Winch_Daiwa.h b/libraries/AP_Winch/AP_Winch_Daiwa.h index cdbbdf58be..9e0cdf8a86 100644 --- a/libraries/AP_Winch/AP_Winch_Daiwa.h +++ b/libraries/AP_Winch/AP_Winch_Daiwa.h @@ -64,7 +64,7 @@ private: static const uint8_t buff_len_max = 20; // buffer maximum length static const int16_t output_dz = 100; // output deadzone in scale of -1000 to +1000 - const float line_length_correction_factor = 0.0357f; // convert winch counter to meters + const float line_length_correction_factor = 0.003333f; // convert winch counter to meters AP_HAL::UARTDriver *uart; char buff[buff_len_max]; // buffer holding latest data from winch