├── .gitattributes ├── README.md ├── examples └── iBUSTelemetry_example │ └── iBUSTelemetry_example.ino ├── iBUSSensors.h ├── iBUSTelemetry.cpp ├── iBUSTelemetry.h └── keywords.txt /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # iBUSTelemetry-Arduino 2 | This library allows you to emulate FlySky's iBus telemetry sensors in the way you want (Voltage, RPM, GPS, Temperature and many others) using Arduino. 3 | It's based on interrupts, so main thread is not blocked. You can easily get values from your Arduino modules and send it to your transmitter screen! 4 | Fully supports 2 and 4 byte sensors, so you can use every one available in qba667 FlyPlus firmware. 5 | It's very simple to use. Included example is self-explaining. 6 | Enjoy! -------------------------------------------------------------------------------- /examples/iBUSTelemetry_example/iBUSTelemetry_example.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define UPDATE_INTERVAL 500 4 | 5 | iBUSTelemetry telemetry(11); // I use only PCINT0 interrupt vector, so you can use D8 to D13 pins. 6 | 7 | uint32_t prevMillis = 0; // Necessary to updateValues() method. Look below. 8 | float i = 0; 9 | 10 | void setup() 11 | { 12 | telemetry.begin(); // Let's start having fun! 13 | 14 | telemetry.addSensor(0x01); // You can use sensors definitions from iBUSSensors.h instead of numbers. 15 | // Ex.: telemetry.addSensor(IBUS_MEAS_TYPE_TEM); 16 | 17 | telemetry.addSensor(IBUS_MEAS_TYPE_GPS_STATUS); 18 | telemetry.addSensor(IBUS_MEAS_TYPE_SPE); 19 | telemetry.addSensor(IBUS_MEAS_TYPE_GPS_LAT); 20 | telemetry.addSensor(IBUS_MEAS_TYPE_ARMED); 21 | telemetry.addSensor(IBUS_MEAS_TYPE_FLIGHT_MODE); 22 | telemetry.addSensor(IBUS_MEAS_TYPE_ALT); 23 | } 24 | 25 | void loop() 26 | { 27 | updateValues(); // Very important! iBUS protocol is very sensitive to timings. 28 | // DO NOT USE ANY delay()! Look at updateValues() method. 29 | // It's an example of how to use intervals without delays. 30 | 31 | telemetry.run(); //It must be here. Period. 32 | } 33 | 34 | void updateValues() 35 | { 36 | uint32_t currMillis = millis(); 37 | 38 | if (currMillis - prevMillis >= UPDATE_INTERVAL) { // Code in the middle of these brackets will be performed every 500ms. 39 | prevMillis = currMillis; 40 | 41 | telemetry.setSensorValueFP(1, i); // Now, you have two ways to set sensors values. Using floating point variables 42 | // or directly in 32bit integer, but then you have to format variable by yourself. 43 | // Ex.: telemetry.setSensorValueFP(1, 24.45); is equivalent to telemetry.setSensorValue(1, 2445); 44 | // The values differ for different sensors. 45 | 46 | telemetry.setSensorValue(2, telemetry.gpsStateValues(3, 8)); // As GPS status consists of two values, 47 | // use gpsStateValues(firstVal, secondVal) to set it properly. 48 | 49 | telemetry.setSensorValue(3, 123 * 10); 50 | 51 | telemetry.setSensorValue(4, 179583647); // You can set LAT / LON using FP metohod, but due to fact floats have only 6 digits precision, 52 | // your values on tx may be corrupted in some cases. It's better to use setSensorValue(). 53 | 54 | telemetry.setSensorValue(5, UNARMED); // ARMED / UNARMED or 1 / 0 could be used. 55 | 56 | telemetry.setSensorValue(6, LOITER); // Available flight modes: STAB 0 57 | // ACRO 1 58 | // AHOLD 2 59 | // AUTO 3 60 | // GUIDED 4 61 | // LOITER 5 62 | // RTL 6 63 | // CIRCLE 7 64 | // PHOLD 8 65 | // LAND 9 66 | 67 | telemetry.setSensorValueFP(7, 54.87); 68 | 69 | i += 0.1; 70 | if (i > 50) 71 | i = 0; 72 | 73 | // These were the most difficult sensors to use. I hope that this library will be useful for you and will make your work easier. :) 74 | } 75 | } /* updateValues */ -------------------------------------------------------------------------------- /iBUSSensors.h: -------------------------------------------------------------------------------- 1 | /* 2 | * iBUSTelemetry.cpp - adis1313 3 | * 4 | * Libraries or parts of the code used in this project: 5 | * 6 | * SoftwareSerialWithHalfDuplex 7 | * https://github.com/nickstedman/SoftwareSerialWithHalfDuplex 8 | * 9 | * iBUStelemetry 10 | * https://github.com/Hrastovc/iBUStelemetry 11 | * 12 | * FlySkyI6 13 | * https://github.com/qba667/FlySkyI6/blob/master/source/source/ibustelemetry.h 14 | * 15 | * Big thanks to the authors! 16 | */ 17 | 18 | #ifndef iBUSSensors_h 19 | #define iBUSSensors_h 20 | 21 | #define MAX_SENS_COUNT 15 22 | 23 | #define STAB 0 24 | #define ACRO 1 25 | #define AHOLD 2 26 | #define AUTO 3 27 | #define GUIDED 4 28 | #define LOITER 5 29 | #define RTL 6 30 | #define CIRCLE 7 31 | #define PHOLD 8 32 | #define LAND 9 33 | 34 | #define UNARMED 0 35 | #define ARMED 1 36 | 37 | // 2 byte sensors 38 | 39 | #define IBUS_MEAS_TYPE_TEM 0x01 // Temperature 40 | #define IBUS_MEAS_TYPE_EXTV 0x03 // External voltage 41 | #define IBUS_MEAS_TYPE_CELL 0x04 // Avg cell voltage 42 | #define IBUS_MEAS_TYPE_BAT_CURR 0x05 // Battery current 43 | #define IBUS_MEAS_TYPE_FUEL 0x06 // Remaining battery percentage 44 | #define IBUS_MEAS_TYPE_RPM 0x07 // Throttle value / battery capacity 45 | #define IBUS_MEAS_TYPE_CMP_HEAD 0x08 // Heading 46 | #define IBUS_MEAS_TYPE_CLIMB_RATE 0x09 // Climb rate 47 | #define IBUS_MEAS_TYPE_COG 0x0a // Course over ground 48 | #define IBUS_MEAS_TYPE_GPS_STATUS 0x0b // GPS status (2 values) 49 | #define IBUS_MEAS_TYPE_ACC_X 0x0c // Acc X 50 | #define IBUS_MEAS_TYPE_ACC_Y 0x0d // Acc Y 51 | #define IBUS_MEAS_TYPE_ACC_Z 0x0e // Acc Z 52 | #define IBUS_MEAS_TYPE_ROLL 0x0f // Roll 53 | #define IBUS_MEAS_TYPE_PITCH 0x10 // Pitch 54 | #define IBUS_MEAS_TYPE_YAW 0x11 // Yaw 55 | #define IBUS_MEAS_TYPE_VERTICAL_SPEED 0x12 // Vertical speed 56 | #define IBUS_MEAS_TYPE_GROUND_SPEED 0x13 // Speed m/s 57 | #define IBUS_MEAS_TYPE_GPS_DIST 0x14 // Distance from home 58 | #define IBUS_MEAS_TYPE_ARMED 0x15 // Armed / unarmed 59 | #define IBUS_MEAS_TYPE_FLIGHT_MODE 0x16 // Flight mode 60 | 61 | #define IBUS_MEAS_TYPE_PRES 0x41 // Pressure 62 | #define IBUS_MEAS_TYPE_SPE 0x7e // Speed km/h 63 | 64 | // 4 byte sensors 65 | 66 | #define IBUS_MEAS_TYPE_GPS_LAT 0x80 // WGS84 in degrees * 1E7 67 | #define IBUS_MEAS_TYPE_GPS_LON 0x81 // WGS84 in degrees * 1E7 68 | #define IBUS_MEAS_TYPE_GPS_ALT 0x82 // GPS altitude 69 | #define IBUS_MEAS_TYPE_ALT 0x83 // Altitude 70 | #define IBUS_MEAS_TYPE_ALT_MAX 0x84 // Max altitude 71 | #define IBUS_MEAS_TYPE_S85 0x85 72 | #define IBUS_MEAS_TYPE_S86 0x86 73 | #define IBUS_MEAS_TYPE_S87 0x87 74 | #define IBUS_MEAS_TYPE_S88 0x88 75 | #define IBUS_MEAS_TYPE_S89 0x89 76 | #define IBUS_MEAS_TYPE_S8a 0x8a 77 | 78 | #endif // ifndef iBUSSensors_h -------------------------------------------------------------------------------- /iBUSTelemetry.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * iBUSTelemetry.cpp - adis1313 3 | * 4 | * Libraries or parts of the code used in this project: 5 | * 6 | * SoftwareSerialWithHalfDuplex 7 | * https://github.com/nickstedman/SoftwareSerialWithHalfDuplex 8 | * 9 | * iBUStelemetry 10 | * https://github.com/Hrastovc/iBUStelemetry 11 | * 12 | * FlySkyI6 13 | * https://github.com/qba667/FlySkyI6/blob/master/source/source/ibustelemetry.h 14 | * 15 | * Big thanks to the authors! 16 | */ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | iBUSTelemetry * iBUSTelemetry::active_object = 0; 25 | char iBUSTelemetry::_receive_buffer[_SS_MAX_RX_BUFF]; 26 | volatile uint8_t iBUSTelemetry::_receive_buffer_tail = 0; 27 | volatile uint8_t iBUSTelemetry::_receive_buffer_head = 0; 28 | 29 | inline void 30 | iBUSTelemetry::tunedDelay(uint16_t delay) 31 | { 32 | _delay_loop_2(delay); 33 | } 34 | 35 | bool 36 | iBUSTelemetry::listen() 37 | { 38 | if (!_rx_delay_stopbit) 39 | return false; 40 | 41 | if (active_object != this) { 42 | if (active_object) 43 | active_object->stopListening(); 44 | 45 | _buffer_overflow = false; 46 | _receive_buffer_head = _receive_buffer_tail = 0; 47 | active_object = this; 48 | setRxIntMsk(true); 49 | return true; 50 | } 51 | return false; 52 | } 53 | 54 | bool 55 | iBUSTelemetry::stopListening() 56 | { 57 | if (active_object == this) { 58 | setRxIntMsk(false); 59 | active_object = NULL; 60 | return true; 61 | } 62 | return false; 63 | } 64 | 65 | void 66 | iBUSTelemetry::recv() 67 | { 68 | uint8_t d = 0; 69 | 70 | if (!rx_pin_read()) { 71 | setRxIntMsk(false); 72 | tunedDelay(_rx_delay_centering); 73 | 74 | for (uint8_t i = 8; i > 0; --i) { 75 | tunedDelay(_rx_delay_intrabit); 76 | d >>= 1; 77 | if (rx_pin_read()) 78 | d |= 0x80; 79 | } 80 | 81 | uint8_t next = (_receive_buffer_tail + 1) % _SS_MAX_RX_BUFF; 82 | if (next != _receive_buffer_head) { 83 | _receive_buffer[_receive_buffer_tail] = d; 84 | _receive_buffer_tail = next; 85 | } else { 86 | _buffer_overflow = true; 87 | } 88 | 89 | tunedDelay(_rx_delay_stopbit); 90 | setRxIntMsk(true); 91 | } 92 | } 93 | 94 | uint8_t 95 | iBUSTelemetry::rx_pin_read() 96 | { 97 | return *_receivePortRegister & _pinBitMask; 98 | } 99 | 100 | inline void 101 | iBUSTelemetry::handle_interrupt() 102 | { 103 | if (active_object) { 104 | active_object->recv(); 105 | } 106 | } 107 | 108 | #if defined(PCINT0_vect) 109 | ISR(PCINT0_vect) 110 | { 111 | iBUSTelemetry::handle_interrupt(); 112 | } 113 | #endif 114 | 115 | iBUSTelemetry::iBUSTelemetry(uint8_t pin) : 116 | _rx_delay_centering(0), 117 | _rx_delay_intrabit(0), 118 | _rx_delay_stopbit(0), 119 | _tx_delay(0), 120 | _buffer_overflow(false) 121 | { 122 | setPin(pin); 123 | sensorCount = 0; 124 | } 125 | 126 | iBUSTelemetry::~iBUSTelemetry() 127 | { 128 | end(); 129 | } 130 | 131 | void 132 | iBUSTelemetry::setPin(uint8_t pin) 133 | { 134 | pinMode(pin, INPUT); 135 | digitalWrite(pin, HIGH); 136 | _pin = pin; 137 | _pinBitMask = digitalPinToBitMask(pin); 138 | uint8_t port = digitalPinToPort(pin); 139 | _transmitPortRegister = portOutputRegister(port); 140 | _receivePortRegister = portInputRegister(port); 141 | } 142 | 143 | uint16_t 144 | iBUSTelemetry::subtract_cap(uint16_t num, uint16_t sub) 145 | { 146 | if (num > sub) 147 | return num - sub; 148 | else 149 | return 1; 150 | } 151 | 152 | void 153 | iBUSTelemetry::begin() 154 | { 155 | _rx_delay_centering = _rx_delay_intrabit = _rx_delay_stopbit = _tx_delay = 0; 156 | 157 | uint16_t bit_delay = (F_CPU / 115200) / 4; 158 | 159 | _tx_delay = subtract_cap(bit_delay, 15 / 4); 160 | 161 | if (digitalPinToPCICR(_pin)) { 162 | _rx_delay_centering = subtract_cap(bit_delay / 2, (4 + 4 + 75 + 17 - 23) / 4); 163 | _rx_delay_intrabit = subtract_cap(bit_delay, 23 / 4); 164 | _rx_delay_stopbit = subtract_cap(bit_delay * 3 / 4, (37 + 11) / 4); 165 | 166 | *digitalPinToPCICR(_pin) |= _BV(digitalPinToPCICRbit(_pin)); 167 | _pcint_maskreg = digitalPinToPCMSK(_pin); 168 | _pcint_maskvalue = _BV(digitalPinToPCMSKbit(_pin)); 169 | 170 | tunedDelay(_tx_delay); 171 | } 172 | listen(); 173 | } 174 | 175 | void 176 | iBUSTelemetry::setRxIntMsk(bool enable) 177 | { 178 | if (enable) 179 | *_pcint_maskreg |= _pcint_maskvalue; 180 | else 181 | *_pcint_maskreg &= ~_pcint_maskvalue; 182 | } 183 | 184 | void 185 | iBUSTelemetry::end() 186 | { 187 | stopListening(); 188 | } 189 | 190 | int 191 | iBUSTelemetry::read() 192 | { 193 | if (!isListening()) 194 | return -1; 195 | 196 | if (_receive_buffer_head == _receive_buffer_tail) 197 | return -1; 198 | 199 | uint8_t d = _receive_buffer[_receive_buffer_head]; 200 | _receive_buffer_head = (_receive_buffer_head + 1) % _SS_MAX_RX_BUFF; 201 | return d; 202 | } 203 | 204 | int 205 | iBUSTelemetry::available() 206 | { 207 | if (!isListening()) 208 | return 0; 209 | 210 | return (_receive_buffer_tail + _SS_MAX_RX_BUFF - _receive_buffer_head) % _SS_MAX_RX_BUFF; 211 | } 212 | 213 | size_t 214 | iBUSTelemetry::write(uint8_t b) 215 | { 216 | if (_tx_delay == 0) { 217 | setWriteError(); 218 | return 0; 219 | } 220 | 221 | volatile uint8_t * reg = _transmitPortRegister; 222 | uint8_t reg_mask = _pinBitMask; 223 | uint8_t inv_mask = ~_pinBitMask; 224 | uint8_t oldSREG = SREG; 225 | uint16_t delay = _tx_delay; 226 | 227 | cli(); 228 | pinMode(_pin, OUTPUT); 229 | *reg &= inv_mask; 230 | tunedDelay(delay); 231 | 232 | for (uint8_t i = 8; i > 0; --i) { 233 | if (b & 1) 234 | *reg |= reg_mask; 235 | else 236 | *reg &= inv_mask; 237 | 238 | tunedDelay(delay); 239 | b >>= 1; 240 | } 241 | 242 | *reg |= reg_mask; 243 | pinMode(_pin, INPUT); 244 | *reg |= reg_mask; // ? 245 | 246 | SREG = oldSREG; 247 | tunedDelay(_tx_delay); 248 | 249 | return 1; 250 | } // iBUSTelemetry::write 251 | 252 | void 253 | iBUSTelemetry::flush() 254 | { 255 | if (!isListening()) 256 | return; 257 | 258 | uint8_t oldSREG = SREG; 259 | cli(); 260 | _receive_buffer_head = _receive_buffer_tail = 0; 261 | SREG = oldSREG; 262 | } 263 | 264 | int 265 | iBUSTelemetry::peek() 266 | { 267 | if (!isListening()) 268 | return -1; 269 | 270 | if (_receive_buffer_head == _receive_buffer_tail) 271 | return -1; 272 | 273 | return _receive_buffer[_receive_buffer_head]; 274 | } 275 | 276 | void 277 | iBUSTelemetry::addSensor(uint8_t type) 278 | { 279 | if (sensorCount < MAX_SENS_COUNT) { 280 | sensorArray[sensorCount].type = type; 281 | sensorArray[sensorCount].value = 0; 282 | sensorCount++; 283 | } 284 | } 285 | 286 | void 287 | iBUSTelemetry::setSensorValue(uint8_t adr, int32_t value) 288 | { 289 | uint8_t sensAdrArr = adr - 1; 290 | 291 | if ((adr > 0) && (adr <= sensorCount)) { 292 | switch (sensorArray[sensAdrArr].type) { 293 | // Some empty cases to prevent setting these values by default case 294 | case 0x00: 295 | // IBUS_MEAS_TYPE_INTV 296 | break; 297 | case 0x01: 298 | sensorArray[sensAdrArr].value = value + 400; 299 | break; 300 | case 0x02: 301 | // IBUS_MEAS_TYPE_MOT 302 | break; 303 | case 0x7C: 304 | // IBUS_MEAS_TYPE_ODO1 305 | break; 306 | case 0x7D: 307 | // IBUS_MEAS_TYPE_ODO2 308 | break; 309 | case 0x7F: 310 | // IBUS_MEAS_TYPE_TX_V 311 | break; 312 | default: 313 | sensorArray[sensAdrArr].value = value; 314 | } 315 | } 316 | } 317 | 318 | void 319 | iBUSTelemetry::setSensorValueFP(uint8_t adr, float value) 320 | { 321 | uint8_t sensAdrArr = adr - 1; 322 | 323 | if ((adr > 0) && (adr <= sensorCount)) { 324 | switch (sensorArray[sensAdrArr].type) { 325 | case 0x01: 326 | setSensorValue(adr, (int32_t) (value * 10)); 327 | break; 328 | case 0x03: 329 | sensorArray[sensAdrArr].value = (uint16_t) (value * 100); 330 | break; 331 | case 0x04: 332 | sensorArray[sensAdrArr].value = (uint16_t) (value * 100); 333 | break; 334 | case 0x05: 335 | sensorArray[sensAdrArr].value = (uint16_t) (value * 100); 336 | break; 337 | case 0x06: 338 | sensorArray[sensAdrArr].value = (uint16_t) value; 339 | break; 340 | case 0x07: 341 | sensorArray[sensAdrArr].value = (uint16_t) value; 342 | break; 343 | case 0x08: 344 | sensorArray[sensAdrArr].value = (uint16_t) value; 345 | break; 346 | case 0x09: 347 | sensorArray[sensAdrArr].value = (int16_t) (value * 100); 348 | break; 349 | case 0x0A: 350 | sensorArray[sensAdrArr].value = (uint16_t) value; 351 | break; 352 | case 0x0B: 353 | sensorArray[sensAdrArr].value = value; 354 | break; 355 | case 0x0C: 356 | sensorArray[sensAdrArr].value = (int16_t) (value * 100); 357 | break; 358 | case 0x0D: 359 | sensorArray[sensAdrArr].value = (int16_t) (value * 100); 360 | break; 361 | case 0x0E: 362 | sensorArray[sensAdrArr].value = (int16_t) (value * 100); 363 | break; 364 | case 0x0F: 365 | sensorArray[sensAdrArr].value = (int16_t) (value * 100); 366 | break; 367 | case 0x10: 368 | sensorArray[sensAdrArr].value = (int16_t) (value * 100); 369 | break; 370 | case 0x11: 371 | sensorArray[sensAdrArr].value = (int16_t) (value * 100); 372 | break; 373 | case 0x12: 374 | sensorArray[sensAdrArr].value = (int16_t) (value * 100); 375 | break; 376 | case 0x13: 377 | sensorArray[sensAdrArr].value = (uint16_t) (value * 100); 378 | break; 379 | case 0x14: 380 | sensorArray[sensAdrArr].value = (uint16_t) value; 381 | break; 382 | case 0x15: 383 | sensorArray[sensAdrArr].value = value; 384 | break; 385 | case 0x16: 386 | sensorArray[sensAdrArr].value = value; 387 | break; 388 | 389 | case 0x41: 390 | sensorArray[sensAdrArr].value = (uint16_t) (value * 100); 391 | break; 392 | case 0x7E: 393 | sensorArray[sensAdrArr].value = (uint16_t) (value * 10); 394 | break; 395 | 396 | case 0x80: 397 | sensorArray[sensAdrArr].value = (int32_t) (value * 10000000); 398 | break; 399 | case 0x81: 400 | sensorArray[sensAdrArr].value = (int32_t) (value * 10000000); 401 | break; 402 | case 0x82: 403 | sensorArray[sensAdrArr].value = (int32_t) (value * 100); 404 | break; 405 | case 0x83: 406 | sensorArray[sensAdrArr].value = (int32_t) (value * 100); 407 | break; 408 | case 0x84: 409 | sensorArray[sensAdrArr].value = (int32_t) (value * 100); 410 | break; 411 | 412 | case 0x85: 413 | sensorArray[sensAdrArr].value = (uint32_t) value; 414 | break; 415 | case 0x86: 416 | sensorArray[sensAdrArr].value = (uint32_t) value; 417 | break; 418 | case 0x87: 419 | sensorArray[sensAdrArr].value = (uint32_t) value; 420 | break; 421 | case 0x88: 422 | sensorArray[sensAdrArr].value = (uint32_t) value; 423 | break; 424 | case 0x89: 425 | sensorArray[sensAdrArr].value = (uint32_t) value; 426 | break; 427 | case 0x8A: 428 | sensorArray[sensAdrArr].value = (uint32_t) value; 429 | break; 430 | } 431 | } 432 | } // iBUSTelemetry::setSensorValueFP 433 | 434 | uint8_t 435 | iBUSTelemetry::getSensorSize(uint8_t type) 436 | { 437 | return (type < 0x80) ? 2 : 4; 438 | } 439 | 440 | uint16_t 441 | iBUSTelemetry::gpsStateValues(uint8_t firstVal, uint8_t secondVal) 442 | { 443 | return (secondVal << 8) | firstVal; 444 | } 445 | 446 | void 447 | iBUSTelemetry::run() 448 | { 449 | if ((available() >= 4) && (sensorCount > 0)) { 450 | bool msgReceived = 0; 451 | uint8_t len; 452 | uint8_t msgAdr; 453 | uint8_t msg; 454 | uint8_t sensAdr; 455 | uint8_t sensAdrArr; 456 | uint8_t sensSize; 457 | uint8_t pByte; 458 | uint16_t checksum; 459 | uint16_t checksumCalc; 460 | 461 | do { 462 | len = _receive_buffer[_receive_buffer_head]; 463 | msgAdr = _receive_buffer[(_receive_buffer_head + 1) % _SS_MAX_RX_BUFF]; 464 | checksum = _receive_buffer[(_receive_buffer_head + 2) % _SS_MAX_RX_BUFF]; 465 | checksum |= _receive_buffer[(_receive_buffer_head + 3) % _SS_MAX_RX_BUFF] << 8; 466 | checksumCalc = 0xFFFF - (len + msgAdr); 467 | 468 | if (checksumCalc == checksum) { 469 | msgReceived = 1; 470 | _receive_buffer_head = (_receive_buffer_head + 4) % _SS_MAX_RX_BUFF; 471 | } else { 472 | msgReceived = 0; 473 | _receive_buffer_head = (_receive_buffer_head + 1) % _SS_MAX_RX_BUFF; 474 | } 475 | } while ((!msgReceived) && (_receive_buffer_head != _receive_buffer_tail)); 476 | 477 | if (msgReceived) { 478 | msg = msgAdr >> 4; 479 | sensAdr = msgAdr & 0b1111; 480 | sensAdrArr = sensAdr - 1; 481 | sensSize = getSensorSize(sensorArray[sensAdrArr].type); 482 | 483 | switch (msg) { 484 | case 0b1000: 485 | delayMicroseconds(100); 486 | if (sensAdr <= sensorCount) { 487 | write(0x04); 488 | write((0b1000 << 4) + sensAdr); 489 | checksum = 0xFFFF - (0x04 + ((0b1000 << 4) + sensAdr)); 490 | write(checksum); 491 | write(checksum >> 8); 492 | } 493 | break; 494 | case 0b1001: 495 | delayMicroseconds(100); 496 | write(0x06); 497 | write((0b1001 << 4) + sensAdr); 498 | write(sensorArray[sensAdrArr].type); 499 | write(sensSize); 500 | checksum = 0xFFFF - (0x06 + ((0b1001 << 4) + sensAdr) + sensorArray[sensAdrArr].type + sensSize); 501 | write(checksum); 502 | write(checksum >> 8); 503 | break; 504 | case 0b1010: 505 | delayMicroseconds(100); 506 | write(sensSize + 4); 507 | write((0b1010 << 4) + sensAdr); 508 | checksum = 0xFFFF - ((sensSize + 4) + ((0b1010 << 4) + sensAdr)); 509 | 510 | for (int i = 0; i < sensSize; i++) { 511 | pByte = (sensorArray[sensAdrArr].value >> (8 * i)) & 0xFF; 512 | checksum -= pByte; 513 | write(pByte); 514 | } 515 | 516 | write(checksum); 517 | write(checksum >> 8); 518 | break; 519 | } 520 | } 521 | } 522 | } // iBUSTelemetry::run -------------------------------------------------------------------------------- /iBUSTelemetry.h: -------------------------------------------------------------------------------- 1 | /* 2 | * iBUSTelemetry.cpp - adis1313 3 | * 4 | * Libraries or parts of the code used in this project: 5 | * 6 | * SoftwareSerialWithHalfDuplex 7 | * https://github.com/nickstedman/SoftwareSerialWithHalfDuplex 8 | * 9 | * iBUStelemetry 10 | * https://github.com/Hrastovc/iBUStelemetry 11 | * 12 | * FlySkyI6 13 | * https://github.com/qba667/FlySkyI6/blob/master/source/source/ibustelemetry.h 14 | * 15 | * Big thanks to the authors! 16 | */ 17 | 18 | #ifndef iBUSTelemetry_h 19 | #define iBUSTelemetry_h_h 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #define _SS_MAX_RX_BUFF 64 // RX buffer size 26 | #ifndef GCC_VERSION 27 | # define GCC_VERSION (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__) 28 | #endif 29 | 30 | class iBUSTelemetry : public Stream 31 | { 32 | private: 33 | uint8_t _pin; 34 | uint8_t _pinBitMask; 35 | volatile uint8_t * _receivePortRegister; 36 | volatile uint8_t * _transmitPortRegister; 37 | volatile uint8_t * _pcint_maskreg; 38 | uint8_t _pcint_maskvalue; 39 | 40 | uint16_t _rx_delay_centering; 41 | uint16_t _rx_delay_intrabit; 42 | uint16_t _rx_delay_stopbit; 43 | uint16_t _tx_delay; 44 | 45 | bool _buffer_overflow; 46 | 47 | static char _receive_buffer[_SS_MAX_RX_BUFF]; 48 | static volatile uint8_t _receive_buffer_tail; 49 | static volatile uint8_t _receive_buffer_head; 50 | static iBUSTelemetry * active_object; 51 | 52 | void 53 | recv() __attribute__((__always_inline__)); 54 | uint8_t 55 | rx_pin_read(); 56 | 57 | void 58 | setPin(uint8_t pin); 59 | void 60 | setRxIntMsk(bool enable) __attribute__((__always_inline__)); 61 | 62 | static uint16_t 63 | subtract_cap(uint16_t num, uint16_t sub); 64 | 65 | static inline void 66 | tunedDelay(uint16_t delay); 67 | 68 | uint8_t sensorCount; 69 | 70 | typedef struct iBUSSensor { 71 | uint8_t type; 72 | int32_t value; 73 | } iBUSSensor; 74 | 75 | iBUSSensor sensorArray[MAX_SENS_COUNT]; 76 | 77 | uint8_t 78 | getSensorSize(uint8_t type); 79 | 80 | public: 81 | iBUSTelemetry(uint8_t receivePin); 82 | ~iBUSTelemetry(); 83 | void 84 | begin(); 85 | void 86 | run(); 87 | bool 88 | listen(); 89 | void 90 | end(); 91 | bool 92 | isListening(){ return this == active_object; } 93 | 94 | bool 95 | stopListening(); 96 | bool 97 | overflow(){ bool ret = _buffer_overflow; if (ret) _buffer_overflow = false; return ret; } 98 | 99 | int 100 | peek(); 101 | 102 | void 103 | addSensor(uint8_t type); 104 | void 105 | setSensorValue(uint8_t adr, int32_t value); 106 | void 107 | setSensorValueFP(uint8_t adr, float value); 108 | uint16_t 109 | gpsStateValues(uint8_t firstVal, uint8_t secondVal); 110 | 111 | virtual size_t 112 | write(uint8_t byte); 113 | virtual int 114 | read(); 115 | virtual int 116 | available(); 117 | virtual void 118 | flush(); 119 | operator bool (){ return true; } 120 | 121 | using Print::write; 122 | 123 | static inline void 124 | handle_interrupt() __attribute__((__always_inline__)); 125 | }; 126 | #endif // ifndef iBUSTelemetry_h -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map for iBUSTelemetry 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | 9 | iBUSTelemetry KEYWORD1 10 | 11 | ####################################### 12 | # Methods and Functions (KEYWORD2) 13 | ####################################### 14 | 15 | begin KEYWORD2 16 | run KEYWORD2 17 | listen KEYWORD2 18 | end KEYWORD2 19 | isListening KEYWORD2 20 | stopListening KEYWORD2 21 | overflow KEYWORD2 22 | peek KEYWORD2 23 | addSensor KEYWORD2 24 | setSensorValue KEYWORD2 25 | setSensorValueFP KEYWORD2 26 | gpsStateValues KEYWORD2 27 | write KEYWORD2 28 | read KEYWORD2 29 | available KEYWORD2 30 | flush KEYWORD2 31 | 32 | ####################################### 33 | # Constants (LITERAL1) 34 | ####################################### --------------------------------------------------------------------------------