diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index c5136e3b7ae..1e72bad4d1d 100755 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -46,6 +46,7 @@ enum { CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE = 2, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8, CRSF_FRAME_BAROMETER_ALTITUDE_PAYLOAD_SIZE = 2, + CRSF_FRAME_AIRSPEED_PAYLOAD_SIZE = 2, CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10, CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes. CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6, @@ -88,6 +89,9 @@ typedef enum { CRSF_FRAMETYPE_VARIO_SENSOR = 0x07, CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08, CRSF_FRAMETYPE_BAROMETER_ALTITUDE = 0x09, + CRSF_FRAMETYPE_AIRSPEED_SENSOR = 0x0A, + CRSF_FRAMETYPE_RPM = 0x0C, + CRSF_FRAMETYPE_TEMP = 0x0D, CRSF_FRAMETYPE_LINK_STATISTICS = 0x14, CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16, CRSF_FRAMETYPE_ATTITUDE = 0x1E, diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 3a804485f20..e8475dbbc6f 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -46,6 +46,7 @@ #include "fc/runtime_config.h" #include "flight/imu.h" +#include "flight/mixer.h" #include "io/gps.h" #include "io/serial.h" @@ -56,7 +57,10 @@ #include "rx/rx.h" #include "sensors/battery.h" +#include "sensors/esc_sensor.h" +#include "sensors/pitotmeter.h" #include "sensors/sensors.h" +#include "sensors/temperature.h" #include "telemetry/crsf.h" #include "telemetry/telemetry.h" @@ -151,6 +155,16 @@ static void crsfSerialize16(sbuf_t *dst, uint16_t v) crsfSerialize8(dst, (uint8_t)v); } +#ifdef USE_ESC_SENSOR +static void crsfSerialize24(sbuf_t *dst, uint32_t v) +{ + // Use BigEndian format + crsfSerialize8(dst, (v >> 16)); + crsfSerialize8(dst, (v >> 8)); + crsfSerialize8(dst, (uint8_t)v); +} +#endif + static void crsfSerialize32(sbuf_t *dst, uint32_t v) { // Use BigEndian format @@ -285,6 +299,86 @@ static void crsfBarometerAltitude(sbuf_t *dst) crsfSerialize16(dst, altitude_packed); } +#ifdef USE_PITOT +/* +0x0A Airspeed sensor +Payload: +int16 Air speed ( dm/s ) +*/ +static void crsfFrameAirSpeedSensor(sbuf_t *dst) +{ + // use sbufWrite since CRC does not include frame length + sbufWriteU8(dst, CRSF_FRAME_AIRSPEED_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); + crsfSerialize8(dst, CRSF_FRAMETYPE_AIRSPEED_SENSOR); + crsfSerialize16(dst, (uint16_t)(getAirspeedEstimate() * 36 / 100)); +} +#endif + +#ifdef USE_ESC_SENSOR +/* +0x0C RPM +Payload: +uint8_t rpm_source_id; // Identifies the source of the RPM data (e.g., 0 = Motor 1, 1 = Motor 2, etc.) +int24_t rpm_value[]; // 1 - 19 RPM values with negative ones representing the motor spinning in reverse +*/ +static void crsfRpm(sbuf_t *dst) +{ + uint8_t motorCount = getMotorCount(); + + if (STATE(ESC_SENSOR_ENABLED) && motorCount > 0) { + sbufWriteU8(dst, 1 + (motorCount * 3) + CRSF_FRAME_LENGTH_TYPE_CRC); + crsfSerialize8(dst, CRSF_FRAMETYPE_RPM); + // 0 = FC including all ESCs + crsfSerialize8(dst, 0); + + for (uint8_t i = 0; i < motorCount; i++) { + const escSensorData_t *escState = getEscTelemetry(i); + crsfSerialize24(dst, (escState) ? escState->rpm : 0); + } + } +} +#endif + +/* +0x0D TEMP +Payload: +uint8_t temp_source_id; // Identifies the source of the temperature data (e.g., 0 = FC including all ESCs, 1 = Ambient, etc.) +int16_t temperature[]; // up to 20 temperature values in deci-degree (tenths of a degree) Celsius (e.g., 250 = 25.0°C, -50 = -5.0°C) +*/ +static void crsfTemperature(sbuf_t *dst) +{ + + uint8_t tempCount = 0; + int16_t temperatures[20]; + +#ifdef USE_ESC_SENSOR + uint8_t motorCount = getMotorCount(); + if (STATE(ESC_SENSOR_ENABLED) && motorCount > 0) { + for (uint8_t i = 0; i < motorCount; i++) { + const escSensorData_t *escState = getEscTelemetry(i); + temperatures[tempCount++] = (escState) ? escState->temperature * 10 : TEMPERATURE_INVALID_VALUE; + } + } +#endif + +#ifdef USE_TEMPERATURE_SENSOR + for (uint8_t i = 0; i < MAX_TEMP_SENSORS; i++) { + int16_t value; + if (getSensorTemperature(i, &value)) + temperatures[tempCount++] = value; + } +#endif + + if (tempCount > 0) { + sbufWriteU8(dst, 1 + (tempCount * 2) + CRSF_FRAME_LENGTH_TYPE_CRC); + crsfSerialize8(dst, CRSF_FRAMETYPE_TEMP); + // 0 = FC including all ESCs + crsfSerialize8(dst, 0); + for (uint8_t i = 0; i < tempCount; i++) + crsfSerialize16(dst, temperatures[i]); + } +} + typedef enum { CRSF_ACTIVE_ANTENNA1 = 0, CRSF_ACTIVE_ANTENNA2 = 1 @@ -443,11 +537,14 @@ typedef enum { CRSF_FRAME_GPS_INDEX, CRSF_FRAME_VARIO_SENSOR_INDEX, CRSF_FRAME_BAROMETER_ALTITUDE_INDEX, + CRSF_FRAME_TEMP_INDEX, + CRSF_FRAME_RPM_INDEX, + CRSF_FRAME_AIRSPEED_INDEX, CRSF_SCHEDULE_COUNT_MAX } crsfFrameTypeIndex_e; static uint8_t crsfScheduleCount; -static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX]; +static uint16_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX]; #if defined(USE_MSP_OVER_TELEMETRY) @@ -476,7 +573,7 @@ void crsfSendMspResponse(uint8_t *payload) static void processCrsf(void) { static uint8_t crsfScheduleIndex = 0; - const uint8_t currentSchedule = crsfSchedule[crsfScheduleIndex]; + const uint16_t currentSchedule = crsfSchedule[crsfScheduleIndex]; sbuf_t crsfPayloadBuf; sbuf_t *dst = &crsfPayloadBuf; @@ -496,6 +593,20 @@ static void processCrsf(void) crsfFrameFlightMode(dst); crsfFinalize(dst); } +#ifdef USE_ESC_SENSOR + if (currentSchedule & BV(CRSF_FRAME_RPM_INDEX)) { + crsfInitializeFrame(dst); + crsfRpm(dst); + crsfFinalize(dst); + } +#endif +#if defined(USE_ESC_SENSOR) || defined(USE_TEMPERATURE_SENSOR) + if (currentSchedule & BV(CRSF_FRAME_TEMP_INDEX)) { + crsfInitializeFrame(dst); + crsfTemperature(dst); + crsfFinalize(dst); + } +#endif #ifdef USE_GPS if (currentSchedule & BV(CRSF_FRAME_GPS_INDEX)) { crsfInitializeFrame(dst); @@ -514,6 +625,13 @@ static void processCrsf(void) crsfBarometerAltitude(dst); crsfFinalize(dst); } +#endif +#ifdef USE_PITOT + if (currentSchedule & BV(CRSF_FRAME_AIRSPEED_INDEX)) { + crsfInitializeFrame(dst); + crsfFrameAirSpeedSensor(dst); + crsfFinalize(dst); + } #endif crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount; } @@ -552,6 +670,17 @@ void initCrsfTelemetry(void) if (sensors(SENSOR_BARO)) { crsfSchedule[index++] = BV(CRSF_FRAME_BAROMETER_ALTITUDE_INDEX); } +#endif +#ifdef USE_ESC_SENSOR + crsfSchedule[index++] = BV(CRSF_FRAME_RPM_INDEX); +#endif +#if defined(USE_ESC_SENSOR) || defined(USE_TEMPERATURE_SENSOR) + crsfSchedule[index++] = BV(CRSF_FRAME_TEMP_INDEX); +#endif +#ifdef USE_PITOT + if (sensors(SENSOR_PITOT)) { + crsfSchedule[index++] = BV(CRSF_FRAME_AIRSPEED_INDEX); + } #endif crsfScheduleCount = (uint8_t)index; }