feat: support for ELRS/CRSF 0x03 GPS time frame (#7317)
This commit is contained in:
@@ -66,6 +66,7 @@ const CrossfireSensor crossfireSensors[] = {
|
|||||||
CS(GPS_ID, 3, STR_DEF(STR_SENSOR_HDG), UNIT_DEGREE, 2),
|
CS(GPS_ID, 3, STR_DEF(STR_SENSOR_HDG), UNIT_DEGREE, 2),
|
||||||
CS(GPS_ID, 4, STR_DEF(STR_SENSOR_GPSALT), UNIT_METERS, 0),
|
CS(GPS_ID, 4, STR_DEF(STR_SENSOR_GPSALT), UNIT_METERS, 0),
|
||||||
CS(GPS_ID, 5, STR_DEF(STR_SENSOR_SATELLITES), UNIT_RAW, 0),
|
CS(GPS_ID, 5, STR_DEF(STR_SENSOR_SATELLITES), UNIT_RAW, 0),
|
||||||
|
CS(GPS_TIME_ID, 0, STR_DEF(STR_SENSOR_GPSDATETIME), UNIT_DATETIME, 0),
|
||||||
CS(ATTITUDE_ID, 0, STR_DEF(STR_SENSOR_PITCH), UNIT_RADIANS, 3),
|
CS(ATTITUDE_ID, 0, STR_DEF(STR_SENSOR_PITCH), UNIT_RADIANS, 3),
|
||||||
CS(ATTITUDE_ID, 1, STR_DEF(STR_SENSOR_ROLL), UNIT_RADIANS, 3),
|
CS(ATTITUDE_ID, 1, STR_DEF(STR_SENSOR_ROLL), UNIT_RADIANS, 3),
|
||||||
CS(ATTITUDE_ID, 2, STR_DEF(STR_SENSOR_YAW), UNIT_RADIANS, 3),
|
CS(ATTITUDE_ID, 2, STR_DEF(STR_SENSOR_YAW), UNIT_RADIANS, 3),
|
||||||
@@ -95,6 +96,8 @@ const CrossfireSensor & getCrossfireSensor(uint8_t id, uint8_t subId)
|
|||||||
return crossfireSensors[BATT_VOLTAGE_INDEX + subId];
|
return crossfireSensors[BATT_VOLTAGE_INDEX + subId];
|
||||||
else if (id == GPS_ID)
|
else if (id == GPS_ID)
|
||||||
return crossfireSensors[GPS_LATITUDE_INDEX + subId];
|
return crossfireSensors[GPS_LATITUDE_INDEX + subId];
|
||||||
|
else if (id == GPS_TIME_ID)
|
||||||
|
return crossfireSensors[GPS_TIME_INDEX];
|
||||||
else if (id == CF_VARIO_ID)
|
else if (id == CF_VARIO_ID)
|
||||||
return crossfireSensors[VERTICAL_SPEED_INDEX];
|
return crossfireSensors[VERTICAL_SPEED_INDEX];
|
||||||
else if (id == ATTITUDE_ID)
|
else if (id == ATTITUDE_ID)
|
||||||
@@ -176,6 +179,31 @@ void processCrossfireTelemetryFrame(uint8_t module, uint8_t* rxBuffer,
|
|||||||
processCrossfireTelemetryValue(GPS_SATELLITES_INDEX, value);
|
processCrossfireTelemetryValue(GPS_SATELLITES_INDEX, value);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case GPS_TIME_ID:
|
||||||
|
{
|
||||||
|
// Payload: year (2B BE), month, day, hour, min, sec, millisecond (2B BE)
|
||||||
|
const CrossfireSensor & sensor = crossfireSensors[GPS_TIME_INDEX];
|
||||||
|
if (!getCrossfireTelemetryValue<2>(3, value, rxBuffer))
|
||||||
|
break;
|
||||||
|
uint8_t year = (uint8_t)((uint16_t)value - 2000);
|
||||||
|
uint8_t month = rxBuffer[5];
|
||||||
|
uint8_t day = rxBuffer[6];
|
||||||
|
uint8_t hour = rxBuffer[7];
|
||||||
|
uint8_t minute = rxBuffer[8];
|
||||||
|
uint8_t sec = rxBuffer[9];
|
||||||
|
// Date record: low byte non-zero (acts as date/time discriminator)
|
||||||
|
uint32_t dateVal = ((uint32_t)year << 24) | ((uint32_t)month << 16)
|
||||||
|
| ((uint32_t)day << 8) | 0xFF;
|
||||||
|
setTelemetryValue(PROTOCOL_TELEMETRY_CROSSFIRE, sensor.id, 0, sensor.subId,
|
||||||
|
(int32_t)dateVal, UNIT_DATETIME, 0);
|
||||||
|
// Time record: low byte zero
|
||||||
|
uint32_t timeVal = ((uint32_t)hour << 24) | ((uint32_t)minute << 16)
|
||||||
|
| ((uint32_t)sec << 8);
|
||||||
|
setTelemetryValue(PROTOCOL_TELEMETRY_CROSSFIRE, sensor.id, 0, sensor.subId,
|
||||||
|
(int32_t)timeVal, UNIT_DATETIME, 0);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
case BARO_ALT_ID:
|
case BARO_ALT_ID:
|
||||||
if (getCrossfireTelemetryValue<2>(3, value, rxBuffer)) {
|
if (getCrossfireTelemetryValue<2>(3, value, rxBuffer)) {
|
||||||
if (value & 0x8000) {
|
if (value & 0x8000) {
|
||||||
|
|||||||
@@ -32,6 +32,7 @@
|
|||||||
|
|
||||||
// Frame id
|
// Frame id
|
||||||
#define GPS_ID 0x02
|
#define GPS_ID 0x02
|
||||||
|
#define GPS_TIME_ID 0x03
|
||||||
#define CF_VARIO_ID 0x07
|
#define CF_VARIO_ID 0x07
|
||||||
#define BATTERY_ID 0x08
|
#define BATTERY_ID 0x08
|
||||||
#define BARO_ALT_ID 0x09
|
#define BARO_ALT_ID 0x09
|
||||||
@@ -85,6 +86,7 @@ enum CrossfireSensorIndexes {
|
|||||||
GPS_HEADING_INDEX,
|
GPS_HEADING_INDEX,
|
||||||
GPS_ALTITUDE_INDEX,
|
GPS_ALTITUDE_INDEX,
|
||||||
GPS_SATELLITES_INDEX,
|
GPS_SATELLITES_INDEX,
|
||||||
|
GPS_TIME_INDEX,
|
||||||
ATTITUDE_PITCH_INDEX,
|
ATTITUDE_PITCH_INDEX,
|
||||||
ATTITUDE_ROLL_INDEX,
|
ATTITUDE_ROLL_INDEX,
|
||||||
ATTITUDE_YAW_INDEX,
|
ATTITUDE_YAW_INDEX,
|
||||||
|
|||||||
Reference in New Issue
Block a user