Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

Merge pull request #9438 from dronecontrol-ru/baro_alt_vario

Browse source 
Add support crsf baro altitude combined vario sensor
This commit is contained in:
Paweł Spychalski 2023-12-26 11:54:51 +01:00 • committed by GitHub
commit 5d77d07cec
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 44 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/rx/crsf.h
View file

@ -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,

49
src/main/telemetry/crsf.c
View file

@ -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;
}