SRV_Channels: set channels above 16 to GPIO by defualt

This commit is contained in:
Iampete1 2022-01-02 17:41:22 +00:00 committed by Andrew Tridgell
parent 48b3549158
commit a5928b752e
1 changed files with 6 additions and 0 deletions

View File

@ -356,6 +356,12 @@ SRV_Channels::SRV_Channels(void)
// setup ch_num on channels
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
channels[i].ch_num = i;
#if NUM_SERVO_CHANNELS > 16
if (i >= 16) {
// default to GPIO, this disables the pin and stops logging
channels[i].function.set_default(SRV_Channel::k_GPIO);
}
#endif
}
#if AP_FETTEC_ONEWIRE_ENABLED