src/lib/ServoOutput/devServoOutput.cpp

   1 #if defined(GPIO_PIN_PWM_OUTPUTS)
   2
   3 #include "devServoOutput.h"
   4 #include "PWM.h"
   5 #include "CRSF.h"
   6 #include "config.h"
   7 #include "logging.h"
   8 #include "rxtx_intf.h"
   9
  10 static int8_t servoPins[PWM_MAX_CHANNELS];
  11 static pwm_channel_t pwmChannels[PWM_MAX_CHANNELS];
  12 static uint16_t pwmChannelValues[PWM_MAX_CHANNELS];
  13
  14 #if (defined(PLATFORM_ESP32))
  15 static DShotRMT *dshotInstances[PWM_MAX_CHANNELS] = {nullptr};
  16 const uint8_t RMT_MAX_CHANNELS = 8;
  17 #endif
  18
  19 // true when the RX has a new channels packet
  20 static bool newChannelsAvailable;
  21 // Absolute max failsafe time if no update is received, regardless of LQ
  22 static constexpr uint32_t FAILSAFE_ABS_TIMEOUT_MS = 1000U;
  23
  24 void ICACHE_RAM_ATTR servoNewChannelsAvailable()
  25 {
  26     newChannelsAvailable = true;
  27 }
  28
  29 uint16_t servoOutputModeToFrequency(eServoOutputMode mode)
  30 {
  31     switch (mode)
  32     {
  33     case som50Hz:
  34         return 50U;
  35     case som60Hz:
  36         return 60U;
  37     case som100Hz:
  38         return 100U;
  39     case som160Hz:
  40         return 160U;
  41     case som333Hz:
  42         return 333U;
  43     case som400Hz:
  44         return 400U;
  45     case som10KHzDuty:
  46         return 10000U;
  47     default:
  48         return 0;
  49     }
  50 }
  51
  52 static void servoWrite(uint8_t ch, uint16_t us)
  53 {
  54     const rx_config_pwm_t *chConfig = config.GetPwmChannel(ch);
  55 #if defined(PLATFORM_ESP32)
  56     if ((eServoOutputMode)chConfig->val.mode == somDShot)
  57     {
  58         // DBGLN("Writing DShot output: us: %u, ch: %d", us, ch);
  59         if (dshotInstances[ch])
  60         {
  61             dshotInstances[ch]->send_dshot_value(((us - 1000) * 2) + 47); // Convert PWM signal in us to DShot value
  62         }
  63     }
  64     else
  65 #endif
  66     if (servoPins[ch] != UNDEF_PIN && pwmChannelValues[ch] != us)
  67     {
  68         pwmChannelValues[ch] = us;
  69         if ((eServoOutputMode)chConfig->val.mode == somOnOff)
  70         {
  71             digitalWrite(servoPins[ch], us > 1500);
  72         }
  73         else if ((eServoOutputMode)chConfig->val.mode == som10KHzDuty)
  74         {
  75             PWM.setDuty(pwmChannels[ch], constrain(us, 1000, 2000) - 1000);
  76         }
  77         else
  78         {
  79             PWM.setMicroseconds(pwmChannels[ch], us / (chConfig->val.narrow + 1));
  80         }
  81     }
  82 }
  83
  84 static void servosFailsafe()
  85 {
  86     constexpr unsigned SERVO_FAILSAFE_MIN = 988U;
  87     for (int ch = 0 ; ch < GPIO_PIN_PWM_OUTPUTS_COUNT ; ++ch)
  88     {
  89         const rx_config_pwm_t *chConfig = config.GetPwmChannel(ch);
  90         if (chConfig->val.failsafeMode == PWMFAILSAFE_SET_POSITION) {
  91             // Note: Failsafe values do not respect the inverted flag, failsafe values are absolute
  92             uint16_t us = chConfig->val.failsafe + SERVO_FAILSAFE_MIN;
  93             // Always write the failsafe position even if the servo has never been started,
  94             // so all the servos go to their expected position
  95             servoWrite(ch, us);
  96         }
  97         else if (chConfig->val.failsafeMode == PWMFAILSAFE_NO_PULSES) {
  98             servoWrite(ch, 0);
  99         }
 100         else if (chConfig->val.failsafeMode == PWMFAILSAFE_LAST_POSITION) {
 101             // do nothing
 102         }
 103     }
 104 }
 105
 106 static void servosUpdate(unsigned long now)
 107 {
 108     static uint32_t lastUpdate;
 109     if (newChannelsAvailable)
 110     {
 111         newChannelsAvailable = false;
 112         lastUpdate = now;
 113         for (int ch = 0 ; ch < GPIO_PIN_PWM_OUTPUTS_COUNT ; ++ch)
 114         {
 115             const rx_config_pwm_t *chConfig = config.GetPwmChannel(ch);
 116             const unsigned crsfVal = ChannelData[chConfig->val.inputChannel];
 117             // crsfVal might 0 if this is a switch channel, and it has not been
 118             // received yet. Delay initializing the servo until the channel is valid
 119             if (crsfVal == 0)
 120             {
 121                 continue;
 122             }
 123
 124             uint16_t us = CRSF_to_US(crsfVal);
 125             // Flip the output around the mid-value if inverted
 126             // (1500 - usOutput) + 1500
 127             if (chConfig->val.inverted)
 128             {
 129                 us = 3000U - us;
 130             }
 131             servoWrite(ch, us);
 132         } /* for each servo */
 133     }     /* if newChannelsAvailable */
 134
 135     // LQ goes to 0 (100 packets missed in a row)
 136     // OR last update older than FAILSAFE_ABS_TIMEOUT_MS
 137     // go to failsafe
 138     else if (lastUpdate && ((getLq() == 0) || (now - lastUpdate > FAILSAFE_ABS_TIMEOUT_MS)))
 139     {
 140         servosFailsafe();
 141         lastUpdate = 0;
 142     }
 143 }
 144
 145 static void initialize()
 146 {
 147     if (!OPT_HAS_SERVO_OUTPUT)
 148     {
 149         return;
 150     }
 151
 152 #if defined(PLATFORM_ESP32)
 153     uint8_t rmtCH = 0;
 154 #endif
 155     for (int ch = 0; ch < GPIO_PIN_PWM_OUTPUTS_COUNT; ++ch)
 156     {
 157         pwmChannelValues[ch] = UINT16_MAX;
 158         pwmChannels[ch] = -1;
 159         int8_t pin = GPIO_PIN_PWM_OUTPUTS[ch];
 160 #if (defined(DEBUG_LOG) || defined(DEBUG_RCVR_LINKSTATS)) && (defined(PLATFORM_ESP8266) || defined(PLATFORM_ESP32))
 161         // Disconnect the debug UART pins if DEBUG_LOG
 162         if (pin == U0RXD_GPIO_NUM || pin == U0TXD_GPIO_NUM)
 163         {
 164             pin = UNDEF_PIN;
 165         }
 166 #endif
 167         // Mark servo pins that are being used for serial (or other purposes) as disconnected
 168         auto mode = (eServoOutputMode)config.GetPwmChannel(ch)->val.mode;
 169         if (mode >= somSerial)
 170         {
 171             pin = UNDEF_PIN;
 172         }
 173 #if defined(PLATFORM_ESP32)
 174         else if (mode == somDShot)
 175         {
 176             if (rmtCH < RMT_MAX_CHANNELS)
 177             {
 178                 auto gpio = (gpio_num_t)pin;
 179                 auto rmtChannel = (rmt_channel_t)rmtCH;
 180                 DBGLN("Initializing DShot: gpio: %u, ch: %d, rmtChannel: %u", gpio, ch, rmtChannel);
 181                 pinMode(pin, OUTPUT);
 182                 dshotInstances[ch] = new DShotRMT(gpio, rmtChannel); // Initialize the DShotRMT instance
 183                 rmtCH++;
 184             }
 185             pin = UNDEF_PIN;
 186         }
 187 #endif
 188         servoPins[ch] = pin;
 189         // Initialize all servos to low ASAP
 190         if (pin != UNDEF_PIN)
 191         {
 192             if (mode == somOnOff)
 193             {
 194                 DBGLN("Initializing digital output: ch: %d, pin: %d", ch, pin);
 195             }
 196             else
 197             {
 198                 DBGLN("Initializing PWM output: ch: %d, pin: %d", ch, pin);
 199             }
 200
 201             pinMode(pin, OUTPUT);
 202             digitalWrite(pin, LOW);
 203         }
 204     }
 205 }
 206
 207 static int start()
 208 {
 209     for (int ch = 0; ch < GPIO_PIN_PWM_OUTPUTS_COUNT; ++ch)
 210     {
 211         const rx_config_pwm_t *chConfig = config.GetPwmChannel(ch);
 212         auto frequency = servoOutputModeToFrequency((eServoOutputMode)chConfig->val.mode);
 213         if (frequency && servoPins[ch] != UNDEF_PIN)
 214         {
 215             pwmChannels[ch] = PWM.allocate(servoPins[ch], frequency);
 216         }
 217 #if defined(PLATFORM_ESP32)
 218         else if (((eServoOutputMode)chConfig->val.mode) == somDShot)
 219         {
 220             dshotInstances[ch]->begin(DSHOT300, false); // Set DShot protocol and bidirectional dshot bool
 221             dshotInstances[ch]->send_dshot_value(0);         // Set throttle low so the ESC can continue initialsation
 222         }
 223 #endif
 224     }
 225     return DURATION_NEVER;
 226 }
 227
 228 static int event()
 229 {
 230     if (!OPT_HAS_SERVO_OUTPUT || connectionState == disconnected)
 231     {
 232         // Disconnected should come after failsafe on the RX,
 233         // so it is safe to shut down when disconnected
 234         return DURATION_NEVER;
 235     }
 236     else if (connectionState == wifiUpdate)
 237     {
 238         for (int ch = 0; ch < GPIO_PIN_PWM_OUTPUTS_COUNT; ++ch)
 239         {
 240             if (pwmChannels[ch] != -1)
 241             {
 242                 PWM.release(pwmChannels[ch]);
 243                 pwmChannels[ch] = -1;
 244             }
 245 #if defined(PLATFORM_ESP32)
 246             if (dshotInstances[ch] != nullptr)
 247             {
 248                 delete dshotInstances[ch];
 249                 dshotInstances[ch] = nullptr;
 250             }
 251 #endif
 252             servoPins[ch] = UNDEF_PIN;
 253         }
 254         return DURATION_NEVER;
 255     }
 256     return DURATION_IMMEDIATELY;
 257 }
 258
 259 static int timeout()
 260 {
 261     servosUpdate(millis());
 262     return DURATION_IMMEDIATELY;
 263 }
 264
 265 device_t ServoOut_device = {
 266     .initialize = initialize,
 267     .start = start,
 268     .event = event,
 269     .timeout = timeout,
 270 };
 271
 272 #endif