|
rcbench
Servo and ESC test bench: panel, IOmcu and shared C
|
The panel's half of the SERVO screen on the link (servo_link.h). More...
#include "servo_link.h"#include <stdatomic.h>#include <stddef.h>#include <string.h>#include "bind_link.h"#include "link_pages.h"#include "outputs.h"#include "servo_page.h"Macros | |
| #define | SERVO_HOLD_MS 100u |
| The servo bench's output is whichever channels the operator bound as surfaces, and this file names no pin. | |
| #define | SERVO_MIN_US 1000u |
| #define | SERVO_MAX_US 2000u |
| #define | SERVO_BIND_HZ 50u |
Enumerations | |
| enum | rate_result_t { RATE_LANDED , RATE_REFUSED , RATE_NO_ANSWER } |
| What became of a rate written: the pins run at it, the binding will not have it – final until something changes – or nothing answered. | |
Functions | |
| static uint32_t | sl_now_ms (void) |
| The callbacks, each called from one function of its own. | |
| static bool | sl_armed (void) |
| Whether the output bank is armed, through the armed callback. | |
| static bool | sl_stopped (void) |
| Whether a stop was applied or a disarm asked for, through the stopped callback. | |
| static void | sl_alert (servo_link_alert_t a) |
| Raise an alert for the operator, through the alert callback. | |
| static void | sl_disarm (bool link_up) |
| Disarm the bench, through the disarm callback. | |
| static uint32_t | sl_stop_count (void) |
| The arming policy's count of stops, through the stop_count callback. | |
| static void | sl_arm_surfaces (void) |
| Ask the arming policy to arm the surfaces and the programming ports, and no motor, through the arm_surfaces callback. | |
| static bool | write_regs (uint8_t page, uint8_t offset, uint8_t count, const uint16_t *regs, link_msg_t *reply) |
| A write of any width goes out one frame per exchange, each acknowledged before the next is sent (link_write_acked()). | |
| static uint16_t | us_to_span (uint16_t us, uint16_t min_us, uint16_t max_us) |
| A pulse in microseconds as a proportion of the servo's own travel, which is what the CHANNELS page carries. | |
| void | servo_link_surfaces_publish (void) |
| For the screen's SWEEP and its horn: a programming port as target, or a binding read with a surface in it (servo_link_surfaces()). | |
| static bool | servo_cfg_write (uint8_t port, uint8_t first, uint8_t count, const uint16_t *cfg, link_msg_t *reply) |
One run's range and speed, to the standard outputs' CHAN_CFG or, with programming port port (1 + n) as target, to that port's registers. | |
| static bool | servo_cmd_write (uint8_t port, uint8_t first, uint8_t count, const uint16_t *cmd, link_msg_t *reply) |
| And its command, 0 to LINK_CH_SPAN: CHANNELS, or the port's CMD. | |
| static uint8_t | servo_run_bits (uint8_t first, uint8_t count) |
| The channels of one run, as bits. | |
| static bool | servo_next_run (uint8_t mask, uint8_t from, uint8_t *first, uint8_t *count) |
The next run of set channels in mask at or after from. | |
| static bool | servo_countermanded (void) |
| Whether the operator has asked, since this began, for the thing being written to stop: a stop applied by the pump, or a disarm or release posted while an exchange was on the wire. | |
| static bool | servo_named (const servo_cmd_t *sv) |
| Whether a command names a range the far end takes. | |
| static void | servo_cfg (const servo_cmd_t *sv, uint16_t *cfg) |
The CHAN_CFG page for the surfaces under sv's range and speed. | |
| static void | servo_resume_taken (uint16_t start_seq) |
| The held sweep's resume has been taken: its repeats say the curve, and publish no further resumed start. | |
| static bool | write_servo (const servo_cmd_t sv) |
| One servo command, as configuration and pulse. | |
| static void | servo_rate_show (servo_rate_state_t st, uint16_t hz) |
| Publish the rate's state and Hz for servo_link_rate_shown(). | |
| static rate_result_t | write_servo_rate (uint16_t hz) |
| Write a frame rate to the SERVO page, and record what became of it. | |
| static uint16_t | servo_rate_now (void) |
| The rate the PWM target runs at as far as this end knows, or SERVO_HZ_UNKNOWN. | |
| static bool | write_servo_and_rate (const servo_cmd_t sv) |
| One position with its rate. | |
| void | servo_link_rate_reset_note (bool page, bool landed) |
What a rate reset left behind: page whether the IOmcu has the SERVO page, landed whether the reset was acknowledged. | |
| static bool | servo_rate_reset (void) |
| Put each slot back at its own rate: 0 written to the SERVO page's frame rate, or nothing to write when the IOmcu has no such page. | |
| bool | servo_link_rate_settled (void) |
| Whether the rate the surfaces run at is known, after one more reset when it is not. Asked before every arm. | |
| void | servo_link_apply (const servo_cmd_t sv, bool link_up, uint32_t stops) |
One command from the screen, with the link state link_up and the stop count stops its sender had seen. | |
| void | servo_link_let_go (void) |
| Stop holding the servo. | |
| void | servo_link_service (bool link_up) |
| Every pass: settle a rate not known while a release is owed or the bank is armed, pay an owed release, and say a held position again once SERVO_HOLD_MS (100ms) has passed. | |
| void | servo_link_init (const servo_link_ctx_t *ctx) |
Start over with ctx: nothing held, no release owed, no binding read, the rate not known, no sweep. | |
| void | servo_link_came_up (uint16_t minor) |
An IOmcu of protocol minor minor started answering. | |
| void | servo_link_lost (void) |
| The link went down: a sweep or a hold held to be said again is dropped (servo_cmd_survives_link_loss()). | |
| void | servo_link_binding (uint8_t channels, bool known) |
The binding read back: channels the surfaces' channel mask, bit n channel n, and known whether the pages read. | |
| void | servo_link_forget_binding (void) |
| A binding was written: the mask is unknown until the next read. | |
| bool | servo_link_binding_known (void) |
| Whether a binding has been read since the last servo_link_init(), servo_link_forget_binding() or unknown servo_link_binding(). | |
| void | servo_link_prog_service (void) |
| The SERVO screen's half of the programming ports' service, after the page's read: a position still out is let go first while the bench is at rest and a target change is pending, then at most one write towards what the screen wants (prog_link_next()), a pulse that comes on preceded by the range the screen names; then what a TARGET change means for the rate (prog_link_rate()). | |
| bool | servo_link_release_owed (void) |
| Whether a release is owed: a surface may still hold a position. | |
| uint16_t | servo_link_hz_sent (void) |
| The rate the IOmcu acknowledged, 0 for each slot's own, or SERVO_HZ_UNKNOWN. | |
| bool | servo_link_take_release (void) |
| A release the screen asked for through servo_link_ask_release(): taken, it drops the position held and owes the release. | |
| void | servo_link_ask_release (void) |
| From the screens' task: a release, or a disarm with one, asked for. A sweep being written gives way to it. | |
| void | servo_link_ask_hold (void) |
| From the screens' task: a HOLD, or a command that ends a sweep, asked for. | |
| unsigned | servo_link_rate_shown (void) |
| The rate's state and its Hz, as (servo_rate_state_t << 16) | Hz. | |
| bool | servo_link_sweep_able (void) |
| Whether the IOmcu can sweep: protocol 4.2 or later. | |
| bool | servo_link_surfaces (void) |
| What servo_link_surfaces_publish() last published. | |
| bool | servo_link_take_sweep_refused (void) |
| A sweep found no surface to sweep, once. | |
| bool | servo_link_take_hold_lost (uint16_t *pause_seq) |
The far end let go of a hold, once, with its pause in pause_seq. | |
| bool | servo_link_take_sweep_held (uint16_t *pause_seq, uint32_t *kept_ms) |
The far end took the HOLD of pause pause_seq and kept its curve kept_ms in, once. | |
| bool | servo_link_take_sweep_start (void) |
| The far end started a sweep, once. | |
| servo_link_start_t | servo_link_sweep_start (void) |
| Read the last sweep start the far end acknowledged. | |
Variables | |
| static const servo_link_ctx_t * | s_ctx |
| The panel, as servo_link_init() handed it over. | |
| static servo_cmd_t | s_servo_held |
| What the servo screen is holding, so it can be said again before the far end times it out. SERVO_CMD_NONE means nothing is being held. | |
| static uint32_t | s_servo_next_ms |
| static bool | s_servo_release_owed |
| A position the far end still holds and this end has finished with. | |
| static atomic_bool | s_servo_release_request |
| static atomic_bool | s_servo_hold_request |
| HOLD asked for: a sweep being written, which can take several exchanges, gives way to it at once. | |
| static uint8_t | s_servo_channels |
| Which channels the horn drives: the ones the binding marks as surfaces. | |
| static atomic_bool | s_servo_surfaces |
| For the screen's SWEEP: a binding read, with a surface in it. | |
| static atomic_bool | s_sweep_refused |
| A sweep command with no surface to sweep: the screen stops waiting. | |
| static servo_pause_end_t | s_pause_ended |
| The pause the panel last let go of: commands asked during it and queued behind the HOLD that ended it are not sent (servo_cmd_stale()). | |
| static bool | s_servo_known |
| Whether that mask is an answer at all. | |
| static uint8_t | s_servo_written |
| And whether the last position actually reached any of them. | |
| static servo_cmd_t | s_servo_range |
| The range and speed the SERVO screen last named. | |
| static bool | s_servo_sweep_page |
| The sweep on the SERVO page (protocol 4.2). | |
| static bool | s_servo_sweeping |
| static bool | s_servo_holding |
| static uint32_t | s_servo_hold_ms |
| static atomic_bool | s_servo_hold_lost |
| static atomic_uint | s_servo_hold_lost_seq |
| static bool | s_servo_sweep_unknown |
| Whether the far end's sweep ability is not known yet. | |
| static atomic_bool | s_servo_sweep_able |
| static uint16_t | s_servo_minor |
| static uint16_t | s_servo_curve [4] |
| The curve the far end last took, and when it started one: a curve that differs from the one running starts over there, and the screen draws the horn from that moment rather than from its tap. | |
| static uint32_t | s_servo_curve_ms |
| static atomic_uint | s_sweep_start_ms |
| static atomic_uint | s_sweep_start_from |
| static atomic_uint | s_sweep_frozen_ms |
| static atomic_bool | s_sweep_start_new |
| static atomic_uint | s_sweep_start_seq |
| static servo_phase_t | s_far_phase |
| The far end's phase as acknowledgements time it, and the HOLD that paused a running sweep: which pause, and the phase it kept. | |
| static bool | s_hold_unanswered |
| A HOLD write that went unanswered: it may have reached the far end, so the phase it kept is not known here when a retry is acknowledged. | |
| static atomic_uint | s_sweep_held_seq |
| static atomic_uint | s_sweep_held_kept |
| static atomic_bool | s_sweep_held_new |
| static bool | s_servo_rate_page |
| static uint16_t | s_servo_hz_sent = SERVO_HZ_UNKNOWN |
| static uint16_t | s_servo_hz_refused |
| static atomic_uint | s_servo_rate_shown |
The panel's half of the SERVO screen on the link (servo_link.h).
The control task is the only caller but for the atomics servo_link.h names: it is the only task that puts anything on the wire.
SPDX-License-Identifier: MIT
| #define SERVO_HOLD_MS 100u |
The servo bench's output is whichever channels the operator bound as surfaces, and this file names no pin.
The wiring is described once, on the OUTPUTS screen, and the horn asks the binding which channels carry it; a pin named here as well would be a second record of the same wiring, and the two would disagree the first time the operator rebound one.
The slots stay the operator's. This screen writes channels and never the OUTPUTS page, so no drag can take a pin away from what it was bound to, and no channel the binding marked a motor is written by a servo horn.
|
static |
The CHAN_CFG page for the surfaces under sv's range and speed.
| sv | The command whose range and speed to use. |
| cfg | Receives LINK_CC_COUNT registers, one entry per channel. |
|
static |
One run's range and speed, to the standard outputs' CHAN_CFG or, with programming port port (1 + n) as target, to that port's registers.
A port is written as channel 0 of cfg.
| port | 0 for the standard outputs, else 1 + the programming port. |
| first | First channel of the run. |
| count | Channels in the run. |
| cfg | The CHAN_CFG page, LINK_CC_COUNT registers. |
| reply | Receives the far end's reply. |
|
static |
And its command, 0 to LINK_CH_SPAN: CHANNELS, or the port's CMD.
| port | 0 for the standard outputs, else 1 + the programming port. |
| first | First channel of the run. |
| count | Channels in the run. |
| cmd | Commands indexed by channel. A port takes its first entry. |
| reply | Receives the far end's reply. |
|
static |
Whether the operator has asked, since this began, for the thing being written to stop: a stop applied by the pump, or a disarm or release posted while an exchange was on the wire.
| void servo_link_apply | ( | const servo_cmd_t | sv, |
| bool | link_up, | ||
| uint32_t | stops ) |
One command from the screen, with the link state link_up and the stop count stops its sender had seen.
ARM centres the surfaces and asks the policy to arm, unless a stop came in between; DISARM and RELEASE centre them; a sweep waits for an owed release to be paid; a command asked during a pause let go of is not sent.
| sv | The command. |
| link_up | Whether the link is up now. |
| stops | The stop count the sender had seen, from the policy's stop_count callback. |
| void servo_link_ask_hold | ( | void | ) |
From the screens' task: a HOLD, or a command that ends a sweep, asked for.
A sweep being written gives way to it; cleared when servo_link_apply() takes the next command.
| void servo_link_binding | ( | uint8_t | channels, |
| bool | known ) |
The binding read back: channels the surfaces' channel mask, bit n channel n, and known whether the pages read.
An unknown binding is not an empty one: no command reaches the standard outputs and a release stays owed until a read succeeds. Publishes servo_link_surfaces().
| channels | Surface channel mask: bit n set when channel n is a surface. |
| known | true when the pages were read; false marks the binding unknown. |
| bool servo_link_binding_known | ( | void | ) |
Whether a binding has been read since the last servo_link_init(), servo_link_forget_binding() or unknown servo_link_binding().
| void servo_link_came_up | ( | uint16_t | minor | ) |
An IOmcu of protocol minor minor started answering.
Whether it has a SERVO rate (4.1), a sweep (4.2) and a resume (4.6); each slot back at its own rate, one exchange; the sweep's state not known until the next command writes a stop.
| minor | The protocol minor the IOmcu reports: 1 for the SERVO rate, 2 for the sweep, 6 for the resume. |
| void servo_link_forget_binding | ( | void | ) |
A binding was written: the mask is unknown until the next read.
Does not publish servo_link_surfaces().
| uint16_t servo_link_hz_sent | ( | void | ) |
The rate the IOmcu acknowledged, 0 for each slot's own, or SERVO_HZ_UNKNOWN.
| void servo_link_init | ( | const servo_link_ctx_t * | ctx | ) |
Start over with ctx: nothing held, no release owed, no binding read, the rate not known, no sweep.
ctx is kept and must outlive the module's use. Before any other call.
| ctx | The panel's callbacks. Kept by pointer, not copied. |
| void servo_link_let_go | ( | void | ) |
Stop holding the servo.
A position written or held leaves a release owed.
| void servo_link_rate_reset_note | ( | bool | page, |
| bool | landed ) |
What a rate reset left behind: page whether the IOmcu has the SERVO page, landed whether the reset was acknowledged.
For the reset a binding's own sequence sends (bind_link_write()) and a TARGET change (prog_link_rate()).
| page | true when the IOmcu has the SERVO page. |
| landed | true when the reset was acknowledged. |
| bool servo_link_rate_settled | ( | void | ) |
Whether the rate the surfaces run at is known, after one more reset when it is not. Asked before every arm.
| unsigned servo_link_rate_shown | ( | void | ) |
The rate's state and its Hz, as (servo_rate_state_t << 16) | Hz.
| bool servo_link_release_owed | ( | void | ) |
Whether a release is owed: a surface may still hold a position.
| void servo_link_service | ( | bool | link_up | ) |
Every pass: settle a rate not known while a release is owed or the bank is armed, pay an owed release, and say a held position again once SERVO_HOLD_MS (100ms) has passed.
Nothing while link_up is false.
| link_up | Whether the link is up now. |
| bool servo_link_surfaces | ( | void | ) |
What servo_link_surfaces_publish() last published.
| bool servo_link_sweep_able | ( | void | ) |
Whether the IOmcu can sweep: protocol 4.2 or later.
| servo_link_start_t servo_link_sweep_start | ( | void | ) |
Read the last sweep start the far end acknowledged.
| bool servo_link_take_hold_lost | ( | uint16_t * | pause_seq | ) |
The far end let go of a hold, once, with its pause in pause_seq.
| pause_seq | Receives the sequence number of the pause. |
| bool servo_link_take_release | ( | void | ) |
A release the screen asked for through servo_link_ask_release(): taken, it drops the position held and owes the release.
True when one was asked for.
| bool servo_link_take_sweep_held | ( | uint16_t * | pause_seq, |
| uint32_t * | kept_ms ) |
The far end took the HOLD of pause pause_seq and kept its curve kept_ms in, once.
| pause_seq | Receives the sequence number of the pause. |
| kept_ms | Receives the phase the curve kept, in milliseconds. |
| bool servo_link_take_sweep_refused | ( | void | ) |
A sweep found no surface to sweep, once.
| bool servo_link_take_sweep_start | ( | void | ) |
The far end started a sweep, once.
Its details: servo_link_sweep_start().
|
static |
Whether a command names a range the far end takes.
A command that names none keeps the standard one.
| sv | The command. |
|
static |
The next run of set channels in mask at or after from.
Runs rather than single channels: a binding's surfaces are contiguous unless a motor sits between them, and each exchange on this wire can wait a second. Returns false when there is no run left.
| mask | Channel mask, bit n is channel n. |
| from | First channel to examine. |
| first | Receives the first channel of the run. |
| count | Receives the number of channels in the run. |
|
static |
The rate the PWM target runs at as far as this end knows, or SERVO_HZ_UNKNOWN.
|
static |
Put each slot back at its own rate: 0 written to the SERVO page's frame rate, or nothing to write when the IOmcu has no such page.
|
static |
Publish the rate's state and Hz for servo_link_rate_shown().
| st | The rate's state. |
| hz | The rate, in Hz. |
|
static |
The held sweep's resume has been taken: its repeats say the curve, and publish no further resumed start.
| start_seq | The start sequence the resume acknowledged. |
|
static |
The channels of one run, as bits.
| first | First channel of the run. |
| count | Channels in the run. |
|
static |
Raise an alert for the operator, through the alert callback.
| a | The alert to raise. |
|
static |
Whether the output bank is armed, through the armed callback.
|
static |
Disarm the bench, through the disarm callback.
| link_up | Whether the link is up, passed on to the callback. |
|
static |
The callbacks, each called from one function of its own.
Not inlined, so tools/stack_check.py follows each by name (SERVO_CALLS).
The port's clock.
|
static |
The arming policy's count of stops, through the stop_count callback.
|
static |
Whether a stop was applied or a disarm asked for, through the stopped callback.
|
static |
A pulse in microseconds as a proportion of the servo's own travel, which is what the CHANNELS page carries.
Clamped, not wrapped, below the range, as the IOmcu's conversion is.
| us | The pulse, in microseconds. |
| min_us | The pulse at the low end of the travel, in microseconds. |
| max_us | The pulse at the high end of the travel, in microseconds. |
max_us is not above min_us or us is at or below min_us.
|
static |
A write of any width goes out one frame per exchange, each acknowledged before the next is sent (link_write_acked()).
| page | The page to write. |
| offset | First register on the page. |
| count | Registers to write. |
| regs | The registers, count of them. |
| reply | Receives the far end's reply: ACK, or the NACK of the first frame it refused. |
reply is then not meaningful.
|
static |
One servo command, as configuration and pulse.
A command for no surface returns true without a write, and a HOLD on an IOmcu that cannot sweep does too.
| sv | The command to send. |
|
static |
One position with its rate.
The pins must never carry a fast rate with the wider pulses of a slower profile: a 760us tail servo going back to STANDARD PWM takes 50Hz first and then 1000 to 2000us, never 1500us at 560Hz. So a rate going up follows the pulse widths, and only once every one of them landed; any other rate leads them, and a position waits for it unless the pins are known to run no faster than it. A rate not known counts as fast. A position held back is asked again with the next hold.
Done only when the rate is settled too: a rate nobody answered leaves a release owed, so it is written again. A refused one is settled – the screen says REFUSED and the pins keep the slower rate – because a release owed for ever would stop the bench arming.
| sv | The position and its frame rate. |
|
static |
Write a frame rate to the SERVO page, and record what became of it.
| hz | The frame rate, in Hz. 0 names no rate and lands at once. |
|
static |
Which channels the horn drives: the ones the binding marks as surfaces.
Written and read by the control task alone, which is the only task that puts anything on the wire, and refreshed from the binding read back (servo_link_binding()).
Zero is a bench with nothing bound as a surface, and then the horn drives nothing. Falling back to a channel number instead is how this screen came to command whatever was bound first, which on a bench with an ESC on the lowest pin is the motor.
|
static |
for the screen: it let go
|
static |
of this pause
|
static |
when the far end last took it
|
static |
HOLD asked for: a sweep being written, which can take several exchanges, gives way to it at once.
Cleared when the drain reaches a servo command, the HOLD itself or one after it.
|
static |
Whether that mask is an answer at all.
Empty and unknown are different benches. Empty is a binding that was read and names no surface – a bench carrying only a motor, which must still arm. Unknown is a binding nobody has read, or one whose write did not come back: the far end may be rendering surfaces this end cannot name, and treating that as empty would let a release report success without settling them and let the arm that follows drive them.
False until a binding has been read from the far end. Nothing this end merely sent counts: a write whose acknowledgement was lost was applied over there all the same.
|
static |
the far end's protocol minor
|
static |
The range and speed the SERVO screen last named.
A release goes to the midpoint of the range the far end holds, which is the one the last position carried; a profile chosen since, with no position under it yet, would be released at the old profile's centre. So a release restates the range first. Kind NONE until a command names one.
|
static |
A position the far end still holds and this end has finished with.
Kept as a debt rather than written and forgotten: a screen left while the link is down cannot send the release, and the far end keeps the channel command through a failsafe. A later arm – from either screen – would then drive the surface to where it was before.
|
static |
for the screen's SWEEP
|
static |
The sweep on the SERVO page (protocol 4.2).
While one runs the IOmcu commands the surfaces itself every pass, so a centre or a position written under it would last one pass: anything but a sweep stops it first. Not known after the link comes up: then the next command, a sweep included, writes a stop first, which also clears a finished sweep that the same curve would otherwise not start again.
|
static |
And whether the last position actually reached any of them.
Separate from s_servo_channels because the two can differ: a write that failed part way through leaves some surfaces holding a position and not others. It says a release is owed, not where the release goes – that is always the channels bound as surfaces now, because a channel that has stopped being one must not be written by this screen.
|
static |
when it froze, for FROZEN
|
static |
|
static |
when the far end's sweep started, ms
|
static |
whether a new start is to be handed to the screen
|
static |
the command the start acknowledges