* Refactor CRSF and SmartPort telemetry #11296
Draft
+2,196
−648
Conversation
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
* Telemetry scheduler for CRSF and SmartPort
Branch Targeting SuggestionYou've targeted the
If This is an automated suggestion to help route contributions to the appropriate branch. |
Contributor
ⓘ Your approaching your monthly quota for Qodo. Upgrade your plan PR Compliance Guide 🔍All compliance sections have been disabled in the configurations. |
![qodo-code-review[bot]](https://avatars.githubusercontent.com/in/484649?s=60&v=4){kind=small}
Comment on lines
+292
to
+316
| void crsfSensorEncodeEscRpm(telemetrySensor_t *sensor, sbuf_t *buf) | ||
| { | ||
| UNUSED(sensor); | ||
| uint8_t motorCount = MAX(getMotorCount(), 1); //must send at least one motor, to avoid CRSF frame shifting | ||
| motorCount = MIN(getMotorCount(), CRSF_PAYLOAD_SIZE_MAX / 3); // 3 bytes per RPM value | ||
| motorCount = MIN(motorCount, MAX_SUPPORTED_MOTORS); // ensure we don't exceed available ESC telemetry data | ||
|
|
||
| for (uint8_t i = 0; i < motorCount; i++) { | ||
| const escSensorData_t *escState = getEscTelemetry(i); | ||
| crsfSerialize24BE(buf, escState->rpm & 0xFFFFFF); | ||
| } | ||
| return frameSize; | ||
| } | ||
|
|
||
| void crsfSensorEncodeEscTemp(telemetrySensor_t *sensor, sbuf_t *buf) | ||
| { | ||
| UNUSED(sensor); | ||
| uint8_t motorCount = MAX(getMotorCount(), 1); //must send at least one motor, to avoid CRSF frame shifting | ||
| motorCount = MIN(getMotorCount(), CRSF_PAYLOAD_SIZE_MAX / 3); // 3 bytes per RPM value | ||
| motorCount = MIN(motorCount, MAX_SUPPORTED_MOTORS); // ensure we don't exceed available ESC telemetry data | ||
|
|
||
| for (uint8_t i = 0; i < motorCount; i++) { | ||
| const escSensorData_t *escState = getEscTelemetry(i); | ||
| crsfSerialize16BE(buf, escState->temperature & 0xFFFF); | ||
| } | ||
| } |
Contributor
There was a problem hiding this comment.
Suggestion: Add a null check for the escState pointer in crsfSensorEncodeEscRpm and crsfSensorEncodeEscTemp to prevent a potential crash before dereferencing it. [possible issue, importance: 8]
Suggested change
| void crsfSensorEncodeEscRpm(telemetrySensor_t *sensor, sbuf_t *buf) | |
| { | |
| UNUSED(sensor); | |
| uint8_t motorCount = MAX(getMotorCount(), 1); //must send at least one motor, to avoid CRSF frame shifting | |
| motorCount = MIN(getMotorCount(), CRSF_PAYLOAD_SIZE_MAX / 3); // 3 bytes per RPM value | |
| motorCount = MIN(motorCount, MAX_SUPPORTED_MOTORS); // ensure we don't exceed available ESC telemetry data | |
| for (uint8_t i = 0; i < motorCount; i++) { | |
| const escSensorData_t *escState = getEscTelemetry(i); | |
| crsfSerialize24BE(buf, escState->rpm & 0xFFFFFF); | |
| } | |
| return frameSize; | |
| } | |
| void crsfSensorEncodeEscTemp(telemetrySensor_t *sensor, sbuf_t *buf) | |
| { | |
| UNUSED(sensor); | |
| uint8_t motorCount = MAX(getMotorCount(), 1); //must send at least one motor, to avoid CRSF frame shifting | |
| motorCount = MIN(getMotorCount(), CRSF_PAYLOAD_SIZE_MAX / 3); // 3 bytes per RPM value | |
| motorCount = MIN(motorCount, MAX_SUPPORTED_MOTORS); // ensure we don't exceed available ESC telemetry data | |
| for (uint8_t i = 0; i < motorCount; i++) { | |
| const escSensorData_t *escState = getEscTelemetry(i); | |
| crsfSerialize16BE(buf, escState->temperature & 0xFFFF); | |
| } | |
| } | |
| void crsfSensorEncodeEscRpm(telemetrySensor_t *sensor, sbuf_t *buf) | |
| { | |
| UNUSED(sensor); | |
| uint8_t motorCount = MAX(getMotorCount(), 1); //must send at least one motor, to avoid CRSF frame shifting | |
| motorCount = MIN(getMotorCount(), CRSF_PAYLOAD_SIZE_MAX / 3); // 3 bytes per RPM value | |
| motorCount = MIN(motorCount, MAX_SUPPORTED_MOTORS); // ensure we don't exceed available ESC telemetry data | |
| for (uint8_t i = 0; i < motorCount; i++) { | |
| const escSensorData_t *escState = getEscTelemetry(i); | |
| crsfSerialize24BE(buf, escState ? (escState->rpm & 0xFFFFFF) : 0); | |
| } | |
| } | |
| void crsfSensorEncodeEscTemp(telemetrySensor_t *sensor, sbuf_t *buf) | |
| { | |
| UNUSED(sensor); | |
| uint8_t motorCount = MAX(getMotorCount(), 1); //must send at least one motor, to avoid CRSF frame shifting | |
| motorCount = MIN(getMotorCount(), CRSF_PAYLOAD_SIZE_MAX / 3); // 3 bytes per RPM value | |
| motorCount = MIN(motorCount, MAX_SUPPORTED_MOTORS); // ensure we don't exceed available ESC telemetry data | |
| for (uint8_t i = 0; i < motorCount; i++) { | |
| const escSensorData_t *escState = getEscTelemetry(i); | |
| crsfSerialize16BE(buf, escState ? (escState->temperature & 0xFFFF) : 0); | |
| } | |
| } |
Comment on lines
+156
to
+176
| static void smartPortSensorEncodeLat(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) | ||
| { | ||
| uint32_t lat = (uint32_t)(abs(gpsSol.llh.lat) * 6) / 100; //danger zone, may overflow for latitudes > 3579139.2 degrees, so convert int from abs function to uint32_t | ||
|
|
||
| if (gpsSol.llh.lat < 0) { | ||
| lat |= BIT(30); | ||
| } | ||
|
|
||
| payload->data = lat; | ||
| } | ||
|
|
||
| static void smartPortSensorEncodeLon(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) | ||
| { | ||
| uint32_t lon = (uint32_t)(abs(gpsSol.llh.lon) * 6) / 100; //danger zone, may overflow for longitudes > 3579139.2 degrees , so convert int from abs function to uint32_t | ||
|
|
||
| if (gpsSol.llh.lon < 0) { | ||
| lon |= BIT(30); | ||
| } | ||
|
|
||
| payload->data = lon | BIT(31); | ||
| } |
Contributor
There was a problem hiding this comment.
Suggestion: Use floating-point arithmetic for GPS coordinate calculations in smartPortSensorEncodeLat and smartPortSensorEncodeLon to avoid precision loss. [general, importance: 7]
Suggested change
| static void smartPortSensorEncodeLat(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) | |
| { | |
| uint32_t lat = (uint32_t)(abs(gpsSol.llh.lat) * 6) / 100; //danger zone, may overflow for latitudes > 3579139.2 degrees, so convert int from abs function to uint32_t | |
| if (gpsSol.llh.lat < 0) { | |
| lat |= BIT(30); | |
| } | |
| payload->data = lat; | |
| } | |
| static void smartPortSensorEncodeLon(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) | |
| { | |
| uint32_t lon = (uint32_t)(abs(gpsSol.llh.lon) * 6) / 100; //danger zone, may overflow for longitudes > 3579139.2 degrees , so convert int from abs function to uint32_t | |
| if (gpsSol.llh.lon < 0) { | |
| lon |= BIT(30); | |
| } | |
| payload->data = lon | BIT(31); | |
| } | |
| static void smartPortSensorEncodeLat(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) | |
| { | |
| uint32_t lat = (uint32_t)(fabsf(gpsSol.llh.lat) * 0.06f); | |
| if (gpsSol.llh.lat < 0) { | |
| lat |= BIT(30); | |
| } | |
| payload->data = lat; | |
| } | |
| static void smartPortSensorEncodeLon(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) | |
| { | |
| uint32_t lon = (uint32_t)(fabsf(gpsSol.llh.lon) * 0.06f); | |
| if (gpsSol.llh.lon < 0) { | |
| lon |= BIT(30); | |
| } | |
| payload->data = lon | BIT(31); | |
| } |
Comment on lines
+277
to
+293
| static void smartPortSensorEncodeLat(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) | ||
| { | ||
| uint32_t tmpui = abs(gpsSol.llh.lon); // now we have unsigned value and one bit to spare | ||
| tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast | ||
| if (gpsSol.llh.lon < 0) tmpui |= 0x40000000; | ||
|
|
||
| payload->data = tmpui; | ||
| } | ||
|
|
||
| static void smartPortSensorEncodeLon(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) | ||
| { | ||
| uint32_t tmpui = abs(gpsSol.llh.lat); // now we have unsigned value and one bit to spare | ||
| tmpui = (tmpui + tmpui / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast | ||
| if (gpsSol.llh.lat < 0) tmpui |= 0x40000000; | ||
|
|
||
| payload->data = tmpui; | ||
| } |
Contributor
There was a problem hiding this comment.
Suggestion: Swap the use of gpsSol.llh.lon and gpsSol.llh.lat in the smartPortSensorEncodeLat and smartPortSensorEncodeLon functions to correctly encode GPS coordinates. [possible issue, importance: 9]
Suggested change
| static void smartPortSensorEncodeLat(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) | |
| { | |
| uint32_t tmpui = abs(gpsSol.llh.lon); // now we have unsigned value and one bit to spare | |
| tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast | |
| if (gpsSol.llh.lon < 0) tmpui |= 0x40000000; | |
| payload->data = tmpui; | |
| } | |
| static void smartPortSensorEncodeLon(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) | |
| { | |
| uint32_t tmpui = abs(gpsSol.llh.lat); // now we have unsigned value and one bit to spare | |
| tmpui = (tmpui + tmpui / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast | |
| if (gpsSol.llh.lat < 0) tmpui |= 0x40000000; | |
| payload->data = tmpui; | |
| } | |
| static void smartPortSensorEncodeLat(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) | |
| { | |
| uint32_t tmpui = abs(gpsSol.llh.lat); // now we have unsigned value and one bit to spare | |
| tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast | |
| if (gpsSol.llh.lat < 0) tmpui |= 0x40000000; | |
| payload->data = tmpui; | |
| } | |
| static void smartPortSensorEncodeLon(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) | |
| { | |
| uint32_t tmpui = abs(gpsSol.llh.lon); // now we have unsigned value and one bit to spare | |
| tmpui = (tmpui + tmpui / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast | |
| if (gpsSol.llh.lon < 0) tmpui |= 0x40000000; | |
| payload->data = tmpui; | |
| } |
Comment on lines
+133
to
+134
| case TELEM_GPS_AZIMUTH: | ||
| return ((GPS_directionToHome < 0 ? GPS_directionToHome + 360 : GPS_directionToHome) + 180) % 360; |
Contributor
There was a problem hiding this comment.
Suggestion: Remove the addition of 180 degrees from the TELEM_GPS_AZIMUTH calculation to report the correct direction to home. [possible issue, importance: 8]
Suggested change
| case TELEM_GPS_AZIMUTH: | |
| return ((GPS_directionToHome < 0 ? GPS_directionToHome + 360 : GPS_directionToHome) + 180) % 360; | |
| case TELEM_GPS_AZIMUTH: | |
| return GPS_directionToHome < 0 ? GPS_directionToHome + 360 : GPS_directionToHome; |
Comment on lines
+285
to
+305
| void telemetryScheduleUpdate(timeUs_t currentTime) | ||
| { | ||
| timeDelta_t delta = cmpTimeUs(currentTime, sch.update_time); | ||
|
|
||
| for (int i = 0; i < sch.sensor_count; i++) { | ||
| telemetrySensor_t * sensor = &sch.sensors[i]; | ||
| if (sensor->active) { | ||
| int value = telemetrySensorValue(sensor->sensor_id); | ||
| if (sensor->ratio_den) | ||
| value = value * sensor->ratio_num / sensor->ratio_den; | ||
| sensor->update |= (value != sensor->value); | ||
| sensor->value = value; | ||
|
|
||
| const int interval = (sensor->update) ? sensor->fast_interval : sensor->slow_interval; | ||
| sensor->bucket += delta * 1000 / interval; | ||
| sensor->bucket = constrain(sensor->bucket, sch.min_level, sch.max_level); | ||
| } | ||
| } | ||
|
|
||
| sch.update_time = currentTime; | ||
| } |
Contributor
There was a problem hiding this comment.
Suggestion: In telemetryScheduleUpdate, add a check to ensure interval is greater than zero before performing division to prevent a potential division-by-zero crash. [possible issue, importance: 8]
Suggested change
| void telemetryScheduleUpdate(timeUs_t currentTime) | |
| { | |
| timeDelta_t delta = cmpTimeUs(currentTime, sch.update_time); | |
| for (int i = 0; i < sch.sensor_count; i++) { | |
| telemetrySensor_t * sensor = &sch.sensors[i]; | |
| if (sensor->active) { | |
| int value = telemetrySensorValue(sensor->sensor_id); | |
| if (sensor->ratio_den) | |
| value = value * sensor->ratio_num / sensor->ratio_den; | |
| sensor->update |= (value != sensor->value); | |
| sensor->value = value; | |
| const int interval = (sensor->update) ? sensor->fast_interval : sensor->slow_interval; | |
| sensor->bucket += delta * 1000 / interval; | |
| sensor->bucket = constrain(sensor->bucket, sch.min_level, sch.max_level); | |
| } | |
| } | |
| sch.update_time = currentTime; | |
| } | |
| void telemetryScheduleUpdate(timeUs_t currentTime) | |
| { | |
| timeDelta_t delta = cmpTimeUs(currentTime, sch.update_time); | |
| for (int i = 0; i < sch.sensor_count; i++) { | |
| telemetrySensor_t * sensor = &sch.sensors[i]; | |
| if (sensor->active) { | |
| int value = telemetrySensorValue(sensor->sensor_id); | |
| if (sensor->ratio_den) | |
| value = value * sensor->ratio_num / sensor->ratio_den; | |
| sensor->update |= (value != sensor->value); | |
| sensor->value = value; | |
| const int interval = (sensor->update) ? sensor->fast_interval : sensor->slow_interval; | |
| if (interval > 0) { | |
| sensor->bucket += delta * 1000 / interval; | |
| } | |
| sensor->bucket = constrain(sensor->bucket, sch.min_level, sch.max_level); | |
| } | |
| } | |
| sch.update_time = currentTime; | |
| } |
Comment on lines
+374
to
+375
| TLM_SENSOR(LEGACY_LAT , 0x0800, 200, 200, 0, 0, 0, Lat), | ||
| TLM_SENSOR(LEGACY_LON , 0x0800, 200, 200, 0, 0, 0, Lon), |
Contributor
There was a problem hiding this comment.
Suggestion: Assign a unique app_id to LEGACY_LON to prevent a collision with LEGACY_LAT in the SmartPort sensor definitions. [possible issue, importance: 9]
Suggested change
| TLM_SENSOR(LEGACY_LAT , 0x0800, 200, 200, 0, 0, 0, Lat), | |
| TLM_SENSOR(LEGACY_LON , 0x0800, 200, 200, 0, 0, 0, Lon), | |
| TLM_SENSOR(LEGACY_LAT , 0x0800, 200, 200, 0, 0, 0, Lat), | |
| TLM_SENSOR(LEGACY_LON , 0x0810, 200, 200, 0, 0, 0, Lon), |
Comment on lines
+215
to
+222
| static void smartPortSensorEncodeFuel(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) | ||
| { | ||
| if (telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_PERCENT) { | ||
| payload->data = calculateBatteryPercentage(); // Show remaining battery % if smartport_fuel_percent=ON | ||
| } else if (isAmperageConfigured()) { | ||
| payload->data = telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_MAH ? getMAhDrawn() : getMWhDrawn(); | ||
| } | ||
| } |
Contributor
There was a problem hiding this comment.
Suggestion: In smartPortSensorEncodeFuel, add an else block to initialize payload->data to 0, preventing it from being uninitialized if no fuel reporting method is configured. [general, importance: 6]
Suggested change
| static void smartPortSensorEncodeFuel(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) | |
| { | |
| if (telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_PERCENT) { | |
| payload->data = calculateBatteryPercentage(); // Show remaining battery % if smartport_fuel_percent=ON | |
| } else if (isAmperageConfigured()) { | |
| payload->data = telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_MAH ? getMAhDrawn() : getMWhDrawn(); | |
| } | |
| } | |
| static void smartPortSensorEncodeFuel(__unused telemetrySensor_t *sensor, smartPortPayload_t *payload) | |
| { | |
| if (telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_PERCENT) { | |
| payload->data = calculateBatteryPercentage(); | |
| } else if (isAmperageConfigured()) { | |
| payload->data = telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_MAH ? getMAhDrawn() : getMWhDrawn(); | |
| } else { | |
| payload->data = 0; | |
| } | |
| } |
| const float rate = telemetryConfig()->crsf_telemetry_link_rate; | ||
| const float ratio = telemetryConfig()->crsf_telemetry_link_ratio; | ||
|
|
||
| crsfTelemetryRateQuanta = rate / (ratio * 1000000); |
Contributor
There was a problem hiding this comment.
Suggestion: Add validation to prevent divide-by-zero when ratio is zero. Check that ratio > 0 before performing the division, and provide a fallback value or early return if invalid. [Learned best practice, importance: 6]
Suggested change
| crsfTelemetryRateQuanta = rate / (ratio * 1000000); | |
| if (ratio > 0) { | |
| crsfTelemetryRateQuanta = rate / (ratio * 1000000); | |
| } else { | |
| crsfTelemetryRateQuanta = 0; | |
| } |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
User description
Recreated PR.
Original PR was closed after branch was deleted/recreated.
Last commit preserved: 28b6ed8 :(
Note: this PR needs updates in INAV Configurator
Global changes
SmarPort
Ethos DIY sensors, how to add
it's native Ethos functionality, no needed to use any extra LUA script
Ethos sensors
CRSF
CSFR CUSTOMtelemetry sensors
CSFR NATIVE telemetry sensors
For considering
Need to do
PR Type
Enhancement
Description
This description is generated by an AI tool. It may have inaccuracies
smartportFuelUnitsettingcrsf_telemetry_modesetting supporting NATIVE (CRSF native telemetry) and CUSTOM modescrsf_telemetry_link_rateandcrsf_telemetry_link_ratiosettings for ELRS synchronizationDiagram Walkthrough
File Walkthrough