Easier to understand vbatt telem?

robomaniac · Feb 25, 2022 · 86dc161 · 86dc161
1 parent 2e719a8
commit 86dc161
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 3 deletions.
diff --git a/lib/CrsfSerial/crsf_protocol.h b/lib/CrsfSerial/crsf_protocol.h
@@ -124,3 +124,23 @@ typedef struct crsf_sensor_battery_s
     unsigned capacity : 24; // mah
     unsigned remaining : 8; // %
 } PACKED crsf_sensor_battery_t;
+
+#if !defined(__linux__)
+static inline uint16_t htobe16(uint16_t val)
+{
+#if (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__)
+    return val;
+#else
+    return __builtin_bswap16(val);
+#endif
+}
+
+static inline uint32_t htobe32(uint32_t val)
+{
+#if (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__)
+    return val;
+#else
+    return __builtin_bswap32(val);
+#endif
+}
+#endif
diff --git a/src/main.cpp b/src/main.cpp
@@ -131,10 +131,10 @@ static void checkVbatt()
     unsigned int adc = g_State.vbatSmooth;
     g_State.vbatValue = 330U * adc * (VBAT_R1 + VBAT_R2) / VBAT_R2 / ((1 << 12) - 1);
 
-    uint8_t crsfbatt[CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE] = { 0 };
+    crsf_sensor_battery_t crsfbatt = { 0 };
     uint16_t scaledVoltage = g_State.vbatValue * VBAT_SCALE;
-    crsfbatt[0] = scaledVoltage >> 8;
-    crsfbatt[1] = scaledVoltage & 0xff;
+    // Values are MSB first (BigEndian)
+    crsfbatt.voltage = htobe16(scaledVoltage);
     crsf.queuePacket(CRSF_SYNC_BYTE, CRSF_FRAMETYPE_BATTERY_SENSOR, &crsfbatt, sizeof(crsfbatt));
 
     //Serial.print("ADC="); Serial.print(adc, DEC);