Send a dshot command, if command timout is 0 then 10 commands are sent chan is the servo channel to send the command to
| 94 | // Send a dshot command, if command timout is 0 then 10 commands are sent |
| 95 | // chan is the servo channel to send the command to |
| 96 | void RCOutput::send_dshot_command(uint8_t command, uint8_t chan, uint32_t command_timeout_ms, uint16_t repeat_count, bool priority) |
| 97 | { |
| 98 | // once armed only priority commands will be accepted |
| 99 | if (hal.util->get_soft_armed() && !priority) { |
| 100 | return; |
| 101 | } |
| 102 | // not an FMU channel |
| 103 | if (chan < chan_offset || chan == ALL_CHANNELS) { |
| 104 | #if HAL_WITH_IO_MCU |
| 105 | if (iomcu_dshot) { |
| 106 | iomcu.send_dshot_command(command, chan, command_timeout_ms, repeat_count, priority); |
| 107 | } |
| 108 | #endif |
| 109 | if (chan != ALL_CHANNELS) { |
| 110 | return; |
| 111 | } |
| 112 | } |
| 113 | |
| 114 | DshotCommandPacket pkt; |
| 115 | pkt.command = command; |
| 116 | if (chan != ALL_CHANNELS) { |
| 117 | pkt.chan = chan - chan_offset; // normalize to FMU channel |
| 118 | } else { |
| 119 | pkt.chan = ALL_CHANNELS; |
| 120 | } |
| 121 | |
| 122 | if (command_timeout_ms == 0) { |
| 123 | pkt.cycle = MAX(10, repeat_count); |
| 124 | } else { |
| 125 | pkt.cycle = MAX(command_timeout_ms * 1000 / _dshot_period_us, repeat_count); |
| 126 | } |
| 127 | |
| 128 | // prioritize anything that is not an LED or BEEP command |
| 129 | if (!_dshot_command_queue.push(pkt) && priority) { |
| 130 | _dshot_command_queue.push_force(pkt); |
| 131 | } |
| 132 | } |
| 133 | |
| 134 | // Set the dshot outputs that should be reversed (as opposed to 3D) |
| 135 | // The chanmask passed is added (ORed) into any existing mask. |
no test coverage detected