From 97057a135057173c1d53837659e9504c9796291a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 29 Jun 2021 00:24:15 +0200 Subject: [PATCH] SITL: add simulated FETtec ESC --- libraries/SITL/SIM_Aircraft.cpp | 4 + libraries/SITL/SIM_Aircraft.h | 4 + libraries/SITL/SIM_FETtecOneWireESC.cpp | 507 ++++++++++++++++++++++++ libraries/SITL/SIM_FETtecOneWireESC.h | 296 ++++++++++++++ libraries/SITL/SITL.cpp | 3 + libraries/SITL/SITL.h | 2 + 6 files changed, 816 insertions(+) create mode 100644 libraries/SITL/SIM_FETtecOneWireESC.cpp create mode 100644 libraries/SITL/SIM_FETtecOneWireESC.h diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 41f2248809..d0adafbb87 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -947,6 +947,10 @@ void Aircraft::update_external_payload(const struct sitl_input &input) richenpower->update(input); } + if (fetteconewireesc) { + fetteconewireesc->update(*this); + } + sitl->shipsim.update(); // update IntelligentEnergy 2.4kW generator diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index afbac4029c..3605bd17da 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -28,6 +28,7 @@ #include "SIM_Parachute.h" #include "SIM_Precland.h" #include "SIM_RichenPower.h" +#include "SIM_FETtecOneWireESC.h" #include "SIM_I2C.h" #include "SIM_Buzzer.h" #include "SIM_Battery.h" @@ -144,6 +145,7 @@ public: void set_sprayer(Sprayer *_sprayer) { sprayer = _sprayer; } void set_parachute(Parachute *_parachute) { parachute = _parachute; } void set_richenpower(RichenPower *_richenpower) { richenpower = _richenpower; } + void set_fetteconewireesc(FETtecOneWireESC *_fetteconewireesc) { fetteconewireesc = _fetteconewireesc; } void set_ie24(IntelligentEnergy24 *_ie24) { ie24 = _ie24; } void set_gripper_servo(Gripper_Servo *_gripper) { gripper = _gripper; } void set_gripper_epm(Gripper_EPM *_gripper_epm) { gripper_epm = _gripper_epm; } @@ -331,6 +333,8 @@ private: Gripper_EPM *gripper_epm; Parachute *parachute; RichenPower *richenpower; + FETtecOneWireESC *fetteconewireesc; + IntelligentEnergy24 *ie24; SIM_Precland *precland; class I2C *i2c; diff --git a/libraries/SITL/SIM_FETtecOneWireESC.cpp b/libraries/SITL/SIM_FETtecOneWireESC.cpp new file mode 100644 index 0000000000..7fc8a90bd4 --- /dev/null +++ b/libraries/SITL/SIM_FETtecOneWireESC.cpp @@ -0,0 +1,507 @@ +/* + 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 . + */ +/* + Simulator for the FETtecOneWireESC + + TODO: + - verify the assertion that DMA is required + - stop ignoring REQ_TYPE while in bootloader? + - correct visibility of members in simulation + - half-duplex will require the use of a thread as every time we call update() we expect to send out a configuration message + - tidy break vs return in AP_FETtec::handle_message + - determine if we should have a "REQ_OK" as well as an "OK" + - should rename simulated ESC "pwm" field to "value" or "fettec_value" or something + - periodically log _unknown_esc_message, _message_invalid_in_state_count, _period_too_short, _receive_buf_used to dataflash using a low prio thread. + - log type, version, subversion and sn to dataflash once. + + + +Protocol: + - SET_FAST_COM_LENGTH could set a 32-bit bitmask that will be present rather than requring consecutive motors + - Use two magic bytes in the header instead of just one + - Use a 16bit CRC + - the reply request needs to repeat the data that it replies to, to make sure the reply can be clearly assigned to a request + - need to cope with reversals + - in the case that we don't have ESC telemetry, consider probing ESCs periodically with an "OK"-request while disarmed +*/ + +#include + +#include "SIM_FETtecOneWireESC.h" +#include "SITL.h" +#include + +#include "SIM_Aircraft.h" + +#include +#include + +using namespace SITL; + +// table of user settable parameters +const AP_Param::GroupInfo FETtecOneWireESC::var_info[] = { + + // @Param: ENA + // @DisplayName: FETtec OneWire ESC simulator enable/disable + // @Description: Allows you to enable (1) or disable (0) the FETtecOneWireESC simulator + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + AP_GROUPINFO("ENA", 1, FETtecOneWireESC, _enabled, 0), + + // @Param: PWOF + // @DisplayName: Power off FETtec ESC mask + // @Description: Allows you to turn power off to the simulated ESCs. Bits correspond to the ESC ID, *NOT* their servo channel. + // @User: Advanced + AP_GROUPINFO("POW", 2, FETtecOneWireESC, _powered_mask, 0xfff), + + AP_GROUPEND +}; + +FETtecOneWireESC::FETtecOneWireESC() : SerialDevice::SerialDevice() +{ + AP_Param::setup_object_defaults(this, var_info); + + // initialise serial numbers and IDs + for (uint8_t n=0; n { + esc.id, + OK{} + }); + } + } + + for (auto &esc : escs) { + if (esc.state != ESC::State::RUNNING) { + continue; + } + // FIXME: this may not be an entirely accurate model of the + // temperature profile of these ESCs. + esc.temperature += esc.pwm/100000; + esc.temperature *= 0.95; + } +} + +void FETtecOneWireESC::update(const class Aircraft &aircraft) +{ + if (!_enabled.get()) { + return; + } + + update_escs(); + + update_input(); + update_send(aircraft); +} + +void FETtecOneWireESC::handle_config_message() +{ + ESC &esc = escs[u.config_message_header.target_id-1]; + simfet_debug("Config message type=%u esc=%u", (unsigned)u.config_message_header.request_type, (unsigned)u.config_message_header.target_id); + if ((ResponseFrameHeaderID)u.config_message_header.header != ResponseFrameHeaderID::MASTER) { + AP_HAL::panic("Unexpected header ID"); + } + switch (esc.state) { + case ESC::State::POWERED_OFF: + return; + case ESC::State::IN_BOOTLOADER: + return bootloader_handle_config_message(esc); + case ESC::State::RUNNING_START: + return; + case ESC::State::RUNNING: + return running_handle_config_message(esc); + } + AP_HAL::panic("Unknown state"); +} + +template +void FETtecOneWireESC::send_response(const T &r) +{ + // simfet_debug("Sending response"); + if (write_to_autopilot((char*)&r, sizeof(r)) != sizeof(r)) { + AP_HAL::panic("short write"); + } +} + +void FETtecOneWireESC::bootloader_handle_config_message(FETtecOneWireESC::ESC &esc) +{ + switch ((ConfigMessageType)u.config_message_header.request_type) { + case ConfigMessageType::OK: { + PackedMessage msg { + esc.id, + OK{}, + }; + msg.header = (uint8_t)ResponseFrameHeaderID::BOOTLOADER; + msg.update_checksum(); + send_response(msg); + return; + } + case ConfigMessageType::BL_PAGE_CORRECT: // BL only + case ConfigMessageType::NOT_OK: + break; + case ConfigMessageType::BL_START_FW: // BL only + esc.set_state(ESC::State::RUNNING_START); + // the main firmware sends an OK + return; + case ConfigMessageType::BL_PAGES_TO_FLASH: // BL only + break; + case ConfigMessageType::REQ_TYPE: + // ignore this for now + return; + case ConfigMessageType::REQ_SN: + case ConfigMessageType::REQ_SW_VER: + case ConfigMessageType::BEEP: + case ConfigMessageType::SET_FAST_COM_LENGTH: + case ConfigMessageType::SET_TLM_TYPE: //1 for alternative telemetry. ESC sends full telem per ESC: Temp, Volt, Current, ERPM, Consumption, CrcErrCount + case ConfigMessageType::SET_LED_TMP_COLOR: + break; + } + return; + AP_HAL::panic("Unhandled config message in bootloader (%u)", + (unsigned)u.config_message_header.request_type); +} + +void FETtecOneWireESC::running_handle_config_message(FETtecOneWireESC::ESC &esc) +{ + switch ((ConfigMessageType)u.config_message_header.request_type) { + + case ConfigMessageType::OK: + return send_response(PackedMessage { + esc.id, + OK{} + }); + + case ConfigMessageType::BL_PAGE_CORRECT: // BL only + case ConfigMessageType::NOT_OK: + break; + case ConfigMessageType::BL_START_FW: // BL only + ::fprintf(stderr, "received unexpected BL_START_FW message\n"); + AP_HAL::panic("received unexpected BL_START_FW message"); + return; + case ConfigMessageType::BL_PAGES_TO_FLASH: // BL only + break; + + case ConfigMessageType::REQ_TYPE: + return send_response(PackedMessage { + esc.id, + ESC_TYPE{esc.type} + }); + + case ConfigMessageType::REQ_SN: + return send_response(PackedMessage { + esc.id, + SN{esc.sn, ARRAY_SIZE(esc.sn)} + }); + + case ConfigMessageType::REQ_SW_VER: + return send_response(PackedMessage { + esc.id, + SW_VER{esc.sw_version, esc.sw_subversion} + }); + + case ConfigMessageType::BEEP: + break; + + case ConfigMessageType::SET_FAST_COM_LENGTH: + esc.fast_com.length = u.packed_set_fast_com_length.msg.length; + esc.fast_com.byte_count = u.packed_set_fast_com_length.msg.byte_count; + esc.fast_com.min_esc_id = u.packed_set_fast_com_length.msg.min_esc_id; + esc.fast_com.id_count = u.packed_set_fast_com_length.msg.id_count; + return send_response(PackedMessage { + esc.id, + OK{} + }); + + case ConfigMessageType::SET_TLM_TYPE: //1 for alternative telemetry. ESC sends full telem per ESC: Temp, Volt, Current, ERPM, Consumption, CrcErrCount + return handle_config_message_set_tlm_type(esc); + + case ConfigMessageType::SET_LED_TMP_COLOR: + break; + } + AP_HAL::panic("Unknown config message (%u)", (unsigned)u.config_message_header.request_type); +} + + +void FETtecOneWireESC::handle_config_message_set_tlm_type(ESC &esc) +{ + const TLMType type = (TLMType)u.packed_set_tlm_type.msg.type; + switch (type) { + case TLMType::NORMAL: + case TLMType::ALTERNATIVE: + esc.telem_type = type; + send_response(PackedMessage { + esc.id, + OK{} + }); + return; + } + AP_HAL::panic("unknown telem type=%u", (unsigned)type); +} + +void FETtecOneWireESC::handle_fast_esc_data() +{ + // decode first byte - see driver for details + const uint8_t telem_request = u.buffer[0] >> 4; + + // offset into escs array for first esc involved in fast-throttle + // command: + const uint8_t esc0_ofs = fast_com.min_esc_id - 1; + + // ::fprintf(stderr, "telem_request=%u\n", (unsigned)telem_request); + uint16_t esc0_pwm; + esc0_pwm = ((u.buffer[0] >> 3) & 0x1) << 10; + if ((u.buffer[0] & 0b111) != 0x1) { + AP_HAL::panic("expected fast-throttle command"); + } + + // decode second byte + esc0_pwm |= (u.buffer[1] >> 5) << 7; + if ((u.buffer[1] & 0b00011111) != 0x1f) { + AP_HAL::panic("Unexpected 5-bit target id"); + } + + // decode enough of third byte to complete pwm[0] + esc0_pwm |= u.buffer[2] >> 1; + + if (escs[esc0_ofs].state == ESC::State::RUNNING) { + ESC &esc { escs[esc0_ofs] }; + esc.pwm = esc0_pwm; + + if (telem_request == esc.id) { + esc.telem_request = true; + } + simfet_debug("esc=%u out: %u", esc.id, (unsigned)esc.pwm); + } + + // decode remainder of ESC values + + // slides a window across the input buffer, extracting 11-bit ESC + // values. The top 11 bits in "window" are the ESC value. + uint8_t byte_ofs = 2; + uint32_t window = u.buffer[byte_ofs++]<<24; + window <<= 7; + uint8_t bits_free = 32-1; + for (uint8_t i=esc0_ofs+1; i 7) { + window |= u.buffer[byte_ofs++] << (bits_free-8); + bits_free -= 8; + } + ESC &esc { escs[i] }; + if (esc.state == ESC::State::RUNNING) { + if (telem_request == esc.id) { + esc.telem_request = true; + } + esc.pwm = window >> 21; + simfet_debug("esc=%u out: %u", esc.id, (unsigned)esc.pwm); + } + window <<= 11; + bits_free += 11; + } + + for (uint8_t i=0; i= 1000 && esc.pwm <= 2000) { + continue; + } + AP_HAL::panic("transmitted value out of range (%u)", esc.pwm); + } +} + +void FETtecOneWireESC::consume_bytes(uint8_t count) +{ + if (count > buflen) { + AP_HAL::panic("Consuming more bytes than in buffer?"); + } + if (buflen == count) { + buflen = 0; + return; + } + memmove(&u.buffer[0], &u.buffer[u.config_message_header.frame_len], buflen - u.config_message_header.frame_len); + buflen -= u.config_message_header.frame_len; +} + +void FETtecOneWireESC::update_input() +{ + const ssize_t n = read_from_autopilot((char*)&u.buffer[buflen], ARRAY_SIZE(u.buffer) - buflen - 1); + if (n < 0) { + // TODO: do better here + if (errno != EAGAIN && errno != EWOULDBLOCK && errno != 0) { + AP_HAL::panic("Failed to read from autopilot"); + } + } else { + buflen += n; + } + + // bool config_message_checksum_fail = false; + + if (buflen > offsetof(ConfigMessageHeader, header) && + u.config_message_header.header == 0x01 && + buflen > offsetof(ConfigMessageHeader, frame_len) && + buflen >= u.config_message_header.frame_len) { + const uint8_t calculated_checksum = crc8_dvb_update(0, u.buffer, u.config_message_header.frame_len-1); + const uint8_t received_checksum = u.buffer[u.config_message_header.frame_len-1]; + if (calculated_checksum == received_checksum) { + handle_config_message(); + // consume the message: + consume_bytes(u.config_message_header.frame_len); + return; + } else { + simfet_debug("Checksum mismatch"); + abort(); + // config_message_checksum_fail = true; + } + return; // 1 message/loop.... + } + + // no config message, so let's see if there's fast PWM input. + if (fast_com.id_count == 255) { + // see if any ESC has been configured: + for (uint8_t i=0; i= 0) { + abort(); + } + buflen = 0; +} + +void FETtecOneWireESC::update_sitl_input_pwm(struct sitl_input &input) +{ + // overwrite the SITL input values passed through from + // sitl_model->update_model with those we're receiving serially + for (auto &esc : escs) { + if (esc.id > ARRAY_SIZE(input.servos)) { + // silently ignore; input.servos is 12-long, we are + // usually 16-long + continue; + } + input.servos[esc.id-1] = esc.pwm; + } +} + +void FETtecOneWireESC::send_esc_telemetry(const Aircraft &aircraft) +{ + for (auto &esc : escs) { + if (!esc.telem_request) { + continue; + } + esc.telem_request = false; + if (esc.state != ESC::State::RUNNING) { + continue; + } + if (esc.telem_type != TLMType::ALTERNATIVE) { + // no idea what "normal" looks like + abort(); + } + + const int8_t temp_cdeg = esc.temperature * 100; + const uint16_t voltage = aircraft.get_battery_voltage() * 100; + const uint16_t current = (6 + esc.id * 100); + + // FIXME: the vehicle models should be supplying this RPM! + const uint16_t Kv = 1000; + const float p = (esc.pwm-1000)/1000.0; + int16_t rpm = aircraft.get_battery_voltage() * Kv * p; + + const uint16_t consumption_mah = 0; + const uint16_t errcount = 17; + send_response(PackedMessage { + esc.id, + ESCTelem{temp_cdeg, voltage, current, rpm, consumption_mah, errcount} + }); + } +} + +void FETtecOneWireESC::update_send(const Aircraft &aircraft) +{ + send_esc_telemetry(aircraft); +} diff --git a/libraries/SITL/SIM_FETtecOneWireESC.h b/libraries/SITL/SIM_FETtecOneWireESC.h new file mode 100644 index 0000000000..683eb9785b --- /dev/null +++ b/libraries/SITL/SIM_FETtecOneWireESC.h @@ -0,0 +1,296 @@ +/* + 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 . + */ +/* + Simulator for the FETtecOneWire ESCs + +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:fetteconewireesc --speedup=1 --console + +param set SERIAL5_PROTOCOL 38 +param set SERIAL5_BAUD 500000 +param set SERVO_FTW_MASK 15 +param set SIM_FTOWESC_ENA 1 +reboot + +param fetch + +#param set SIM_FTOWESC_FM 1 # fail mask + +./Tools/autotest/autotest.py --gdb --debug build.ArduCopter fly.ArduCopter.FETtecESC + +*/ + +#pragma once + +#include + +#include "SITL_Input.h" + +#include "SIM_SerialDevice.h" + +#define SIM_FTW_DEBUGGING 0 +#if SIM_FTW_DEBUGGING +#include +#define simfet_debug(fmt, args ...) do {::fprintf(stderr,"SIMFET: %s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0) +#else +#define simfet_debug(fmt, args ...) +#endif + +#include + +namespace SITL { + +class FETtecOneWireESC : public SerialDevice { +public: + + FETtecOneWireESC(); + + // update state + void update(const class Aircraft &aircraft); + + static const AP_Param::GroupInfo var_info[]; + + bool enabled() const { return _enabled.get(); } + + void update_sitl_input_pwm(struct sitl_input &input); + +private: + + AP_Int8 _enabled; // enable FETtec ESC sim + AP_Int32 _powered_mask; // mask of ESCs with power + + struct PACKED ConfigMessageHeader { + uint8_t header; // master is always 0x01 + uint8_t target_id; // 5 bits only + uint16_t frame_type; + uint8_t frame_len; + uint8_t request_type; + }; + + enum class TLMType : uint8_t { + NORMAL = 0, + ALTERNATIVE =1, + }; + + template + class PACKED PackedMessage { + public: + PackedMessage(uint8_t _target_id, T _msg) : + target_id{_target_id}, + msg{_msg} + { + frame_len = 6 + sizeof(T); + update_checksum(); + } + uint8_t header = (uint8_t)ResponseFrameHeaderID::ESC; // master is always 0x01 + uint8_t target_id; // 5 bits only + uint16_t frame_type = 0; + uint8_t frame_len; + T msg; + uint8_t checksum; + + uint8_t calculate_checksum() const WARN_IF_UNUSED { + return crc8_dvb_update(0, (const uint8_t*)this, frame_len-1); + } + + void update_checksum() { + checksum = calculate_checksum(); + } + }; + + class PACKED SetTLMType { + public: + SetTLMType(uint8_t _type) : + type(_type) + { } + uint8_t request_type { (uint8_t)ConfigMessageType::SET_TLM_TYPE }; + uint8_t type; + }; + + class PACKED OK { + public: + OK() { } + uint8_t request_type { (uint8_t)ConfigMessageType::OK }; + }; + + class PACKED ESC_TYPE { + public: + ESC_TYPE(uint8_t _type) : + type{_type} { } + uint8_t type; + }; + + class PACKED SW_VER { + public: + SW_VER(uint8_t _version, uint8_t _subversion) : + version{_version}, + subversion{_subversion} + { } + uint8_t version; + uint8_t subversion; + }; + + class PACKED SN { + public: + SN(uint8_t *_sn, uint8_t snlen) { + memset(sn, 0, ARRAY_SIZE(sn)); + memcpy(sn, _sn, MIN(ARRAY_SIZE(sn), snlen)); + } + uint8_t sn[12]; + }; + + class PACKED SetFastComLength { + public: + SetFastComLength() { + } + uint8_t length; + uint8_t byte_count; + uint8_t min_esc_id; + uint8_t id_count; + }; + + // tlm_from_id = (uint8_t)telem[1]; + + // t.temperature_cdeg = int16_t(telem[5+0] * 100); + // t.voltage = float((telem[5+1]<<8)|telem[5+2]) * 0.01f; + // t.current = float((telem[5+3]<<8)|telem[5+4]) * 0.01f; + // centi_erpm = (telem[5+5]<<8)|telem[5+6]; + // t.consumption_mah = float((telem[5+7]<<8)|telem[5+8]); + // tx_err_count = (telem[5+9]<<8)|telem[5+10]; + class PACKED ESCTelem { + public: + ESCTelem(int8_t _temp, uint16_t _voltage, uint16_t _current, int16_t _rpm, uint16_t _consumption_mah, uint16_t _tx_err_count) : + temp{_temp}, + voltage{_voltage}, + current{_current}, + rpm{_rpm}, + consumption_mah{_consumption_mah}, + tx_err_count{_tx_err_count} + { } + int8_t temp; // centidegrees + uint16_t voltage; // centivolts + uint16_t current; // centiamps (signed?) + int16_t rpm; // centi-rpm + uint16_t consumption_mah; // ??? + uint16_t tx_err_count; + }; + + + union MessageUnion { + MessageUnion() { } + uint8_t buffer[255]; +// uint16_t checksum_buffer[35]; + struct ConfigMessageHeader config_message_header; + + PackedMessage packed_ok; + PackedMessage packed_set_tlm_type; + PackedMessage packed_set_fast_com_length; + + // void update_checksum(); + }; + MessageUnion u; + uint8_t buflen; + + // remove count bytes from the start of u.buffer + void consume_bytes(uint8_t count); + + enum class ConfigMessageType { + OK = 0, + BL_PAGE_CORRECT = 1, // BL only + NOT_OK = 2, + BL_START_FW = 3, // BL only + BL_PAGES_TO_FLASH = 4, // BL only + REQ_TYPE = 5, + REQ_SN = 6, + REQ_SW_VER = 7, + BEEP = 13, + SET_FAST_COM_LENGTH = 26, + SET_TLM_TYPE = 27, //1 for alternative telemetry. ESC sends full telem per ESC: Temp, Volt, Current, ERPM, Consumption, CrcErrCount + SET_LED_TMP_COLOR = 51, + }; + + class ESC { + public: + + enum class State { + POWERED_OFF = 17, + IN_BOOTLOADER, + RUNNING_START, + RUNNING, + // UNRESPONSIVE, + }; + + void set_state(State _state) { + simfet_debug("Moving ESC.id=%u from state=%u to state=%u", (unsigned)id, (unsigned)state, (unsigned)_state); + state = _state; + } + + State state = State::POWERED_OFF; + + uint8_t sn[12]; + TLMType telem_type; + uint8_t id; + uint8_t type = 34; + static const uint8_t sw_version = 3; + static const uint8_t sw_subversion = 4; + + // make sure to zero any state when powering the virtual ESC on + struct { + uint8_t length; + uint8_t byte_count; + uint8_t min_esc_id; + uint8_t id_count = 255; + } fast_com; + + uint16_t pwm; + bool telem_request; // true if we've been asked to send telem + + uint8_t ofs; + + float temperature; + }; + + // canonical structure used for fast com, copied from an ESC + struct { + uint8_t min_esc_id; + uint8_t id_count = 255; + } fast_com; + + void bootloader_handle_config_message(ESC &esc); + void running_handle_config_message(ESC &esc); + + void handle_config_message(); + void handle_config_message_set_tlm_type(ESC &esc); + + void handle_fast_esc_data(); + + void update_escs(); + void update_send(const class Aircraft &aircraft); + void update_input(); + + void send_esc_telemetry(const class Aircraft &aircraft); + + template + void send_response(const T& r); + + enum class ResponseFrameHeaderID { + MASTER = 0x01, + BOOTLOADER = 0x02, + ESC = 0x03, + }; + + ESC escs[16]; +}; + +} diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index c7f4985619..c2b757102e 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -213,6 +213,9 @@ const AP_Param::GroupInfo SIM::var_info3[] = { // count of simulated IMUs AP_GROUPINFO("IMU_COUNT", 23, SIM, imu_count, 2), + // @Path: ./SIM_FETtecOneWireESC.cpp + AP_SUBGROUPINFO(fetteconewireesc_sim, "FTOWESC_", 30, SIM, FETtecOneWireESC), + // @Path: ./SIM_RichenPower.cpp AP_SUBGROUPINFO(richenpower_sim, "RICH_", 31, SIM, RichenPower), diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 65479fe56c..3ff4d2d483 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -20,6 +20,7 @@ #include "SIM_ToneAlarm.h" #include "SIM_EFI_MegaSquirt.h" #include "SIM_RichenPower.h" +#include "SIM_FETtecOneWireESC.h" #include "SIM_IntelligentEnergy24.h" #include "SIM_Ship.h" #include @@ -421,6 +422,7 @@ public: SIM_Precland precland_sim; RichenPower richenpower_sim; IntelligentEnergy24 ie24_sim; + FETtecOneWireESC fetteconewireesc_sim; // ESC telemetry AP_Int8 esc_telem;