diff --git a/libraries/AP_SBusOut/AP_SBusOut.cpp b/libraries/AP_SBusOut/AP_SBusOut.cpp index 35d9d2e044..a49df6d018 100644 --- a/libraries/AP_SBusOut/AP_SBusOut.cpp +++ b/libraries/AP_SBusOut/AP_SBusOut.cpp @@ -51,13 +51,9 @@ extern const AP_HAL::HAL& hal; #define SBUS_DEBUG 0 -// SBUS1 constant definitions -// pulse widths measured using FrSky Sbus/PWM converter -#define SBUS_BSIZE 25 +#define SBUS_BSIZE 25 #define SBUS_CHANNELS 16 -#define SBUS_MIN 880.0f -#define SBUS_MAX 2156.0f -#define SBUS_SCALE (2048.0f / (SBUS_MAX - SBUS_MIN)) +#define SBUS_MIN 875 const AP_Param::GroupInfo AP_SBusOut::var_info[] = { // @Param: RATE @@ -91,11 +87,12 @@ void AP_SBusOut::sbus_format_frame(uint16_t *channels, uint8_t num_channels, uin buffer[0] = 0x0f; /* 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) { /*protect from out of bounds values and limit to 11 bits*/ - uint16_t pwmval = MAX(channels[i], SBUS_MIN); - uint16_t value = (uint16_t)((pwmval - SBUS_MIN) * SBUS_SCALE); + const uint16_t pwmval = MAX(channels[i], SBUS_MIN); + const uint32_t v1 = uint32_t(pwmval - SBUS_MIN) * 1600U; + uint16_t value = uint16_t(v1 / 1000U) + 1; if (value > 0x07ff) { value = 0x07ff; }