Englebert
2 years ago
7 changed files with 66 additions and 236 deletions
-
4platformio.ini
-
BINsrc/.main.cpp.swp
-
BINsrc/.main.h.swp
-
71src/main.cpp
-
10src/main.h
-
140src/sbus.cpp
-
77src/sbus.h
@ -1,140 +0,0 @@ |
|||
// 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; |
|||
@ -1,77 +0,0 @@ |
|||
#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