diff --git a/flix/mavlink.ino b/flix/mavlink.ino index b3f508a..69e32fb 100644 --- a/flix/mavlink.ino +++ b/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; } diff --git a/gazebo/Arduino.h b/gazebo/Arduino.h index 3267981..505e99a 100644 --- a/gazebo/Arduino.h +++ b/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 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; } diff --git a/tools/pyflix/README.md b/tools/pyflix/README.md index 963adad..110ad74 100644 --- a/tools/pyflix/README.md +++ b/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: diff --git a/tools/pyflix/flix.py b/tools/pyflix/flix.py index 6241ab3..a160135 100644 --- a/tools/pyflix/flix.py +++ b/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': diff --git a/tools/test.py b/tools/test.py index 582d92d..20cce4a 100755 --- a/tools/test.py +++ b/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)