1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 07:15:18 +03:00

Merge pull request #7074 from pulquero/telm_config2

Support for enabling individual telemetry sensors
This commit is contained in:
Michael Keller 2018-11-23 01:43:33 +13:00 committed by GitHub
commit 2a748e73d0
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
9 changed files with 112 additions and 39 deletions

View file

@ -971,7 +971,7 @@ const clivalue_t valueTable[] = {
// Set to $size_of_battery to get a percentage of battery used. // Set to $size_of_battery to get a percentage of battery used.
{ "mavlink_mah_as_heading_divisor", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 30000 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, mavlink_mah_as_heading_divisor) }, { "mavlink_mah_as_heading_divisor", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 30000 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, mavlink_mah_as_heading_divisor) },
#endif #endif
{ "telemetry_disabled_sensors", VAR_UINT32 | MASTER_VALUE, .config.minmax = { 0, 15 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, disabledSensors)}, { "telemetry_disabled_sensors", VAR_UINT32 | MASTER_VALUE, .config.u32_max = SENSOR_ALL, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, disabledSensors)},
#endif // USE_TELEMETRY #endif // USE_TELEMETRY
// PG_LED_STRIP_CONFIG // PG_LED_STRIP_CONFIG

View file

@ -446,15 +446,17 @@ void initCrsfTelemetry(void)
#endif #endif
int index = 0; int index = 0;
if (sensors(SENSOR_ACC)) { if (sensors(SENSOR_ACC) && telemetryIsSensorEnabled(SENSOR_PITCH | SENSOR_ROLL | SENSOR_HEADING)) {
crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE_INDEX); crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE_INDEX);
} }
if (isBatteryVoltageConfigured() || isAmperageConfigured()) { if ((isBatteryVoltageConfigured() && telemetryIsSensorEnabled(SENSOR_VOLTAGE))
|| (isAmperageConfigured() && telemetryIsSensorEnabled(SENSOR_CURRENT | SENSOR_FUEL))) {
crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR_INDEX); crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR_INDEX);
} }
crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE_INDEX); crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE_INDEX);
#ifdef USE_GPS #ifdef USE_GPS
if (featureIsEnabled(FEATURE_GPS)) { if (featureIsEnabled(FEATURE_GPS)
&& telemetryIsSensorEnabled(SENSOR_ALTITUDE | SENSOR_LAT_LONG | SENSOR_GROUND_SPEED | SENSOR_HEADING)) {
crsfSchedule[index++] = BV(CRSF_FRAME_GPS_INDEX); crsfSchedule[index++] = BV(CRSF_FRAME_GPS_INDEX);
} }
#endif #endif

View file

@ -519,7 +519,7 @@ void processFrSkyHubTelemetry(timeUs_t currentTimeUs)
cycleNum++; cycleNum++;
if (sensors(SENSOR_ACC)) { if (sensors(SENSOR_ACC) && telemetryIsSensorEnabled(SENSOR_ACC_X | SENSOR_ACC_Y | SENSOR_ACC_Z)) {
// Sent every 125ms // Sent every 125ms
sendAccel(); sendAccel();
} }
@ -530,11 +530,13 @@ void processFrSkyHubTelemetry(timeUs_t currentTimeUs)
// Send vertical speed for opentx. ID_VERT_SPEED // Send vertical speed for opentx. ID_VERT_SPEED
// Unit is cm/s // Unit is cm/s
#ifdef USE_VARIO #ifdef USE_VARIO
if (telemetryIsSensorEnabled(SENSOR_VARIO)) {
frSkyHubWriteFrame(ID_VERT_SPEED, getEstimatedVario()); frSkyHubWriteFrame(ID_VERT_SPEED, getEstimatedVario());
}
#endif #endif
// Sent every 500ms // Sent every 500ms
if ((cycleNum % 4) == 0) { if ((cycleNum % 4) == 0 && telemetryIsSensorEnabled(SENSOR_ALTITUDE)) {
int32_t altitudeCm = getEstimatedAltitudeCm(); int32_t altitudeCm = getEstimatedAltitudeCm();
/* Allow 5s to boot correctly othervise send zero to prevent OpenTX /* Allow 5s to boot correctly othervise send zero to prevent OpenTX
@ -550,7 +552,7 @@ void processFrSkyHubTelemetry(timeUs_t currentTimeUs)
#endif #endif
#if defined(USE_MAG) #if defined(USE_MAG)
if (sensors(SENSOR_MAG)) { if (sensors(SENSOR_MAG) && telemetryIsSensorEnabled(SENSOR_HEADING)) {
// Sent every 500ms // Sent every 500ms
if ((cycleNum % 4) == 0) { if ((cycleNum % 4) == 0) {
sendHeading(); sendHeading();
@ -564,21 +566,33 @@ void processFrSkyHubTelemetry(timeUs_t currentTimeUs)
sendThrottleOrBatterySizeAsRpm(); sendThrottleOrBatterySizeAsRpm();
if (isBatteryVoltageConfigured()) { if (isBatteryVoltageConfigured()) {
if (telemetryIsSensorEnabled(SENSOR_VOLTAGE)) {
sendVoltageCells(); sendVoltageCells();
sendVoltageAmp(); sendVoltageAmp();
}
if (isAmperageConfigured()) { if (isAmperageConfigured()) {
if (telemetryIsSensorEnabled(SENSOR_CURRENT)) {
sendAmperage(); sendAmperage();
}
if (telemetryIsSensorEnabled(SENSOR_FUEL)) {
sendFuelLevel(); sendFuelLevel();
} }
} }
}
#if defined(USE_GPS) #if defined(USE_GPS)
if (sensors(SENSOR_GPS)) { if (sensors(SENSOR_GPS)) {
if (telemetryIsSensorEnabled(SENSOR_GROUND_SPEED)) {
sendSpeed(); sendSpeed();
}
if (telemetryIsSensorEnabled(SENSOR_ALTITUDE)) {
sendGpsAltitude(); sendGpsAltitude();
}
sendSatalliteSignalQualityAsTemperature2(cycleNum); sendSatalliteSignalQualityAsTemperature2(cycleNum);
if (telemetryIsSensorEnabled(SENSOR_LAT_LONG)) {
sendGPSLatLong(); sendGPSLatLong();
}
} else } else
#endif #endif
#if defined(USE_MAG) #if defined(USE_MAG)

View file

@ -320,10 +320,12 @@ static void initSmartPortSensors(void)
{ {
frSkyDataIdTableInfo.index = 0; frSkyDataIdTableInfo.index = 0;
if (telemetryIsSensorEnabled(SENSOR_MODE)) {
ADD_SENSOR(FSSP_DATAID_T1); ADD_SENSOR(FSSP_DATAID_T1);
ADD_SENSOR(FSSP_DATAID_T2); ADD_SENSOR(FSSP_DATAID_T2);
}
if (isBatteryVoltageConfigured()) { if (isBatteryVoltageConfigured() && telemetryIsSensorEnabled(SENSOR_VOLTAGE)) {
#ifdef USE_ESC_SENSOR_TELEMETRY #ifdef USE_ESC_SENSOR_TELEMETRY
if (!telemetryIsSensorEnabled(ESC_SENSOR_VOLTAGE)) if (!telemetryIsSensorEnabled(ESC_SENSOR_VOLTAGE))
#endif #endif
@ -334,7 +336,7 @@ static void initSmartPortSensors(void)
ADD_SENSOR(FSSP_DATAID_A4); ADD_SENSOR(FSSP_DATAID_A4);
} }
if (isAmperageConfigured()) { if (isAmperageConfigured() && telemetryIsSensorEnabled(SENSOR_CURRENT)) {
#ifdef USE_ESC_SENSOR_TELEMETRY #ifdef USE_ESC_SENSOR_TELEMETRY
if (!telemetryIsSensorEnabled(ESC_SENSOR_CURRENT)) if (!telemetryIsSensorEnabled(ESC_SENSOR_CURRENT))
#endif #endif
@ -342,29 +344,51 @@ static void initSmartPortSensors(void)
ADD_SENSOR(FSSP_DATAID_CURRENT); ADD_SENSOR(FSSP_DATAID_CURRENT);
} }
if (telemetryIsSensorEnabled(SENSOR_FUEL)) {
ADD_SENSOR(FSSP_DATAID_FUEL); ADD_SENSOR(FSSP_DATAID_FUEL);
} }
}
if (sensors(SENSOR_ACC)) { if (sensors(SENSOR_ACC)) {
if (telemetryIsSensorEnabled(SENSOR_HEADING)) {
ADD_SENSOR(FSSP_DATAID_HEADING); ADD_SENSOR(FSSP_DATAID_HEADING);
}
if (telemetryIsSensorEnabled(SENSOR_ACC_X)) {
ADD_SENSOR(FSSP_DATAID_ACCX); ADD_SENSOR(FSSP_DATAID_ACCX);
}
if (telemetryIsSensorEnabled(SENSOR_ACC_Y)) {
ADD_SENSOR(FSSP_DATAID_ACCY); ADD_SENSOR(FSSP_DATAID_ACCY);
}
if (telemetryIsSensorEnabled(SENSOR_ACC_Z)) {
ADD_SENSOR(FSSP_DATAID_ACCZ); ADD_SENSOR(FSSP_DATAID_ACCZ);
} }
}
if (sensors(SENSOR_BARO)) { if (sensors(SENSOR_BARO)) {
if (telemetryIsSensorEnabled(SENSOR_ALTITUDE)) {
ADD_SENSOR(FSSP_DATAID_ALTITUDE); ADD_SENSOR(FSSP_DATAID_ALTITUDE);
}
if (telemetryIsSensorEnabled(SENSOR_VARIO)) {
ADD_SENSOR(FSSP_DATAID_VARIO); ADD_SENSOR(FSSP_DATAID_VARIO);
} }
}
#ifdef USE_GPS #ifdef USE_GPS
if (featureIsEnabled(FEATURE_GPS)) { if (featureIsEnabled(FEATURE_GPS)) {
if (telemetryIsSensorEnabled(SENSOR_GROUND_SPEED)) {
ADD_SENSOR(FSSP_DATAID_SPEED); ADD_SENSOR(FSSP_DATAID_SPEED);
}
if (telemetryIsSensorEnabled(SENSOR_LAT_LONG)) {
ADD_SENSOR(FSSP_DATAID_LATLONG); ADD_SENSOR(FSSP_DATAID_LATLONG);
ADD_SENSOR(FSSP_DATAID_LATLONG); // twice (one for lat, one for long) ADD_SENSOR(FSSP_DATAID_LATLONG); // twice (one for lat, one for long)
}
if (telemetryIsSensorEnabled(SENSOR_DISTANCE)) {
ADD_SENSOR(FSSP_DATAID_HOME_DIST); ADD_SENSOR(FSSP_DATAID_HOME_DIST);
}
if (telemetryIsSensorEnabled(SENSOR_ALTITUDE)) {
ADD_SENSOR(FSSP_DATAID_GPS_ALT); ADD_SENSOR(FSSP_DATAID_GPS_ALT);
} }
}
#endif #endif
frSkyDataIdTableInfo.size = frSkyDataIdTableInfo.index; frSkyDataIdTableInfo.size = frSkyDataIdTableInfo.index;

View file

@ -42,14 +42,30 @@ typedef enum {
} frskyUnit_e; } frskyUnit_e;
typedef enum { typedef enum {
ESC_SENSOR_CURRENT = 1 << 0, SENSOR_VOLTAGE = 1 << 0,
ESC_SENSOR_VOLTAGE = 1 << 1, SENSOR_CURRENT = 1 << 1,
ESC_SENSOR_RPM = 1 << 2, SENSOR_FUEL = 1 << 2,
ESC_SENSOR_TEMPERATURE = 1 << 3, SENSOR_MODE = 1 << 3,
SENSOR_ACC_X = 1 << 4,
SENSOR_ACC_Y = 1 << 5,
SENSOR_ACC_Z = 1 << 6,
SENSOR_PITCH = 1 << 7,
SENSOR_ROLL = 1 << 8,
SENSOR_HEADING = 1 << 9,
SENSOR_ALTITUDE = 1 << 10,
SENSOR_VARIO = 1 << 11,
SENSOR_LAT_LONG = 1 << 12,
SENSOR_GROUND_SPEED = 1 << 13,
SENSOR_DISTANCE = 1 << 14,
ESC_SENSOR_CURRENT = 1 << 15,
ESC_SENSOR_VOLTAGE = 1 << 16,
ESC_SENSOR_RPM = 1 << 17,
ESC_SENSOR_TEMPERATURE = 1 << 18,
ESC_SENSOR_ALL = ESC_SENSOR_CURRENT \ ESC_SENSOR_ALL = ESC_SENSOR_CURRENT \
| ESC_SENSOR_VOLTAGE \ | ESC_SENSOR_VOLTAGE \
| ESC_SENSOR_RPM \ | ESC_SENSOR_RPM \
| ESC_SENSOR_TEMPERATURE, | ESC_SENSOR_TEMPERATURE,
SENSOR_ALL = (1 << 19) - 1,
} sensor_e; } sensor_e;
typedef struct telemetryConfig_s { typedef struct telemetryConfig_s {

View file

@ -304,4 +304,9 @@ extern "C" {
int32_t getMAhDrawn(void) { int32_t getMAhDrawn(void) {
return testmAhDrawn; return testmAhDrawn;
} }
bool telemetryIsSensorEnabled(sensor_e) {
return true;
}
} }

View file

@ -309,6 +309,7 @@ serialPortConfig_t *findSerialPortConfig(serialPortFunction_e) {return NULL;}
bool telemetryDetermineEnabledState(portSharing_e) {return true;} bool telemetryDetermineEnabledState(portSharing_e) {return true;}
bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return true;} bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return true;}
bool telemetryIsSensorEnabled(sensor_e) {return true;}
portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunction_e) {return PORTSHARING_NOT_SHARED;} portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunction_e) {return PORTSHARING_NOT_SHARED;}

View file

@ -255,6 +255,11 @@ bool telemetryDetermineEnabledState(portSharing_e)
return true; return true;
} }
bool telemetryIsSensorEnabled(sensor_e sensor) {
UNUSED(sensor);
return true;
}
portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunction_e) portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunction_e)
{ {
return PORTSHARING_NOT_SHARED; return PORTSHARING_NOT_SHARED;

View file

@ -184,6 +184,12 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing)
} }
bool telemetryIsSensorEnabled(sensor_e sensor) {
UNUSED(sensor);
return true;
}
bool isSerialPortShared(const serialPortConfig_t *portConfig, bool isSerialPortShared(const serialPortConfig_t *portConfig,
uint16_t functionMask, uint16_t functionMask,
serialPortFunction_e sharedWithFunction) serialPortFunction_e sharedWithFunction)