Merge pull request #2772 from martinbudden/bf_sbuf_bigendian

Added big-endian support to sbuf and used it in CRSF

src/main/common/streambuf.c

@@ -39,6 +39,21 @@ void sbufWriteU32(sbuf_t *dst, uint32_t val)
     sbufWriteU8(dst, val >> 24);
 }
 
+void sbufWriteU16BigEndian(sbuf_t *dst, uint16_t val)
+{
+    sbufWriteU8(dst, val >> 8);
+    sbufWriteU8(dst, (uint8_t)val);
+}
+
+void sbufWriteU32BigEndian(sbuf_t *dst, uint32_t val)
+{
+    sbufWriteU8(dst, val >> 24);
+    sbufWriteU8(dst, val >> 16);
+    sbufWriteU8(dst, val >> 8);
+    sbufWriteU8(dst, (uint8_t)val);
+}
+
+
 void sbufWriteData(sbuf_t *dst, const void *data, int len)
 {
     memcpy(dst->ptr, data, len);
@@ -47,7 +62,7 @@ void sbufWriteData(sbuf_t *dst, const void *data, int len)
 
 void sbufWriteString(sbuf_t *dst, const char *string)
 {
-    sbufWriteData(dst, string, strlen(string));
+    sbufWriteData(dst, string, strlen(string) + 1); // include zero terminator
 }
 
 uint8_t sbufReadU8(sbuf_t *src)
@@ -90,6 +105,11 @@ uint8_t* sbufPtr(sbuf_t *buf)
     return buf->ptr;
 }
 
+const uint8_t* sbufConstPtr(const sbuf_t *buf)
+{
+    return buf->ptr;
+}
+
 // advance buffer pointer
 // reader - skip data
 // writer - commit written data

src/main/common/streambuf.h

@@ -30,6 +30,8 @@ typedef struct sbuf_s {
 void sbufWriteU8(sbuf_t *dst, uint8_t val);
 void sbufWriteU16(sbuf_t *dst, uint16_t val);
 void sbufWriteU32(sbuf_t *dst, uint32_t val);
+void sbufWriteU16BigEndian(sbuf_t *dst, uint16_t val);
+void sbufWriteU32BigEndian(sbuf_t *dst, uint32_t val);
 void sbufWriteData(sbuf_t *dst, const void *data, int len);
 void sbufWriteString(sbuf_t *dst, const char *string);
 
@@ -40,6 +42,7 @@ void sbufReadData(sbuf_t *dst, void *data, int len);
 
 int sbufBytesRemaining(sbuf_t *buf);
 uint8_t* sbufPtr(sbuf_t *buf);
+const uint8_t* sbufConstPtr(const sbuf_t *buf);
 void sbufAdvance(sbuf_t *buf, int size);
 
 void sbufSwitchToReader(sbuf_t *buf, uint8_t * base);

src/main/telemetry/crsf.c

@@ -67,50 +67,29 @@
 #define CRSF_CYCLETIME_US                   100000 // 100ms, 10 Hz
 
 static bool crsfTelemetryEnabled;
-static uint8_t crsfCrc;
 static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX];
 
 static void crsfInitializeFrame(sbuf_t *dst)
 {
-    crsfCrc = 0;
     dst->ptr = crsfFrame;
     dst->end = ARRAYEND(crsfFrame);
 
     sbufWriteU8(dst, CRSF_ADDRESS_BROADCAST);
 }
 
-static void crsfSerialize8(sbuf_t *dst, uint8_t v)
+static void crsfWriteCrc(sbuf_t *dst, uint8_t *start)
 {
-    sbufWriteU8(dst, v);
+    uint8_t crc = 0;
-    crsfCrc = crc8_dvb_s2(crsfCrc, v);
+    uint8_t *end = sbufPtr(dst);
-}
+    for (uint8_t *ptr = start; ptr < end; ++ptr) {
-
+        crc = crc8_dvb_s2(crc, *ptr);
-static void crsfSerialize16(sbuf_t *dst, uint16_t v)
-{
-    // Use BigEndian format
-    crsfSerialize8(dst,  (v >> 8));
-    crsfSerialize8(dst, (uint8_t)v);
-}
-
-static void crsfSerialize32(sbuf_t *dst, uint32_t v)
-{
-    // Use BigEndian format
-    crsfSerialize8(dst, (v >> 24));
-    crsfSerialize8(dst, (v >> 16));
-    crsfSerialize8(dst, (v >> 8));
-    crsfSerialize8(dst, (uint8_t)v);
-}
-
-static void crsfSerializeData(sbuf_t *dst, const uint8_t *data, int len)
-{
-    for (int ii = 0; ii< len; ++ii) {
-        crsfSerialize8(dst, data[ii]);
     }
+    sbufWriteU8(dst, crc);
 }
 
 static void crsfFinalize(sbuf_t *dst)
 {
-    sbufWriteU8(dst, crsfCrc);
+    crsfWriteCrc(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length
     sbufSwitchToReader(dst, crsfFrame);
     // write the telemetry frame to the receiver.
     crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
@@ -118,7 +97,7 @@ static void crsfFinalize(sbuf_t *dst)
 
 static int crsfFinalizeBuf(sbuf_t *dst, uint8_t *frame)
 {
-    sbufWriteU8(dst, crsfCrc);
+    crsfWriteCrc(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length
     sbufSwitchToReader(dst, crsfFrame);
     const int frameSize = sbufBytesRemaining(dst);
     for (int ii = 0; sbufBytesRemaining(dst); ++ii) {
@@ -133,7 +112,7 @@ CRSF frame has the structure:
 Device address: (uint8_t)
 Frame length:   length in  bytes including Type (uint8_t)
 Type:           (uint8_t)
-CRC:            (uint8_t)
+CRC:            (uint8_t), crc of <Type> and <Payload>
 */
 
 /*
@@ -150,15 +129,15 @@ void crsfFrameGps(sbuf_t *dst)
 {
     // use sbufWrite since CRC does not include frame length
     sbufWriteU8(dst, CRSF_FRAME_GPS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
-    crsfSerialize8(dst, CRSF_FRAMETYPE_GPS);
+    sbufWriteU8(dst, CRSF_FRAMETYPE_GPS);
-    crsfSerialize32(dst, GPS_coord[LAT]); // CRSF and betaflight use same units for degrees
+    sbufWriteU32BigEndian(dst, GPS_coord[LAT]); // CRSF and betaflight use same units for degrees
-    crsfSerialize32(dst, GPS_coord[LON]);
+    sbufWriteU32BigEndian(dst, GPS_coord[LON]);
-    crsfSerialize16(dst, (GPS_speed * 36 + 5) / 10); // GPS_speed is in 0.1m/s
+    sbufWriteU16BigEndian(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
+    sbufWriteU16BigEndian(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;
-    crsfSerialize16(dst, altitude);
+    sbufWriteU16BigEndian(dst, altitude);
-    crsfSerialize8(dst, GPS_numSat);
+    sbufWriteU8(dst, GPS_numSat);
 }
 
 /*
@@ -173,24 +152,24 @@ void crsfFrameBatterySensor(sbuf_t *dst)
 {
     // use sbufWrite since CRC does not include frame length
     sbufWriteU8(dst, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
-    crsfSerialize8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR);
+    sbufWriteU8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR);
-    crsfSerialize16(dst, getBatteryVoltage()); // vbat is in units of 0.1V
+    sbufWriteU16BigEndian(dst, getBatteryVoltage()); // vbat is in units of 0.1V
 #ifdef CLEANFLIGHT
     const amperageMeter_t *amperageMeter = getAmperageMeter(batteryConfig()->amperageMeterSource);
     const int16_t amperage = constrain(amperageMeter->amperage, -0x8000, 0x7FFF) / 10; // send amperage in 0.01 A steps, range is -320A to 320A
-    crsfSerialize16(dst, amperage); // amperage is in units of 0.1A
+    sbufWriteU16BigEndian(dst, amperage); // amperage is in units of 0.1A
     const uint32_t batteryCapacity = batteryConfig()->batteryCapacity;
     const uint8_t batteryRemainingPercentage = batteryCapacityRemainingPercentage();
 #else
-    crsfSerialize16(dst, getAmperage() / 10);
+    sbufWriteU16BigEndian(dst, getAmperage() / 10);
     const uint32_t batteryCapacity = batteryConfig()->batteryCapacity;
     const uint8_t batteryRemainingPercentage = calculateBatteryPercentageRemaining();
 #endif
-    crsfSerialize8(dst, (batteryCapacity >> 16));
+    sbufWriteU8(dst, (batteryCapacity >> 16));
-    crsfSerialize8(dst, (batteryCapacity >> 8));
+    sbufWriteU8(dst, (batteryCapacity >> 8));
-    crsfSerialize8(dst, (uint8_t)batteryCapacity);
+    sbufWriteU8(dst, (uint8_t)batteryCapacity);
 
-    crsfSerialize8(dst, batteryRemainingPercentage);
+    sbufWriteU8(dst, batteryRemainingPercentage);
 }
 
 typedef enum {
@@ -227,10 +206,10 @@ int16_t     Yaw angle ( rad / 10000 )
 void crsfFrameAttitude(sbuf_t *dst)
 {
      sbufWriteU8(dst, CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
-     crsfSerialize8(dst, CRSF_FRAMETYPE_ATTITUDE);
+     sbufWriteU8(dst, CRSF_FRAMETYPE_ATTITUDE);
-     crsfSerialize16(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.pitch));
+     sbufWriteU16BigEndian(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.pitch));
-     crsfSerialize16(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.roll));
+     sbufWriteU16BigEndian(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.roll));
-     crsfSerialize16(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.yaw));
+     sbufWriteU16BigEndian(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.yaw));
 }
 
 /*
@@ -240,11 +219,10 @@ char[]      Flight mode ( Null­terminated string )
 */
 void crsfFrameFlightMode(sbuf_t *dst)
 {
-    // just do Angle for the moment as a placeholder
     // write zero for frame length, since we don't know it yet
     uint8_t *lengthPtr = sbufPtr(dst);
     sbufWriteU8(dst, 0);
-    crsfSerialize8(dst, CRSF_FRAMETYPE_FLIGHT_MODE);
+    sbufWriteU8(dst, CRSF_FRAMETYPE_FLIGHT_MODE);
 
     // use same logic as OSD, so telemetry displays same flight text as OSD
     const char *flightMode = "ACRO";
@@ -258,9 +236,8 @@ void crsfFrameFlightMode(sbuf_t *dst)
     } else if (FLIGHT_MODE(HORIZON_MODE)) {
         flightMode = "HOR";
     }
-    crsfSerializeData(dst, (const uint8_t*)flightMode, strlen(flightMode));
+    sbufWriteString(dst, flightMode);
-    crsfSerialize8(dst, 0); // zero terminator for string
+    // write in the frame length
-    // write in the length
     *lengthPtr = sbufPtr(dst) - lengthPtr;
 }