Skip to content

Commit

Permalink
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge branch 'master' into gov_exponent
Browse files Browse the repository at this point in the history
rotorflight authored Oct 8, 2024
2 parents 6a94805 + b2d1472 commit b2c7bca
Showing 4 changed files with 14 additions and 3 deletions.
1 change: 1 addition & 0 deletions src/main/cli/settings.c
Original file line number Diff line number Diff line change
@@ -1248,6 +1248,7 @@ const clivalue_t valueTable[] = {
{ "telemetry_enable_headspeed", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(SENSOR_HEADSPEED), PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)},
{ "telemetry_enable_tailspeed", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(SENSOR_TAILSPEED), PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)},
{ "telemetry_enable_throttle_control",VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(SENSOR_THROTTLE_CONTROL),PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)},
{ "telemetry_enable_arming_flags", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(SENSOR_ARMING_FLAGS), PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)},
#else
{ "telemetry_enable_sensors", VAR_UINT32 | MASTER_VALUE, .config.u32Max = SENSOR_ALL, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)},
#endif
3 changes: 2 additions & 1 deletion src/main/pg/telemetry.h
Original file line number Diff line number Diff line change
@@ -54,7 +54,8 @@ typedef enum {
SENSOR_BEC_VOLTAGE = BIT(26),
SENSOR_HEADSPEED = BIT(27),
SENSOR_TAILSPEED = BIT(28),
SENSOR_THROTTLE_CONTROL = BIT(29),
SENSOR_THROTTLE_CONTROL= BIT(29),
SENSOR_ARMING_FLAGS = BIT(30),
} sensor_e;

typedef enum {
2 changes: 1 addition & 1 deletion src/main/telemetry/crsf.c
Original file line number Diff line number Diff line change
@@ -605,7 +605,7 @@ void crsfSensorEncodeControl(sbuf_t *buf, telemetrySensor_t *sensor)
UNUSED(sensor);
const int p = lrintf(mixerGetInput(MIXER_IN_STABILIZED_PITCH) * 1200);
const int r = lrintf(mixerGetInput(MIXER_IN_STABILIZED_ROLL) * 1200);
const int y = lrintf(mixerGetInput(MIXER_IN_STABILIZED_YAW) * 2400);
const int y = lrintf(mixerGetInput(MIXER_IN_STABILIZED_YAW) * 800);
const int c = lrintf(mixerGetInput(MIXER_IN_STABILIZED_COLLECTIVE) * 1200);
sbufWriteU8(buf, ((p >> 8) & 0x0F) | ((r >> 4) & 0xF0));
sbufWriteU8(buf, (p & 0xFF));
11 changes: 10 additions & 1 deletion src/main/telemetry/smartport.c
Original file line number Diff line number Diff line change
@@ -118,6 +118,7 @@ enum
FSSP_DATAID_THROTTLE_CONTROL = 0x5440 , // custom
FSSP_DATAID_GOV_MODE = 0x5450 , // custom
FSSP_DATAID_MODEL_ID = 0x5460 , // custom
FSSP_DATAID_ARMING_FLAGS = 0x5462 , // custom
FSSP_DATAID_PID_PROFILE = 0x5471 , // custom
FSSP_DATAID_RATES_PROFILE = 0x5472 , // custom
#if defined(USE_ACC)
@@ -139,7 +140,7 @@ enum
};

// if adding more sensors then increase this value (should be equal to the maximum number of ADD_SENSOR calls)
#define MAX_DATAIDS 30
#define MAX_DATAIDS 31

static uint16_t frSkyDataIdTable[MAX_DATAIDS];

@@ -322,6 +323,10 @@ static void initSmartPortSensors(void)
ADD_SENSOR(FSSP_DATAID_MODEL_ID);
}

if (telemetryIsSensorEnabled(SENSOR_ARMING_FLAGS)) {
ADD_SENSOR(FSSP_DATAID_ARMING_FLAGS);
}

if (telemetryIsSensorEnabled(SENSOR_PID_PROFILE)) {
ADD_SENSOR(FSSP_DATAID_PID_PROFILE);
}
@@ -662,6 +667,10 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
smartPortSendPackage(id, telemetrySensorValue(TELEM_MODEL_ID));
*clearToSend = false;
break;
case FSSP_DATAID_ARMING_FLAGS :
smartPortSendPackage(id, telemetrySensorValue(TELEM_ARMING_FLAGS));
*clearToSend = false;
break;
case FSSP_DATAID_PID_PROFILE :
smartPortSendPackage(id, telemetrySensorValue(TELEM_PID_PROFILE));
*clearToSend = false;

0 comments on commit b2c7bca

Please sign in to comment.