static void sendBaro(void)
{
sendDataHead(0x10);
serialize16(EstAlt / 100);
sendDataHead(0x21);
serialize16(EstAlt % 100);
}
static void sendHeading(void)
{
sendDataHead(ID_COURSE_BP);
serialize16((int16_t)heading);
sendDataHead(ID_COURSE_AP);
serialize16(0);
}
static void sendBaro(void)
{
sendDataHead(ID_ALTITUDE_BP);
serialize16((int32_t)EstAlt / 100);
sendDataHead(ID_ALTITUDE_AP);
serialize16((int32_t)EstAlt % 100);
}
static void sendBaro(void)
{
sendDataHead(ID_ALTITUDE_BP);
serialize16(BaroAlt / 100);
sendDataHead(ID_ALTITUDE_AP);
serialize16(ABS(BaroAlt % 100));
}
static void sendHeading(void)
{
sendDataHead(ID_COURSE_BP);
serialize16(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
sendDataHead(ID_COURSE_AP);
serialize16(0);
}
/*
* Send voltage with ID_VOLTAGE_AMP
*/
static void sendVoltageAmp()
{
uint16_t voltage = (vbat * 110) / 21;
sendDataHead(ID_VOLTAGE_AMP_BP);
serialize16(voltage / 100);
sendDataHead(ID_VOLTAGE_AMP_AP);
serialize16(((voltage % 100) + 5) / 10);
}
static void sendSpeed(void)
{
if (!STATE(GPS_FIX)) {
return;
}
//Speed should be sent in m/s (GPS speed is in cm/s)
sendDataHead(ID_GPS_SPEED_BP);
serialize16((GPS_speed * 0.01 + 0.5));
sendDataHead(ID_GPS_SPEED_AP);
serialize16(0); //Not dipslayed
}
static void sendTime(void)
{
uint32_t seconds = millis() / 1000;
uint8_t minutes = (seconds / 60) % 60;
// if we fly for more than an hour, something's wrong anyway
sendDataHead(ID_HOUR_MINUTE);
serialize16(minutes << 8);
sendDataHead(ID_SECOND);
serialize16(seconds % 60);
}
static void sendGpsAltitude(void)
{
uint16_t altitude = GPS_altitude;
//Send real GPS altitude only if it's reliable (there's a GPS fix)
if (!STATE(GPS_FIX)) {
altitude = 0;
}
sendDataHead(ID_GPS_ALTIDUTE_BP);
serialize16(altitude);
sendDataHead(ID_GPS_ALTIDUTE_AP);
serialize16(0);
}
static void sendSpeed(void)
{
if (!STATE(GPS_FIX)) {
return;
}
//Speed should be sent in knots (GPS speed is in cm/s)
sendDataHead(ID_GPS_SPEED_BP);
//convert to knots: 1cm/s = 0.0194384449 knots
serialize16(GPS_speed * 1944 / 100000);
sendDataHead(ID_GPS_SPEED_AP);
serialize16((GPS_speed * 1944 / 100) % 100);
}
// Course
void inline send_Course(void)
{
uint16_t Datas_Course_bp;
uint16_t Datas_Course_ap;
Datas_Course_bp = att.heading ; //att.heading
Datas_Course_ap = 0;
sendDataHead(ID_Course_bp);
write_FrSky16(Datas_Course_bp);
sendDataHead(ID_Course_ap);
write_FrSky16(Datas_Course_ap);
}
static void inline sendTwoPart(uint8_t bpId, uint8_t apId, float value, uint16_t resolution = 100)
{
int16_t bpVal;
uint16_t apVal;
bpVal = floor(value); // value before the decimal point ("bp" is "before point")
apVal = (value - int(value)) * resolution; // value after the decimal point
sendDataHead(bpId);
write_FrSky16(bpVal);
sendDataHead(apId);
write_FrSky16(apVal);
}
// ACC
void send_Accel(void)
{
int16_t Datas_Ang_X;
int16_t Datas_Ang_Y;
Datas_Ang_X = att.angle[0];
Datas_Ang_Y = att.angle[1];
sendDataHead(ID_Ang_X);
write_FrSky16(Datas_Ang_X);
sendDataHead(ID_Ang_Y);
write_FrSky16(Datas_Ang_Y);
//Frsky
}
// Time
void inline send_Time(void)
{
uint16_t seconds_since_start;
uint16_t Data_Minutes_hours;
uint16_t Data_seconds;
if (showTime.TimerStart) {
seconds_since_start = (millis() - showTime.armingTime) / 1000;
Data_Minutes_hours = seconds_since_start / 60;
Data_seconds = seconds_since_start - 60 * Data_Minutes_hours;
sendDataHead(ID_Hour_Minute);
write_FrSky16(Data_Minutes_hours * 256);
sendDataHead(ID_Second);
write_FrSky16(Data_seconds);
}
}
static void sendAccel(void)
{
for (int i = 0; i < 3; i++) {
sendDataHead(ID_ACC_X + i);
serialize16(1000 * (int32_t)accSmooth[i] / acc.acc_1G);
}
}
static void sendAmperage(void)
{
amperageMeter_t *state = getAmperageMeter(batteryConfig()->amperageMeterSource);
sendDataHead(ID_CURRENT);
serialize16((uint16_t)(state->amperage / 10));
}
/*
* Send voltage via ID_VOLT
*
* NOTE: This sends voltage divided by batteryCellCount. To get the real
* battery voltage, you need to multiply the value by batteryCellCount.
*/
static void sendVoltage(void)
{
static uint16_t currentCell = 0;
uint32_t cellVoltage;
uint16_t payload;
/*
* Format for Voltage Data for single cells is like this:
*
* llll llll cccc hhhh
* l: Low voltage bits
* h: High voltage bits
* c: Cell number (starting at 0)
*
* The actual value sent for cell voltage has resolution of 0.002 volts
* Since vbat has resolution of 0.1 volts it has to be multiplied by 50
*/
cellVoltage = ((uint32_t)vbat * 100 + batteryCellCount) / (batteryCellCount * 2);
// Cell number is at bit 9-12
payload = (currentCell << 4);
// Lower voltage bits are at bit 0-8
payload |= ((cellVoltage & 0x0ff) << 8);
// Higher voltage bits are at bits 13-15
payload |= ((cellVoltage & 0xf00) >> 8);
sendDataHead(ID_VOLT);
serialize16(payload);
currentCell++;
currentCell %= batteryCellCount;
}
// Temperature 2
void inline send_Distance(void)
{
if (f.GPS_FIX_HOME)
{
sendDataHead(ID_Temperature2);
write_FrSky16(GPS_distanceToHome); // Distance to home alias Temp2
}
}
/*
* Send voltage with ID_VOLTAGE_AMP
*/
static void sendVoltageAmp(void)
{
if (telemetryConfig->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) {
/*
* Use new ID 0x39 to send voltage directly in 0.1 volts resolution
*/
sendDataHead(ID_VOLTAGE_AMP);
serialize16(vbat);
} else {
uint16_t voltage = (vbat * 110) / 21;
sendDataHead(ID_VOLTAGE_AMP_BP);
serialize16(voltage / 100);
sendDataHead(ID_VOLTAGE_AMP_AP);
serialize16(((voltage % 100) + 5) / 10);
}
}
// Voltage (Ampere Sensor)
void inline send_Voltage_ampere(void)
{
#if defined(VBAT)
uint16_t Data_Voltage_vBat_bp;
uint16_t Data_Voltage_vBat_ap;
uint16_t volts = analog.vbat * TELEMETRY_FRSKY_VBAT / 21;
Data_Voltage_vBat_bp = volts / 100;
Data_Voltage_vBat_ap = (volts % 100) / 10;
sendDataHead(ID_Voltage_Amp_bp);
write_FrSky16(Data_Voltage_vBat_bp);
sendDataHead(ID_Voltage_Amp_ap);
write_FrSky16(Data_Voltage_vBat_ap);
#endif
}
static void sendTemperature1(void)
{
sendDataHead(ID_TEMPRATURE1);
#ifdef BARO
serialize16((baroTemperature + 50)/ 100); //Airmamaf
#else
serialize16(telemTemperature1 / 10);
#endif
}
static void sendAccel(void)
{
int i;
for (i = 0; i < 3; i++) {
sendDataHead(ID_ACC_X + i);
serialize16(((float)accSmooth[i] / acc.acc_1G) * 1000);
}
}
static void sendAccel(void)
{
uint8_t i;
for (i = 0; i < 3; i++) {
sendDataHead(0x24 + i);
serialize16(((float)accSmooth[i] / acc_1G) * 1000);
}
}
// GPS speed
void inline send_GPS_speed(void)
{
//frsky
if (f.GPS_FIX && GPS_numSat >= 4)
{
uint16_t Datas_GPS_speed_bp;
uint16_t Datas_GPS_speed_ap;
Datas_GPS_speed_bp = GPS_speed * 0.036; //cm/s to km/h
Datas_GPS_speed_ap = 0;
sendDataHead(ID_GPS_speed_bp);
write_FrSky16(Datas_GPS_speed_bp);
sendDataHead(ID_GPS_speed_ap);
write_FrSky16(Datas_GPS_speed_ap);
}
//frsky
}
static void sendFuelLevel(void)
{
sendDataHead(ID_FUEL_LEVEL);
if (batteryConfig->batteryCapacity > 0) {
serialize16((uint16_t)calculateBatteryCapacityRemainingPercentage());
} else {
serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF));
}
}
static void sendThrottleOrBatterySizeAsRpm(void)
{
sendDataHead(ID_RPM);
if (ARMING_FLAG(ARMED)) {
serialize16(rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER);
} else {
serialize16((batteryConfig->batteryCapacity / BLADE_NUMBER_DIVIDER));
}
}
// Temperature
void inline send_Temperature(void)
{
//Frsky
int16_t Datas_Temprature1;
Datas_Temprature1 = GPS_numSat; // Number of Satalits alias Temp1
sendDataHead(ID_Temperature1);
write_FrSky16(Datas_Temprature1);
//Frsky
}
// Fuel level
void inline send_Num_Sat(void)
{
//Frsky
uint16_t Datas_Fuel_level;
Datas_Fuel_level = 0;
sendDataHead(ID_Fuel_level);
write_FrSky16(Datas_Fuel_level);
//Frsky
}
static void sendFuelLevel(void)
{
sendDataHead(ID_FUEL_LEVEL);
if (batteryConfig()->batteryCapacity > 0) {
serialize16((uint16_t)batteryCapacityRemainingPercentage());
} else {
amperageMeter_t *state = getAmperageMeter(batteryConfig()->amperageMeterSource);
serialize16((uint16_t)constrain(state->mAhDrawn, 0, 0xFFFF));
}
}
static void sendGPS(void)
{
int32_t localGPS_coord[2] = {0,0};
// Don't set dummy GPS data, if we already had a GPS fix
// it can be usefull to keep last valid coordinates
static uint8_t gpsFixOccured = 0;
//Dummy data if no 3D fix, this way we can display heading in Taranis
if (STATE(GPS_FIX) || gpsFixOccured == 1) {
localGPS_coord[LAT] = GPS_coord[LAT];
localGPS_coord[LON] = GPS_coord[LON];
gpsFixOccured = 1;
} else {
// Send dummy GPS Data in order to display compass value
localGPS_coord[LAT] = (telemetryConfig->gpsNoFixLatitude * GPS_DEGREES_DIVIDER);
localGPS_coord[LON] = (telemetryConfig->gpsNoFixLongitude * GPS_DEGREES_DIVIDER);
}
gpsCoordinateDDDMMmmmm_t coordinate;
GPStoDDDMM_MMMM(localGPS_coord[LAT], &coordinate);
sendDataHead(ID_LATITUDE_BP);
serialize16(coordinate.dddmm);
sendDataHead(ID_LATITUDE_AP);
serialize16(coordinate.mmmm);
sendDataHead(ID_N_S);
serialize16(localGPS_coord[LAT] < 0 ? 'S' : 'N');
GPStoDDDMM_MMMM(localGPS_coord[LON], &coordinate);
sendDataHead(ID_LONGITUDE_BP);
serialize16(coordinate.dddmm);
sendDataHead(ID_LONGITUDE_AP);
serialize16(coordinate.mmmm);
sendDataHead(ID_E_W);
serialize16(localGPS_coord[LON] < 0 ? 'W' : 'E');
}
