mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 16:25:31 +03:00
321 lines
9.4 KiB
C
321 lines
9.4 KiB
C
/*
|
|
* This file is part of Cleanflight and Betaflight.
|
|
*
|
|
* Cleanflight and Betaflight are free software. You can redistribute
|
|
* this software and/or modify this software under the terms of the
|
|
* GNU General Public License as published by the Free Software
|
|
* Foundation, either version 3 of the License, or (at your option)
|
|
* any later version.
|
|
*
|
|
* Cleanflight and Betaflight are distributed in the hope that they
|
|
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
|
|
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
|
* See the GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this software.
|
|
*
|
|
* If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
|
|
#include "platform.h"
|
|
|
|
#ifdef USE_TELEMETRY_GHST
|
|
|
|
#include "build/atomic.h"
|
|
#include "build/build_config.h"
|
|
#include "build/version.h"
|
|
|
|
#include "config/feature.h"
|
|
#include "pg/pg.h"
|
|
#include "pg/pg_ids.h"
|
|
|
|
#include "common/crc.h"
|
|
#include "common/maths.h"
|
|
#include "common/printf.h"
|
|
#include "common/streambuf.h"
|
|
#include "common/utils.h"
|
|
|
|
#include "cms/cms.h"
|
|
|
|
#include "drivers/nvic.h"
|
|
|
|
#include "config/config.h"
|
|
#include "fc/rc_modes.h"
|
|
#include "fc/runtime_config.h"
|
|
|
|
#include "flight/imu.h"
|
|
#include "flight/position.h"
|
|
|
|
#include "io/gps.h"
|
|
#include "io/serial.h"
|
|
|
|
#include "rx/ghst.h"
|
|
#include "rx/ghst_protocol.h"
|
|
|
|
#include "sensors/battery.h"
|
|
#include "sensors/sensors.h"
|
|
|
|
#include "telemetry/telemetry.h"
|
|
#include "telemetry/msp_shared.h"
|
|
|
|
#include "telemetry/ghst.h"
|
|
|
|
#define GHST_CYCLETIME_US 100000 // 10x/sec
|
|
#define GHST_FRAME_PACK_PAYLOAD_SIZE 10
|
|
#define GHST_FRAME_GPS_PAYLOAD_SIZE 10
|
|
#define GHST_FRAME_MAGBARO_PAYLOAD_SIZE 10
|
|
#define GHST_FRAME_LENGTH_CRC 1
|
|
#define GHST_FRAME_LENGTH_TYPE 1
|
|
|
|
static bool ghstTelemetryEnabled;
|
|
static uint8_t ghstFrame[GHST_FRAME_SIZE];
|
|
|
|
static void ghstInitializeFrame(sbuf_t *dst)
|
|
{
|
|
dst->ptr = ghstFrame;
|
|
dst->end = ARRAYEND(ghstFrame);
|
|
|
|
sbufWriteU8(dst, GHST_ADDR_RX);
|
|
}
|
|
|
|
static void ghstFinalize(sbuf_t *dst)
|
|
{
|
|
crc8_dvb_s2_sbuf_append(dst, &ghstFrame[2]); // start at byte 2, since CRC does not include device address and frame length
|
|
sbufSwitchToReader(dst, ghstFrame);
|
|
// write the telemetry frame to the receiver.
|
|
ghstRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
|
|
}
|
|
|
|
// Battery (Pack) status
|
|
void ghstFramePackTelemetry(sbuf_t *dst)
|
|
{
|
|
// use sbufWrite since CRC does not include frame length
|
|
sbufWriteU8(dst, GHST_FRAME_PACK_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE);
|
|
sbufWriteU8(dst, GHST_DL_PACK_STAT);
|
|
|
|
if (telemetryConfig()->report_cell_voltage) {
|
|
sbufWriteU16(dst, getBatteryAverageCellVoltage()); // units of 10mV
|
|
} else {
|
|
sbufWriteU16(dst, getBatteryVoltage());
|
|
}
|
|
sbufWriteU16(dst, getAmperage()); // units of 10mA
|
|
|
|
sbufWriteU16(dst, getMAhDrawn() / 10); // units of 10mAh (range of 0-655.36Ah)
|
|
|
|
sbufWriteU8(dst, 0x00); // Rx Voltage, units of 100mV (not passed from BF, added in Ghost Rx)
|
|
|
|
sbufWriteU8(dst, 0x00); // tbd1
|
|
sbufWriteU8(dst, 0x00); // tbd2
|
|
sbufWriteU8(dst, 0x00); // tbd3
|
|
}
|
|
|
|
// GPS data, primary, positional data
|
|
void ghstFrameGpsPrimaryTelemetry(sbuf_t *dst)
|
|
{
|
|
// use sbufWrite since CRC does not include frame length
|
|
sbufWriteU8(dst, GHST_FRAME_GPS_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE);
|
|
sbufWriteU8(dst, GHST_DL_GPS_PRIMARY);
|
|
|
|
sbufWriteU32(dst, gpsSol.llh.lat);
|
|
sbufWriteU32(dst, gpsSol.llh.lon);
|
|
|
|
int32_t altitudeCm = gpsSol.llh.altCm; // gps Altitude (absolute)
|
|
if (!STATE(GPS_FIX)) {
|
|
altitudeCm = 0;
|
|
}
|
|
|
|
const int16_t altitude = altitudeCm / 100;
|
|
sbufWriteU16(dst, altitude);
|
|
}
|
|
|
|
// GPS data, secondary, auxiliary data
|
|
void ghstFrameGpsSecondaryTelemetry(sbuf_t *dst)
|
|
{
|
|
// use sbufWrite since CRC does not include frame length
|
|
sbufWriteU8(dst, GHST_FRAME_GPS_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE);
|
|
sbufWriteU8(dst, GHST_DL_GPS_SECONDARY);
|
|
|
|
sbufWriteU16(dst, gpsSol.groundSpeed); // speed in 0.1m/s
|
|
sbufWriteU16(dst, gpsSol.groundCourse); // degrees * 10
|
|
sbufWriteU8(dst, gpsSol.numSat);
|
|
|
|
sbufWriteU16(dst, GPS_distanceToHome / 10); // use units of 10m to increase range of U16 to 655.36km
|
|
sbufWriteU16(dst, GPS_directionToHome / 10);
|
|
|
|
uint8_t gpsFlags = 0;
|
|
if (STATE(GPS_FIX)) {
|
|
gpsFlags |= GPS_FLAGS_FIX;
|
|
}
|
|
if (STATE(GPS_FIX_HOME)) {
|
|
gpsFlags |= GPS_FLAGS_FIX_HOME;
|
|
}
|
|
sbufWriteU8(dst, gpsFlags);
|
|
}
|
|
|
|
// Mag, Baro (and Vario) data
|
|
void ghstFrameMagBaro(sbuf_t *dst)
|
|
{
|
|
int16_t vario = 0;
|
|
int16_t altitude = 0;
|
|
int16_t yaw = 0;
|
|
uint8_t flags = 0;
|
|
|
|
#ifdef USE_VARIO
|
|
if (sensors(SENSOR_VARIO) && telemetryIsSensorEnabled(SENSOR_VARIO)) {
|
|
vario = getEstimatedVario(); // vario, cm/s
|
|
flags |= MISC_FLAGS_VARIO;
|
|
}
|
|
#endif
|
|
|
|
#ifdef USE_BARO
|
|
if (sensors(SENSOR_BARO) && telemetryIsSensorEnabled(SENSOR_ALTITUDE)) {
|
|
flags |= MISC_FLAGS_BAROALT;
|
|
altitude = (constrain(getEstimatedAltitudeCm(), -32000 * 100, 32000 * 100) / 100);
|
|
}
|
|
#endif
|
|
|
|
#ifdef USE_MAG
|
|
if (sensors(SENSOR_MAG) && telemetryIsSensorEnabled(SENSOR_HEADING)) {
|
|
flags |= MISC_FLAGS_MAGHEAD;
|
|
yaw = attitude.values.yaw;
|
|
}
|
|
#endif
|
|
|
|
// use sbufWrite since CRC does not include frame length
|
|
sbufWriteU8(dst, GHST_FRAME_MAGBARO_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE);
|
|
sbufWriteU8(dst, GHST_DL_MAGBARO);
|
|
|
|
sbufWriteU16(dst, yaw); // magHeading, deci-degrees
|
|
sbufWriteU16(dst, altitude); // baroAltitude, m
|
|
sbufWriteU8(dst, vario); // cm/s
|
|
|
|
sbufWriteU16(dst, 0);
|
|
sbufWriteU16(dst, 0);
|
|
|
|
sbufWriteU8(dst, flags);
|
|
}
|
|
|
|
// schedule array to decide how often each type of frame is sent
|
|
typedef enum {
|
|
GHST_FRAME_START_INDEX = 0,
|
|
GHST_FRAME_PACK_INDEX = GHST_FRAME_START_INDEX, // Battery (Pack) data
|
|
GHST_FRAME_GPS_PRIMARY_INDEX, // GPS, primary values (Lat, Long, Alt)
|
|
GHST_FRAME_GPS_SECONDARY_INDEX, // GPS, secondary values (Sat Count, HDOP, etc.)
|
|
GHST_FRAME_MAGBARO_INDEX, // Magnetometer/Baro values
|
|
GHST_SCHEDULE_COUNT_MAX
|
|
} ghstFrameTypeIndex_e;
|
|
|
|
static uint8_t ghstScheduleCount;
|
|
static uint8_t ghstSchedule[GHST_SCHEDULE_COUNT_MAX];
|
|
|
|
static void processGhst(void)
|
|
{
|
|
static uint8_t ghstScheduleIndex = 0;
|
|
|
|
const uint8_t currentSchedule = ghstSchedule[ghstScheduleIndex];
|
|
|
|
sbuf_t ghstPayloadBuf;
|
|
sbuf_t *dst = &ghstPayloadBuf;
|
|
|
|
if (currentSchedule & BIT(GHST_FRAME_PACK_INDEX)) {
|
|
ghstInitializeFrame(dst);
|
|
ghstFramePackTelemetry(dst);
|
|
ghstFinalize(dst);
|
|
}
|
|
|
|
#if defined(USE_GPS)
|
|
if (currentSchedule & BIT(GHST_FRAME_GPS_PRIMARY_INDEX)) {
|
|
ghstInitializeFrame(dst);
|
|
ghstFrameGpsPrimaryTelemetry(dst);
|
|
ghstFinalize(dst);
|
|
}
|
|
|
|
if (currentSchedule & BIT(GHST_FRAME_GPS_SECONDARY_INDEX)) {
|
|
ghstInitializeFrame(dst);
|
|
ghstFrameGpsSecondaryTelemetry(dst);
|
|
ghstFinalize(dst);
|
|
}
|
|
#endif
|
|
|
|
if (currentSchedule & BIT(GHST_FRAME_MAGBARO_INDEX)) {
|
|
ghstInitializeFrame(dst);
|
|
ghstFrameMagBaro(dst);
|
|
ghstFinalize(dst);
|
|
}
|
|
|
|
ghstScheduleIndex = (ghstScheduleIndex + 1) % ghstScheduleCount;
|
|
}
|
|
|
|
void initGhstTelemetry(void)
|
|
{
|
|
// If the GHST Rx driver is active, since tx and rx share the same pin, assume telemetry
|
|
// can be initialized but not enabled yet.
|
|
if (!ghstRxIsActive()) {
|
|
return;
|
|
}
|
|
|
|
ghstTelemetryEnabled = false;
|
|
|
|
int index = 0;
|
|
if ((isBatteryVoltageConfigured() && telemetryIsSensorEnabled(SENSOR_VOLTAGE))
|
|
|| (isAmperageConfigured() && telemetryIsSensorEnabled(SENSOR_CURRENT | SENSOR_FUEL))) {
|
|
ghstSchedule[index++] = BIT(GHST_FRAME_PACK_INDEX);
|
|
}
|
|
|
|
#ifdef USE_GPS
|
|
if (featureIsEnabled(FEATURE_GPS)
|
|
&& telemetryIsSensorEnabled(SENSOR_ALTITUDE | SENSOR_LAT_LONG)) {
|
|
ghstSchedule[index++] = BIT(GHST_FRAME_GPS_PRIMARY_INDEX);
|
|
}
|
|
|
|
if (featureIsEnabled(FEATURE_GPS)
|
|
&& telemetryIsSensorEnabled(SENSOR_GROUND_SPEED | SENSOR_HEADING)) {
|
|
ghstSchedule[index++] = BIT(GHST_FRAME_GPS_SECONDARY_INDEX);
|
|
}
|
|
#endif
|
|
|
|
#if defined(USE_BARO) || defined(USE_MAG) || defined(USE_VARIO)
|
|
if ((sensors(SENSOR_BARO) && telemetryIsSensorEnabled(SENSOR_ALTITUDE))
|
|
|| (sensors(SENSOR_MAG) && telemetryIsSensorEnabled(SENSOR_HEADING))
|
|
|| (sensors(SENSOR_VARIO) && telemetryIsSensorEnabled(SENSOR_VARIO))) {
|
|
ghstSchedule[index++] = BIT(GHST_FRAME_MAGBARO_INDEX);
|
|
}
|
|
#endif
|
|
|
|
ghstScheduleCount = index;
|
|
}
|
|
|
|
void setGhstTelemetryState(bool state)
|
|
{
|
|
ghstTelemetryEnabled = state;
|
|
}
|
|
|
|
bool checkGhstTelemetryState(void)
|
|
{
|
|
return ghstTelemetryEnabled;
|
|
}
|
|
|
|
// Called periodically by the scheduler
|
|
void handleGhstTelemetry(timeUs_t currentTimeUs)
|
|
{
|
|
static timeUs_t ghstLastCycleTime;
|
|
|
|
if (!ghstTelemetryEnabled) {
|
|
return;
|
|
}
|
|
|
|
// Ready to send telemetry?
|
|
if (currentTimeUs >= ghstLastCycleTime + (GHST_CYCLETIME_US / ghstScheduleCount)) {
|
|
ghstLastCycleTime = currentTimeUs;
|
|
processGhst();
|
|
}
|
|
|
|
// telemetry is sent from the Rx driver, ghstProcessFrame
|
|
}
|
|
|
|
#endif
|