Non-GPS navigation indoors with encoders: possible?

On a Balance Bot with encoders, on this CCW lap to a circuit:


the WENC distances obtained are, as shown:
Dist0 (left (inner)): 129m.
Dist1 (right (outer)): 132m.

Applying the Haversine formula to the 32 points in this one lap GPS mission:


the result is 132.84m, so the system is very precise.

Can encoders be used for non-GPS navigation indoors? How?
Is a Lua script the way to go? If so, some example?

1 Like

yes. By reducing wheel slippage to a minimum.
Maybe. No.

1 Like

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ā€

1 Like

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):


with any orientation, we want:
-Point north.
-Advance 1.5m.
-Point west.
-Advance 3m
-Point south.
-Advance 3m.
-Point east.
-Advance 3m.
-Point north.
-Advance 1.5m, reaching P2.

@Webillo,

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)

@Webillo,

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.

1 Like

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:

  • rename the struct member to irq_state.last_reading_us;
  • leave above line as irq_state.last_reading_us = timestamp; (or comment it and see if there is any anomaly).

On a test similar to above with the new code, but travelling the four mission waypoints alternatively CW and CCW this happened:


The difference between traces is significant, all data for both wheels are the same, and I canā€™t find any diameter difference. I will exchange both wheels and see what happens, but I wonder if this anomaly is related to the additional floating point operation.

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:
20220616_Barbie-12_72_KMZ
However (see video end), there is a slight drift.

Pending to test this (wheel encoders navigation) in a real circuit, without RTK.

1 Like

Far from having a deep knowledge of the code and its compilation, I tested the struct member changed to being understood as Ī¼s:

  1. Check compilation and linking with irq_state.last_reading_ms = timestamp * 1e-3f; in libraries/AP_WheelEncoder/WheelEncoder_Quadrature.cpp:
    ./waf configure --board Pixhawk1
    ./waf rover --board=Pixhawk1
    resulting:
    Target Text (B) Data (B) BSS (B) Total Flash Used (B) Free Flash (B)
    --------------------------------------------------------------------------------
    bin/ardurover 1375616 1832 194984 1377448 703320
  2. In libraries/AP_WheelEncoder/WheelEncoder_Quadrature.h change member declaration and compile:
  • Change to uint32_t last_reading_us;
  • ./waf rover --board=Pixhawk1
    Error indicated:
    [471/771] Compiling libraries/AP_WheelEncoder/AP_WheelEncoder.cpp
    ā€¦/ā€¦/libraries/AP_WheelEncoder/WheelEncoder_Quadrature.cpp: In member function ā€˜virtual void AP_WheelEncoder_Quadrature::update()ā€™:
    ā€¦/ā€¦/libraries/AP_WheelEncoder/WheelEncoder_Quadrature.cpp:72:38: error: ā€˜struct AP_WheelEncoder_Quadrature::IrqStateā€™ has no member named ā€˜last_reading_msā€™; did you mean ā€˜last_reading_usā€™?
    72 | irq_state.last_reading_ms);
    | ^~~~~~~~~~~~~~~
    | last_reading_us
  1. 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;

  2. 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)

  3. 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.

@Webillo,

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.

Please, see this video (4K, from here), with v4.2.3rc1, without GPS, using wheel encoders for guidance.