mdchaparror/microcontroladores
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

IBUSM

Browse Source
This commit is contained in:
Martin Daniel Chaparro Ruiz
2021-06-21 19:59:16 -05:00
parent 3ccbd309b2
commit 68f02588e2
10 changed files with 527 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

296
ESP32/IBUSM/src/IBusBM.cpp Normal file
View File

@@ -0,0 +1,296 @@
/*
* 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 <Arduino.h>
#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;
}

33
ESP32/IBUSM/src/main.cpp Normal file
View File

@@ -0,0 +1,33 @@
#include <Arduino.h>
#include "IBusBM.h"
IBusBM IBus;
int saveval = 0;
void setup()
{
Serial.begin(115200);
delay(100);
IBus.begin(Serial2, IBUSBM_NOTIMER);
delay(100);
Serial.println("Start IBus2PWM_ESP32");
}
void loop()
{
uint16_t val;
IBus.loop();
for (int i=0; i<10; i++) {
Serial.print(IBus.readChannel(i));
Serial.print(" ");
}
Serial.println("");
val = IBus.readChannel(2); // get latest value for servo channel 1
if (saveval != val)
{
//Serial.println(map(val, 1000, 2000, 0, 500));
saveval = val;
}
delay(800);
}