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 : i-BUS telemetry for FlySky/Turnigy receivers and other peripherals
17 : (eg iA6B, iA10) by Nicole Ashley <[email protected]>.
18 :
19 : Originally based on work by Jan Verhulst:
20 : https://github.com/ArduPilot/ardupilot/pull/16545
21 :
22 : Libraries used for reference and inspiration:
23 :
24 : * iBUStelemetry
25 : https://github.com/Hrastovc/iBUStelemetry
26 :
27 : * IBusBM
28 : https://github.com/bmellink/IBusBM
29 :
30 : * BetaFlight
31 : https://github.com/betaflight/betaflight/blob/master/src/main/telemetry/ibus_shared.c
32 : */
33 :
34 : #include <AP_IBus_Telem/AP_IBus_Telem.h>
35 :
36 : #if AP_IBUS_TELEM_ENABLED
37 :
38 : #include <AP_HAL/AP_HAL.h>
39 : #include <AP_Vehicle/AP_Vehicle.h>
40 : #include <AP_BattMonitor/AP_BattMonitor.h>
41 : #include <AP_Arming/AP_Arming.h>
42 : #include <AP_GPS/AP_GPS.h>
43 : #include <AP_RPM/AP_RPM.h>
44 :
45 : // 2-byte values
46 : #define IBUS_SENSOR_TYPE_TEMPERATURE 0x01 // Temperature (in 0.1 degrees, where 0=-40'C)
47 : #define IBUS_SENSOR_TYPE_RPM_FLYSKY 0x02 // FlySky-specific throttle value
48 : #define IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE 0x03 // External voltage (in centivolts, so 1450 is 14.50V)
49 : #define IBUS_SENSOR_TYPE_AVERAGE_CELL_VOLTAGE 0x04 // Avg cell voltage (in centivolts, so 1450 is 14.50V)
50 : #define IBUS_SENSOR_TYPE_BATTERY_CURRENT 0x05 // Battery current (centi-amps)
51 : #define IBUS_SENSOR_TYPE_FUEL 0x06 // Remaining battery percentage
52 : #define IBUS_SENSOR_TYPE_RPM 0x07 // Throttle value (in 0.01, so 1200 is 12.00%)
53 : #define IBUS_SENSOR_TYPE_COMPASS_HEADING 0x08 // Heading (0-360 degrees)
54 : #define IBUS_SENSOR_TYPE_CLIMB_RATE 0x09 // Climb rate (cm/s)
55 : #define IBUS_SENSOR_TYPE_COG 0x0a // Course over ground (centidegrees, so 27015 is 270.15 degrees)
56 : #define IBUS_SENSOR_TYPE_GPS_STATUS 0x0b // GPS status (2 values: fix type, and number of satellites)
57 : #define IBUS_SENSOR_TYPE_ACC_X 0x0c // Acc X (cm/s)
58 : #define IBUS_SENSOR_TYPE_ACC_Y 0x0d // Acc Y (cm/s)
59 : #define IBUS_SENSOR_TYPE_ACC_Z 0x0e // Acc Z (cm/s)
60 : #define IBUS_SENSOR_TYPE_ROLL 0x0f // Roll (centidegrees)
61 : #define IBUS_SENSOR_TYPE_PITCH 0x10 // Pitch (centidegrees)
62 : #define IBUS_SENSOR_TYPE_YAW 0x11 // Yaw (centidegrees)
63 : #define IBUS_SENSOR_TYPE_VERTICAL_SPEED 0x12 // Vertical speed (cm/s)
64 : #define IBUS_SENSOR_TYPE_GROUND_SPEED 0x13 // Speed (cm/s)
65 : #define IBUS_SENSOR_TYPE_GPS_DIST 0x14 // Distance from home (m)
66 : #define IBUS_SENSOR_TYPE_ARMED 0x15 // Armed / unarmed (1 = armed, 0 = unarmed)
67 : #define IBUS_SENSOR_TYPE_FLIGHT_MODE 0x16 // Flight mode
68 : #define IBUS_SENSOR_TYPE_ODO1 0x7c // Odometer1
69 : #define IBUS_SENSOR_TYPE_ODO2 0x7d // Odometer2
70 : #define IBUS_SENSOR_TYPE_SPEED 0x7e // Speed km/h
71 : #define IBUS_SENSOR_TYPE_ALT_FLYSKY 0xf9 // FlySky-specific altitude (metres)
72 :
73 : // 4-byte values
74 : #define IBUS_SENSOR_TYPE_TEMPERATURE_PRESSURE 0x41 // Combined temperature & pressure value
75 : #define IBUS_SENSOR_TYPE_GPS_LAT 0x80 // WGS84 in degrees * 1E7
76 : #define IBUS_SENSOR_TYPE_GPS_LNG 0x81 // WGS84 in degrees * 1E7
77 : #define IBUS_SENSOR_TYPE_GPS_ALT 0x82 // GPS (cm)
78 : #define IBUS_SENSOR_TYPE_ALT 0x83 // Alt (cm)
79 : #define IBUS_SENSOR_TYPE_ALT_MAX 0x84 // MaxAlt (cm)
80 :
81 : // i-BUS vehicle modes
82 : #define IBUS_VEHICLE_MODE_STAB 0
83 : #define IBUS_VEHICLE_MODE_ACRO 1
84 : #define IBUS_VEHICLE_MODE_AHOLD 2
85 : #define IBUS_VEHICLE_MODE_AUTO 3
86 : #define IBUS_VEHICLE_MODE_GUIDED 4
87 : #define IBUS_VEHICLE_MODE_LOITER 5
88 : #define IBUS_VEHICLE_MODE_RTL 6
89 : #define IBUS_VEHICLE_MODE_CIRCLE 7
90 : #define IBUS_VEHICLE_MODE_PHOLD 8
91 : #define IBUS_VEHICLE_MODE_LAND 9
92 : #define IBUS_VEHICLE_MODE_UNKNOWN 255 // Must be positive and 0 is already used; out of range blanks the value
93 :
94 : // All the sensors we can accurately provide are listed here.
95 : // i-BUS will generally only query up to 15 sensors, so subjectively
96 : // higher-value sensors are sorted to the top to make the most of a
97 : // small telemetry window. In the future these could be configurable.
98 : static const AP_IBus_Telem::SensorDefinition sensors[] {
99 : #if AP_ARMING_ENABLED
100 : {.sensor_type = IBUS_SENSOR_TYPE_ARMED, .sensor_length = 2},
101 : #endif
102 : {.sensor_type = IBUS_SENSOR_TYPE_FLIGHT_MODE, .sensor_length = 2},
103 : #if AP_GPS_ENABLED
104 : {.sensor_type = IBUS_SENSOR_TYPE_GPS_STATUS, .sensor_length = 2},
105 : #endif
106 : #if AP_BATTERY_ENABLED
107 : {.sensor_type = IBUS_SENSOR_TYPE_FUEL, .sensor_length = 2},
108 : {.sensor_type = IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE, .sensor_length = 2},
109 : #endif
110 : #if AP_BARO_ENABLED
111 : {.sensor_type = IBUS_SENSOR_TYPE_ALT, .sensor_length = 4},
112 : #endif
113 : #if AP_AHRS_ENABLED
114 : {.sensor_type = IBUS_SENSOR_TYPE_GPS_DIST, .sensor_length = 2},
115 : #endif
116 : #if AP_BARO_ENABLED
117 : {.sensor_type = IBUS_SENSOR_TYPE_CLIMB_RATE, .sensor_length = 2},
118 : #endif
119 : #if AP_GPS_ENABLED
120 : {.sensor_type = IBUS_SENSOR_TYPE_GROUND_SPEED, .sensor_length = 2},
121 : #endif
122 : #if AP_AHRS_ENABLED
123 : {.sensor_type = IBUS_SENSOR_TYPE_ROLL, .sensor_length = 2},
124 : {.sensor_type = IBUS_SENSOR_TYPE_PITCH, .sensor_length = 2},
125 : {.sensor_type = IBUS_SENSOR_TYPE_YAW, .sensor_length = 2},
126 : #endif
127 : #if AP_AIRSPEED_ENABLED
128 : {.sensor_type = IBUS_SENSOR_TYPE_SPEED, .sensor_length = 2},
129 : #endif
130 : #if AP_BARO_ENABLED
131 : {.sensor_type = IBUS_SENSOR_TYPE_TEMPERATURE_PRESSURE, .sensor_length = 4},
132 : #endif
133 : #if AP_RPM_ENABLED
134 : {.sensor_type = IBUS_SENSOR_TYPE_RPM, .sensor_length = 2},
135 : #endif
136 : #if AP_BATTERY_ENABLED
137 : {.sensor_type = IBUS_SENSOR_TYPE_BATTERY_CURRENT, .sensor_length = 2},
138 : {.sensor_type = IBUS_SENSOR_TYPE_AVERAGE_CELL_VOLTAGE, .sensor_length = 2},
139 : #endif
140 : #if AP_AHRS_ENABLED
141 : {.sensor_type = IBUS_SENSOR_TYPE_COMPASS_HEADING, .sensor_length = 2},
142 : #endif
143 : #if AP_GPS_ENABLED
144 : {.sensor_type = IBUS_SENSOR_TYPE_COG, .sensor_length = 2},
145 : {.sensor_type = IBUS_SENSOR_TYPE_GPS_LAT, .sensor_length = 4},
146 : {.sensor_type = IBUS_SENSOR_TYPE_GPS_LNG, .sensor_length = 4},
147 : {.sensor_type = IBUS_SENSOR_TYPE_GPS_ALT, .sensor_length = 4},
148 : #endif
149 : #if AP_AHRS_ENABLED
150 : {.sensor_type = IBUS_SENSOR_TYPE_ACC_X, .sensor_length = 2},
151 : {.sensor_type = IBUS_SENSOR_TYPE_ACC_Y, .sensor_length = 2},
152 : {.sensor_type = IBUS_SENSOR_TYPE_ACC_Z, .sensor_length = 2},
153 : #endif
154 : };
155 :
156 : #if APM_BUILD_TYPE(APM_BUILD_Rover)
157 :
158 : /* Rover modes:
159 : MANUAL = 0
160 : ACRO = 1
161 : STEERING = 3
162 : HOLD = 4
163 : LOITER = 5
164 : FOLLOW = 6
165 : SIMPLE = 7
166 : DOCK = 8
167 : CIRCLE = 9
168 : AUTO = 10
169 : RTL = 11
170 : SMART_RTL = 12
171 : GUIDED = 15
172 : INITIALISING = 16
173 : */
174 : static const AP_IBus_Telem::ModeMap mode_map[] {
175 : {.ap_mode = 1, .ibus_mode = IBUS_VEHICLE_MODE_ACRO},
176 : {.ap_mode = 4, .ibus_mode = IBUS_VEHICLE_MODE_PHOLD},
177 : {.ap_mode = 5, .ibus_mode = IBUS_VEHICLE_MODE_LOITER},
178 : {.ap_mode = 9, .ibus_mode = IBUS_VEHICLE_MODE_CIRCLE},
179 : {.ap_mode = 10, .ibus_mode = IBUS_VEHICLE_MODE_AUTO},
180 : {.ap_mode = 11, .ibus_mode = IBUS_VEHICLE_MODE_RTL},
181 : {.ap_mode = 12, .ibus_mode = IBUS_VEHICLE_MODE_RTL},
182 : {.ap_mode = 15, .ibus_mode = IBUS_VEHICLE_MODE_GUIDED},
183 : };
184 :
185 : #elif APM_BUILD_COPTER_OR_HELI
186 :
187 : /* Copter modes:
188 : STABILIZE = 0
189 : ACRO = 1
190 : ALT_HOLD = 2
191 : AUTO = 3
192 : GUIDED = 4
193 : LOITER = 5
194 : RTL = 6
195 : CIRCLE = 7
196 : LAND = 9
197 : DRIFT = 11
198 : SPORT = 13
199 : FLIP = 14
200 : AUTOTUNE = 15
201 : POSHOLD = 16
202 : BRAKE = 17
203 : THROW = 18
204 : AVOID_ADSB = 19
205 : GUIDED_NOGPS = 20
206 : SMART_RTL = 21
207 : FLOWHOLD = 22
208 : FOLLOW = 23
209 : ZIGZAG = 24
210 : SYSTEMID = 25
211 : AUTOROTATE = 26
212 : AUTO_RTL = 27
213 : TURTLE = 28
214 : */
215 : static const AP_IBus_Telem::ModeMap mode_map[] {
216 : {.ap_mode = 0, .ibus_mode = IBUS_VEHICLE_MODE_STAB},
217 : {.ap_mode = 1, .ibus_mode = IBUS_VEHICLE_MODE_ACRO},
218 : {.ap_mode = 2, .ibus_mode = IBUS_VEHICLE_MODE_AHOLD},
219 : {.ap_mode = 3, .ibus_mode = IBUS_VEHICLE_MODE_AUTO},
220 : {.ap_mode = 4, .ibus_mode = IBUS_VEHICLE_MODE_GUIDED},
221 : {.ap_mode = 5, .ibus_mode = IBUS_VEHICLE_MODE_LOITER},
222 : {.ap_mode = 6, .ibus_mode = IBUS_VEHICLE_MODE_RTL},
223 : {.ap_mode = 7, .ibus_mode = IBUS_VEHICLE_MODE_CIRCLE},
224 : {.ap_mode = 9, .ibus_mode = IBUS_VEHICLE_MODE_LAND},
225 : {.ap_mode = 16, .ibus_mode = IBUS_VEHICLE_MODE_PHOLD},
226 : {.ap_mode = 21, .ibus_mode = IBUS_VEHICLE_MODE_RTL},
227 : {.ap_mode = 22, .ibus_mode = IBUS_VEHICLE_MODE_PHOLD},
228 : {.ap_mode = 27, .ibus_mode = IBUS_VEHICLE_MODE_RTL},
229 : };
230 :
231 : #elif APM_BUILD_TYPE(APM_BUILD_ArduPlane)
232 :
233 : /* Plane modes:
234 : MANUAL = 0
235 : CIRCLE = 1
236 : STABILIZE = 2
237 : TRAINING = 3
238 : ACRO = 4
239 : FLY_BY_WIRE_A = 5
240 : FLY_BY_WIRE_B = 6
241 : CRUISE = 7
242 : AUTOTUNE = 8
243 : AUTO = 10
244 : RTL = 11
245 : LOITER = 12
246 : TAKEOFF = 13
247 : AVOID_ADSB = 14
248 : GUIDED = 15
249 : INITIALISING = 16
250 : QSTABILIZE = 17
251 : QHOVER = 18
252 : QLOITER = 19
253 : QLAND = 20
254 : QRTL = 21
255 : QAUTOTUNE = 22
256 : QACRO = 23
257 : THERMAL = 24
258 : LOITER_ALT_QLAND = 25
259 : */
260 : static const AP_IBus_Telem::ModeMap mode_map[] {
261 : {.ap_mode = 1, .ibus_mode = IBUS_VEHICLE_MODE_CIRCLE},
262 : {.ap_mode = 2, .ibus_mode = IBUS_VEHICLE_MODE_STAB},
263 : {.ap_mode = 4, .ibus_mode = IBUS_VEHICLE_MODE_ACRO},
264 : {.ap_mode = 5, .ibus_mode = IBUS_VEHICLE_MODE_STAB},
265 : {.ap_mode = 6, .ibus_mode = IBUS_VEHICLE_MODE_STAB},
266 : {.ap_mode = 7, .ibus_mode = IBUS_VEHICLE_MODE_STAB},
267 : {.ap_mode = 10, .ibus_mode = IBUS_VEHICLE_MODE_AUTO},
268 : {.ap_mode = 11, .ibus_mode = IBUS_VEHICLE_MODE_RTL},
269 : {.ap_mode = 12, .ibus_mode = IBUS_VEHICLE_MODE_LOITER},
270 : {.ap_mode = 13, .ibus_mode = IBUS_VEHICLE_MODE_AUTO},
271 : {.ap_mode = 15, .ibus_mode = IBUS_VEHICLE_MODE_GUIDED},
272 : {.ap_mode = 17, .ibus_mode = IBUS_VEHICLE_MODE_STAB},
273 : {.ap_mode = 18, .ibus_mode = IBUS_VEHICLE_MODE_PHOLD},
274 : {.ap_mode = 19, .ibus_mode = IBUS_VEHICLE_MODE_LOITER},
275 : {.ap_mode = 20, .ibus_mode = IBUS_VEHICLE_MODE_LAND},
276 : {.ap_mode = 21, .ibus_mode = IBUS_VEHICLE_MODE_RTL},
277 : {.ap_mode = 23, .ibus_mode = IBUS_VEHICLE_MODE_ACRO},
278 : {.ap_mode = 25, .ibus_mode = IBUS_VEHICLE_MODE_LOITER},
279 : };
280 :
281 : #elif APM_BUILD_TYPE(APM_BUILD_ArduSub)
282 :
283 : /* Submarine modes:
284 : STABILIZE = 0
285 : ACRO = 1
286 : ALT_HOLD = 2
287 : AUTO = 3
288 : GUIDED = 4
289 : CIRCLE = 7
290 : SURFACE = 9
291 : POSHOLD = 16
292 : MANUAL = 19
293 : MOTOR_DETECT = 20
294 : */
295 : static const AP_IBus_Telem::ModeMap mode_map[] {
296 : {.ap_mode = 0, .ibus_mode = IBUS_VEHICLE_MODE_STAB},
297 : {.ap_mode = 1, .ibus_mode = IBUS_VEHICLE_MODE_ACRO},
298 : {.ap_mode = 2, .ibus_mode = IBUS_VEHICLE_MODE_AHOLD},
299 : {.ap_mode = 3, .ibus_mode = IBUS_VEHICLE_MODE_AUTO},
300 : {.ap_mode = 4, .ibus_mode = IBUS_VEHICLE_MODE_GUIDED},
301 : {.ap_mode = 5, .ibus_mode = IBUS_VEHICLE_MODE_LOITER},
302 : {.ap_mode = 6, .ibus_mode = IBUS_VEHICLE_MODE_RTL},
303 : {.ap_mode = 7, .ibus_mode = IBUS_VEHICLE_MODE_CIRCLE},
304 : {.ap_mode = 8, .ibus_mode = IBUS_VEHICLE_MODE_PHOLD},
305 : {.ap_mode = 9, .ibus_mode = IBUS_VEHICLE_MODE_LAND},
306 : {.ap_mode = 16, .ibus_mode = IBUS_VEHICLE_MODE_PHOLD},
307 : };
308 :
309 : #elif APM_BUILD_TYPE(APM_BUILD_Blimp)
310 :
311 : /* Blimp modes:
312 : LAND = 0
313 : MANUAL = 1
314 : VELOCITY = 2
315 : LOITER = 3
316 : RTL = 4
317 : */
318 : static const AP_IBus_Telem::ModeMap mode_map[] {
319 : {.ap_mode = 0, .ibus_mode = IBUS_VEHICLE_MODE_LAND},
320 : {.ap_mode = 3, .ibus_mode = IBUS_VEHICLE_MODE_LOITER},
321 : {.ap_mode = 4, .ibus_mode = IBUS_VEHICLE_MODE_RTL},
322 : };
323 :
324 : #else
325 :
326 : static const AP_IBus_Telem::ModeMap mode_map[] {};
327 :
328 : #endif
329 :
330 2086 : void AP_IBus_Telem::init()
331 : {
332 2086 : const AP_SerialManager &serial_manager = AP::serialmanager();
333 2086 : if ((port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_IBUS_Telem, 0))) {
334 1 : port->set_options(port->OPTION_HDPLEX);
335 1 : hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_IBus_Telem::tick, void));
336 : }
337 2086 : }
338 :
339 7916 : void AP_IBus_Telem::tick(void)
340 : {
341 7916 : if (!initialized) {
342 1 : port->begin(115200);
343 1 : initialized = true;
344 : }
345 :
346 7916 : const uint16_t available = MIN(port->available(), 1024U);
347 7916 : if (available == 0) {
348 7907 : return;
349 : }
350 :
351 9 : const uint32_t now = AP_HAL::millis();
352 9 : if ((now - last_received_message_time_ms) < PROTOCOL_TIMEGAP) {
353 : // Any bytes that arrive too soon since the last message should be discarded
354 0 : port->discard_input();
355 0 : return;
356 : }
357 :
358 : union {
359 : CommandPacket incoming_message;
360 : uint8_t buffer[sizeof(CommandPacket)];
361 : } u;
362 :
363 9 : if (available < sizeof(u.incoming_message)) {
364 : // We've got here mid-message; come back in the next loop when it might be complete
365 0 : return;
366 : }
367 :
368 9 : last_received_message_time_ms = now;
369 :
370 9 : if (available > sizeof(u.incoming_message)) {
371 : // We've received too many bytes, so probably a malformed message
372 0 : port->discard_input();
373 0 : return;
374 : }
375 :
376 9 : if (port->read(u.buffer, sizeof(u.buffer)) != sizeof(u.buffer)) {
377 : // Despite all our checks this is still the wrong size; safest to wait until the next message
378 0 : port->discard_input();
379 0 : return;
380 : }
381 :
382 9 : if (u.incoming_message.message_length != PROTOCOL_INCOMING_MESSAGE_LENGTH) {
383 : // Every message starts with this fixed message length; if not then it's not valid
384 0 : return;
385 : }
386 :
387 9 : uint16_t calculated_checksum = 0xFFFF;
388 27 : for (uint8_t i = 0; i < sizeof(u.buffer) - 2; i++) {
389 18 : calculated_checksum -= u.buffer[i];
390 : }
391 :
392 9 : if (u.incoming_message.checksum != calculated_checksum) {
393 0 : return;
394 : }
395 :
396 9 : handle_incoming_message(u.incoming_message);
397 : }
398 :
399 9 : void AP_IBus_Telem::handle_incoming_message(const CommandPacket &incoming)
400 : {
401 : // Sensor 0 is reserved for the transmitter, so if for some reason it's requested we should ignore it
402 9 : if (incoming.sensor_id == 0) {
403 0 : return;
404 : }
405 :
406 : // If we've reached the end of our sensor list, we shouldn't respond; this tells the receiver that there
407 : // are no more sensors to discover.
408 : if (incoming.sensor_id > ARRAY_SIZE(sensors)) {
409 : return;
410 : }
411 :
412 9 : const auto &sensor_definition = sensors[incoming.sensor_id - 1];
413 :
414 9 : switch (incoming.command << 4) {
415 3 : case PROTOCOL_COMMAND_DISCOVER:
416 3 : handle_discover_command(incoming);
417 3 : break;
418 :
419 3 : case PROTOCOL_COMMAND_TYPE:
420 3 : handle_type_command(incoming, sensor_definition);
421 3 : break;
422 :
423 3 : case PROTOCOL_COMMAND_VALUE:
424 3 : handle_value_command(incoming, sensor_definition);
425 3 : break;
426 : }
427 : }
428 :
429 : /* A discovery query has the following format:
430 : * 0x04: Message length
431 : * 0x81: 0x80 for discovery + 0x01 for sensor ID 1
432 : * 0x7A: Checksum low byte
433 : * 0xFF: Checksum high byte
434 : Checksums are 0xFFFF minus the sum of the previous bytes
435 :
436 : To acknowledge a discovery query, we echo the command back.
437 : */
438 3 : void AP_IBus_Telem::handle_discover_command(const CommandPacket &incoming)
439 : {
440 : struct protocol_command_discover_response_t {
441 : uint8_t command_length;
442 : uint8_t address;
443 : uint16_t checksum;
444 3 : } packet {
445 : PROTOCOL_FOUR_LENGTH,
446 3 : (uint8_t)(PROTOCOL_COMMAND_DISCOVER | incoming.sensor_id)
447 3 : };
448 3 : populate_checksum((uint8_t*)&packet, sizeof(packet));
449 3 : port->write((uint8_t*)&packet, sizeof(packet));
450 3 : }
451 :
452 : /* A type query has the following format:
453 : * 0x04: Message length
454 : * 0x91: 0x90 for type + 0x01 for sensor ID 1
455 : * 0x6A: Checksum low byte
456 : * 0xFF: Checksum high byte
457 : Checksums are 0xFFFF minus the sum of the previous bytes
458 :
459 : To respond to a type query, we send:
460 : * 0x06: Message length
461 : * 0x91: 0x90 for type + 0x01 for sensor ID 1
462 : * 0x03: Sensor type, eg 0x03 for external voltage
463 : * 0x02: Sensor length (2 or 4 bytes)
464 : * 0x63: Checksum low byte
465 : * 0xFF: Checksum high byte
466 : Checksums are 0xFFFF minus the sum of the previous bytes
467 : */
468 3 : void AP_IBus_Telem::handle_type_command(const CommandPacket &incoming, const SensorDefinition &sensor)
469 : {
470 : struct protocol_command_type_response_t {
471 : uint8_t command_length;
472 : uint8_t address;
473 : uint8_t type;
474 : uint8_t length;
475 : uint16_t checksum;
476 3 : } packet {
477 : PROTOCOL_SIX_LENGTH,
478 3 : (uint8_t)(PROTOCOL_COMMAND_TYPE | incoming.sensor_id),
479 3 : sensor.sensor_type,
480 3 : sensor.sensor_length
481 3 : };
482 3 : populate_checksum((uint8_t*)&packet, sizeof(packet));
483 3 : port->write((uint8_t*)&packet, sizeof(packet));
484 3 : }
485 :
486 : /* A value query has the following format:
487 : * 0x04: Message length
488 : * 0xA1: 0xA0 for value + 0x01 for sensor ID 1
489 : * 0x5A: Checksum low byte
490 : * 0xFF: Checksum high byte
491 : Checksums are 0xFFFF minus the sum of the previous bytes
492 :
493 : To respond to a value query, we send:
494 : * 0x06: Message length (or 0x08 for a 4-byte sensor)
495 : * 0xA1: 0xA1 for value + 0x01 for sensor ID 1
496 : * 0xD4: Value byte (value is 12,500 in this example)
497 : * 0x30: Value byte
498 : * 0x54: Checksum low byte
499 : * 0xFE: Checksum high byte
500 : Checksums are 0xFFFF minus the sum of the previous bytes
501 : */
502 3 : void AP_IBus_Telem::handle_value_command(const CommandPacket &incoming, const SensorDefinition &sensor)
503 : {
504 3 : const SensorValue value = get_sensor_value(sensor.sensor_type);
505 3 : if (sensor.sensor_length == 2) {
506 3 : handle_2_byte_value_command(incoming, value);
507 : } else {
508 0 : handle_4_byte_value_command(incoming, value);
509 : }
510 3 : }
511 :
512 3 : void AP_IBus_Telem::handle_2_byte_value_command(const CommandPacket &incoming, const SensorValue &value)
513 : {
514 : struct protocol_command_2byte_value_response_t {
515 : uint8_t command_length;
516 : uint8_t address;
517 : uint8_t value[2];
518 : uint16_t checksum;
519 3 : } packet {
520 : PROTOCOL_SIX_LENGTH,
521 3 : (uint8_t)(PROTOCOL_COMMAND_VALUE | incoming.sensor_id),
522 3 : {value.byte[0], value.byte[1]}
523 3 : };
524 3 : populate_checksum((uint8_t*)&packet, sizeof(packet));
525 3 : port->write((uint8_t*)&packet, sizeof(packet));
526 3 : }
527 :
528 0 : void AP_IBus_Telem::handle_4_byte_value_command(const CommandPacket &incoming, const SensorValue &value)
529 : {
530 : struct protocol_command_4byte_value_response_t {
531 : uint8_t command_length;
532 : uint8_t address;
533 : uint8_t value[4];
534 : uint16_t checksum;
535 0 : } packet {
536 : PROTOCOL_EIGHT_LENGTH,
537 0 : (uint8_t)(PROTOCOL_COMMAND_VALUE | incoming.sensor_id),
538 0 : {value.byte[0], value.byte[1], value.byte[2], value.byte[3]}
539 0 : };
540 0 : populate_checksum((uint8_t*)&packet, sizeof(packet));
541 0 : port->write((uint8_t*)&packet, sizeof(packet));
542 0 : }
543 :
544 3 : AP_IBus_Telem::SensorValue AP_IBus_Telem::get_sensor_value(const uint8_t sensor_type)
545 : {
546 : SensorValue value;
547 3 : switch (sensor_type) {
548 : #if AP_BATTERY_ENABLED
549 1 : case IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE:
550 1 : value.uint16 = AP::battery().voltage() * 100;
551 1 : break;
552 :
553 0 : case IBUS_SENSOR_TYPE_AVERAGE_CELL_VOLTAGE:
554 0 : value.uint16 = get_average_cell_voltage_cV();
555 0 : break;
556 :
557 0 : case IBUS_SENSOR_TYPE_BATTERY_CURRENT:
558 0 : value.uint16 = get_current_cAh();
559 0 : break;
560 :
561 0 : case IBUS_SENSOR_TYPE_FUEL:
562 0 : value.uint16 = get_battery_or_fuel_level_pct();
563 0 : break;
564 : #endif // AP_BATTERY_ENABLED
565 :
566 : #if AP_RPM_ENABLED
567 0 : case IBUS_SENSOR_TYPE_RPM:
568 : case IBUS_SENSOR_TYPE_RPM_FLYSKY:
569 0 : value.uint16 = get_rpm();
570 0 : break;
571 : #endif
572 :
573 : #if AP_AHRS_ENABLED
574 0 : case IBUS_SENSOR_TYPE_COMPASS_HEADING:
575 0 : value.uint16 = uint16_t(AP::ahrs().get_yaw_deg());
576 0 : break;
577 : #endif
578 :
579 : #if AP_BARO_ENABLED
580 0 : case IBUS_SENSOR_TYPE_CLIMB_RATE:
581 : case IBUS_SENSOR_TYPE_VERTICAL_SPEED:
582 0 : value.int16 = AP::baro().get_climb_rate() * 100;
583 0 : break;
584 : #endif
585 :
586 : #if AP_GPS_ENABLED
587 0 : case IBUS_SENSOR_TYPE_COG:
588 0 : value.uint16 = AP::gps().ground_course() * 100;
589 0 : break;
590 :
591 0 : case IBUS_SENSOR_TYPE_GPS_STATUS:
592 0 : value.byte[0] = get_gps_status();
593 0 : value.byte[1] = AP::gps().num_sats();
594 0 : break;
595 : #endif // AP_GPS_ENABLED
596 :
597 : #if AP_AHRS_ENABLED
598 0 : case IBUS_SENSOR_TYPE_ACC_X:
599 0 : value.int16 = (AP::ahrs().get_accel() - AP::ahrs().get_accel_bias()).x * 1000;
600 0 : break;
601 :
602 0 : case IBUS_SENSOR_TYPE_ACC_Y:
603 0 : value.int16 = (AP::ahrs().get_accel() - AP::ahrs().get_accel_bias()).y * 1000;
604 0 : break;
605 :
606 0 : case IBUS_SENSOR_TYPE_ACC_Z:
607 0 : value.int16 = (AP::ahrs().get_accel() - AP::ahrs().get_accel_bias()).z * 1000;
608 0 : break;
609 :
610 0 : case IBUS_SENSOR_TYPE_ROLL:
611 0 : value.int16 = AP::ahrs().roll_sensor;
612 0 : break;
613 :
614 0 : case IBUS_SENSOR_TYPE_PITCH:
615 0 : value.int16 = AP::ahrs().pitch_sensor;
616 0 : break;
617 :
618 0 : case IBUS_SENSOR_TYPE_YAW:
619 0 : value.int16 = AP::ahrs().yaw_sensor;
620 0 : break;
621 : #endif // AP_AHRS_ENABLED
622 :
623 : #if AP_GPS_ENABLED
624 0 : case IBUS_SENSOR_TYPE_GROUND_SPEED:
625 0 : value.uint16 = AP::gps().ground_speed()*100;
626 0 : break;
627 : #endif
628 :
629 : #if AP_AHRS_ENABLED
630 0 : case IBUS_SENSOR_TYPE_GPS_DIST:
631 0 : value.uint16 = get_distance_from_home_m();
632 0 : break;
633 : #endif
634 :
635 : #if AP_ARMING_ENABLED
636 1 : case IBUS_SENSOR_TYPE_ARMED:
637 1 : value.uint16 = AP::arming().is_armed();
638 1 : break;
639 : #endif
640 :
641 1 : case IBUS_SENSOR_TYPE_FLIGHT_MODE:
642 1 : value.uint16 = get_vehicle_mode();
643 1 : break;
644 :
645 : #if AP_AIRSPEED_ENABLED
646 0 : case IBUS_SENSOR_TYPE_SPEED:
647 0 : value.uint16 = AP::airspeed()->get_airspeed();
648 0 : break;
649 : #endif
650 :
651 : #if AP_BARO_ENABLED
652 0 : case IBUS_SENSOR_TYPE_TEMPERATURE_PRESSURE: {
653 0 : const uint32_t pressure = AP::baro().get_pressure();
654 0 : const uint32_t temperature = (AP::baro().get_temperature() + 40) * 10;
655 0 : value.uint32 = pressure | (temperature << 19);
656 0 : break;
657 : }
658 : #endif
659 :
660 : #if AP_GPS_ENABLED
661 0 : case IBUS_SENSOR_TYPE_GPS_LAT:
662 0 : value.int32 = AP::gps().location().lat;
663 0 : break;
664 :
665 0 : case IBUS_SENSOR_TYPE_GPS_LNG:
666 0 : value.int32 = AP::gps().location().lng;
667 0 : break;
668 :
669 0 : case IBUS_SENSOR_TYPE_GPS_ALT:
670 0 : value.int32 = AP::gps().location().alt;
671 0 : break;
672 : #endif // AP_GPS_ENABLED
673 :
674 : #if AP_BARO_ENABLED
675 0 : case IBUS_SENSOR_TYPE_ALT:
676 0 : value.int32 = AP::baro().get_altitude() * 100;
677 0 : break;
678 : #endif
679 :
680 0 : default:
681 0 : value.int32 = 0;
682 : }
683 :
684 3 : return value;
685 : }
686 :
687 : #if AP_BATTERY_ENABLED
688 0 : uint16_t AP_IBus_Telem::get_average_cell_voltage_cV()
689 : {
690 0 : if (!AP::battery().has_cell_voltages()) {
691 0 : return 0;
692 : }
693 :
694 0 : const auto &cell_voltages = AP::battery().get_cell_voltages();
695 0 : const uint8_t number_of_cells = ARRAY_SIZE(cell_voltages.cells);
696 : if (number_of_cells == 0) {
697 : return 0;
698 : }
699 :
700 0 : uint32_t voltage_sum = 0;
701 0 : for (auto i = 0; i < number_of_cells; i++) {
702 0 : voltage_sum += cell_voltages.cells[i] * 0.001;
703 : }
704 0 : return voltage_sum / number_of_cells * 100;
705 :
706 : }
707 :
708 0 : uint16_t AP_IBus_Telem::get_current_cAh()
709 : {
710 0 : float current = 0;
711 0 : IGNORE_RETURN(AP::battery().current_amps(current));
712 0 : return current * 100;
713 : }
714 :
715 0 : uint8_t AP_IBus_Telem::get_battery_or_fuel_level_pct()
716 : {
717 0 : uint8_t percentage = 0;
718 0 : IGNORE_RETURN(AP::battery().capacity_remaining_pct(percentage));
719 0 : return percentage;
720 : }
721 : #endif // AP_BATTERY_ENABLED
722 :
723 : #if AP_RPM_ENABLED
724 0 : uint16_t AP_IBus_Telem::get_rpm()
725 : {
726 0 : const AP_RPM *rpm = AP::rpm();
727 : float rpm_value;
728 0 : if (rpm && rpm->get_rpm(0, rpm_value)) {
729 0 : return rpm_value;
730 : }
731 :
732 0 : return 0;
733 : }
734 : #endif
735 :
736 : #if AP_GPS_ENABLED
737 0 : uint8_t AP_IBus_Telem::get_gps_status()
738 : {
739 0 : if (!AP::gps().is_healthy()) {
740 0 : return 0;
741 : }
742 :
743 0 : const AP_GPS_FixType gps_status = AP::gps().status();
744 0 : if (gps_status >= AP_GPS_FixType::FIX_3D) {
745 0 : return 3;
746 0 : } else if (gps_status >= AP_GPS_FixType::FIX_2D) {
747 0 : return 2;
748 0 : } else if (gps_status == AP_GPS_FixType::NONE) {
749 0 : return 1;
750 : } else {
751 0 : return 0;
752 : }
753 : }
754 : #endif // AP_GPS_ENABLED
755 :
756 : #if AP_AHRS_ENABLED
757 0 : uint16_t AP_IBus_Telem::get_distance_from_home_m()
758 : {
759 0 : Vector2f home;
760 0 : if (AP::ahrs().get_relative_position_NE_home(home)) {
761 0 : return home.length();
762 : }
763 :
764 0 : return 0;
765 : }
766 : #endif
767 :
768 1 : uint16_t AP_IBus_Telem::get_vehicle_mode()
769 : {
770 1 : const uint8_t vehicle_mode = AP::vehicle()->get_mode();
771 1 : for (uint8_t i = 0; i < ARRAY_SIZE(mode_map); i++) {
772 1 : if (mode_map[i].ap_mode == vehicle_mode) {
773 1 : return mode_map[i].ibus_mode;
774 : }
775 : }
776 0 : return IBUS_VEHICLE_MODE_UNKNOWN;
777 : }
778 :
779 : // Populate the last two bytes of packet with the checksum of the preceding bytes
780 9 : void AP_IBus_Telem::populate_checksum(uint8_t *packet, const uint16_t size)
781 : {
782 9 : uint16_t checksum = 0xFFFF;
783 :
784 39 : for (int i=0; i<size-2; i++) {
785 30 : checksum -= packet[i];
786 : }
787 :
788 9 : packet[size-2] = checksum & 0x0ff;
789 9 : packet[size-1] = checksum >> 8;
790 9 : }
791 :
792 : #endif
|
