Hi everyone, I have this question I'm trying to do telemetry. I am using ia6b and Flysky i6. I am using the ibus library. I control the body IBusServo.addSensor (IBUS_MEAS_TYPE_GPS_STATUS);
takes 4 bytes as two values.
However, I cannot introduce two values; (
I enter two numbers:
IBusServo.setSensorMeasurement (1, (77,22));
And I get:
Can anyone help me how to enter it so that it correctly displays 2 numerical variables?
My code:
void setup() {
Serial.begin(115200);
IBusServo.begin(Serial1);
// adding 2 sensors
IBusServo.addSensor(IBUS_MEAS_TYPE_GPS_STATUS);
IBusServo.addSensor(IBUS_MEAS_TYPE_S85);
IBusServo.addSensor(IBUS_MEAS_TYPE_FLIGHT_MODE);
}
void loop() {
ibusServo();
}
void ibusServo()
{
IBusServo.setSensorMeasurement(1, (77,22));
IBusServo.setSensorMeasurement(2,23);
IBusServo.setSensorMeasurement(3,1); // 5 Return, 6 RTL, 2 low V, 3 wait, 1 HOME, 4 Manual, 7 circle, 8 Error, 0 Save, 9 to point
delay(2000);
}
IbusBM.cpp
/*
* 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;
}
IbusBM.h
/*
* Interface to the RC IBus protocol
*
* 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
*/
#ifndef IBusBM_h
#define IBusBM_h
#include <inttypes.h>
#if defined(ARDUINO_ARCH_MBED)
#include "mbed.h"
#include "HardwareSerial.h"
#endif
// if you have an opentx transciever you can add additional sensor types here.
// see https://github.com/cleanflight/cleanflight/blob/7cd417959b3cb605aa574fc8c0f16759943527ef/src/main/telemetry/ibus_shared.h
// below the values supported by the Turnigy FS-MT6 transceiver
#define IBUSS_INTV 0x00 // Internal voltage (in 0.01)
#define IBUS_MEAS_TYPE_TEM 0x01 // Temperature (in 0.1 degrees, where 0=-40'C)
#define IBUSS_RPM 0x02 // RPM
#define IBUS_MEAS_TYPE_EXTV 0x03 // External voltage (in 0.01)
#define IBUS_MEAS_TYPE_CELL 0x04 // Avg cell voltage
#define IBUS_MEAS_TYPE_BAT_CURR 0x05 // Battery current
#define IBUS_MEAS_TYPE_FUEL 0x06 // Remaining battery percentage
#define IBUS_MEAS_TYPE_RPM 0x07 // Throttle value / battery capacity
#define IBUS_MEAS_TYPE_CMP_HEAD 0x08 // Heading
#define IBUS_MEAS_TYPE_CLIMB_RATE 0x09 // Climb rate
#define IBUS_MEAS_TYPE_COG 0x0a // Course over ground
#define IBUS_MEAS_TYPE_GPS_STATUS 0x0b // GPS status (2 values)
#define IBUS_MEAS_TYPE_ACC_X 0x0c // Acc X
#define IBUS_MEAS_TYPE_ACC_Y 0x0d // Acc Y
#define IBUS_MEAS_TYPE_ACC_Z 0x0e // Acc Z
#define IBUS_MEAS_TYPE_ROLL 0x0f // Roll
#define IBUS_MEAS_TYPE_PITCH 0x10 // Pitch
#define IBUS_MEAS_TYPE_YAW 0x11 // Yaw
#define IBUS_MEAS_TYPE_VERTICAL_SPEED 0x12 // Vertical speed
#define IBUS_MEAS_TYPE_GROUND_SPEED 0x13 // Speed m/s
#define IBUS_MEAS_TYPE_GPS_DIST 0x14 // Distance from home
#define IBUS_MEAS_TYPE_ARMED 0x15 // Armed / unarmed
#define IBUS_MEAS_TYPE_FLIGHT_MODE 0x16 // Flight mode
#define IBUS_MEAS_TYPE_PRES 0x41 // Pressure (in Pa)
#define IBUS_MEAS_TYPE_SPE 0x7e // Speed km/h
#define IBUS_SERVO 0xfd // Servo value
// 4 byte sensors
#define IBUS_MEAS_TYPE_GPS_LAT 0x80 // WGS84 in degrees * 1E7
#define IBUS_MEAS_TYPE_GPS_LON 0x81 // WGS84 in degrees * 1E7
#define IBUS_MEAS_TYPE_GPS_ALT 0x82 // GPS altitude
#define IBUS_MEAS_TYPE_ALT 0x83 // Altitude
#define IBUS_MEAS_TYPE_ALT_MAX 0x84 // Max altitude
#define IBUS_MEAS_TYPE_S85 0x85
#define IBUS_MEAS_TYPE_S86 0x86
#define IBUS_MEAS_TYPE_S87 0x87
#define IBUS_MEAS_TYPE_S88 0x88
#define IBUS_MEAS_TYPE_S89 0x89
#define IBUS_MEAS_TYPE_S8a 0x8a
#if defined(ARDUINO_ARCH_MBED)
#define HardwareSerial arduino::HardwareSerial
#else
#if !defined(ARDUINO_ARCH_MEGAAVR)
class HardwareSerial;
#endif
#endif
class Stream;
class IBusBM {
public:
#if defined(_VARIANT_ARDUINO_STM32_)
#if !defined(STM32_CORE_VERSION) || (STM32_CORE_VERSION < 0x01090000)
#error "Due to API change, this sketch is compatible with STM32_CORE_VERSION >= 0x01090000"
#endif
#define IBUSBM_NOTIMER NULL // no timer interrupt used
void begin(HardwareSerial &serial, TIM_TypeDef * timerid=TIM1, int8_t rxPin=-1, int8_t txPin=-1);
#else
#define IBUSBM_NOTIMER -1 // no timer interrupt used
void begin(HardwareSerial &serial, int8_t timerid=0, int8_t rxPin=-1, int8_t txPin=-1);
#endif
uint16_t readChannel(uint8_t channelNr); // read servo channel 0..9
uint8_t addSensor(uint8_t type, uint8_t len=2); // add sensor type and data length (2 or 4), returns address
void setSensorMeasurement(uint8_t adr, int32_t value);
void loop(void); // used internally for interrupt handline, but needs to be defined as public
volatile uint8_t cnt_poll; // count received number of sensor poll messages
volatile uint8_t cnt_sensor; // count times a sensor value has been sent back
volatile uint8_t cnt_rec; // count received number of servo messages
private:
enum State {GET_LENGTH, GET_DATA, GET_CHKSUML, GET_CHKSUMH, DISCARD};
static const uint8_t PROTOCOL_LENGTH = 0x20;
static const uint8_t PROTOCOL_OVERHEAD = 3; // packet is <len><cmd><data....><chkl><chkh>, overhead=cmd+chk bytes
static const uint8_t PROTOCOL_TIMEGAP = 3; // Packets are received very ~7ms so use ~half that for the gap
static const uint8_t PROTOCOL_CHANNELS = 14;
static const uint8_t PROTOCOL_COMMAND40 = 0x40; // Command to set servo or motor speed is always 0x40
static const uint8_t PROTOCOL_COMMAND_DISCOVER = 0x80; // Command discover sensor (lowest 4 bits are sensor)
static const uint8_t PROTOCOL_COMMAND_TYPE = 0x90; // Command discover sensor (lowest 4 bits are sensor)
static const uint8_t PROTOCOL_COMMAND_VALUE = 0xA0; // Command send sensor data (lowest 4 bits are sensor)
static const uint8_t SENSORMAX = 10; // Max number of sensors
uint8_t state; // state machine state for iBUS protocol
HardwareSerial *stream; // serial port
uint32_t last; // milis() of prior message
uint8_t buffer[PROTOCOL_LENGTH]; // message buffer
uint8_t ptr; // pointer in buffer
uint8_t len; // message length
uint16_t channel[PROTOCOL_CHANNELS]; // servo data received
uint16_t chksum; // checksum calculation
uint8_t lchksum; // checksum lower byte received
typedef struct {
uint8_t sensorType; // sensor type (0,1,2,3, etc)
uint8_t sensorLength; // data length for defined sensor (can be 2 or 4)
int32_t sensorValue; // sensor data for defined sensors (16 or 32 bits)
} sensorinfo;
sensorinfo sensors[SENSORMAX];
uint8_t NumberSensors = 0; // number of sensors
IBusBM* IBusBMnext = NULL; // pointer to the next class instance to be used to call the loop() method from timer interrupt
};
#endif