Skip to content

Commit

Permalink
[S.PORT] Powerbox fields added
Browse files Browse the repository at this point in the history
  • Loading branch information
@bsongis
bsongis committed Oct 14, 2015
1 parent 701082f commit 1b2ee0c
Show file tree
Hide file tree
Showing 8 changed files with 105 additions and 78 deletions.
2 changes: 1 addition & 1 deletion radio/src/myeeprom.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1286,7 +1286,7 @@ PACK(typedef struct {
uint8_t logs:1;
uint8_t persistent:1;
uint8_t onlyPositive:1;
uint8_t spare:3;
uint8_t subId:3;
union {
PACK(struct {
uint16_t ratio;
Expand Down
10 changes: 5 additions & 5 deletions radio/src/telemetry/frsky.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -590,19 +590,19 @@ void telemetryReset()
switch (sensor.id)
{
case RSSI_ID:
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, RSSI_ID, sensor.instance , 75, UNIT_RAW, 0);
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, RSSI_ID, 0, sensor.instance , 75, UNIT_RAW, 0);
break;
case ADC1_ID:
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, ADC1_ID, sensor.instance, 100, UNIT_RAW, 0);
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, ADC1_ID, 0, sensor.instance, 100, UNIT_RAW, 0);
break;
case ADC2_ID:
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, ADC2_ID, sensor.instance, 245, UNIT_RAW, 0);
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, ADC2_ID, 0, sensor.instance, 245, UNIT_RAW, 0);
break;
case SWR_ID:
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, SWR_ID, sensor.instance, 30, UNIT_RAW, 0);
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, SWR_ID, 0, sensor.instance, 30, UNIT_RAW, 0);
break;
case BATT_ID:
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, BATT_ID, sensor.instance, 100, UNIT_RAW, 0);
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, BATT_ID, 0, sensor.instance, 100, UNIT_RAW, 0);
break;
}
}
Expand Down
4 changes: 2 additions & 2 deletions radio/src/telemetry/frsky.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -154,7 +154,7 @@
#define POWERBOX_STATE_FIRST_ID 0x0b20
#define POWERBOX_STATE_LAST_ID 0x0b2f
#define POWERBOX_CNSP_FIRST_ID 0x0b30
#define POWERBOX_CNS_LAST_ID 0x0b3f
#define POWERBOX_CNSP_LAST_ID 0x0b3f
#define RSSI_ID 0xf101
#define ADC1_ID 0xf102
#define ADC2_ID 0xf103
Expand Down Expand Up @@ -366,7 +366,7 @@ struct FrskyData {
#else
#define IS_VALID_XJT_VERSION() (1)
#endif
#define IS_HIDDEN_TELEMETRY_VALUE() ((appId == SP2UART_A_ID) || (appId == SP2UART_B_ID) || (appId == XJT_VERSION_ID) || ((appId == SWR_ID) && !IS_VALID_XJT_VERSION()))
#define IS_HIDDEN_TELEMETRY_VALUE(id) ((id == SP2UART_A_ID) || (id == SP2UART_B_ID) || (id == XJT_VERSION_ID) || ((id == SWR_ID) && !IS_VALID_XJT_VERSION()))

enum AlarmLevel {
alarm_off = 0,
Expand Down
8 changes: 4 additions & 4 deletions radio/src/telemetry/frsky_d_arm.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,9 +86,9 @@ void frskyDProcessPacket(uint8_t *packet)
{
case LINKPKT: // A1/A2/RSSI values
{
setTelemetryValue(TELEM_PROTO_FRSKY_D, D_A1_ID, 0, packet[1], UNIT_VOLTS, 0);
setTelemetryValue(TELEM_PROTO_FRSKY_D, D_A2_ID, 0, packet[2], UNIT_VOLTS, 0);
setTelemetryValue(TELEM_PROTO_FRSKY_D, D_RSSI_ID, 0, packet[3], UNIT_RAW, 0);
setTelemetryValue(TELEM_PROTO_FRSKY_D, D_A1_ID, 0, 0, packet[1], UNIT_VOLTS, 0);
setTelemetryValue(TELEM_PROTO_FRSKY_D, D_A2_ID, 0, 0, packet[2], UNIT_VOLTS, 0);
setTelemetryValue(TELEM_PROTO_FRSKY_D, D_RSSI_ID, 0, 0, packet[3], UNIT_RAW, 0);
frskyData.rssi.set(packet[3]);
frskyStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected
break;
Expand Down Expand Up @@ -257,7 +257,7 @@ void processHubPacket(uint8_t id, int16_t value)
}
}

setTelemetryValue(TELEM_PROTO_FRSKY_D, id, 0, data, unit, precision);
setTelemetryValue(TELEM_PROTO_FRSKY_D, id, 0, 0, data, unit, precision);
}

void frskyDSetDefault(int index, uint16_t id)
Expand Down
147 changes: 86 additions & 61 deletions radio/src/telemetry/frsky_sport.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,46 +51,53 @@
struct FrSkySportSensor {
const uint16_t firstId;
const uint16_t lastId;
const uint8_t subId;
const char * name;
const TelemetryUnit unit;
const uint8_t prec;
};

const FrSkySportSensor sportSensors[] = {
{ RSSI_ID, RSSI_ID, ZSTR_RSSI, UNIT_DB, 0 },
{ SWR_ID, SWR_ID, ZSTR_SWR, UNIT_RAW, 0 },
{ ADC1_ID, ADC1_ID, ZSTR_A1, UNIT_VOLTS, 1 },
{ ADC2_ID, ADC2_ID, ZSTR_A2, UNIT_VOLTS, 1 },
{ A3_FIRST_ID, A3_LAST_ID, ZSTR_A3, UNIT_VOLTS, 2 },
{ A4_FIRST_ID, A4_LAST_ID, ZSTR_A4, UNIT_VOLTS, 2 },
{ BATT_ID, BATT_ID, ZSTR_BATT, UNIT_VOLTS, 1 },
{ T1_FIRST_ID, T1_LAST_ID, ZSTR_TEMP1, UNIT_CELSIUS, 0 },
{ T2_FIRST_ID, T2_LAST_ID, ZSTR_TEMP2, UNIT_CELSIUS, 0 },
{ RPM_FIRST_ID, RPM_LAST_ID, ZSTR_RPM, UNIT_RPMS, 0 },
{ FUEL_FIRST_ID, FUEL_LAST_ID, ZSTR_FUEL, UNIT_PERCENT, 0 },
{ ALT_FIRST_ID, ALT_LAST_ID, ZSTR_ALT, UNIT_METERS, 2 },
{ VARIO_FIRST_ID, VARIO_LAST_ID, ZSTR_VSPD, UNIT_METERS_PER_SECOND, 2 },
{ ACCX_FIRST_ID, ACCX_LAST_ID, ZSTR_ACCX, UNIT_G, 2 },
{ ACCY_FIRST_ID, ACCY_LAST_ID, ZSTR_ACCY, UNIT_G, 2 },
{ ACCZ_FIRST_ID, ACCZ_LAST_ID, ZSTR_ACCZ, UNIT_G, 2 },
{ CURR_FIRST_ID, CURR_LAST_ID, ZSTR_CURR, UNIT_AMPS, 1 },
{ VFAS_FIRST_ID, VFAS_LAST_ID, ZSTR_VFAS, UNIT_VOLTS, 2 },
{ AIR_SPEED_FIRST_ID, AIR_SPEED_LAST_ID, ZSTR_ASPD, UNIT_KMH, 1 },
{ GPS_SPEED_FIRST_ID, GPS_SPEED_LAST_ID, ZSTR_GSPD, UNIT_KTS, 3 },
{ CELLS_FIRST_ID, CELLS_LAST_ID, ZSTR_CELLS, UNIT_CELLS, 2 },
{ GPS_ALT_FIRST_ID, GPS_ALT_LAST_ID, ZSTR_GPSALT, UNIT_METERS, 2 },
{ GPS_TIME_DATE_FIRST_ID, GPS_TIME_DATE_LAST_ID, ZSTR_GPSDATETIME, UNIT_DATETIME, 0 },
{ GPS_LONG_LATI_FIRST_ID, GPS_LONG_LATI_LAST_ID, ZSTR_GPS, UNIT_GPS, 0 },
{ FUEL_QTY_FIRST_ID, FUEL_QTY_LAST_ID, ZSTR_FUEL, UNIT_MILLILITERS, 2 },
{ GPS_COURS_FIRST_ID, GPS_COURS_LAST_ID, ZSTR_HDG, UNIT_DEGREE, 2 },
{ 0, 0, NULL, UNIT_RAW, 0 } // sentinel
{ RSSI_ID, RSSI_ID, 0, ZSTR_RSSI, UNIT_DB, 0 },
{ SWR_ID, SWR_ID, 0, ZSTR_SWR, UNIT_RAW, 0 },
{ ADC1_ID, ADC1_ID, 0, ZSTR_A1, UNIT_VOLTS, 1 },
{ ADC2_ID, ADC2_ID, 0, ZSTR_A2, UNIT_VOLTS, 1 },
{ A3_FIRST_ID, A3_LAST_ID, 0, ZSTR_A3, UNIT_VOLTS, 2 },
{ A4_FIRST_ID, A4_LAST_ID, 0, ZSTR_A4, UNIT_VOLTS, 2 },
{ BATT_ID, BATT_ID, 0, ZSTR_BATT, UNIT_VOLTS, 1 },
{ T1_FIRST_ID, T1_LAST_ID, 0, ZSTR_TEMP1, UNIT_CELSIUS, 0 },
{ T2_FIRST_ID, T2_LAST_ID, 0, ZSTR_TEMP2, UNIT_CELSIUS, 0 },
{ RPM_FIRST_ID, RPM_LAST_ID, 0, ZSTR_RPM, UNIT_RPMS, 0 },
{ FUEL_FIRST_ID, FUEL_LAST_ID, 0, ZSTR_FUEL, UNIT_PERCENT, 0 },
{ ALT_FIRST_ID, ALT_LAST_ID, 0, ZSTR_ALT, UNIT_METERS, 2 },
{ VARIO_FIRST_ID, VARIO_LAST_ID, 0, ZSTR_VSPD, UNIT_METERS_PER_SECOND, 2 },
{ ACCX_FIRST_ID, ACCX_LAST_ID, 0, ZSTR_ACCX, UNIT_G, 2 },
{ ACCY_FIRST_ID, ACCY_LAST_ID, 0, ZSTR_ACCY, UNIT_G, 2 },
{ ACCZ_FIRST_ID, ACCZ_LAST_ID, 0, ZSTR_ACCZ, UNIT_G, 2 },
{ CURR_FIRST_ID, CURR_LAST_ID, 0, ZSTR_CURR, UNIT_AMPS, 1 },
{ VFAS_FIRST_ID, VFAS_LAST_ID, 0, ZSTR_VFAS, UNIT_VOLTS, 2 },
{ AIR_SPEED_FIRST_ID, AIR_SPEED_LAST_ID, 0, ZSTR_ASPD, UNIT_KMH, 1 },
{ GPS_SPEED_FIRST_ID, GPS_SPEED_LAST_ID, 0, ZSTR_GSPD, UNIT_KTS, 3 },
{ CELLS_FIRST_ID, CELLS_LAST_ID, 0, ZSTR_CELLS, UNIT_CELLS, 2 },
{ GPS_ALT_FIRST_ID, GPS_ALT_LAST_ID, 0, ZSTR_GPSALT, UNIT_METERS, 2 },
{ GPS_TIME_DATE_FIRST_ID, GPS_TIME_DATE_LAST_ID, 0, ZSTR_GPSDATETIME, UNIT_DATETIME, 0 },
{ GPS_LONG_LATI_FIRST_ID, GPS_LONG_LATI_LAST_ID, 0, ZSTR_GPS, UNIT_GPS, 0 },
{ FUEL_QTY_FIRST_ID, FUEL_QTY_LAST_ID, 0, ZSTR_FUEL, UNIT_MILLILITERS, 2 },
{ GPS_COURS_FIRST_ID, GPS_COURS_LAST_ID, 0, ZSTR_HDG, UNIT_DEGREE, 2 },
{ POWERBOX_BATT1_FIRST_ID, POWERBOX_BATT1_LAST_ID, 0, ZSTR_PBOX1, UNIT_VOLTS, 2 },
{ POWERBOX_BATT2_FIRST_ID, POWERBOX_BATT2_LAST_ID, 0, ZSTR_PBOX2, UNIT_VOLTS, 2 },
{ POWERBOX_BATT1_FIRST_ID, POWERBOX_BATT1_LAST_ID, 1, ZSTR_PBOX1, UNIT_AMPS, 2 },
{ POWERBOX_BATT2_FIRST_ID, POWERBOX_BATT2_LAST_ID, 1, ZSTR_PBOX2, UNIT_AMPS, 2 },
{ POWERBOX_CNSP_FIRST_ID, POWERBOX_CNSP_LAST_ID, 0, ZSTR_PBOX1, UNIT_MAH, 0 },
{ POWERBOX_CNSP_FIRST_ID, POWERBOX_CNSP_LAST_ID, 1, ZSTR_PBOX2, UNIT_MAH, 0 },
{ 0, 0, 0, NULL, UNIT_RAW, 0 } // sentinel
};

const FrSkySportSensor * getFrSkySportSensor(uint16_t id)
const FrSkySportSensor * getFrSkySportSensor(uint16_t id, uint8_t subId=0)
{
const FrSkySportSensor * result = NULL;
for (const FrSkySportSensor * sensor = sportSensors; sensor->firstId; sensor++) {
if (id >= sensor->firstId && id <= sensor->lastId) {
if (id >= sensor->firstId && id <= sensor->lastId && subId == sensor->subId) {
result = sensor;
break;
}
Expand Down Expand Up @@ -170,11 +177,35 @@ void processSportUpdatePacket(uint8_t *packet)
}
}

void processSportPacket(uint8_t *packet)
void processSportPacket(uint16_t id, uint8_t subId, uint8_t instance, uint32_t data)
{
uint8_t dataId = (packet[0] & 0x1F) + 1;
const FrSkySportSensor * sensor = getFrSkySportSensor(id, subId);
TelemetryUnit unit = UNIT_RAW;
uint8_t precision = 0;
if (sensor) {
unit = sensor->unit;
precision = sensor->prec;
}
if (unit == UNIT_CELLS) {
uint8_t cellsCount = (data & 0xF0) >> 4;
uint8_t cellIndex = (data & 0x0F);
uint32_t mask = (cellsCount << 24) + (cellIndex << 16);
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, id, 0, instance, mask + (((data & 0x000FFF00) >> 8) / 5), unit, precision);
if (cellIndex+1 < cellsCount) {
mask += (1 << 16);
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, id, 0, instance, mask + (((data & 0xFFF00000) >> 20) / 5), unit, precision);
}
}
else {
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, id, 0, instance, data, unit, precision);
}
}

void processSportPacket(uint8_t * packet)
{
uint8_t instance = (packet[0] & 0x1F) + 1;
uint8_t prim = packet[1];
uint16_t appId = *((uint16_t *)(packet+2));
uint16_t id = *((uint16_t *)(packet+2));

#if defined(PCBTARANIS) && !defined(SIMU)
if (sportUpdateState != SPORT_IDLE) {
Expand All @@ -192,77 +223,71 @@ void processSportPacket(uint8_t *packet)
if (prim == DATA_FRAME) {
uint32_t data = SPORT_DATA_S32(packet);

if (appId == RSSI_ID) {
if (id == RSSI_ID) {
frskyStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected
data = SPORT_DATA_U8(packet);
frskyData.rssi.set(data);
}
#if defined(PCBTARANIS) && defined(REVPLUS)
else if (appId == XJT_VERSION_ID) {
else if (id == XJT_VERSION_ID) {
frskyData.xjtVersion = HUB_DATA_U16(packet);
if (!IS_VALID_XJT_VERSION()) {
frskyData.swr.set(0x00);
}
}
else if (appId == SWR_ID) {
else if (id == SWR_ID) {
if (IS_VALID_XJT_VERSION())
frskyData.swr.set(SPORT_DATA_U8(packet));
else
frskyData.swr.set(0x00);
}
#else
else if (appId == XJT_VERSION_ID) {
else if (id == XJT_VERSION_ID) {
frskyData.xjtVersion = HUB_DATA_U16(packet);
}
else if (appId == SWR_ID) {
else if (id == SWR_ID) {
frskyData.swr.set(SPORT_DATA_U8(packet));
}
#endif

if (TELEMETRY_STREAMING()/* because when Rx is OFF it happens that some old A1/A2 values are sent from the XJT module*/) {
if ((appId >> 8) == 0) {
if ((id >> 8) == 0) {
// The old FrSky IDs
uint8_t id = (uint8_t)appId;
uint16_t value = HUB_DATA_U16(packet);
processHubPacket(id, value);
processHubPacket(id, HUB_DATA_U16(packet));
}
else if (!IS_HIDDEN_TELEMETRY_VALUE()) {
if (appId == ADC1_ID || appId == ADC2_ID || appId == BATT_ID || appId == SWR_ID) {
else if (!IS_HIDDEN_TELEMETRY_VALUE(id)) {
if (id == ADC1_ID || id == ADC2_ID || id == BATT_ID || id == SWR_ID) {
data = SPORT_DATA_U8(packet);
}
const FrSkySportSensor * sensor = getFrSkySportSensor(appId);
TelemetryUnit unit = UNIT_RAW;
uint8_t precision = 0;
if (sensor) {
unit = sensor->unit;
precision = sensor->prec;
if (id >= POWERBOX_BATT1_FIRST_ID && id <= POWERBOX_BATT2_LAST_ID) {
processSportPacket(id, 0, instance, data & 0xffff);
processSportPacket(id, 1, instance, data >> 16);
}
else if (id >= POWERBOX_CNSP_FIRST_ID && id <= POWERBOX_CNSP_LAST_ID) {
processSportPacket(id, 0, instance, data & 0xffff);
processSportPacket(id, 1, instance, data >> 16);
}
if (unit == UNIT_CELLS) {
uint8_t cellsCount = (data & 0xF0) >> 4;
uint8_t cellIndex = (data & 0x0F);
uint32_t mask = (cellsCount << 24) + (cellIndex << 16);
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, appId, dataId, mask + (((data & 0x000FFF00) >> 8) / 5), unit, precision);
if (cellIndex+1 < cellsCount) {
mask += (1 << 16);
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, appId, dataId, mask + (((data & 0xFFF00000) >> 20) / 5), unit, precision);
}
else if (id >= POWERBOX_STATE_FIRST_ID && id <= POWERBOX_STATE_LAST_ID) {
// TODO
processSportPacket(id, 0, instance, data);
}
else {
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, appId, dataId, data, unit, precision);
processSportPacket(id, 0, instance, data);
}
}
}
}
}

void frskySportSetDefault(int index, uint16_t id, uint8_t instance)
void frskySportSetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance)
{
TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];

telemetrySensor.id = id;
telemetrySensor.subId = subId;
telemetrySensor.instance = instance;

const FrSkySportSensor * sensor = getFrSkySportSensor(id);
const FrSkySportSensor * sensor = getFrSkySportSensor(id, subId);
if (sensor) {
TelemetryUnit unit = sensor->unit;
uint8_t prec = min<uint8_t>(2, sensor->prec);
Expand Down
6 changes: 3 additions & 3 deletions radio/src/telemetry/telemetry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -483,13 +483,13 @@ int lastUsedTelemetryIndex()
return -1;
}

void setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t instance, int32_t value, uint32_t unit, uint32_t prec)
void setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t subId, uint8_t instance, int32_t value, uint32_t unit, uint32_t prec)
{
bool available = false;

for (int index=0; index<MAX_SENSORS; index++) {
TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
if (telemetrySensor.type == TELEM_TYPE_CUSTOM && telemetrySensor.id == id && (telemetrySensor.instance == instance || g_model.ignoreSensorIds)) {
if (telemetrySensor.type == TELEM_TYPE_CUSTOM && telemetrySensor.id == id && telemetrySensor.subId == subId && (telemetrySensor.instance == instance || g_model.ignoreSensorIds)) {
telemetryItems[index].setValue(telemetrySensor, value, unit, prec);
available = true;
// we continue search here, because sensors can share the same id and instance
Expand All @@ -505,7 +505,7 @@ void setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t instance
switch (protocol) {
#if defined(FRSKY_SPORT)
case TELEM_PROTO_FRSKY_SPORT:
frskySportSetDefault(index, id, instance);
frskySportSetDefault(index, id, subId, instance);
break;
#endif
#if defined(FRSKY)
Expand Down
4 changes: 2 additions & 2 deletions radio/src/telemetry/telemetry.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -183,14 +183,14 @@ inline bool isTelemetryFieldComparisonAvailable(int index)
return (sensor.id != 0);
}

void setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t instance, int32_t value, uint32_t unit, uint32_t prec);
void setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t subId, uint8_t instance, int32_t value, uint32_t unit, uint32_t prec);
void delTelemetryIndex(uint8_t index);
int availableTelemetryIndex();
int lastUsedTelemetryIndex();
int32_t getTelemetryValue(uint8_t index, uint8_t & prec);
int32_t convertTelemetryValue(int32_t value, uint8_t unit, uint8_t prec, uint8_t destUnit, uint8_t destPrec);

void frskySportSetDefault(int index, uint16_t type, uint8_t instance);
void frskySportSetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance);
void frskyDSetDefault(int index, uint16_t id);

#define IS_DISTANCE_UNIT(unit) ((unit) == UNIT_METERS || (unit) == UNIT_FEET)
Expand Down
2 changes: 2 additions & 0 deletions radio/src/translations/en.h.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1127,3 +1127,5 @@
#define ZSTR_GPSALT "GAlt"
#define ZSTR_GPSDATETIME "Date"
#define ZSTR_GPS "GPS"
#define ZSTR_PBOX1 "PBx1"
#define ZSTR_PBOX2 "PBx2"

0 comments on commit 1b2ee0c

Please sign in to comment.