Merge pull request iNavFlight#9438 from dronecontrol-ru/baro_alt_vario

Add support crsf baro altitude combined vario sensor
JulianTiller · Dec 26, 2023 · 5d77d07 · 5d77d07
2 parents 3b218b6 + d0a403a
commit 5d77d07
Show file tree

Hide file tree

Showing 2 changed files with 44 additions and 7 deletions.
diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h
@@ -45,6 +45,7 @@ enum {
  CRSF_FRAME_GPS_PAYLOAD_SIZE = 15,
  CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE = 2,
  CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8,
+ CRSF_FRAME_BAROVARIO_SENSOR_PAYLOAD_SIZE = 4,
  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,
@@ -86,6 +87,7 @@ typedef enum {
  CRSF_FRAMETYPE_GPS = 0x02,
  CRSF_FRAMETYPE_VARIO_SENSOR = 0x07,
  CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
+ CRSF_FRAMETYPE_BAROVARIO_SENSOR = 0x09,
  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
@@ -57,7 +57,7 @@
 
 #include "sensors/battery.h"
 #include "sensors/sensors.h"
-
+#include "sensors/barometer.h"
 #include "telemetry/crsf.h"
 #include "telemetry/telemetry.h"
 #include "telemetry/msp_shared.h"
@@ -258,6 +258,22 @@ static void crsfFrameBatterySensor(sbuf_t *dst)
  crsfSerialize8(dst, batteryRemainingPercentage);
 }
 
+/*
+0x09 Baro+Vario sensor
+Payload:
+uint16 Altitude
+int16 Vertical speed ( cm/s )
+*/
+static void crsfFrameBaroVarioSensor(sbuf_t *dst)
+{
+ // use sbufWrite since CRC does not include frame length
+ sbufWriteU8(dst, CRSF_FRAME_BAROVARIO_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
+ crsfSerialize8(dst, CRSF_FRAMETYPE_BAROVARIO_SENSOR);
+ int32_t altitude = baroGetLatestAltitude() / 10; // Altitude in decimeters
+ crsfSerialize16(dst, altitude + 10000 < 0x8000 ? altitude + 10000 : 0x8000 + altitude / 10);
+ crsfSerialize16(dst, lrintf(getEstimatedActualVelocity(Z)));
+}
+
 typedef enum {
  CRSF_ACTIVE_ANTENNA1 = 0,
  CRSF_ACTIVE_ANTENNA2 = 1
@@ -411,6 +427,7 @@ typedef enum {
  CRSF_FRAME_FLIGHT_MODE_INDEX,
  CRSF_FRAME_GPS_INDEX,
  CRSF_FRAME_VARIO_SENSOR_INDEX,
+ CRSF_FRAME_BAROVARIO_SENSOR_INDEX,
  CRSF_SCHEDULE_COUNT_MAX
 } crsfFrameTypeIndex_e;
 
@@ -470,13 +487,18 @@ static void processCrsf(void)
  crsfFrameGps(dst);
  crsfFinalize(dst);
  }
-#endif
-#if defined(USE_BARO) || defined(USE_GPS)
  if (currentSchedule & BV(CRSF_FRAME_VARIO_SENSOR_INDEX)) {
  crsfInitializeFrame(dst);
  crsfFrameVarioSensor(dst);
  crsfFinalize(dst);
  }
+#endif
+#if defined(USE_BARO)
+ if (currentSchedule & BV(CRSF_FRAME_BAROVARIO_SENSOR_INDEX)) {
+ crsfInitializeFrame(dst);
+ crsfFrameBaroVarioSensor(dst);
+ crsfFinalize(dst);
+ }
 #endif
  crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount;
 }
@@ -501,14 +523,23 @@ void initCrsfTelemetry(void)
  crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE_INDEX);
  crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR_INDEX);
  crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE_INDEX);
-#ifdef USE_GPS
+#if defined(USE_GPS)
  if (feature(FEATURE_GPS)) {
  crsfSchedule[index++] = BV(CRSF_FRAME_GPS_INDEX);
+ #if !defined(USE_BARO)
+ crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_SENSOR_INDEX);
+ #endif
  }
 #endif
-#if defined(USE_BARO) || defined(USE_GPS)
- if (sensors(SENSOR_BARO) || (STATE(FIXED_WING_LEGACY) && feature(FEATURE_GPS))) {
- crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_SENSOR_INDEX);
+#if defined(USE_BARO)
+ if (sensors(SENSOR_BARO)) {
+ crsfSchedule[index++] = BV(CRSF_FRAME_BAROVARIO_SENSOR_INDEX);
+ } else {
+ #if defined(USE_GPS)
+ if (feature(FEATURE_GPS)) {
+ crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_SENSOR_INDEX);
+ }
+ #endif
  }
 #endif
  crsfScheduleCount = (uint8_t)index;
@@ -587,7 +618,11 @@ int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
  case CRSF_FRAMETYPE_VARIO_SENSOR:
  crsfFrameVarioSensor(sbuf);
  break;
+ case CRSF_FRAMETYPE_BAROVARIO_SENSOR:
+ crsfFrameBaroVarioSensor(sbuf);
+ break;
  }
+
  const int frameSize = crsfFinalizeBuf(sbuf, frame);
  return frameSize;
 }