BMC_SBUS M5Stack porting



  • 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