From 22c5b88fd0c6518ae692bc60d544778aefdd2824 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 25 Nov 2016 11:35:36 +0000 Subject: [PATCH] Updated CRSF telemetry --- src/main/telemetry/crsf.c | 68 ++++++------------------ src/main/telemetry/crsf.h | 1 - src/test/unit/telemetry_crsf_unittest.cc | 24 +++------ 3 files changed, 23 insertions(+), 70 deletions(-) diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index dcf448eda..79a40e298 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -142,7 +142,7 @@ CRC: (uint8_t) Payload: int32_t Latitude ( degree / 10`000`000 ) int32_t Longitude (degree / 10`000`000 ) -uint16_t Groundspeed ( km/h / 100 ) +uint16_t Groundspeed ( km/h / 10 ) uint16_t GPS heading ( degree / 100 ) uint16 Altitude ( meter ­ 1000m offset ) uint8_t Satellites in use ( counter ) @@ -154,7 +154,7 @@ void crsfFrameGps(sbuf_t *dst) crsfSerialize8(dst, CRSF_FRAMETYPE_GPS); crsfSerialize32(dst, GPS_coord[LAT]); // CRSF and betaflight use same units for degrees crsfSerialize32(dst, GPS_coord[LON]); - crsfSerialize16(dst, GPS_speed * 36); // GPS_speed is in 0.1m/s + crsfSerialize16(dst, (GPS_speed * 36 + 5) / 10); // GPS_speed is in 0.1m/s crsfSerialize16(dst, GPS_ground_course * 10); // GPS_ground_course is degrees * 10 //Send real GPS altitude only if it's reliable (there's a GPS fix) const uint16_t altitude = (STATE(GPS_FIX) ? GPS_altitude : 0) + 1000; @@ -215,39 +215,6 @@ typedef enum { CRSF_RF_POWER_2000_mW = 6, } crsrRfPower_e; -/* -0x14 Link statistics -Uplink is the connection from the ground to the UAV and downlink the opposite direction. -Payload: -uint8_t UplinkRSSI Ant.1(dBm*­1) -uint8_t UplinkRSSI Ant.2(dBm*­1) -uint8_t Uplink Package success rate / Link quality ( % ) -int8_t Uplink SNR ( db ) -uint8_t Diversity active antenna ( enum ant. 1 = 0, ant. 2 ) -uint8_t RF Mode ( enum 4fps = 0 , 50fps, 150hz) -uint8_t Uplink TX Power ( enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW ) -uint8_t Downlink RSSI ( dBm * ­-1 ) -uint8_t Downlink package success rate / Link quality ( % ) -int8_t Downlink SNR ( db ) -*/ - -void crsfFrameLinkStatistics(sbuf_t *dst) -{ - // use sbufWrite since CRC does not include frame length - sbufWriteU8(dst, CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); - crsfSerialize8(dst, CRSF_FRAMETYPE_LINK_STATISTICS); - crsfSerialize8(dst, 0); - crsfSerialize8(dst, 0); - crsfSerialize8(dst, 0); - crsfSerialize8(dst, 0); - crsfSerialize8(dst, 0); - crsfSerialize8(dst, 0); - crsfSerialize8(dst, 0); - crsfSerialize8(dst, 0); - crsfSerialize8(dst, 0); - crsfSerialize8(dst, 0); -} - /* 0x1E Attitude Payload: @@ -301,14 +268,10 @@ void crsfFrameFlightMode(sbuf_t *dst) #define BV(x) (1 << (x)) // bit value // schedule array to decide how often each type of frame is sent -#define CRSF_SCHEDULE_COUNT 5 -static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT] = { - BV(CRSF_FRAME_ATTITUDE), - BV(CRSF_FRAME_BATTERY_SENSOR), - BV(CRSF_FRAME_LINK_STATISTICS), - BV(CRSF_FRAME_FLIGHT_MODE), - BV(CRSF_FRAME_GPS), -}; +#define CRSF_SCHEDULE_COUNT_MAX 5 +static uint8_t crsfScheduleCount; +static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX]; + static void processCrsf(void) { @@ -328,11 +291,6 @@ static void processCrsf(void) crsfFrameBatterySensor(dst); crsfFinalize(dst); } - if (currentSchedule & BV(CRSF_FRAME_LINK_STATISTICS)) { - crsfInitializeFrame(dst); - crsfFrameLinkStatistics(dst); - crsfFinalize(dst); - } if (currentSchedule & BV(CRSF_FRAME_FLIGHT_MODE)) { crsfInitializeFrame(dst); crsfFrameFlightMode(dst); @@ -345,7 +303,7 @@ static void processCrsf(void) crsfFinalize(dst); } #endif - crsfScheduleIndex = (crsfScheduleIndex + 1) % CRSF_SCHEDULE_COUNT; + crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount; } void initCrsfTelemetry(void) @@ -353,6 +311,15 @@ void initCrsfTelemetry(void) // check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX) // and feature is enabled, if so, set CRSF telemetry enabled crsfTelemetryEnabled = crsfRxIsActive(); + int index = 0; + crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE); + crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR); + crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE); + if (feature(FEATURE_GPS)) { + crsfSchedule[index++] = BV(CRSF_FRAME_GPS); + } + crsfScheduleCount = (uint8_t)index; + } bool checkCrsfTelemetryState(void) @@ -396,9 +363,6 @@ int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType) case CRSF_FRAME_BATTERY_SENSOR: crsfFrameBatterySensor(sbuf); break; - case CRSF_FRAME_LINK_STATISTICS: - crsfFrameLinkStatistics(sbuf); - break; case CRSF_FRAME_FLIGHT_MODE: crsfFrameFlightMode(sbuf); break; diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index 3b6c5d4cc..d6a126a29 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -23,7 +23,6 @@ typedef enum { CRSF_FRAME_START = 0, CRSF_FRAME_ATTITUDE = CRSF_FRAME_START, CRSF_FRAME_BATTERY_SENSOR, - CRSF_FRAME_LINK_STATISTICS, CRSF_FRAME_FLIGHT_MODE, CRSF_FRAME_GPS, CRSF_FRAME_COUNT diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index 028b0e75a..6f1762c57 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -68,10 +68,11 @@ uint8_t crfsCrc(uint8_t *frame, int frameLen) } return crc; } + /* int32_t Latitude ( degree / 10`000`000 ) int32_t Longitude (degree / 10`000`000 ) -uint16_t Groundspeed ( km/h / 100 ) +uint16_t Groundspeed ( km/h / 10 ) uint16_t GPS heading ( degree / 100 ) uint16 Altitude ( meter ­ 1000m offset ) uint8_t Satellites in use ( counter ) @@ -81,8 +82,7 @@ uint16_t GPS_distanceToHome; // distance to home point in meters uint16_t GPS_altitude; // altitude in m uint16_t GPS_speed; // speed in 0.1m/s uint16_t GPS_ground_course = 0; // degrees * 10 - - */ +*/ #define FRAME_HEADER_FOOTER_LEN 4 TEST(TelemetryCrsfTest, TestGPS) @@ -112,7 +112,7 @@ TEST(TelemetryCrsfTest, TestGPS) GPS_coord[LON] = 163 * GPS_DEGREES_DIVIDER; ENABLE_STATE(GPS_FIX); GPS_altitude = 2345; // altitude in m - GPS_speed = 163; // speed in 0.1m/s, 16.3 m/s = 58.68 km/h + GPS_speed = 163; // speed in 0.1m/s, 16.3 m/s = 58.68 km/h, so CRSF (km/h *10) value is 587 GPS_numSat = 9; GPS_ground_course = 1479; // degrees * 10 frameLen = getCrsfFrame(frame, CRSF_FRAME_GPS); @@ -121,7 +121,7 @@ TEST(TelemetryCrsfTest, TestGPS) longitude = frame[7] << 24 | frame[8] << 16 | frame[9] << 8 | frame[10]; EXPECT_EQ(1630000000, longitude); groundSpeed = frame[11] << 8 | frame[12]; - EXPECT_EQ(5868, groundSpeed); + EXPECT_EQ(587, groundSpeed); GPSheading = frame[13] << 8 | frame[14]; EXPECT_EQ(14790, GPSheading); altitude = frame[15] << 8 | frame[16]; @@ -169,18 +169,6 @@ TEST(TelemetryCrsfTest, TestBattery) EXPECT_EQ(crfsCrc(frame, frameLen), frame[11]); } -TEST(TelemetryCrsfTest, TestLinkStatistics) -{ - uint8_t frame[CRSF_FRAME_SIZE_MAX]; - - int frameLen = getCrsfFrame(frame, CRSF_FRAME_LINK_STATISTICS); - EXPECT_EQ(CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen); - EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address - EXPECT_EQ(12, frame[1]); // length - EXPECT_EQ(0x14, frame[2]); // type - EXPECT_EQ(crfsCrc(frame, frameLen), frame[13]); -} - TEST(TelemetryCrsfTest, TestAttitude) { uint8_t frame[CRSF_FRAME_SIZE_MAX]; @@ -303,6 +291,8 @@ void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);} uint32_t micros(void) {return 0;} +bool feature(uint32_t) {return true;} + uint32_t serialRxBytesWaiting(const serialPort_t *) {return 0;} uint32_t serialTxBytesFree(const serialPort_t *) {return 0;} uint8_t serialRead(serialPort_t *) {return 0;}