diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index 4e0e028bb1..2142f644c0 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/libraries/SITL/SIM_Aircraft.cpp
@@ -944,6 +944,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 923229b33b..1c79eb8aee 100644
--- a/libraries/SITL/SIM_Aircraft.h
+++ b/libraries/SITL/SIM_Aircraft.h
@@ -29,6 +29,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; }
@@ -332,6 +334,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..c1159a4352
--- /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[count], buflen - count);
+ buflen -= count;
+}
+
+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 8d8c14e4fe..bdf7ffa7fc 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -213,6 +213,9 @@ const AP_Param::GroupInfo SITL::var_info3[] = {
// count of simulated IMUs
AP_GROUPINFO("IMU_COUNT", 23, SITL, imu_count, 2),
+ // @Path: ./SIM_FETtecOneWireESC.cpp
+ AP_SUBGROUPINFO(fetteconewireesc_sim, "FTOWESC_", 30, SITL, FETtecOneWireESC),
+
// @Path: ./SIM_RichenPower.cpp
AP_SUBGROUPINFO(richenpower_sim, "RICH_", 31, SITL, RichenPower),
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index 63ab85e173..7e4e272a38 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;