mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-12 19:10:27 +03:00
Merge 070dded1a3 into ab3411ad4e
This commit is contained in:
Roman Sorokin
GitHub
commit
d65ec0c21f
2 changed files with 61 additions and 17 deletions
|
|
@ -45,7 +45,10 @@ 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_BAROMETER_ALTITUDE_PAYLOAD_SIZE = 2, // 0x09 Altitude only
|
||||
CRSF_FRAME_BAROMETER_ALTITUDE_VARIO_PAYLOAD_SIZE = 3, // 0x09 Altitude + Vario (TBS CRSF standard)
|
||||
// CRSF_FRAME_BAROMETER_ALTITUDE_VARIO_PAYLOAD_SIZE = 4, // 0x09 Altitude + Vario (EdgeTX / ELRS historical standard)
|
||||
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,
|
||||
|
|
@ -85,9 +88,15 @@ typedef enum {
|
|||
|
||||
typedef enum {
|
||||
CRSF_FRAMETYPE_GPS = 0x02,
|
||||
CRSF_FRAMETYPE_GPS_EXTENDED = 0x06,
|
||||
CRSF_FRAMETYPE_VARIO_SENSOR = 0x07,
|
||||
CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
|
||||
CRSF_FRAMETYPE_BAROMETER_ALTITUDE = 0x09,
|
||||
CRSF_FRAMETYPE_BAROMETER_ALTITUDE_VARIO = 0x09,
|
||||
CRSF_FRAMETYPE_AIRSPEED_SENSOR = 0x0A,
|
||||
CRSF_FRAMETYPE_RPM = 0x0C,
|
||||
CRSF_FRAMETYPE_TEMP = 0x0D,
|
||||
CRSF_FRAMETYPE_VOLTAGES = 0x0E,
|
||||
CRSF_FRAMETYPE_VTX = 0x10,
|
||||
CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
|
||||
CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,
|
||||
CRSF_FRAMETYPE_ATTITUDE = 0x1E,
|
||||
|
|
|
|||
|
|
@ -18,6 +18,7 @@
|
|||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <limits.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
|
|
@ -33,6 +34,7 @@
|
|||
#include "common/time.h"
|
||||
#include "common/utils.h"
|
||||
#include "common/printf.h"
|
||||
#include "common/maths.h"
|
||||
|
||||
#include "config/feature.h"
|
||||
|
||||
|
|
@ -57,13 +59,14 @@
|
|||
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/pitotmeter.h"
|
||||
|
||||
#include "telemetry/crsf.h"
|
||||
#include "telemetry/telemetry.h"
|
||||
#include "telemetry/msp_shared.h"
|
||||
|
||||
|
||||
#define CRSF_CYCLETIME_US 100000 // 100ms, 10 Hz
|
||||
#define CRSF_CYCLETIME_US 20000 // 20ms, 50 Hz
|
||||
#define CRSF_DEVICEINFO_VERSION 0x01
|
||||
// According to TBS: "CRSF over serial should always use a sync byte at the beginning of each frame.
|
||||
// To get better performance it's recommended to use the sync byte 0xC8 to get better performance"
|
||||
|
|
@ -214,8 +217,7 @@ static void crsfFrameGps(sbuf_t *dst)
|
|||
crsfSerialize32(dst, gpsSol.llh.lon);
|
||||
crsfSerialize16(dst, (gpsSol.groundSpeed * 36 + 50) / 100); // gpsSol.groundSpeed is in cm/s
|
||||
crsfSerialize16(dst, DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse)); // gpsSol.groundCourse is 0.1 degrees, need 0.01 deg
|
||||
const uint16_t altitude = (getEstimatedActualPosition(Z) / 100) + 1000;
|
||||
crsfSerialize16(dst, altitude);
|
||||
crsfSerialize16(dst, gpsSol.llh.alt / 100 + 1000);
|
||||
crsfSerialize8(dst, gpsSol.numSat);
|
||||
}
|
||||
|
||||
|
|
@ -261,13 +263,16 @@ static void crsfFrameBatterySensor(sbuf_t *dst)
|
|||
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;
|
||||
const float VARIO_KL = 100.0f; // TBS CRSF standard
|
||||
const float VARIO_KR = .026f; // TBS CRSF standard
|
||||
|
||||
/*
|
||||
0x09 Barometer altitude and vertical speed
|
||||
Payload:
|
||||
uint16_t altitude_packed ( dm - 10000 )
|
||||
int16_t vario_packed
|
||||
*/
|
||||
static void crsfBarometerAltitude(sbuf_t *dst)
|
||||
static void crsfBarometerAltitudeVario(sbuf_t *dst)
|
||||
{
|
||||
int32_t altitude_dm = lrintf(getEstimatedActualPosition(Z) / 10);
|
||||
uint16_t altitude_packed;
|
||||
|
|
@ -280,9 +285,35 @@ static void crsfBarometerAltitude(sbuf_t *dst)
|
|||
} 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);
|
||||
|
||||
|
||||
// TBS CRSF standard
|
||||
float vario_sm = getEstimatedActualVelocity(Z);
|
||||
int8_t sign = vario_sm < 0 ? -1 : ( vario_sm > 0 ? 1 : 0 );
|
||||
int8_t vario_packed = (int8_t)constrain( lrintf(__builtin_logf(ABS(vario_sm) / VARIO_KL + 1) / VARIO_KR * sign ), SCHAR_MIN, SCHAR_MAX );
|
||||
|
||||
// EdgeTX / ELRS historical standart
|
||||
// int16_t vario_sm = lrintf(getEstimatedActualVelocity(Z));
|
||||
|
||||
sbufWriteU8(dst, CRSF_FRAME_BAROMETER_ALTITUDE_VARIO_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
|
||||
crsfSerialize8(dst, CRSF_FRAMETYPE_BAROMETER_ALTITUDE_VARIO);
|
||||
crsfSerialize16(dst, altitude_packed);
|
||||
crsfSerialize8(dst, vario_packed); // TBS CRSF standard
|
||||
//crsfSerialize16(dst, vario_sm); // EdgeTX / ELRS historical standard
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
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));
|
||||
}
|
||||
|
||||
typedef enum {
|
||||
|
|
@ -442,7 +473,8 @@ typedef enum {
|
|||
CRSF_FRAME_FLIGHT_MODE_INDEX,
|
||||
CRSF_FRAME_GPS_INDEX,
|
||||
CRSF_FRAME_VARIO_SENSOR_INDEX,
|
||||
CRSF_FRAME_BAROMETER_ALTITUDE_INDEX,
|
||||
CRSF_FRAME_BAROMETER_ALTITUDE_VARIO_INDEX,
|
||||
CRSF_FRAME_AIRSPEED_INDEX,
|
||||
CRSF_SCHEDULE_COUNT_MAX
|
||||
} crsfFrameTypeIndex_e;
|
||||
|
||||
|
|
@ -504,18 +536,21 @@ static void processCrsf(void)
|
|||
}
|
||||
#endif
|
||||
#if defined(USE_BARO) || defined(USE_GPS)
|
||||
if (currentSchedule & BV(CRSF_FRAME_VARIO_SENSOR_INDEX)) {
|
||||
if (currentSchedule & BV(CRSF_FRAME_BAROMETER_ALTITUDE_VARIO_INDEX)) {
|
||||
crsfInitializeFrame(dst);
|
||||
crsfFrameVarioSensor(dst);
|
||||
crsfBarometerAltitudeVario(dst);
|
||||
crsfFinalize(dst);
|
||||
}
|
||||
if (currentSchedule & BV(CRSF_FRAME_BAROMETER_ALTITUDE_INDEX)) {
|
||||
#endif
|
||||
#if defined(USE_PITOT)
|
||||
if (currentSchedule & BV(CRSF_FRAME_AIRSPEED_INDEX)) {
|
||||
crsfInitializeFrame(dst);
|
||||
crsfBarometerAltitude(dst);
|
||||
crsfFrameAirSpeedSensor(dst);
|
||||
crsfFinalize(dst);
|
||||
}
|
||||
#endif
|
||||
crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount;
|
||||
|
||||
}
|
||||
|
||||
void crsfScheduleDeviceInfoResponse(void)
|
||||
|
|
@ -545,12 +580,12 @@ void initCrsfTelemetry(void)
|
|||
#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);
|
||||
crsfSchedule[index++] = BV(CRSF_FRAME_BAROMETER_ALTITUDE_VARIO_INDEX);
|
||||
}
|
||||
#endif
|
||||
#ifdef USE_BARO
|
||||
if (sensors(SENSOR_BARO)) {
|
||||
crsfSchedule[index++] = BV(CRSF_FRAME_BAROMETER_ALTITUDE_INDEX);
|
||||
#ifdef USE_PITOT
|
||||
if (sensors(SENSOR_PITOT)) {
|
||||
crsfSchedule[index++] = BV(CRSF_FRAME_AIRSPEED_INDEX);
|
||||
}
|
||||
#endif
|
||||
crsfScheduleCount = (uint8_t)index;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue