From bdeb004bd0861e2336352b84e467c68ebd489dca Mon Sep 17 00:00:00 2001 From: jflyper Date: Fri, 30 Jun 2017 14:08:40 +0900 Subject: [PATCH] Add gyro flag to MSP_STATUS_EX In detected sensor field (16-bits), lower 5-bits (bits 0-4) represents traditional sensors (ACC, BARO, MAG, GPS and SONAR). Bit 5 is an extension bit, and if on, bit 6 indicates gyro detection status. --- src/main/fc/fc_msp.c | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 53774dc225..7fceff3f36 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -912,7 +912,14 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn #else sbufWriteU16(dst, 0); #endif - sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); + + // Lower 5-bits (bits 0-4) are traditional sensor flags. + // If bit 5 is on, then bit 6 indicates gyro detection status. + uint16_t sensorBits = sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4; + sensorBits |= (1 << 5); // Extention indicator bit + sensorBits |= sensors(SENSOR_GYRO) << 6; + sbufWriteU16(dst, sensorBits); + sbufWriteData(dst, &flightModeFlags, 4); // unconditional part of flags, first 32 bits sbufWriteU8(dst, getCurrentPidProfileIndex()); sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));