diff --git a/docs/ESC and servo outputs.md b/docs/ESC and servo outputs.md index eba8187ca56..a3c4a39d199 100644 --- a/docs/ESC and servo outputs.md +++ b/docs/ESC and servo outputs.md @@ -33,3 +33,9 @@ INAV 7 introduced extra functionality that let you force only some outputs to be The main restrictions is that outputs are associated with timers, which can be shared between multiple outputs and two outputs on the same timer need to have the same function. The easiest way to modify outputs, is to use the Mixer tab in the Configurator, as it will clearly show you which timer is used by all outputs, but you can also use `timer_output_mode` on the cli. + +## ESCs that are not driven from an output pin + +A Spektrum Smart ESC is connected to a UART rather than to a motor output, because +its protocol is serial rather than a timer waveform. None of the output mapping +above applies to it. See [Spektrum Smart ESC](Spektrum%20Smart%20ESC.md). diff --git a/docs/Settings.md b/docs/Settings.md index 91c406e6199..723ab6c48fe 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1046,6 +1046,38 @@ Enable when BLHeli32 Auto Telemetry function is used. Disable in every other cas --- +### esc_srxl2_reverse_channel + +For an SRXL2 Smart ESC, the 1-based auxiliary channel its "Thrust Rev." setting selects to arm reverse. Spektrum allow channels 5 to 9 and ship channel 7 by default. Must match how the ESC was programmed, because nothing on the wire advertises it. 0 disables reverse, and anything between 1 and 4 is treated as 0 at boot - the range cannot express the hole, and a channel the ESC cannot watch would offer a mode that does nothing. + +| Default | Min | Max | +| --- | --- | --- | +| 7 | 0 | 9 | + +--- + +### esc_srxl2_telemetry + +Read ESC telemetry off the SRXL2 link. Only applies when motor_pwm_protocol is SRXL2, where telemetry shares the throttle wire and so cannot be turned off by leaving a port unassigned as it would be for a conventional ESC. + +| Default | Min | Max | +| --- | --- | --- | +| ON | OFF | ON | + +--- + +### esc_srxl2_telemetry_rate + +How often ESC telemetry arrives from an SRXL2 Smart ESC, in readings per second. The ESC answers about two requests in three and rotates its reply between three sensors, so it delivers roughly a ninth of what is asked for - these are the delivered rates, measured, not the request rate. The reply shares the throttle wire, so a faster rate leaves the bus less headroom; only the RPM filter benefits from it. The range is bounded at both ends by the ESC: asking on every frame makes an Avian keep the link and stop obeying the throttle, and asking slower than 1 Hz makes the link time out on a healthy ESC, because its reply is the only thing that proves it is still there. + +| Allowed Values | | +| --- | --- | +| 1HZ | Default | +| 3HZ | | +| 2HZ | | + +--- + ### ez_aggressiveness EzTune aggressiveness @@ -3520,6 +3552,7 @@ Protocol that is used to send motor updates to ESCs. Possible values - STANDARD, | DSHOT150 | | | DSHOT300 | | | DSHOT600 | | +| SRXL2 | | --- diff --git a/docs/Spektrum Smart ESC.md b/docs/Spektrum Smart ESC.md new file mode 100644 index 00000000000..02b7a4cfe76 --- /dev/null +++ b/docs/Spektrum Smart ESC.md @@ -0,0 +1,345 @@ +# Spektrum Smart ESC (SRXL2) + +Spektrum's "Smart Throttle" is the SRXL2 protocol carried on the ESC's throttle +signal wire: the ESC is an SRXL2 device and the receiver, or here the flight +controller, is the bus master. The same single wire carries the throttle one way +and telemetry the other. + +INAV can take that master role, which makes two things available that a PWM +connection cannot give you: + +* **Thrust reverse.** On an Avian this is only reachable over Smart Throttle. Wired + as a conventional PWM ESC there is no way to tell it to reverse at all. +* **Telemetry with no extra wire.** Voltage, current, rpm and temperatures arrive on + the throttle wire. There is no telemetry lead to run and no ESC telemetry pad to + connect. + +## Wiring + +The ESC's normal three-wire servo lead goes to a **UART**, not to a motor pad: + +| Wire | Goes to | +|---|---| +| signal | the **TX pin** of a port assigned `Spektrum Smart ESC (SRXL2)` | +| +5 V (BEC) | as usual | +| ground | as usual | + +The signal goes to TX rather than RX because the protocol is half duplex: one +conductor carries both directions, and the UART is put into single-wire mode. + +Nothing connects to the flight controller's ESC telemetry pad. That pad is for ESCs +with a separate telemetry lead, such as BLHeli or HobbyWing; a Smart ESC has no such +lead. + +The motor pad that would normally have driven this ESC is simply left unused. + +## Setting it up + +1. **Ports tab**: assign `Spektrum Smart ESC (SRXL2)` to a spare UART, one per + motor. They are matched to motors in port order; see below. +2. **Outputs tab**: set the ESC protocol to `SRXL2`. +3. **Outputs tab**: set **Motor poles** correctly. This matters more than usual; see + the RPM filter section below. +4. If the ESC is programmed for reverse, set **Thrust Reverse: ESC channel** to the + channel its own `Thrust Rev.` parameter selects, then assign the **THRUST + REVERSE** mode to a switch in the Modes tab. See below. + +Both steps 1 and 2 are needed. This protocol has **no fallback to PWM**: the pin is +a UART pin, not a timer output, so a motor protocol of `SRXL2` with no port assigned +means the motor is never driven. The Outputs tab warns when that is the case. + +If your firmware was not built with this support the protocol does not appear in the +list at all, and neither does the port function. It is enabled by default on H7 and +AT32 targets; other targets can add `#define USE_MOTOR_SRXL2` to their `target.h`. + +## One ESC per port, several ports + +A single SRXL2 bus can address several ESCs, at device IDs 0x40 to 0x4F, but each +would need a distinct unit ID and the specification states that setting a unit ID +over SRXL2 "is not implemented", because it expects physical switches or jumpers, which +Avian ESCs do not have. So it is one ESC per bus, and a model with several motors +needs a port for each. + +Up to four are supported. **Motors are matched to ports in order:** motor 1 is the +lowest-numbered assigned UART, motor 2 the next, and so on. Nothing on the wire +says which motor an ESC drives, so the wiring order is what carries that. + +If there are fewer ports than the mixer has motors, the board **refuses to arm** +and reports `Not enough motor outputs/timers`. A motor with no port has nowhere to +send its command and no timer output to fall back on, so a twin that can only +drive one side must not be allowed into the air. The Outputs tab says so before it +gets that far. + +Each port is an independent bus: its own handshake, its own baud negotiation, its +own telemetry. A twin with one ESC unplugged therefore reports the link as down +rather than partly up. + +Note that SRXL2 sends at tens of hertz by design, where DSHOT sends at kilohertz. +That is ample for an aircraft holding a cruise throttle and nowhere near enough +for a multirotor, whatever the wiring. + +## Throttle range calibration + +A Spektrum ESC learns its throttle endpoints from the signal present as it powers +up: full throttle first, then low within five seconds of the tones that acknowledge +it. That normally needs a Spektrum transmitter, so INAV can drive the sequence +itself for anyone who does not own one. + +**Do this before the first flight.** It is not a convenience. Measured on an Avian +70 A, sweeping the channel value across its whole range and reading back the +throttle the ESC reports: + +| | uncalibrated | after calibration | +|---|---|---| +| starts responding at | 12220 (1178 us) | 2687 (1029 us) | +| saturates at | 50820 (1781 us) | 64307 (1993 us) | +| share of the channel used | 59 % | 94 % | + +Uncalibrated, the bottom sixth of the throttle does nothing and the top fifth is +already at full power, so the stick reaches everything it will ever reach at +about three quarters travel - and nothing says so. Calibrated, what the ESC +reports tracks what INAV commands to within a point across the whole range: +1050 us gives 5 %, 1500 gives 50 %, 2000 gives 100 %. A pilot who skips this +finds out about it on the takeoff roll. + +**Outputs tab**, Throttle range calibration: + +1. Remove the propeller and disconnect the battery. The wizard refuses to start with + the battery connected, and the button stays inert until you confirm both. +2. Press **Start calibration**. Full throttle goes on the wire, and with no battery + nothing can spin. +3. Connect the battery. The ESC sounds its tones, and the throttle drops to minimum + by itself about three seconds later. +4. A long tone means the range was stored. + +From the CLI the same thing is `esc_calibrate start`, with `esc_calibrate high` and +`low` available for boards that cannot sense battery voltage and therefore cannot +detect the ESC powering up. + +Either phase ends on its own if left alone, and arming cancels a sequence in +progress. + +With more than one ESC all of them are calibrated together. They share a battery, +so they power up together and the window the sequence aims at is the same window +for all of them. + +## Telemetry and the RPM filter + +Telemetry is read from the SRXL2 link and feeds everything that consumes ESC +telemetry: OSD, Blackbox, current estimation, and the gyro RPM filter. Each port's +ESC reports as its own motor, so motor 2's telemetry is motor 2's. It can be +switched off with `esc_srxl2_telemetry`, which exists because telemetry shares the +throttle wire and so cannot be declined by leaving a port unassigned as it would be +for a conventional ESC. + +**The current this ESC reports is the motor's, not the pack's.** Measured on the +bench with both readings taken at the same instant: + +| throttle | ESC reports | supply delivers | ratio | 1/duty | +| --- | --- | --- | --- | --- | +| 15 % | 0.00 A | 0.305 A | | 6.67 | +| 25 % | 2.62 A | 0.720 A | 3.64 | 4.00 | +| 35 % | 2.75 A | 0.915 A | 3.00 | 2.86 | + +The ratio follows 1/duty rather than staying put, which is the converter +relationship: an ESC trades voltage for current, so pack current is roughly motor +current times duty. A scale error in the sensor would give the same ratio at both +points. It shows without the arithmetic too, since what the ESC reports barely +moves while pack current rises by a quarter. + +Two consequences. The figure runs high as a measure of what the battery is +delivering, worst at low throttle and converging at full, so a capacity count fed +from it counts down too fast. And below roughly 2 A the field reports 0.00 A +outright: at 15 % throttle the motor was turning at 6900 rpm on 0.305 A from the +supply and the ESC still called it zero. + +The driver passes the number through unscaled. Two operating points are not a +curve, the multiplier that would be correct is the ESC's own modulation duty +rather than the throttle it reports, and this is one ESC family rather than the +protocol. A per-manufacturer scale belongs beside INAV's other current meters, +not inside a driver. + +So prefer the board's own sensor with `current_meter_type = ADC` wherever one +exists. The two are alternatives +rather than additive - INAV takes current from one source - and a shunt in the +battery lead also sees what the servos and the video transmitter draw, which no +ESC can report. + +**Set Motor poles correctly before enabling the RPM filter.** The wire carries +electrical rpm, and INAV converts it to mechanical rpm using the pole count. A wrong +pole count puts the notch at the wrong frequency, which is worse than having no +notch at all. + +**The RPM filter is not recommended over this link.** The rpm it delivers is too +slow for the filter to track a changing throttle, and that is a hardware limit +rather than a tuning one. Measured on an Avian 70 A: the ESC answers about two +requests in three and rotates its reply between a text page, a battery page and +the ESC page, so rpm arrives at roughly a ninth of the request rate - 2.7 readings a second at the fastest setting the link tolerates, +1.1 at the default. There is no margin to recover, either: requesting on every +frame makes the ESC stop obeying the throttle, and this ESC advertises no support +for 400000 baud, so the wire cannot be made faster. + +For comparison, bidirectional DSHOT reports rpm every loop. Two to three orders +of magnitude separate the two, so: + +* **On a multirotor, leave `rpm_gyro_filter_enabled` off.** The vibration peak + moves with the throttle several times a second, and a notch updated twice a + second spends most of its time in the wrong place - which is worse than no + notch, because it attenuates signal rather than noise. +* **On a fixed wing holding a cruise throttle** the peak moves slowly enough that + the update rate might be adequate. Might: that is reasoning from how slowly + cruise rpm changes, and nobody has flown it. Until somebody does and compares a + logged flight with the filter on and off, treat it as unproven rather than as a + milder version of the same recommendation. +* INAV's own advice for this setting applies unchanged: turn it on only once ESC + telemetry is working and the reported rpm looks right. + +## Reverse + +Reverse on a Smart ESC is a switch, not a throttle value below neutral. The ESC's +`Thrust Rev.` parameter names an auxiliary channel; when that channel goes high the +ESC reverses, and Spektrum describe the effect plainly, *"flipping the designated +switch reverses motor rotation, throttle will still control motor speed"*. So the +throttle goes on meaning throttle. + +Three things have to agree: + +* the ESC, with **both** of its own parameters set: `Brake Type` to `Reverse`, + which is what enables reversing at all, and `Thrust Rev.`, which only chooses + the channel that arms it. Spektrum's programming instructions are explicit that + one does not work without the other - *"Thrust Rev - Use this option to select + the channel used to activate motor reversing. Reverse must set in the Brake Type + menu"*. They also recommend `Brake Force` of 7 alongside `Brake Type = Reverse`; +* `esc_srxl2_reverse_channel`, set to that same channel; +* a switch, assigned to the **THRUST REVERSE** mode in the Modes tab. + +Pick a channel nothing else uses. This is Spektrum's own warning about the +parameter, and it is about flight behaviour rather than tidiness: *"Reverse mode +needs to be assigned to an OPEN channel on your transmitter, this channel is +selected in ESC menu item #15 using in conjunction with another function can cause +unexpected behavior in flight."* On this link the channel is a slot between the +flight controller and the ESC rather than a transmitter channel, so the flight +controller is what has to leave it alone - which is why the setting refuses the +throttle slot. + +Spektrum allow channels **5 to 9** for this and ship **channel 7** as the factory +default. Nothing on the wire advertises which one the ESC is watching, so a +mismatch simply means reverse never engages, and does so silently. Check the channel against +your own ESC's programming rather than trusting the default: the parameter is not +present on every Avian model, and where it is present the range and default have +varied. + +The channel setting is not a transmitter channel. It selects a slot on the SRXL2 +wire between the flight controller and the ESC. The transmitter switch is chosen in +the Modes tab like any other mode. + +`esc_srxl2_reverse_channel` applies to every ESC on the model, so a twin needs both +ESCs programmed with the same `Thrust Rev.` channel. Reverse is armed on all of +them together and never on one alone: asymmetric reverse thrust on a twin is the +outcome most worth engineering against. + +Reverse is released whenever the aircraft is disarmed, so a machine that landed +under reverse does not sit on the ground with it still armed. + +### Check it on the bench before the first flight + +Some Spektrum documentation shows a bipolar throttle scale for the reverse brake +mode, where centre is zero thrust and below centre is reverse. That contradicts the +switch-and-normal-throttle behaviour above, and the two cannot both be true of the +same ESC. INAV drives the switch arrangement, which is the one Spektrum state in +words, so it is worth confirming your ESC is that kind. + +An Avian 70 A with `Brake Type = Reverse` and the factory `Thrust Rev.` channel +was confirmed to be that kind: with channel 7 low and then high, against an +unchanged 1250 us throttle, the motor ran counter-clockwise, clockwise, +counter-clockwise, clockwise, reporting the same 14.0 % and about 6610 rpm in +every case. The throttle never changed meaning, and telemetry says nothing about +which way the shaft is turning - only an eye on the motor can tell you. + +**Propeller off.** Arm with THRUST REVERSE off and watch the motor at *minimum* +throttle: + +* **stopped**, or idling gently forward, the switch arrangement, which is what + this driver expects. Nothing more to do. +* **spinning backwards hard**, the other kind. Disarm. INAV does not drive that + arrangement over SRXL2: the throttle it sends would be read as reverse thrust + through most of the stick. + +`FEATURE_REVERSIBLE_MOTORS` is not the answer to the second case and is cleared +automatically when the protocol is SRXL2. It recentres INAV's own throttle output, +which on a switch-type ESC means roughly half throttle at the point the stick says +stop. + +### Engaging it with the motor running + +Nothing stops the switch being thrown at speed, on the ESC's side or INAV's: +`srxl2MotorSetReverse()` writes its channel whenever the mode is active, without +consulting the throttle. Tried deliberately on the bench, with the motor turning +at 2690 rpm under an unchanged 1200 us command, the Avian simply changed +direction - one telemetry sample caught it passing through zero rpm, and the next +had it back at the same speed the other way. No stall, no cutout, and no current +step large enough to read at that load. + +That is the ESC behaving well, not a licence to do it. With a propeller loaded in +flight the same reversal has to absorb the airflow driving the blades, which is +a different question from a bare motor on a bench, and it is the reason Spektrum +put reverse on a switch the pilot has to mean to throw. + +### What reverse cannot do + +Reverse is a manual, stick-and-switch capability. INAV's automatic throttle paths, +RTH, autoland, failsafe and launch, all clamp throttle to at least idle, so none of +them can call for reverse thrust. An automatic landing will not use it. + +## If the motor does not come back after a reboot + +An Avian announces itself for about a third of a second after it powers up - six +handshakes in 300 milliseconds, measured on a 70 A - and then never speaks again +unless it is asked something it recognises. A flight controller that starts while +the ESC is already running has missed that window, and the ESC will not answer it +afterwards: polled by name, broadcast to, addressed on every ID from 0x40 to +0x4F, or spoken to as though the link already existed, it stayed silent through +every one. + +So the link is made at power-up or not at all. Connecting the battery powers both +together and the announcement lands while the flight controller is listening, +which is the normal case and needs nothing. The cases that bite are the other +ones: + +* the flight controller reboots - a firmware update, a brownout, the Configurator + asking for a restart - while the battery stays connected. **Unplug the battery + and plug it in again**, or the motor will not respond. +* bench work on USB with the ESC powered from a separate supply. Power the ESC + after the board has booted, not before. + +Nothing in the firmware can work around this, so it refuses to hide it instead: +arming is blocked while an SRXL2 link is missing, and the OSD says the hardware +is not there. + +What the firmware does not do is give up on an ESC that has gone quiet while it +is being flown. Telemetry has nothing to do with the throttle on these ESCs: +measured with a bench supply as the witness, an Avian held 0.30 A through ten +seconds with no telemetry requested at all. What stops the motor is the absence +of control frames - the current falls to the ESC's own 58 mA within about half a +second - and it takes the throttle back up by itself when frames return, with no +re-arm and no power cycle. So a silent ESC keeps being commanded, and only the +telemetry goes stale. Where ESC and board come up together the block clears in about a +second and is never seen; where it does not clear, the throttle would have done +nothing anyway. + +## Settings + +| Setting | Meaning | +|---|---| +| `motor_pwm_protocol = SRXL2` | drive motors over SRXL2 | +| `esc_srxl2_reverse_channel` | SRXL2 channel the ESC watches for reverse, 5 to 9; Spektrum default 7, 0 disables | +| `esc_srxl2_telemetry` | read telemetry from the SRXL2 link | +| `esc_srxl2_telemetry_rate` | how often telemetry arrives: 3, 2 or 1 Hz | +| `motor_poles` | required for correct rpm, see above | + +## Reference + +Protocol: [Specification for Spektrum SRXL2](https://github.com/SpektrumRC/SRXL2), +published by Horizon Hobby under the MIT licence. The library in that repository +implements the device side; the bus master side is not part of the open release, so +the master in INAV is written from the specification. diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 752d44e2449..054af703408 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -389,6 +389,8 @@ main_sources(COMMON_SRC io/adsb.h io/beeper.c io/beeper.h + io/motor_srxl2.c + io/motor_srxl2.h io/servo_sbus.c io/servo_sbus.h io/frsky_osd.c diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 61302832983..06cdf5ea279 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -42,6 +42,7 @@ #include "sensors/rangefinder.h" #include "io/serial.h" +#include "io/motor_srxl2.h" #include "io/servo_sbus.h" enum { @@ -70,6 +71,11 @@ static const motorProtocolProperties_t motorProtocolProperties[] = { [PWM_TYPE_DSHOT150] = { .usesHwTimer = true, .isDSHOT = true }, [PWM_TYPE_DSHOT300] = { .usesHwTimer = true, .isDSHOT = true }, [PWM_TYPE_DSHOT600] = { .usesHwTimer = true, .isDSHOT = true }, + /* Not a timer waveform at all: the ESC hangs off a UART. The entry has to be + * here even on a target built without the protocol, because the value is + * stored in configuration and a diff restored from a board that does have it + * would otherwise index past the end of this array. */ + [PWM_TYPE_SRXL2] = { .usesHwTimer = false, .isDSHOT = false }, }; pwmInitError_e getPwmInitError(void) @@ -429,6 +435,20 @@ static void pwmInitMotors(timMotorServoHardware_t * timOutputs) // Do the pre-configuration. For motors w/o hardware timers this should be sufficient pwmMotorPreconfigure(); +#ifdef USE_MOTOR_SRXL2 + /* + * SRXL2 carries one ESC per port, so the mixer's motor count has to be met by + * that many assigned ports. A motor with no port has nowhere to send its + * command and no timer output to fall back on, and dropping it silently would + * mean arming a twin that can only drive one side. + */ + if (motorConfig()->motorPwmProtocol == PWM_TYPE_SRXL2 && srxl2MotorCount() < motorCount) { + pwmInitError = PWM_INIT_ERROR_NOT_ENOUGH_MOTOR_OUTPUTS; + LOG_ERROR(PWM, "Not enough SRXL2 ports. Mixer requested %d, ports %d", motorCount, srxl2MotorCount()); + return; + } +#endif + // Now if we need to configure individual motor outputs - do that if (!motorsUseHardwareTimers()) { LOG_INFO(PWM, "Skipped timer init for motors"); diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index afe9301f7e7..158ab8ce6b5 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -47,6 +47,10 @@ typedef enum { PWM_TYPE_DSHOT150, PWM_TYPE_DSHOT300, PWM_TYPE_DSHOT600, + /* Appended, not inserted: the value is stored in configuration. Unlike + * everything above it this is a UART protocol rather than a timer waveform, + * so the ESC signal goes to a serial pin and not to a motor pad. */ + PWM_TYPE_SRXL2, } motorPwmProtocolTypes_e; typedef enum { diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index e6fcfaeeaf3..891d59c23e7 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -34,6 +34,7 @@ #include "drivers/timer.h" #include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" +#include "io/motor_srxl2.h" #include "io/servo_sbus.h" #include "sensors/esc_sensor.h" @@ -616,6 +617,20 @@ void pwmMotorPreconfigure(void) motorWritePtr = pwmWriteDigital; break; #endif + +#ifdef USE_MOTOR_SRXL2 + case PWM_TYPE_SRXL2: + /* Nothing to fall back on if this fails: the pin is a UART pin, not a + * timer output, so there is no PWM to degrade to. Leaving + * motorWritePtr null keeps the motor unwritten, which is the honest + * outcome of a port that was never assigned. */ + if (srxl2MotorInitialize()) { + srxl2MotorSetReverseChannel(motorConfig()->srxl2ReverseChannel); + srxl2MotorSetTelemetryRate(motorConfig()->srxl2TelemetryRate); + motorWritePtr = srxl2MotorUpdate; + } + break; +#endif } } diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index f2e446077c0..d81c5d15d63 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -95,6 +95,7 @@ bool cliMode = false; #include "io/ledstrip.h" #include "io/osd.h" #include "io/osd/custom_elements.h" +#include "io/motor_srxl2.h" #include "io/serial.h" #include "fc/fc_msp_box.h" @@ -4743,6 +4744,88 @@ static void cliDiff(char *cmdline) printConfig(cmdline, true); } +#ifdef USE_MOTOR_SRXL2 +static void cliEscCalibratePrintResult(srxl2CalResult_e r) +{ + switch (r) { + case SRXL2_CAL_ACCEPTED: + break; + case SRXL2_CAL_REJECT_ARMED: + cliPrintErrorLinef("Not while armed"); + break; + case SRXL2_CAL_REJECT_NO_PORT: + cliPrintErrorLinef("No SRXL2 ESC port. Assign one and set motor_pwm_protocol = SRXL2"); + break; + case SRXL2_CAL_REJECT_BATTERY_PRESENT: + cliPrintErrorLinef("Disconnect the battery first. The ESC only reads its"); + cliPrintErrorLinef("endpoints as it powers up, and full throttle must not be"); + cliPrintErrorLinef("presented to an ESC that can already act on it."); + break; + case SRXL2_CAL_REJECT_NO_VOLTAGE_SENSOR: + cliPrintErrorLinef("No battery voltage sensing, so the ESC powering up cannot be"); + cliPrintErrorLinef("detected. Use 'esc_calibrate high' and 'low' by hand instead."); + break; + } +} + +static void cliEscCalibrate(char *cmdline) +{ + static const char * const phaseName[] = { + "off", "waiting for battery", "holding high", "holding low", + "holding high (manual)", "holding low (manual)" + }; + + if (isEmpty(cmdline)) { + cliPrintLinef("Phase: %s", phaseName[srxl2MotorCalibrationPhase()]); + cliPrintLine(""); + cliPrintLine("Teaches a Spektrum Smart ESC its throttle endpoints. The ESC reads"); + cliPrintLine("them from the signal present as it powers up, so the sequence is"); + cliPrintLine("timed from the moment the battery goes in."); + cliPrintLine(""); + cliPrintLine("REMOVE THE PROPELLER. This commands full throttle."); + cliPrintLine(""); + cliPrintLine(" esc_calibrate start battery DISCONNECTED, then plug it in"); + cliPrintLine(" when told. The rest is automatic."); + cliPrintLine(" esc_calibrate off abort"); + cliPrintLine(""); + cliPrintLine("By hand, for boards without battery voltage sensing:"); + cliPrintLine(" esc_calibrate high then connect the battery"); + cliPrintLine(" esc_calibrate low within five seconds of the two short tones"); + cliPrintLine(""); + cliPrintLine("Every phase ends by itself. Arming cancels it."); + return; + } + + if (sl_strcasecmp(cmdline, "start") == 0) { + const srxl2CalResult_e r = srxl2MotorCalibrationBegin(); + cliEscCalibratePrintResult(r); + if (r == SRXL2_CAL_ACCEPTED) { + cliPrintLine("Propeller off? Full throttle is now on the wire."); + cliPrintLine("Connect the battery. The ESC will sound its tones, and the"); + cliPrintLine("throttle drops to minimum on its own about three seconds later."); + cliPrintLine("A long tone means the range was stored."); + } + } else if (sl_strcasecmp(cmdline, "high") == 0) { + const srxl2CalResult_e r = srxl2MotorCalibrationManual(SRXL2_CAL_HIGH_MANUAL); + cliEscCalibratePrintResult(r); + if (r == SRXL2_CAL_ACCEPTED) { + cliPrintLine("Full throttle on the wire. Connect the battery now."); + } + } else if (sl_strcasecmp(cmdline, "low") == 0) { + const srxl2CalResult_e r = srxl2MotorCalibrationManual(SRXL2_CAL_LOW_MANUAL); + cliEscCalibratePrintResult(r); + if (r == SRXL2_CAL_ACCEPTED) { + cliPrintLine("Low throttle on the wire. Listen for the cell count, then a long tone."); + } + } else if (sl_strcasecmp(cmdline, "off") == 0) { + srxl2MotorCalibrationAbort(); + cliPrintLine("Aborted."); + } else { + cliShowParseError(); + } +} +#endif + #ifdef USE_USB_MSC static void cliMsc(char *cmdline) { @@ -5025,6 +5108,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("memory", "view memory usage", NULL, cliMemory), CLI_COMMAND_DEF("mmix", "custom motor mixer", NULL, cliMotorMix), CLI_COMMAND_DEF("motor", "get/set motor", " []", cliMotor), +#ifdef USE_MOTOR_SRXL2 + CLI_COMMAND_DEF("esc_calibrate", "teach a Spektrum Smart ESC its throttle range", "[start|high|low|off]", cliEscCalibrate), +#endif #ifdef USE_USB_MSC CLI_COMMAND_DEF("msc", "switch into msc mode", NULL, cliMsc), #endif diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 5e1c87ac145..f8b5bdf25f0 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -266,11 +266,40 @@ void validateAndFixConfig(void) // Limitations of different protocols #if !defined(USE_DSHOT) - if (motorConfig()->motorPwmProtocol > PWM_TYPE_BRUSHED) { + // Named explicitly rather than tested as "above BRUSHED". This is a DSHOT + // check, and the enum has since grown a UART protocol above DSHOT600 that a + // build without DSHOT can still drive perfectly well - a range test would + // quietly rewrite it to MULTISHOT on every boot. + if (motorConfig()->motorPwmProtocol >= PWM_TYPE_DSHOT150 && + motorConfig()->motorPwmProtocol <= PWM_TYPE_DSHOT600) { motorConfigMutable()->motorPwmProtocol = PWM_TYPE_MULTISHOT; } #endif +#if !defined(USE_MOTOR_SRXL2) + // A configuration restored onto a build without the driver would keep SRXL2 + // selected, and nothing would drive the motors: the branch that installs the + // SRXL2 writer is compiled out, so the writer stays null, while the protocol + // is not timer-based and so escapes the "not enough outputs" check too. The + // result is a model that arms and does nothing, which is the one outcome + // worth spending a boot-time rewrite on. + if (motorConfig()->motorPwmProtocol == PWM_TYPE_SRXL2) { + motorConfigMutable()->motorPwmProtocol = PWM_TYPE_STANDARD; + } +#else + // Reverse is either off or one of the channels a Smart ESC can be programmed + // to watch, which Spektrum document as 5 to 9. The setting's range cannot + // express "zero, or five to nine", so anything between is normalised here: + // channel 1 aliases the throttle and the driver refuses it outright, and 2 + // to 4 would be sent faithfully to an ESC with no way to act on them. Both + // used to leave a reverse mode the Configurator offered and nothing + // performed. Corrected to off, where it can be seen. + const uint8_t reverseChannel = motorConfig()->srxl2ReverseChannel; + if (reverseChannel != 0 && (reverseChannel < 5 || reverseChannel > 9)) { + motorConfigMutable()->srxl2ReverseChannel = 0; + } +#endif + // Call target-specific validation function validateAndFixTargetConfig(); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 1990dc070a1..7e4060da4a6 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -37,6 +37,7 @@ #include "drivers/time.h" #include "drivers/system.h" #include "drivers/pwm_output.h" +#include "drivers/pwm_mapping.h" #include "sensors/sensors.h" #include "sensors/diagnostics.h" @@ -88,6 +89,8 @@ #include "flight/mixer_profile.h" #include "flight/mixer.h" + +#include "io/motor_srxl2.h" #include "flight/servos.h" #include "flight/pid.h" #include "flight/imu.h" @@ -301,7 +304,29 @@ static void updateArmingStatus(void) } /* CHECK: */ - if (!isHardwareHealthy()) { + bool escLinkMissing = false; +#ifdef USE_MOTOR_SRXL2 + /* + * An SRXL2 ESC announces itself in the third of a second after it gains + * power, and is silent from then on: if that announcement is missed the + * link never forms, and the throttle reaches nothing. Arming meanwhile + * commands a motor that is not listening - the model looks armed, the + * telemetry looks sane, and the propeller does not turn. + * + * Refuse to arm until the link is actually up. Where both come up on the + * same battery this costs about a second at power-up and is invisible; + * where it does not, it is the difference between finding out on the + * bench and finding out on the takeoff roll. + */ + escLinkMissing = (motorConfig()->motorPwmProtocol == PWM_TYPE_SRXL2) + && !srxl2MotorIsConnected(); +#ifdef USE_SIMULATOR + // Not while a simulator flies the aircraft: HITL disables the outputs itself, so + // there is no motor to command and nothing this would protect + escLinkMissing = escLinkMissing && !ARMING_FLAG(SIMULATOR_MODE_HITL); +#endif +#endif + if (!isHardwareHealthy() || escLinkMissing) { ENABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE); } else { diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 8b6b03bf42c..9ba5a8e442c 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -117,6 +117,7 @@ #include "io/osd.h" #include "io/osd_dji_hd.h" #include "io/rcdevice_cam.h" +#include "io/motor_srxl2.h" #include "io/serial.h" #include "io/displayport_msp.h" #include "io/smartport_master.h" @@ -340,6 +341,19 @@ void init(void) if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) { featureClear(FEATURE_REVERSIBLE_MOTORS); } +#ifdef USE_MOTOR_SRXL2 + /* + * A Spektrum Smart ESC reverses on a switch and goes on reading the throttle + * normally - Spektrum put it plainly: "flipping the designated switch reverses + * motor rotation, throttle will still control motor speed". Reversible motors + * means the other arrangement, where the stick centre is zero thrust, and + * enabling it here would hand the ESC roughly half throttle at the point the + * pilot expects the motor stopped. Reverse is the THRUST REVERSE mode instead. + */ + if (motorConfig()->motorPwmProtocol == PWM_TYPE_SRXL2) { + featureClear(FEATURE_REVERSIBLE_MOTORS); + } +#endif if (!STATE(ALTITUDE_CONTROL)) { featureClear(FEATURE_AIRMODE); } @@ -353,6 +367,21 @@ void init(void) } #else DISABLE_ARMING_FLAG(ARMING_DISABLED_PWM_OUTPUT_ERROR); +#ifdef USE_MOTOR_SRXL2 + /* + * SITL has no motor output layer - the simulator reads the mixer's motor[] + * array directly, so pwmMotorPreconfigure() never runs and nothing would open + * the SRXL2 ports. Open them here instead: SITL maps every UART onto a TCP + * port, so this is what lets a simulated ESC be attached to the real driver + * and the handshake, telemetry and calibration paths be exercised - and the + * Configurator show its ESC block - without any hardware. + */ + if (motorConfig()->motorPwmProtocol == PWM_TYPE_SRXL2) { + srxl2MotorInitialize(); + srxl2MotorSetReverseChannel(motorConfig()->srxl2ReverseChannel); + srxl2MotorSetTelemetryRate(motorConfig()->srxl2TelemetryRate); + } +#endif #endif systemState |= SYSTEM_STATE_MOTORS_READY; diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 19a2e33991f..637ba9e7fc8 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -98,6 +98,7 @@ #include "io/rangefinder.h" #include "io/ledstrip.h" #include "io/osd.h" +#include "io/motor_srxl2.h" #include "io/serial.h" #include "io/serial_4way.h" #include "io/vtx.h" @@ -1691,6 +1692,24 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF #endif break; +#ifdef USE_MOTOR_SRXL2 + case MSP2_INAV_ESC_SRXL2_STATUS: + sbufWriteU8(dst, srxl2MotorCalibrationPhase()); + sbufWriteU8(dst, srxl2MotorIsConnected() ? 1 : 0); + /* Why the last start was refused. MSP2_INAV_ESC_SRXL2_CALIBRATE is an IN + * command and so has nowhere to answer; without this a caller sees only + * that it failed, and can tell the operator nothing. */ + sbufWriteU8(dst, srxl2MotorCalibrationLastResult()); + /* Ports opened, and motors the mixer wants. These are the two numbers + * pwmInitMotors() compares to decide whether the board may arm, so + * reporting both means a caller never has to infer either. In particular + * MSP2_INAV_MIXER does NOT carry the model motor count - its last two + * bytes are MAX_SUPPORTED_MOTORS and MAX_SUPPORTED_SERVOS, the ceilings. */ + sbufWriteU8(dst, srxl2MotorCount()); + sbufWriteU8(dst, getMotorCount()); + break; +#endif + case MSP2_INAV_WIND: #ifdef USE_WIND_ESTIMATOR { @@ -3808,6 +3827,32 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } break; +#ifdef USE_MOTOR_SRXL2 + case MSP2_INAV_ESC_SRXL2_CALIBRATE: + /* + * One of these phases commands full throttle with the aircraft disarmed, + * so the refusals live in the driver and are not re-implemented here: a + * caller that skipped them would otherwise be trusted. + */ + if (!sbufReadU8Safe(&tmp_u8, src)) { + return MSP_RESULT_ERROR; + } + if (tmp_u8 == SRXL2_CAL_OFF) { + srxl2MotorCalibrationAbort(); + } else if (tmp_u8 == SRXL2_CAL_WAIT_BATTERY) { + if (srxl2MotorCalibrationBegin() != SRXL2_CAL_ACCEPTED) { + return MSP_RESULT_ERROR; + } + } else if (tmp_u8 == SRXL2_CAL_HIGH_MANUAL || tmp_u8 == SRXL2_CAL_LOW_MANUAL) { + if (srxl2MotorCalibrationManual(tmp_u8) != SRXL2_CAL_ACCEPTED) { + return MSP_RESULT_ERROR; + } + } else { + return MSP_RESULT_ERROR; + } + break; +#endif + case MSP2_INAV_SELECT_MIXER_PROFILE: if (!ARMING_FLAG(ARMED) && sbufReadU8Safe(&tmp_u8, src)) { setConfigMixerProfileAndWriteEEPROM(tmp_u8); diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 0be7382ad07..742c90f4510 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -29,11 +29,13 @@ #include "fc/config.h" #include "fc/fc_msp_box.h" #include "fc/runtime_config.h" + #include "flight/mixer.h" #include "flight/mixer_profile.h" #include "io/osd.h" +#include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" #include "sensors/diagnostics.h" @@ -117,6 +119,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { .boxId = BOXAUTOSPEED, .boxName = "AUTO SPEED", .permanentId = 69 }, { .boxId = BOXTERRAINAGLHOLD, .boxName = "TERRAIN AGL HOLD", .permanentId = 70 }, { .boxId = BOXINFLIGHTMENU, .boxName = "IN FLIGHT MENU", .permanentId = 71 }, + { .boxId = BOXTHRUSTREVERSE, .boxName = "THRUST REVERSE", .permanentId = 72 }, { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF } }; @@ -393,6 +396,28 @@ void initActiveBoxIds(void) #ifdef USE_CMS ADD_ACTIVE_BOX(BOXINFLIGHTMENU); #endif + +#ifdef USE_MOTOR_SRXL2 + /* + * Thrust reverse on a Spektrum Smart ESC is a switch, not a throttle value: + * the ESC's "Thrust Rev." parameter names an auxiliary channel, and Spektrum + * describe the effect as "flipping the designated switch reverses motor + * rotation, throttle will still control motor speed". + * + * So it belongs on a mode, the way every other pilot-commanded action does. + * The alternative - deriving it from the reversible-motor mixer state - needs + * FEATURE_REVERSIBLE_MOTORS, which recentres the throttle stick so that mid + * stick is zero thrust. That suits a 3D model and is wrong for an aeroplane + * that wants reverse only on the landing roll, where chopping the throttle on + * short final would otherwise command reverse thrust in the air. + * + * Offered only when the protocol can act on it and a channel is set, so it + * does not appear as a mode that silently does nothing. + */ + if (motorConfig()->motorPwmProtocol == PWM_TYPE_SRXL2 && motorConfig()->srxl2ReverseChannel != 0) { + ADD_ACTIVE_BOX(BOXTHRUSTREVERSE); + } +#endif } #define IS_ENABLED(mask) ((mask) == 0 ? 0 : 1) @@ -491,6 +516,11 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) #ifdef USE_CMS CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXINFLIGHTMENU)), BOXINFLIGHTMENU); #endif +#ifdef USE_MOTOR_SRXL2 + /* Advertised in initActiveBoxIds() but never reported back, so the mode + * showed as off in the Configurator while the driver was acting on it. */ + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTHRUSTREVERSE)), BOXTHRUSTREVERSE); +#endif memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t)); for (uint32_t i = 0; i < activeBoxIdCount; i++) { diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 6d35c7cae4b..31143e92706 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -73,6 +73,7 @@ #include "io/vtx_msp.h" #include "io/osd_dji_hd.h" #include "io/displayport_msp_osd.h" +#include "io/motor_srxl2.h" #include "io/servo_sbus.h" #include "io/adsb.h" @@ -317,6 +318,27 @@ void taskSyncServoDriver(timeUs_t currentTimeUs) sbusServoSendUpdate(); #endif +#if defined(USE_MOTOR_SRXL2) + /* 200 Hz is the cadence this wants: Spektrum's reference application advances + * its state machine on a 5 ms tick, and the master has to run several times + * faster than its own Control Data interval to collect replies promptly on a + * half-duplex wire. */ + if (motorConfig()->motorPwmProtocol == PWM_TYPE_SRXL2) { + /* + * Reverse is a mode, because on this ESC it is a switch: Spektrum describe + * the reverse channel as flipping rotation while the throttle goes on + * meaning throttle. There is deliberately no second route through the + * mixer's reversible-motor state - that models a centre-zero stick, which + * is a different kind of ESC, and FEATURE_REVERSIBLE_MOTORS is cleared for + * this protocol at startup for the same reason. + * + * Gated on being armed so that an aircraft which landed under reverse does + * not sit on the ground with the ESC's reverse channel still held. + */ + srxl2MotorSetReverse(ARMING_FLAG(ARMED) && IS_RC_MODE_ACTIVE(BOXTHRUSTREVERSE)); + srxl2MotorProcess(); + } +#endif } #ifdef USE_OSD @@ -418,7 +440,12 @@ void fcTasksInit(void) setTaskEnabled(TASK_STACK_CHECK, true); #endif #if defined(USE_SERVO_SBUS) - setTaskEnabled(TASK_PWMDRIVER, (servoConfig()->servo_protocol == SERVO_TYPE_SBUS) || (servoConfig()->servo_protocol == SERVO_TYPE_SBUS_PWM)); + setTaskEnabled(TASK_PWMDRIVER, (servoConfig()->servo_protocol == SERVO_TYPE_SBUS) + || (servoConfig()->servo_protocol == SERVO_TYPE_SBUS_PWM) +#ifdef USE_MOTOR_SRXL2 + || (motorConfig()->motorPwmProtocol == PWM_TYPE_SRXL2) +#endif + ); #endif #ifdef USE_CMS #ifdef USE_MSP_DISPLAYPORT diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 59de340bac9..a49733bfbe3 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -88,6 +88,7 @@ typedef enum { BOXAUTOSPEED = 60, BOXTERRAINAGLHOLD = 61, BOXINFLIGHTMENU = 62, + BOXTHRUSTREVERSE = 63, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 0744c98403c..e3dfa364ab5 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -27,7 +27,14 @@ tables: - name: blackbox_device values: ["SERIAL", "SPIFLASH", "SDCARD", "FILE"] - name: motor_pwm_protocol - values: ["STANDARD", "ONESHOT125", "MULTISHOT", "BRUSHED", "DSHOT150", "DSHOT300", "DSHOT600"] + values: ["STANDARD", "ONESHOT125", "MULTISHOT", "BRUSHED", "DSHOT150", "DSHOT300", "DSHOT600", "SRXL2"] + # The default is FIRST on purpose, so that it is index 0. This field was appended + # to motorConfig_t and landed inside the struct's existing padding, so sizeof did + # not grow and pgLoad()'s memcpy copies the old zero over the reset default: an + # existing configuration reads whatever index 0 happens to be. Putting the default + # there makes that harmless instead of silently selecting the fastest rate. + - name: esc_srxl2_telemetry_rate + values: ["1HZ", "3HZ", "2HZ"] - name: servo_protocol values: ["PWM", "SBUS", "SBUS_PWM"] - name: failsafe_procedure @@ -862,6 +869,25 @@ groups: default_value: "ONESHOT125" field: motorPwmProtocol table: motor_pwm_protocol + - name: esc_srxl2_telemetry + description: "Read ESC telemetry off the SRXL2 link. Only applies when motor_pwm_protocol is SRXL2, where telemetry shares the throttle wire and so cannot be turned off by leaving a port unassigned as it would be for a conventional ESC." + default_value: ON + field: srxl2Telemetry + condition: USE_MOTOR_SRXL2 + type: bool + - name: esc_srxl2_telemetry_rate + description: "How often ESC telemetry arrives from an SRXL2 Smart ESC, in readings per second. The ESC answers about two requests in three and rotates its reply between three sensors, so it delivers roughly a ninth of what is asked for - these are the delivered rates, measured, not the request rate. The reply shares the throttle wire, so a faster rate leaves the bus less headroom; only the RPM filter benefits from it. The range is bounded at both ends by the ESC: asking on every frame makes an Avian keep the link and stop obeying the throttle, and asking slower than 1 Hz makes the link time out on a healthy ESC, because its reply is the only thing that proves it is still there." + default_value: "1HZ" # must stay index 0, see the table comment + field: srxl2TelemetryRate + condition: USE_MOTOR_SRXL2 + table: esc_srxl2_telemetry_rate + - name: esc_srxl2_reverse_channel + description: "For an SRXL2 Smart ESC, the 1-based auxiliary channel its \"Thrust Rev.\" setting selects to arm reverse. Spektrum allow channels 5 to 9 and ship channel 7 by default. Must match how the ESC was programmed, because nothing on the wire advertises it. 0 disables reverse, and anything between 1 and 4 is treated as 0 at boot - the range cannot express the hole, and a channel the ESC cannot watch would offer a mode that does nothing." + default_value: 7 + field: srxl2ReverseChannel + condition: USE_MOTOR_SRXL2 + min: 0 + max: 9 - name: motor_poles field: motorPoleCount description: "The number of motor poles. Required to compute motor RPM" diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 9743be74dd3..bd5778b71be 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -96,6 +96,11 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig, .motorPwmRate = SETTING_MOTOR_PWM_RATE_DEFAULT, .mincommand = SETTING_MIN_COMMAND_DEFAULT, .motorPoleCount = SETTING_MOTOR_POLES_DEFAULT, // Most brushless motors that we use are 14 poles +#ifdef USE_MOTOR_SRXL2 + .srxl2ReverseChannel = SETTING_ESC_SRXL2_REVERSE_CHANNEL_DEFAULT, + .srxl2TelemetryRate = SETTING_ESC_SRXL2_TELEMETRY_RATE_DEFAULT, + .srxl2Telemetry = SETTING_ESC_SRXL2_TELEMETRY_DEFAULT, +#endif ); PG_REGISTER_ARRAY_WITH_RESET_FN(timerOverride_t, HARDWARE_TIMER_DEFINITION_COUNT, timerOverrides, PG_TIMER_OVERRIDE_CONFIG, 0); diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index d6095cf2240..120b21f4d9b 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -95,6 +95,32 @@ typedef struct motorConfig_s { uint8_t motorPwmProtocol; uint16_t digitalIdleOffsetValue; uint8_t motorPoleCount; // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry +#ifdef USE_MOTOR_SRXL2 + /* + * Appended at the end, but not immediately after motorPoleCount. + * + * pgLoad() applies the reset defaults and then copies MIN(stored, current) + * bytes over them, comparing only the parameter group version and never the + * size. Before these fields the group was ten bytes: nine in use and one of + * tail padding, the struct being two-byte aligned. That tenth byte is inside + * what an older configuration stored, and it stored it as zero, because + * pgResetInstance() copies the reset template whole and a template's padding + * is zero. So a field placed there has its default overwritten with zero by + * any configuration saved before this firmware, which for the reverse + * channel means the feature comes up disabled on exactly the boards that + * already had settings worth keeping. + * + * This byte absorbs that overlap, so the fields after it begin past the end + * of the old record and keep their defaults. The alternative is bumping the + * group version, which would work by discarding every user's motor settings + * - protocol, rates, pole count - in order to add an optional field. + */ + uint8_t srxl2PadOverlap; // absorbs the old tail padding; never read + + uint8_t srxl2ReverseChannel; // 1-based aux channel an SRXL2 ESC uses to arm reverse; 0 disables + uint8_t srxl2Telemetry; // read ESC telemetry off the SRXL2 link + uint8_t srxl2TelemetryRate; // how often to ask the ESC for telemetry, as srxl2TelemetryRate_e +#endif } motorConfig_t; PG_DECLARE(motorConfig_t, motorConfig); diff --git a/src/main/io/motor_srxl2.c b/src/main/io/motor_srxl2.c new file mode 100644 index 00000000000..ba4fe48d5bb --- /dev/null +++ b/src/main/io/motor_srxl2.c @@ -0,0 +1,977 @@ +/* + * This file is part of INAV. + * + * INAV is free software. You can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +/* + * SRXL2 bus master for Spektrum Smart ESCs ("Smart Throttle"), following + * "Specification for Spektrum SRXL2" Rev K (https://github.com/SpektrumRC/SRXL2). + * Packet structures come from rx/srxl2_types.h, shared with the receiver side. + * + * The specification describes the bus and says nothing about any particular ESC. + * What an ESC decides for itself is kept to the named constants and the + * reverse-channel setter below, so what needs confirming against real hardware + * is countable. docs/Spektrum Smart ESC.md has the bench measurements. + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef USE_MOTOR_SRXL2 + +#include "build/debug.h" + +#include "common/crc.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "drivers/serial.h" +#include "drivers/time.h" + +#include "fc/runtime_config.h" + +#include "sensors/battery.h" + +#include "io/serial.h" +#include "io/motor_srxl2.h" + +#include "rx/srxl2_types.h" + +/*--------------------------------------------------------------------------- + * Wire constants + *-------------------------------------------------------------------------*/ + +#define SRXL2_MAGIC 0xA6 +#define SRXL2_MAX_FRAME 80 +#define SRXL2_MIN_FRAME 5 + +#define SRXL2_BAUD_LOW 115200 +#define SRXL2_BAUD_HIGH 400000 +#define SRXL2_BAUD_BIT_400K 0x01 /* baudSupported bit for 400000 */ + +#define SRXL2_PORT_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO | SERIAL_BIDIR) + +/* ESC device IDs. rx/srxl2_types.h names the flight controller range; this is + * the adjacent range in the same table of the specification. */ +#define SRXL2_ESC_ID_FIRST 0x40 +#define SRXL2_ESC_ID_LAST 0x4F + +// Flight controller type, unit ID 1. Not unit 0: specification 7.1.1 has a device whose +// lower nibble is 0 announce itself unprompted, which is the slave's behaviour, and a +// master doing it would collide with the ESC's own announcements +#define SRXL2_OUR_DEVICE_ID 0x31 + +// The bus picks its master by lowest device ID and we never implement standing down, since +// this port is a dedicated link to one ESC, which cannot outrank us from 0x40. It would be +// wrong on a bus shared with a Spektrum receiver, which is master at 0x21 + +// The protocol's failsafe channel-data command, 0x01, is deliberately never sent: INAV owns +// failsafe and keeps commanding the motors all the way down, so handing throttle authority +// to the ESC mid-landing would be the opposite of helpful. A dead wire is covered by the +// ESC's own receive timeout, which needs nothing from us +#define SRXL2_CMD_CHANNEL_DATA 0x00 + +/* Reply ID 0x00 means "no reply wanted" (specification 7.1.1). */ +#define SRXL2_REPLY_NONE 0x00 + +// The frame carries the throttle and, when configured, the reverse channel. Nothing else: +// the ESC reads CH1 and the channel its own Thrust Rev. parameter names. How many channels +// the mask holds makes no difference to the ESC, measured from one to eight; the telemetry +// request rate does, see SRXL2_TELEM_REQUEST_MIN + +/* X-Bus telemetry sensor IDs */ +#define SRXL2_TELEM_SENSOR_ESC 0x20 + +// Specification 7.2.1: an ESC with unit ID 0 repeats an unprompted handshake every 50 ms +// for the first 200 ms after reset, so listen a little longer than that before polling. An +// ESC with a non-zero unit ID never announces itself and would otherwise never be found +#define SRXL2_LISTEN_WINDOW_MS 250 +#define SRXL2_HANDSHAKE_INTERVAL_MS 50 + +// Rate at which Control Data goes out once the link is up, which is not the motor update +// rate: the specification has the master emit one packet per RF frame, tens of hertz. 115200 +// baud would not carry more anyway, an eighteen-byte frame being about 1.6 ms on the wire +#define SRXL2_CONTROL_INTERVAL_MS 20 /* 50 Hz */ + +// Ask for telemetry every Nth Control Data packet: a reply forces a half-duplex turnaround +// for values that change slowly. Every fifth frame at 50 Hz asks ten times a second, which +// arrives as about 1.1 ESC readings a second once the ESC's own sensor rotation is counted +#define SRXL2_TELEM_REQUEST_DEFAULT 5 + +// Never ask on every frame. An Avian asked at 50 Hz answers, and its telemetry is correct, +// but it stops obeying the throttle and reports zero per cent from every stick position. +// Every 2nd, 3rd and 4th frame all behave, and dropping back recovers it without a power +// cycle. Measured on an Avian 70A against a fixed 1250 us command +#define SRXL2_TELEM_REQUEST_MIN 2 + +// Declare the link dead if the ESC stops answering for this long. A running Avian never +// speaks unprompted, so only a telemetry reply refreshes this timer and a request rate +// slower than the timeout would disconnect a healthy ESC on schedule. It answers about two +// requests in three, so the margin also has to cover consecutive misses. This is why the +// telemetry rate table stops at one request every five frames +#define SRXL2_LINK_TIMEOUT_MS 500 + +// How long after the link comes up before the ESC will actually turn the motor. An Avian +// announces itself within 300 ms of gaining power but then plays its startup tones for about +// five seconds, and only drives the motor once the last of them has sounded. Timed on the +// bench against the tones. Arming waits for this rather than for the handshake alone, which +// costs nothing: nobody arms that soon after connecting the battery +#define SRXL2_READY_DELAY_MS 6500 + +// Telemetry older than this reads as stale. Generous next to the link timeout on purpose: +// the ESC rotates its reply between three sensors, so its own readings arrive about once a +// second and a tighter window made a healthy sensor flicker. An ESC that has actually +// stopped is caught by SRXL2_LINK_TIMEOUT_MS, which invalidates the reading anyway +#define SRXL2_TELEM_STALE_MS 3000 + +// Calibration phases end themselves: the high one has to outlast a person reaching for a +// battery lead, the low one only the ESC's tones. Neither may persist, one is full throttle +#define SRXL2_CAL_WAIT_TIMEOUT_MS 60000 /* time to walk over and plug the battery in */ +#define SRXL2_CAL_MANUAL_TIMEOUT_MS 30000 + +// How long to hold full throttle after the ESC gains power, then how long to hold minimum. +// The published tone timings suggest three and five seconds; measured on an Avian 70A that +// lands on the edge of the window and stores nothing, while four and seven store every time. +// The extra second either side costs nothing: the sequence runs once, on a bench, with the +// propeller off. docs/Spektrum Smart ESC.md has the before and after figures +#define SRXL2_CAL_SETTLE_MS 4000 + +/* Long enough for the cell-count tones and the closing long tone. */ +#define SRXL2_CAL_LOW_MS 7000 + +/* Endpoints presented during calibration, on INAV's usual motor scale. */ +#define SRXL2_CAL_HIGH_US 2000 +#define SRXL2_CAL_LOW_US 1000 + +// Channel value scaling, the exact inverse of what rx/srxl2.c applies when it decodes: +// us = 988 + (value >> 6). 1500 us lands on 0x8000, which the specification calls Servo +// Center, and the shift leaves the low two bits clear as it requires. It deliberately stops +// short of the ends of the 0..65532 range, because a Spektrum receiver's full travel decodes +// to 988..2012 us, and looking like a receiver is what an ESC's stored endpoints expect. +// The 1.2 % of travel this costs at the top beats stretching 1000..2000 over the full range, +// which would move the centre, and the centre is where Reverse brake mode takes no thrust +#define SRXL2_PULSE_OFFSET_US 988 +#define SRXL2_PULSE_SHIFT 6 +#define SRXL2_VALUE_MAX 0xFFFC /* specification caps values here */ + +// Throttle is channel index 0 by Spektrum convention, confirmed against an Avian 70A +#define SRXL2_CHANNEL_THROTTLE 0 + +// How many channels the driver keeps values for. The wire's mask is 32 bits wide, but only +// the throttle and the reverse channel are ever written, and esc_srxl2_reverse_channel +// cannot name anything above 9 +#define SRXL2_CHANNEL_COUNT 10 + +/*--------------------------------------------------------------------------- + * State + *-------------------------------------------------------------------------*/ + +typedef enum { + SRXL2_DISABLED = 0, + SRXL2_LISTENING, /* silent, giving an auto-announcing ESC room to speak */ + SRXL2_POLLING, /* actively asking the ESC device ID to answer */ + SRXL2_FINALISING, /* broadcasting the agreed baud rate */ + SRXL2_RUNNING, +} srxl2State_e; + +// One per ESC. Each is an independent bus with its own handshake, baud negotiation, framing +// and telemetry; nothing is shared but our own device ID, which is safe precisely because +// the buses never hear each other +typedef struct { + serialPort_t *port; + srxl2State_e state; + + uint8_t rxBuf[SRXL2_MAX_FRAME]; + uint8_t rxLen; + uint8_t rxExpected; + + timeMs_t stateEnteredMs; + timeMs_t lastRxMs; + timeMs_t lastTxMs; + timeMs_t lastControlMs; + + uint8_t deviceId; /* 0 until discovered */ + uint8_t baudSupported; + uint8_t pollId; /* offset from SRXL2_ESC_ID_FIRST, while polling */ + timeMs_t runningSinceMs; /* when the link came up, for SRXL2_READY_DELAY_MS */ + uint8_t agreedBaudBits; + bool baudSwitchPending; /* waiting for TX to drain */ + + uint16_t channelValue[SRXL2_CHANNEL_COUNT]; + uint32_t channelMask; + uint8_t telemRequestCounter; + + srxl2EscTelemetry_t telemetry; + + uint32_t statTxFrames, statRxFrames, statCrcErrors, statHandshakes; +} srxl2Esc_t; + +static srxl2Esc_t esc[SRXL2_ESC_MAX_MOTORS]; +static uint8_t escCount; /* ports successfully opened */ + +/* Shared, because these describe the aircraft rather than one bus. */ +static uint8_t reverseChannel1Based = 7; /* Spektrum ship "Thrust Rev." on CH7 */ + +// Control frames between telemetry requests: at a 20 ms interval, 5 asks at 10 Hz. Held as +// a divisor rather than a rate because that is what the transmit path counts +static uint8_t telemRequestEvery = SRXL2_TELEM_REQUEST_DEFAULT; + +static srxl2CalPhase_e calPhase = SRXL2_CAL_OFF; +static srxl2CalResult_e calLastResult = SRXL2_CAL_ACCEPTED; +static timeMs_t calPhaseMs; /* when the current phase began */ +static uint32_t calHandshakeMark; /* total handshakes when the phase began */ + +/*--------------------------------------------------------------------------- + * Helpers + *-------------------------------------------------------------------------*/ + +static inline uint16_t srxl2UsToValue(uint16_t us) +{ + if (us < SRXL2_PULSE_OFFSET_US) { + us = SRXL2_PULSE_OFFSET_US; + } + const uint32_t v = ((uint32_t)(us - SRXL2_PULSE_OFFSET_US)) << SRXL2_PULSE_SHIFT; + return (v > SRXL2_VALUE_MAX) ? SRXL2_VALUE_MAX : (uint16_t)v; +} + +static inline uint16_t be16(const uint8_t *p) +{ + return (uint16_t)((p[0] << 8) | p[1]); +} + +static bool srxl2ReverseChannelUsable(uint8_t channel1Based); + +// Rebuilt rather than accumulated, so the frame does not change shape depending on what has +// been called since power-up, and so that clearing the reverse channel stops sending it +static void srxl2BuildChannelMask(srxl2Esc_t *e) +{ + uint32_t mask = 1u << SRXL2_CHANNEL_THROTTLE; + if (srxl2ReverseChannelUsable(reverseChannel1Based)) { + mask |= 1u << (reverseChannel1Based - 1); + } + e->channelMask = mask; +} + +static void srxl2SetState(srxl2Esc_t *e, srxl2State_e next) +{ + e->state = next; + e->stateEnteredMs = millis(); +} + +// Handshakes seen across every bus. The calibration watches this to notice an ESC gaining +// power, and the first to speak is signal enough: they all share a pack +static uint32_t srxl2TotalHandshakes(void) +{ + uint32_t total = 0; + for (uint8_t i = 0; i < escCount; i++) { + total += esc[i].statHandshakes; + } + return total; +} + +// Append the CRC and push the frame; buf[2] must already hold the length. Returns false when +// the frame did not go out, which matters for one frame only: a dropped Control Data is of +// no consequence, another follows in 20 ms, but raising the baud rate while only believing +// the broadcast was sent leaves the ESC behind at the old rate with no way back short of a +// power cycle. Seen on real hardware, and not recoverable in flight +static bool srxl2SendFrame(srxl2Esc_t *e, uint8_t *buf, uint8_t len) +{ + if (!e->port || len < SRXL2_MIN_FRAME || len > SRXL2_MAX_FRAME) { + return false; + } + + if (serialTxBytesFree(e->port) < len) { + return false; + } + + const uint16_t crc = crc16_ccitt_update(0, buf, len - 2); + buf[len - 2] = (uint8_t)(crc >> 8); + buf[len - 1] = (uint8_t)(crc & 0xFF); + + serialWriteBuf(e->port, buf, len); + e->lastTxMs = millis(); + e->statTxFrames++; + return true; +} + +static bool srxl2SendHandshake(srxl2Esc_t *e, uint8_t destinationId, uint8_t baudField) +{ + uint8_t buf[sizeof(Srxl2HandshakeFrame)]; + Srxl2HandshakeFrame *f = (Srxl2HandshakeFrame *)buf; + + f->header.id = SRXL2_MAGIC; + f->header.packetType = Handshake; + f->header.length = sizeof(Srxl2HandshakeFrame); + + f->payload.sourceDeviceId = SRXL2_OUR_DEVICE_ID; + f->payload.destinationDeviceId = destinationId; + f->payload.priority = 10; + /* When polling a specific device this advertises what we can do; in the + * broadcast it states what every device must switch to. */ + f->payload.baudSupported = baudField; + f->payload.info = 0; /* non-RF device, no RF telemetry */ + f->payload.uniqueId = 0x494E4156; /* "INAV"; only has to make a + * simultaneous-reply collision + * improbable. The same value on + * every bus is fine, because each + * bus has exactly one master. */ + + return srxl2SendFrame(e, buf, sizeof(Srxl2HandshakeFrame)); +} + +/* Tell the bus which rate everyone moves to, and arrange to follow once the + * frame has actually left the port. Switching immediately would clock the tail + * of that very frame out at the new rate and lose it. */ +static void srxl2Finalise(srxl2Esc_t *e) +{ + e->agreedBaudBits = SRXL2_BAUD_BIT_400K & e->baudSupported; + + /* Only arm the switch if the broadcast is actually on its way. If the port + * had no room, stay where we are and try again on the next pass: a rate the + * ESC was never told about is worse than a slow negotiation. */ + if (!srxl2SendHandshake(e, Broadcast, e->agreedBaudBits)) { + return; + } + + e->baudSwitchPending = (e->agreedBaudBits & SRXL2_BAUD_BIT_400K) != 0; + srxl2SetState(e, SRXL2_FINALISING); +} + +/*--------------------------------------------------------------------------- + * Telemetry decoding: STRU_TELE_ESC, big-endian on the wire + *-------------------------------------------------------------------------*/ + +static void srxl2DecodeEscTelemetry(srxl2Esc_t *e, const uint8_t *payload) +{ + /* payload[0] is the sensor id, payload[1] a secondary id. */ + const uint16_t rpm = be16(&payload[2]); + const uint16_t voltsIn = be16(&payload[4]); + const uint16_t tempFet = be16(&payload[6]); + const uint16_t currentMot = be16(&payload[8]); + const uint16_t tempBec = be16(&payload[10]); + const uint8_t currentBec = payload[12]; + const uint8_t voltsBec = payload[13]; + const uint8_t throttle = payload[14]; + const uint8_t powerOut = payload[15]; + + srxl2EscTelemetry_t *t = &e->telemetry; + memset(t, 0, sizeof(*t)); + + /* 0xFFFF and 0xFF mean "no data" and must not be taken for readings - + * 0xFFFF volts at 0.01 V per count would otherwise look like 655 V. */ + if (rpm != 0xFFFF) { t->rpm = (uint32_t)rpm * 10; t->fields |= SRXL2_TELEM_FIELD_RPM; } + if (voltsIn != 0xFFFF) { t->voltage = voltsIn; t->fields |= SRXL2_TELEM_FIELD_VOLTAGE; } + if (currentMot != 0xFFFF) { t->current = currentMot; t->fields |= SRXL2_TELEM_FIELD_CURRENT; } + if (tempFet != 0xFFFF) { t->temperatureFet = (int16_t)tempFet; t->fields |= SRXL2_TELEM_FIELD_TEMP_FET; } + if (tempBec != 0xFFFF) { t->temperatureBec = (int16_t)tempBec; t->fields |= SRXL2_TELEM_FIELD_TEMP_BEC; } + if (currentBec != 0xFF) { t->currentBec = (uint16_t)currentBec * 10; } /* 100 mA -> 0.01 A */ + if (voltsBec != 0xFF) { t->voltageBec = (uint16_t)voltsBec * 5; } /* 0.05 V -> 0.01 V */ + if (throttle != 0xFF) { t->throttlePercent = MIN((uint8_t)(throttle / 2), 100); } + if (powerOut != 0xFF) { t->powerPercent = MIN((uint8_t)(powerOut / 2), 100); } + + t->lastUpdateMs = millis(); + t->valid = true; +} + +/*--------------------------------------------------------------------------- + * Received frame handling + *-------------------------------------------------------------------------*/ + +static void srxl2HandleHandshake(srxl2Esc_t *e, const uint8_t *buf) +{ + const Srxl2HandshakeFrame *f = (const Srxl2HandshakeFrame *)buf; + const uint8_t src = f->payload.sourceDeviceId; + + if (src < SRXL2_ESC_ID_FIRST || src > SRXL2_ESC_ID_LAST) { + /* Something else on the bus. This driver only speaks to ESCs. */ + return; + } + + // Enter the negotiation once, and only from the states still looking for an ESC. + // Restarting it on every handshake makes the two ends ping-pong: our broadcast draws a + // handshake, which would restart the sequence, and a bus looping through FINALISING + // never reaches its first control frame. A slave that genuinely reset is not missed, + // since it comes back at 115200 and the link timeout drops us to POLLING to find it + if (e->state == SRXL2_FINALISING || e->state == SRXL2_RUNNING) { + /* Still answer a running ESC, so it knows the master is there - but say + * nothing mid-negotiation, where another broadcast is what causes the + * loop. */ + if (e->state == SRXL2_RUNNING && e->deviceId == src) { + srxl2SendHandshake(e, src, SRXL2_BAUD_BIT_400K); + } + return; + } + + e->deviceId = src; + e->baudSupported = f->payload.baudSupported; + + // Counted here rather than on every handshake, so it means "a negotiation started". The + // calibration reads it as an ESC gaining power, which a keepalive answer is not + e->statHandshakes++; + + // Answer the slave so it knows who the master is, then finalise. This also covers the + // ESC being powered after the flight controller, the normal case on a bench: by then we + // are in POLLING, which accepts a handshake, so the ESC is still found + srxl2SendHandshake(e, e->deviceId, SRXL2_BAUD_BIT_400K); + srxl2Finalise(e); +} + +static void srxl2HandleTelemetry(srxl2Esc_t *e, const uint8_t *buf, uint8_t len) +{ + /* header(3) + destDeviceId(1) + 16 byte payload + crc(2) */ + if (len < 3 + 1 + 16 + 2) { + return; + } + const uint8_t *payload = &buf[4]; + if (payload[0] == SRXL2_TELEM_SENSOR_ESC) { + srxl2DecodeEscTelemetry(e, payload); + } +} + +static void srxl2HandleFrame(srxl2Esc_t *e, const uint8_t *buf, uint8_t len) +{ + const uint16_t crc = crc16_ccitt_update(0, buf, len - 2); + if (buf[len - 2] != (uint8_t)(crc >> 8) || buf[len - 1] != (uint8_t)(crc & 0xFF)) { + e->statCrcErrors++; + return; + } + + e->statRxFrames++; + e->lastRxMs = millis(); + + switch (buf[1]) { + case Handshake: + /* Length checked before the payload is read: a short frame that happens + * to carry a valid CRC would otherwise have its device ID and baud + * fields taken from whatever the receive buffer held last. */ + if (len >= sizeof(Srxl2HandshakeFrame)) { + srxl2HandleHandshake(e, buf); + } + break; + case TelemetrySensorData: + srxl2HandleTelemetry(e, buf, len); + break; + default: + break; + } +} + +static void srxl2DrainRx(srxl2Esc_t *e) +{ + while (serialRxBytesWaiting(e->port)) { + const uint8_t c = serialRead(e->port); + + if (e->rxLen == 0) { + if (c != SRXL2_MAGIC) { + continue; /* resynchronise on the magic byte */ + } + e->rxExpected = 0; + } + + e->rxBuf[e->rxLen++] = c; + + if (e->rxLen == 3) { + e->rxExpected = e->rxBuf[2]; + if (e->rxExpected < SRXL2_MIN_FRAME || e->rxExpected > SRXL2_MAX_FRAME) { + e->rxLen = 0; /* bogus length, drop and resynchronise */ + continue; + } + } + + if (e->rxExpected && e->rxLen >= e->rxExpected) { + srxl2HandleFrame(e, e->rxBuf, e->rxLen); + e->rxLen = 0; + e->rxExpected = 0; + } + } +} + +/*--------------------------------------------------------------------------- + * Control data + *-------------------------------------------------------------------------*/ + +static void srxl2SendControlData(srxl2Esc_t *e) +{ + uint8_t buf[SRXL2_MAX_FRAME]; + uint8_t n = 0; + + /* Request telemetry only every so often: the reply shares the wire with the + * control data, so asking on every frame halves the headroom for no gain on a + * sensor whose values move slowly. */ + uint8_t replyId = SRXL2_REPLY_NONE; + if (++e->telemRequestCounter >= telemRequestEvery) { + e->telemRequestCounter = 0; + replyId = e->deviceId; + } + + // Calibration substitutes the throttle as the frame is built, rather than staging it, + // so no mixer path can write over it between the two. It substitutes rather than stores, + // so that the phase ending is enough to restore what the mixer staged: writing into + // channelValue would leave full throttle there until the mixer happened to run again. + // Every ESC is calibrated at once, sharing a battery and therefore a power-up window. + uint16_t throttle = e->channelValue[SRXL2_CHANNEL_THROTTLE]; + if (calPhase != SRXL2_CAL_OFF) { + const bool high = (calPhase == SRXL2_CAL_WAIT_BATTERY) + || (calPhase == SRXL2_CAL_SETTLE) + || (calPhase == SRXL2_CAL_HIGH_MANUAL); + throttle = srxl2UsToValue(high ? SRXL2_CAL_HIGH_US : SRXL2_CAL_LOW_US); + } + + buf[n++] = SRXL2_MAGIC; + buf[n++] = ControlData; + buf[n++] = 0; /* length, patched below */ + buf[n++] = SRXL2_CMD_CHANNEL_DATA; + buf[n++] = replyId; + + // RSSI has to read as a healthy link even though we are not an RF device: Spektrum's own + // receiver code takes a received zero as loss of link, so sending 0 would announce a + // failed link on every frame + buf[n++] = 100; + buf[n++] = 0; /* frameLosses low */ + buf[n++] = 0; /* frameLosses high */ + + buf[n++] = (uint8_t)(e->channelMask & 0xFF); + buf[n++] = (uint8_t)((e->channelMask >> 8) & 0xFF); + buf[n++] = (uint8_t)((e->channelMask >> 16) & 0xFF); + buf[n++] = (uint8_t)((e->channelMask >> 24) & 0xFF); + + // A contiguous block, little-endian, lowest index first. Channels the driver has nothing + // to say on are padded at their minimum and never at centre: on an aircraft ESC 1500 us + // is half throttle, so if the ESC read throttle on an unexpected index, centred padding + // would spin the motor while minimum padding leaves it idle + for (uint8_t ch = 0; ch < SRXL2_CHANNEL_COUNT; ch++) { + if (e->channelMask & (1u << ch)) { + const uint16_t v = (ch == SRXL2_CHANNEL_THROTTLE) ? throttle : e->channelValue[ch]; + buf[n++] = (uint8_t)(v & 0xFF); + buf[n++] = (uint8_t)(v >> 8); + } + } + + n += 2; /* room for the CRC */ + buf[2] = n; + srxl2SendFrame(e, buf, n); +} + +/*--------------------------------------------------------------------------- + * Public API + *-------------------------------------------------------------------------*/ + +bool srxl2MotorInitialize(void) +{ + memset(esc, 0, sizeof(esc)); + escCount = 0; + + // One ESC per port, so open every port assigned the function. The enumeration follows + // UART order, so motor 1 is the lowest-numbered assigned UART: nothing on an SRXL2 bus + // says which motor an ESC drives, so the wiring order has to carry it + const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_ESC_SRXL2); + + while (portConfig && escCount < SRXL2_ESC_MAX_MOTORS) { + serialPort_t *port = openSerialPort(portConfig->identifier, FUNCTION_ESC_SRXL2, + NULL, NULL, SRXL2_BAUD_LOW, MODE_RXTX, + SRXL2_PORT_OPTIONS); + if (port) { + srxl2Esc_t *e = &esc[escCount++]; + + e->port = port; + + /* Every channel starts at its lowest value rather than zero, so the + * first frame after a handshake cannot be read as something + * unexpected whichever index the ESC happens to care about. */ + for (uint8_t ch = 0; ch < SRXL2_CHANNEL_COUNT; ch++) { + e->channelValue[ch] = srxl2UsToValue(1000); + } + srxl2BuildChannelMask(e); + + const timeMs_t now = millis(); + e->lastRxMs = now; + e->lastTxMs = now; + e->lastControlMs = now; + + srxl2SetState(e, SRXL2_LISTENING); + } + + portConfig = findNextSerialPortConfig(FUNCTION_ESC_SRXL2); + } + + return escCount > 0; +} + +void srxl2MotorUpdate(uint8_t index, uint16_t value) +{ + if (index >= escCount) { + /* No port for this motor. There is nothing sensible to do here - no timer + * output to fall back on - so the shortfall is reported through + * srxl2MotorCount() and caught at arming rather than absorbed. */ + return; + } + /* Staging only. The wire is driven at its own rate from srxl2MotorProcess(), + * not at whatever rate the mixer happens to run. */ + esc[index].channelValue[SRXL2_CHANNEL_THROTTLE] = srxl2UsToValue(value); +} + +// Whether a configured reverse channel can be used. Zero means the model has no reverse, and +// anything landing on the throttle channel is refused: srxl2MotorSetReverse() writes its +// channel unconditionally at task rate, so an aliased one would overwrite the staged throttle +// hundreds of times a second. The setting's own range cannot express the hole, so the check +// belongs here; the upper bound is the width of the channel array +static bool srxl2ReverseChannelUsable(uint8_t channel1Based) +{ + if (channel1Based == 0 || channel1Based > SRXL2_CHANNEL_COUNT) { + return false; + } + return (channel1Based - 1) != SRXL2_CHANNEL_THROTTLE; +} + +void srxl2MotorSetReverse(bool armed) +{ + if (!srxl2ReverseChannelUsable(reverseChannel1Based)) { + return; /* reverse not configured, or the channel is not usable */ + } + const uint8_t idx = reverseChannel1Based - 1; + const uint16_t v = srxl2UsToValue(armed ? 2000 : 1000); + + /* Every ESC, because the mixer decides a direction for the aircraft rather + * than for one motor. Reversing one side of a twin and not the other is the + * one outcome here worth engineering against. */ + for (uint8_t i = 0; i < escCount; i++) { + esc[i].channelValue[idx] = v; + } +} + +void srxl2MotorSetReverseChannel(uint8_t channel1Based) +{ + reverseChannel1Based = srxl2ReverseChannelUsable(channel1Based) ? channel1Based : 0; + + /* The block has to reach it, and this may be called after the ports opened. */ + for (uint8_t i = 0; i < escCount; i++) { + srxl2BuildChannelMask(&esc[i]); + } +} + +void srxl2MotorSetTelemetryRate(srxl2TelemetryRate_e rate) +{ + /* Indexed by srxl2TelemetryRate_e. Each entry is how many 50 Hz control + * frames pass between requests; the setting is named for what comes back, + * which is roughly a ninth of what is asked for. */ + static const uint8_t divisor[] = { 5, 2, 3 }; + + uint8_t every = (rate < ARRAYLEN(divisor)) ? divisor[rate] : SRXL2_TELEM_REQUEST_DEFAULT; + + /* Clamped here as well as in the table, so that no future entry - or a + * configuration written by an older build, where index 1 meant every + * frame - can ask at a rate the ESC answers but will not fly at. */ + telemRequestEvery = (every < SRXL2_TELEM_REQUEST_MIN) ? SRXL2_TELEM_REQUEST_MIN : every; +} + +/* Checked on both sides rather than trusting the caller, because one of these + * phases commands full throttle with the aircraft disarmed. */ +static srxl2CalResult_e srxl2CalCommonChecks(void) +{ + if (ARMING_FLAG(ARMED)) { + return SRXL2_CAL_REJECT_ARMED; + } + if (escCount == 0) { + return SRXL2_CAL_REJECT_NO_PORT; + } + return SRXL2_CAL_ACCEPTED; +} + +srxl2CalResult_e srxl2MotorCalibrationBegin(void) +{ + const srxl2CalResult_e common = srxl2CalCommonChecks(); + if (common != SRXL2_CAL_ACCEPTED) { + return (calLastResult = common); + } + + /* Detecting the ESC powering up is the whole mechanism, so say so plainly + * instead of starting a sequence that can never advance. */ + if (!isBatteryVoltageConfigured()) { + return (calLastResult = SRXL2_CAL_REJECT_NO_VOLTAGE_SENSOR); + } + + /* + * Refuse if the pack is already in. The ESC only reads its endpoints as it + * powers up, so starting with it already running would achieve nothing - and + * it would mean presenting full throttle to an ESC that can act on it. + */ + if (getBatteryState() != BATTERY_NOT_PRESENT) { + return (calLastResult = SRXL2_CAL_REJECT_BATTERY_PRESENT); + } + + calPhase = SRXL2_CAL_WAIT_BATTERY; + calPhaseMs = millis(); + calHandshakeMark = srxl2TotalHandshakes(); + return (calLastResult = SRXL2_CAL_ACCEPTED); +} + +srxl2CalResult_e srxl2MotorCalibrationManual(srxl2CalPhase_e phase) +{ + const srxl2CalResult_e common = srxl2CalCommonChecks(); + if (common != SRXL2_CAL_ACCEPTED) { + return (calLastResult = common); + } + + // The high phase commands full throttle, so it may not start against an ESC that already + // has power. Boards that cannot sense the pack report it absent and are unaffected, which + // is the case this manual path exists for + if (phase == SRXL2_CAL_HIGH_MANUAL && getBatteryState() != BATTERY_NOT_PRESENT) { + return (calLastResult = SRXL2_CAL_REJECT_BATTERY_PRESENT); + } + + calPhase = phase; + calPhaseMs = millis(); + return (calLastResult = SRXL2_CAL_ACCEPTED); +} + +void srxl2MotorCalibrationAbort(void) +{ + calPhase = SRXL2_CAL_OFF; +} + +srxl2CalPhase_e srxl2MotorCalibrationPhase(void) +{ + return calPhase; +} + +srxl2CalResult_e srxl2MotorCalibrationLastResult(void) +{ + return calLastResult; +} + +/* Advance the unattended sequence. Every phase leaves on a deadline, so nothing + * here can strand the output at full throttle. */ +static void srxl2CalProcess(timeMs_t now) +{ + if (calPhase == SRXL2_CAL_OFF) { + return; + } + + if (ARMING_FLAG(ARMED)) { + calPhase = SRXL2_CAL_OFF; + return; + } + + const timeMs_t elapsed = now - calPhaseMs; + + switch (calPhase) { + case SRXL2_CAL_WAIT_BATTERY: + // What opens the window is the ESC gaining power, which is an event, so both signals + // have to be events too: the pack appearing, or a fresh handshake on any bus. A test + // on persistent state instead would skip the wait on any board that had already + // talked to its ESC once + if (getBatteryState() != BATTERY_NOT_PRESENT || srxl2TotalHandshakes() != calHandshakeMark) { + calPhase = SRXL2_CAL_SETTLE; + calPhaseMs = now; + } else if (elapsed >= SRXL2_CAL_WAIT_TIMEOUT_MS) { + calPhase = SRXL2_CAL_OFF; + } + break; + + case SRXL2_CAL_SETTLE: + if (elapsed >= SRXL2_CAL_SETTLE_MS) { + calPhase = SRXL2_CAL_LOW; + calPhaseMs = now; + } + break; + + case SRXL2_CAL_LOW: + if (elapsed >= SRXL2_CAL_LOW_MS) { + calPhase = SRXL2_CAL_OFF; + } + break; + + case SRXL2_CAL_HIGH_MANUAL: + case SRXL2_CAL_LOW_MANUAL: + if (elapsed >= SRXL2_CAL_MANUAL_TIMEOUT_MS) { + calPhase = SRXL2_CAL_OFF; + } + break; + + default: + calPhase = SRXL2_CAL_OFF; + break; + } +} + +void srxl2MotorSendUpdate(void) +{ + /* Nothing to do: values are staged by srxl2MotorUpdate() and transmitted on + * the protocol's own schedule. Kept so the motor output layer can call the + * same hook it calls for every other protocol. */ +} + +/* One bus, advanced by one tick. */ +static void srxl2ProcessEsc(srxl2Esc_t *e, timeMs_t now) +{ + srxl2DrainRx(e); + + /* A deferred baud change completes as soon as the broadcast has left. */ + if (e->baudSwitchPending && isSerialTransmitBufferEmpty(e->port)) { + serialSetBaudRate(e->port, SRXL2_BAUD_HIGH); + e->baudSwitchPending = false; + } + + switch (e->state) { + case SRXL2_LISTENING: + /* Silent on purpose; an auto-announcing ESC is handled in + * srxl2HandleHandshake(), which moves us on. */ + if (now - e->stateEnteredMs >= SRXL2_LISTEN_WINDOW_MS) { + srxl2SetState(e, SRXL2_POLLING); + } + break; + + case SRXL2_POLLING: + if (now - e->lastTxMs >= SRXL2_HANDSHAKE_INTERVAL_MS) { + // Walk the whole ESC range rather than the default ID alone: an ESC with a + // non-zero unit ID never announces itself, and polling is the only way to find + // it. Polling the same ID on every bus is not a collision, each ESC hearing only + // its own master. What finds an Avian, though, is its own announcement at + // power-up: a running one answered none of 128 handshakes, 128 broadcasts and + // 319 telemetry requests, so a board that reboots under a powered ESC never + // links, and no amount of asking changes that + srxl2SendHandshake(e, SRXL2_ESC_ID_FIRST + e->pollId, SRXL2_BAUD_BIT_400K); + e->pollId++; + if (SRXL2_ESC_ID_FIRST + e->pollId > SRXL2_ESC_ID_LAST) { + e->pollId = 0; + } + } + break; + + case SRXL2_FINALISING: + /* Hold until the broadcast is out and any baud change has taken, then + * start driving the ESC. */ + if (!e->baudSwitchPending && isSerialTransmitBufferEmpty(e->port)) { + e->lastRxMs = now; /* do not time out on the handshake gap */ + e->lastControlMs = now; + e->runningSinceMs = now; + srxl2SetState(e, SRXL2_RUNNING); + } + break; + + case SRXL2_RUNNING: + if (now - e->lastControlMs >= SRXL2_CONTROL_INTERVAL_MS) { + e->lastControlMs = now; + srxl2SendControlData(e); + } + + if (e->telemetry.valid && (now - e->telemetry.lastUpdateMs) > SRXL2_TELEM_STALE_MS) { + e->telemetry.valid = false; + } + + if (now - e->lastRxMs >= SRXL2_LINK_TIMEOUT_MS) { + e->telemetry.valid = false; + + // Silence from the ESC is not a reason to stop commanding it. An Avian holds + // throttle indefinitely with no telemetry request sent at all; what stops it is + // the absence of control frames, and it picks the throttle back up by itself + // when they return. Tearing the link down here would cause the outage it means + // to detect, and permanently, since a running Avian answers no discovery. The + // one case that does need it is a slave that reset, which comes back at 115200 + // and cannot be heard from 400000 + if (e->agreedBaudBits != 0) { + serialSetBaudRate(e->port, SRXL2_BAUD_LOW); + e->baudSwitchPending = false; + e->agreedBaudBits = 0; + e->deviceId = 0; + srxl2SetState(e, SRXL2_POLLING); + } + } + break; + + default: + break; + } +} + +void srxl2MotorProcess(void) +{ + if (escCount == 0) { + return; + } + + const timeMs_t now = millis(); + + srxl2CalProcess(now); + + for (uint8_t i = 0; i < escCount; i++) { + srxl2ProcessEsc(&esc[i], now); + } + + // The first two words carry a nibble per ESC, so a twin can be diagnosed without a debug + // channel per bus. Both fit: the state enum is small, and ESC device IDs run 0x40..0x4F, + // so the low nibble identifies the unit, with bit 3 marking "found" against unit 0 + uint16_t states = 0, ids = 0; + uint32_t rxFrames = 0, crcErrors = 0; + for (uint8_t i = 0; i < escCount; i++) { + states |= (uint16_t)(esc[i].state & 0x07) << (4 * i); + if (esc[i].deviceId) { + ids |= (uint16_t)((esc[i].deviceId & 0x07) | 0x08) << (4 * i); + } + rxFrames += esc[i].statRxFrames; + crcErrors += esc[i].statCrcErrors; + } + + DEBUG_SET(DEBUG_ALWAYS, 0, states); + DEBUG_SET(DEBUG_ALWAYS, 1, ids); + DEBUG_SET(DEBUG_ALWAYS, 2, rxFrames); + DEBUG_SET(DEBUG_ALWAYS, 3, crcErrors); +} + +uint8_t srxl2MotorCount(void) +{ + return escCount; +} + +// Whether every ESC is not merely being driven, but answering. Deliberately stricter than +// being in RUNNING: since a telemetry gap no longer tears the link down, a board whose ESC +// was unplugged stays in RUNNING and commands a motor that is not there. Right for a machine +// already flying, wrong for one about to arm, so the arming check asks this instead +bool srxl2MotorIsConnected(void) +{ + if (escCount == 0) { + return false; + } + const timeMs_t now = millis(); + for (uint8_t i = 0; i < escCount; i++) { + if (esc[i].state != SRXL2_RUNNING || esc[i].deviceId == 0) { + return false; + } + if (now - esc[i].lastRxMs >= SRXL2_LINK_TIMEOUT_MS) { + return false; + } + if (now - esc[i].runningSinceMs < SRXL2_READY_DELAY_MS) { + return false; /* linked, but still sounding its startup tones */ + } + } + return true; +} + +bool srxl2MotorGetTelemetry(uint8_t index, srxl2EscTelemetry_t *out) +{ + if (index >= escCount || !out || !esc[index].telemetry.valid) { + return false; + } + if (millis() - esc[index].telemetry.lastUpdateMs > SRXL2_TELEM_STALE_MS) { + return false; + } + *out = esc[index].telemetry; + return true; +} + +#endif /* USE_MOTOR_SRXL2 */ diff --git a/src/main/io/motor_srxl2.h b/src/main/io/motor_srxl2.h new file mode 100644 index 00000000000..d7a04406cd5 --- /dev/null +++ b/src/main/io/motor_srxl2.h @@ -0,0 +1,238 @@ +/* + * This file is part of INAV. + * + * INAV is free software. You can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +/* + * SRXL2 motor output: acts as the SRXL2 bus master towards a Spektrum Smart + * ESC, which is what "Smart Throttle" is on the wire. + * + * Unlike every other motor protocol in INAV, this one is a UART protocol rather + * than a timer waveform, so the ESC signal wire goes to a serial pin assigned + * FUNCTION_ESC_SRXL2 and not to a motor pad. The structure deliberately mirrors + * io/servo_sbus.c, which is the existing precedent for a serial protocol + * driving outputs. + * + * Protocol reference: "Specification for Spektrum SRXL2", Rev K + * (https://github.com/SpektrumRC/SRXL2, MIT). Half-duplex single wire, + * 115200 baud for the handshake, optionally negotiated up to 400000. + */ + +#pragma once + +#include +#include + +/* + * One ESC per bus, several buses. + * + * The specification assigns ESCs device IDs 0x40..0x4F so a single bus could + * carry several, but each would need a distinct unit ID and the specification + * states that setting one over SRXL2 "is not implemented" - it expects physical + * switches, which Avian ESCs do not have. So each ESC gets its own port, and a + * multi-motor model needs as many ports as motors. + * + * Four is a practical ceiling: every instance costs a UART, and the protocol's + * update rate suits aircraft rather than multirotors anyway. + */ +#define SRXL2_ESC_MAX_MOTORS 4 + +/* Which measurements a telemetry frame actually carried. The wire has a "no + * data" code per field (0xFFFF, or 0xFF for the byte-sized ones), and a missing + * measurement must not reach a consumer as a confident zero: nothing downstream + * can tell 0.00 A meaning "idle" from 0.00 A meaning "this ESC does not report + * current". */ +typedef enum { + SRXL2_TELEM_FIELD_RPM = (1 << 0), + SRXL2_TELEM_FIELD_VOLTAGE = (1 << 1), + SRXL2_TELEM_FIELD_CURRENT = (1 << 2), + SRXL2_TELEM_FIELD_TEMP_FET = (1 << 3), + SRXL2_TELEM_FIELD_TEMP_BEC = (1 << 4), +} srxl2TelemetryField_e; + +/* Decoded ESC telemetry, from STRU_TELE_ESC (X-Bus sensor ID 0x20). + * Units are INAV's, not the wire's: the wire sends 10 rpm, 0.01 V, 10 mA and + * 0.1 degree steps, and this struct is already converted. + * A field the ESC reports as "no data" is left at 0 and its bit in `fields` is + * clear; `valid` says only that a frame arrived and is recent. */ +typedef struct { + uint32_t rpm; /* electrical rpm */ + uint16_t voltage; /* 0.01 V */ + uint16_t current; /* 0.01 A */ + int16_t temperatureFet; /* 0.1 degC */ + int16_t temperatureBec; /* 0.1 degC */ + uint16_t currentBec; /* 0.01 A */ + uint16_t voltageBec; /* 0.01 V */ + uint8_t throttlePercent; /* 0..100 */ + uint8_t powerPercent; /* 0..100 */ + uint32_t lastUpdateMs; + uint8_t fields; /* srxl2TelemetryField_e bits actually reported */ + bool valid; +} srxl2EscTelemetry_t; + +/* + * ESC throttle-range calibration. + * + * Spektrum ESCs learn their endpoints from the signal present as the battery is + * connected: full throttle first, then low within five seconds. A Spektrum + * transmitter does this with the stick, and INAV otherwise cannot, because it + * outputs mincommand while disarmed - which leaves anyone without a Spektrum + * radio unable to calibrate the ESC at all. + * + * These phases override the throttle channel so the sequence can be driven from + * the flight controller. HIGH and LOW both abort on their own after a timeout, so + * the output cannot be left commanding full throttle. + */ +typedef enum { + SRXL2_CAL_OFF = 0, + SRXL2_CAL_WAIT_BATTERY, /* full throttle held, waiting for the ESC to power up */ + SRXL2_CAL_SETTLE, /* ESC powered: holding high while it accepts the endpoint */ + SRXL2_CAL_LOW, /* low throttle so it accepts the other endpoint */ + SRXL2_CAL_HIGH_MANUAL, /* fallback for boards with no voltage sensing */ + SRXL2_CAL_LOW_MANUAL, +} srxl2CalPhase_e; + +typedef enum { + SRXL2_CAL_ACCEPTED = 0, + SRXL2_CAL_REJECT_ARMED, + SRXL2_CAL_REJECT_NO_PORT, + SRXL2_CAL_REJECT_BATTERY_PRESENT, + SRXL2_CAL_REJECT_NO_VOLTAGE_SENSOR, +} srxl2CalResult_e; + +/* + * Run the whole sequence unattended: refuses to start unless the battery is + * disconnected, then holds full throttle, waits for the ESC to power up, and + * times the drop to low itself. The five second window in the manual starts at + * the ESC's tones, which only a person standing there can hear, so timing it + * from the moment the ESC gains power is the only way to hit it without asking + * the operator to type against a stopwatch. + */ +srxl2CalResult_e srxl2MotorCalibrationBegin(void); + +/* Drive one endpoint by hand. For boards that cannot sense battery voltage and + * therefore cannot detect the ESC powering up. */ +srxl2CalResult_e srxl2MotorCalibrationManual(srxl2CalPhase_e phase); + +void srxl2MotorCalibrationAbort(void); +srxl2CalPhase_e srxl2MotorCalibrationPhase(void); + +/* + * Why the last request to start a calibration was refused, or SRXL2_CAL_ACCEPTED + * if it was not. MSP cannot carry a reason back on an IN command, so a caller that + * only saw the command fail could not tell the operator anything useful - and the + * useful thing here is precisely which precondition was not met. + */ +srxl2CalResult_e srxl2MotorCalibrationLastResult(void); + +/* + * Open the serial port assigned FUNCTION_ESC_SRXL2 and start the handshake. + * Returns false if no port is assigned or it could not be opened, in which case + * the caller must fall back to leaving the motors unwritten - this protocol has + * no silent degradation to PWM, because the pin is not a timer output. + */ +bool srxl2MotorInitialize(void); + +/* + * Stage one motor value. Takes microseconds on INAV's usual 1000..2000 scale + * (or the reversible-motor scale, where the neutral sits in the middle), so it + * is interchangeable with pwmWriteMotor() as a motorWritePtr target. + * Staging only: nothing reaches the wire until srxl2MotorSendUpdate(). + */ +void srxl2MotorUpdate(uint8_t index, uint16_t value); + +/* + * Arm or release the Thrust Reverse channel. + * + * Reverse on an Avian is not a sub-neutral throttle value: the ESC's + * "Thrust Rev." parameter selects an auxiliary channel (named CH5..CH9 on a + * Spektrum transmitter) that arms the Reverse Brake, and Brake Type must be set + * to Reverse. So the master has to send that channel too, and which one it is + * has to match how the ESC was programmed. + */ +void srxl2MotorSetReverse(bool armed); + +/* + * Which auxiliary channel arms the Reverse Brake, given as the 1-based channel + * number the ESC is programmed with (the Avian offers CH5..CH9). Zero disables + * reverse entirely. Must match the ESC's "Thrust Rev." setting, because nothing + * on the wire advertises it. + */ +void srxl2MotorSetReverseChannel(uint8_t channel1Based); + +/* + * How often ESC telemetry arrives. Named after the rate the ESC actually delivers, + * not the rate we ask at: an Avian answers roughly two requests in three and + * rotates its reply between three sensors - a text page, a battery page and the + * ESC page - so rpm and current land at about a ninth of the request rate. + * Measured on an Avian 70A: asking 25 times a second yields 2.7 ESC frames a + * second, 10 yields 1.1, 5 yields 0.5, 2 yields 0.2. + * + * The reply shares the throttle wire, so this trades bus headroom against how + * promptly rpm and voltage move - which only matters to the RPM filter, + * everything else being a display. + * + * Nothing slower than 1 Hz is offered, and that is a link-liveness limit rather + * than a taste: the ESC only ever speaks when asked, so the request rate is also + * the rate at which the master learns the ESC is still there. See + * SRXL2_LINK_TIMEOUT_MS. + */ +typedef enum { + /* 1 Hz is first so that it is zero. The setting lives in a field appended to + * motorConfig_t that fell inside existing padding, so a configuration saved + * before it existed loads index 0 rather than the reset default - which must + * therefore be the value we would have chosen anyway, not the fastest one. */ + SRXL2_TELEM_1HZ = 0, + SRXL2_TELEM_3HZ, + SRXL2_TELEM_2HZ, +} srxl2TelemetryRate_e; + +void srxl2MotorSetTelemetryRate(srxl2TelemetryRate_e rate); + +/* + * Emit the staged values as an SRXL2 Control Data packet. Intended to be called + * once per motor update from pwmCompleteMotorUpdate(). + */ +void srxl2MotorSendUpdate(void); + +/* + * Drain the receive buffer and advance the master state machine: handshake, + * baud negotiation, telemetry collection, timeout recovery. Must be called + * from task context, not from an ISR. + * + * Wants roughly a 5 ms cadence: that is the tick Spektrum's own reference + * application advances its state machine with, and it has to run several times + * faster than the Control Data interval for received frames to be picked up + * promptly on a half-duplex wire. TASK_PWMDRIVER, which already exists for the + * SBUS servo output, runs at 200 Hz and fits exactly. + */ +void srxl2MotorProcess(void); + +/* How many ports were found and opened. Fewer than the model has motors means + * some motor has nowhere to send its command, which the caller must treat as a + * configuration error rather than carrying on. */ +uint8_t srxl2MotorCount(void); + +/* True once every opened ESC has answered the handshake and is still + * responding. One silent ESC on a twin is asymmetric thrust, so this is + * deliberately all of them rather than any. */ +bool srxl2MotorIsConnected(void); + +/* + * Latest telemetry for a motor. Returns false if no ESC is bound to that index + * or nothing has been received yet; the caller should not read *out in that + * case. Feeds sensors/esc_sensor.c. + */ +bool srxl2MotorGetTelemetry(uint8_t index, srxl2EscTelemetry_t *out); diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 36f2e02328a..f9ec6dbb4fd 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -59,6 +59,9 @@ typedef enum { FUNCTION_MSP_OSD = (1 << 25), // 33554432 FUNCTION_GIMBAL = (1 << 26), // 67108864 FUNCTION_GIMBAL_HEADTRACKER = (1 << 27), // 134217728 + /* 28 is left free: the Configurator already assigns it to the MassZero thermal + * camera, whose firmware side is not on this branch yet. */ + FUNCTION_ESC_SRXL2 = (1 << 29), // 536870912: Spektrum Smart ESC (Smart Throttle) } serialPortFunction_e; #define FUNCTION_VTX_MSP FUNCTION_MSP_OSD diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 5ab76a23ee4..eadd294a0ab 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -159,3 +159,9 @@ #define MSP2_INAV_WIND 0x2231 #define MSP2_INAV_MAG_UNALIGNED 0x2232 //out message compass reading with zero/gain calibration applied but no alignment rotation; payload: 3x I16 (X,Y,Z) + +// 0x2232 was taken by MSP2_INAV_MAG_UNALIGNED upstream while this branch was +// out of tree, so these moved up rather than the other way round: a released +// command keeps its number, an unmerged one has no claim on it. +#define MSP2_INAV_ESC_SRXL2_STATUS 0x2233 +#define MSP2_INAV_ESC_SRXL2_CALIBRATE 0x2234 diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index a06814a6846..0611dd0ad44 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -43,6 +43,9 @@ #include "flight/mixer.h" #include "drivers/pwm_output.h" #include "sensors/esc_sensor.h" +#include "drivers/pwm_mapping.h" + +#include "io/motor_srxl2.h" #include "io/serial.h" #include "fc/config.h" #include "fc/runtime_config.h" @@ -156,7 +159,17 @@ escSensorData_t NOINLINE * getEscTelemetry(uint8_t esc) escSensorData_t * escSensorGetData(void) { - if (!escSensorPort) { + /* + * Asks whether there is ESC telemetry, not whether a serial port is open. The + * two used to be the same thing, but a Smart ESC reports over the throttle wire + * itself, handled by the motor driver, so escSensorPort stays NULL while the + * data is perfectly good. Testing the port left the OSD, Blackbox, current + * estimation and the telemetry backends with nothing on such a board - and the + * OSD inconsistent with itself, since it checks the state flag and then asks + * here. For a conventional ESC the state is only set once the port has opened, + * so nothing changes there. + */ + if (!STATE(ESC_SENSOR_ENABLED)) { return NULL; } @@ -212,6 +225,30 @@ bool escSensorInitialize(void) return false; } +#ifdef USE_MOTOR_SRXL2 + /* + * An SRXL2 ESC reports telemetry back over the same wire that carries its + * throttle, so there is no separate telemetry port to open. Taking that + * source here rather than anywhere else means every existing consumer - + * the RPM filter, OSD, Blackbox, current estimation - is fed without + * knowing where the numbers came from. + * + * esc_srxl2_telemetry can turn it off, which for a conventional ESC needs no + * setting at all - you leave the port unassigned. This wire has no port to + * leave unassigned, so saying no needs somewhere to say it. + */ + if (motorConfig()->motorPwmProtocol == PWM_TYPE_SRXL2) { + if (!motorConfig()->srxl2Telemetry) { + return false; + } + for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + escSensorData[i].dataAge = ESC_DATA_INVALID; + } + ENABLE_STATE(ESC_SENSOR_ENABLED); + return true; + } +#endif + // FUNCTION_ESCSERIAL is shared between SERIALSHOT and ESC_SENSOR telemetry // They are mutually exclusive serialPortConfig_t * portConfig = findSerialPortConfig(FUNCTION_ESCSERIAL); @@ -235,6 +272,54 @@ bool escSensorInitialize(void) void escSensorUpdate(timeUs_t currentTimeUs) { +#ifdef USE_MOTOR_SRXL2 + if (motorConfig()->motorPwmProtocol == PWM_TYPE_SRXL2) { + /* One ESC per port, so escSensorData[i] belongs to the i-th assigned + * port. Motors past the last assigned port have no telemetry for the + * same reason they have no throttle, and are left invalid. */ + const uint8_t count = MIN(srxl2MotorCount(), (uint8_t)MAX_SUPPORTED_MOTORS); + + for (uint8_t i = 0; i < count; i++) { + srxl2EscTelemetry_t t; + if (srxl2MotorGetTelemetry(i, &t)) { + escSensorData[i].dataAge = 0; + /* + * Only what the frame actually carried. An ESC that reports no + * current sends a "no data" code, and copying that through as + * 0.00 A would be indistinguishable from a motor at rest - which + * is a reading the battery estimate would happily believe. Where + * a field is absent the last known value is left in place and + * ages out with the rest. + */ + if (t.fields & SRXL2_TELEM_FIELD_TEMP_FET) { + escSensorData[i].temperature = t.temperatureFet / 10; /* 0.1 degC -> degC */ + } + if (t.fields & SRXL2_TELEM_FIELD_VOLTAGE) { + escSensorData[i].voltage = t.voltage; /* both 0.01 V */ + } + if (t.fields & SRXL2_TELEM_FIELD_CURRENT) { + escSensorData[i].current = t.current; /* both 0.01 A */ + } + /* + * The wire carries electrical rpm; everything downstream expects + * mechanical, which is what computeRpm() produces for the serial + * backends. Same division, done here because our value is already + * in rpm rather than the LSB units that function takes. + */ + if (t.fields & SRXL2_TELEM_FIELD_RPM) { + const uint8_t poles = motorConfig()->motorPoleCount; + escSensorData[i].rpm = poles ? (t.rpm / (poles / 2)) : 0; + } + } else if (escSensorData[i].dataAge < ESC_DATA_INVALID) { + escSensorData[i].dataAge++; + } + } + + escSensorDataNeedsUpdate = true; + return; + } +#endif + if (!escSensorPort) { return; } diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 7052b29e978..8156bbc8f7a 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -54,6 +54,11 @@ #define USE_UART8 #define SERIAL_PORT_COUNT 8 + +// Opted in explicitly, as any target outside the H7 and AT32 rule in common.h would. Worth +// having here because SITL puts each UART on a TCP port, so a simulated ESC can be attached +// to the real driver and the whole path exercised without hardware +#define USE_MOTOR_SRXL2 #define SITL_SERIAL_TASK_US (500) #define DEFAULT_RX_FEATURE FEATURE_RX_MSP diff --git a/src/main/target/common.h b/src/main/target/common.h index 5386acf7927..bbf7e9062b8 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -74,6 +74,13 @@ #define USE_SERVO_SBUS #endif +// Spektrum Smart ESC, one per spare UART. Default-on only where flash is plentiful: it +// costs about 3.5 KB and AIKONF7 for one already sits at 93.4 %. Any other target can opt +// in with a #define USE_MOTOR_SRXL2 in its own target.h, which is included after this file +#if !defined(USE_MOTOR_SRXL2) && (defined(STM32H7) || defined(AT32F43x)) +#define USE_MOTOR_SRXL2 +#endif + #ifndef USE_ADC_AVERAGING #define USE_ADC_AVERAGING #endif