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

Merge pull request #4786 from shellixyz/add_vario_sensor_to_crsf_telemetry

Browse source 
Add vario sensor to CRSF telemetry
This commit is contained in:
Konstantin Sharlaimov 2019-06-03 19:57:02 +02:00 • committed by GitHub
commit 0a21ee02d6
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 43 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -43,6 +43,7 @@ enum {
enum {
CRSF_FRAME_GPS_PAYLOAD_SIZE = 15,
CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE = 2,
CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8,
CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10,
CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes.
@ -83,6 +84,7 @@ typedef enum {
typedef enum {
CRSF_FRAMETYPE_GPS = 0x02,
CRSF_FRAMETYPE_VARIO_SENSOR = 0x07,
CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,

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

@ -56,6 +56,7 @@
#include "rx/rx.h"
#include "sensors/battery.h"
#include "sensors/sensors.h"
#include "telemetry/crsf.h"
#include "telemetry/telemetry.h"
@ -204,7 +205,7 @@ uint16_t GPS heading ( degree / 100 )
uint16 Altitude ( meter ­1000m offset )
uint8_t Satellites in use ( counter )
*/
void crsfFrameGps(sbuf_t *dst)
static void crsfFrameGps(sbuf_t *dst)
{
// use sbufWrite since CRC does not include frame length
sbufWriteU8(dst, CRSF_FRAME_GPS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
@ -218,6 +219,19 @@ void crsfFrameGps(sbuf_t *dst)
crsfSerialize8(dst, gpsSol.numSat);
}
/*
0x07 Vario sensor
Payload:
int16 Vertical speed ( cm/s )
*/
static void crsfFrameVarioSensor(sbuf_t *dst)
{
// use sbufWrite since CRC does not include frame length
sbufWriteU8(dst, CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
crsfSerialize8(dst, CRSF_FRAMETYPE_VARIO_SENSOR);
crsfSerialize16(dst, getEstimatedActualVelocity(Z));
}
/*
0x08 Battery sensor
Payload:
@ -226,7 +240,7 @@ uint16_t Current ( mA * 100 )
uint24_t Capacity ( mAh )
uint8_t Battery remaining ( percent )
*/
void crsfFrameBatterySensor(sbuf_t *dst)
static void crsfFrameBatterySensor(sbuf_t *dst)
{
// use sbufWrite since CRC does not include frame length
sbufWriteU8(dst, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
@ -271,7 +285,7 @@ int16_t Yaw angle ( rad / 10000 )
#define DECIDEGREES_TO_RADIANS10000(angle) ((int16_t)(1000.0f * (angle) * RAD))
void crsfFrameAttitude(sbuf_t *dst)
static void crsfFrameAttitude(sbuf_t *dst)
{
sbufWriteU8(dst, CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
crsfSerialize8(dst, CRSF_FRAMETYPE_ATTITUDE);
@ -285,7 +299,7 @@ void crsfFrameAttitude(sbuf_t *dst)
Payload:
char[] Flight mode ( Null­terminated string )
*/
void crsfFrameFlightMode(sbuf_t *dst)
static void crsfFrameFlightMode(sbuf_t *dst)
{
// just do "OK" for the moment as a placeholder
// write zero for frame length, since we don't know it yet
@ -350,8 +364,8 @@ uint32_t Null Bytes
uint8_t 255 (Max MSP Parameter)
uint8_t 0x01 (Parameter version 1)
*/
void crsfFrameDeviceInfo(sbuf_t *dst) {
static void crsfFrameDeviceInfo(sbuf_t *dst)
{
char buff[30];
tfp_sprintf(buff, "%s %s: %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, TARGET_BOARD_IDENTIFIER);
@ -379,6 +393,7 @@ typedef enum {
CRSF_FRAME_BATTERY_SENSOR_INDEX,
CRSF_FRAME_FLIGHT_MODE_INDEX,
CRSF_FRAME_GPS_INDEX,
CRSF_FRAME_VARIO_SENSOR_INDEX,
CRSF_SCHEDULE_COUNT_MAX
} crsfFrameTypeIndex_e;
@ -438,6 +453,13 @@ 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
crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount;
}
@ -462,12 +484,18 @@ 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 (feature(FEATURE_GPS)) {
crsfSchedule[index++] = BV(CRSF_FRAME_GPS_INDEX);
}
#endif
#if defined(USE_BARO) || defined(USE_GPS)
if (sensors(SENSOR_BARO) || (STATE(FIXED_WING) && feature(FEATURE_GPS))) {
crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_SENSOR_INDEX);
}
#endif
crsfScheduleCount = (uint8_t)index;
}
}
bool checkCrsfTelemetryState(void)
{
@ -489,7 +517,7 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs)
// in between the RX frames.
crsfRxSendTelemetryData();
// Send ad-hoc response frames as soon as possible
// Send ad-hoc response frames as soon as possible
#if defined(USE_MSP_OVER_TELEMETRY)
if (mspReplyPending) {
mspReplyPending = handleCrsfMspFrameBuffer(CRSF_FRAME_TX_MSP_FRAME_SIZE, &crsfSendMspResponse);
@ -510,7 +538,7 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs)
}
// Actual telemetry data only needs to be sent at a low frequency, ie 10Hz
// Spread out scheduled frames evenly so each frame is sent at the same frequency.
// Spread out scheduled frames evenly so each frame is sent at the same frequency.
if (currentTimeUs >= crsfLastCycleTime + (CRSF_CYCLETIME_US / crsfScheduleCount)) {
crsfLastCycleTime = currentTimeUs;
processCrsf();
@ -539,6 +567,9 @@ int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
crsfFrameGps(sbuf);
break;
#endif
case CRSF_FRAMETYPE_VARIO_SENSOR:
crsfFrameVarioSensor(sbuf);
break;
}
const int frameSize = crsfFinalizeBuf(sbuf, frame);
return frameSize;