initialise RC output driver */
| 93 | initialise RC output driver |
| 94 | */ |
| 95 | void RCOutput::init() |
| 96 | { |
| 97 | if (_initialised) { |
| 98 | // cannot init RCOutput twice |
| 99 | return; |
| 100 | } |
| 101 | |
| 102 | #if HAL_WITH_IO_MCU |
| 103 | if (AP_BoardConfig::io_enabled()) { |
| 104 | // with IOMCU the local (FMU) channels start at 8 |
| 105 | chan_offset = 8; |
| 106 | iomcu_enabled = true; |
| 107 | } |
| 108 | if (AP_BoardConfig::io_dshot()) { |
| 109 | iomcu_dshot = true; |
| 110 | } |
| 111 | #endif |
| 112 | |
| 113 | for (auto &group : pwm_group_list) { |
| 114 | const uint8_t i = &group - pwm_group_list; |
| 115 | //Start Pwm groups |
| 116 | group.current_mode = MODE_PWM_NORMAL; |
| 117 | group.dshot_event_mask = EVENT_MASK(i); |
| 118 | |
| 119 | for (uint8_t j = 0; j < 4; j++ ) { |
| 120 | #if !defined(IOMCU_FW) |
| 121 | uint8_t chan = group.chan[j]; |
| 122 | if (SRV_Channels::is_GPIO(chan+chan_offset)) { |
| 123 | group.chan[j] = CHAN_DISABLED; |
| 124 | } else if (SRV_Channels::is_alarm(chan+chan_offset) |
| 125 | || SRV_Channels::is_alarm_inverted(chan+chan_offset)) { |
| 126 | // alarm takes the whole timer |
| 127 | group.ch_mask = 0; |
| 128 | group.current_mode = MODE_PWM_NONE; |
| 129 | for (uint8_t k = 0; k < 4; k++) { |
| 130 | group.chan[k] = CHAN_DISABLED; |
| 131 | group.pwm_cfg.channels[k].mode = PWM_OUTPUT_DISABLED; |
| 132 | } |
| 133 | ChibiOS::Util::from(hal.util)->toneAlarm_init(group.pwm_cfg, group.pwm_drv, j, |
| 134 | SRV_Channels::is_alarm(chan+chan_offset)); |
| 135 | break; |
| 136 | } |
| 137 | #endif |
| 138 | if (group.chan[j] != CHAN_DISABLED) { |
| 139 | num_fmu_channels = MAX(num_fmu_channels, group.chan[j]+1); |
| 140 | group.ch_mask |= (1U<<group.chan[j]); |
| 141 | } |
| 142 | #ifdef HAL_WITH_BIDIR_DSHOT |
| 143 | group.bdshot.telem_tim_ch[j] = CHAN_DISABLED; |
| 144 | #endif |
| 145 | } |
| 146 | if (group.ch_mask != 0) { |
| 147 | pwmStart(group.pwm_drv, &group.pwm_cfg); |
| 148 | group.pwm_started = true; |
| 149 | } |
| 150 | chVTObjectInit(&group.dma_timeout); |
| 151 | } |
| 152 |
nothing calls this directly
no test coverage detected