zephyr/rims_app/src/phase_modulation.cpp

#include "phase_modulation.hpp"

#include "circular_buffer.hpp"
#include "ctrl.pb.h"
#include "log.hpp"
#include "pb_encode.h"
#include "zephyr.hpp"
#include "zero_cross_detection.hpp"

#include <algorithm>
#include <chrono>
#include <functional>
#include <zephyr/drivers/gpio.h>
#include <zephyr/kernel.h>

namespace rims {

K_FIFO_DEFINE(ctrlIngress);
K_FIFO_DEFINE(ctrlEggress);

zephyr_fifo_buffer<ctrl_IngressMessages, 2> ctrlIngressQueue{ctrlIngress};
zephyr_fifo_buffer<ctrl_EgressMessages, 2>  ctrlEgressQueue{ctrlEggress};

std::array<gpio_dt_spec, Channels> pins = {
    gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(triac_enable_ch1), gpios),
    gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(triac_enable_ch2), gpios)
};

struct PhaseControl {
    int cycles{100};
    PhaseControl() {
    }
};

PhaseControlBase::PhaseControlBase(const gpio_dt_spec &gpio) : _gpio{gpio} {
    ULOG_INFO("triac GPIO %s/%d", gpio.port->name, gpio.pin);
}

void PhaseControlBase::action_triacEnable() const {
    gpio_pin_set_dt(&_gpio, 1);
}

void PhaseControlBase::action_triacDisable() const {
    gpio_pin_set_dt(&_gpio, 0);
}

PhaseModulation::PhaseModulation(const gpio_dt_spec &gpio)
    : PhaseControlBase{gpio}, //
      _triacTimer{[this]() { this->action_triacEnable(); }, std::chrono::milliseconds{0}},
      _off{[this]() { this->action_triacDisable(); }, std::chrono::milliseconds{0}} {
    ULOG_INFO("Started");
}

void PhaseModulation::tickFallingEdge() {
    // run on zero crossing, calculate time from ZCD to triac enable, and set timer accordingly
    /// TODO calculate time needed to achieve given power requirements and start a single shoot timer

    // power
    // auto fullCycleTime = std::chrono::milliseconds{10};
    auto offset = (1000 - int(_power * 10)) * std::chrono::microseconds{10};
    _triacTimer.setInterval(offset);
    _triacTimer.start();
    _off.setInterval(offset + std::chrono::microseconds{100});
    _off.start();
}

PhaseModulationOrchestrator::PhaseModulationOrchestrator()
    : _channel{placement_unique<NoneModulation>(_buffer[0])(), placement_unique<NoneModulation>(_buffer[1])()} {
    _channel[0] = placement_unique<PhaseModulation>(_buffer[0])(pins[0]);
    _channel[1] = placement_unique<PhaseModulation>(_buffer[1])(pins[1]);

    ZeroCrossDetectionEventQueue.k_event_init(_events[0]);
    ctrlIngressQueue.k_event_init(_events[1]);
}

void PhaseModulationOrchestrator::loop() {
    /// TODO power limiter (after power measurement)
    while (1) {
        auto ret = zephyr::event_pool::k_poll_forever(_events);
        if (ret == 0) {
            zephyr::event_pool::k_poll_handle(_events[0], [&]() { event_zeroCrossDetection(); });
            zephyr::event_pool::k_poll_handle(_events[1], [&]() { event_ctrlMessageArrived(); });
        }
    }
}

int PhaseModulationThread::do_hardwarenInit() {
    auto configure = [](const auto &dt_spec) { zephyr::gpio::pin_configure(dt_spec, GPIO_OUTPUT_INACTIVE); };
    std::for_each(pins.begin(), pins.end(), configure);
    return 0;
}

constexpr float PI        = 3.141592f;
constexpr float FREQUENCY = 0.5f; // 2 Hz
constexpr float MIN_VALUE = 10.0f;
constexpr float MAX_VALUE = 25.0f;

float sinewave(std::chrono::steady_clock::time_point timePoint) {
    using namespace std::chrono;
    static auto startTime = steady_clock::now();

    float elapsedSeconds = duration<float>(timePoint - startTime).count();
    float sineValue      = sinf(2.0f * PI * FREQUENCY * elapsedSeconds);

    // return std::sin(2.0 * PI * FREQUENCY * elapsedSeconds);
    return MIN_VALUE + (sineValue + 1.0f) * 0.5f * (MAX_VALUE - MIN_VALUE);
}

void PhaseModulationOrchestrator::event_zeroCrossDetection() {
    auto setPower        = [](auto &channel) { channel->setPower(sinewave(std::chrono::steady_clock::now())); };
    auto tickFallingEdge = [](auto &channel) { channel->tickFallingEdge(); };
    auto tickRisingEdge  = [](auto &channel) { channel->tickRisingEdge(); };

    std::visit(setPower, _channel[0]);
    /// TODO check proper channel
    ZeroCrossDetectionEventQueue.try_consume([&](ZeroCrossDetectionEvent &event) {
        if (event.state) {
            std::visit(tickRisingEdge, _channel[event.channel]);
        } else {
            std::visit(tickFallingEdge, _channel[event.channel]);
        }
    });
}

void PhaseModulationOrchestrator::event_ctrlMessageArrived() {
    ctrlIngressQueue.try_consume([&](const ctrl_IngressMessages &request) {
        ULOG_INFO("control request message handler");
        ctrlEgressQueue.try_produce([&](ctrl_EgressMessages &response) {
            ULOG_INFO("control response message handler");
            switch (request.which_data)
            case ctrl_IngressMessages_manualPowerControlRequest_tag: {
                response.which_data = ctrl_EgressMessages_manualPowerControlResponse_tag;
                handler_manualPowerControlResponse( //
                    request.data.manualPowerControlRequest,
                    response.data.manualPowerControlResponse
                );
                response.requestID = request.requestID;
                break;
            }
                return true;
        });
    });
}

void PhaseModulationOrchestrator::handler_manualPowerControlResponse(const ManualPowerControlRequest &req, ManualPowerControlResponse &resp) {
}

void PhaseModulationThread::threadMain() {
    ULOG_INFO("PhaseModulationThread start");
    /// runs in context of new thread, on new thread stack etc.
    PhaseModulationOrchestrator thread{};
    thread.loop();
}

} // namespace rims