// SBUS decoder
// SBUS into RX0 via transistor inverter
// I2C OLED display on A4/A5
//
// ceptimus June 2020 https://mode-zero.uk/viewtopic.php?f=42&t=1010&sid=40ed83fbcefd5f4429024c9efed70858
//
// This sbusDecode4 version, February 2021 adds alternate display modes for the OLED
#include "display.h"
// alter the MICROSECOND_SCALING ranges below to set the SBUS to Microseconds scaling you want
// order of numbers is SBUS low value, SBUS high value, Microseconds low value, Microseconds high value
// commas between the numbers and no other punctuation or comments on the line
// the low and high SBUS values 150, 1901, are what my Orange 3-channel + SBUS receiver gives out - but different manufacturers use different ranges
#define MICROSECOND_SCALING 172, 1811, 1000, 2000 // FrSky receivers will output a range of 172 - 1811
uint8_t frame[24]; // SBUS received bytes. 1st byte of 25-byte frame is always 0x0F, and is not stored
char buffer[22]; // printing to OLED display
bool displayCleared;
// Display SDA = A4;
// Display SCL = A5;
int PPMWert [] = {1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500}; // Eingangswerte der Kanäle 1-8
//boolean Fehler = true; // Sammelfehler
boolean Fehler = false; // Sammelfehler
// Ansteuerung der PWM Module
const byte Motor_1_FWD = 7; // Motor 1 vorwärts
const byte Motor_1_REV = 8; // Motor 1 rückwärts
const byte Motor_1_PWM = 9; // PWM Signal zum Motor 1 - Timer 1
const byte Motor_2_PWM = 10; // PWM Signal zum Motor 2 - Timer 1
const byte Motor_2_FWD = A3; // Motor 2 vorwärts
const byte Motor_2_REV = 12; // Motor 2 rückwärts
const byte Motor_3_FWD = 2; // Kanone heben
const byte Motor_3_REV = 4; // Kanone senken
const byte Motor_3_PWM = 3; // PWM Signal zum Motor 3 - Timer 2
const byte Motor_4_FWD = 2; // Turm rechts drehen
const byte Motor_4_REV = 4; // Turm links drehen
const byte Motor_4_PWM = 11; // PWM Signal zum Motor 4 - Timer 2
const byte OffsetPin_1 = A7; //Analgoger Eingang Offset Motor 1 zum Ableich der Ketten
// Debug Werte ob der Ausgang geschaltet ist
boolean M1_FWD = false;
boolean M1_REV = false;
boolean M2_FWD = false;
boolean M2_REV = false;
//Variablen für die PWM Regelung und Lenkfaktor
boolean Vorwaerts = false;
boolean Rueckwaerts = false;
boolean Neutral = true;
int PWMsignal = 0;
int PWMrechts = 0;
int PWMlinks = 0;
int PWM_rechts = 0;
int PWM_links = 0;
int Steuerrechts = 0;
int Steuerlinks = 0;
int Aufloesung_PWM = 0;
unsigned long Korrektur = 0;
int Offset_1 = 0; // Offset Motor 1 zum Ableich der Ketten
// Geschwindigkeit - Fahrhebel
int Fahrhebel = 0;
// Steuerung - rechts/links
int Steuerung =0;
//Bremslichtsteuerung
const byte LEDrt = 13; // Ausgang ist an Pin13 - Bremslicht rot
boolean Halt = 0; // Nullstellung wird erkannt
boolean vorher_Halt = 0; // Nullstellung vorher
unsigned long Bremslicht_aus =0;
unsigned long Bremslicht_ein =0;
boolean Bremslicht;
// Fahrlichtsteuerung
const byte Licht = 5; // Ausgang - Fahrlicht ein
int Lichttimer = 0;
boolean Lichtstatus = false;
boolean Umschaltsperre = false;
//xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
void setup() {
_display::start();
displayBackground();
// SBUS runs at 100kBaud 8E2. Each 25-byte frame, without gaps, takes 3000us
Serial.begin(100000, SERIAL_8E2);
//Deklaration Ausgangstreiber für die Motoren
pinMode(Motor_1_PWM, OUTPUT);
pinMode(Motor_2_PWM, OUTPUT);
analogWrite(Motor_1_PWM, 0); //Setzt die PWM wirklich auf 0
analogWrite(Motor_2_PWM, 0); //dito
pinMode (Motor_1_FWD, OUTPUT);
pinMode (Motor_1_REV, OUTPUT);
pinMode (Motor_2_FWD, OUTPUT);
pinMode (Motor_2_REV, OUTPUT);
digitalWrite (Motor_1_FWD, LOW);
digitalWrite (Motor_1_REV, LOW);
digitalWrite (Motor_2_FWD, LOW);
digitalWrite (Motor_2_REV, LOW);
//Deklaration Ausgangstreiber für die Lampen
pinMode (Licht, OUTPUT); // Pin 5 - Fahrlicht ein
pinMode (LEDrt, OUTPUT); // Pin 13 - Bremslicht rot
digitalWrite (Licht, LOW);
digitalWrite (LEDrt, LOW);
// Timer 1 ist ein 16 Bit Timer! - der Timer wird auch von der Servo Libary verwendet!
// Fast PWM Mode 14, variable Frequnez, Auflösung wie ICR1, Teiler 1
// Beispiel 10 kHz: ICR1 = (fclock/fpwm)-1 = (16000000 Hz/10000 Hz)-1 =1599
TCCR1A = B00000010;
TCCR1B = B00011001;
//ICR1 = 1600; // 10 kHz
//Aufloesung_PWM = 1600; //10kHz
ICR1 = 1000; // 16 kHz
Aufloesung_PWM = 1000; //16 kHz
}
//xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
void displayBackground() {
_display::fillDisplay(' ');
_display::displayStringSmall(0, 0, F("Ch"), 0);
_display::displayStringSmall(0, 18, F("ms"), 0);
// for (int row = 1; row < 7; row++) { // alle 6 Reihen anzeigen
for (int row = 1; row < 4; row++) { // nur 3 Reihen anzeigen
int i = row * 3 - 2; // channel number label: 1, 4, 7, (10, 13, 16)
sprintf(buffer, "%2d:", i);
_display::displayStringSmall(row, 0, buffer, 0);
}
// "No SBUS Signal" is only if there phisical input, timeout will trigger the failsafe
_display::displayStringLarge(2, 3, F(" No SBUS "), 1);
_display::displayStringLarge(4, 3, F(" Signal "), 1);
_display::displayStringSmall(7, 0, F(" ceptimus Feb 2021 "), 1);
displayCleared = true;
_display::displayStringSmall(0, 3, F("(PPM \x7Fs)"), 0); // character: x7F = µ
//showDisplayMode();
}
int channel(int ch) { // extract 11-bit channel[ch] value from frame. ch 0-15
int k = 0; // offset into frame array: 0 for channels 0-7, 12 for channels 8-15
switch (ch % 8) { // pattern repeats (except for k-offset) after 8 channels
case 0:
return (int)frame[0+k] | ((((int)frame[1+k]) & 0x07) << 8);
case 1:
return ((int)(frame[1+k] & 0xF8) >> 3) | ((((int)frame[2+k]) & 0x3F) << 5);
case 2:
return ((int)(frame[2+k] & 0xC0) >> 6) | ((((int)frame[3+k])) << 2) | ((((int)frame[4+k]) & 0x01) << 10);
case 3:
return ((int)(frame[4+k] & 0xFE) >> 1) | ((((int)frame[5+k]) & 0x0F) << 7);
case 4:
return ((int)(frame[5+k] & 0xF0) >> 4) | ((((int)frame[6+k]) & 0x7F) << 4);
case 5:
return ((int)(frame[6+k] & 0x80) >> 7) | ((((int)frame[7+k])) << 1) | ((((int)frame[8+k]) & 0x03) << 9);
case 6:
return ((int)(frame[8+k] & 0xFC) >> 2) | ((((int)frame[9+k]) & 0x1F) << 6);
case 7:
return ((int)(frame[9+k] & 0xE0) >> 5) | (((int)frame[10+k]) << 3);
}
return -1; // execution never reaches here, but this supresses a compliler warning
}
void loop() {
static uint32_t previousFrameReceivedAt = 0UL; // timing SBUS frame intervals
static uint32_t lastDisplayUpdate = 0UL; // for slowing display update to make more readable
bool frameReceived = false;
uint32_t frameBeganAt = micros();
while(Serial.available()) { // try to receive a 25-byte frame that begins with 0x0F and ends with 0x00
int b = Serial.read(); // scan until an 0x0F byte is received, or the receive buffer is empty
if (b != 0x0F) {
frameBeganAt = micros();
continue;
}
frame[23] = 0xFF; // a good received frame overwrites this with 0x00
// now attempt to receive 24 bytes, or time out and abort after 4ms
int i = 0;
while (i < 24) {
if ((micros() - frameBeganAt) > 4000) {
break;
}
i += Serial.readBytes(frame + i, min(Serial.available(), 24 - i));
}
if (i == 24 && frame[23] == 0x000) {
frameReceived = true;
Fehler = false; // complete signal received
break;
}
}
// copy first 8 channels received in SBUS frame to CPPM outputs
uint32_t now = micros();
uint32_t elapsedMicros = now - previousFrameReceivedAt;
if (frameReceived) {
previousFrameReceivedAt = now;
int sbus[8];
for (int i = 0; i < 8; i++) { // take the first 8 SBUS signals and de-code it
sbus[i] = channel(i); // jump to sub-function "channel" to decode signal
PPMWert [i] = map(sbus[i], MICROSECOND_SCALING); // fill up Array with PPM values 0 to 8
}
// only refresh display three times per second - makes fast-changing values more readable
uint32_t e = now - lastDisplayUpdate;
if (e > 333333UL) { // update time of the display 33333µs = 333 ms
if (displayCleared) { // remove 'No SBUS message'
for (int i = 2; i < 7; i++) {
_display::displayStringSmall(i, 3, F(" "), 0);
}
}
// display latest time between frames to nearest 0.1ms
elapsedMicros += 50; // dividing by 100 to get 0.1ms, so add 50 to round to nearest
sprintf(buffer, "%4ld", elapsedMicros / 1000UL); // formatting done in two integer stages to avoid floats
buffer[4] = '.';
buffer[5] = (char)((elapsedMicros % 1000UL) / 100UL + '0');
buffer[6] = '\0'; // terminate string
_display::displayStringSmall(0, 11, buffer, 0); // (row, column, "Text", Background 0= black / 1 = white)
for (int i = 0; i < 8; i++) { // display only the first 8 channels for the CPPM signal
int sbus11bit = (i < 8) ? sbus[i] : channel(i);
int microsecs;
// may need to change scaling to suit different manufacturers
microsecs = map(sbus11bit, MICROSECOND_SCALING);
sprintf(buffer, "%5d", microsecs); // buffer is the variable to print
_display::displayStringSmall(i / 3 + 1, (i % 3) * 6 + 4, buffer, 0); // (row, column, "Text", Background 0= black / 1 = white)
}
_display::displayStringSmall(7, 0, frame[22] & 0x04 ? F("Frame lost ") : F(" "), 0); // Frame lost flag
_display::displayStringSmall(7, 11, frame[22] & 0x04 ? F(" Failsafe") : F(" "), 0); // Failsafe flag
lastDisplayUpdate = now;
displayCleared = false;
}
} else if ((elapsedMicros > 2000000UL) && !displayCleared) { // no frame during last 2 seconds
Fehler = true; // no signal received in the last 2 seconds
displayBackground(); // jump to display background
}
// xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
// Auswertung Fahrhebel wird Kanal 3 zugewiesen (+1 Offset)
Fahrhebel = PPMWert[2]; // der Fahrhebel wird Kanal 3 zugewiesen (+1 Offset)
if ((Fahrhebel <= 1450) &&! (Fehler == true)) { // Vorwärts &&! = UND NICHT
Vorwaerts = true; // Fahrstatus vorwärts für Ausgangstreiber
Rueckwaerts = false;
Neutral = false;
PWMsignal = map (Fahrhebel, 1000, 1450, 1000, 0); // der Bereich von 990 bis 1430 ms wird 0-1000 Schritten zugewiesen
}
if ((Fahrhebel >= 1550) &&! (Fehler == true)){ // Rückwärts
Vorwaerts = false;
Rueckwaerts = true; // Fahrstatus vorwärts für Ausgangstreiber
Neutral = false;
PWMsignal = map (Fahrhebel, 2000, 1550, 1000, 0); // der Bereich von 1950 bis 1510 ms wird 0-1000 Schritten zugewiesen
}
if (((Fahrhebel >= 1450) && (Fahrhebel <= 1550))|| (Fehler == true)) { //Halt
Vorwaerts = false;
Rueckwaerts = false;
Neutral = true; // Fahrstatus vorwärts für Ausgangstreiber
PWMsignal = 0; // alle PWM Signale werden gelöscht
}
// Auswertung Lenkung
Steuerung = PPMWert[1]; // die Lenkung wird Kanal 2 zugewiesen (+1 Offset)
if ((Steuerung <= 1450) &&! (Fehler == true)) { // links &&! = UND NICHT
Steuerrechts = 0; // rechten Korrekturfaktor ausschalten
Steuerlinks = map (Steuerung, 1000, 1450, 1000, 0); // links ist PPMsignal 1000- 1450, skaliert aus 0 bis 1000
Korrektur = (PWMsignal*(unsigned long)Steuerlinks)/1000; //unsigned long muss sein, sonst rechnet er wieder mit int. 1050 wegen Überlauf vom Servosignal
PWMrechts = (PWMsignal - Korrektur); // Motor 1
PWMlinks = PWMsignal; // Motor 2
}
if ((Steuerung >= 1550) &&! (Fehler == true)){ // rechts
Steuerlinks = 0; // linken Korrekturfaktor ausschalten
Steuerrechts = map (Steuerung, 2000, 1550, 1000, 0); // rechts ist PPMsignal 1550 - 2100, skaliert aus 0 bis 1000
Korrektur = (PWMsignal*(unsigned long)Steuerrechts)/1000; //unsigned long muss sein, sonst rechnet er wieder mit int. 1050 wegen Überlauf vom Servosignal
PWMlinks = (PWMsignal - Korrektur); // Motor 2
PWMrechts = PWMsignal; // Motor 1
}
if ((Steuerung >= 1450) && (Steuerung <= 1550)) { // jetzt ist Neutral -> gerade aus
Steuerlinks = 0; // linken Korrekturfaktor ausschalten
Steuerrechts = 0; // rechten Korrekturfaktor ausschalten
PWMrechts = PWMsignal; // Motor 1
PWMlinks = PWMsignal; // Motor 2
}
//Ausgangstreiber Vorwärts - Neutral - Rückwärts
if ((Vorwaerts == true) && (Steuerrechts <=799) && (Steuerlinks <=799) &&! (Fehler == true)) {
digitalWrite (Motor_1_FWD, HIGH); // Motor 1 = rechts
M1_FWD = true; // Debug Wert
digitalWrite (Motor_1_REV, LOW);
M1_REV = false; // Debug Wert
digitalWrite (Motor_2_FWD, HIGH); // Motor 2 = links
M2_FWD = true; // Debug Wert
digitalWrite (Motor_2_REV, LOW);
M2_REV = false; // Debug Wert
}
if ((Rueckwaerts == true) && (Steuerrechts <=799) && (Steuerlinks <=799) &&! (Fehler == true)) {
digitalWrite (Motor_1_FWD, LOW); // Motor 1 = rechts
M1_FWD = false; // Debug Wert
digitalWrite (Motor_1_REV, HIGH);
M1_REV = true;
digitalWrite (Motor_2_FWD, LOW); // Motor 2 = links
M2_FWD = false;
digitalWrite (Motor_2_REV, HIGH);
M2_REV = true;
}
if ((Neutral == true) || (Fehler == true)){
digitalWrite (Motor_1_FWD, LOW); // Motor 1 = rechts
M1_FWD = false; // Debug Wert
digitalWrite (Motor_1_REV, LOW);
M1_REV = false;
digitalWrite (Motor_2_FWD, LOW); // Motor 2 = links
M2_FWD = false;
digitalWrite (Motor_2_REV, LOW);
M2_REV = false;
}
//Ausgangstreiber PWM
Offset_1 = (analogRead (OffsetPin_1))/3; // Offsetwert für Motor 1, geteilt durch 3, als analoger Eingang vom Poti
if (Fehler == false) { // PWM Signal wird zugewiesen wenn kein Fehler anliegt
PWM_rechts = map (PWMrechts, 0, 1000, (Offset_1 + 400), Aufloesung_PWM); // die Motoren laufen erst bei ~ 60% PWM an (510 = 51%)
PWM_links = map (PWMlinks, 0, 1000, 510, Aufloesung_PWM);
}
else { // PWM Signal wird gelöscht wenn ein Fehler anliegt
PWM_rechts = 0;
PWM_links = 0;
}
analogWrite(Motor_1_PWM, PWM_rechts); // Setzt die PWM an Pin 9
analogWrite(Motor_2_PWM, PWM_links); // Setzt die PWM an Pin 10
// xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
// Ansteuerung des Bremslichts an Pin 13
if (vorher_Halt == false && Halt == true) // Abfrage ob vorher HAlt war
{ // vorher kein Halt -> jetzt Halt-> Bremslicht an
digitalWrite (LEDrt,HIGH); // LED Pin1 13 an
Bremslicht = true; // nur für Debug
Bremslicht_ein = millis(); // aktuellen Zeitstempel speichern
}
Bremslicht_aus = (millis () - Bremslicht_ein); // hier wird geprüft wie lange das Bremslicht schon an ist
if ((Bremslicht_aus > 10000) || Halt == false) { // wenn Bremslicht 10s an war oder wieder FWD bzw RVS, dann Bremslicht aus
digitalWrite (LEDrt,LOW); // LED Pin 13 aus
Bremslicht = false; // nur für Debug
}
vorher_Halt = Halt; // alten Halt Wert speichern
Halt = false; // aktuellen Haltwert zurück setzen
// xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
// Ansteuerung Fahrlicht, Schaltsignal wird gesetzt und gehalten
Lichttimer = PPMWert[4]; // der Fahrhebel wird Kanal 5 zugewiesen (+1 Offset)
if ((Lichttimer >= 1700) && (Umschaltsperre == false) && (Lichtstatus == false)) { // Knüppelposition erreicht und Umschaltsperre ist frei
digitalWrite (Licht, HIGH); // Licht an
Lichtstatus = true; // Status Licht setzen
Umschaltsperre = true; // Umschaltsperre einschalten, Knüppel muss erst mal wieder nach Null
}
if (Lichttimer <= 1550 && (Umschaltsperre == true)){ // Umschaltsperre ausschalten wenn der Knüppel in Null ist
Umschaltsperre = false;
}
if ((Lichttimer >= 1700) && (Umschaltsperre == false) && (Lichtstatus == true)) { //Licht ausschalten wenn der Knüppel noch mal betätigt wurde
digitalWrite (Licht, LOW); // Licht aus
Lichtstatus = false; // Status Licht setzen
Umschaltsperre = true; // Umschaltsperre einschalten, Knüppel muss erst mal wieder nach Null
}
// xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
}
