BMC_SBUS M5Stack porting

[cepics]

Hi all
I'm trying to port an Arduino Mega sketch, using BMC_SBUS, to M5Stack!!
https://github.com/boldstelvis/BMC_SBUS ... /README.md
to use this: https://www.kinowheels.com/
with this: https://forum.dji.com/thread-167232-1-1.html
this is the connections: https://forum11.djicdn.com/data/attachm ... htjjsl.png

this is the Arduino Mega sketch connected to the ronin D-bus (it works!! I can move the ronin X and Y axis):

#define BAUDRATE 100000  // oder  100000 115200
#define SERIALPORT Serial  // - uncomment this line if using an arduino based board with more than one HW serial port

class BMC_SBUS
{
  public:
    uint8_t sbusData[25];
    int16_t servos[18];
    void begin(void);
    void Servo(uint8_t ch, int16_t position);
    void Send(void);
    void Update(void);

  private:
    uint8_t byte_in_sbus;
    uint8_t bit_in_sbus;
    uint8_t ch;
    uint8_t bit_in_servo;
};


void BMC_SBUS::begin()
{
  //intialise private data arrays
  //sbus_data is formatted for correct serial output
  //note that the actual 11bit sbus data for each channel is embedded across multiple data bytes in a very stange order
  //byte 1 and bytes 24 and 25 should be left as is 
  //the first is a start byte, the last is a stop byte and the second last holds various flags
  //servos is the internal per channel position and is more straightforward - one int_16 per channel

  uint8_t loc_sbusData[25] = {0x0f,0x01,0x04,0x20,0x00,0xff,0x07,0x40,0x00,0x02,0x10,0x80,0x2c,0x64,0x21,0x0b,0x59,0x08,0x40,0x00,0x02,0x10,0x80,0x00,0x00};
  int16_t loc_servos[18]   = {1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,0,0};

  //setup serial port to transmit at 100k baud and use 1 parity and 2 stop bits

  SERIALPORT.begin(BAUDRATE, SERIAL_8E2);

  //setup public data arrays

  memcpy(sbusData,loc_sbusData,25);
  memcpy(servos,loc_servos,18);
}

void BMC_SBUS::Servo(uint8_t ch, int16_t position) 
{
  //set servo position on single channel

  if ((ch>0)&&(ch<=16)) 
  {
    constrain (position, 0, 2048); //keep within min/max values
    servos[ch-1] = position; //expects a non zero starting index to the channel
  }
}

void BMC_SBUS::Send(void)
{
  //send data over serial port
  SERIALPORT.write(sbusData, 25); //according to docs for Serial we can send the array along as is without a loop
}

void BMC_SBUS::Update(void) 
{
  //update positions for all servo channels within the SBUS data frame
  //ignores digital servos and any failsafe mode stuff that was originally written

  //clear out existing sbus data for all channel data bytes
  //ignores first and last bytes in the array (start and stop bytes)
  //mapping loop relies on initial 0 values - do not omit this step!

  uint8_t i;
  for (i=1; i<24; i++) 
  {
    sbusData[i] = 0;
  }

  //reset counters

  ch = 0;
  bit_in_servo = 0;
  byte_in_sbus = 1;
  bit_in_sbus = 0;

  //format sbus data - maps sevo data array to sbus data array 1bit at a time
  //correctly deals with the little endian byte order in the process

  for (i=0; i<176; i++) //16channels*11bits = 176bits
  {
    if (servos[ch] & (1<<bit_in_servo)) //bitwise AND to check if the correct servo databit is set to 1
    {
      sbusData[byte_in_sbus] |= (1<<bit_in_sbus); //bitwise OR sets the correct sbus databit if true
    }

    //increment bit counters

    bit_in_sbus++;
    bit_in_servo++;

    //if end of sbus byte reset sbus bit counter and increment sbus byte counter

    if (bit_in_sbus == 8) 
    {
      bit_in_sbus = 0;
      byte_in_sbus++;
    }

    // if we have reached bit 11 in the servo data increment channel index and reset servo bit counter

    if (bit_in_servo == 11) 
    {
      bit_in_servo = 0;
      ch++;
    }
  }
}


//Declare BMC_SBUS Object
BMC_SBUS mySBUS;

// Sbus delay value
const int sbusWAIT = 7;      //frame timing delay in msecs

// Declare sbus control channels
int panChannel = 1;
int tiltChannel = 2;
int rollChannel = 4;


// Declare Kinowheels Stuff
int XA_SIG=0, XB_SIG=1, YA_SIG=0, YB_SIG=1, pulsesX, pulsesY;

// Declare Stuff for calculating Speed
int xStampEnd=0, yStampEnd=0, timeStampEnd=0, xPassed, yPassed, timePassed, sendX=1023, sendY=1023;




void setup() {
  // Serial.begin(100000); überflüssig, weil in MC_SBUS enthalten


  // Start  KinoWheels Stuff
  attachInterrupt(0, XA_RISE, RISING); // Pin 2
  attachInterrupt(1, XB_RISE, RISING); // Pin 3
  attachInterrupt(4, YA_RISE, RISING); // Pin 19
  attachInterrupt(5, YB_RISE, RISING); // Pin 18
   
  
   // Start BMC_SBUS object
  mySBUS.begin();
  
}

void loop() {

  for (int i=0; i<1; i++){        //SBUS needs data every 7 Milliseconds. I repeat it three times for some time to pass for calculating speeds.

  mySBUS.Servo(tiltChannel,sendY);
  mySBUS.Servo(panChannel,sendX);

  // Update SBUS object and send data
  mySBUS.Update();
  mySBUS.Send();
 
  delay(sbusWAIT);
  
}

  
 timePassed = millis() - timeStampEnd;
 xPassed = xStampEnd - pulsesX;
 yPassed = pulsesY - yStampEnd;

 sendX = 1023 + 100* xPassed / timePassed;
 sendY = 1023 + 100* yPassed / timePassed;

for (int i=0; i<1; i++){          //Maybe this one is not needed. Will find it out later

  mySBUS.Servo(tiltChannel,sendY);
  mySBUS.Servo(panChannel,sendX);

  // Update SBUS object and send data
  mySBUS.Update();
  mySBUS.Send();
 
  delay(sbusWAIT);
  
}

 xStampEnd = pulsesX;
 yStampEnd = pulsesY;
 timeStampEnd = millis(); 
}



//Rotary Encoder Stuff by KinoWheels

void XA_RISE(){
 detachInterrupt(0);
 //delay(1);
 XA_SIG=1;
 
 if(XB_SIG==0)
 pulsesX++;//moving forward
 if(XB_SIG==1)
 pulsesX--;//moving reverse

 attachInterrupt(0, XA_FALL, FALLING);
}

void XA_FALL(){
  detachInterrupt(0);
  //delay(1);
 XA_SIG=0;
 
 if(XB_SIG==1)
 pulsesX++;//moving forward
 if(XB_SIG==0)
 pulsesX--;//moving reverse

 attachInterrupt(0, XA_RISE, RISING);  
}

void XB_RISE(){
 detachInterrupt(1);
 //delay(1);
 XB_SIG=1;
 
 if(XA_SIG==1)
 pulsesX++;//moving forward
 if(XA_SIG==0)
 pulsesX--;//moving reverse

 attachInterrupt(1, XB_FALL, FALLING);
}

void XB_FALL(){
 detachInterrupt(1);
 //delay(1);
 XB_SIG=0;
 
 if(XA_SIG==0)
 pulsesX++;//moving forward
 if(XA_SIG==1)
 pulsesX--;//moving reverse

 attachInterrupt(1, XB_RISE, RISING);
}


void YA_RISE(){
 detachInterrupt(4);
 //delay(1);
 YA_SIG=1;
 
 if(YB_SIG==0)
 pulsesY++;//moving forward
 if(YB_SIG==1)
 pulsesY--;//moving reverse


 attachInterrupt(4, YA_FALL, FALLING);
}

void YA_FALL(){
  detachInterrupt(4);
  //delay(1);
 YA_SIG=0;
 
 if(YB_SIG==1)
 pulsesY++;//moving forward
 if(YB_SIG==0)
 pulsesY--;//moving reverse

 attachInterrupt(4, YA_RISE, RISING);  
}

void YB_RISE(){
 detachInterrupt(5);
 //delay(1);
 YB_SIG=1;
 
 if(YA_SIG==1)
 pulsesY++;//moving forward
 if(YA_SIG==0)
 pulsesY--;//moving reverse

 attachInterrupt(5, YB_FALL, FALLING);
}

void YB_FALL(){
 detachInterrupt(5);
 //delay(1);
 YB_SIG=0;
 
 if(YA_SIG==0)
 pulsesY++;//moving forward
 if(YA_SIG==1)
 pulsesY--;//moving reverse

 attachInterrupt(5, YB_RISE, RISING);
}

How can I port to M5Stack?

this in what I done ... but I have not Y axe control....
M5Stack----------ronin D-bus
grd-------------------grd
5V--------------------5V
TXD2(G17)------data

#include <M5Stack.h>
#include <ESP32Encoder.h>

ESP32Encoder encoderX;
ESP32Encoder encoderY;

#define BAUDRATE 100000  // oder  100000 115200
#define SERIALPORT Serial2  // - uncomment this line if using an arduino based board with more than one HW serial port


class BMC_SBUS
{
  public:
    uint8_t sbusData[25];
    int16_t servos[18];
    void begin(void);
    void Servo(uint8_t ch, int16_t position);
    void Send(void);
    void Update(void);

  private:
    uint8_t byte_in_sbus;
    uint8_t bit_in_sbus;
    uint8_t ch;
    uint8_t bit_in_servo;
};


void BMC_SBUS::begin()
{
  //intialise private data arrays
  //sbus_data is formatted for correct serial output
  //note that the actual 11bit sbus data for each channel is embedded across multiple data bytes in a very stange order
  //byte 1 and bytes 24 and 25 should be left as is
  //the first is a start byte, the last is a stop byte and the second last holds various flags
  //servos is the internal per channel position and is more straightforward - one int_16 per channel

  uint8_t loc_sbusData[25] = {0x0f, 0x01, 0x04, 0x20, 0x00, 0xff, 0x07, 0x40, 0x00, 0x02, 0x10, 0x80, 0x2c, 0x64, 0x21, 0x0b, 0x59, 0x08, 0x40, 0x00, 0x02, 0x10, 0x80, 0x00, 0x00};
  int16_t loc_servos[18]   = {1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 0, 0};

  //setup serial port to transmit at 100k baud and use 1 parity and 2 stop bits

  SERIALPORT.begin(BAUDRATE, SERIAL_8E2);

  //setup public data arrays

  memcpy(sbusData, loc_sbusData, 25);
  memcpy(servos, loc_servos, 18);
}

void BMC_SBUS::Servo(uint8_t ch, int16_t position)
{
  //set servo position on single channel

  if ((ch > 0) && (ch <= 16))
  {
    constrain (position, 0, 2048); //keep within min/max values
    servos[ch - 1] = position; //expects a non zero starting index to the channel
  }
}

void BMC_SBUS::Send(void)
{
  //send data over serial port
  SERIALPORT.write(sbusData, 25); //according to docs for Serial we can send the array along as is without a loop
}

void BMC_SBUS::Update(void)
{
  //update positions for all servo channels within the SBUS data frame
  //ignores digital servos and any failsafe mode stuff that was originally written

  //clear out existing sbus data for all channel data bytes
  //ignores first and last bytes in the array (start and stop bytes)
  //mapping loop relies on initial 0 values - do not omit this step!

  uint8_t i;
  for (i = 1; i < 24; i++)
  {
    sbusData[i] = 0;
  }

  //reset counters

  ch = 0;
  bit_in_servo = 0;
  byte_in_sbus = 1;
  bit_in_sbus = 0;

  //format sbus data - maps sevo data array to sbus data array 1bit at a time
  //correctly deals with the little endian byte order in the process

  for (i = 0; i < 176; i++) //16channels*11bits = 176bits
  {
    if (servos[ch] & (1 << bit_in_servo)) //bitwise AND to check if the correct servo databit is set to 1
    {
      sbusData[byte_in_sbus] |= (1 << bit_in_sbus); //bitwise OR sets the correct sbus databit if true
    }

    //increment bit counters

    bit_in_sbus++;
    bit_in_servo++;

    //if end of sbus byte reset sbus bit counter and increment sbus byte counter

    if (bit_in_sbus == 8)
    {
      bit_in_sbus = 0;
      byte_in_sbus++;
    }

    // if we have reached bit 11 in the servo data increment channel index and reset servo bit counter

    if (bit_in_servo == 11)
    {
      bit_in_servo = 0;
      ch++;
    }
  }
}

//Declare BMC_SBUS Object
BMC_SBUS mySBUS;

// Sbus delay value
const int sbusWAIT = 7;      //frame timing delay in msecs

// Declare sbus control channels
int panChannel = 1;
int tiltChannel = 2;
int rollChannel = 4;

float pulsesX, pulsesY;

int xStampEnd = 0;
int yStampEnd = 0;
int timeStampEnd = 0;
int xPassed;
int yPassed;
int timePassed;
int sendX;// ??????
int sendY;// ??????

// Declare Poti Values
int potiX = 100;
int potiY = 100;

bool PANTILTtoggle = 0;


void setup() {
  M5.begin();

  // clear the encoder's raw count and set the tracked count to zero
  encoderX.clearCount();
  encoderY.clearCount();

  // Attache pins for use as encoder pins
  encoderX.attachHalfQuad(2, 3);
  encoderY.attachHalfQuad(36, 35);
  
  M5.update();

  sendX = 1023;
  sendY = 1023;

  // Start BMC_SBUS object
  mySBUS.begin();
}

void loop() {

  pulsesX = encoderX.getCount();
  pulsesY = encoderY.getCount();

  for (int i = 0; i < 1; i++) {   //SBUS needs data every 7 Milliseconds. I repeat it three times for some time to pass for calculating speeds.

    mySBUS.Servo(tiltChannel, sendY);
    mySBUS.Servo(panChannel, sendX);

    // Update SBUS object and send data
    mySBUS.Update();
    mySBUS.Send();

    delay(sbusWAIT);

  }


  timePassed = millis() - timeStampEnd;
  xPassed = xStampEnd - pulsesX;
  yPassed = pulsesY - yStampEnd;

  M5.update();

    sendX = 1023 + 100 * xPassed / timePassed;
    sendY = 1023 + 100 * yPassed / timePassed;

  if (sendX > 2047) {
    sendX = 2047;
  }
  if (sendX < 0) {
    sendX = 0;
  }
  if (sendY > 2047) {
    sendY = 2047;
  }
  if (sendY < 0) {
    sendY = 0;
  }

  for (int i = 0; i < 1; i++) {     //Maybe this one is not needed. Will find it out later

    mySBUS.Servo(tiltChannel, sendY);
    mySBUS.Servo(panChannel, sendX);

    // Update SBUS object and send data
    mySBUS.Update();
    mySBUS.Send();

    delay(sbusWAIT);

  }

  xStampEnd = pulsesX;
  yStampEnd = pulsesY;
  timeStampEnd = millis();

  M5.update();
}

the strange thing is that the information about X axe, go to the machine connected at G17 (TXD2), but not the Y one..
I'm using an inverter on the TX-----RX line...for (d-bus/s-bus)

tnks a lot

[Lewis-H]

codice arduino mega stabilizzatore

#define BAUDRATE 100000 // oder 100000 115200
//#define BAUDRATE 115200 // oder 100000 115200
#define SERIALPORT Serial // - uncomment this line if using an arduino based board with more than one HW serial port

class BMC_SBUS
{
public:
uint8_t sbusData[25];
int16_t servos[18];
void begin(void);
void Servo(uint8_t ch, int16_t position);
void Send(void);
void Update(void);

private:
uint8_t byte_in_sbus;
uint8_t bit_in_sbus;
uint8_t ch;
uint8_t bit_in_servo;
};

void BMC_SBUS::begin()
{
//intialise private data arrays
//sbus_data is formatted for correct serial output
//note that the actual 11bit sbus data for each channel is embedded across multiple data bytes in a very stange order
//byte 1 and bytes 24 and 25 should be left as is
//the first is a start byte, the last is a stop byte and the second last holds various flags
//servos is the internal per channel position and is more straightforward - one int_16 per channel

uint8_t loc_sbusData[25] = {0x0f, 0x01, 0x04, 0x20, 0x00, 0xff, 0x07, 0x40, 0x00, 0x02, 0x10, 0x80, 0x2c, 0x64, 0x21, 0x0b, 0x59, 0x08, 0x40, 0x00, 0x02, 0x10, 0x80, 0x00, 0x00};
int16_t loc_servos[18] = {1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 0, 0};

//setup serial port to transmit at 100k baud and use 1 parity and 2 stop bits

SERIALPORT.begin(BAUDRATE, SERIAL_8E2);

//setup public data arrays

memcpy(sbusData, loc_sbusData, 25);
memcpy(servos, loc_servos, 18);
}

void BMC_SBUS::Servo(uint8_t ch, int16_t position)
{
//set servo position on single channel

if ((ch > 0) && (ch <= 16))
{
constrain (position, 0, 2048); //keep within min/max values
servos[ch - 1] = position; //expects a non zero starting index to the channel
}
}

void BMC_SBUS::Send(void)
{
//send data over serial port
SERIALPORT.write(sbusData, 25); //according to docs for Serial we can send the array along as is without a loop
}

void BMC_SBUS::Update(void)
{
//update positions for all servo channels within the SBUS data frame
//ignores digital servos and any failsafe mode stuff that was originally written

//clear out existing sbus data for all channel data bytes
//ignores first and last bytes in the array (start and stop bytes)
//mapping loop relies on initial 0 values - do not omit this step!

uint8_t i;
for (i = 1; i < 24; i++)
{
sbusData[i] = 0;
}

//reset counters

ch = 0;
bit_in_servo = 0;
byte_in_sbus = 1;
bit_in_sbus = 0;

//format sbus data - maps sevo data array to sbus data array 1bit at a time
//correctly deals with the little endian byte order in the process

for (i = 0; i < 176; i++) //16channels*11bits = 176bits
{
if (servos[ch] & (1 << bit_in_servo)) //bitwise AND to check if the correct servo databit is set to 1
{
sbusData[byte_in_sbus] |= (1 << bit_in_sbus); //bitwise OR sets the correct sbus databit if true
}

//increment bit counters

bit_in_sbus++;
bit_in_servo++;

//if end of sbus byte reset sbus bit counter and increment sbus byte counter

if (bit_in_sbus == 8)
{
  bit_in_sbus = 0;
  byte_in_sbus++;
}

// if we have reached bit 11 in the servo data increment channel index and reset servo bit counter

if (bit_in_servo == 11)
{
  bit_in_servo = 0;
  ch++;
}

}
}

//Declare BMC_SBUS Object
BMC_SBUS mySBUS;

// Sbus delay value
const int sbusWAIT = 7; //frame timing delay in msecs

// Declare sbus control channels
int panChannel = 1;
int tiltChannel = 2;
int rollChannel = 4;

// Declare Kinowheels Stuff
int XA_SIG = 0, XB_SIG = 1, YA_SIG = 0, YB_SIG = 1, pulsesX, pulsesY;

// Declare Stuff for calculating Speed
int xStampEnd = 0, yStampEnd = 0, timeStampEnd = 0, xPassed, yPassed, timePassed, sendX = 1023, sendY = 1023;

void setup() {
// Serial.begin(100000); überflüssig, weil in MC_SBUS enthalten

// Start KinoWheels Stuff
attachInterrupt(0, XA_RISE, RISING); // Pin 2
attachInterrupt(1, XB_RISE, RISING); // Pin 3
attachInterrupt(4, YA_RISE, RISING); // Pin 19
attachInterrupt(5, YB_RISE, RISING); // Pin 18

// Start BMC_SBUS object
mySBUS.begin();

}

void loop() {

for (int i = 0; i < 1; i++) { //SBUS needs data every 7 Milliseconds. I repeat it three times for some time to pass for calculating speeds.

mySBUS.Servo(tiltChannel, sendY);
mySBUS.Servo(panChannel, sendX);

// Update SBUS object and send data
mySBUS.Update();
mySBUS.Send();

delay(sbusWAIT);

}

timePassed = millis() - timeStampEnd;
xPassed = xStampEnd - pulsesX;
yPassed = pulsesY - yStampEnd;

sendX = 1023 + 100 * xPassed / timePassed;
sendY = 1023 + 100 * yPassed / timePassed;

for (int i = 0; i < 1; i++) { //Maybe this one is not needed. Will find it out later

mySBUS.Servo(tiltChannel, sendY);
mySBUS.Servo(panChannel, sendX);

// Update SBUS object and send data
mySBUS.Update();
mySBUS.Send();

delay(sbusWAIT);

}

xStampEnd = pulsesX;
yStampEnd = pulsesY;
timeStampEnd = millis();
}

//Rotary Encoder Stuff by KinoWheels

void XA_RISE() {
detachInterrupt(0);
//delay(1);
XA_SIG = 1;

if (XB_SIG == 0)
pulsesX++;//moving forward
if (XB_SIG == 1)
pulsesX--;//moving reverse

attachInterrupt(0, XA_FALL, FALLING);
}

void XA_FALL() {
detachInterrupt(0);
//delay(1);
XA_SIG = 0;

if (XB_SIG == 1)
pulsesX++;//moving forward
if (XB_SIG == 0)
pulsesX--;//moving reverse

attachInterrupt(0, XA_RISE, RISING);
}

void XB_RISE() {
detachInterrupt(1);
//delay(1);
XB_SIG = 1;

if (XA_SIG == 1)
pulsesX++;//moving forward
if (XA_SIG == 0)
pulsesX--;//moving reverse

attachInterrupt(1, XB_FALL, FALLING);
}

void XB_FALL() {
detachInterrupt(1);
//delay(1);
XB_SIG = 0;

if (XA_SIG == 0)
pulsesX++;//moving forward
if (XA_SIG == 1)
pulsesX--;//moving reverse

attachInterrupt(1, XB_RISE, RISING);
}

void YA_RISE() {
detachInterrupt(4);
//delay(1);
YA_SIG = 1;

if (YB_SIG == 0)
pulsesY++;//moving forward
if (YB_SIG == 1)
pulsesY--;//moving reverse

attachInterrupt(4, YA_FALL, FALLING);
}

void YA_FALL() {
detachInterrupt(4);
//delay(1);
YA_SIG = 0;

if (YB_SIG == 1)
pulsesY++;//moving forward
if (YB_SIG == 0)
pulsesY--;//moving reverse

attachInterrupt(4, YA_RISE, RISING);
}

void YB_RISE() {
detachInterrupt(5);
//delay(1);
YB_SIG = 1;

if (YA_SIG == 1)
pulsesY++;//moving forward
if (YA_SIG == 0)
pulsesY--;//moving reverse

attachInterrupt(5, YB_FALL, FALLING);
}

void YB_FALL() {
detachInterrupt(5);
//delay(1);
YB_SIG = 0;

if (YA_SIG == 0)
pulsesY++;//moving forward
if (YA_SIG == 1)
pulsesY--;//moving reverse

attachInterrupt(5, YB_RISE, RISING);
}