mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-22 16:48:29 -04:00
AP_ToshibaCAN: handle negative RPM
This commit is contained in:
parent
08a710235d
commit
0850b0acae
@ -310,7 +310,7 @@ void AP_ToshibaCAN::loop()
|
|||||||
const uint8_t esc_id = recv_frame.id - MOTOR_DATA1;
|
const uint8_t esc_id = recv_frame.id - MOTOR_DATA1;
|
||||||
if (esc_id < TOSHIBACAN_MAX_NUM_ESCS) {
|
if (esc_id < TOSHIBACAN_MAX_NUM_ESCS) {
|
||||||
WITH_SEMAPHORE(_telem_sem);
|
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].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].voltage_cv = be16toh(reply_data.voltage_mv) * 0.1f; // millivolts to centi-volts
|
||||||
_telemetry[esc_id].count++;
|
_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;
|
voltage[j] = _telemetry[esc_id].voltage_cv;
|
||||||
current[j] = _telemetry[esc_id].current_ca;
|
current[j] = _telemetry[esc_id].current_ca;
|
||||||
current_tot[j] = constrain_float(_telemetry[esc_id].current_tot_mah, 0, UINT16_MAX);
|
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;
|
count[j] = _telemetry[esc_id].count;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -78,7 +78,7 @@ private:
|
|||||||
// telemetry data (rpm, voltage)
|
// telemetry data (rpm, voltage)
|
||||||
HAL_Semaphore _telem_sem;
|
HAL_Semaphore _telem_sem;
|
||||||
struct telemetry_info_t {
|
struct telemetry_info_t {
|
||||||
uint16_t rpm; // rpm
|
int16_t rpm; // rpm
|
||||||
uint16_t voltage_cv; // voltage in centi-volts
|
uint16_t voltage_cv; // voltage in centi-volts
|
||||||
uint16_t current_ca; // current in centi-amps
|
uint16_t current_ca; // current in centi-amps
|
||||||
uint16_t esc_temp; // esc temperature in degrees
|
uint16_t esc_temp; // esc temperature in degrees
|
||||||
@ -156,7 +156,7 @@ private:
|
|||||||
struct PACKED {
|
struct PACKED {
|
||||||
uint8_t rxng:1;
|
uint8_t rxng:1;
|
||||||
uint8_t state:7;
|
uint8_t state:7;
|
||||||
uint16_t rpm;
|
int16_t rpm;
|
||||||
uint16_t current_ma; // current in milliamps
|
uint16_t current_ma; // current in milliamps
|
||||||
uint16_t voltage_mv; // voltage in millivolts
|
uint16_t voltage_mv; // voltage in millivolts
|
||||||
uint8_t position_est_error;
|
uint8_t position_est_error;
|
||||||
|
Loading…
Reference in New Issue
Block a user