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

Merge pull request #10694 from her01n/crsf-barometer-height

Browse source 
add barometer altitude to crsf telemetry
This commit is contained in:
Sensei 2025-05-26 13:31:25 -05:00 • committed by GitHub
commit ef01a98de1
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
2 changed files with 40 additions and 0 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_BAROMETER_ALTITUDE_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,
@ -86,6 +87,7 @@ typedef enum {
CRSF_FRAMETYPE_GPS = 0x02,
CRSF_FRAMETYPE_VARIO_SENSOR = 0x07,
CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
CRSF_FRAMETYPE_BAROMETER_ALTITUDE = 0x09,
CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,
CRSF_FRAMETYPE_ATTITUDE = 0x1E,

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

@ -258,6 +258,33 @@ static void crsfFrameBatterySensor(sbuf_t *dst)
crsfSerialize8(dst, batteryRemainingPercentage);
}
const int32_t ALT_MIN_DM = 10000;
const int32_t ALT_THRESHOLD_DM = 0x8000 - ALT_MIN_DM;
const int32_t ALT_MAX_DM = 0x7ffe * 10 - 5;
/*
0x09 Barometer altitude and vertical speed
Payload:
uint16_t altitude_packed ( dm - 10000 )
*/
static void crsfBarometerAltitude(sbuf_t *dst)
{
int32_t altitude_dm = lrintf(getEstimatedActualPosition(Z) / 10);
uint16_t altitude_packed;
if (altitude_dm < -ALT_MIN_DM) {
altitude_packed = 0;
} else if (altitude_dm > ALT_MAX_DM) {
altitude_packed = 0xfffe;
} else if (altitude_dm < ALT_THRESHOLD_DM) {
altitude_packed = altitude_dm + ALT_MIN_DM;
} else {
altitude_packed = ((altitude_dm + 5) / 10) | 0x8000;
}
sbufWriteU8(dst, CRSF_FRAME_BAROMETER_ALTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
crsfSerialize8(dst, CRSF_FRAMETYPE_BAROMETER_ALTITUDE);
crsfSerialize16(dst, altitude_packed);
}
typedef enum {
CRSF_ACTIVE_ANTENNA1 = 0,
CRSF_ACTIVE_ANTENNA2 = 1
@ -415,6 +442,7 @@ typedef enum {
CRSF_FRAME_FLIGHT_MODE_INDEX,
CRSF_FRAME_GPS_INDEX,
CRSF_FRAME_VARIO_SENSOR_INDEX,
CRSF_FRAME_BAROMETER_ALTITUDE_INDEX,
CRSF_SCHEDULE_COUNT_MAX
} crsfFrameTypeIndex_e;
@ -481,6 +509,11 @@ static void processCrsf(void)
crsfFrameVarioSensor(dst);
crsfFinalize(dst);
}
if (currentSchedule & BV(CRSF_FRAME_BAROMETER_ALTITUDE_INDEX)) {
crsfInitializeFrame(dst);
crsfBarometerAltitude(dst);
crsfFinalize(dst);
}
#endif
crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount;
}
@ -514,6 +547,11 @@ void initCrsfTelemetry(void)
if (sensors(SENSOR_BARO) || (STATE(FIXED_WING_LEGACY) && feature(FEATURE_GPS))) {
crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_SENSOR_INDEX);
}
#endif
#ifdef USE_BARO
if (sensors(SENSOR_BARO)) {
crsfSchedule[index++] = BV(CRSF_FRAME_BAROMETER_ALTITUDE_INDEX);
}
#endif
crsfScheduleCount = (uint8_t)index;
}