Skip to content
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
30 changes: 30 additions & 0 deletions radio/src/pulses/crossfire.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -133,6 +133,35 @@ uint8_t createCrossfireChannelsFrame(uint8_t moduleIdx, uint8_t * frame, int16_t
return buf - frame;
}

// Range for pulses (channels output) is [-1024:+1024]
uint8_t createCrossfireExtendedChannelsFrame(uint8_t moduleIdx, uint8_t * frame, int16_t * pulses)
{
//
// sends extended channel data
ModuleData *md = &g_model.moduleData[moduleIdx];

uint8_t * buf = frame;
*buf++ = MODULE_ADDRESS;
*buf++ = 24; // 1(ID) + 22(channel data) + 1(CRC)
uint8_t * crc_start = buf;
*buf++ = EXTENDED_CHANNELS_ID;
uint32_t bits = 0;
uint8_t bitsavailable = 0;
for (int i=0; i<CROSSFIRE_CHANNELS_COUNT; i++) {
uint32_t val = limit(0, CROSSFIRE_CENTER + (CROSSFIRE_CENTER_CH_OFFSET(i) * 4) / 5 + (pulses[i+16] * 4) / 5, 2 * CROSSFIRE_CENTER);
bits |= val << bitsavailable;
bitsavailable += CROSSFIRE_CH_BITS;
while (bitsavailable >= 8) {
*buf++ = bits;
bits >>= 8;
bitsavailable -= 8;
}
}

*buf++ = crc8(crc_start, 23);
return buf - frame;
}

static void setupPulsesCrossfire(uint8_t module, uint8_t*& p_buf,
uint8_t endpoint, int16_t* channels,
uint8_t nChannels)
Expand Down Expand Up @@ -185,6 +214,7 @@ static void setupPulsesCrossfire(uint8_t module, uint8_t*& p_buf,
} else {
/* TODO: nChannels */
p_buf += createCrossfireChannelsFrame(module, p_buf, channels);
p_buf += createCrossfireExtendedChannelsFrame(module, p_buf, channels);
}
}
}
Expand Down
1 change: 1 addition & 0 deletions radio/src/telemetry/crossfire.h
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,7 @@
#define LINK_TX_ID 0x1D
#define ATTITUDE_ID 0x1E
#define FLIGHT_MODE_ID 0x21
#define EXTENDED_CHANNELS_ID 0x26
#define PING_DEVICES_ID 0x28
#define DEVICE_INFO_ID 0x29
#define REQUEST_SETTINGS_ID 0x2A
Expand Down