Browse Source

AP_Winch: correct Daiwa line lengtha and speed scaling

zr-v5.1
Randy Mackay 4 years ago
parent
commit
1c1caa9374
  1. 2
      libraries/AP_Winch/AP_Winch_Daiwa.cpp
  2. 2
      libraries/AP_Winch/AP_Winch_Daiwa.h

2
libraries/AP_Winch/AP_Winch_Daiwa.cpp

@ -60,7 +60,7 @@ void AP_Winch_Daiwa::send_status(const GCS_MAVLINK &channel) @@ -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(

2
libraries/AP_Winch/AP_Winch_Daiwa.h

@ -64,7 +64,7 @@ private: @@ -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

Loading…
Cancel
Save