/*
FrSky S-Port Telemetry library example
(c) Pawelsky 20150921
Not for commercial use
Note that you need 328P based (e.g. Pro Mini, Nano, Uno) board and FrSkySportTelemetry library for this example to work
Modified for Naza Telemetry by der-Frickler.
*/
#include "NazaDecoderLib.h"
#include "FrSkySportSensor.h"
#include "FrSkySportSensorFcs.h"
#include "FrSkySportSensorFlvss.h"
#include "FrSkySportSensorGps.h"
#include "FrSkySportSensorRpm.h"
#include "FrSkySportSensorVario.h"
#include "FrSkySportSingleWireSerial.h"
#include "FrSkySportTelemetry.h"
#include "SoftwareSerial.h"
FrSkySportSensorFcs fcs; // Create FCS sensor with default ID
FrSkySportSensorFlvss flvss; // Create FLVSS sensor with default ID
FrSkySportSensorGps gps; // Create GPS sensor with default ID
FrSkySportSensorRpm rpm; // Create RPM sensor with default ID
FrSkySportSensorVario vario; // Create Variometer sensor with default ID
FrSkySportTelemetry telemetry; // Create Variometer telemetry object
#define P_VOLTAGE A0
#define P_CURRENT A1
#define P_CELL1 A2
#define P_CELL2 A3
#define P_CELL3 A4
#define P_CELL4 A5
long voltage = 0;
long voltageAVG = 0;
long current = 0;
long currentAVG = 0;
int satFIX = 0;
void setup()
{
Serial.begin(115200);
// Configure the telemetry serial port and sensors (remember to use & to specify a pointer to sensor)
telemetry.begin(FrSkySportSingleWireSerial::SOFT_SERIAL_PIN_12, &fcs, &flvss, &gps, &rpm, &vario);
}
void loop()
{
// read voltage and make average
voltage = analogRead(P_VOLTAGE);
voltage = map(voltage, 0, 1024, 0, 2000); // map 5V A/D range to 0-20V 1/4 Divider with 10k,30k resistors
voltageAVG = (0.1*voltage + 0.9*voltageAVG);
// read current and make average
current = analogRead(P_CURRENT);
current = map(current, 0, 1024, 3778, -3778); // map 5V A/D range to ACS715 30A Current Sensor
currentAVG = (0.1*current + 0.9*currentAVG);
// Set current/voltage sensor (FCS) data
fcs.setData(
((float)currentAVG/100.0),
((float)voltageAVG/100.0)
);
// Set LiPo voltage sensor (FLVSS) data
//flvss.setData(4.01, 4.02, 4.03, 4.04, 4.05, 4.06); // Cell voltages in volts (cells 1-6)
// parse naza gps data
if(Serial.available()) {
if(NazaDecoder.decode(Serial.read()) == NAZA_MESSAGE_GPS) {
satFIX = NazaDecoder.getFixType() + (NazaDecoder.getNumSat()*10);
rpm.setData(0.0, NazaDecoder.getNumSat(), satFIX);
// if satfix -> update telemetry
if (NazaDecoder.getFixType() >= 3 {
gps.setData(NazaDecoder.getLat(), NazaDecoder.getLon(),
NazaDecoder.getGpsAlt(), NazaDecoder.getSpeed(), NazaDecoder.getCog(),
NazaDecoder.getYear(), NazaDecoder.getMonth(), NazaDecoder.getDay(),
NazaDecoder.getHour(), NazaDecoder.getMinute(), NazaDecoder.getSecond());
vario.setData(NazaDecoder.getGpsAlt(), NazaDecoder.getGpsVsi());
}
}
}
// Send the telemetry data, note that the data will only be sent for sensors
// that are being polled at given moment
telemetry.send();
}
