Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_ToshibaCAN: consume current sent by ESC

This commit is contained in:
Randy Mackay 2019-10-30 14:23:01 +09:00
parent f6b669e3d8
commit ad55b961a4
2 changed files with 28 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_ToshibaCAN/AP_ToshibaCAN.cpp
View File

@ -313,9 +313,18 @@ void AP_ToshibaCAN::loop()
if (esc_id < TOSHIBACAN_MAX_NUM_ESCS) {
WITH_SEMAPHORE(_telem_sem);
_telemetry[esc_id].rpm = be16toh(reply_data.rpm);
_telemetry[esc_id].voltage_mv = be16toh(reply_data.voltage_mv);
_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++;
_telemetry[esc_id].new_data = true;
// update total current
const uint32_t now_ms = AP_HAL::millis();
const uint32_t diff_ms = now_ms - _telemetry[esc_id].last_update_ms;
if (diff_ms <= 1000) {
// convert centi-amps miiliseconds to mAh
_telemetry[esc_id].current_tot_mah += _telemetry[esc_id].current_ca * diff_ms * centiamp_ms_to_mah;
}
_telemetry[esc_id].last_update_ms = now_ms;
_esc_present_bitmask |= ((uint32_t)1 << esc_id);
}
}
@ -431,10 +440,10 @@ void AP_ToshibaCAN::update()
if (_telemetry[i].new_data) {
logger->Write_ESC(i, time_us,
_telemetry[i].rpm * 100U,
_telemetry[i].voltage_mv * 0.1f,
0,
_telemetry[i].voltage_cv,
_telemetry[i].current_ca,
_telemetry[i].temperature * 100.0f,
0);
constrain_float(_telemetry[i].current_tot_mah, 0, UINT16_MAX));
_telemetry[i].new_data = false;
}
}
@ -473,15 +482,18 @@ void AP_ToshibaCAN::send_esc_telemetry_mavlink(uint8_t mav_chan)
// arrays to hold output
uint8_t temperature[4] {};
uint16_t voltage[4] {};
uint16_t current[4] {};
uint16_t current_tot[4] {};
uint16_t rpm[4] {};
uint16_t count[4] {};
uint16_t nosup[4] {}; // single empty array for unsupported current and current_tot
// fill in output arrays
for (uint8_t j = 0; j < 4; j++) {
uint8_t esc_id = i * 4 + j;
temperature[j] = _telemetry[esc_id].temperature;
voltage[j] = _telemetry[esc_id].voltage_mv * 0.1f;
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;
count[j] = _telemetry[esc_id].count;
}
@ -489,13 +501,13 @@ void AP_ToshibaCAN::send_esc_telemetry_mavlink(uint8_t mav_chan)
// send messages
switch (i) {
case 0:
mavlink_msg_esc_telemetry_1_to_4_send((mavlink_channel_t)mav_chan, temperature, voltage, nosup, nosup, rpm, count);
mavlink_msg_esc_telemetry_1_to_4_send((mavlink_channel_t)mav_chan, temperature, voltage, current, current_tot, rpm, count);
break;
case 1:
mavlink_msg_esc_telemetry_5_to_8_send((mavlink_channel_t)mav_chan, temperature, voltage, nosup, nosup, rpm, count);
mavlink_msg_esc_telemetry_5_to_8_send((mavlink_channel_t)mav_chan, temperature, voltage, current, current_tot, rpm, count);
break;
case 2:
mavlink_msg_esc_telemetry_9_to_12_send((mavlink_channel_t)mav_chan, temperature, voltage, nosup, nosup, rpm, count);
mavlink_msg_esc_telemetry_9_to_12_send((mavlink_channel_t)mav_chan, temperature, voltage, current, current_tot, rpm, count);
break;
default:
break;

10
libraries/AP_ToshibaCAN/AP_ToshibaCAN.h
View File

@ -70,13 +70,17 @@ private:
HAL_Semaphore _telem_sem;
struct telemetry_info_t {
uint16_t rpm; // rpm
uint16_t voltage_mv; // voltage in millivolts
uint16_t voltage_cv; // voltage in centi-volts
uint16_t current_ca; // current in centi-amps
uint16_t temperature; // temperature in degrees
uint16_t count; // total number of packets sent
bool new_data;
uint32_t last_update_ms; // system time telemetry was last update (used to calc total current)
float current_tot_mah; // total current in mAh
bool new_data; // true if new telemetry data has been filled in but not logged yet
} _telemetry[TOSHIBACAN_MAX_NUM_ESCS];
uint32_t _telemetry_req_ms; // system time (in milliseconds) to request data from escs (updated at 10hz)
uint8_t _telemetry_temp_req_counter; // counter used to trigger temp data requests from ESCs (10x slower than other telem data)
const float centiamp_ms_to_mah = 1.0f / 360000.0f; // for converting centi-amps milliseconds to mAh
// bitmask of which escs seem to be present
uint16_t _esc_present_bitmask;
@ -136,7 +140,7 @@ private:
uint8_t rxng:1;
uint8_t state:7;
uint16_t rpm;
uint16_t reserved;
uint16_t current_ma; // current in milliamps
uint16_t voltage_mv; // voltage in millivolts
uint8_t position_est_error;
};