Add GPS values and update others
This commit is contained in:
parent
8207dab3e5
commit
fb72fa7d03
|
@ -30,6 +30,7 @@
|
||||||
#include "build/build_config.h"
|
#include "build/build_config.h"
|
||||||
#include "build/debug.h"
|
#include "build/debug.h"
|
||||||
#include "fc/runtime_config.h"
|
#include "fc/runtime_config.h"
|
||||||
|
#include "config/feature.h"
|
||||||
|
|
||||||
#include "common/utils.h"
|
#include "common/utils.h"
|
||||||
#include "common/bitarray.h"
|
#include "common/bitarray.h"
|
||||||
|
@ -38,17 +39,19 @@
|
||||||
#include "drivers/serial_uart.h"
|
#include "drivers/serial_uart.h"
|
||||||
#include "drivers/time.h"
|
#include "drivers/time.h"
|
||||||
|
|
||||||
|
|
||||||
#include "flight/position.h"
|
#include "flight/position.h"
|
||||||
#include "flight/imu.h"
|
#include "flight/imu.h"
|
||||||
|
|
||||||
#include "pg/rx.h"
|
|
||||||
|
|
||||||
#include "io/serial.h"
|
#include "io/serial.h"
|
||||||
|
#include "io/gps.h"
|
||||||
|
|
||||||
#include "rx/rx.h"
|
#include "rx/rx.h"
|
||||||
#include "rx/jetiexbus.h"
|
#include "rx/jetiexbus.h"
|
||||||
|
|
||||||
#include "sensors/battery.h"
|
#include "sensors/battery.h"
|
||||||
#include "sensors/sensors.h"
|
#include "sensors/sensors.h"
|
||||||
|
#include "sensors/acceleration.h"
|
||||||
|
|
||||||
#include "telemetry/jetiexbus.h"
|
#include "telemetry/jetiexbus.h"
|
||||||
#include "telemetry/telemetry.h"
|
#include "telemetry/telemetry.h"
|
||||||
|
@ -58,7 +61,7 @@
|
||||||
#define EXTEL_SYNC_LEN 1
|
#define EXTEL_SYNC_LEN 1
|
||||||
#define EXTEL_CRC_LEN 1
|
#define EXTEL_CRC_LEN 1
|
||||||
#define EXTEL_HEADER_LEN 6
|
#define EXTEL_HEADER_LEN 6
|
||||||
#define EXTEL_MAX_LEN 28
|
#define EXTEL_MAX_LEN 26
|
||||||
#define EXTEL_OVERHEAD (EXTEL_SYNC_LEN + EXTEL_HEADER_LEN + EXTEL_CRC_LEN)
|
#define EXTEL_OVERHEAD (EXTEL_SYNC_LEN + EXTEL_HEADER_LEN + EXTEL_CRC_LEN)
|
||||||
#define EXTEL_MAX_PAYLOAD (EXTEL_MAX_LEN - EXTEL_OVERHEAD)
|
#define EXTEL_MAX_PAYLOAD (EXTEL_MAX_LEN - EXTEL_OVERHEAD)
|
||||||
#define EXBUS_MAX_REQUEST_BUFFER_SIZE (EXBUS_OVERHEAD + EXTEL_MAX_LEN)
|
#define EXBUS_MAX_REQUEST_BUFFER_SIZE (EXBUS_OVERHEAD + EXTEL_MAX_LEN)
|
||||||
|
@ -115,15 +118,27 @@ typedef struct exBusSensor_s {
|
||||||
// after every 15 sensors a new header has to be inserted (e.g. "BF D2")
|
// after every 15 sensors a new header has to be inserted (e.g. "BF D2")
|
||||||
const exBusSensor_t jetiExSensors[] = {
|
const exBusSensor_t jetiExSensors[] = {
|
||||||
{"BF D1", "", EX_TYPE_DES, 0 }, // device descripton
|
{"BF D1", "", EX_TYPE_DES, 0 }, // device descripton
|
||||||
{"Voltage", "V", EX_TYPE_14b, DECIMAL_MASK(1)},
|
{"Voltage", "V", EX_TYPE_22b, DECIMAL_MASK(1)},
|
||||||
{"Current", "A", EX_TYPE_14b, DECIMAL_MASK(2)},
|
{"Current", "A", EX_TYPE_22b, DECIMAL_MASK(2)},
|
||||||
{"Altitude", "m", EX_TYPE_14b, DECIMAL_MASK(2)},
|
{"Altitude", "m", EX_TYPE_22b, DECIMAL_MASK(2)},
|
||||||
{"Capacity", "mAh", EX_TYPE_22b, DECIMAL_MASK(0)},
|
{"Capacity", "mAh", EX_TYPE_22b, DECIMAL_MASK(0)},
|
||||||
{"Power", "W", EX_TYPE_22b, DECIMAL_MASK(1)},
|
{"Power", "W", EX_TYPE_22b, DECIMAL_MASK(1)},
|
||||||
{"Roll angle", "\xB0", EX_TYPE_14b, DECIMAL_MASK(1)},
|
{"Roll angle", "\xB0", EX_TYPE_22b, DECIMAL_MASK(1)},
|
||||||
{"Pitch angle", "\xB0", EX_TYPE_14b, DECIMAL_MASK(1)},
|
{"Pitch angle", "\xB0", EX_TYPE_22b, DECIMAL_MASK(1)},
|
||||||
{"Heading", "\xB0", EX_TYPE_14b, DECIMAL_MASK(1)},
|
{"Heading", "\xB0", EX_TYPE_22b, DECIMAL_MASK(1)},
|
||||||
{"Vario", "m/s", EX_TYPE_22b, DECIMAL_MASK(2)}
|
{"Vario", "m/s", EX_TYPE_22b, DECIMAL_MASK(2)},
|
||||||
|
{"GPS Sats", "", EX_TYPE_22b, DECIMAL_MASK(0)},
|
||||||
|
{"GPS Long", "", EX_TYPE_GPS, DECIMAL_MASK(0)},
|
||||||
|
{"GPS Lat", "", EX_TYPE_GPS, DECIMAL_MASK(0)},
|
||||||
|
{"GPS Speed", "m/s", EX_TYPE_22b, DECIMAL_MASK(2)},
|
||||||
|
{"GPS H-Distance", "m", EX_TYPE_22b, DECIMAL_MASK(0)},
|
||||||
|
{"GPS H-Direction", "\xB0", EX_TYPE_22b, DECIMAL_MASK(1)},
|
||||||
|
{"BF D2", "", EX_TYPE_DES, 0 }, // device descripton
|
||||||
|
{"GPS Heading", "\xB0", EX_TYPE_22b, DECIMAL_MASK(1)},
|
||||||
|
{"GPS Altitude", "m", EX_TYPE_22b, DECIMAL_MASK(2)},
|
||||||
|
{"G-Force X", "", EX_TYPE_22b, DECIMAL_MASK(3)},
|
||||||
|
{"G-Force Y", "", EX_TYPE_22b, DECIMAL_MASK(3)},
|
||||||
|
{"G-Force Z", "", EX_TYPE_22b, DECIMAL_MASK(3)}
|
||||||
};
|
};
|
||||||
|
|
||||||
// after every 15 sensors increment the step by 2 (e.g. ...EX_VAL15, EX_VAL16 = 17) to skip the device description
|
// after every 15 sensors increment the step by 2 (e.g. ...EX_VAL15, EX_VAL16 = 17) to skip the device description
|
||||||
|
@ -136,9 +151,27 @@ enum exSensors_e {
|
||||||
EX_ROLL_ANGLE,
|
EX_ROLL_ANGLE,
|
||||||
EX_PITCH_ANGLE,
|
EX_PITCH_ANGLE,
|
||||||
EX_HEADING,
|
EX_HEADING,
|
||||||
EX_VARIO
|
EX_VARIO,
|
||||||
|
EX_GPS_SATS,
|
||||||
|
EX_GPS_LONG,
|
||||||
|
EX_GPS_LAT,
|
||||||
|
EX_GPS_SPEED,
|
||||||
|
EX_GPS_DISTANCE_TO_HOME,
|
||||||
|
EX_GPS_DIRECTION_TO_HOME,
|
||||||
|
EX_GPS_HEADING = 17,
|
||||||
|
EX_GPS_ALTITUDE,
|
||||||
|
EX_GFORCE_X,
|
||||||
|
EX_GFORCE_Y,
|
||||||
|
EX_GFORCE_Z
|
||||||
};
|
};
|
||||||
|
|
||||||
|
union{
|
||||||
|
int32_t vInt;
|
||||||
|
uint16_t vWord[2];
|
||||||
|
char vBytes[4];
|
||||||
|
} exGps;
|
||||||
|
|
||||||
|
|
||||||
#define JETI_EX_SENSOR_COUNT (ARRAYLEN(jetiExSensors))
|
#define JETI_EX_SENSOR_COUNT (ARRAYLEN(jetiExSensors))
|
||||||
|
|
||||||
static uint8_t jetiExBusTelemetryFrame[40];
|
static uint8_t jetiExBusTelemetryFrame[40];
|
||||||
|
@ -182,7 +215,6 @@ void initJetiExBusTelemetry(void)
|
||||||
jetiExTelemetryFrame[EXTEL_HEADER_LSN_HB] = 0x00;
|
jetiExTelemetryFrame[EXTEL_HEADER_LSN_HB] = 0x00;
|
||||||
jetiExTelemetryFrame[EXTEL_HEADER_RES] = 0x00; // reserved, by default 0x00
|
jetiExTelemetryFrame[EXTEL_HEADER_RES] = 0x00; // reserved, by default 0x00
|
||||||
|
|
||||||
//exSensorEnabled = 0x3fe;
|
|
||||||
// Check which sensors are available
|
// Check which sensors are available
|
||||||
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) {
|
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) {
|
||||||
bitArraySet(&exSensorEnabled, EX_VOLTAGE);
|
bitArraySet(&exSensorEnabled, EX_VOLTAGE);
|
||||||
|
@ -201,10 +233,25 @@ void initJetiExBusTelemetry(void)
|
||||||
if (sensors(SENSOR_ACC)) {
|
if (sensors(SENSOR_ACC)) {
|
||||||
bitArraySet(&exSensorEnabled, EX_ROLL_ANGLE);
|
bitArraySet(&exSensorEnabled, EX_ROLL_ANGLE);
|
||||||
bitArraySet(&exSensorEnabled, EX_PITCH_ANGLE);
|
bitArraySet(&exSensorEnabled, EX_PITCH_ANGLE);
|
||||||
|
bitArraySet(&exSensorEnabled, EX_GFORCE_X);
|
||||||
|
bitArraySet(&exSensorEnabled, EX_GFORCE_Y);
|
||||||
|
bitArraySet(&exSensorEnabled, EX_GFORCE_Z);
|
||||||
}
|
}
|
||||||
if (sensors(SENSOR_MAG)) {
|
if (sensors(SENSOR_MAG)) {
|
||||||
bitArraySet(&exSensorEnabled, EX_HEADING);
|
bitArraySet(&exSensorEnabled, EX_HEADING);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (featureIsEnabled(FEATURE_GPS)) {
|
||||||
|
bitArraySet(&exSensorEnabled, EX_GPS_SATS);
|
||||||
|
bitArraySet(&exSensorEnabled, EX_GPS_LONG);
|
||||||
|
bitArraySet(&exSensorEnabled, EX_GPS_LAT);
|
||||||
|
bitArraySet(&exSensorEnabled, EX_GPS_SPEED);
|
||||||
|
bitArraySet(&exSensorEnabled, EX_GPS_DISTANCE_TO_HOME);
|
||||||
|
bitArraySet(&exSensorEnabled, EX_GPS_DIRECTION_TO_HOME);
|
||||||
|
bitArraySet(&exSensorEnabled, EX_GPS_HEADING);
|
||||||
|
bitArraySet(&exSensorEnabled, EX_GPS_ALTITUDE);
|
||||||
|
}
|
||||||
|
|
||||||
firstActiveSensor = getNextActiveSensor(0); // find the first active sensor
|
firstActiveSensor = getNextActiveSensor(0); // find the first active sensor
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -224,19 +271,35 @@ void createExTelemetryTextMessage(uint8_t *exMessage, uint8_t messageID, const e
|
||||||
exMessage[exMessage[EXTEL_HEADER_TYPE_LEN] + EXTEL_CRC_LEN] = calcCRC8(&exMessage[EXTEL_HEADER_TYPE_LEN], exMessage[EXTEL_HEADER_TYPE_LEN]);
|
exMessage[exMessage[EXTEL_HEADER_TYPE_LEN] + EXTEL_CRC_LEN] = calcCRC8(&exMessage[EXTEL_HEADER_TYPE_LEN], exMessage[EXTEL_HEADER_TYPE_LEN]);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
uint32_t calcGpsDDMMmmm(int32_t value, bool isLong)
|
||||||
|
{
|
||||||
|
uint32_t absValue = ABS(value);
|
||||||
|
uint16_t deg16 = absValue / GPS_DEGREES_DIVIDER;
|
||||||
|
uint16_t min16 = (absValue - deg16 * GPS_DEGREES_DIVIDER) * 6 / 1000;
|
||||||
|
|
||||||
|
exGps.vInt = 0;
|
||||||
|
exGps.vWord[0] = min16;
|
||||||
|
exGps.vWord[1] = deg16;
|
||||||
|
exGps.vWord[1] |= isLong ? 0x2000 : 0;
|
||||||
|
exGps.vWord[1] |= (value < 0) ? 0x4000 : 0;
|
||||||
|
|
||||||
|
return exGps.vInt;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
int32_t getSensorValue(uint8_t sensor)
|
int32_t getSensorValue(uint8_t sensor)
|
||||||
{
|
{
|
||||||
switch(sensor) {
|
switch(sensor) {
|
||||||
case EX_VOLTAGE:
|
case EX_VOLTAGE:
|
||||||
return getBatteryVoltageLatest();
|
return getBatteryVoltage();
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case EX_CURRENT:
|
case EX_CURRENT:
|
||||||
return getAmperageLatest();
|
return getAmperage();
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case EX_ALTITUDE:
|
case EX_ALTITUDE:
|
||||||
return getEstimatedAltitudeCm() / 100;
|
return getEstimatedAltitudeCm();
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case EX_CAPACITY:
|
case EX_CAPACITY:
|
||||||
|
@ -244,7 +307,7 @@ int32_t getSensorValue(uint8_t sensor)
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case EX_POWER:
|
case EX_POWER:
|
||||||
return (getBatteryVoltageLatest() * getAmperageLatest() / 100);
|
return (getBatteryVoltage() * getAmperage() / 100);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case EX_ROLL_ANGLE:
|
case EX_ROLL_ANGLE:
|
||||||
|
@ -263,6 +326,50 @@ int32_t getSensorValue(uint8_t sensor)
|
||||||
return getEstimatedVario();
|
return getEstimatedVario();
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case EX_GPS_SATS:
|
||||||
|
return gpsSol.numSat;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case EX_GPS_LONG:
|
||||||
|
return calcGpsDDMMmmm(gpsSol.llh.lon, true);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case EX_GPS_LAT:
|
||||||
|
return calcGpsDDMMmmm(gpsSol.llh.lat, false);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case EX_GPS_SPEED:
|
||||||
|
return gpsSol.groundSpeed;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case EX_GPS_DISTANCE_TO_HOME:
|
||||||
|
return GPS_distanceToHome;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case EX_GPS_DIRECTION_TO_HOME:
|
||||||
|
return GPS_directionToHome;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case EX_GPS_HEADING:
|
||||||
|
return gpsSol.groundCourse;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case EX_GPS_ALTITUDE:
|
||||||
|
return gpsSol.llh.altCm;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case EX_GFORCE_X:
|
||||||
|
return (int16_t)(((float)acc.accADC[0] / acc.dev.acc_1G) * 1000);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case EX_GFORCE_Y:
|
||||||
|
return (int16_t)(((float)acc.accADC[1] / acc.dev.acc_1G) * 1000);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case EX_GFORCE_Z:
|
||||||
|
return (int16_t)(((float)acc.accADC[2] / acc.dev.acc_1G) * 1000);
|
||||||
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
@ -303,7 +410,11 @@ uint8_t createExTelemetryValueMessage(uint8_t *exMessage, uint8_t item)
|
||||||
sensorValue = sensorValue >> 8;
|
sensorValue = sensorValue >> 8;
|
||||||
iCount--;
|
iCount--;
|
||||||
}
|
}
|
||||||
|
if(jetiExSensors[item].exDataType != EX_TYPE_GPS){
|
||||||
*p++ = (sensorValue & 0x9F) | jetiExSensors[item].decimals;
|
*p++ = (sensorValue & 0x9F) | jetiExSensors[item].decimals;
|
||||||
|
}else{
|
||||||
|
*p++ = sensorValue;
|
||||||
|
}
|
||||||
|
|
||||||
item = getNextActiveSensor(item);
|
item = getNextActiveSensor(item);
|
||||||
|
|
||||||
|
@ -359,9 +470,7 @@ void handleJetiExBusTelemetry(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
if ((jetiExBusRequestFrame[EXBUS_HEADER_DATA_ID] == EXBUS_EX_REQUEST) && (jetiExBusCalcCRC16(jetiExBusRequestFrame, jetiExBusRequestFrame[EXBUS_HEADER_MSG_LEN]) == 0)) {
|
if ((jetiExBusRequestFrame[EXBUS_HEADER_DATA_ID] == EXBUS_EX_REQUEST) && (jetiExBusCalcCRC16(jetiExBusRequestFrame, jetiExBusRequestFrame[EXBUS_HEADER_MSG_LEN]) == 0)) {
|
||||||
// switch to TX mode
|
|
||||||
if (serialRxBytesWaiting(jetiExBusPort) == 0) {
|
if (serialRxBytesWaiting(jetiExBusPort) == 0) {
|
||||||
serialSetMode(jetiExBusPort, MODE_TX);
|
|
||||||
jetiExBusTransceiveState = EXBUS_TRANS_TX;
|
jetiExBusTransceiveState = EXBUS_TRANS_TX;
|
||||||
item = sendJetiExBusTelemetry(jetiExBusRequestFrame[EXBUS_HEADER_PACKET_ID], item);
|
item = sendJetiExBusTelemetry(jetiExBusRequestFrame[EXBUS_HEADER_PACKET_ID], item);
|
||||||
jetiExBusRequestState = EXBUS_STATE_PROCESSED;
|
jetiExBusRequestState = EXBUS_STATE_PROCESSED;
|
||||||
|
@ -376,7 +485,6 @@ void handleJetiExBusTelemetry(void)
|
||||||
// check the state if transmit is ready
|
// check the state if transmit is ready
|
||||||
if (jetiExBusTransceiveState == EXBUS_TRANS_IS_TX_COMPLETED) {
|
if (jetiExBusTransceiveState == EXBUS_TRANS_IS_TX_COMPLETED) {
|
||||||
if (isSerialTransmitBufferEmpty(jetiExBusPort)) {
|
if (isSerialTransmitBufferEmpty(jetiExBusPort)) {
|
||||||
serialSetMode(jetiExBusPort, MODE_RX);
|
|
||||||
jetiExBusTransceiveState = EXBUS_TRANS_RX;
|
jetiExBusTransceiveState = EXBUS_TRANS_RX;
|
||||||
jetiExBusRequestState = EXBUS_STATE_ZERO;
|
jetiExBusRequestState = EXBUS_STATE_ZERO;
|
||||||
}
|
}
|
||||||
|
@ -417,5 +525,3 @@ uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item)
|
||||||
}
|
}
|
||||||
#endif // TELEMETRY
|
#endif // TELEMETRY
|
||||||
#endif // SERIAL_RX
|
#endif // SERIAL_RX
|
||||||
|
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue