Skip to content

Commit

Permalink
Initial release
Browse files Browse the repository at this point in the history
  • Loading branch information
@garfield38
garfield38 committed Dec 15, 2014
1 parent 04bd304 commit ddf813f
Show file tree
Hide file tree
Showing 3 changed files with 437 additions and 0 deletions.
256 changes: 256 additions & 0 deletions DJI_DBUS.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,256 @@
/*
DJI DBUS data decoder library
(c) S. Driussi 20141215
Not for commercial use
Work started from mikeshub Futaba library
https://github.com/mikeshub/FUTABA_SBUS
Refer to naza_dbus_decoder_wiring.jpg diagram for proper connection
*/
#include "DJI_DBUS.h"

void DJI_DBUS::begin(){
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_channels[18] = {
1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,0,0};
int16_t loc_servos[18] = {
1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,0,0};
_serial.begin(BAUDRATE);

memcpy(sbusData,loc_sbusData,25);
memcpy(channels,loc_channels,18);
memcpy(servos,loc_servos,18);
failsafe_status = DBUS_SIGNAL_OK;
sbus_passthrough = 1;
toChannels = 0;
bufferIndex=0;
feedState = 0;
}

int16_t DJI_DBUS::Channel(uint8_t ch) {
// Read channel data
if ((ch>0)&&(ch<=16)){
return channels[ch-1];
}
else{
return 1023;
}
}
uint8_t DJI_DBUS::DigiChannel(uint8_t ch) {
// Read digital channel data
if ((ch>0) && (ch<=2)) {
return channels[15+ch];
}
else{
return 0;
}
}
void DJI_DBUS::Servo(uint8_t ch, int16_t position) {
// Set servo position
if ((ch>0)&&(ch<=16)) {
if (position>2048) {
position=2048;
}
servos[ch-1] = position;
}
}
void DJI_DBUS::DigiServo(uint8_t ch, uint8_t position) {
// Set digital servo position
if ((ch>0) && (ch<=2)) {
if (position>1) {
position=1;
}
servos[15+ch] = position;
}
}
uint8_t DJI_DBUS::Failsafe(void) {
return failsafe_status;
}

void DJI_DBUS::PassthroughSet(int mode) {
// Set passtrough mode, if true, received channel data is send to servos
sbus_passthrough = mode;
}

int DJI_DBUS::PassthroughRet(void) {
// Return current passthrough mode
return sbus_passthrough;
}
void DJI_DBUS::UpdateServos(void) {
// Send data to servos
// Passtrough mode = false >> send own servo data
// Passtrough mode = true >> send received channel data
uint8_t i;
if (sbus_passthrough==0) {
// clear received channel data
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;

// store servo data
for (i=0; i<176; i++) {
if (servos[ch] & (1<<bit_in_servo)) {
sbusData[byte_in_sbus] |= (1<<bit_in_sbus);
}
bit_in_sbus++;
bit_in_servo++;

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

// DigiChannel 1
if (channels[16] == 1) {
sbusData[23] |= (1<<0);
}
// DigiChannel 2
if (channels[17] == 1) {
sbusData[23] |= (1<<1);
}

// Failsafe
if (failsafe_status == DBUS_SIGNAL_LOST) {
sbusData[23] |= (1<<2);
}

if (failsafe_status == DBUS_SIGNAL_FAILSAFE) {
sbusData[23] |= (1<<2);
sbusData[23] |= (1<<3);
}
}
// send data out
//serialPort.write(sbusData,25);
//for (i=0;i<25;i++) {
// _serial.write(sbusData[i]);
//}
}
void DJI_DBUS::UpdateChannels(void) {
uint8_t i;
//uint8_t sbus_pointer = 0;
// clear channels[]
/*for (i=0; i<16; i++) {
channels[i] = 0;
}
// reset counters
byte_in_sbus = 1;
bit_in_sbus = 0;
ch = 0;
bit_in_channel = 0;
//this method is much slower than the other method
// process actual sbus data
for (i=0; i<176; i++) {
if (sbusData[byte_in_sbus] & (1<<bit_in_sbus)) {
channels[ch] |= (1<<bit_in_channel);
}
bit_in_sbus++;
bit_in_channel++;
if (bit_in_sbus == 8) {
bit_in_sbus =0;
byte_in_sbus++;
}
if (bit_in_channel == 11) {
bit_in_channel =0;
ch++;
}
}*/

channels[0] = ((sbusData[1]|sbusData[2]<< 8) & 0x07FF);
channels[1] = ((sbusData[2]>>3|sbusData[3]<<5) & 0x07FF);
channels[2] = ((sbusData[3]>>6|sbusData[4]<<2|sbusData[5]<<10) & 0x07FF);
channels[3] = ((sbusData[5]>>1|sbusData[6]<<7) & 0x07FF);
channels[4] = ((sbusData[6]>>4|sbusData[7]<<4) & 0x07FF);
channels[5] = ((sbusData[7]>>7|sbusData[8]<<1|sbusData[9]<<9) & 0x07FF);
channels[6] = ((sbusData[9]>>2|sbusData[10]<<6) & 0x07FF);
channels[7] = ((sbusData[10]>>5|sbusData[11]<<3) & 0x07FF); // & the other 8 + 2 channels if you need them
#ifdef ALL_CHANNELS
channels[8] = ((sbusData[12]|sbusData[13]<< 8) & 0x07FF);
channels[9] = ((sbusData[13]>>3|sbusData[14]<<5) & 0x07FF);
channels[10] = ((sbusData[14]>>6|sbusData[15]<<2|sbusData[16]<<10) & 0x07FF);
channels[11] = ((sbusData[16]>>1|sbusData[17]<<7) & 0x07FF);
channels[12] = ((sbusData[17]>>4|sbusData[18]<<4) & 0x07FF);
channels[13] = ((sbusData[18]>>7|sbusData[19]<<1|sbusData[20]<<9) & 0x07FF);
channels[14] = ((sbusData[20]>>2|sbusData[21]<<6) & 0x07FF);
channels[15] = ((sbusData[21]>>5|sbusData[22]<<3) & 0x07FF);
#endif
// DigiChannel 1
/*if (sbusData[23] & (1<<0)) {
channels[16] = 1;
}
else{
channels[16] = 0;
}
// DigiChannel 2
if (sbusData[23] & (1<<1)) {
channels[17] = 1;
}
else{
channels[17] = 0;
}*/

for (i=0; i<8; i++) {
channels[i] = map(channels[i],364,1684,1000,2000);
}
// Failsafe
failsafe_status = DBUS_SIGNAL_OK;
if (sbusData[23] & (1<<2)) {
failsafe_status = DBUS_SIGNAL_FAILSAFE;
}
if (sbusData[23] & (1<<3)) {
failsafe_status = DBUS_SIGNAL_LOST;
}

}
void DJI_DBUS::FeedLine(void){
if (_serial.available() > 24){
while(_serial.available() > 0){
inData = _serial.read();
switch (feedState){
case 0:
if (inData != 0x0f){
while(_serial.available() > 0){//read the contents of in buffer this should resync the transmission
inData = _serial.read();
}
return;
}
else{
bufferIndex = 0;
inBuffer[bufferIndex] = inData;
inBuffer[24] = 0xff;
feedState = 1;
}
break;
case 1:
bufferIndex ++;
inBuffer[bufferIndex] = inData;
if (bufferIndex < 24 && _serial.available() == 0){
feedState = 0;
}
if (bufferIndex == 24){
feedState = 0;
if (inBuffer[0]==0x0f && inBuffer[24] == 0x00){
memcpy(sbusData,inBuffer,25);
toChannels = 1;
}
}
break;
}
}
}
}

60 changes: 60 additions & 0 deletions DJI_DBUS.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,60 @@
/*
DJI DBUS data decoder library
(c) S. Driussi 20141215
Not for commercial use
Work started from mikeshub Futaba library
https://github.com/mikeshub/FUTABA_SBUS
Refer to naza_dbus_decoder_wiring.jpg diagram for proper connection
*/
#ifndef DJI_DBUS_h
#define DJI_DBUS_h

#include <Arduino.h>


#define DBUS_SIGNAL_OK 0x00
#define DBUS_SIGNAL_LOST 0x01
#define DBUS_SIGNAL_FAILSAFE 0x03
#define BAUDRATE 100000
#define port Serial1
//#define ALL_CHANNELS


class DJI_DBUS
{
public:
DJI_DBUS(HardwareSerial & serial) : _serial (serial) {}
uint8_t sbusData[25];
int16_t channels[18];
int16_t servos[18];
uint8_t failsafe_status;
int sbus_passthrough;
int toChannels;
void begin();
int16_t Channel(uint8_t ch);
uint8_t DigiChannel(uint8_t ch);
void Servo(uint8_t ch, int16_t position);
void DigiServo(uint8_t ch, uint8_t position);
uint8_t Failsafe(void);
void PassthroughSet(int mode);
int PassthroughRet(void);
void UpdateServos(void);
void UpdateChannels(void);
void FeedLine(void);
private:
HardwareSerial & _serial;
uint8_t byte_in_sbus;
uint8_t bit_in_sbus;
uint8_t ch;
uint8_t bit_in_channel;
uint8_t bit_in_servo;
uint8_t inBuffer[25];
int bufferIndex;
uint8_t inData;
int feedState;

};

#endif
Loading

0 comments on commit ddf813f

Please sign in to comment.