diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index b3729f2f2c..0481a6f94e 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -267,19 +267,39 @@ void crsfFrameFlightMode(sbuf_t *dst) sbufWriteU8(dst, 0); sbufWriteU8(dst, CRSF_FRAMETYPE_FLIGHT_MODE); - // use same logic as OSD, so telemetry displays same flight text as OSD + // Acro is the default mode const char *flightMode = "ACRO"; - if (airmodeIsEnabled()) { - flightMode = "AIR"; - } + + // Modes that are only relevant when disarmed + if (!ARMING_FLAG(ARMED) && isArmingDisabled()) { + flightMode = "!ERR"; + } else + +#if defined(USE_GPS) + if (!ARMING_FLAG(ARMED) && featureIsEnabled(FEATURE_GPS) && (!STATE(GPS_FIX) || !STATE(GPS_FIX_HOME))) { + flightMode = "WAIT"; // Waiting for GPS lock + } else +#endif + + // Flight modes in decreasing order of importance if (FLIGHT_MODE(FAILSAFE_MODE)) { - flightMode = "!FS"; + flightMode = "!FS!"; + } else if (FLIGHT_MODE(GPS_RESCUE_MODE)) { + flightMode = "RTH"; + } else if (FLIGHT_MODE(PASSTHRU_MODE)) { + flightMode = "MANU"; } else if (FLIGHT_MODE(ANGLE_MODE)) { flightMode = "STAB"; } else if (FLIGHT_MODE(HORIZON_MODE)) { flightMode = "HOR"; + } else if (airmodeIsEnabled()) { + flightMode = "AIR"; } + sbufWriteString(dst, flightMode); + if (!ARMING_FLAG(ARMED)) { + sbufWriteU8(dst, '*'); + } sbufWriteU8(dst, '\0'); // zero-terminate string // write in the frame length *lengthPtr = sbufPtr(dst) - lengthPtr; diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index cbb6057113..38543c96d3 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -216,9 +216,30 @@ TEST(TelemetryCrsfTest, TestFlightMode) { uint8_t frame[CRSF_FRAME_SIZE_MAX]; - // nothing set, so ACRO mode + ENABLE_STATE(GPS_FIX); + ENABLE_STATE(GPS_FIX_HOME); + airMode = false; + + DISABLE_ARMING_FLAG(ARMED); + + // nothing set, so ACRO mode int frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_FLIGHT_MODE); + EXPECT_EQ(6 + FRAME_HEADER_FOOTER_LEN, frameLen); + EXPECT_EQ(CRSF_SYNC_BYTE, frame[0]); // address + EXPECT_EQ(8, frame[1]); // length + EXPECT_EQ(0x21, frame[2]); // type + EXPECT_EQ('A', frame[3]); + EXPECT_EQ('C', frame[4]); + EXPECT_EQ('R', frame[5]); + EXPECT_EQ('O', frame[6]); + EXPECT_EQ('*', frame[7]); + EXPECT_EQ(0, frame[8]); + EXPECT_EQ(crfsCrc(frame, frameLen), frame[9]); + + ENABLE_ARMING_FLAG(ARMED); + + frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_FLIGHT_MODE); EXPECT_EQ(5 + FRAME_HEADER_FOOTER_LEN, frameLen); EXPECT_EQ(CRSF_SYNC_BYTE, frame[0]); // address EXPECT_EQ(7, frame[1]); // length @@ -230,7 +251,6 @@ TEST(TelemetryCrsfTest, TestFlightMode) EXPECT_EQ(0, frame[7]); EXPECT_EQ(crfsCrc(frame, frameLen), frame[8]); - enableFlightMode(ANGLE_MODE); EXPECT_EQ(ANGLE_MODE, FLIGHT_MODE(ANGLE_MODE)); frameLen = getCrsfFrame(frame, CRSF_FRAMETYPE_FLIGHT_MODE);