esp32 BMC_SBUS porting

cepics · April 7, 2020, 6:35pm

buona sera a tutti, sto cercando di "portare" un codice che su arduino mega funziona alla grande, su un M5Stack (esp32)...

si tratta di un merge tra du e codici per poter dirigere uno stabilizzatore dji attraverso l'S-BUS-

ho due encoder rotativi che devono poter comandare due dei tre assi di cui lo stabilizzatore dispone..

in principio utilizzavo un arduino mega (seguendo questo tutorial) che muove tilt e pan all'interno di un simulatore...

successivamente ho modificato il codice per poter muovere i due assi dello stabilizzatore dji...

successivamente ho "portato" il codice su un esp32 (penso ad una versione wifi) per muoversi nel simulatore e questo funziona...

ora non capisco perche invece la versione esp32 per lo stabilizzatore non va... cioè mi va solo un asse, quello orizzontale... ma il tilt proprio non vuole...

allego un po di cose qui e nel prossimo post per questione di troppi caratteri:

codice arduino mega simulatore

float pulsesX, XA_SIG=0, XB_SIG=1, pulsesY, YA_SIG=0, YB_SIG=1;

void setup(){
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
Serial.begin(115200);
delay(100);
Serial.println(“Connected”);
}//setup

void loop(){
Serial.print(“x”);
Serial.print(pulsesX);
Serial.print(“y”);
Serial.print(pulsesY);
Serial.println(“end”);
delay(20);
}

//X-Axis

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);
}

//Y-Axis

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);
}

segue...

cepics · April 7, 2020, 6:37pm

... segue

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);
}

segue ancora...

cepics · April 7, 2020, 6:37pm

....segue

codice esp32 simulatore


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

ESP32Encoder encoderX;
ESP32Encoder encoderY;

float pulsesX, pulsesY;

void setup() {
  M5.begin();

  M5.update();

  Serial.begin(115200);

  // 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(34, 35);
  // probabilmente per invertire la direzione si puo invertire qui i pin


}

void loop() {

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

  Serial.print("x");
  Serial.print(pulsesX);
  Serial.print("y");
  Serial.print(pulsesY);
  Serial.println("end");
  delay(20);
}

codice esp32 stabilizzatore(che non funzia il tilt)


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

ESP32Encoder encoderX;
ESP32Encoder encoderY;

#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;

float pulsesX, pulsesY;

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

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

int potiXX;
int potiYY;



bool PANTILTtoggle = 0;


void setup() {
  M5.begin();

  if (digitalRead(BUTTON_A_PIN) == 0) {
    Serial.println("Will Load menu binary");
    updateFromFS(SD);
    ESP.restart();
  }

  // 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();

  // 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;


  if (M5.BtnA.isPressed()) {
    delay(200);

    if (PANTILTtoggle) {
      potiXX = potiXX + 10;
    } else {
      potiYY = potiYY + 10;
    }
  }

  if (M5.BtnB.isPressed()) {
    delay(200);
    PANTILTtoggle = !PANTILTtoggle;

  }

  if (M5.BtnC.isPressed()) {

    delay(200);

    if (PANTILTtoggle) {
      potiXX = potiXX - 10;
    } else {
      potiYY = potiYY - 10;
    }
  }

  potiX = map(potiXX, 0, 1024, -200, 200);
  potiY = map(potiYY, 0, 1024, -200, 200);


  M5.update();

  sendX = 1023 + potiX * xPassed / timePassed;
  sendY = 1023 + potiY * 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();
}

collegamenti mega simulatore

collegamenti mega stabilizzatore

collegamenti esp32 dovrei andare a seguire le tracce del pcb che ho fatto.. cmq non dubito di questi collegamenti in quanto nel simulatore gli encoder vanno bene tutti e due...

cepics · April 9, 2020, 2:32pm

nobody?

cepics · April 10, 2020, 7:02pm

dopo quanti giorni senza risposta posso crossPostare nel forum galattico?

gpb01 · April 10, 2020, 7:08pm

cepics:
dopo quanti giorni senza risposta posso crossPostare nel forum galattico?

... direi che puoi farlo ... non mi sembra che qui qualcuno sappia aiutarti ... :

Guglielmo

Topic		Replies	Views
Why does the arm servo moveee? General Guidance	17	206	March 25, 2026
My nema 17 motor isnt working with my a4988 and esp32 Motors, Mechanics, Power and CNC	64	933	June 12, 2026
ESP32 D1 Mini + BT + Servo Deutsch	37	522	January 11, 2025
Will this rotary encoder work with Arduino? Programming	82	1101	February 17, 2025
Den vorhandenen Winkel von Servos beim Einschalten lesen Deutsch	46	803	September 28, 2025

esp32 BMC_SBUS porting

Related topics