Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_ToshibaCAN: handle negative RPM

Browse Source
This commit is contained in:
Randy Mackay 2020-06-25 11:17:48 +09:00
parent 08a710235d
commit 0850b0acae
2 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_ToshibaCAN/AP_ToshibaCAN.cpp
View File

@ -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;
}

4
libraries/AP_ToshibaCAN/AP_ToshibaCAN.h
View File

@ -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;