Implemented flight mode telemetry

src/main/telemetry/crsf.c

@@ -44,6 +44,7 @@
 #include "io/gps.h"
 #include "io/serial.h"
 
+#include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
 
 #include "io/gps.h"
@@ -104,6 +105,13 @@ static void crsfSerialize32(sbuf_t *dst, uint32_t v)
     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]);
+    }
+}
+
 static void crsfFinalize(sbuf_t *dst)
 {
     sbufWriteU8(dst, crsfCrc);
@@ -268,15 +276,26 @@ char[]      Flight mode ( Null­terminated string )
 void crsfFrameFlightMode(sbuf_t *dst)
 {
     // just do Angle for the moment as a placeholder
-    const char *flightMode = "Angle";
+    // write zero for frame length, since we don't know it yet
-    // use sbufWrite since CRC does not include frame length
+    uint8_t *lengthPtr = sbufPtr(dst);
-    sbufWriteU8(dst, strlen(flightMode) + 1 + CRSF_FRAME_LENGTH_TYPE_CRC);
+    sbufWriteU8(dst, 0);
     crsfSerialize8(dst, CRSF_FRAMETYPE_FLIGHT_MODE);
-    const int len = strlen(flightMode);
+
-    for (int ii = 0; ii< len; ++ii) {
+    const char *flightMode = "ACRO";
-        crsfSerialize8(dst, flightMode[ii]);
+    if (isAirmodeActive()) {
+        flightMode = "AIR";
     }
-    crsfSerialize8(dst, 0);
+    if (FLIGHT_MODE(FAILSAFE_MODE)) {
+        flightMode = "!FS";
+    } else if (FLIGHT_MODE(ANGLE_MODE)) {
+        flightMode = "STAB";
+    } else if (FLIGHT_MODE(HORIZON_MODE)) {
+        flightMode = "HOR";
+    }
+    crsfSerializeData(dst, (const uint8_t*)flightMode, strlen(flightMode));
+    crsfSerialize8(dst, 0); // zero terminator for string
+    // write in the length
+    *lengthPtr = sbufPtr(dst) - lengthPtr;
 }
 
 #define BV(x)  (1 << (x)) // bit value

src/test/unit/telemetry_crsf_unittest.cc

@@ -53,6 +53,7 @@ extern "C" {
     #include "flight/imu.h"
     #include "flight/gps_conversion.h"
 
+    bool airMode;
 }
 
 #include "unittest_macros.h"
@@ -81,12 +82,14 @@ 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)
 {
     uint8_t frame[CRSF_FRAME_SIZE_MAX];
 
     int frameLen = getCrsfFrame(frame, CRSF_FRAME_GPS);
-    EXPECT_EQ(CRSF_FRAME_GPS_PAYLOAD_SIZE + 4, frameLen);
+    EXPECT_EQ(CRSF_FRAME_GPS_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen);
     EXPECT_EQ(CRSF_RECEIVER_ADDRESS, frame[0]); // address
     EXPECT_EQ(17, frame[1]); // length
     EXPECT_EQ(0x02, frame[2]); // type
@@ -136,7 +139,7 @@ TEST(TelemetryCrsfTest, TestBattery)
     memset(batteryConfig, 0, sizeof(batteryConfig_t));
     vbat = 0; // 0.1V units
     int frameLen = getCrsfFrame(frame, CRSF_FRAME_BATTERY_SENSOR);
-    EXPECT_EQ(CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + 4, frameLen);
+    EXPECT_EQ(CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen);
     EXPECT_EQ(CRSF_RECEIVER_ADDRESS, frame[0]); // address
     EXPECT_EQ(10, frame[1]); // length
     EXPECT_EQ(0x08, frame[2]); // type
@@ -170,7 +173,7 @@ 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 + 4, frameLen);
+    EXPECT_EQ(CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen);
     EXPECT_EQ(CRSF_RECEIVER_ADDRESS, frame[0]); // address
     EXPECT_EQ(12, frame[1]); // length
     EXPECT_EQ(0x14, frame[2]); // type
@@ -185,7 +188,7 @@ TEST(TelemetryCrsfTest, TestAttitude)
     attitude.values.roll = 0;
     attitude.values.yaw = 0;
     int frameLen = getCrsfFrame(frame, CRSF_FRAME_ATTITUDE);
-    EXPECT_EQ(CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + 4, frameLen);
+    EXPECT_EQ(CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen);
     EXPECT_EQ(CRSF_RECEIVER_ADDRESS, frame[0]); // address
     EXPECT_EQ(8, frame[1]); // length
     EXPECT_EQ(0x1e, frame[2]); // type
@@ -214,20 +217,61 @@ TEST(TelemetryCrsfTest, TestFlightMode)
 {
     uint8_t frame[CRSF_FRAME_SIZE_MAX];
 
+    // nothing set, so ACRO mode
+    airMode = false;
     int frameLen = getCrsfFrame(frame, CRSF_FRAME_FLIGHT_MODE);
-    EXPECT_EQ(6 + 4, frameLen);
+    EXPECT_EQ(5 + FRAME_HEADER_FOOTER_LEN, frameLen);
-    enableFlightMode(ANGLE_MODE);
-    EXPECT_EQ(1, FLIGHT_MODE(ANGLE_MODE));
     EXPECT_EQ(CRSF_RECEIVER_ADDRESS, frame[0]); // address
-    EXPECT_EQ(8, frame[1]); // length
+    EXPECT_EQ(7, frame[1]); // length
     EXPECT_EQ(0x21, frame[2]); // type
-    EXPECT_EQ('A', frame[3]); // type
+    EXPECT_EQ('A', frame[3]);
-    EXPECT_EQ('n', frame[4]); // type
+    EXPECT_EQ('C', frame[4]);
-    EXPECT_EQ('g', frame[5]); // type
+    EXPECT_EQ('R', frame[5]);
-    EXPECT_EQ('l', frame[6]); // type
+    EXPECT_EQ('O', frame[6]);
-    EXPECT_EQ('e', frame[7]); // type
+    EXPECT_EQ(0, frame[7]);
-    EXPECT_EQ(0, frame[8]); // type
+    EXPECT_EQ(crfsCrc(frame, frameLen), frame[8]);
-    EXPECT_EQ(crfsCrc(frame, frameLen), frame[9]);
+
+
+    enableFlightMode(ANGLE_MODE);
+    EXPECT_EQ(ANGLE_MODE, FLIGHT_MODE(ANGLE_MODE));
+    frameLen = getCrsfFrame(frame, CRSF_FRAME_FLIGHT_MODE);
+    EXPECT_EQ(5 + FRAME_HEADER_FOOTER_LEN, frameLen);
+    EXPECT_EQ(CRSF_RECEIVER_ADDRESS, frame[0]); // address
+    EXPECT_EQ(7, frame[1]); // length
+    EXPECT_EQ(0x21, frame[2]); // type
+    EXPECT_EQ('S', frame[3]);
+    EXPECT_EQ('T', frame[4]);
+    EXPECT_EQ('A', frame[5]);
+    EXPECT_EQ('B', frame[6]);
+    EXPECT_EQ(0, frame[7]);
+    EXPECT_EQ(crfsCrc(frame, frameLen), frame[8]);
+
+    disableFlightMode(ANGLE_MODE);
+    enableFlightMode(HORIZON_MODE);
+    EXPECT_EQ(HORIZON_MODE, FLIGHT_MODE(HORIZON_MODE));
+    frameLen = getCrsfFrame(frame, CRSF_FRAME_FLIGHT_MODE);
+    EXPECT_EQ(4 + FRAME_HEADER_FOOTER_LEN, frameLen);
+    EXPECT_EQ(CRSF_RECEIVER_ADDRESS, frame[0]); // address
+    EXPECT_EQ(6, frame[1]); // length
+    EXPECT_EQ(0x21, frame[2]); // type
+    EXPECT_EQ('H', frame[3]);
+    EXPECT_EQ('O', frame[4]);
+    EXPECT_EQ('R', frame[5]);
+    EXPECT_EQ(0, frame[6]);
+    EXPECT_EQ(crfsCrc(frame, frameLen), frame[7]);
+
+    disableFlightMode(HORIZON_MODE);
+    airMode = true;
+    frameLen = getCrsfFrame(frame, CRSF_FRAME_FLIGHT_MODE);
+    EXPECT_EQ(4 + FRAME_HEADER_FOOTER_LEN, frameLen);
+    EXPECT_EQ(CRSF_RECEIVER_ADDRESS, frame[0]); // address
+    EXPECT_EQ(6, frame[1]); // length
+    EXPECT_EQ(0x21, frame[2]); // type
+    EXPECT_EQ('A', frame[3]);
+    EXPECT_EQ('I', frame[4]);
+    EXPECT_EQ('R', frame[5]);
+    EXPECT_EQ(0, frame[6]);
+    EXPECT_EQ(crfsCrc(frame, frameLen), frame[7]);
 }
 
 // STUBS
@@ -256,73 +300,25 @@ int32_t mAhDrawn;
 
 void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);}
 
-uint32_t serialRxBytesWaiting(const serialPort_t *instance) {
+uint32_t serialRxBytesWaiting(const serialPort_t *) {return 0;}
-    UNUSED(instance);
+uint32_t serialTxBytesFree(const serialPort_t *) {return 0;}
-    return 0;
+uint8_t serialRead(serialPort_t *) {return 0;}
-}
+void serialWrite(serialPort_t *, uint8_t) {}
+void serialWriteBuf(serialPort_t *, const uint8_t *, int) {}
+void serialSetMode(serialPort_t *, portMode_t ) {}
+serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, uint32_t, portMode_t, portOptions_t) {return NULL;}
+void closeSerialPort(serialPort_t *) {}
 
-uint32_t serialTxBytesFree(const serialPort_t *instance) {
+serialPortConfig_t *findSerialPortConfig(serialPortFunction_e) {return NULL;}
-    UNUSED(instance);
-    return 0;
-}
 
-uint8_t serialRead(serialPort_t *instance) {
+bool telemetryDetermineEnabledState(portSharing_e) {return true;}
-    UNUSED(instance);
-    return 0;
-}
 
-void serialWrite(serialPort_t *instance, uint8_t ch) {
+portSharing_e determinePortSharing(serialPortConfig_t *, serialPortFunction_e) {return PORTSHARING_NOT_SHARED;}
-    UNUSED(instance);
-    UNUSED(ch);
-}
-void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count) {
-    UNUSED(instance);
-    UNUSED(data);
-    UNUSED(count);
-}
-
-void serialSetMode(serialPort_t *instance, portMode_t mode) {
-    UNUSED(instance);
-    UNUSED(mode);
-}
-
-
-serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e functionMask, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, portOptions_t options) {
-    UNUSED(identifier);
-    UNUSED(functionMask);
-    UNUSED(baudRate);
-    UNUSED(callback);
-    UNUSED(mode);
-    UNUSED(options);
-
-    return NULL;
-}
-
-void closeSerialPort(serialPort_t *serialPort) {
-    UNUSED(serialPort);
-}
-
-serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function) {
-    UNUSED(function);
-
-    return NULL;
-}
-
-bool telemetryDetermineEnabledState(portSharing_e) {
-    return true;
-}
-
-portSharing_e determinePortSharing(serialPortConfig_t *, serialPortFunction_e) {
-    return PORTSHARING_NOT_SHARED;
-}
 
 uint8_t batteryCapacityRemainingPercentage(void) {return 67;}
-
 uint8_t calculateBatteryCapacityRemainingPercentage(void) {return 67;}
-
+batteryState_e getBatteryState(void) {return BATTERY_OK;}
-batteryState_e getBatteryState(void) {
+bool isAirmodeActive(void) {return airMode;}
-    return BATTERY_OK;
-}
 
 }