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

AP_SBusOut: make sbus output exactly match sbus input decoding

Browse Source
This commit is contained in:
Andrew Tridgell 2023-10-04 11:08:46 +11:00
parent ca4f26db17
commit 11b24e3ec0
1 changed files with 6 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_SBusOut/AP_SBusOut.cpp
View File

@ -51,13 +51,9 @@ extern const AP_HAL::HAL& hal;
#define SBUS_DEBUG 0 #define SBUS_DEBUG 0
// SBUS1 constant definitions #define SBUS_BSIZE 25
// pulse widths measured using FrSky Sbus/PWM converter
#define SBUS_BSIZE 25
#define SBUS_CHANNELS 16 #define SBUS_CHANNELS 16
#define SBUS_MIN 880.0f #define SBUS_MIN 875
#define SBUS_MAX 2156.0f
#define SBUS_SCALE (2048.0f / (SBUS_MAX - SBUS_MIN))
const AP_Param::GroupInfo AP_SBusOut::var_info[] = { const AP_Param::GroupInfo AP_SBusOut::var_info[] = {
// @Param: RATE // @Param: RATE
@ -91,11 +87,12 @@ void AP_SBusOut::sbus_format_frame(uint16_t *channels, uint8_t num_channels, uin
buffer[0] = 0x0f; buffer[0] = 0x0f;
/* construct sbus frame representing channels 1 through 16 (max) */ /* construct sbus frame representing channels 1 through 16 (max) */
uint8_t nchan = MIN(num_channels, SBUS_CHANNELS); const uint8_t nchan = MIN(num_channels, SBUS_CHANNELS);
for (unsigned i = 0; i < nchan; ++i) { for (unsigned i = 0; i < nchan; ++i) {
/*protect from out of bounds values and limit to 11 bits*/ /*protect from out of bounds values and limit to 11 bits*/
uint16_t pwmval = MAX(channels[i], SBUS_MIN); const uint16_t pwmval = MAX(channels[i], SBUS_MIN);
uint16_t value = (uint16_t)((pwmval - SBUS_MIN) * SBUS_SCALE); const uint32_t v1 = uint32_t(pwmval - SBUS_MIN) * 1600U;
uint16_t value = uint16_t(v1 / 1000U) + 1;
if (value > 0x07ff) { if (value > 0x07ff) {
value = 0x07ff; value = 0x07ff;
} }