Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : Simulator for the FETtecOneWire ESCs
17 :
18 : ./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:fetteconewireesc --speedup=1 --console
19 :
20 : param set SERIAL5_PROTOCOL 38
21 : param set SERIAL5_BAUD 500000
22 : param set SERVO_FTW_MASK 15
23 : param set SIM_FTOWESC_ENA 1
24 : reboot
25 :
26 : param fetch
27 :
28 : #param set SIM_FTOWESC_FM 1 # fail mask
29 :
30 : ./Tools/autotest/autotest.py --gdb --debug build.ArduCopter fly.ArduCopter.FETtecESC
31 :
32 : */
33 :
34 : #pragma once
35 :
36 : #include <AP_Param/AP_Param.h>
37 :
38 : #include "SITL_Input.h"
39 :
40 : #include "SIM_SerialDevice.h"
41 :
42 : #include <stdio.h>
43 :
44 : #define SIM_FTW_DEBUGGING 0
45 : #if SIM_FTW_DEBUGGING
46 : #define simfet_debug(fmt, args ...) do {::fprintf(stderr,"SIMFET: %s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
47 : #else
48 : #define simfet_debug(fmt, args ...)
49 : #endif
50 :
51 :
52 : namespace SITL {
53 :
54 : class FETtecOneWireESC : public SerialDevice {
55 : public:
56 :
57 : FETtecOneWireESC();
58 :
59 : // update state
60 : void update(const class Aircraft &aircraft);
61 :
62 : static const AP_Param::GroupInfo var_info[];
63 :
64 187275482 : bool enabled() const { return _enabled.get(); }
65 :
66 : void update_sitl_input_pwm(struct sitl_input &input);
67 :
68 : private:
69 :
70 : AP_Int8 _enabled; // enable FETtec ESC sim
71 : AP_Int32 _powered_mask; // mask of ESCs with power
72 :
73 : struct PACKED ConfigMessageHeader {
74 : uint8_t header; // master is always 0x01
75 : uint8_t target_id; // 5 bits only
76 : uint16_t frame_type;
77 : uint8_t frame_len;
78 : uint8_t request_type;
79 : };
80 :
81 : enum class TLMType : uint8_t {
82 : NORMAL = 0,
83 : ALTERNATIVE =1,
84 : };
85 :
86 : template <typename T>
87 : class PACKED PackedMessage {
88 : public:
89 188422 : PackedMessage(uint8_t _target_id, T _msg) :
90 188422 : target_id{_target_id},
91 188422 : msg{_msg}
92 : {
93 188422 : frame_len = 6 + sizeof(T);
94 188422 : update_checksum();
95 188422 : }
96 : uint8_t header = (uint8_t)ResponseFrameHeaderID::ESC; // master is always 0x01
97 : uint8_t target_id; // 5 bits only
98 : uint16_t frame_type = 0;
99 : uint8_t frame_len;
100 : T msg;
101 : uint8_t checksum;
102 :
103 188487 : uint8_t calculate_checksum() const WARN_IF_UNUSED {
104 188487 : return crc8_dvb_update(0, (const uint8_t*)this, frame_len-1);
105 : }
106 :
107 188487 : void update_checksum() {
108 188487 : checksum = calculate_checksum();
109 188487 : }
110 : };
111 :
112 : class PACKED SetTLMType {
113 : public:
114 : SetTLMType(uint8_t _type) :
115 : type(_type)
116 : { }
117 : uint8_t request_type { (uint8_t)ConfigMessageType::SET_TLM_TYPE };
118 : uint8_t type;
119 : };
120 :
121 : class PACKED OK {
122 : public:
123 260 : OK() { }
124 : uint8_t request_type { (uint8_t)ConfigMessageType::OK };
125 : };
126 :
127 : class PACKED ESC_TYPE {
128 : public:
129 0 : ESC_TYPE(uint8_t _type) :
130 0 : type{_type} { }
131 : uint8_t type;
132 : };
133 :
134 : class PACKED SW_VER {
135 : public:
136 0 : SW_VER(uint8_t _version, uint8_t _subversion) :
137 0 : version{_version},
138 0 : subversion{_subversion}
139 0 : { }
140 : uint8_t version;
141 : uint8_t subversion;
142 : };
143 :
144 : class PACKED SN {
145 : public:
146 0 : SN(uint8_t *_sn, uint8_t snlen) {
147 0 : memset(sn, 0, ARRAY_SIZE(sn));
148 0 : memcpy(sn, _sn, MIN(ARRAY_SIZE(sn), snlen));
149 0 : }
150 : uint8_t sn[12];
151 : };
152 :
153 : class PACKED SetFastComLength {
154 : public:
155 : SetFastComLength() {
156 : }
157 : uint8_t length;
158 : uint8_t byte_count;
159 : uint8_t min_esc_id;
160 : uint8_t id_count;
161 : };
162 :
163 : // tlm_from_id = (uint8_t)telem[1];
164 :
165 : // t.temperature_cdeg = int16_t(telem[5+0] * 100);
166 : // t.voltage = float((telem[5+1]<<8)|telem[5+2]) * 0.01f;
167 : // t.current = float((telem[5+3]<<8)|telem[5+4]) * 0.01f;
168 : // centi_erpm = (telem[5+5]<<8)|telem[5+6];
169 : // t.consumption_mah = float((telem[5+7]<<8)|telem[5+8]);
170 : // tx_err_count = (telem[5+9]<<8)|telem[5+10];
171 : class PACKED ESCTelem {
172 : public:
173 188162 : ESCTelem(int8_t _temp, uint16_t _voltage, uint16_t _current, int16_t _rpm, uint16_t _consumption_mah, uint16_t _tx_err_count) :
174 188162 : temp{_temp},
175 188162 : voltage{_voltage},
176 188162 : current{_current},
177 188162 : rpm{_rpm},
178 188162 : consumption_mah{_consumption_mah},
179 188162 : tx_err_count{_tx_err_count}
180 188162 : { }
181 : int8_t temp; // centidegrees
182 : uint16_t voltage; // centivolts
183 : uint16_t current; // centiamps (signed?)
184 : int16_t rpm; // centi-rpm
185 : uint16_t consumption_mah; // ???
186 : uint16_t tx_err_count;
187 : };
188 :
189 :
190 : union MessageUnion {
191 2096 : MessageUnion() { }
192 : uint8_t buffer[255];
193 : // uint16_t checksum_buffer[35];
194 : struct ConfigMessageHeader config_message_header;
195 :
196 : PackedMessage<OK> packed_ok;
197 : PackedMessage<SetTLMType> packed_set_tlm_type;
198 : PackedMessage<SetFastComLength> packed_set_fast_com_length;
199 :
200 : // void update_checksum();
201 : };
202 : MessageUnion u;
203 : uint8_t buflen;
204 :
205 : // remove count bytes from the start of u.buffer
206 : void consume_bytes(uint8_t count);
207 :
208 : enum class ConfigMessageType {
209 : OK = 0,
210 : BL_PAGE_CORRECT = 1, // BL only
211 : NOT_OK = 2,
212 : BL_START_FW = 3, // BL only
213 : BL_PAGES_TO_FLASH = 4, // BL only
214 : REQ_TYPE = 5,
215 : REQ_SN = 6,
216 : REQ_SW_VER = 7,
217 : BEEP = 13,
218 : SET_FAST_COM_LENGTH = 26,
219 : SET_TLM_TYPE = 27, //1 for alternative telemetry. ESC sends full telem per ESC: Temp, Volt, Current, ERPM, Consumption, CrcErrCount
220 : SET_LED_TMP_COLOR = 51,
221 : };
222 :
223 : class ESC {
224 : public:
225 :
226 : enum class State {
227 : POWERED_OFF = 17,
228 : IN_BOOTLOADER,
229 : RUNNING_START,
230 : RUNNING,
231 : // UNRESPONSIVE,
232 : };
233 :
234 130 : void set_state(State _state) {
235 : simfet_debug("Moving ESC.id=%u from state=%u to state=%u", (unsigned)id, (unsigned)state, (unsigned)_state);
236 130 : state = _state;
237 130 : }
238 :
239 : State state = State::POWERED_OFF;
240 :
241 : uint8_t sn[12];
242 : TLMType telem_type;
243 : uint8_t id;
244 : uint8_t type = 34;
245 : static const uint8_t sw_version = 3;
246 : static const uint8_t sw_subversion = 4;
247 :
248 : // make sure to zero any state when powering the virtual ESC on
249 : struct {
250 : uint8_t length;
251 : uint8_t byte_count;
252 : uint8_t min_esc_id;
253 : uint8_t id_count = 255;
254 : } fast_com;
255 :
256 : uint16_t pwm;
257 : bool telem_request; // true if we've been asked to send telem
258 :
259 : uint8_t ofs;
260 :
261 : float temperature;
262 : };
263 :
264 : // canonical structure used for fast com, copied from an ESC
265 : struct {
266 : uint8_t min_esc_id;
267 : uint8_t id_count = 255;
268 : } fast_com;
269 :
270 : void bootloader_handle_config_message(ESC &esc);
271 : void running_handle_config_message(ESC &esc);
272 :
273 : void handle_config_message();
274 : void handle_config_message_set_tlm_type(ESC &esc);
275 :
276 : void handle_fast_esc_data();
277 :
278 : void update_escs();
279 : void update_send(const class Aircraft &aircraft);
280 : void update_input();
281 :
282 : void send_esc_telemetry(const class Aircraft &aircraft);
283 :
284 : template <typename T>
285 : void send_response(const T& r);
286 :
287 : enum class ResponseFrameHeaderID {
288 : MASTER = 0x01,
289 : BOOTLOADER = 0x02,
290 : ESC = 0x03,
291 : };
292 :
293 : ESC escs[16];
294 : };
295 :
296 : }
|
