diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 872d72a3ee..2b54417fc5 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -15,15 +15,14 @@ #include "AP_ESC_Telem.h" #include +#include +#include -#if HAL_MAX_CAN_PROTOCOL_DRIVERS - #include - #include - #include - #include - #include - #include -#endif +#if HAL_WITH_ESC_TELEM + +#include + +//#define ESC_TELEM_DEBUG extern const AP_HAL::HAL& hal; @@ -35,25 +34,331 @@ AP_ESC_Telem::AP_ESC_Telem() _singleton = this; } -// get an individual ESC's usage time in seconds if available, returns true on success -bool AP_ESC_Telem::get_usage_seconds(uint8_t esc_id, uint32_t& usage_sec) const +// return the average motor frequency in Hz for dynamic filtering +float AP_ESC_Telem::get_average_motor_frequency_hz() const { -#if HAL_MAX_CAN_PROTOCOL_DRIVERS - const uint8_t num_drivers = AP::can().get_num_drivers(); - for (uint8_t i = 0; i < num_drivers; i++) { - if (AP::can().get_driver_type(i) == AP_CANManager::Driver_Type_ToshibaCAN) { - AP_ToshibaCAN *tcan = AP_ToshibaCAN::get_tcan(i); - if (tcan != nullptr) { - usage_sec = tcan->get_usage_seconds(esc_id); - return true; - } - return true; + float motor_freq = 0.0f; + uint8_t valid_escs = 0; + + // average the rpm of each motor and convert to Hz + for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { + float rpm; + if (get_rpm(i, rpm)) { + motor_freq += rpm * (1.0f / 60.0f); + valid_escs++; } } -#endif + if (valid_escs > 0) { + motor_freq /= valid_escs; + } + + return motor_freq; +} + +// return all the motor frequencies in Hz for dynamic filtering +uint8_t AP_ESC_Telem::get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) const +{ + uint8_t valid_escs = 0; + + // average the rpm of each motor as reported by BLHeli and convert to Hz + for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS && i < nfreqs; i++) { + float rpm; + if (get_rpm(i, rpm)) { + freqs[valid_escs++] = rpm * (1.0f / 60.0f); + } + } + + return MIN(valid_escs, nfreqs); +} + +// return number of active ESCs present +uint8_t AP_ESC_Telem::get_num_active_escs() const { + uint8_t nmotors = 0; + uint32_t now = AP_HAL::millis(); + for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { + if (now - _telem_data[i].last_update_ms < ESC_TELEM_DATA_TIMEOUT_MS) { + nmotors++; + } + } + return nmotors; +} + +// get an individual ESC's slewed rpm if available, returns true on success +bool AP_ESC_Telem::get_rpm(uint8_t esc_index, float& rpm) const +{ + const volatile AP_ESC_Telem_Backend::RpmData& rpmdata = _rpm_data[esc_index]; + + if (esc_index > ESC_TELEM_MAX_ESCS || is_zero(rpmdata.update_rate_hz)) { + return false; + } + + const uint32_t now = AP_HAL::micros(); + if (rpmdata.last_update_us > 0 && (now >= rpmdata.last_update_us) + && (now - rpmdata.last_update_us < ESC_RPM_DATA_TIMEOUT_US)) { + const float slew = MIN(1.0f, (now - rpmdata.last_update_us) * rpmdata.update_rate_hz * (1.0f / 1e6f)); + rpm = (rpmdata.prev_rpm + (rpmdata.rpm - rpmdata.prev_rpm) * slew); + return true; + } return false; } +// get an individual ESC's raw rpm if available, returns true on success +bool AP_ESC_Telem::get_raw_rpm(uint8_t esc_index, float& rpm) const +{ + const volatile AP_ESC_Telem_Backend::RpmData& rpmdata = _rpm_data[esc_index]; + + const uint32_t now = AP_HAL::micros(); + + if (esc_index >= ESC_TELEM_MAX_ESCS || now < rpmdata.last_update_us + || now - rpmdata.last_update_us > ESC_RPM_DATA_TIMEOUT_US) { + return false; + } + + rpm = rpmdata.rpm; + return true; +} + +// get an individual ESC's temperature in centi-degrees if available, returns true on success +bool AP_ESC_Telem::get_temperature(uint8_t esc_index, int16_t& temp) const +{ + if (esc_index >= ESC_TELEM_MAX_ESCS + || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS + || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE)) { + return false; + } + temp = _telem_data[esc_index].temperature_cdeg; + return true; +} + +// get an individual motor's temperature in centi-degrees if available, returns true on success +bool AP_ESC_Telem::get_motor_temperature(uint8_t esc_index, int16_t& temp) const +{ + if (esc_index >= ESC_TELEM_MAX_ESCS + || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS + || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE)) { + return false; + } + temp = _telem_data[esc_index].motor_temp_cdeg; + return true; +} + +// get an individual ESC's current in Ampere if available, returns true on success +bool AP_ESC_Telem::get_current(uint8_t esc_index, float& amps) const +{ + if (esc_index >= ESC_TELEM_MAX_ESCS + || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS + || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::CURRENT)) { + return false; + } + amps = _telem_data[esc_index].current; + return true; +} + +// get an individual ESC's voltage in Volt if available, returns true on success +bool AP_ESC_Telem::get_voltage(uint8_t esc_index, float& volts) const +{ + if (esc_index >= ESC_TELEM_MAX_ESCS + || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS + || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::VOLTAGE)) { + return false; + } + volts = _telem_data[esc_index].voltage; + return true; +} + +// get an individual ESC's energy consumption in milli-Ampere.hour if available, returns true on success +bool AP_ESC_Telem::get_consumption_mah(uint8_t esc_index, float& consumption_mah) const +{ + if (esc_index >= ESC_TELEM_MAX_ESCS + || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS + || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION)) { + return false; + } + consumption_mah = _telem_data[esc_index].consumption_mah; + return true; +} + +// get an individual ESC's usage time in seconds if available, returns true on success +bool AP_ESC_Telem::get_usage_seconds(uint8_t esc_index, uint32_t& usage_s) const +{ + if (esc_index >= ESC_TELEM_MAX_ESCS + || AP_HAL::millis() - _telem_data[esc_index].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS + || !(_telem_data[esc_index].types & AP_ESC_Telem_Backend::TelemetryType::USAGE)) { + return false; + } + usage_s = _telem_data[esc_index].usage_s; + return true; +} + +// send ESC telemetry messages over MAVLink +void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan) +{ + static_assert(ESC_TELEM_MAX_ESCS <= 12, "AP_ESC_Telem::send_esc_telemetry_mavlink() only supports up-to 12 motors"); + + uint32_t now = AP_HAL::millis(); + uint32_t now_us = AP_HAL::micros(); + // 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; + } +#define ESC_DATA_STALE(idx) \ + (now - _telem_data[idx].last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS \ + && now_us - _rpm_data[idx].last_update_us > ESC_RPM_DATA_TIMEOUT_US) + + // skip this group of ESCs if no data to send + if (ESC_DATA_STALE(i * 4) && ESC_DATA_STALE(i * 4 + 1) && ESC_DATA_STALE(i * 4 + 2) && ESC_DATA_STALE(i * 4 + 3)) { + 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++) { + const uint8_t esc_id = i * 4 + j; + temperature[j] = _telem_data[esc_id].temperature_cdeg / 100; + voltage[j] = constrain_float(_telem_data[esc_id].voltage * 100.0f, 0, UINT16_MAX); + current[j] = constrain_float(_telem_data[esc_id].current * 100.0f, 0, UINT16_MAX); + current_tot[j] = constrain_float(_telem_data[esc_id].consumption_mah, 0, UINT16_MAX); + float rpmf = 0.0f; + if (get_rpm(esc_id, rpmf)) { + rpm[j] = constrain_float(rpmf, 0, UINT16_MAX); + } else { + rpm[j] = 0; + } + count[j] = _telem_data[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; + } + } +} + +// record an update to the telemetry data together with timestamp +// this should be called by backends when new telemetry values are available +void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem_Backend::TelemetryData& new_data, const uint16_t data_mask) +{ + // rpm and telemetry data are not protected by a semaphore even though updated from different threads + // all data is per-ESC and only written from the update thread and read by the user thread + // each element is a primitive type and the timestamp is only updated at the end, thus a caller + // can only get slightly more up-to-date information that perhaps they were expecting or might + // read data that has just gone stale - both of these are safe and avoid the overhead of locking + + if (esc_index > ESC_TELEM_MAX_ESCS) { + return; + } + + if (data_mask & AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE) { + _telem_data[esc_index].temperature_cdeg = new_data.temperature_cdeg; + } + if (data_mask & AP_ESC_Telem_Backend::TelemetryType::MOTOR_TEMPERATURE) { + _telem_data[esc_index].motor_temp_cdeg = new_data.motor_temp_cdeg; + } + if (data_mask & AP_ESC_Telem_Backend::TelemetryType::VOLTAGE) { + _telem_data[esc_index].voltage = new_data.voltage; + } + if (data_mask & AP_ESC_Telem_Backend::TelemetryType::CURRENT) { + _telem_data[esc_index].current = new_data.current; + } + if (data_mask & AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION) { + _telem_data[esc_index].consumption_mah = new_data.consumption_mah; + } + if (data_mask & AP_ESC_Telem_Backend::TelemetryType::USAGE) { + _telem_data[esc_index].usage_s = new_data.usage_s; + } + + _telem_data[esc_index].count++; + _telem_data[esc_index].types |= data_mask; + _telem_data[esc_index].last_update_ms = AP_HAL::millis(); +} + +// record an update to the RPM together with timestamp, this allows the notch values to be slewed +// this should be called by backends when new telemetry values are available +void AP_ESC_Telem::update_rpm(const uint8_t esc_index, const uint16_t new_rpm, const float error_rate) +{ + if (esc_index > ESC_TELEM_MAX_ESCS) { + return; + } + const uint32_t now = AP_HAL::micros(); + volatile AP_ESC_Telem_Backend::RpmData& rpmdata = _rpm_data[esc_index]; + + rpmdata.prev_rpm = rpmdata.rpm; + rpmdata.rpm = new_rpm; + if (now > rpmdata.last_update_us) { // cope with wrapping + rpmdata.update_rate_hz = 1.0e6f / (now - rpmdata.last_update_us); + } + rpmdata.last_update_us = now; + rpmdata.error_rate = error_rate; + +#ifdef ESC_TELEM_DEBUG + hal.console->printf("RPM: rate=%.1fhz, rpm=%d)\n", rpmdata.update_rate_hz, new_rpm); +#endif +} + +// log ESC telemetry at 10Hz +void AP_ESC_Telem::update() +{ + AP_Logger *logger = AP_Logger::get_singleton(); + + // Push received telemtry data into the logging system + if (logger && logger->logging_enabled()) { + + for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { + if (_telem_data[i].last_update_ms != _last_telem_log_ms[i] + || _rpm_data[i].last_update_us != _last_rpm_log_us[i]) { + + float rpm = 0.0f; + get_rpm(i, rpm); + + // Write ESC status messages + // id starts from 0 + // rpm is eRPM (rpm * 100) + // voltage is in Volt + // current is in Ampere + // esc_temp is in centi-degrees Celsius + // current_tot is in mili-Ampere hours + // motor_temp is in centi-degrees Celsius + // error_rate is in percentage + const struct log_Esc pkt{ + LOG_PACKET_HEADER_INIT(uint8_t(LOG_ESC_MSG)), + time_us : AP_HAL::micros64(), + instance : i, + rpm : (int32_t) rpm * 100, + voltage : _telem_data[i].voltage, + current : _telem_data[i].current, + esc_temp : _telem_data[i].temperature_cdeg, + current_tot : _telem_data[i].consumption_mah, + motor_temp : _telem_data[i].motor_temp_cdeg, + error_rate : _rpm_data[i].error_rate + }; + AP::logger().WriteBlock(&pkt, sizeof(pkt)); + _last_telem_log_ms[i] = _telem_data[i].last_update_ms; + _last_rpm_log_us[i] = _rpm_data[i].last_update_us; + } + } + } +} + AP_ESC_Telem *AP_ESC_Telem::_singleton = nullptr; /* @@ -72,3 +377,5 @@ AP_ESC_Telem &esc_telem() } }; + +#endif diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h index 16bd549aff..402d2939f4 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h @@ -1,9 +1,17 @@ #pragma once #include +#include "AP_ESC_Telem_Backend.h" + +#if HAL_WITH_ESC_TELEM + +#define ESC_TELEM_MAX_ESCS 12 +#define ESC_TELEM_DATA_TIMEOUT_MS 5000UL +#define ESC_RPM_DATA_TIMEOUT_US 1000000UL class AP_ESC_Telem { public: + friend class AP_ESC_Telem_Backend; AP_ESC_Telem(); @@ -13,15 +21,70 @@ public: static AP_ESC_Telem *get_singleton(); + // get an individual ESC's slewed rpm if available, returns true on success + bool get_rpm(uint8_t esc_index, float& rpm) const; + + // get an individual ESC's raw rpm if available + bool get_raw_rpm(uint8_t esc_index, float& rpm) const; + + // get an individual ESC's temperature in centi-degrees if available, returns true on success + bool get_temperature(uint8_t esc_index, int16_t& temp) const; + + // get an individual motor's temperature in centi-degrees if available, returns true on success + bool get_motor_temperature(uint8_t esc_index, int16_t& temp) const; + + // get an individual ESC's current in Ampere if available, returns true on success + bool get_current(uint8_t esc_index, float& amps) const; + // get an individual ESC's usage time in seconds if available, returns true on success - bool get_usage_seconds(uint8_t esc_id, uint32_t& usage_sec) const; + bool get_usage_seconds(uint8_t esc_index, uint32_t& usage_sec) const; + + // get an individual ESC's voltage in Volt if available, returns true on success + bool get_voltage(uint8_t esc_index, float& volts) const; + + // get an individual ESC's consumption in milli-Ampere.hour if available, returns true on success + bool get_consumption_mah(uint8_t esc_index, float& consumption_mah) const; + + // return the average motor frequency in Hz for dynamic filtering + float get_average_motor_frequency_hz() const; + + // return all of the motor frequencies in Hz for dynamic filtering + uint8_t get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) const; + + // get the number of valid ESCs + uint8_t get_num_active_escs() const; + + // return the last time telemetry data was received in ms for the given ESC or 0 if never + uint32_t get_last_telem_data_ms(uint8_t esc_index) const { + if (esc_index > ESC_TELEM_MAX_ESCS) return 0; + return _telem_data[esc_index].last_update_ms; + } + + // send telemetry data to mavlink + void send_esc_telemetry_mavlink(uint8_t mav_chan); + + // udpate at 10Hz to log telemetry + void update(); private: + // callback to update the rpm in the frontend, should be called by the driver when new data is available + void update_rpm(const uint8_t esc_index, const uint16_t new_rpm, const float error_rate); + // callback to update the data in the frontend, should be called by the driver when new data is available + void update_telem_data(const uint8_t esc_index, const AP_ESC_Telem_Backend::TelemetryData& new_data, const uint16_t data_mask); + + // rpm data + volatile AP_ESC_Telem_Backend::RpmData _rpm_data[ESC_TELEM_MAX_ESCS]; + // telemetry data + volatile AP_ESC_Telem_Backend::TelemetryData _telem_data[ESC_TELEM_MAX_ESCS]; + + uint32_t _last_telem_log_ms[ESC_TELEM_MAX_ESCS]; + uint32_t _last_rpm_log_us[ESC_TELEM_MAX_ESCS]; static AP_ESC_Telem *_singleton; - }; namespace AP { AP_ESC_Telem &esc_telem(); }; + +#endif diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp new file mode 100644 index 0000000000..7733dc6a09 --- /dev/null +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp @@ -0,0 +1,43 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "AP_ESC_Telem_Backend.h" +#include "AP_ESC_Telem.h" +#include + +#if HAL_WITH_ESC_TELEM + +#include + +extern const AP_HAL::HAL& hal; + +AP_ESC_Telem_Backend::AP_ESC_Telem_Backend() { + _frontend = AP_ESC_Telem::_singleton; + if (_frontend == nullptr) { + AP_HAL::panic("No ESC frontend"); + } +} + +// callback to update the rpm in the frontend, should be called by the driver when new data is available +void AP_ESC_Telem_Backend::update_rpm(const uint8_t esc_index, const uint16_t new_rpm, const float error_rate) { + _frontend->update_rpm(esc_index, new_rpm, error_rate); +} + +// callback to update the data in the frontend, should be called by the driver when new data is available +void AP_ESC_Telem_Backend::update_telem_data(const uint8_t esc_index, const TelemetryData& new_data, const uint16_t data_present_mask) { + _frontend->update_telem_data(esc_index, new_data, data_present_mask); +} + +#endif \ No newline at end of file diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h new file mode 100644 index 0000000000..5934e224e3 --- /dev/null +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h @@ -0,0 +1,63 @@ +#pragma once + +#include + +#ifndef HAL_WITH_ESC_TELEM +#define HAL_WITH_ESC_TELEM HAL_SUPPORT_RCOUT_SERIAL || HAL_MAX_CAN_PROTOCOL_DRIVERS +#endif + +#if HAL_WITH_ESC_TELEM + +class AP_ESC_Telem; + +class AP_ESC_Telem_Backend { +public: + + struct TelemetryData { + int16_t temperature_cdeg; // centi-degrees C, negative values allowed + float voltage; // Volt + float current; // Ampere + float consumption_mah; // milli-Ampere.hours + uint32_t usage_s; // usage seconds + int16_t motor_temp_cdeg; // centi-degrees C, negative values allowed + uint32_t last_update_ms; // last update time in miliseconds, determines whether active + uint16_t types; // telemetry types present + uint16_t count; // number of times updated + }; + + struct RpmData { + float rpm; // rpm + float prev_rpm; // previous rpm + float error_rate; // error rate in percent + uint32_t last_update_us; // last update time, determines whether active + float update_rate_hz; + }; + + enum TelemetryType { + TEMPERATURE = 1 << 0, + MOTOR_TEMPERATURE = 1 << 1, + VOLTAGE = 1 << 2, + CURRENT = 1 << 3, + CONSUMPTION = 1 << 4, + USAGE = 1 << 5 + }; + + + AP_ESC_Telem_Backend(); + + /* Do not allow copies */ + AP_ESC_Telem_Backend(const AP_ESC_Telem_Backend &other) = delete; + AP_ESC_Telem_Backend &operator=(const AP_ESC_Telem_Backend&) = delete; + +protected: + // callback to update the rpm in the frontend, should be called by the driver when new data is available + void update_rpm(const uint8_t esc_index, const uint16_t new_rpm, const float error_rate = 0.0f); + + // callback to update the data in the frontend, should be called by the driver when new data is available + void update_telem_data(const uint8_t esc_index, const TelemetryData& new_data, const uint16_t data_present_mask); + +private: + AP_ESC_Telem* _frontend; +}; + +#endif diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.cpp new file mode 100644 index 0000000000..7a325250b9 --- /dev/null +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.cpp @@ -0,0 +1,48 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "AP_ESC_Telem_SITL.h" +#include "AP_ESC_Telem.h" +#include +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + +#include + +extern const AP_HAL::HAL& hal; + +AP_ESC_Telem_SITL::AP_ESC_Telem_SITL() +{ +} + +void AP_ESC_Telem_SITL::update() +{ + SITL::SITL* sitl = AP::sitl(); + + if (!sitl) { + return; + } + + if (is_zero(sitl->throttle)) { + return; + } + + for (uint8_t i = 0; i < sitl->state.num_motors; i++) { + update_rpm(i, sitl->state.rpm[sitl->state.vtol_motor_start+i]); + } +} + +#endif \ No newline at end of file diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.h b/libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.h new file mode 100644 index 0000000000..4218f718b3 --- /dev/null +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.h @@ -0,0 +1,20 @@ +#pragma once + +#include +#include "AP_ESC_Telem_Backend.h" +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + +class AP_ESC_Telem_SITL : public AP_ESC_Telem_Backend { +public: + AP_ESC_Telem_SITL(); + + void update(); + +protected: + +private: +}; + +#endif diff --git a/libraries/AP_ESC_Telem/LogStructure.h b/libraries/AP_ESC_Telem/LogStructure.h new file mode 100644 index 0000000000..055723c98b --- /dev/null +++ b/libraries/AP_ESC_Telem/LogStructure.h @@ -0,0 +1,34 @@ +#pragma once + +#include + +#define LOG_IDS_FROM_ESC_TELEM \ + LOG_ESC_MSG + +// @LoggerMessage: ESC +// @Description: Feedback received from ESCs +// @Field: TimeUS: microseconds since system startup +// @Field: Instance: ESC instance number +// @Field: RPM: reported motor rotation rate +// @Field: Volt: Perceived input voltage for the ESC +// @Field: Curr: Perceived current through the ESC +// @Field: Temp: ESC temperature in centi-degrees C +// @Field: CTot: current consumed total mAh +// @Field: MotTemp: measured motor temperature in centi-degrees C +// @Field: Err: error rate +struct PACKED log_Esc { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t instance; + int32_t rpm; + float voltage; + float current; + int16_t esc_temp; + float current_tot; + int16_t motor_temp; + float error_rate; +}; + +#define LOG_STRUCTURE_FROM_ESC_TELEM \ + { LOG_ESC_MSG, sizeof(log_Esc), \ + "ESC", "QBeffcfcf", "TimeUS,Instance,RPM,Volt,Curr,Temp,CTot,MotTemp,Err", "s#qvAOaO%", "F-B--BCB-" },