rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Updated CRSF telemetry

This commit is contained in:
Martin Budden 2016-11-25 11:35:36 +00:00
parent f51f11d7cc
commit 22c5b88fd0
3 changed files with 23 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
src/main/telemetry/crsf.c
View File

@ -142,7 +142,7 @@ CRC: (uint8_t)
Payload: Payload:
int32_t Latitude ( degree / 10`000`000 ) int32_t Latitude ( degree / 10`000`000 )
int32_t Longitude (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_t GPS heading ( degree / 100 )
uint16 Altitude ( meter ­ 1000m offset ) uint16 Altitude ( meter ­ 1000m offset )
uint8_t Satellites in use ( counter ) uint8_t Satellites in use ( counter )
@ -154,7 +154,7 @@ void crsfFrameGps(sbuf_t *dst)
crsfSerialize8(dst, CRSF_FRAMETYPE_GPS); crsfSerialize8(dst, CRSF_FRAMETYPE_GPS);
crsfSerialize32(dst, GPS_coord[LAT]); // CRSF and betaflight use same units for degrees crsfSerialize32(dst, GPS_coord[LAT]); // CRSF and betaflight use same units for degrees
crsfSerialize32(dst, GPS_coord[LON]); 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 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) //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; const uint16_t altitude = (STATE(GPS_FIX) ? GPS_altitude : 0) + 1000;
@ -215,39 +215,6 @@ typedef enum {
CRSF_RF_POWER_2000_mW = 6, CRSF_RF_POWER_2000_mW = 6,
} crsrRfPower_e; } 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 0x1E Attitude
Payload: Payload:
@ -301,14 +268,10 @@ void crsfFrameFlightMode(sbuf_t *dst)
#define BV(x) (1 << (x)) // bit value #define BV(x) (1 << (x)) // bit value
// schedule array to decide how often each type of frame is sent // schedule array to decide how often each type of frame is sent
#define CRSF_SCHEDULE_COUNT 5 #define CRSF_SCHEDULE_COUNT_MAX 5
static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT] = { static uint8_t crsfScheduleCount;
BV(CRSF_FRAME_ATTITUDE), static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX];
BV(CRSF_FRAME_BATTERY_SENSOR),
BV(CRSF_FRAME_LINK_STATISTICS),
BV(CRSF_FRAME_FLIGHT_MODE),
BV(CRSF_FRAME_GPS),
};
static void processCrsf(void) static void processCrsf(void)
{ {
@ -328,11 +291,6 @@ static void processCrsf(void)
crsfFrameBatterySensor(dst); crsfFrameBatterySensor(dst);
crsfFinalize(dst); crsfFinalize(dst);
} }
if (currentSchedule & BV(CRSF_FRAME_LINK_STATISTICS)) {
crsfInitializeFrame(dst);
crsfFrameLinkStatistics(dst);
crsfFinalize(dst);
}
if (currentSchedule & BV(CRSF_FRAME_FLIGHT_MODE)) { if (currentSchedule & BV(CRSF_FRAME_FLIGHT_MODE)) {
crsfInitializeFrame(dst); crsfInitializeFrame(dst);
crsfFrameFlightMode(dst); crsfFrameFlightMode(dst);
@ -345,7 +303,7 @@ static void processCrsf(void)
crsfFinalize(dst); crsfFinalize(dst);
} }
#endif #endif
crsfScheduleIndex = (crsfScheduleIndex + 1) % CRSF_SCHEDULE_COUNT; crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount;
} }
void initCrsfTelemetry(void) 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) // 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 // and feature is enabled, if so, set CRSF telemetry enabled
crsfTelemetryEnabled = crsfRxIsActive(); 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) bool checkCrsfTelemetryState(void)
@ -396,9 +363,6 @@ int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
case CRSF_FRAME_BATTERY_SENSOR: case CRSF_FRAME_BATTERY_SENSOR:
crsfFrameBatterySensor(sbuf); crsfFrameBatterySensor(sbuf);
break; break;
case CRSF_FRAME_LINK_STATISTICS:
crsfFrameLinkStatistics(sbuf);
break;
case CRSF_FRAME_FLIGHT_MODE: case CRSF_FRAME_FLIGHT_MODE:
crsfFrameFlightMode(sbuf); crsfFrameFlightMode(sbuf);
break; break;

1
src/main/telemetry/crsf.h
View File

@ -23,7 +23,6 @@ typedef enum {
CRSF_FRAME_START = 0, CRSF_FRAME_START = 0,
CRSF_FRAME_ATTITUDE = CRSF_FRAME_START, CRSF_FRAME_ATTITUDE = CRSF_FRAME_START,
CRSF_FRAME_BATTERY_SENSOR, CRSF_FRAME_BATTERY_SENSOR,
CRSF_FRAME_LINK_STATISTICS,
CRSF_FRAME_FLIGHT_MODE, CRSF_FRAME_FLIGHT_MODE,
CRSF_FRAME_GPS, CRSF_FRAME_GPS,
CRSF_FRAME_COUNT CRSF_FRAME_COUNT

24
src/test/unit/telemetry_crsf_unittest.cc
View File

@ -68,10 +68,11 @@ uint8_t crfsCrc(uint8_t *frame, int frameLen)
} }
return crc; return crc;
} }
/* /*
int32_t Latitude ( degree / 10`000`000 ) int32_t Latitude ( degree / 10`000`000 )
int32_t Longitude (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_t GPS heading ( degree / 100 )
uint16 Altitude ( meter ­ 1000m offset ) uint16 Altitude ( meter ­ 1000m offset )
uint8_t Satellites in use ( counter ) 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_altitude; // altitude in m
uint16_t GPS_speed; // speed in 0.1m/s uint16_t GPS_speed; // speed in 0.1m/s
uint16_t GPS_ground_course = 0; // degrees * 10 uint16_t GPS_ground_course = 0; // degrees * 10
*/
*/
#define FRAME_HEADER_FOOTER_LEN 4 #define FRAME_HEADER_FOOTER_LEN 4
TEST(TelemetryCrsfTest, TestGPS) TEST(TelemetryCrsfTest, TestGPS)
@ -112,7 +112,7 @@ TEST(TelemetryCrsfTest, TestGPS)
GPS_coord[LON] = 163 * GPS_DEGREES_DIVIDER; GPS_coord[LON] = 163 * GPS_DEGREES_DIVIDER;
ENABLE_STATE(GPS_FIX); ENABLE_STATE(GPS_FIX);
GPS_altitude = 2345; // altitude in m 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_numSat = 9;
GPS_ground_course = 1479; // degrees * 10 GPS_ground_course = 1479; // degrees * 10
frameLen = getCrsfFrame(frame, CRSF_FRAME_GPS); 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]; longitude = frame[7] << 24 | frame[8] << 16 | frame[9] << 8 | frame[10];
EXPECT_EQ(1630000000, longitude); EXPECT_EQ(1630000000, longitude);
groundSpeed = frame[11] << 8 | frame[12]; groundSpeed = frame[11] << 8 | frame[12];
EXPECT_EQ(5868, groundSpeed); EXPECT_EQ(587, groundSpeed);
GPSheading = frame[13] << 8 | frame[14]; GPSheading = frame[13] << 8 | frame[14];
EXPECT_EQ(14790, GPSheading); EXPECT_EQ(14790, GPSheading);
altitude = frame[15] << 8 | frame[16]; altitude = frame[15] << 8 | frame[16];
@ -169,18 +169,6 @@ TEST(TelemetryCrsfTest, TestBattery)
EXPECT_EQ(crfsCrc(frame, frameLen), frame[11]); 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) TEST(TelemetryCrsfTest, TestAttitude)
{ {
uint8_t frame[CRSF_FRAME_SIZE_MAX]; uint8_t frame[CRSF_FRAME_SIZE_MAX];
@ -303,6 +291,8 @@ void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);}
uint32_t micros(void) {return 0;} uint32_t micros(void) {return 0;}
bool feature(uint32_t) {return true;}
uint32_t serialRxBytesWaiting(const serialPort_t *) {return 0;} uint32_t serialRxBytesWaiting(const serialPort_t *) {return 0;}
uint32_t serialTxBytesFree(const serialPort_t *) {return 0;} uint32_t serialTxBytesFree(const serialPort_t *) {return 0;}
uint8_t serialRead(serialPort_t *) {return 0;} uint8_t serialRead(serialPort_t *) {return 0;}