mirror of https://github.com/ArduPilot/ardupilot
AP_ToshibaCAN: generalise ESC telemetry to allow harmonic notch handling with other ESCs
remove send_esc_telemetry_mavlink() log telemetry data in frontend remove datastructures and semaphore obsoleted by AP_ESC_Telem* (<amilcar.lucas@iav.de>) record volts, amps and consumption as floats
This commit is contained in:
Andy Piper
Andrew Tridgell
parent
70fae36a89
commit
677d863401
|
|
@ -305,19 +305,24 @@ void AP_ToshibaCAN::loop()
|
|||
// store response in telemetry array
|
||||
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 = (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++;
|
||||
_telemetry[esc_id].new_data = true;
|
||||
update_rpm(esc_id, (int16_t)be16toh(reply_data.rpm));
|
||||
|
||||
// update total current
|
||||
const uint32_t now_ms = AP_HAL::native_millis();
|
||||
const uint32_t diff_ms = now_ms - _telemetry[esc_id].last_update_ms;
|
||||
TelemetryData t {};
|
||||
t.voltage = float(be16toh(reply_data.voltage_mv)) * 0.001f; // millivolts to volts
|
||||
t.current = MAX((int16_t)be16toh(reply_data.current_ma), 0) * (4.0f * 0.001f); // milli-amps to amps
|
||||
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;
|
||||
// convert centi-amps miliseconds to mAh
|
||||
_telemetry[esc_id].current_tot_mah += t.current * diff_ms * amp_ms_to_mah;
|
||||
}
|
||||
t.consumption_mah = _telemetry[esc_id].current_tot_mah;
|
||||
update_telem_data(esc_id, t,
|
||||
AP_ESC_Telem_Backend::TelemetryType::CURRENT
|
||||
| AP_ESC_Telem_Backend::TelemetryType::VOLTAGE
|
||||
| AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION);
|
||||
|
||||
_telemetry[esc_id].last_update_ms = now_ms;
|
||||
_esc_present_bitmask_recent |= ((uint32_t)1 << esc_id);
|
||||
}
|
||||
|
|
@ -340,10 +345,16 @@ void AP_ToshibaCAN::loop()
|
|||
// store response in telemetry array
|
||||
uint8_t esc_id = recv_frame.id - MOTOR_DATA2;
|
||||
if (esc_id < TOSHIBACAN_MAX_NUM_ESCS) {
|
||||
WITH_SEMAPHORE(_telem_sem);
|
||||
_telemetry[esc_id].esc_temp = temp_max < 100 ? 0 : temp_max / 5 - 20;
|
||||
_telemetry[esc_id].motor_temp = motor_temp < 100 ? 0 : motor_temp / 5 - 20;
|
||||
const int16_t esc_temp_deg = temp_max < 100 ? 0 : temp_max / 5 - 20;
|
||||
const int16_t motor_temp_deg = motor_temp < 100 ? 0 : motor_temp / 5 - 20;
|
||||
_esc_present_bitmask_recent |= ((uint32_t)1 << esc_id);
|
||||
|
||||
TelemetryData t {
|
||||
.temperature_cdeg = int16_t(esc_temp_deg * 100)
|
||||
};
|
||||
t.motor_temp_cdeg = int16_t(motor_temp_deg * 100);
|
||||
update_telem_data(esc_id, t, AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE |
|
||||
AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -358,9 +369,11 @@ void AP_ToshibaCAN::loop()
|
|||
// store response in telemetry array
|
||||
uint8_t esc_id = recv_frame.id - MOTOR_DATA3;
|
||||
if (esc_id < TOSHIBACAN_MAX_NUM_ESCS) {
|
||||
WITH_SEMAPHORE(_telem_sem);
|
||||
_telemetry[esc_id].usage_sec = usage_sec;
|
||||
_esc_present_bitmask_recent |= ((uint32_t)1 << esc_id);
|
||||
|
||||
TelemetryData t {};
|
||||
t.usage_s = usage_sec;
|
||||
update_telem_data(esc_id, t, AP_ESC_Telem_Backend::TelemetryType::USAGE);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -456,102 +469,6 @@ void AP_ToshibaCAN::update()
|
|||
}
|
||||
update_count++;
|
||||
}
|
||||
|
||||
// log ESCs telemetry info
|
||||
AP_Logger *logger = AP_Logger::get_singleton();
|
||||
if (logger && logger->logging_enabled()) {
|
||||
WITH_SEMAPHORE(_telem_sem);
|
||||
|
||||
// log if any new data received. Logging only supports up to 8 ESCs
|
||||
const uint64_t time_us = AP_HAL::native_micros64();
|
||||
for (uint8_t i = 0; i < MIN(TOSHIBACAN_MAX_NUM_ESCS, 8); i++) {
|
||||
if (_telemetry[i].new_data) {
|
||||
logger->Write_ESC(i, time_us,
|
||||
_telemetry[i].rpm * 100U,
|
||||
_telemetry[i].voltage_cv,
|
||||
_telemetry[i].current_ca,
|
||||
_telemetry[i].esc_temp * 100U,
|
||||
constrain_float(_telemetry[i].current_tot_mah, 0, UINT16_MAX),
|
||||
_telemetry[i].motor_temp * 100U);
|
||||
_telemetry[i].new_data = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// send ESC telemetry messages over MAVLink
|
||||
void AP_ToshibaCAN::send_esc_telemetry_mavlink(uint8_t mav_chan)
|
||||
{
|
||||
// compile time check this method handles the correct number of motors
|
||||
static_assert(TOSHIBACAN_MAX_NUM_ESCS == 12, "update AP_ToshibaCAN::send_esc_telemetry_mavlink only handles 12 motors");
|
||||
|
||||
// return immediately if no ESCs have been found
|
||||
if (_esc_present_bitmask == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
// output telemetry messages
|
||||
{
|
||||
// take semaphore to access telemetry data
|
||||
WITH_SEMAPHORE(_telem_sem);
|
||||
|
||||
// loop through 3 groups of 4 ESCs
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
|
||||
// return if no space in output buffer to send mavlink messages
|
||||
if (!HAVE_PAYLOAD_SPACE((mavlink_channel_t)mav_chan, ESC_TELEMETRY_1_TO_4)) {
|
||||
return;
|
||||
}
|
||||
|
||||
// skip this group of ESCs if no data to send
|
||||
if ((_esc_present_bitmask & ((uint32_t)0x0F << i*4)) == 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// 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] {};
|
||||
|
||||
// fill in output arrays
|
||||
for (uint8_t j = 0; j < 4; j++) {
|
||||
uint8_t esc_id = i * 4 + j;
|
||||
temperature[j] = _telemetry[esc_id].esc_temp;
|
||||
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] = abs(_telemetry[esc_id].rpm); // mavlink message only accepts positive rpm values
|
||||
count[j] = _telemetry[esc_id].count;
|
||||
}
|
||||
|
||||
// send messages
|
||||
switch (i) {
|
||||
case 0:
|
||||
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, current, current_tot, rpm, count);
|
||||
break;
|
||||
case 2:
|
||||
mavlink_msg_esc_telemetry_9_to_12_send((mavlink_channel_t)mav_chan, temperature, voltage, current, current_tot, rpm, count);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// return total usage time in seconds
|
||||
uint32_t AP_ToshibaCAN::get_usage_seconds(uint8_t esc_id) const
|
||||
{
|
||||
if (esc_id >= TOSHIBACAN_MAX_NUM_ESCS) {
|
||||
return 0;
|
||||
}
|
||||
return _telemetry[esc_id].usage_sec;
|
||||
}
|
||||
|
||||
// helper function to create motor_request_data_cmd_t
|
||||
|
|
|
|||
|
|
@ -15,6 +15,7 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <AP_ESC_Telem/AP_ESC_Telem_Backend.h>
|
||||
#include <AP_CANManager/AP_CANDriver.h>
|
||||
#include <AP_HAL/Semaphores.h>
|
||||
|
||||
|
|
@ -22,7 +23,7 @@
|
|||
|
||||
class CANTester;
|
||||
|
||||
class AP_ToshibaCAN : public AP_CANDriver {
|
||||
class AP_ToshibaCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
|
||||
friend class CANTester;
|
||||
public:
|
||||
AP_ToshibaCAN();
|
||||
|
|
@ -42,15 +43,9 @@ public:
|
|||
// called from SRV_Channels
|
||||
void update();
|
||||
|
||||
// send ESC telemetry messages over MAVLink
|
||||
void send_esc_telemetry_mavlink(uint8_t mav_chan);
|
||||
|
||||
// return a bitmask of escs that are "present" which means they are responding to requests. Bitmask matches RC outputs
|
||||
uint16_t get_present_mask() const { return _esc_present_bitmask; }
|
||||
|
||||
// return total usage time in seconds
|
||||
uint32_t get_usage_seconds(uint8_t esc_id) const;
|
||||
|
||||
private:
|
||||
|
||||
// data format for messages from flight controller
|
||||
|
|
@ -101,24 +96,14 @@ private:
|
|||
uint16_t update_count_sent; // counter of outputs successfully sent
|
||||
uint8_t send_stage; // stage of sending algorithm (each stage sends one frame to ESCs)
|
||||
|
||||
// telemetry data (rpm, voltage)
|
||||
HAL_Semaphore _telem_sem;
|
||||
struct telemetry_info_t {
|
||||
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
|
||||
uint16_t motor_temp; // motor temperature in degrees
|
||||
uint16_t count; // total number of packets sent
|
||||
uint32_t usage_sec; // motor's total usage in seconds
|
||||
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)
|
||||
uint8_t _telemetry_usage_req_counter; // counter used to trigger usage data requests from ESCs (100x slower than other telem data)
|
||||
const float centiamp_ms_to_mah = 1.0f / 360000.0f; // for converting centi-amps milliseconds to mAh
|
||||
const float amp_ms_to_mah = 1.0f / 3600.0f; // for converting amp milliseconds to mAh
|
||||
|
||||
// variables for updating bitmask of responsive escs
|
||||
uint16_t _esc_present_bitmask; // bitmask of which escs seem to be present
|
||||
|
|
|
|||
Loading…
Reference in New Issue