From fb72fa7d03ee807b770682733169d5736dfe4aae Mon Sep 17 00:00:00 2001 From: Thomas Miric Date: Sat, 15 Sep 2018 09:53:34 +0200 Subject: [PATCH 1/2] Add GPS values and update others --- src/main/telemetry/jetiexbus.c | 160 +++++++++++++++++++++++++++------ 1 file changed, 133 insertions(+), 27 deletions(-) diff --git a/src/main/telemetry/jetiexbus.c b/src/main/telemetry/jetiexbus.c index 2c28c4a3c9..694081c642 100644 --- a/src/main/telemetry/jetiexbus.c +++ b/src/main/telemetry/jetiexbus.c @@ -30,6 +30,7 @@ #include "build/build_config.h" #include "build/debug.h" #include "fc/runtime_config.h" +#include "config/feature.h" #include "common/utils.h" #include "common/bitarray.h" @@ -38,17 +39,19 @@ #include "drivers/serial_uart.h" #include "drivers/time.h" + #include "flight/position.h" #include "flight/imu.h" -#include "pg/rx.h" - #include "io/serial.h" +#include "io/gps.h" + #include "rx/rx.h" #include "rx/jetiexbus.h" #include "sensors/battery.h" #include "sensors/sensors.h" +#include "sensors/acceleration.h" #include "telemetry/jetiexbus.h" #include "telemetry/telemetry.h" @@ -58,7 +61,7 @@ #define EXTEL_SYNC_LEN 1 #define EXTEL_CRC_LEN 1 #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_MAX_PAYLOAD (EXTEL_MAX_LEN - EXTEL_OVERHEAD) #define EXBUS_MAX_REQUEST_BUFFER_SIZE (EXBUS_OVERHEAD + EXTEL_MAX_LEN) @@ -114,16 +117,28 @@ typedef struct exBusSensor_s { // list of telemetry messages // after every 15 sensors a new header has to be inserted (e.g. "BF D2") const exBusSensor_t jetiExSensors[] = { - {"BF D1", "", EX_TYPE_DES, 0 }, // device descripton - {"Voltage", "V", EX_TYPE_14b, DECIMAL_MASK(1)}, - {"Current", "A", EX_TYPE_14b, DECIMAL_MASK(2)}, - {"Altitude", "m", EX_TYPE_14b, DECIMAL_MASK(2)}, - {"Capacity", "mAh", EX_TYPE_22b, DECIMAL_MASK(0)}, - {"Power", "W", EX_TYPE_22b, DECIMAL_MASK(1)}, - {"Roll angle", "\xB0", EX_TYPE_14b, DECIMAL_MASK(1)}, - {"Pitch angle", "\xB0", EX_TYPE_14b, DECIMAL_MASK(1)}, - {"Heading", "\xB0", EX_TYPE_14b, DECIMAL_MASK(1)}, - {"Vario", "m/s", EX_TYPE_22b, DECIMAL_MASK(2)} + {"BF D1", "", EX_TYPE_DES, 0 }, // device descripton + {"Voltage", "V", EX_TYPE_22b, DECIMAL_MASK(1)}, + {"Current", "A", EX_TYPE_22b, DECIMAL_MASK(2)}, + {"Altitude", "m", EX_TYPE_22b, DECIMAL_MASK(2)}, + {"Capacity", "mAh", EX_TYPE_22b, DECIMAL_MASK(0)}, + {"Power", "W", EX_TYPE_22b, DECIMAL_MASK(1)}, + {"Roll angle", "\xB0", EX_TYPE_22b, DECIMAL_MASK(1)}, + {"Pitch angle", "\xB0", EX_TYPE_22b, DECIMAL_MASK(1)}, + {"Heading", "\xB0", EX_TYPE_22b, DECIMAL_MASK(1)}, + {"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 @@ -136,9 +151,27 @@ enum exSensors_e { EX_ROLL_ANGLE, EX_PITCH_ANGLE, 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)) static uint8_t jetiExBusTelemetryFrame[40]; @@ -182,7 +215,6 @@ void initJetiExBusTelemetry(void) jetiExTelemetryFrame[EXTEL_HEADER_LSN_HB] = 0x00; jetiExTelemetryFrame[EXTEL_HEADER_RES] = 0x00; // reserved, by default 0x00 - //exSensorEnabled = 0x3fe; // Check which sensors are available if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) { bitArraySet(&exSensorEnabled, EX_VOLTAGE); @@ -201,10 +233,25 @@ void initJetiExBusTelemetry(void) if (sensors(SENSOR_ACC)) { bitArraySet(&exSensorEnabled, EX_ROLL_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)) { 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 } @@ -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]); } +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) { switch(sensor) { case EX_VOLTAGE: - return getBatteryVoltageLatest(); + return getBatteryVoltage(); break; case EX_CURRENT: - return getAmperageLatest(); + return getAmperage(); break; case EX_ALTITUDE: - return getEstimatedAltitudeCm() / 100; + return getEstimatedAltitudeCm(); break; case EX_CAPACITY: @@ -244,7 +307,7 @@ int32_t getSensorValue(uint8_t sensor) break; case EX_POWER: - return (getBatteryVoltageLatest() * getAmperageLatest() / 100); + return (getBatteryVoltage() * getAmperage() / 100); break; case EX_ROLL_ANGLE: @@ -263,6 +326,50 @@ int32_t getSensorValue(uint8_t sensor) return getEstimatedVario(); 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: return -1; } @@ -289,7 +396,7 @@ uint8_t createExTelemetryValueMessage(uint8_t *exMessage, uint8_t item) uint8_t messageSize; uint32_t sensorValue; - exMessage[EXTEL_HEADER_LSN_LB] = item & 0xF0; // Device ID + exMessage[EXTEL_HEADER_LSN_LB] = item & 0xF0; // Device ID uint8_t *p = &exMessage[EXTEL_HEADER_ID]; while (item < sensorItemMaxGroup) { @@ -303,7 +410,11 @@ uint8_t createExTelemetryValueMessage(uint8_t *exMessage, uint8_t item) sensorValue = sensorValue >> 8; iCount--; } - *p++ = (sensorValue & 0x9F) | jetiExSensors[item].decimals; + if(jetiExSensors[item].exDataType != EX_TYPE_GPS){ + *p++ = (sensorValue & 0x9F) | jetiExSensors[item].decimals; + }else{ + *p++ = sensorValue; + } 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)) { - // switch to TX mode if (serialRxBytesWaiting(jetiExBusPort) == 0) { - serialSetMode(jetiExBusPort, MODE_TX); jetiExBusTransceiveState = EXBUS_TRANS_TX; item = sendJetiExBusTelemetry(jetiExBusRequestFrame[EXBUS_HEADER_PACKET_ID], item); jetiExBusRequestState = EXBUS_STATE_PROCESSED; @@ -376,7 +485,6 @@ void handleJetiExBusTelemetry(void) // check the state if transmit is ready if (jetiExBusTransceiveState == EXBUS_TRANS_IS_TX_COMPLETED) { if (isSerialTransmitBufferEmpty(jetiExBusPort)) { - serialSetMode(jetiExBusPort, MODE_RX); jetiExBusTransceiveState = EXBUS_TRANS_RX; jetiExBusRequestState = EXBUS_STATE_ZERO; } @@ -416,6 +524,4 @@ uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item) return item; } #endif // TELEMETRY -#endif // SERIAL_RX - - +#endif // SERIAL_RX \ No newline at end of file From b38ae4148e4ad3752b64e438e76751cef522f5af Mon Sep 17 00:00:00 2001 From: Thomas Miric Date: Thu, 20 Sep 2018 19:09:55 +0200 Subject: [PATCH 2/2] The GPS data will not be transmitted until the sensor(SENSOR_GPS) is not valid ...and corrected code formating. --- src/main/telemetry/jetiexbus.c | 54 +++++++++++++++++++++++++--------- 1 file changed, 40 insertions(+), 14 deletions(-) diff --git a/src/main/telemetry/jetiexbus.c b/src/main/telemetry/jetiexbus.c index 694081c642..07972ae68e 100644 --- a/src/main/telemetry/jetiexbus.c +++ b/src/main/telemetry/jetiexbus.c @@ -193,6 +193,29 @@ uint8_t calcCRC8(uint8_t *pt, uint8_t msgLen) return(crc); } +void enableGpsTelemetry(bool enable) +{ + if (enable) { + 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); + } else { + bitArrayClr(&exSensorEnabled, EX_GPS_SATS); + bitArrayClr(&exSensorEnabled, EX_GPS_LONG); + bitArrayClr(&exSensorEnabled, EX_GPS_LAT); + bitArrayClr(&exSensorEnabled, EX_GPS_SPEED); + bitArrayClr(&exSensorEnabled, EX_GPS_DISTANCE_TO_HOME); + bitArrayClr(&exSensorEnabled, EX_GPS_DIRECTION_TO_HOME); + bitArrayClr(&exSensorEnabled, EX_GPS_HEADING); + bitArrayClr(&exSensorEnabled, EX_GPS_ALTITUDE); + } +} + /* * ----------------------------------------------- * Jeti Ex Bus Telemetry @@ -241,16 +264,7 @@ void initJetiExBusTelemetry(void) 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); - } + enableGpsTelemetry(featureIsEnabled(FEATURE_GPS)); firstActiveSensor = getNextActiveSensor(0); // find the first active sensor } @@ -289,7 +303,7 @@ uint32_t calcGpsDDMMmmm(int32_t value, bool isLong) int32_t getSensorValue(uint8_t sensor) { - switch(sensor) { + switch (sensor) { case EX_VOLTAGE: return getBatteryVoltage(); break; @@ -410,9 +424,9 @@ uint8_t createExTelemetryValueMessage(uint8_t *exMessage, uint8_t item) sensorValue = sensorValue >> 8; iCount--; } - if(jetiExSensors[item].exDataType != EX_TYPE_GPS){ + if (jetiExSensors[item].exDataType != EX_TYPE_GPS) { *p++ = (sensorValue & 0x9F) | jetiExSensors[item].decimals; - }else{ + } else { *p++ = sensorValue; } @@ -495,6 +509,7 @@ uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item) { static uint8_t sensorDescriptionCounter = 0xFF; static uint8_t requestLoop = 0xFF; + static bool allSensorsActive = true; uint8_t *jetiExTelemetryFrame = &jetiExBusTelemetryFrame[EXBUS_HEADER_DATA]; if (requestLoop) { @@ -510,12 +525,23 @@ uint8_t sendJetiExBusTelemetry(uint8_t packetID, uint8_t item) createExTelemetryTextMessage(jetiExTelemetryFrame, sensorDescriptionCounter, &jetiExSensors[sensorDescriptionCounter]); createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID); requestLoop--; - if (requestLoop == 0){ + if (requestLoop == 0) { item = firstActiveSensor; + if (featureIsEnabled(FEATURE_GPS)) { + enableGpsTelemetry(false); + allSensorsActive = false; + } } } else { item = createExTelemetryValueMessage(jetiExTelemetryFrame, item); createExBusMessage(jetiExBusTelemetryFrame, jetiExTelemetryFrame, packetID); + + if (!allSensorsActive) { + if (sensors(SENSOR_GPS)) { + enableGpsTelemetry(true); + allSensorsActive = true; + } + } } serialWriteBuf(jetiExBusPort, jetiExBusTelemetryFrame, jetiExBusTelemetryFrame[EXBUS_HEADER_MSG_LEN]);