// Returns whether the data was successfully decoded
static bool decode_packet(uint8_t *packet, uint16_t *data)
{
switch (boundprotocol) {
case PROTO_NONE:
decode_bind_packet(packet);
break;
case PROTO_V2X2:
// Decode packet
if ((packet[14] & V2X2_FLAG_BIND) == V2X2_FLAG_BIND) {
return false;
}
if (packet[7] != usersettings.txid[0] ||
packet[8] != usersettings.txid[1] ||
packet[9] != usersettings.txid[2])
{
bad_packets++;
return false;
}
// Restore regular interval
rx_timeout = 10000L; // 4ms interval, duplicate packets, (8ms unique) + 25%
// TREA order in packet to MultiWii order is handled by
// correct assignment to channelindex
// Throttle 0..255 to 1000..2000
data[v2x2_channelindex[0]] = ((uint16_t)packet[0]) * 1000 / 255 + 1000;
for (int i = 1; i < 4; ++i) {
uint8_t a = packet[i];
data[v2x2_channelindex[i]] = ((uint16_t)(a < 0x80 ? 0x7f - a : a)) * 1000 / 255 + 1000;
}
uint8_t flags[] = {V2X2_FLAG_LED, V2X2_FLAG_FLIP, V2X2_FLAG_CAMERA, V2X2_FLAG_VIDEO}; // two more unknown bits
for (int i = 4; i < 8; ++i) {
data[v2x2_channelindex[i]] = 1000 + ((packet[14] & flags[i-4]) ? 1000 : 0);
}
packet_timer = lib_timers_starttimer();
if (++valid_packets > 50) bind_phase = PHASE_BOUND;
return true;
}
return false;
}
// Returns whether the data was successfully decoded
static rx_spi_received_e decode_packet(uint8_t *packet)
{
if (bind_phase == PHASE_NOT_BOUND) {
decode_bind_packet(packet);
return RX_SPI_RECEIVED_BIND;
}
// Decode packet
// Restore regular interval
rx_timeout = 13000L; // 13ms if data received
bind_phase = PHASE_RECEIVED;
for (int i = 0; i < 4; ++i) {
uint16_t a = packet[i*2];
uint16_t b = packet[(i*2)+1];
rxSpiRcData[kn_channelindex[i]] = ((uint16_t)(a<<8)+b) * 1000 / 1024 + 1000;
}
const uint8_t flags[] = {KN_FLAG_DR, KN_FLAG_TRHOLD, KN_FLAG_IDLEUP, KN_FLAG_TD};
for (int i = 4; i < 8; ++i) {
rxSpiRcData[kn_channelindex[i]] = (packet[12] & flags[i-4]) ? PWM_RANGE_MAX : PWM_RANGE_MIN;
}
packet_timer = micros();
return RX_SPI_RECEIVED_DATA;
}
// Returns whether the data was successfully decoded
static rx_spi_received_e decode_packet(uint8_t *packet)
{
if (bind_phase != PHASE_BOUND) {
decode_bind_packet(packet);
return RX_SPI_RECEIVED_BIND;
}
// Decode packet
if ((packet[14] & V2X2_FLAG_BIND) == V2X2_FLAG_BIND) {
return RX_SPI_RECEIVED_BIND;
}
if (packet[7] != txid[0] ||
packet[8] != txid[1] ||
packet[9] != txid[2]) {
return RX_SPI_RECEIVED_NONE;
}
// Restore regular interval
rx_timeout = 10000L; // 4ms interval, duplicate packets, (8ms unique) + 25%
// TREA order in packet to MultiWii order is handled by
// correct assignment to channelindex
// Throttle 0..255 to 1000..2000
rxSpiRcData[v2x2_channelindex[0]] = ((uint16_t)packet[0]) * 1000 / 255 + 1000;
for (int i = 1; i < 4; ++i) {
uint8_t a = packet[i];
rxSpiRcData[v2x2_channelindex[i]] = ((uint16_t)(a < 0x80 ? 0x7f - a : a)) * 1000 / 255 + 1000;
}
const uint8_t flags[] = {V2X2_FLAG_LED, V2X2_FLAG_FLIP, V2X2_FLAG_CAMERA, V2X2_FLAG_VIDEO}; // two more unknown bits
for (int i = 4; i < 8; ++i) {
rxSpiRcData[v2x2_channelindex[i]] = (packet[14] & flags[i-4]) ? PWM_RANGE_MAX : PWM_RANGE_MIN;
}
const uint8_t flags10[] = {V2X2_FLAG_HEADLESS, V2X2_FLAG_MAG_CAL_X, V2X2_FLAG_MAG_CAL_Y};
for (int i = 8; i < 11; ++i) {
rxSpiRcData[v2x2_channelindex[i]] = (packet[10] & flags10[i-8]) ? PWM_RANGE_MAX : PWM_RANGE_MIN;
}
packet_timer = micros();
return RX_SPI_RECEIVED_DATA;
}