/* * Interface to the RC IBus protocol * * Based on original work from: https://gitlab.com/timwilkinson/FlySkyIBus * Extended to also handle sensors/telemetry data to be sent back to the transmitter, * interrupts driven and other features. * * This lib requires a hardware UART for communication * Another version using software serial is here https://github.com/Hrastovc/iBUStelemetry * * Explaination of sensor/ telemetry prtocol here: * https://github.com/betaflight/betaflight/wiki/Single-wire-FlySky-(IBus)-telemetry * * This library is free software; you can redistribute it and/or * modify it under the terms of the GNU Lesser General Public * License as published by the Free Software Foundation; either * version 2.1 of the License, or (at your option) any later version. * * Created 12 March 2019 Bart Mellink * Updated 4 April 2019 to support ESP32 * updated 13 jun 2019 to support STM32 (pauluzs) * Updated 21 Jul 2020 to support MBED (David Peverley) */ #include #include "IBusBM.h" // pointer to the first class instance to be used to call the loop() method from timer interrupt // will be initiated by class constructor, then daisy channed to other class instances if we have more than one IBusBM* IBusBMfirst = NULL; // Interrupt on timer0 - called every 1 ms // we call the IBusSensor.loop() here, so we are certain we respond to sensor requests in a timely matter #ifdef ARDUINO_ARCH_AVR SIGNAL(TIMER0_COMPA_vect) { if (IBusBMfirst) IBusBMfirst->loop(); // gets new servo values if available and process any sensor data } #else void onTimer() { if (IBusBMfirst) IBusBMfirst->loop(); // gets new servo values if available and process any sensor data } #endif #if defined(ARDUINO_ARCH_MBED) extern "C" { void TIMER4_IRQHandler_v() { if (NRF_TIMER4->EVENTS_COMPARE[0] == 1) { onTimer(); NRF_TIMER4->EVENTS_COMPARE[0] = 0; } } } #endif /* * supports max 14 channels in this lib (with messagelength of 0x20 there is room for 14 channels) Example set of bytes coming over the iBUS line for setting servos: 20 40 DB 5 DC 5 54 5 DC 5 E8 3 D0 7 D2 5 E8 3 DC 5 DC 5 DC 5 DC 5 DC 5 DC 5 DA F3 Explanation Protocol length: 20 Command code: 40 Channel 0: DB 5 -> value 0x5DB Channel 1: DC 5 -> value 0x5Dc Channel 2: 54 5 -> value 0x554 Channel 3: DC 5 -> value 0x5DC Channel 4: E8 3 -> value 0x3E8 Channel 5: D0 7 -> value 0x7D0 Channel 6: D2 5 -> value 0x5D2 Channel 7: E8 3 -> value 0x3E8 Channel 8: DC 5 -> value 0x5DC Channel 9: DC 5 -> value 0x5DC Channel 10: DC 5 -> value 0x5DC Channel 11: DC 5 -> value 0x5DC Channel 12: DC 5 -> value 0x5DC Channel 13: DC 5 -> value 0x5DC Checksum: DA F3 -> calculated by adding up all previous bytes, total must be FFFF */ #if defined(_VARIANT_ARDUINO_STM32_) void IBusBM::begin(HardwareSerial &serial, TIM_TypeDef * timerid, int8_t rxPin, int8_t txPin) { #else void IBusBM::begin(HardwareSerial &serial, int8_t timerid, int8_t rxPin, int8_t txPin) { #endif #ifdef ARDUINO_ARCH_ESP32 serial.begin(115200, SERIAL_8N1, rxPin, txPin); #else serial.begin(115200, SERIAL_8N1); #endif this->stream = &serial; this->state = DISCARD; this->last = millis(); this->ptr = 0; this->len = 0; this->chksum = 0; this->lchksum = 0; // we need to process the iBUS sensor protocol handler frequently enough (at least once each ms) to ensure the response data // from the sensor is sent on time to the receiver // if timerid==IBUSBM_NOTIMER the user is responsible for calling the loop function this->IBusBMnext = IBusBMfirst; if (!IBusBMfirst && timerid != IBUSBM_NOTIMER) { #ifdef ARDUINO_ARCH_AVR // on AVR architectures Timer0 is already used for millis() - we'll just interrupt somewhere in the middle and call the TIMER0_COMPA_vect interrupt OCR0A = 0xAF; TIMSK0 |= _BV(OCIE0A); #else // on other architectures we need to use a time #if defined(ARDUINO_ARCH_ESP32) hw_timer_t * timer = NULL; timer = timerBegin(timerid, F_CPU / 1000000L, true); // defaults to timer_id = 0; divider=80 (1 ms); countUp = true; timerAttachInterrupt(timer, &onTimer, true); // edge = true timerAlarmWrite(timer, 1000, true); //1 ms timerAlarmEnable(timer); #elif defined(_VARIANT_ARDUINO_STM32_) // see https://github.com/stm32duino/wiki/wiki/HardwareTimer-library HardwareTimer *stimer_t = new HardwareTimer(timerid); stimer_t->setOverflow(1000, HERTZ_FORMAT); // 1000 Hz stimer_t->attachInterrupt(onTimer); stimer_t->resume(); #elif defined(ARDUINO_ARCH_MBED) NRF_TIMER4->TASKS_STOP = 1; // Stop timer NRF_TIMER4->MODE = TIMER_MODE_MODE_Timer; // Set the timer in Counter Mode NRF_TIMER4->BITMODE = TIMER_BITMODE_BITMODE_16Bit << TIMER_BITMODE_BITMODE_Pos; NRF_TIMER2->TASKS_CLEAR = 1; // clear the task first to be usable for later // Set prescaler & compare register. // Prescaler = 0 gives 16MHz timer. // Prescaler = 4 (2^4) gives 1MHz timer. NRF_TIMER4->PRESCALER = 4 << TIMER_PRESCALER_PRESCALER_Pos; NRF_TIMER4->CC[0] = 1000; // Enable interrupt on Timer 4 for CC[0] compare match events NRF_TIMER4->INTENSET = TIMER_INTENSET_COMPARE0_Enabled << TIMER_INTENSET_COMPARE0_Pos; NRF_TIMER4->SHORTS = TIMER_SHORTS_COMPARE0_CLEAR_Enabled << TIMER_SHORTS_COMPARE0_CLEAR_Pos; NVIC_EnableIRQ(TIMER4_IRQn); NRF_TIMER4->TASKS_START = 1; // Start TIMER2 #else // It should not be too difficult to support additional architectures as most have timer functions, but I only tested AVR and ESP32 #warning "Timing only supportted for AVR, ESP32 and STM32 architectures. Use timerid IBUSBM_NOTIMER" #endif #endif } IBusBMfirst = this; } // called from timer interrupt or mannually by user (if IBUSBM_NOTIMER set in begin()) void IBusBM::loop(void) { // if we have multiple instances of IBusBM, we (recursively) call the other instances loop() function if (IBusBMnext) IBusBMnext->loop(); // only process data already in our UART receive buffer while (stream->available() > 0) { // only consider a new data package if we have not heard anything for >3ms uint32_t now = millis(); if (now - last >= PROTOCOL_TIMEGAP){ state = GET_LENGTH; } last = now; uint8_t v = stream->read(); switch (state) { case GET_LENGTH: if (v <= PROTOCOL_LENGTH && v > PROTOCOL_OVERHEAD) { ptr = 0; len = v - PROTOCOL_OVERHEAD; chksum = 0xFFFF - v; state = GET_DATA; } else { state = DISCARD; } break; case GET_DATA: buffer[ptr++] = v; chksum -= v; if (ptr == len) { state = GET_CHKSUML; } break; case GET_CHKSUML: lchksum = v; state = GET_CHKSUMH; break; case GET_CHKSUMH: // Validate checksum if (chksum == (v << 8) + lchksum) { // Checksum is all fine Execute command - uint8_t adr = buffer[0] & 0x0f; if (buffer[0]==PROTOCOL_COMMAND40) { // Valid servo command received - extract channel data for (uint8_t i = 1; i < PROTOCOL_CHANNELS * 2 + 1; i += 2) { channel[i / 2] = buffer[i] | (buffer[i + 1] << 8); } cnt_rec++; } else if (adr<=NumberSensors && adr>0 && len==1) { // all sensor data commands go here // we only process the len==1 commands (=message length is 4 bytes incl overhead) to prevent the case the // return messages from the UART TX port loop back to the RX port and are processed again. This is extra // precaution as it will also be prevented by the PROTOCOL_TIMEGAP required sensorinfo *s = &sensors[adr-1]; delayMicroseconds(100); switch (buffer[0] & 0x0f0) { case PROTOCOL_COMMAND_DISCOVER: // 0x80, discover sensor cnt_poll++; // echo discover command: 0x04, 0x81, 0x7A, 0xFF stream->write(0x04); stream->write(PROTOCOL_COMMAND_DISCOVER + adr); chksum = 0xFFFF - (0x04 + PROTOCOL_COMMAND_DISCOVER + adr); break; case PROTOCOL_COMMAND_TYPE: // 0x90, send sensor type // echo sensortype command: 0x06 0x91 0x00 0x02 0x66 0xFF stream->write(0x06); stream->write(PROTOCOL_COMMAND_TYPE + adr); stream->write(s->sensorType); stream->write(s->sensorLength); chksum = 0xFFFF - (0x06 + PROTOCOL_COMMAND_TYPE + adr + s->sensorType + s->sensorLength); break; case PROTOCOL_COMMAND_VALUE: // 0xA0, send sensor data cnt_sensor++; uint8_t t; // echo sensor value command: 0x06 0x91 0x00 0x02 0x66 0xFF stream->write(t = 0x04 + s->sensorLength); chksum = 0xFFFF - t; stream->write(t = PROTOCOL_COMMAND_VALUE + adr); chksum -= t; stream->write(t = s->sensorValue & 0x0ff); chksum -= t; stream->write(t = (s->sensorValue >> 8) & 0x0ff); chksum -= t; if (s->sensorLength==4) { stream->write(t = (s->sensorValue >> 16) & 0x0ff); chksum -= t; stream->write(t = (s->sensorValue >> 24) & 0x0ff); chksum -= t; } break; default: adr=0; // unknown command, prevent sending chksum break; } if (adr>0) { stream->write(chksum & 0x0ff); stream->write(chksum >> 8); } } } state = DISCARD; break; case DISCARD: default: break; } } } uint16_t IBusBM::readChannel(uint8_t channelNr) { if (channelNr < PROTOCOL_CHANNELS) { return channel[channelNr]; } else { return 0; } } uint8_t IBusBM::addSensor(uint8_t type, uint8_t len) { // add a sensor, return sensor number if (len!=2 && len!=4) len = 2; if (NumberSensors < SENSORMAX) { sensorinfo *s = &sensors[NumberSensors]; s->sensorType = type; s->sensorLength = len; s->sensorValue = 0; NumberSensors++; } return NumberSensors; } void IBusBM::setSensorMeasurement(uint8_t adr, int32_t value) { if (adr<=NumberSensors && adr>0) sensors[adr-1].sensorValue = value; }