diff --git a/libraries/AP_HAL_FLYMAPLE/examples/RCPassthroughTest/RCPassthroughTest.cpp b/libraries/AP_HAL_FLYMAPLE/examples/RCPassthroughTest/RCPassthroughTest.cpp index 87c6450e91..ac88a6773c 100644 --- a/libraries/AP_HAL_FLYMAPLE/examples/RCPassthroughTest/RCPassthroughTest.cpp +++ b/libraries/AP_HAL_FLYMAPLE/examples/RCPassthroughTest/RCPassthroughTest.cpp @@ -35,12 +35,6 @@ void individualread(AP_HAL::RCInput* in, uint16_t* channels) { } -void multiwrite(AP_HAL::RCOutput* out, uint16_t* channels) { - out->write(0, channels, 8); - /* Upper channels duplicate lower channels*/ - out->write(8, channels, 8); -} - void individualwrite(AP_HAL::RCOutput* out, uint16_t* channels) { for (int ch = 0; ch < 8; ch++) { out->write(ch, channels[ch]); @@ -64,12 +58,7 @@ void loop (void) { if (ctr > 1000) ctr = 0; } - /* Cycle between individual output and multichannel output */ - if (ctr % 500 < 250) { - multiwrite(hal.rcout, channels); - } else { - individualwrite(hal.rcout, channels); - } + individualwrite(hal.rcout, channels); // hal.gpio->write(13, 0); hal.scheduler->delay(4);