File Dispatcher.cpp

File List > fw > rbcx-coprocessor > src > Dispatcher.cpp

Go to the documentation of this file.

#include "Dispatcher.hpp"
#include "FreeRTOS.h"
#include "utils/Debug.hpp"

#include "Bsp.hpp"
#include "BuzzerController.hpp"
#include "ControlLink.hpp"
#include "Esp32Manager.hpp"
#include "I2cController.hpp"
#include "MotorController.hpp"
#include "Power.hpp"
#include "SmartServoController.hpp"
#include "StupidServoController.hpp"
#include "UltrasoundController.hpp"
#include "queue.h"
#include "rbcx.pb.h"
#include "utils/QueueWrapper.hpp"

static QueueWrapper<CoprocStat, 64> statusQueue;
static QueueWrapper<CoprocReq, 8> requestQueue;

void dispatcherInit() {
    statusQueue.create();
    requestQueue.create();
}

bool dispatcherEnqueueStatus(const CoprocStat& status) {
    BaseType_t pxHigherPriorityTaskWoken = pdFALSE;
    auto ok = statusQueue.push_back(status, 0, &pxHigherPriorityTaskWoken);
    if (!ok) {
        DEBUG("Status queue overflow\n");
    }
    portYIELD_FROM_ISR(pxHigherPriorityTaskWoken);
    return ok;
}

bool dispatcherEnqueueRequest(const CoprocReq& request) {
    BaseType_t pxHigherPriorityTaskWoken = pdFALSE;
    auto ok = requestQueue.push_back(request, 0, &pxHigherPriorityTaskWoken);
    if (!ok) {
        DEBUG("Request queue overflow\n");
    }
    portYIELD_FROM_ISR(pxHigherPriorityTaskWoken);
    return ok;
}

static void sendButtonsStat() {
    CoprocStat status = {
        .which_payload = CoprocStat_buttonsStat_tag,
    };
    status.payload.buttonsStat.buttonsPressed
        = CoprocStat_ButtonsEnum(getButtons());
    controlLinkTx(status);
}

static void sendVersionStat() {
    CoprocStat status = {
        .which_payload = CoprocStat_versionStat_tag,
    };

    auto& v = status.payload.versionStat;

    static_assert(sizeof(v.revision) == 8);
    static_assert(sizeof(RBCX_VER_REVISION) == 9); // + NULL byte
    memcpy(v.revision, RBCX_VER_REVISION, 8);

    v.dirty = RBCX_VER_DIRTY;
    v.number = RBCX_VER_NUMBER;
    controlLinkTx(status);
}

static void dispatcherProcessReq(const CoprocReq& request) {
    switch (request.which_payload) {
    case CoprocReq_setLeds_tag: {
        setLeds(request.payload.setLeds.ledsOn);
        const CoprocStat status = {
            .which_payload = CoprocStat_ledsStat_tag,
        };
        controlLinkTx(status);
        break;
    }
    case CoprocReq_getButtons_tag:
        sendButtonsStat();
        break;
    case CoprocReq_setStupidServo_tag:
        stupidServoDispatch(request.payload.setStupidServo);
        break;
    case CoprocReq_ultrasoundReq_tag:
        ultrasoundDispatch(request.payload.ultrasoundReq);
        break;
    case CoprocReq_buzzerReq_tag:
        buzzerSetState(request.payload.buzzerReq.on);
        buzzerSetState(request.payload.buzzerReq.on);
        break;
    case CoprocReq_versionReq_tag:
        sendVersionStat();
        break;
    case CoprocReq_calibratePower_tag: {
        const auto& c = request.payload.calibratePower;
        powerCalibrate(c.vccMv, c.battMidMv, c.vRef33Mv, c.temperatureC);
        break;
    }
    case CoprocReq_shutdownPower_tag:
        powerShutDown();
        break;
    case CoprocReq_motorReq_tag:
        motorDispatch(request.payload.motorReq);
        break;
    case CoprocReq_i2cReq_tag:
        i2cDispatch(request.payload.i2cReq);
        break;
    case CoprocReq_espWatchdogSettings_tag:
        sEsp32Manager.handleSettings(request.payload.espWatchdogSettings);
        break;
    case CoprocReq_coprocStartupMessage_tag: {
        const auto& m = request.payload.coprocStartupMessage;
        if (m.getButtons) {
            sendButtonsStat();
        }
        if (m.getVersion) {
            sendVersionStat();
        }
        if (m.getRtc) {
            // TODO RTC not yet implemented
        }
        if (m.has_espWatchdogSettings) {
            sEsp32Manager.handleSettings(m.espWatchdogSettings);
        }
        break;
    }
    case CoprocReq_smartServoReq_tag:
        smartServoSendRequest(request.payload.smartServoReq);
        break;
    }
}

void dispatcherPoll() {
    CoprocReq request;
    CoprocStat status;

    if (controlLinkRx(request)) {
        sEsp32Manager.resetWatchdog();
        dispatcherProcessReq(request);
    }

    if (requestQueue.pop_front(request, 0)) {
        dispatcherProcessReq(request);
    }

    if (statusQueue.pop_front(status, 0)) {
        controlLinkTx(status);
    }
}

void dispatcherReset() {
    requestQueue.reset();
    statusQueue.reset();
}