Englebert
2 years ago
7 changed files with 375 additions and 25 deletions
-
2platformio.ini
-
BINsrc/.main.cpp.swp
-
BINsrc/.main.h.swp
-
171src/main.cpp
-
8src/main.h
-
140src/sbus.cpp
-
77src/sbus.h
@ -0,0 +1,140 @@ |
|||||
|
// REFERENCES:
|
||||
|
// https://www.circuitbasics.com/basics-uart-communication/
|
||||
|
// https://www.analog.com/en/analog-dialogue/articles/uart-a-hardware-communication-protocol.html
|
||||
|
// https://github.com/uzh-rpg/rpg_quadrotor_control/wiki/SBUS-Protocol
|
||||
|
// https://github.com/plerup/espsoftwareserial/blob/main/src/SoftwareSerial.cpp
|
||||
|
// https://lucidar.me/en/serialib/most-used-baud-rates-table/
|
||||
|
|
||||
|
|
||||
|
// SBUS data structure
|
||||
|
#define RC_CHANS 18
|
||||
|
#define RC_CHANNEL_MIN 990
|
||||
|
#define RC_CHANNEL_MAX 2010
|
||||
|
|
||||
|
|
||||
|
#include "sbus.h"
|
||||
|
|
||||
|
|
||||
|
uint rcChannels[SBUS_CHANNEL_NUMBER]; |
||||
|
uint32_t sbusTime = 0; |
||||
|
|
||||
|
SoftwareSerial swSer1; |
||||
|
|
||||
|
SBUS::SBUS(void) { |
||||
|
} |
||||
|
|
||||
|
void SBUS::begin(void) { |
||||
|
pinMode(SBUS_PORT, OUTPUT); |
||||
|
|
||||
|
// ON it first as IDLE
|
||||
|
//// GPOS = (1 << SBUS_PORT);
|
||||
|
GPIO.out_w1ts = (1 << SBUS_PORT); |
||||
|
|
||||
|
// swSer1.begin(200000, SWSERIAL_8E2, SBUS_PORT, SBUS_PORT, false, 264);
|
||||
|
// swSer1.begin(100000, SWSERIAL_8E2, SBUS_PORT, SBUS_PORT);
|
||||
|
|
||||
|
// Default to SBUS FAST protocol
|
||||
|
/****
|
||||
|
if(sbus_type) { |
||||
|
swSer1.begin(200000, SWSERIAL_8E2, SBUS_PORT, SBUS_PORT); |
||||
|
} else { |
||||
|
swSer1.begin(100000, SWSERIAL_8E2, SBUS_PORT, SBUS_PORT); |
||||
|
} |
||||
|
****/ |
||||
|
swSer1.begin(100000, SWSERIAL_8E2, SBUS_PORT, SBUS_PORT); |
||||
|
swSer1.enableTx(true); |
||||
|
// swSer1.enableIntTx(true);
|
||||
|
|
||||
|
// Initialize to 1000
|
||||
|
for(uint8_t i = 0; i < SBUS_CHANNEL_NUMBER; i++) { |
||||
|
rcCommand[i] = 1000; |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
void SBUS::run(void) { |
||||
|
} |
||||
|
|
||||
|
uint32_t last_profile = 0; |
||||
|
uint32_t total_high = 0; |
||||
|
uint32_t total_low = 0; |
||||
|
|
||||
|
// Sending SBUS packets
|
||||
|
void SBUS::writePackets(void) { |
||||
|
if(signalNotOK) { |
||||
|
rcCommand[2] = 900; |
||||
|
rcCommand[3] = 900; |
||||
|
rcCommand[1] = 900; |
||||
|
rcCommand[0] = 900; |
||||
|
} |
||||
|
|
||||
|
if((uint32_t)millis() - last_sent > SBUS_UPDATE_RATE) { |
||||
|
sbusPreparePacket(rcCommand, signalNotOK, false); |
||||
|
swSer1.write(sbusPacket, SBUS_PACKET_LENGTH); |
||||
|
sbus_packets_sent_raw++; |
||||
|
last_sent = millis(); |
||||
|
} |
||||
|
|
||||
|
// DEBUG ONLY
|
||||
|
// if((uint32_t)(millis() - last_profile) > 1000) {
|
||||
|
// Serial.printf("Cmd: %d Signal: %d Sent: %d NRF: %d\r\n", rcCommand[2], signalNotOK, sbus_packets_sent, nrf_packets_received);
|
||||
|
// last_profile = millis();
|
||||
|
// }
|
||||
|
|
||||
|
} |
||||
|
|
||||
|
|
||||
|
// Delay CPU Cycles
|
||||
|
void SBUS::delay_cycles(uint16_t cpucycles) { |
||||
|
uint32_t cpucycle_start = ESP.getCycleCount(); |
||||
|
while((uint32_t)(ESP.getCycleCount() - cpucycle_start) < cpucycles) {} |
||||
|
} |
||||
|
|
||||
|
|
||||
|
// Preparing SBUS packets
|
||||
|
void SBUS::sbusPreparePacket(uint16_t channels[], bool isSignalLoss, bool isFailsafe){ |
||||
|
static int output[SBUS_CHANNEL_NUMBER] = {0}; |
||||
|
|
||||
|
/*
|
||||
|
* Map 1000-2000 with middle at 1500 chanel values to |
||||
|
* 173-1811 with middle at 992 S.BUS protocol requires |
||||
|
*/ |
||||
|
for(uint8_t i = 0; i < SBUS_CHANNEL_NUMBER; i++) { |
||||
|
output[i] = map(channels[i], RC_CHANNEL_MIN, RC_CHANNEL_MAX, SBUS_MIN_OFFSET, SBUS_MAX_OFFSET); |
||||
|
} |
||||
|
|
||||
|
uint8_t stateByte = 0x00; |
||||
|
if (isSignalLoss) { |
||||
|
stateByte |= SBUS_STATE_SIGNALLOSS; |
||||
|
} |
||||
|
if (isFailsafe) { |
||||
|
stateByte |= SBUS_STATE_FAILSAFE; |
||||
|
} |
||||
|
sbusPacket[0] = SBUS_FRAME_HEADER; //Header
|
||||
|
|
||||
|
sbusPacket[1] = (uint8_t) (output[0] & 0x07FF); |
||||
|
sbusPacket[2] = (uint8_t) ((output[0] & 0x07FF)>>8 | (output[1] & 0x07FF)<<3); |
||||
|
sbusPacket[3] = (uint8_t) ((output[1] & 0x07FF)>>5 | (output[2] & 0x07FF)<<6); |
||||
|
sbusPacket[4] = (uint8_t) ((output[2] & 0x07FF)>>2); |
||||
|
sbusPacket[5] = (uint8_t) ((output[2] & 0x07FF)>>10 | (output[3] & 0x07FF)<<1); |
||||
|
sbusPacket[6] = (uint8_t) ((output[3] & 0x07FF)>>7 | (output[4] & 0x07FF)<<4); |
||||
|
sbusPacket[7] = (uint8_t) ((output[4] & 0x07FF)>>4 | (output[5] & 0x07FF)<<7); |
||||
|
sbusPacket[8] = (uint8_t) ((output[5] & 0x07FF)>>1); |
||||
|
sbusPacket[9] = (uint8_t) ((output[5] & 0x07FF)>>9 | (output[6] & 0x07FF)<<2); |
||||
|
sbusPacket[10] = (uint8_t) ((output[6] & 0x07FF)>>6 | (output[7] & 0x07FF)<<5); |
||||
|
sbusPacket[11] = (uint8_t) ((output[7] & 0x07FF)>>3); |
||||
|
sbusPacket[12] = (uint8_t) ((output[8] & 0x07FF)); |
||||
|
sbusPacket[13] = (uint8_t) ((output[8] & 0x07FF)>>8 | (output[9] & 0x07FF)<<3); |
||||
|
sbusPacket[14] = (uint8_t) ((output[9] & 0x07FF)>>5 | (output[10] & 0x07FF)<<6); |
||||
|
sbusPacket[15] = (uint8_t) ((output[10] & 0x07FF)>>2); |
||||
|
sbusPacket[16] = (uint8_t) ((output[10] & 0x07FF)>>10 | (output[11] & 0x07FF)<<1); |
||||
|
sbusPacket[17] = (uint8_t) ((output[11] & 0x07FF)>>7 | (output[12] & 0x07FF)<<4); |
||||
|
sbusPacket[18] = (uint8_t) ((output[12] & 0x07FF)>>4 | (output[13] & 0x07FF)<<7); |
||||
|
sbusPacket[19] = (uint8_t) ((output[13] & 0x07FF)>>1); |
||||
|
sbusPacket[20] = (uint8_t) ((output[13] & 0x07FF)>>9 | (output[14] & 0x07FF)<<2); |
||||
|
sbusPacket[21] = (uint8_t) ((output[14] & 0x07FF)>>6 | (output[15] & 0x07FF)<<5); |
||||
|
sbusPacket[22] = (uint8_t) ((output[15] & 0x07FF)>>3); |
||||
|
sbusPacket[23] = stateByte; //Flags byte
|
||||
|
sbusPacket[24] = SBUS_FRAME_FOOTER; //Footer
|
||||
|
} |
||||
|
|
||||
|
SBUS sbus; |
@ -0,0 +1,77 @@ |
|||||
|
#ifndef SBUS_H_ |
||||
|
#define SBUS_H_ |
||||
|
|
||||
|
// #define SBUS_MIN_OFFSET 173 |
||||
|
// #define SBUS_MID_OFFSET 992 |
||||
|
// #define SBUS_MAX_OFFSET 1811 |
||||
|
// #define SBUS_CHANNEL_NUMBER 16 |
||||
|
// #define SBUS_PACKET_LENGTH 25 |
||||
|
// #define SBUS_FRAME_HEADER 0x0f |
||||
|
// #define SBUS_FRAME_FOOTER 0x00 |
||||
|
// #define SBUS_FRAME_FOOTER_V2 0x04 |
||||
|
// #define SBUS_STATE_FAILSAFE 0x08 |
||||
|
// #define SBUS_STATE_SIGNALLOSS 0x04 |
||||
|
// #define SBUS_UPDATE_RATE 15 // ms |
||||
|
// #define SBUS_PORT 16 // Trigger pulse from the port |
||||
|
|
||||
|
// FAST SBUS TIMING (Theoritically 50% timing) |
||||
|
#define SBUS_MIN_OFFSET 173 |
||||
|
#define SBUS_MID_OFFSET 992 |
||||
|
#define SBUS_MAX_OFFSET 1811 |
||||
|
#define SBUS_CHANNEL_NUMBER 16 |
||||
|
#define SBUS_PACKET_LENGTH 25 |
||||
|
#define SBUS_FRAME_HEADER 0x0f |
||||
|
#define SBUS_FRAME_FOOTER 0x00 |
||||
|
#define SBUS_FRAME_FOOTER_V2 0x04 |
||||
|
#define SBUS_STATE_FAILSAFE 0x08 |
||||
|
#define SBUS_STATE_SIGNALLOSS 0x04 |
||||
|
#define SBUS_UPDATE_RATE 6 // ms |
||||
|
#define SBUS_PORT 16 // Trigger pulse from the port |
||||
|
|
||||
|
// Enumerators - User Defined |
||||
|
enum rc { |
||||
|
ROLL, |
||||
|
PITCH, |
||||
|
THROTTLE, |
||||
|
YAW, |
||||
|
AUX1, |
||||
|
AUX2, |
||||
|
AUX3, |
||||
|
AUX4, |
||||
|
AUX5, |
||||
|
AUX6, |
||||
|
AUX7, |
||||
|
AUX8, |
||||
|
AUX9 |
||||
|
}; |
||||
|
|
||||
|
|
||||
|
#include <Arduino.h> |
||||
|
#include <SoftwareSerial.h> |
||||
|
|
||||
|
class SBUS { |
||||
|
public: |
||||
|
bool sbus_type = false; |
||||
|
bool signalNotOK = true; |
||||
|
bool invert; |
||||
|
uint16_t rcCommand[18]; // Signal to ESC |
||||
|
uint8_t sbusPacket[SBUS_PACKET_LENGTH]; |
||||
|
uint32_t last_sent = 0; |
||||
|
|
||||
|
int16_t sbus_packets_sent_raw = 0; |
||||
|
int16_t sbus_packets_sent = 0; |
||||
|
|
||||
|
SBUS(void); |
||||
|
void begin(void); |
||||
|
void run(void); |
||||
|
void sbusPreparePacket(uint16_t channels[], bool isSignalLoss, bool isFailsafe); |
||||
|
void writePackets(void); |
||||
|
|
||||
|
void delay_cycles(uint16_t cpucycles); |
||||
|
|
||||
|
private: |
||||
|
uint32_t last_wifi_check, wifi_check_duration; |
||||
|
}; |
||||
|
|
||||
|
extern SBUS sbus; |
||||
|
#endif |
Write
Preview
Loading…
Cancel
Save
Reference in new issue