From 0850b0acae23e056ff8bf633a25c9540bb38213c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 25 Jun 2020 11:17:48 +0900 Subject: [PATCH] AP_ToshibaCAN: handle negative RPM --- libraries/AP_ToshibaCAN/AP_ToshibaCAN.cpp | 4 ++-- libraries/AP_ToshibaCAN/AP_ToshibaCAN.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_ToshibaCAN/AP_ToshibaCAN.cpp b/libraries/AP_ToshibaCAN/AP_ToshibaCAN.cpp index c27d1c01f6..4379c8758b 100644 --- a/libraries/AP_ToshibaCAN/AP_ToshibaCAN.cpp +++ b/libraries/AP_ToshibaCAN/AP_ToshibaCAN.cpp @@ -310,7 +310,7 @@ void AP_ToshibaCAN::loop() const uint8_t esc_id = recv_frame.id - MOTOR_DATA1; if (esc_id < TOSHIBACAN_MAX_NUM_ESCS) { WITH_SEMAPHORE(_telem_sem); - _telemetry[esc_id].rpm = be16toh(reply_data.rpm); + _telemetry[esc_id].rpm = (int16_t)be16toh(reply_data.rpm); _telemetry[esc_id].current_ca = MAX((int16_t)be16toh(reply_data.current_ma), 0) * (4.0f * 0.1f); // milli-amps to centi-amps _telemetry[esc_id].voltage_cv = be16toh(reply_data.voltage_mv) * 0.1f; // millivolts to centi-volts _telemetry[esc_id].count++; @@ -532,7 +532,7 @@ void AP_ToshibaCAN::send_esc_telemetry_mavlink(uint8_t mav_chan) voltage[j] = _telemetry[esc_id].voltage_cv; current[j] = _telemetry[esc_id].current_ca; current_tot[j] = constrain_float(_telemetry[esc_id].current_tot_mah, 0, UINT16_MAX); - rpm[j] = _telemetry[esc_id].rpm; + rpm[j] = abs(_telemetry[esc_id].rpm); // mavlink message only accepts positive rpm values count[j] = _telemetry[esc_id].count; } diff --git a/libraries/AP_ToshibaCAN/AP_ToshibaCAN.h b/libraries/AP_ToshibaCAN/AP_ToshibaCAN.h index 7191346f6d..88c2135e3b 100644 --- a/libraries/AP_ToshibaCAN/AP_ToshibaCAN.h +++ b/libraries/AP_ToshibaCAN/AP_ToshibaCAN.h @@ -78,7 +78,7 @@ private: // telemetry data (rpm, voltage) HAL_Semaphore _telem_sem; struct telemetry_info_t { - uint16_t rpm; // rpm + int16_t rpm; // rpm uint16_t voltage_cv; // voltage in centi-volts uint16_t current_ca; // current in centi-amps uint16_t esc_temp; // esc temperature in degrees @@ -156,7 +156,7 @@ private: struct PACKED { uint8_t rxng:1; uint8_t state:7; - uint16_t rpm; + int16_t rpm; uint16_t current_ma; // current in milliamps uint16_t voltage_mv; // voltage in millivolts uint8_t position_est_error;