Lock-in

[matthuszagh]

I'm closing #173 and opening this PR, since I'm not keeping any commits from that PR.

Usage

The first step is to initialize a Lockin instance with Lockin::new(). This provides the lock-in algorithms with necessary information about the demodulation and filtering steps, such as whether to demodulate with a harmonic of the reference signal and the IIR biquad filter to use. There are then 4 different processing blocks that can be used (these blocks are mutually independent):

• demodulate - Computes the phase of the demodulation signal corresponding to each ADC sample, uses this phase to compute the in-phase and quadrature demodulation signals, and multiplies these demodulation signals by the ADC-sampled signal. This is a method of Lockin since it requires information about how to modify the reference signal for demodulation.
• filter - Performs IIR biquad filtering of in-phase and quadrature signals. This is commonly performed on the in-phase and quadrature components provided by the demodulation step, but can be performed at any other point in the processing chain or omitted entirely. filter is a method of Lockin since it must hold onto the filter configuration and state.
• decimate - This decimates the in-phase and quadrature signals to reduce the load on the DAC output. It does not require any state information and is therefore a normal function (i.e., non-method).
• magnitude_phase - Computes the magnitude and phase of the component of the ADC-sampled signal whose frequency is equal to the demodulation frequency. This does not require any state information and is therefore a normal function.

Considerations

• As discussed, this uses real (i.e., non-integral) phase values and libm. Since libm's sincosf uses f64, it is not currently usable on stabilizer, which only supports f32. I will open a separate issue investigating alternatives.

[ryan-summers]

Some minor changes, but in general the code looks much better!

[matthuszagh]

Thanks for the review @ryan-summers! I've made the changes. Let me know if you find anything else.

[ryan-summers]

LGTM from a rust perspective. Will need to defer to @jordens on the math aspect of things

[ryan-summers]

It's not clear where magnitude and phase are stored. Which magnitude and which phase?

I think this would be simpler if you took ownership fo the in_phase and quadrature arrays and then returned them with the new values as (magnitude, phase) arrays. It would still be in-place updates, but ownership would make it clear that the data has transformed.

E.g.

let (i, q) = //...;
let (mag, phase) = dsp::lockin::magnitude_phase(i, q);

[matthuszagh]

The difficulty with this is that this could be performed on arrays of various sizes. For instance, magnitude_phase might be called after decimate. Alternatively decimate might be omitted. At least, that's my understanding of how the API can be used, though I could be wrong.

[matthuszagh]

Based on Robert's feedback, it looks like this will go away anyway.

[jordens]

I'd suggest a few changes:

1. Use (num-complex) or create a Complex<f32> type. Arrays/slices/iterators over it come automatically.
2. Use one IIR filter for both quadratures.
3. Let's try boiling down the iterators further. Being generic over iterators is probably going to be a bit hairy. Let's not try it. Maybe with the changes above the functions become so small that just chaining iterators in user code is actually the easiest and most flexible way. Could you give something like this a try (just a suggestion, untested, maybe some of these don't work)?

// feed timestamps into pll
timestamps.map(|t| pll.update(&mut pll_state, t)).last();
let last_phase = adc_samples.iter()
    .enumerate()
    .map(|j, x| {
        let phi = pll.get_sample_phase(j);
        let (cos_phi, sin_phi) = cossinf(phi);
        // demodulate
        let (xcp, xsp) = (x*cos_phi, x*sin_phi);
        // filter
        let i = iir.update(&mut iir_state.0, xcp);
        let q = iir.update(&mut iir_state.1, xsp);
        let wraps = unwrap_phase((i, q), &mut wrap_state);  // The processing should later also handle phase unwrapping (which needs two subsequent samples and a state). That unwrapping can and should be done before decimate, maybe even before filtering.
        ((i, q), wraps)
    })
    .step_by(n_k)  // decimate
    .map(|(i, q), wraps| atan2f(q, i) + wraps)  // compute the phase
    .last();  // just look at the last phase in this case
dac_samples.iter_mut()
    .enumerate()
    .for_each(|j, d| *d = last_phase + modulate_waveform[j]);  // update the DAC samples with both the feedback phase and the modulation waveform

[jordens]

We should just merge in-phase and quadrature into one type. I.e. [(f32, f32); ADC_SAMPLE_BUFFER_SIZE]. And better yet, try returning the iterator (ready to be consumed) instead of storing.

[jordens]

It's probably a bit rare that both magnitude and phase are needed in this way. Let's split this and look for the most useful abstraction.

Let's do a crate-wide:
type Complex<T> = (T, T);
And then (wit data: &[Complex<f32>]: data.iter().map(|(i, q)| sqrtf(i*i + q*q)) and data.iter().map(|(i, q)| atan2f(q, i)) in the user code when needed. It's a one-liner and doesn't really need a wrapper to iterate over a slice.
(Or maybe even num-complex).
In these closures it's fine and very readable to use single-letter variables.

[matthuszagh]

Is the goal to hold onto/pass around the iterator? I don't believe it's possible to collect() into an array.

[jordens]

As mentioned above, passing iterators around might be tricky because the types become a bit complicated.
Let's just provide the building blocks for single-sample processing and then have the user use iterators in process() (the ISR in main.rs) where and when convenient. Like #177 (review)

[jordens]

If we simplify this further, we can just do data.step_by(n_k) in user code (data: &[Complex<f32>] or an Iter over it, see below).

[matthuszagh]

@jordens I've implemented most of your suggested revisions. Currently, however, I'm still using arrays rather than iterators as function arguments and return values. Moreover, I haven't provided a user code solution that chains iterators. The reason for this is that computing the demodulation phase for each ADC sample by using the timestamps closest to the ADC sample complicates the demodulation logic somewhat, and the required user code would be somewhat lengthy. In other words, although some of the changes did decrease the amount of code in each function (e.g., using arrays of complex values instead of individual f32 arrays), computing the ADC phases offset these decreases. It's worth noting that demodulate is the only non-trivial function at this point. I've also left magnitude_phase in the code for the time being. This can, of course, easily be removed.

I expect that it will be worth revisiting the idea of chaining iterators in user code when the PLL is implemented.

I've used a custom Complex implementation instead of num-complex. num-complex consists of a type-generic struct with re and im fields. It additionally provides many functions frequently used with complex numbers, which are either provided by std or libm. Whether we should use num-complex depends on how we decide to compute the trig functions (sincos and atan2) and the sqrt function. If we use floating point implementations contributed to libm, then it would probably make sense to use num-complex. Currently, though, the custom implementation is trivial (it is simply pub type Complex<T> = (T, T);). I felt it made sense to wait for a better understanding of these trig functions before adding the num-complex dependency.

[jordens]

On top of the previous changes and the ones pending with the PLL code, a couple things.

[jordens]

@jordens I've implemented most of your suggested revisions. Currently, however, I'm still using arrays rather than iterators as function arguments and return values. Moreover, I haven't provided a user code solution that chains iterators. The reason for this is that computing the demodulation phase for each ADC sample by using the timestamps closest to the ADC sample complicates the demodulation logic somewhat, and the required user code would be somewhat lengthy. In other words, although some of the changes did decrease the amount of code in each function (e.g., using arrays of complex values instead of individual f32 arrays), computing the ADC phases offset these decreases. It's worth noting that demodulate is the only non-trivial function at this point. I've also left magnitude_phase in the code for the time being. This can, of course, easily be removed.

Ack. It's typically very good to reduce the amount of code. Remove trivial functions and reduce the interface of the non-trivial ones to make the universal.
Then with the PLL now this can be solved. Something like #177 (review) should be possible verbatim in process(). The get_sample_phase() will use the batch size and the frequency/phase from the pll update().

I think we will also want to use the existing IIR instances in main.rs to do the filtering of the demodulated signal (and not have IIRs elsewhere in the demodulation functions). They serve the same purpose AFAICS. Also for that reason we need to elevate the code to process().

I expect that it will be worth revisiting the idea of chaining iterators in user code when the PLL is implemented.

Yes.

I've used a custom Complex implementation instead of num-complex. num-complex consists of a type-generic struct with re and im fields. It additionally provides many functions frequently used with complex numbers, which are either provided by std or libm. Whether we should use num-complex depends on how we decide to compute the trig functions (sincos and atan2) and the sqrt function. If we use floating point implementations contributed to libm, then it would probably make sense to use num-complex. Currently, though, the custom implementation is trivial (it is simply pub type Complex<T> = (T, T);). I felt it made sense to wait for a better understanding of these trig functions before adding the num-complex dependency.

Ack. Sounds good!

[jordens]

bors r+

[jordens]

bors r+

[jordens]

Bors r+

[jordens]

Bors retry

[bors]

Already running a review

[jordens]

bors cancel

[jordens]

bors retry