#include <SoftwareSerial.h>
#include <LiquidCrystal.h>
#define DE_BUG // Change this to #define DEBUG if needed
#define DE_BUG_HEX
#define FRS_TX_PIN A2 // not used
#define FRS_RX_PIN A3 // telemetry data
#define FRAME_SIZE 12 // max 12 bytes
#define FRAME_RVLQ 0xFE // second byte 0xFE - Remote Voltage and Link Quality
#define FRAME_USER 0xFD // second byte 0xFD - User Data
#define DISP_INTERVAL 500 // ms
SoftwareSerial FrSky = SoftwareSerial(FRS_TX_PIN, FRS_RX_PIN, true); // orig false
LiquidCrystal lcd(2, 3, 4, 5, 6, 7 );
byte next = 0;
byte buff[FRAME_SIZE]; // frame array
byte index = 0; // index in frame array
byte link = 0; // link established, data received
byte frame = 0; // frame received indicator
byte stuff = 0; // byte stuffing indicator
int Rx[DISP_INTERVAL/15]; // Up Link Quality RSSI
int Tx[DISP_INTERVAL/15]; // Down Link Quality RSSI
int V1[DISP_INTERVAL/15]; // Port 1 voltage, V * 100
int V2[DISP_INTERVAL/15]; // Port 2 voltage, V * 100
unsigned long dispTime = 0; // time when info was updated, ms
unsigned long prevTime = 0; // time when 0xFE frame was received, ms
int frmeTime = 0; // frame period, ms (~36ms)
int nvals = 0; // number of values received since last display
void setup() {
FrSky.begin(9600);
Serial.begin(115200);
dispTime = millis();
lcd.begin(16, 2);
lcd.setCursor(0, 0);
lcd.print("FrSky Telemetry");
lcd.setCursor(0, 1);
lcd.print("Filip Grano 2015");
frame = 0;
index = 0;
link = 0;
stuff = 0;
prevTime = millis();
}
void loop() {
if (FrSky.available()) { // reading input data
next = (byte)FrSky.read();
if (next != 0x7E)
if (next == 0x7D) // byte stuffing indicator
stuff = 1; // set the flag and discard byte
else if (stuff == 0)
buff[index++] = next; // normal byte
else {
buff[index++] = next | 0x20; // byte stuffing set, change next byte
stuff = 0; // and reset the flag
}
else {
if (index == 10 || index == 11) { // end byte
buff[index] = next;
frame = 1;
index = 0;
} else { // start byte?
index = 0;
buff[index++] = next;
}
}
if (index >= FRAME_SIZE)
index = FRAME_SIZE - 1; // prevent buffer overflow
}
if (buff[1] == FRAME_RVLQ && frame == 1) { // second byte 0xFE - Remote Voltage and Link Quality
V1[nvals] = buff[2] * 4 * 330.0 / 255; // V1 - Voltage, up to 13.2V, Internal divider 1:4
V2[nvals] = buff[3] * (22 + 100) / 22 * 330.0 / 255; // V2 - Voltage, up to 18.3V
Rx[nvals] = buff[4]; // Up Link Quality RSSI (min 36, max 116-121)
Tx[nvals] = buff[5] / 2; // Down Link Quality RSSI (min 36, max 116-121)
frmeTime = millis() - prevTime; // frame period in ms
prevTime = millis();
frame = 0;
link = 1; // valid data received
nvals++;
}
if (link == 1 && millis() > dispTime + DISP_INTERVAL && nvals > 0) {
dispTime = millis();
#ifdef DEBUG
printData();
#endif
lcd.setCursor(0, 0);
lcd.print(mode(V1, nvals) / 100.0, 2);
lcd.print("V ");
lcd.setCursor(7, 0);
lcd.print("RX=");
lcd.print(mode(Rx, nvals), 1);
lcd.print("dBc ");
lcd.setCursor(0, 1);
lcd.print(mode(V2, nvals) / 100.0, 2);
lcd.print("V ");
lcd.setCursor(7, 1);
lcd.print("TX=");
lcd.print(mode(Tx, nvals), 1);
lcd.print("dBc ");
nvals=0;
}
}
void printData(void) {
Serial.print(prevTime / 1000.0, 4);
Serial.print(": ");
#ifdef DEBUG_HEX
for (byte i = 0; i <= 10; i++) {
Serial.print(buff[i], HEX);
Serial.print(" ");
}
#endif
Serial.print("V1 = ");
Serial.print(mode(V1, nvals) / 100.0, 2);
Serial.print("V, V2 = ");
Serial.print(mode(V2, nvals) / 100.0, 2);
Serial.print("V, Rx = ");
Serial.print(mode(Rx, nvals));
Serial.print("dBc, Tx = ");
Serial.print(mode(Tx, nvals));
Serial.print("dBc, packets = ");
Serial.print(nvals);
Serial.println();
}
int mode(int arr[], int asize) {
if (asize==0) { return 0; }
int mode = arr[asize-1];
int mcount = 1;
int count = 0;
for(int i=asize; i>0; i--) {
int val = arr[i-1];
for(int j=i; j>0; j--) {
if(arr[j-1] == val) count++;
}
if (count > mcount) {
mode = val;
mcount = count;
}
count = 0;
}
return mode;
} |
