AP_Volz_Protocol: use own thread for output #26691
Conversation
| // each channel take about 425.347us to transmit so total time will be ~ number of channels * 450us | ||
| // rounded to 450 to make sure we don't go over the baud rate. |
There was a problem hiding this comment.
I'm not sure where 425 is from, the correct time is 520, see:
https://github.com/IamPete1/ardupilot/blob/d4f30458b8b19865b5a9012a71b123ec16257e08/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp#L89-L91
Double checked with the Saleae:
The round up to 450 and the 1.3 safety factor puts the actual number used as 585.
There was a problem hiding this comment.
That's correct. Typical baudrate for Volz protocol is 115200bps, it makes 10bit*6bytes/115200 = 521us
| // limit the minimum to 2500 will result a max refresh frequency of 400hz. | ||
| if (channels_micros < 2500) { | ||
| channels_micros = 2500; | ||
| } |
There was a problem hiding this comment.
This limit means that with a 400Hz loop rate we miss updates sometime because both are aiming for the same time so a bit of jitter and it will skip a output.
|
|
||
| uint32_t now = AP_HAL::micros(); | ||
| if (now - last_volz_update_time < volz_time_frame_micros || |
There was a problem hiding this comment.
This time check means that if you have enough servos it will only output every other update, which makes this PR have
less delay (although more variation as the thread update rate phases with the main loop rate)
|
|
||
| uint32_t now = AP_HAL::micros(); | ||
| if (now - last_volz_update_time < volz_time_frame_micros || | ||
| port->txspace() < VOLZ_DATA_FRAME_SIZE) { |
There was a problem hiding this comment.
This checks if there is enough space for one command but then sends more than one command.
| // Wait until there is enough space to transmit a full command | ||
| hal.scheduler->delay_microseconds(10); |
There was a problem hiding this comment.
This should really never happen.
|
If possible it would also be nice to update the wiki page: https://ardupilot.org/copter/docs/common-servo-volz.html so if anyone has comments they think should be added, type them here and I'll create a PR. |
|
@IamPete1 I will test on full duplex servos |
|
Rebased. |
|
I have been unable to find anyone else to test this, I think we just have to merge it or close it at this point. |
| // Create thread to handle output | ||
| if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Volz_Protocol::loop, void), | ||
| "Volz", | ||
| 1024, AP_HAL::Scheduler::PRIORITY_RCOUT, 1)) { |
There was a problem hiding this comment.
i'd like to know how much stack space margin we have with this thread
check a log for the STAK messages, in MAVExplorer do "dump STAK"
|
STAK results from hardware testing: |
IamPete1
dismissed
tridge’s stale review
Stack checked, and no mutex required, its the only thing not in the thread is setting of uint16_t PWM values.
Note that I have been testing with half duplex servos (with 3 in the part number). The current driver is probably better for full duplex (with 5 in the part number) because it remains in lock step with the servo outputs. However, there are a few issues with the existing driver, and I don't have any full duplex servos to test it with. This method will still work with full duplex actuators just there will be slightly more delay in some cases.
This tidies up the library a little and moves the output to its own thread, this means each command is sent and then we wait enough time for the reply (which we don't read) to be sent before sending the next command.
The current driver just sends all the commands in a big block which steps on the reply. For four servos set in the bitmask it looks like this:
You will notice slightly random timing and we only occasionally get the reply, highlighted in blue on the RS485 line (my transceiver is in transmit mode all the time so we don't get the reply on the UART).
This results in the servos having quite a bad time, quite steppy sounding (unmute recommended):
media_20240403_211229_2494658397588330632.webm
This PR send the commands one at a time which looks like this:
I only have one servo connected, but it's sending commands for four servos, so we only see the reply on every fourth command.
Nice even spacing and we don't step on the reply. Scrolling through the trace this is the closest it came:
We could leave a longer gap with a bit more head room, but this does seem to work, it depends on how quick the servo is to reply.
That results in a much happier servo:
media_20240404_161546_1469986467495218372.webm
This also adds a new range parameter. This defaults to the current max range of 200 deg (+- 100). Using the full range of 200 deg means you get a 0.2 deg per PWM US. In most cases you need a much smaller range, so end up having to set servo min/max very close to trim. 1400 to 1600 would be +- 20 deg for example. The range param allow you to set 40 deg range and then use 1000 to 2000 PWM for that range. 40 deg means you can get 0.04 deg per PWM US which is slightly lower than the limit of the protocol.
This also changes the behavior of 0 PWM in safe. Currently it would go to -100 deg. This PR changes it to move to the servo trim position. Unfortunately there is no way to power down the servo with the protocol.