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.pngthis 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
-
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 portclass 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 channeluint8_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 channelif ((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 processfor (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 reverseattachInterrupt(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 reverseattachInterrupt(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 reverseattachInterrupt(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 reverseattachInterrupt(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 reverseattachInterrupt(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 reverseattachInterrupt(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 reverseattachInterrupt(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 reverseattachInterrupt(5, YB_RISE, RISING);
}
Hello! It looks like you're interested in this conversation, but you don't have an account yet.
Getting fed up of having to scroll through the same posts each visit? When you register for an account, you'll always come back to exactly where you were before, and choose to be notified of new replies (either via email, or push notification). You'll also be able to save bookmarks and upvote posts to show your appreciation to other community members.
With your input, this post could be even better 💗
Register Login