englebert
/
FPVBridge
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Removed SBUS with working i2C

master
Englebert 2 years ago
parent
ee897592f9
commit
70d5207d79
2 changed files with 416 additions and 33 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 388
      src/main.cpp
  2. 61
      src/main.h

388
src/main.cpp
View File

@ -3,6 +3,7 @@
void setup() {
Serial.begin(115200);
Serial.setDebugOutput(true);
// initialize EEPROM with predefined size
EEPROM.begin(EEPROM_SIZE);
@ -39,6 +40,61 @@ void setup() {
esp_wifi_set_promiscuous(false);
WiFi.disconnect();
// Setting up the I2C for sending data to Raspberry Pi (Ground Station)
// Wire1.begin(I2C_DEVICE_MAX_ADDRESS_RANGE-2, 18, 19, 0); // Need to change the address later.
// Wire1.onReceive(onI2C0Receive);
// Wire1.onRequest(onI2C0Request);
// Wire.begin((uint8_t)I2C_DEVICE_MAX_ADDRESS_RANGE-2, 21, 22, 0); // Need to change the address later.
// Wire.setPins(I2C_SDA, I2C_SCL);
// Wire.setClock(100000);
// bool ready_i2c = Wire1.begin((uint8_t) (I2C_DEVICE_MAX_ADDRESS_RANGE - 2), I2C_SDA, I2C_SDA, 0); // Need to change the address later.
// bool ready_i2c = Wire1.begin((uint8_t) (I2C_DEVICE_MAX_ADDRESS_RANGE - 4), 21, 22, 100000); // Need to change the address later.
// bool ready_i2c = Wire.begin();
// REF: https://github.com/espressif/arduino-esp32/blob/master/docs/source/api/i2c.rst#i2c-begin
Wire.onReceive(onI2C0Receive);
Wire.onRequest(onI2C0Request);
bool ready_i2c = Wire.begin(0x6D, I2C_SDA, I2C_SCL, 100000);
if(!ready_i2c) {
Serial.println("I2C slave init error. Rebooting in 5 seconds");
delay(5000);
ESP.restart();
} else {
i2c_status = true;
}
//// Wire.beginTransmission(I2C_DEVICE_MAX_ADDRESS_RANGE - 2);
for(int i = 0; i < 24; i++)
s_uRCChannelsOut[i] = 0;
pinMode(BUTTON_MENU, OUTPUT);
pinMode(BUTTON_BACK, OUTPUT);
pinMode(BUTTON_UP, OUTPUT);
pinMode(BUTTON_DOWN, OUTPUT);
digitalWrite(BUTTON_MENU, LOW);
digitalWrite(BUTTON_BACK, LOW);
digitalWrite(BUTTON_UP, LOW);
digitalWrite(BUTTON_DOWN, LOW);
// Initialize Button state
for(uint8_t i = 0; i < 16; i++)
rubybutton[i] = false;
// Initialize time
for(uint8_t i = 0; i < 16; i++)
rubybutton_last[i] = 0;
// Initialize SBUSData
for(uint8_t i = 0; i < 16; i++)
sbusData.ch[i] = 0;
sbusData.failsafe = false;
sbusData.ch17 = false;
sbusData.ch18 = false;
delay(100);
initESP_NOW();
@ -46,8 +102,7 @@ void setup() {
// Setting up SBUS
//// sbus.sbus_type = true; // True: FAST, False: SLOW
//// sbus.begin();
sbus_tx.Begin(); // SBUS object, writing SBUS
//// sbus_tx.Begin(); // SBUS object, writing SBUS
// Task Setup
xTaskCreatePinnedToCore(
@ -94,6 +149,7 @@ void setup() {
}
/**
void taskSBUS(void *pvParameters) {
(void) pvParameters;
@ -106,7 +162,7 @@ void taskSBUS(void *pvParameters) {
}
}
}
**/
void taskDisplay(void *pvParameters) {
(void) pvParameters;
@ -132,12 +188,17 @@ void taskDisplay(void *pvParameters) {
pulse_ms = 250;
}
/***
char msg[64];
sprintf(msg, "I2C Packets: %d\n", i2c_counter);
Serial.print(msg);
/***
// sprintf(msg, "Total Packets: %d Pulse: %d SBUS: %i", counter, pulse_ms, sbus.sbus_packets_sent);
sprintf(msg, "Total Packets: %d Pulse: %d", counter, pulse_ms);
Serial.println(msg);
***/
last_display = millis();
}
@ -167,8 +228,8 @@ void taskCounter(void *pvParameters) {
counter = counter_raw;
counter_raw = 0;
// sbus.sbus_packets_sent = sbus.sbus_packets_sent_raw;
// sbus.sbus_packets_sent_raw = 0;
i2c_counter = i2c_counter_raw;
i2c_counter_raw = 0;
if(counter == 0) {
signalNotOK = true;
@ -258,12 +319,13 @@ void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
}
void OnDataRecv(const uint8_t * mac_addr, const uint8_t *incomingData, int len) {
/***
/**
Serial.print(len);
Serial.print(" bytes of data received from : ");
printMAC(mac_addr);
Serial.println();
***/
**/
// Only allow address will be processed else ignore
bool allow_process = false;
if(
@ -290,18 +352,18 @@ void OnDataRecv(const uint8_t * mac_addr, const uint8_t *incomingData, int len)
memcpy(&incomingReadings, incomingData, sizeof(incomingReadings));
// Serial.println(incomingReadings.throttle);
sbus_data.failsafe = true;
sbus_data.ch17 = true;
sbus_data.ch18 = false;
sbusData.failsafe = true;
sbusData.ch17 = true;
sbusData.ch18 = false;
//// sbus.rcCommand[THROTTLE] = incomingReadings.throttle;
sbus_data.ch[0] = incomingReadings.throttle;
sbusData.ch[0] = incomingReadings.roll;
//// sbus.rcCommand[YAW] = incomingReadings.yaw;
sbus_data.ch[1] = incomingReadings.yaw;
sbusData.ch[1] = incomingReadings.pitch;
//// sbus.rcCommand[PITCH] = incomingReadings.pitch;
sbus_data.ch[2] = incomingReadings.pitch;
sbusData.ch[2] = incomingReadings.throttle;
//// sbus.rcCommand[ROLL] = incomingReadings.roll;
sbus_data.ch[3] = incomingReadings.roll;
sbusData.ch[3] = incomingReadings.yaw;
/**
sbus.rcCommand[AUX1] = incomingReadings.aux[0] ? 2000 : 1000; // 1
@ -314,19 +376,24 @@ void OnDataRecv(const uint8_t * mac_addr, const uint8_t *incomingData, int len)
sbus.rcCommand[AUX8] = incomingReadings.aux[7] ? 2000 : 1000; // 1
sbus.rcCommand[AUX9] = incomingReadings.aux[8] ? 2000 : 1000; // 1
**/
sbus_data.ch[4] = incomingReadings.aux[0] ? 2000 : 1000; // 1
sbus_data.ch[5] = incomingReadings.aux[1] ? 2000 : 1000; // 1
sbus_data.ch[6] = incomingReadings.aux[2] ? 2000 : 1000; // 1
sbus_data.ch[7] = incomingReadings.aux[3] ? 2000 : 1000; // 1
sbus_data.ch[8] = incomingReadings.aux[4] ? 2000 : 1000; // 1
sbus_data.ch[9] = incomingReadings.aux[5] ? 2000 : 1000; // 1
sbus_data.ch[10] = incomingReadings.aux[6] ? 2000 : 1000; // 1
sbus_data.ch[11] = incomingReadings.aux[7] ? 2000 : 1000; // 1
sbus_data.ch[12] = incomingReadings.aux[8] ? 2000 : 1000; // 1
sbusData.ch[4] = (incomingReadings.aux & 0x01) ? 2000 : 1000; // 1
sbusData.ch[5] = ((incomingReadings.aux >> 1) & 0x01) ? 2000 : 1000; // 1
sbusData.ch[6] = ((incomingReadings.aux >> 2) & 0x01) ? 2000 : 1000; // 1
sbusData.ch[7] = ((incomingReadings.aux >> 3) & 0x01) ? 2000 : 1000; // 1
sbusData.ch[8] = ((incomingReadings.aux >> 4) & 0x01) ? 2000 : 1000; // 1
sbusData.ch[9] = ((incomingReadings.aux >> 5) & 0x01) ? 2000 : 1000; // 1
sbusData.ch[10] = ((incomingReadings.aux >> 6) & 0x01) ? 2000 : 1000; // 1
sbusData.ch[11] = ((incomingReadings.aux >> 7) & 0x01) ? 2000 : 1000; // 1
sbusData.ch[12] = ((incomingReadings.aux >> 8) & 0x01) ? 2000 : 1000; // 1
/***
Serial.println(incomingReadings.rubybutton);
***/
// Set to false to make the sbus sending to FC or others.
signalNotOK = false;
counter_raw++;
frame_count++;
break;
@ -380,6 +447,231 @@ void OnDataRecv(const uint8_t * mac_addr, const uint8_t *incomingData, int len)
}
}
void onI2C0Request(void) {
//// Serial.println("onI2C0Request");
/**
s_iI2CLastResponseSentPosition++;
if(
s_iI2CLastResponseSentPosition >= MAX_BUFFER_SIZE ||
s_iI2CLastResponseSentPosition >= s_iI2CLastResponseLength
) {
// Unexpected request from master
s_iI2CLastResponseSentPosition = MAX_BUFFER_SIZE;
s_iI2CLastCommandReadPosition = 0;
s_uI2CLastCommandBuff[1] = 0;
if(s_iI2CLastResponseSentPosition >= MAX_BUFFER_SIZE)
Serial.println("Weird... >MAX_BUFFER_SIZE");
if(s_iI2CLastResponseSentPosition >= s_iI2CLastResponseLength)
Serial.println("Weird... >s_iI2CLastResponseLength");
Wire.write(0);
return;
}
if(s_uI2CLastCommandBuff[1] == I2C_COMMAND_ID_GET_FLAGS) {
// Send flags back in byte
Serial.print("Send flags back, byte");
Serial.print(s_iI2CLastResponseSentPosition + 1);
Serial.print(": ");
Serial.println((int) s_uI2CLastResponseBuff[s_iI2CLastResponseSentPosition]);
}
// Wire.slaveWrite(&s_uI2CLastResponseBuff[s_iI2CLastResponseSentPosition], 1);
Wire.slaveWrite(s_uI2CLastResponseBuff, s_iI2CLastResponseSentPosition);
// Wire.write(&s_uI2CLastResponseBuff[s_iI2CLastResponseSentPosition], 1);
// Wire1.write(s_uI2CLastResponseBuff, s_iI2CLastResponseLength);
**/
}
void parse_i2c_command(void) {
uint8_t crc = 0xFF;
// Firewall (CRC)
/**
for(uint16_t i = 0; i < i2c_received_length - 1; i++)
crc = s_crc_i2c_table[i2c_received[i] ^ crc];
if(crc != i2c_received[i2c_received_length]) {
Serial.printf("Invalid Command CRC: RX = 0x%2X # CRC = 0x%2X # RET = 0x%2X\n", i2c_received[1], i2c_received[i2c_received_length], crc);
return;
}
**/
i2c_send_length = 0;
// 0x01 - I2C_COMMAND_ID_GET_FLAGS
if(i2c_received[1] == I2C_COMMAND_ID_GET_FLAGS) {
Serial.println("Get flags command");
i2c_send[0] =
I2C_CAPABILITY_FLAG_RC_INPUT;
i2c_send[1] = 0;
i2c_send_length = 2;
// 0x20 - I2C_COMMAND_ID_SET_RC_INPUT_FLAGS
} else if(i2c_received[1] == I2C_COMMAND_ID_SET_RC_INPUT_FLAGS) {
Serial.println("Set RC Input Flags");
uint8_t flags = i2c_received[2];
if(flags & I2C_COMMAND_RC_FLAG_SBUS)
Serial.print("SBUS");
if(flags & I2C_COMMAND_RC_FLAG_IBUS)
Serial.print("IBUS");
if(flags & I2C_COMMAND_RC_FLAG_PPM)
Serial.print("PPM");
if(flags & I2C_COMMAND_RC_FLAG_INVERT_UART)
Serial.println(" INVERTED");
else
Serial.println();
i2c_send[0] = 0;
i2c_send_length = 1;
// 0x21 - I2C_COMMAND_ID_RC_GET_CHANNELS
} else if(i2c_received[1] == I2C_COMMAND_ID_RC_GET_CHANNELS) {
/**
Gets the current RC channels values from the slave device;
Master sends: 1 byte: command id;
Slave responds: 26 bytes: 1 byte flags, 1 byte frame number, 24 bytes for RC channels values (16 channels, 12 bits per channel, for 0...4095 posible values).
byte 0: flags: bit 0 is set if RC input failsafe event occured on the slave device.
byte 1: frame number: monotonically increasing on each received RC frame
byte 2 -17 - LSBits (8 bits) of each channel, from ch 1 to ch 16;
byte 18-25 - MSBits (4 bits) of each channel, from ch 1 to ch 16;
Unused channels should be set to zero.
**/
// Serial.println("RC get channels");
i2c_send[0] = 0; // SBUS is Fail Safe.. temporary
i2c_send[1] = frame_count;
// Channel [ 1 ~ 16 ] LSB @ byte 2 ~ 17
for(uint8_t i = 0; i < 16; i++)
i2c_send[2 + i] = sbusData.ch[i] & 0xFF;
// Channel [ 1 ~ 16 ] MSB @ byte 18 ~ 25
for(uint8_t i = 0; i < 16; i++) {
if((i % 2) == 0) {
i2c_send[18 + i/2] = (sbusData.ch[i] >> 8) & 0x0F;
} else {
i2c_send[18 + i/2] = (i2c_send[18 + i/2]) | ((sbusData.ch[i] >> 8) & 0x0F) << 4;
}
}
i2c_send_length = 26;
// 0x03 - I2C_COMMAND_ID_GET_NAME
} else if(i2c_received[1] == I2C_COMMAND_ID_GET_NAME) {
/**
Get name: master asks slave for the device name, to be used in the user interface;
Master sends: 1 byte: command id;
Slave responds: I2C_PROTOCOL_STRING_LENGTH bytes: null terminated string, padded with 0 up to I2C_PROTOCOL_STRING_LENGTH bytes;
**/
uint8_t device_name[] = "ESP32 Extender";
for(uint8_t i = 0; i < 14; i++) {
i2c_send[i] = device_name[i];
}
for(uint8_t i = 14; i < I2C_PROTOCOL_STRING_LENGTH; i++) {
i2c_send[i] = 0;
}
i2c_send_length = I2C_PROTOCOL_STRING_LENGTH;
} else {
Serial.print("Others: ");
for(uint16_t i = 0; i < i2c_received_length; i++)
Serial.printf("0x%2X ", i2c_received[i]);
Serial.println();
}
// Calculating CRC
if(i2c_send_length > 0) {
crc = 0xFF;
for(uint16_t i = 0; i < i2c_send_length; i++) {
crc = s_crc_i2c_table[i2c_send[i] ^ crc];
}
i2c_send[i2c_send_length++] = crc;
Wire.slaveWrite(i2c_send, i2c_send_length);
i2c_counter_raw++;
}
}
void onI2C0Receive(int len) {
int ch = Wire.read();
if(!header_detected) {
if(ch == 0xFF) {
header_detected = true;
} else {
return;
}
}
// Data...
i2c_received[i2c_received_length++] = ch;
/***
All commands sent by Ruby are in this format:
1 byte: start command header, always 0xFF;
2 byte: command id, see below;
N bytes: command data, size depending on command;
1 byte: CRC;
***/
/***
for(int i = 0; i <=i2c_received_length; i++) {
Serial.print(i2c_received[i], HEX); Serial.print(" ");
}
Serial.println();
***/
if(
i2c_received[1] == I2C_COMMAND_ID_GET_FLAGS ||
i2c_received[1] == I2C_COMMAND_ID_GET_NAME ||
i2c_received[1] == I2C_COMMAND_ID_GET_BUTTONS_EVENTS ||
i2c_received[1] == I2C_COMMAND_ID_GET_ROTARY_EVENTS ||
i2c_received[1] == I2C_COMMAND_ID_RC_GET_CHANNELS
) {
if(i2c_received_length == 2) {
parse_i2c_command();
// Reset back to starting point.
i2c_received_length = 0;
header_detected = false;
}
} else if(
i2c_received[1] == I2C_COMMAND_ID_SET_RC_INPUT_FLAGS
) {
if(i2c_received_length == 3) {
/**
for(int i = 0; i <=i2c_received_length; i++) {
Serial.print(i2c_received[i], HEX); Serial.print(" ");
}
Serial.println();
**/
parse_i2c_command();
// Reset back to starting point.
i2c_received_length = 0;
header_detected = false;
}
}
}
void initESP_NOW(){
// Init ESP-NOW
if(esp_now_init() != ESP_OK) {
@ -404,11 +696,51 @@ void initESP_NOW(){
void loop() {
if(!signalNotOK) {
/// sbus.writePackets();
if((uint32_t)(millis() - last_sbus) > 10) {
sbus_tx.data(sbus_data);
sbus_tx.Write();
if((uint32_t)(millis() - last_sent) > 10) {
// sbus_tx.data(sbus_data);
// sbus_tx.Write();
last_sbus = millis();
if((incomingReadings.rubybutton & 0x01) != rubybutton[0]) {
if((uint32_t)(millis() - rubybutton_last[0]) > 100) {
rubybutton[0] = incomingReadings.rubybutton & 0x01;
digitalWrite(BUTTON_MENU, rubybutton[0]);
rubybutton_last[0] = millis();
}
}
if(((incomingReadings.rubybutton >> 1) & 0x01) != rubybutton[1]) {
if((uint32_t)(millis() - rubybutton_last[1]) > 100) {
rubybutton[1] = (incomingReadings.rubybutton >> 1) & 0x01;
digitalWrite(BUTTON_BACK, rubybutton[1]);
rubybutton_last[1] = millis();
}
}
if(((incomingReadings.rubybutton >> 2) & 0x01) != rubybutton[2]) {
if((uint32_t)(millis() - rubybutton_last[2]) > 100) {
rubybutton[2] = (incomingReadings.rubybutton >> 2) & 0x01;
digitalWrite(BUTTON_UP, rubybutton[2]);
rubybutton_last[2] = millis();
}
}
if(((incomingReadings.rubybutton >> 3) & 0x01) != rubybutton[3]) {
if((uint32_t)(millis() - rubybutton_last[3]) > 100) {
rubybutton[3] = (incomingReadings.rubybutton >> 3) & 0x01;
digitalWrite(BUTTON_DOWN, rubybutton[3]);
rubybutton_last[2] = millis();
}
}
last_sent = millis();
}
}
/***
if(i2c_status) {
WireSlave.update();
delay(1);
}
***/
}

61
src/main.h
View File

@ -19,7 +19,10 @@
#include "esp_wifi.h"
#include <esp_now.h>
#include <WiFi.h>
#include "sbus.h"
// #include "sbus.h"
#include <Wire.h>
#include "i2c_protocols.h"
typedef struct struct_data {
uint8_t message_type;
@ -27,9 +30,19 @@ typedef struct struct_data {
uint16_t yaw;
uint16_t roll;
uint16_t pitch;
bool aux[9];
uint16_t aux;
// bool aux[9];
// bool rubybutton[9];
uint16_t rubybutton;
} struct_data;
typedef struct sbus_data {
bool failsafe;
bool ch17;
bool ch18;
uint16_t ch[16];
} struct_sbus;
typedef struct struct_pairing {
uint8_t message_type;
uint8_t channel;
@ -47,24 +60,59 @@ enum MessageType {
DATA
};
// For Ground Station Pi via I2C
#define MAX_BUFFER_SIZE 1024
#define I2C_SCL 22
#define I2C_SDA 21
#define I2C_SLAVE_ADDR 0x60
#define BUTTON_MENU 13
#define BUTTON_BACK 12
#define BUTTON_UP 14
#define BUTTON_DOWN 27
bool show_once = false;
bool header_detected = false;
uint8_t i2c_received[MAX_BUFFER_SIZE];
uint8_t i2c_send[MAX_BUFFER_SIZE];
uint16_t i2c_received_length = 0;
uint16_t i2c_send_length = 0;
uint8_t i2c_received_max = 0;
uint8_t frame_count = 0;
uint8_t s_uRCChannelsOut[24];
uint16_t s_iI2CLastCommandReadPosition = 0;
uint16_t s_iI2CLastCommandExpectedLength = 0;
uint8_t s_uI2CLastCommandBuff[MAX_BUFFER_SIZE];
uint8_t s_uI2CLastResponseBuff[MAX_BUFFER_SIZE];
int16_t s_iI2CLastResponseSentPosition = -1;
uint16_t s_iI2CLastResponseLength = 0;
bool rubybutton[16];
uint32_t rubybutton_last[16];
#define DEFAULT_CHANNEL 1
#define EEPROM_SIZE 7 // define the number of bytes you want to access
/* SBUS data */
bfs::SbusData sbus_data;
bfs::SbusTx sbus_tx(&Serial2, 16, 17, true);
// bfs::SbusData sbus_data;
// bfs::SbusTx sbus_tx(&Serial2, 16, 17, true, false);
/***
SbusTx(HardwareSerial *bus, const int8_t rxpin, const int8_t txpin,
const bool inv) : uart_(bus), inv_(inv), rxpin_(rxpin), txpin_(txpin)
****/
bool signalNotOK = false;
bool i2c_status = false;
uint16_t i2c_counter = 0;
uint16_t i2c_counter_raw = 0;
esp_now_peer_info_t slave;
int channel = 1;
uint32_t last_sbus = 0;
uint32_t last_sent = 0;
bool sync_mode = false;
struct_data incomingReadings;
struct_pairing pairingData;
struct_sbus sbusData;
/// uint8_t broadcast_address[] = { 0x78, 0x21, 0x84, 0x8d, 0xd9, 0x38 };
uint8_t broadcast_address[] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
@ -87,6 +135,9 @@ void printMAC(const uint8_t * mac_addr);
void initESP_NOW();
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status);
void OnDataRecv(const uint8_t * mac_addr, const uint8_t *incomingData, int len);
void onI2C0Receive(int len);
void onI2C0Request(void);
void parse_i2c_command(void);
#endif
Write Preview
Loading…
Cancel
Save