Add feature for setting arbitrary pin to arbitrary pwm over mavlink

Add flix.set_pin method for pyflix.

flix/mavlink.ino

@@ -8,6 +8,7 @@
 
 extern float controlTime;
 extern float voltage;
+extern int voltagePin, rcRxPin;
 
 int mavlinkSysId = 1;
 
@@ -328,6 +329,24 @@ int handleMavlinkCommand(const void *_m) {
 		return MAV_RESULT_ACCEPTED;
 	}
 
+	if (m.command == MAV_CMD_DO_SET_SERVO) {
+		int pin = m.param1;
+		int pwm = m.param2;
+		int frequency = m.param3 > 0 ? m.param3 : 50; // non-standard
+		int resolution = m.param4 > 0 ? m.param4 : 10; // non-standard
+
+		// Basic safety checks to avoid short circuits
+		if (!digitalPinCanOutput(pin)) return MAV_RESULT_DENIED;
+		if (pin == imuIntPin || pin == voltagePin || pin == rcRxPin) return MAV_RESULT_DENIED;
+		if (imuBus == 0 && (pin == imuSckPin || pin == imuMisoPin || pin == imuMosiPin || pin == imuCsPin)) return MAV_RESULT_DENIED;
+		if (imuBus == 1 && (pin == imuSdaPin || pin == imuSclPin)) return MAV_RESULT_DENIED;
+
+		ledcDetach(pin);
+		ledcAttach(pin, frequency, resolution);
+		ledcChangeFrequency(pin, frequency, resolution);
+		return ledcWrite(pin, pwm * frequency * (1 << resolution) / 1000000) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
+	}
+
 	return MAV_RESULT_UNSUPPORTED;
 }
 

gazebo/Arduino.h

@@ -23,6 +23,8 @@
 #define MALLOC_CAP_SPIRAM (1<<10)
 #define MALLOC_CAP_8BIT (1<<2)
 #define ESP_NOW_MAX_DATA_LEN_V2 1470
+#define digitalPinIsValid(pin) true
+#define digitalPinCanOutput(pin) true
 
 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
 template<typename T> T max(T a, T b) { return a > b ? a : b; }
@@ -192,6 +194,7 @@ void *heap_caps_calloc(size_t n, size_t size, uint32_t caps) {
 }
 
 bool ledcAttach(uint8_t pin, uint32_t freq, uint8_t resolution) { return true; }
+bool ledcDetach(uint8_t pin) { return true; }
 bool ledcWrite(uint8_t pin, uint32_t duty) { return true; }
 uint32_t ledcChangeFrequency(uint8_t pin, uint32_t freq, uint8_t resolution) { return freq; }
 int8_t digitalPinToAnalogChannel(uint8_t pin) { return -1; }

tools/pyflix/README.md

@@ -192,6 +192,21 @@ The following methods are in development and are not functional yet:
 
 To exit *AUTO* mode move control sticks and the drone will switch to *STAB* mode.
 
+### Additional methods
+
+You can set arbitrary output to any ESP32 pin using `set_pin` method:
+
+```python
+flix.set_pin(5, 1)  # set pin 5 to HIGH
+flix.set_pin(6, 0)  # set pin 6 to LOW
+flix.set_pin(7, 0.5)  # set pin 7 to 50% duty cycle
+flix.set_pin(8, duty=0.6, frequency=50)  # set pin 7 to 60% duty cycle at 50 Hz
+flix.set_pin(9, pwm=500, frequency=100)  # set pin 9 to 500 us pulse width at 100 Hz
+```
+
+> [!WARNING]
+> The `set_pin` method allows to set output value for almost any pin, with very few safety checks. Some configurations may lead to short circuit or other damage. Use with caution!
+
 ## Usage alongside QGroundControl
 
 You can use the Flix library alongside the QGroundControl app, using proxy mode. To do that:

tools/pyflix/flix.py

@@ -372,6 +372,17 @@ class Flix:
             raise ValueError('throttle must be in range [0, 1]')
         self.mavlink.manual_control_send(self.system_id, int(pitch * 1000), int(roll * 1000), int(throttle * 1000), int(yaw * 1000), 0)  # type: ignore
 
+    def set_pin(self, pin: int, duty: Optional[float] = None, frequency: float = 50, pwm: Optional[int] = None):
+        if frequency <= 0:
+            raise ValueError('Frequency must be positive')
+        if pwm is None:
+            if duty is None or duty < 0 or duty > 1:
+                raise ValueError('Duty cycle must be in range [0, 1]')
+            pwm = int(duty * 1000000 / frequency)
+        if pwm < 0 or pwm > 1000000 / frequency:
+            raise ValueError(f'PWM must be in range [0, {1000000 / frequency}]')
+        self.send_command(mavlink.MAV_CMD_DO_SET_SERVO, (pin, pwm, frequency, 0, 0, 0, 0))
+
     def cli(self, cmd: str, wait_response: bool = True) -> str:
         cmd = cmd.strip()
         if cmd == 'reboot':

tools/test.py

@@ -34,6 +34,13 @@ def test():
     assert isfinite(flix.get_param('CTL_ATT_P_P'))
     flix.set_param('CTL_ATT_P_P', 10.0)
 
+    print('=== Check methods')
+    flix.set_pin(5, 1)  # all pin sets should just return successfully in the sim doing nothing
+    flix.set_pin(6, 0)
+    flix.set_pin(7, 0.5)
+    flix.set_pin(8, duty=0.6, frequency=50)
+    flix.set_pin(9, pwm=500, frequency=100)
+
     print('=== Additional checks')
     assert flix.wait('gyro') == approx((0, 0, 0), abs=0.01)
     flix.wait('armed', False)