On a Balance Bot with encoders, on this CCW lap to a circuit:
Applying the Haversine formula to the 32 points in this one lap GPS mission:
Can encoders be used for non-GPS navigation indoors? How?
Is a Lua script the way to go? If so, some example?
yes. By reducing wheel slippage to a minimum.
Maybe. No.
Yes, it should be possible to use the wheel encoders. Whatās missing from the wheel encoder setup page are the instructions to tell people that they must set the EKF origin. This can be done from the mission planner by right-mouse-button-clicking on the Flight Data screenās map, then select āSet Home Hereā and then āSet EKF Origin Hereā
Thanks. So after:
-Right click ā Set Home Here ā Set Home Here
-(no visible indication)
-Right click ā Set Home Here ā Set EKF Origin Here
-(no visible indication)
what to do?
Suppose the Balance Bot at P1=P2 (in the middle of a 3m each side square) with working encoders and compass, but no GPS (indoors):
One thing that another dev found recently was that the SCHED_LOOP_RATE also needed to be increased to 200 (i.e. main loop at 200hz). Iām very surprised by this but thatās what he reported. Itās possible that 100 will also work. Could you try this to see if it helps? If it does I think we will probably just increase the default loop rate for Rover because itās on the to-do list anyway.
I can try it if knowing how to do above square.
The controller is a conventional Pixhawk fmuv3 (I havenāt opened it to see the real main processor).
With some delay, here it is:
(indoor, no GPS mission, wheel encoders)
Female voice: camera.
Male voice: .tlog capture.
ArduRover v4.2.
SCHED_LOOP_RATE=200 (unchanged)
Thatās looking quite good actually. Iām surprised that there is so little drift, nicely done.
I guess the yellow lines for the mission our a bit out-of-date? The vehicle seems to be following a mission it just doesnāt seem to match the yellow lines.
By the way, we found and fixed a bug in the wheel encoder rate calculation (see PR here) and I found that I needed to re-tune my balance bot (PR here) so I wanted to warn you about this so youāre aware when you next upgrade to ālatestā.
I see that
ā¦/ArduPilot/ardupilot/blob/Rover-4.2.1/libraries/AP_WheelEncoder/WheelEncoder_Quadrature.cpp
(v4.2.1)
still contains the error:
irq_state.last_reading_ms = timestamp;
(should be irq_state.last_reading_ms = timestamp * 1e-3f;)
so Iāll try ālatestā shortly.
Considering the corrected line of code in ā¦/ArduPilot/ardupilot/blob/Rover-4.2.1/libraries/AP_WheelEncoder/WheelEncoder_Quadrature.cpp:
irq_state.last_reading_ms = timestamp * 1e-3f;
for an encoder with 500 lines which rotates at 50 rps it is going to add 25000 floating point operations per second (or 50000 if at both edges) per wheel and per signal (two in quadrature), and there at least two or may be four wheels with encoders, and much worse if within interrupts.
Also, I am not sure if member irq_state.last_reading_ms is used in any calculation or is only informative. If so, above is a full waste of time.
So I think the best would be to use Ī¼s:
On a test similar to above with the new code, but travelling the four mission waypoints alternatively CW and CCW this happened:
Here it is (5K):
Note it is a 5120x2880 video. The PIP capture at lower left was done while in the mission and appears as is, so it appears very small.
Video done with ArduRover v4.3dev firmware (dated june 16), so correction mentioned above:
irq_state.last_reading_ms = timestamp * 1e-3f;
is applied. Certainly, I also had to retune the balance bot, but looking if struct member irq_state.last_reading_ms is used in any calculation (grep -r on sources) I couldnāt find anything, so that variable seems only informative and bug is irrelevant (and as said above the aditional float point operation repeated many times may occupy a lot of processing time), unless I am missing something. Retuning need may be due to other reason.
The mission four points are travelled alternatively CW and CCW, so WENC traces should not depart as mentioned above. These are the trajectories:
However (see video end), there is a slight drift.
Pending to test this (wheel encoders navigation) in a real circuit, without RTK.
Far from having a deep knowledge of the code and its compilation, I tested the struct member changed to being understood as Ī¼s:
Correct two occurrences in libraries/AP_WheelEncoder/WheelEncoder_Quadrature.cpp and change to struct member being understood as Ī¼s:
copy_state_to_frontend(ā¦, irq_state.last_reading_us);
irq_state.last_reading_us = timestamp;
Compile and link again:
./waf rover --board=Pixhawk1
Target Text (B) Data (B) BSS (B) Total Flash Used (B) Free Flash (B)
--------------------------------------------------------------------------------
bin/ardurover 1375592 1832 194984 1377424 703344
(24 bytes saved)
Next would be a test to see if the struct member understood as Ī¼s has any side effect (I canāt find it used elsewhere explicitly (grep -r) but I have no deep knowledge of the code workings), and in addition comment the line and test again.
So as seen, 24 bytes have been saved in code, but what is more important is the code execution time saved (not sure if within interrupts), possibly thousands times per second.
A github pull request is a lot better than describing the changes like you did.
Please open a github pull request.
There is already a PR about this. I donāt know the code internals; only think that corrections should minimally consist of only changing a letter (ā¦_ms to ā¦_us).
I also observe that in libraries/AP_WheelEncoder/WheelEncoder_Quadrature.cpp, AP_WheelEncoder_Quadrature::pin_ab_to_phase(ā¦) can be written as:
// convert pin a and pin b state to a wheel encoder phase
uint8_t AP_WheelEncoder_Quadrature::pin_ab_to_phase(bool pin_a, bool pin_b)
{
return (uint8_t)(pin_a?(pin_b?2:3):(pin_b?1:0));
return (uint8_t)pin_a << 1 | (uint8_t)pin_b;
}
more compact and easier to understand; however there are no byte savings.
Apparently, the second return, which is equivalent in logic (seems an alternative), is useless since it will not be reached, but if first return is commented and the second return is used, the code size is reduced in 24 additional bytes. Again: I donāt know the code (perhaps the programmer has a good reason for first alternative), but it is related to quadrature signals, which change thousands of times per second. Code size reduction probably implies faster code, which is important here.
Thanks for the feedback and happy to see that (after some re-tuning) the balance bot is working.
yes, an alternative fix would have been to store and use the timestamp in _us instead of _ms and this might result in better accuracy. Improving the accuracy would be an important improvement, but we shouldnāt worry about the CPU savings. Integer multiplies are almost freeā¦
Iāll put it on my to-do list to try changing the timestamp to use _us to see if it helps. If/when I do that I might produce a binary for you to test with. Txs again.
Something else must have changed in v4.2.1->v4.3dev for the need to retune.
If indeed code added is integer division, writing it as timestamp * 1e-3f (instead of /1000) is confusing. Anyhow, in that case the operation (for which the compiler may generate a library call) is going to be executed many many times per second, perhaps thousands, for both quadrature signals (more for encoders with many lines), for all motor encoders, in interrupt time, so the best is to optimize code (best is no operation at all using Ī¼s).
If you compile it, Iāll be glad to test it: recall the anomaly in figure above (WENC lines divergence going CW/CCW).
For more performance, the call/returns in WheelEncoder_Quadrature.cpp to update_phase_and_error_count() and pin_ab_to_phase(ā¦) can be suppressed:
void AP_WheelEncoder_Quadrature::irq_handler(uint8_t pin,
bool pin_value,
uint32_t timestamp)
{
// sanity check
if (last_pin_a == 0 || last_pin_b == 0) {
return;
}
// update distance and error counts
if (pin == last_pin_a) {
last_pin_a_value = pin_value;
} else if (pin == last_pin_b) {
last_pin_b_value = pin_value;
} else {
return;
};
// update_phase_and_error_count(); SUPPRESSED
// convert pin state before and after to phases
// uint8_t phase_after = pin_ab_to_phase(last_pin_a_value, last_pin_b_value); SUPPRESSED
uint8_t phase_after = (uint8_t)last_pin_a_value << 1 | (uint8_t)last_pin_b_value;
// look for invalid changes
uint8_t step_forward = irq_state.phase < 3 ? irq_state.phase+1 : 0;
uint8_t step_back = irq_state.phase > 0 ? irq_state.phase-1 : 3;
if (phase_after == step_forward) {
irq_state.phase = phase_after;
irq_state.distance_count++;
} else if (phase_after == step_back) {
irq_state.phase = phase_after;
irq_state.distance_count--;
} else {
irq_state.error_count++;
}
irq_state.total_count++;
// record update time
irq_state.last_reading_us = timestamp;
// is irq_state.last_reading_us used elsewhere? If not, COMMENTABLE
}
leaving the irq_handler as a single monolythic function.