Arduino SBUS lichtmodule

sterretje · October 24, 2023, 1:57pm

Tijdens telefonisch overleg was besloten om het tonen van de kanaal informatie optioneel te maken. Je kunt zetten wat je wilt zien via de seriële monitor.

Formaat

C,n
of
c,n

'C' -> enable channel debug print
'c' -> disable channel debug print
n -> een nummer van 0 to 17 voor het kanaal

Er zijn twee functies toegevoegd om via de seriële poort (serial monitor) aan te geven wat in- en uitgeschakeld moet worden; beiden zijn gebaseerd op Serial Input Basics - updated. Je moet een keuze maken welke je wilt gebruiken.

recvWithEndMarker(). De meer natuurlijke manier waarbij je de data intypt en op <enter> drukt; niet vergeten om de 'line ending' in de seriële monitor op 'new line' te zetten.
recvWithStartEndMarkers(). Hierbij moet je de gegevens die je verstuurt tussen < en > plaatsen; je moet nog steeds op enter> drukken en je kunt de 'line ending' in de seriële monitor op 'no line ending' zetten.

Het onderstaande programma gebruikt nummer (2) die wordt aangeroepen vanuit loop().

Beide functies zetten een vlag newData die gebruikt wordt in de functie parseUserInput() om te kijken of er een compleet bericht is binnen gekomen.

parseUserInput() splitst het bericht op de comma, kijkt of alle gegevens geldig zijn en als dat zo is wordt er een bit in een variabele channelDebugEnable gezet of gewist; de eerste 18 bits zijn relevant en geven aan of kanaal info getoond moet worden of niet. channelDebugEnable wordt nu gebruikt in printChannels() om te bepalen of de data van een kanaal getoond moet worden.

/*
Functioneel werkend programma van https://forum.arduino.cc/t/arduino-sbus-lichtmodule/1176987/41 (eerste programma).
Bekend is dat alarmlichten niet juist werken.

1.1
Uitgebreid met gecorrigeerde versies van printRawBinary en printChannels.
1.2
buffer, channels, failsafe en lost veranderd naar globale variabelen
1.3
parseFutaba geimplementeerd
1.4
processChannels geimmplementeerd
1.5
enable / disable showing of individual channel show
*/


uint32_t channelDebugEnable;
const byte numChars = 32;
char receivedChars[numChars];
bool newData;


#include <SoftwareSerial.h>
//#include <SoftwareServo.h>
#include <Servo.h>
#include <ezLED.h>  // ezLED library

#define NUMELEMENTS(x) (sizeof(x) / sizeof(x[0]))

// variables moved from loop() to global
uint8_t buffer[25];
int channels[18];
int errors = 0;
bool failsafe = 0;
int lost = 0;

/*/#define RAWR 2
//#define RAWL 4
ezLED rawr(RAWR);  // create ezLED object 
ezLED rawl(RAWL);  // create ezLED object 
#define rxPin 19
*/

const uint8_t pinRawR = 2;
const uint8_t pinAchterlicht = 3;
const uint8_t pinRawL = 4;
const uint8_t pinAchteruitrijlicht = 5;
const uint8_t pinKoplamp = 6;
const uint8_t pinVerstralers = 7;

const uint8_t pinDbg2 = 8;
const uint8_t pinDbg3 = 9;
const int rawL_SHORT = 1000;
const int rawR_SHORT = 1000;
const int rawL_LONG = 2000;
const int rawR_LONG = 4000;

ezLED rawr(pinRawR);  // create ezLED object
ezLED rawl(pinRawL);  // create ezLED object

Servo myservo8;  // create servo object to control a servo

int pos = 0;  // variable to store the servo position
bool Brakelight = 0;
bool Koplamp = 0;
bool stadslichtenAan = 0;
bool grootlichtAan = 0;
int rawRAan = 0;
int rawLAan = 0;

int status_rawR;  // the current reading from the input pin
int status_rawL;  // the current reading from the input pin

unsigned long rawR_Press_Time = 0;
unsigned long rawR_Release_Time = 0;
unsigned long rawL_Press_Time = 0;
unsigned long rawL_Release_Time = 0;
int rawR_kort = 0;
int rawR_lang = 0;
bool rawL_kort = false;
bool rawL_lang = false;


//Not used in the Demo
//Servo myservo9;  // create servo object to control a servo
//Servo myservo10;  // create servo object to control a servo
//Servo myservo11;  // create servo object to control a servo
//SoftwareSerial InvertedSerialPort =  SoftwareSerial(rxPin, txPin, true); //(Tx,TX, Inverted mode ping this device uses inverted signaling
//For Piezo Buzzer

//***********************************************************************
//     Setup
//***********************************************************************
void setup()
{
  delay(2000);

  //The pins on the Arduino need to be defined. In this demo pin 9-13 are used. Pin 13 is the LED on the Arduino which is
  //a simple mehod for debugging. The demo turns this on when the Channel 1 (often used for throttle) exceeds a predefined SBUS level.
  pinMode(pinRawL, OUTPUT);               //Richtingaanwijzer links
  pinMode(pinAchterlicht, OUTPUT);        //achterlicht
  pinMode(pinRawR, OUTPUT);               //Richtingaanwijzer rechts
  pinMode(pinAchteruitrijlicht, OUTPUT);  //Achteruitrijlicht
  pinMode(pinKoplamp, OUTPUT);            //Koplampen
  pinMode(pinVerstralers, OUTPUT);        //Debug with LED
  pinMode(pinDbg2, OUTPUT);               //Debug with LED
  pinMode(pinDbg3, OUTPUT);               //Debug with LED
                                          //pinMode(NAW, OUTPUT);               //Debug with LED
                                          //pinMode(NAW, OUTPUT);               //Debug with LED
                                          //pinMode(NAW, OUTPUT);               //Debug with LED
                                          //************************************************************
                                          // VOOR ARDUINO NANO!!!
                                          //************************************************************

  //Serial.begin(100000, SERIAL_8E2);      //The SBUS is a non standard baud rate which can be confirmed using an oscilloscope

  //************************************************************
  // VOOR ARDUINO MEGA!!!!
  //************************************************************
  Serial1.begin(100000, SERIAL_8E2);  //The SBUS is a non standard baud rate which can be confirmed using an oscilloscope
  Serial.begin(1000000);

  rawr.blink(500, 500);  // 500ms ON, 500ms OFF, blink immediately
  rawl.blink(500, 500);  // 500ms ON, 500ms OFF, blink immediately
}
//**********************************************************************
// waarde 190 staat ongeveer gelijk aan 1000 op de zender
// FrSky receivers will output a range of 172 - 1811 with channels set to a range of -100% to +100%.
// Using extended limits of -150% to +150% outputs a range of 0 to 2047, which is the maximum range acheivable with 11 bits of data.
//**********************************************************************


//***********************************************************************
//     Loop
//***********************************************************************
void loop()
{
  recvWithStartEndMarkers();
  //recvWithEndMarker();
  parseUserInput();


  //Declare the variabes
  static int idx;
  byte b;

  //Check the serial port for incoming data
  //This could also be done via the serialEvent()
  if (Serial1.available())
  {
    b = Serial1.read();

    //this is a new package and it' not zero byte then it's probably the start byte B11110000 (sent MSB)
    //so start reading the 25 byte package
    if (idx == 0 && b != 0x0F)
    {  // start byte 15?
       // error - wait for the start byte
    }
    else
    {
      buffer[idx++] = b;  // fill the buffer with the bytes until the end byte B0000000 is recived
    }

    if (idx == 25)
    {  // If we've got 25 bytes then this is a good package so start to decode
      idx = 0;
      if (buffer[24] != 0x00)
      {
        errors++;
      }
      else
      {


        //For this demo, the corresponding channels mapped below need to be also configured on the FrSky Taranis tranmitter accordingly.
        //Any of the Channels can of course be used trigger any of the pins on the Arduino
        //Channels used in the demo are 5,6,7. Mapped these to the sliders on the transmitter to change RGB LED values
        //Channel 1 set to trigger the Internal Arduino LED on pin 13 once the threshold exceeds 1500
        //Channel 10 needs to be mapped to one of the switches on the Taranis which triggers a buzzer on pin 10 of the Arduino
        //Channel 1 also triggers a servo connected to pin 8 on the Arduino. So moving the throttle will also move the servo accordingly

        // sterretje
        printRawBinary();
        parseFutaba();
        printChannels();
        processChannels();

      }  //closing - else
    }    //closing - if (idx == 25)
  }      //closing - if (Serial.available ())
}  //closing void loop

/*
  Print futaba buffer in binary
*/
void printRawBinary()
{
  Serial.println(F("Futaba buffer"));
  for (uint8_t cnt = 0; cnt < NUMELEMENTS(buffer); cnt++)
  {
    // print leading zeroes
    if (buffer[cnt] < 0x80)
      Serial.print(0);
    if (buffer[cnt] < 0x40)
      Serial.print(0);
    if (buffer[cnt] < 0x20)
      Serial.print(0);
    if (buffer[cnt] < 0x10)
      Serial.print(0);
    if (buffer[cnt] < 0x08)
      Serial.print(0);
    if (buffer[cnt] < 0x04)
      Serial.print(0);
    if (buffer[cnt] < 0x02)
      Serial.print(0);
    // print byte
    Serial.print(buffer[cnt], BIN);
    // separator
    Serial.print(F(" "));
  }
  Serial.println();
  Serial.println(F("==="));
}

/*
  Print channel values
*/
void printChannels()
{
  Serial.println(F("Channel info"));
  for (uint8_t cnt = 0; cnt < NUMELEMENTS(channels); cnt++)
  {
    if ((channelDebugEnable & (1L << cnt)) == (1L << cnt))
    {
      Serial.print(F("ch["));
      Serial.print(cnt);
      Serial.print(F("] = "));
      Serial.println(channels[cnt]);
    }
  }
  Serial.println(F("==="));
}

/*
  Parse received data to channels
*/
void parseFutaba()
{
  //  Serial.println("Found Packet");
  // 25 byte packet received is little endian. Details of how the package is explained on our website:
  //http://www.robotmaker.eu/ROBOTmaker/quadcopter-3d-proximity-sensing/sbus-graphical-representation
  channels[0] = (buffer[1] | ((buffer[2] << 8) & 0x07FF));
  channels[1] = ((buffer[2] >> 3) | ((buffer[3] << 5) & 0x07FF));
  channels[2] = ((buffer[3] >> 6) | (buffer[4] << 2) | ((buffer[5] << 10) & 0x07FF));
  channels[3] = ((buffer[5] >> 1) | ((buffer[6] << 7) & 0x07FF));
  channels[4] = ((buffer[6] >> 4) | ((buffer[7] << 4) & 0x07FF));
  channels[5] = ((buffer[7] >> 7) | (buffer[8] << 1) | ((buffer[9] << 9) & 0x07FF));
  channels[6] = ((buffer[9] >> 2) | ((buffer[10] << 6) & 0x07FF));
  channels[7] = ((buffer[10] >> 5) | ((buffer[11] << 3) & 0x07FF));
  channels[8] = (buffer[12] | ((buffer[13] << 8) & 0x07FF));
  channels[9] = ((buffer[13] >> 3) | ((buffer[14] << 5) & 0x07FF));
  channels[10] = ((buffer[14] >> 6) | (buffer[15] << 2) | ((buffer[16] << 10) & 0x07FF));
  channels[11] = ((buffer[16] >> 1) | ((buffer[17] << 7) & 0x07FF));
  channels[12] = ((buffer[17] >> 4) | ((buffer[18] << 4) & 0x07FF));
  channels[13] = ((buffer[18] >> 7) | (buffer[19] << 1) | ((buffer[20] << 9) & 0x07FF));
  channels[14] = ((buffer[20] >> 2) | ((buffer[21] << 6) & 0x07FF));
  channels[15] = ((buffer[21] >> 5) | ((buffer[22] << 3) & 0x07FF));

  channels[16] = ((buffer[23]) & 0x0001) ? 2047 : 0;
  channels[17] = ((buffer[23] >> 1) & 0x0001) ? 2047 : 0;

  failsafe = ((buffer[23] >> 3) & 0x0001) ? 1 : 0;
  if ((buffer[23] >> 2) & 0x0001) lost++;

  //serialPrint (lost); debg the signals lost
}



void processChannels()
{
  word results;
  bool brakeOn = false;
  bool dimlichtAan = false;
  bool stadslichtenAan = false;
  bool grootlichtAan = false;



  //*********************************************************************
  // Script voor Richtingaanwijzer rechts
  //*********************************************************************
  {
    status_rawR = channels[7] < 400;

    if (rawRAan == true && status_rawR == false)  // button is pressed
      rawR_Press_Time = millis();
    else if (rawRAan == false && status_rawR == true)  // button is released
    {
      rawR_Release_Time = millis();


      long rawR_pressDuration = rawR_Release_Time - rawR_Press_Time;
      {
        if (rawR_pressDuration < rawR_SHORT)

          Serial.println("RAW R kort");  // korte bediening
        rawR_kort = !rawR_kort;

        if (rawR_pressDuration > rawR_LONG)


          Serial.println(" RAW R lang ");  // lange bediening
        rawR_lang = !rawR_lang;
      }
    }

    // save the the last state
    rawRAan = status_rawR;
  }

  {

    if (rawR_kort == true)
    {
      //Serial.println ("RAW R");
      rawR_lang = false;

      rawr.loop();  // MUST call the led1.loop() function in loop()
    }
    if (rawR_kort == false)
    {
      digitalWrite(pinRawR, LOW);
    }

    if (rawR_lang == true)
    {
      Serial.println("Gevarenlicht aan");

      //rawr.loop(); // MUST call the led1.loop() function in loop()
      //rawl.loop(); // MUST call the led1.loop() function in loop()
    }
    if (rawR_lang == false)
    {
      //digitalWrite(pinRawR, LOW);
      //digitalWrite(pinRawL, LOW);
    }
  }

  //Serial.println ("rawR_kort");
  //Serial.println (rawR_kort);

  //Serial.println("rawR_lang");
  //Serial.println(rawR_lang);

  //Serial.println("rawL_kort");
  //Serial.println(rawL_kort);

  //Serial.println("rawL_lang");
  //Serial.println(rawL_lang);

  //Serial.println("status_rawR");
  //Serial.println(status_rawR);

  //Serial.println("status_rawL");
  //Serial.println(status_rawL);

  //Serial.println("rawRAan");
  //Serial.println(rawRAan);

  //Serial.println("rawLAan");
  //Serial.println(rawLAan);

  //*********************************************************************
  // Script voor Richtingaanwijzer links
  //*********************************************************************

  status_rawL = channels[7] > 1800;
  {
    if (rawLAan == true && status_rawL == false)  // button is pressed
      rawL_Press_Time = millis();
    else if (rawLAan == false && status_rawL == true)  // button is released
    {
      rawL_Release_Time = millis();


      long rawL_pressDuration = rawL_Release_Time - rawL_Press_Time;

      if (rawL_pressDuration < rawL_SHORT)
        Serial.println("Richtingaanwijzer Links aan");
      rawL_kort = !rawL_kort;

      if (rawL_pressDuration > rawL_LONG)


        Serial.println(" is nog een optie:  ");
      rawL_lang = !rawL_lang;
    }
    // save the the last state
    rawLAan = status_rawL;
  }

  if (rawL_kort == true)
  {
    Serial.println("RAW L");

    rawl.loop();  // MUST call the led1.loop() function in loop()
  }
  if (rawL_kort == false)
  {
    digitalWrite(pinRawL, LOW);
  }

  if (rawL_lang == true)
  {
    Serial.println("Optie aan");
  }
  if (rawL_lang == false)
  {
  }



  //***********************************************************************
  //Brake lights channel 2 .
  //***********************************************************************
  results = channels[1];
  Serial1.write(results);
  if ((channels[1] >= 970) && (channels[1] <= 1010))
  {
    brakeOn = true;
    Serial.println("Brake light");
  }
  else
  {
    brakeOn = false;
  }


  //***********************************************************************
  //Back up lights channel 2 .
  //***********************************************************************
  results = channels[1];
  Serial1.write(results);
  if (channels[1] < 970)
  {
    digitalWrite(pinAchteruitrijlicht, HIGH);
    Serial.println("Back up light");
  }
  else
  {
    digitalWrite(pinAchteruitrijlicht, LOW);
  }

  //***********************************************************************
  //head and tail lights Channel 6
  //***********************************************************************
  results = channels[1];
  Serial1.write(results);
  if (channels[5] > 1800)
  {
    // head lights on
    stadslichtenAan = true;
    Serial.println("Stadslichten en achterlicht aan");


    // not braking
    if (brakeOn == false)
    {
      // tail lights on
      analogWrite(pinAchterlicht, 50);
    }
    // braking
    else
    {
      // tail lights bright
      digitalWrite(pinAchterlicht, HIGH);
    }
  }
  else
  {
    // head lights off
    digitalWrite(pinKoplamp, LOW);

    // not braking
    if (brakeOn == false)
    {
      // tail lights off
      digitalWrite(pinAchterlicht, LOW);
    }
    // braking
    else
    {
      // tail lights bright
      digitalWrite(pinAchterlicht, HIGH);
    }
  }

  //***********************************************************************
  //  Dimlicht channel 7 .
  //***********************************************************************
  results = channels[1];
  Serial1.write(results);
  if ((channels[6] >= 500) && (channels[6] <= 1800))
  {

    dimlichtAan = true;
    Serial.println("dimlicht aan");
  }
  else
  {

    dimlichtAan = false;
  }


  //***********************************************************************
  // Dimlicht
  //***********************************************************************
  if (stadslichtenAan == true)
  {
    // dimlicht aan
    if (dimlichtAan == true)
    {
      // Zet dimlichten aan
      analogWrite(pinKoplamp, 180);
    }
    else
    {

      analogWrite(pinKoplamp, 60);
    }
  }


  //***********************************************************************
  // Grootlicht
  //***********************************************************************
  results = channels[1];
  Serial1.write(results);
  if (channels[6] > 1800)
  {
    // head lights on
    grootlichtAan = true;
    Serial.println("grootlicht aan");
  }
  //dimlicht aan
  if (grootlichtAan == true)
  {
    // grootlicht uit
    if (dimlichtAan == false)
    {
      // dimlichten aan
      analogWrite(pinKoplamp, 255);
    }
  }


  //*******************************************************************************
  //Verstralers aan
  //*******************************************************************************
  if (grootlichtAan == true)
  {
    analogWrite(pinVerstralers, 255);
  }
  else if (stadslichtenAan == true)
  {
    analogWrite(pinVerstralers, 60);
  }
  else if ((grootlichtAan == false) && (stadslichtenAan == false))
  {
    analogWrite(pinVerstralers, 0);
  }
}


void recvWithEndMarker()
{
  static byte ndx = 0;
  char endMarker = '\n';
  char rc;

  if (Serial.available() > 0)
  {
    rc = Serial.read();

    if (rc != endMarker)
    {
      receivedChars[ndx] = rc;
      ndx++;
      if (ndx >= numChars)
      {
        ndx = numChars - 1;
      }
    }
    else
    {
      receivedChars[ndx] = '\0';  // terminate the string
      ndx = 0;
      newData = true;
    }
  }
}


void recvWithStartEndMarkers()
{
  static boolean recvInProgress = false;
  static byte ndx = 0;
  char startMarker = '<';
  char endMarker = '>';
  char rc;

  while (Serial.available() > 0 && newData == false)
  {
    rc = Serial.read();

    if (recvInProgress == true)
    {
      if (rc != endMarker)
      {
        receivedChars[ndx] = rc;
        ndx++;
        if (ndx >= numChars)
        {
          ndx = numChars - 1;
        }
      }
      else
      {
        receivedChars[ndx] = '\0';  // terminate the string
        recvInProgress = false;
        ndx = 0;
        newData = true;
      }
    }

    else if (rc == startMarker)
    {
      recvInProgress = true;
    }
  }
}


void parseUserInput()
{
  // no new data
  if (newData == false)
  {
    // nothing to do
    return;
  }

  newData = false;


  char* strtokIndx;
  // split on first comma
  strtokIndx = strtok(receivedChars, ",");

  // no comma
  if (strtokIndx == nullptr)
  {
    Serial.print(F("Unable to parse user input '"));
    Serial.print(receivedChars);
    Serial.println(F("'"));
    return;
  }

  char cmd = '\0';

  // check if command equals capital C
  if (strcmp(strtokIndx, "C") != 0 && strcmp(strtokIndx, "c") != 0)
  {
    Serial.print(F("Invalid command '"));
    Serial.print(strtokIndx);
    Serial.println(F("'"));
    return;
  }

  cmd = strtokIndx[0];

  // next comma or end of line
  strtokIndx = strtok(NULL, ",");

  // convert text to integer
  char* endptr;
  int chNo = strtol(strtokIndx, &endptr, 10);
  if (endptr == strtokIndx)
  {
    Serial.println(F("Invalid channel number found"));
    return;
  }

  if (*endptr != '\0')
  {
    Serial.println(F("Invalid channel number found; non-digits found"));
    return;
  }

  Serial.print("cmd = ");
  Serial.println(cmd);

  Serial.print("chNo = ");
  Serial.println(chNo);

  if (chNo < 0 && chNo >= 18)
  {
    Serial.print(F("Invalid channel number '"));
    Serial.print(chNo);
    Serial.println(F("'"));
    return;
  }

  if (cmd == 'C')
  {
    channelDebugEnable |= (1L << chNo);
  }
  if (cmd == 'c')
  {
    channelDebugEnable &= ~(1L << chNo);
  }
}

Standaard staat alles uit dus zie je geen kanaal informatie.

Topic		Replies	Views
Arduino 5V PWM naar 12V PWM Nederlands	6	1594	May 6, 2021
PWM signalen herkennen Nederlands	18	6595	May 6, 2021
arduino broedmachine Nederlands	4	1650	May 6, 2021
Vragen voordat ik start Nederlands	5	1082	May 6, 2021
PWM sturing Arduino Nederlands	4	1957	May 6, 2021

Arduino SBUS lichtmodule

Related topics