Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_ChibiOS: use unsigned math as appropriate

Browse Source
This commit is contained in:
Andy Piper 2023-06-08 20:10:12 +01:00 committed by Andrew Tridgell
parent 76b7566cd1
commit a7ad2dc940
1 changed files with 24 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp
View File

@ -548,6 +548,8 @@ uint32_t RCOutput::bdshot_get_output_rate_hz(const enum output_mode mode)
} }
} }
#define INVALID_ERPM 0xffffU
// decode a telemetry packet from a GCR encoded stride buffer, take from betaflight decodeTelemetryPacket // decode a telemetry packet from a GCR encoded stride buffer, take from betaflight decodeTelemetryPacket
// see https://github.com/betaflight/betaflight/pull/8554#issuecomment-512507625 for a description of the protocol // see https://github.com/betaflight/betaflight/pull/8554#issuecomment-512507625 for a description of the protocol
uint32_t RCOutput::bdshot_decode_telemetry_packet(uint32_t* buffer, uint32_t count) uint32_t RCOutput::bdshot_decode_telemetry_packet(uint32_t* buffer, uint32_t count)
@ -560,38 +562,38 @@ uint32_t RCOutput::bdshot_decode_telemetry_packet(uint32_t* buffer, uint32_t cou
for (uint32_t i = 1; i <= count; i++) { for (uint32_t i = 1; i <= count; i++) {
if (i < count) { if (i < count) {
int32_t diff = buffer[i] - oldValue; int32_t diff = buffer[i] - oldValue;
if (bits >= 21) { if (bits >= 21U) {
break; break;
} }
len = (diff + TELEM_IC_SAMPLE/2) / TELEM_IC_SAMPLE; len = (diff + TELEM_IC_SAMPLE/2U) / TELEM_IC_SAMPLE;
} else { } else {
len = 21 - bits; len = 21U - bits;
} }
value <<= len; value <<= len;
value |= 1 << (len - 1); value |= 1U << (len - 1U);
oldValue = buffer[i]; oldValue = buffer[i];
bits += len; bits += len;
} }
if (bits != 21) { if (bits != 21U) {
return 0xffff; return INVALID_ERPM;
} }
static const uint32_t decode[32] = { static const uint32_t decode[32] = {
0, 0, 0, 0, 0, 0, 0, 0, 0, 9, 10, 11, 0, 13, 14, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 9, 10, 11, 0, 13, 14, 15,
0, 0, 2, 3, 0, 5, 6, 7, 0, 0, 8, 1, 0, 4, 12, 0 }; 0, 0, 2, 3, 0, 5, 6, 7, 0, 0, 8, 1, 0, 4, 12, 0 };
uint32_t decodedValue = decode[value & 0x1f]; uint32_t decodedValue = decode[value & 0x1fU];
decodedValue |= decode[(value >> 5) & 0x1f] << 4; decodedValue |= decode[(value >> 5U) & 0x1fU] << 4U;
decodedValue |= decode[(value >> 10) & 0x1f] << 8; decodedValue |= decode[(value >> 10U) & 0x1fU] << 8U;
decodedValue |= decode[(value >> 15) & 0x1f] << 12; decodedValue |= decode[(value >> 15U) & 0x1fU] << 12U;
uint32_t csum = decodedValue; uint32_t csum = decodedValue;
csum = csum ^ (csum >> 8); // xor bytes csum = csum ^ (csum >> 8U); // xor bytes
csum = csum ^ (csum >> 4); // xor nibbles csum = csum ^ (csum >> 4U); // xor nibbles
if ((csum & 0xf) != 0xf) { if ((csum & 0xfU) != 0xfU) {
return 0xffff; return INVALID_ERPM;
} }
decodedValue >>= 4; decodedValue >>= 4;
@ -602,15 +604,15 @@ uint32_t RCOutput::bdshot_decode_telemetry_packet(uint32_t* buffer, uint32_t cou
// update ESC telemetry information. Returns true if valid eRPM data was decoded. // update ESC telemetry information. Returns true if valid eRPM data was decoded.
bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t chan) bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t chan)
{ {
if (encodederpm == 0xFFFF) { if (encodederpm == INVALID_ERPM) {
return false; return false;
} }
// eRPM = m << e (see https://github.com/bird-sanctuary/extended-dshot-telemetry) // eRPM = m << e (see https://github.com/bird-sanctuary/extended-dshot-telemetry)
uint8_t expo = ((encodederpm & 0xfffffe00) >> 9) & 0xFF; uint8_t expo = ((encodederpm & 0xfffffe00U) >> 9U) & 0xffU;
uint16_t value = (encodederpm & 0x000001ff); uint16_t value = (encodederpm & 0x000001ffU);
if (!(value & 0x100) && (_dshot_esc_type == DSHOT_ESC_BLHELI_EDT || _dshot_esc_type == DSHOT_ESC_BLHELI_EDT_S)) { if (!(value & 0x100U) && (_dshot_esc_type == DSHOT_ESC_BLHELI_EDT || _dshot_esc_type == DSHOT_ESC_BLHELI_EDT_S)) {
switch (expo) { switch (expo) {
case 0b001: { // Temperature C case 0b001: { // Temperature C
#if HAL_WITH_ESC_TELEM #if HAL_WITH_ESC_TELEM
@ -659,20 +661,20 @@ bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t c
return false; return false;
} }
erpm = (1000000 * 60 / 100 + erpm / 2) / erpm; erpm = (1000000U * 60U / 100U + erpm / 2U) / erpm;
if (encodederpm == 0x0fff) { // the special 0 encoding if (encodederpm == 0x0fff) { // the special 0 encoding
erpm = 0; erpm = 0;
} }
// update the ESC telemetry data // update the ESC telemetry data
if (erpm < 0xFFFF) { if (erpm < INVALID_ERPM) {
_bdshot.erpm[chan] = erpm; _bdshot.erpm[chan] = erpm;
#if HAL_WITH_ESC_TELEM #if HAL_WITH_ESC_TELEM
update_rpm(chan, erpm * 200 / _bdshot.motor_poles, get_erpm_error_rate(chan)); update_rpm(chan, erpm * 200U / _bdshot.motor_poles, get_erpm_error_rate(chan));
#endif #endif
} }
return erpm < 0xFFFF; return erpm < INVALID_ERPM;
} }
#endif // HAL_WITH_BIDIR_DSHOT #endif // HAL_WITH_BIDIR_DSHOT