Using Frequency Modulation to regulate a DC control voltage - FEZ Duino C#

// Using Frequency Modulation to regulate a DC control voltage, with feedback loop
// Copyright (C) muman.ch, 2022.05.29
// info@muman.ch
// All rights reversed

// For details, see: https://muman.ch/muman/index.htm?muman-using-frequency-modulation.htm

// This C# Code runs on a FEZ Duino microcontroller running the TinyCLR operating system
// see https://www.ghielectronics.com/sitcore/sbc/


using System;
using System.Threading;
using GHIElectronics.TinyCLR.Devices.Adc;
using GHIElectronics.TinyCLR.Devices.Pwm;


namespace MattLabs
{
    /* FrequencyToVoltageController

    A frequency with a fixed pulse width from a PWM output (e.g. it's FM not PWM) charges a capacitor 
    through a resistor (e.g. 470K + 220nF). The voltage on the capacitor, or the final output voltage, 
    is fed back via a (16-bit) analog input and is used to regulate the frequency to produce the 
    desired SetPoint voltage.
    
    Full details are here:
    https://muman.ch/muman/index.htm?muman-using-frequency-modulation.htm

    USAGE EXAMPLE
    -------------
        AdcController adcController = AdcController.FromName(SC20100.Adc.Controller1.Id);
        PwmController pwmController = PwmController.FromName(SC20100.Timer.Pwm.Controller2.Id);

        FrequencyToVoltageController f2v = new();

        f2v.Initialize(
            adcController, SC20100.Adc.Controller1.PA4,
            pwmController, SC20100.Timer.Pwm.Controller2.PA3,
            50,             // feedback loop processing frequency in Hz
            0.067,          // calibration constant at 200mV setpoint
            1.974);         // calibration constant at 2.0V setpoint

        f2v.SetPoint = 1.0;
        ...

    SEE ALSO
    --------
        FEZ Duino:            https://www.ghielectronics.com/sitcore/sbc/
        TinyCLR introduction: https://docs.ghielectronics.com/software/tinyclr/intro.html
        32-bit ARM Cortex M7: https://www.st.com/resource/en/datasheet/stm32h743vi.pdf
        
        MosfetController.cs:  Similar code but optimized for buck/boost converter mosfet control
    */

    public class FrequencyToVoltageController
    {
        AdcController adcController;
        AdcChannel adcChannel;
        double adcResolutionInBits;
        PwmController pwmController;
        PwmChannel pwmChannel;

        Timer timer;                // timer which triggers the control loop
        double maxFrequency;        // just under 20kHz with 50uS pulsewidth
        int processPeriodInMs;      // time between processing runs
        object lockObj;             // for multithreading contention resolution

        // FM configuration
        double pulseWidth = 50e-6;              // e.g. 50 microseconds
        double riseAndFallTime = 2e-6;          // e.g. 2 microseconds
        double frequencyMultiplier = 1000.0;    // reduce to prevent overshoot

        // Calibration points, voltages for CalibrationValueLow/High
        double calibPointLow = 0.2;             // 200mV (can't calibrate at zero, it's 0Hz)
        double calibPointHigh = 2.0;            // 2V

        // ADC voltage reference, the voltage when the ADC value is 0xffff
        // normally ~3.3V (+-100mV), derived from the microcontroller's supply voltage
        double adcVoltageReference = 3.3;


        // Measured feedback voltage when SetPoint is at 'calibPointLow' volts
        public double CalibrationValueLow {
            get => this.calibrationValueLow;
            set {
                lock (this.lockObj) {           // synchronizes thread access
                    this.calibrationValueLow = value;
                }
            }
        }
        double calibrationValueLow;

        // Measured feedback voltage when SetPoint is at 'calibPointHigh' volts
        public double CalibrationValueHigh {
            get => this.calibrationValueHigh;
            set {
                lock (this.lockObj) {
                    this.calibrationValueHigh = value;
                }
            }
        }
        double calibrationValueHigh;

        // The desired value of the feedback voltage, range 0 .. ~2.0V
        public double SetPoint {
            get => this.setPoint;
            set {
                lock (this.lockObj) {
                    this.setPoint = value;
                }
            }
        }
        double setPoint;

        // The current calibrated feedback voltage, range 0 .. ~2.0V
        public double FeedbackVoltage {
            get {
                lock (this.lockObj) {
                    return this.feedbackVoltage;
                }
            }
        }
        double feedbackVoltage;


        public void Initialize(
            AdcController adcController,    // ADC for reading the feedback voltage
            int adcChannelNumber,
            PwmController pwmController,    // controls the frequency output
            int pwmChannelNumber,
            int processFrequency,           // feedback loop processing frequency, 1..1000 Hz
            double calibrationValueLow,     // actual feedback voltage when SetPoint is at calibPointLow
            double calibrationValueHigh)    // actual feedback voltage when SetPoint is at calibPointHigh
        {
            Debug.Assert(processFrequency > 0 && processFrequency <= 1000);

            this.lockObj = new();

            this.maxFrequency = (int)(1.0 / (this.pulseWidth + this.riseAndFallTime));
            this.calibrationValueLow = calibrationValueLow == 0.0 ? this.calibPointLow : calibrationValueLow;
            this.calibrationValueHigh = calibrationValueHigh == 0.0 ? this.calibPointHigh : calibrationValueHigh;

            this.adcController = adcController;
            this.adcResolutionInBits = Math.Pow(2.0, this.adcController.ResolutionInBits) - 1;
            this.adcChannel = this.adcController.OpenChannel(adcChannelNumber);

            this.pwmController = pwmController;
            this.pwmChannel = this.pwmController.OpenChannel(pwmChannelNumber);
            this.pwmController.SetDesiredFrequency(0);
            this.pwmChannel.SetActiveDutyCyclePercentage(0);
            this.pwmChannel.Start();

            this.processPeriodInMs = 1000 / processFrequency;
            this.timer = new Timer(timerCallback, null, 0, this.processPeriodInMs);
        }


        // Note: this runs on an event-handler thread, not the same thread as the rest of the code!
        void timerCallback(object state)
        {
            // probably there is just one event-handler thread in TinyCLR (docs do not say)
            // so re-entry can never happen, but we check for it anyway...
            if (this.running != 0) {
                Debug.WriteLine("TimerCallback() re-entry");
                return;
            }
            lock (this.lockObj) {
                try {
                    this.running = 1;

                    // what's the feedback voltage now? range 0 .. ~3.3V
                    double v = (this.adcChannel.ReadValue() * this.adcVoltageReference) / this.adcResolutionInBits;

                    // rescale and calibrate the feedback voltage to the SetPoint voltage range
                    this.feedbackVoltage = rescale(v, this.calibPointLow, this.calibPointHigh,
                        this.calibrationValueLow, this.calibrationValueHigh);

                    // special case for SetPoint <= 0.0, turn PWM off
                    if (this.setPoint <= 0.0) {
                        pwmChannel.SetActiveDutyCyclePercentage(0);
                        pwmController.SetDesiredFrequency(0);
                        return;
                    }

                    // the error in volts, +ve or -ve
                    double err = this.setPoint - this.feedbackVoltage;

                    // calculate the new frequency using 'proportional factor' only
                    // change in frequency = err * this.frequencyMultiplier, +ve or -ve
                    double newFreq = pwmController.ActualFrequency + (err * this.frequencyMultiplier);
                    newFreq = limit(newFreq, 0.0, this.maxFrequency);

                    // keep the pulse width constant
                    double dutyCyclePercentage = limit(this.pulseWidth * newFreq, 0.0, 1.0);

                    // note: there is a delay of several pulses between changing the frequency and
                    // changing the duty cycle, this means that the new frequency runs for a short
                    // time with the old duty cycle...
                    if (err < 0.0) {
                        // frequency decreases, pulse width increases, change frequency first
                        pwmController.SetDesiredFrequency(newFreq);
                        pwmChannel.SetActiveDutyCyclePercentage(dutyCyclePercentage);
                    }
                    else {
                        // frequency increases, pulse width decreases, change pulse width first
                        pwmChannel.SetActiveDutyCyclePercentage(dutyCyclePercentage);
                        pwmController.SetDesiredFrequency(newFreq);
                    }
                }
                finally {
                    this.running = 0;
                }
            }
        }
        volatile byte running;


        // Limit value to range min..max
        double limit(double value, double min, double max)
        {
            return value > max ? max : value < min ? min : value;
        }

        // Rescale from one linear scale to another, e.g. -1..+1 -> 0..2.0
        double rescale(double value, double inmin, double inmax, double outmin, double outmax)
        {
            return outmin + ((value - inmin) * (outmax - outmin) / (inmax - inmin));
        }
    }
}