72 lines
1.7 KiB
C++
72 lines
1.7 KiB
C++
|
#pragma once
|
||
|
|
|
||
|
|
#include <cstdint>
|
||
|
|
|
||
|
|
namespace Driver::Esc::DShot {
|
||
|
|
|
||
|
|
#define DSHOT1200_PWM_MHZ 24
|
||
|
|
#define DSHOT600_PWM_MHZ 12
|
||
|
|
#define DSHOT300_PWM_MHZ 6
|
||
|
|
#define DSHOT150_PWM_MHZ 3
|
||
|
|
|
||
|
|
#define DSHOT_BIT_0 7
|
||
|
|
#define DSHOT_BIT_1 14
|
||
|
|
#define DSHOT_BIT_LENGTH 20
|
||
|
|
|
||
|
|
inline constexpr std::uint16_t encodePacket(std::uint16_t throttleValue, bool requestTelemetry) {
|
||
|
|
//cap value
|
||
|
|
if (throttleValue > 2047) {
|
||
|
|
throttleValue = 2047;
|
||
|
|
}
|
||
|
|
|
||
|
|
//throttleValue = 11;
|
||
|
|
|
||
|
|
//add request telemetry bit
|
||
|
|
throttleValue = (throttleValue << 1) | (requestTelemetry ? 1 : 0);
|
||
|
|
|
||
|
|
bool useDshotTelemetry = false;
|
||
|
|
|
||
|
|
// compute checksum
|
||
|
|
uint16_t csum = 0;
|
||
|
|
uint16_t csum_data = throttleValue;
|
||
|
|
for (int i = 0; i < 3; i++) {
|
||
|
|
csum ^= csum_data;
|
||
|
|
csum_data >>= 4;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (useDshotTelemetry) {
|
||
|
|
csum = ~csum;
|
||
|
|
}
|
||
|
|
|
||
|
|
//append checksum
|
||
|
|
throttleValue = (throttleValue << 4) | (csum & 0x0f);
|
||
|
|
|
||
|
|
return throttleValue;
|
||
|
|
}
|
||
|
|
|
||
|
|
template <class Word, std::uint8_t ZeroPaddingLength = 1>
|
||
|
|
class DmaBuffer {
|
||
|
|
public:
|
||
|
|
static constexpr std::uint8_t BufferSize = 16 + ZeroPaddingLength;
|
||
|
|
std::uint32_t size() const {
|
||
|
|
return BufferSize;
|
||
|
|
}
|
||
|
|
DmaBuffer() {
|
||
|
|
for (int i = 0; i < ZeroPaddingLength; ++i) {
|
||
|
|
_data[16 + i] = 0;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
void fill(std::uint16_t packet) {
|
||
|
|
for (int i = 0; i < 16; ++i) {
|
||
|
|
_data[i] = (packet & (0x8000 >> i)) ? DSHOT_BIT_1 : DSHOT_BIT_0;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
Word* data() {
|
||
|
|
return _data;
|
||
|
|
}
|
||
|
|
|
||
|
|
private:
|
||
|
|
Word _data[BufferSize];
|
||
|
|
};
|
||
|
|
} // namespace Driver::Esc::DShot
|