HILTransceiver.h

Go to the documentation of this file.

/* Copyright (c) 2020-2024 Skyward Experimental Rocketry
 * Author: Emilio Corigliano
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */
 
#pragma once
 
#include <ActiveObject.h>
#include <drivers/dma/DMA.h>
#include <drivers/timer/TimestampTimer.h>
#include <drivers/usart/USART.h>
#include <utils/Debug.h>
 
#include "HIL.h"
#include "drivers/usart/USART.h"
 
namespace Boardcore
{
 
template <class FlightPhases, class SimulatorData, class ActuatorData>
class HIL;
 
class HILTransceiverBase : public ActiveObject
{
public:
    explicit HILTransceiverBase(USART& hilSerial) : hilSerial(hilSerial) {}
 
    int getLostUpdates() const { return nLostUpdates; }
 
    int64_t getTimestampSimulatorData() const { return timestampSimulatorData; }
 
protected:
    void waitActuatorData()
    {
        miosix::Lock<miosix::FastMutex> l(mutex);
        while (!updated)
            condVar.wait(l);
        updated = false;
    }
 
    USART& hilSerial;
    bool receivedFirstPacket       = false;
    bool updated                   = false;
    int nLostUpdates               = 0;
    int64_t timestampSimulatorData = 0;  // timestamp of the last received
                                         // simulatorData [ns]
    miosix::FastMutex mutex;
    miosix::ConditionVariable condVar;
    PrintLogger logger = Logging::getLogger("HILTransceiver");
};
 
template <class FlightPhases, class SimulatorData, class ActuatorData>
class HILTransceiver : public HILTransceiverBase
{
public:
    explicit HILTransceiver(USART& hilSerial,
                            HILPhasesManager<FlightPhases, SimulatorData,
                                             ActuatorData>* hilPhasesManager)
        : HILTransceiverBase(hilSerial), actuatorData(),
          hilPhasesManager(hilPhasesManager),
          dmaStreamTx(DMADriver::instance().acquireStreamForPeripheral(
              // TODO: remove hardcoded UART
              DMADefs::Peripherals::PE_UART4_TX, std::chrono::seconds(1)))
    {
        if (dmaStreamTx.isValid())
            hilSerial.getPeripheral()->CR3 |=
                USART_CR3_DMAT;  // Enable DMA transmission
    }
 
    void setActuatorData(ActuatorData actuatorData)
    {
        miosix::Lock<miosix::FastMutex> l(mutex);
 
        // If already updated increment lost updates
        if (updated)
            nLostUpdates++;
 
        this->actuatorData = actuatorData;
        updated            = true;
        condVar.signal();
    }
 
    const SimulatorData* getSensorData() const { return &simulatorData; }
 
    bool isDmaEnabled() { return dmaStreamTx.isValid(); }
 
private:
    void run() override;
 
    SimulatorData simulatorData;
    ActuatorData actuatorData;
    HILPhasesManager<FlightPhases, SimulatorData, ActuatorData>*
        hilPhasesManager;
 
    bool writeDma(void* buffer, uint16_t nBytes,
                  std::chrono::nanoseconds timeout);
 
    DMAStreamGuard dmaStreamTx;
};
 
template <class FlightPhases, class SimulatorData, class ActuatorData>
void HILTransceiver<FlightPhases, SimulatorData, ActuatorData>::run()
{
    LOG_INFO(logger, "HIL Transceiver started");
    hilSerial.clearQueue();
 
    miosix::led2On();
    hilSerial.write(&actuatorData, sizeof(ActuatorData));
    miosix::led2Off();
 
    while (!shouldStop())
    {
        // Pausing the kernel in order to copy the data in the shared structure
        {
            SimulatorData tempData;
            nLostUpdates = 0;
            miosix::led3On();
            size_t nRead = 0;
            if (!hilSerial.readBlocking(&tempData, sizeof(SimulatorData),
                                        nRead))
            {
                LOG_ERR(logger, "Failed serial read");
            }
 
            assert(nRead == sizeof(SimulatorData) &&
                   "Read less then SimulatorData bytes");
 
            hilSerial.clearQueue();
            miosix::led3Off();
 
            miosix::PauseKernelLock kLock;
            simulatorData          = tempData;
            timestampSimulatorData = miosix::getTime();
        }
 
        // If this is the first packet to be received, then update the flight
        // phase manager
        if (!receivedFirstPacket)
        {
            receivedFirstPacket = true;
            hilPhasesManager->simulationStarted();
        }
 
        // Trigger events relative to the flight phases
        hilPhasesManager->processFlags(simulatorData);
 
        if (nLostUpdates > 0)
        {
            // This means also that the number of samples used for the mean sent
            // to the HIL simulator is made up of more than the number of
            // samples we though
            LOG_WARN(logger, "{} lost updates", nLostUpdates);
        }
 
        waitActuatorData();
        miosix::led2On();
 
        if (isDmaEnabled())
            writeDma(&actuatorData, sizeof(ActuatorData),
                     std::chrono::milliseconds(100));
        else
            hilSerial.write(&actuatorData, sizeof(ActuatorData));
 
        miosix::led2Off();
    }
}
 
template <class FlightPhases, class SimulatorData, class ActuatorData>
bool HILTransceiver<FlightPhases, SimulatorData, ActuatorData>::writeDma(
    void* buffer, uint16_t nBytes, std::chrono::nanoseconds timeout)
{
    USARTType* usart = hilSerial.getPeripheral();
 
    DMATransaction setup{
        .direction         = DMATransaction::Direction::MEM_TO_PER,
        .priority          = DMATransaction::Priority::VERY_HIGH,
        .srcSize           = DMATransaction::DataSize::BITS_8,
        .dstSize           = DMATransaction::DataSize::BITS_8,
        .srcAddress        = buffer,
        .dstAddress        = (void*)&(usart->TDR),
        .numberOfDataItems = nBytes,
        .srcIncrement      = true,
        .dstIncrement      = false,
        .enableTransferCompleteInterrupt = true,
        .enableTransferErrorInterrupt    = true,
    };
 
    dmaStreamTx->setup(setup);
 
    // Clear the TC flag in the USART_ISR register by setting
    // the TCCF bit in the USART_ICR register
    usart->ICR |= USART_ICR_TCCF;
 
    dmaStreamTx->enable();
    bool ret = dmaStreamTx->timedWaitForTransferComplete(timeout);
 
    /*
    The TC flag can be monitored to make sure that the USART
    communication is complete. This is required to avoid corrupting the last
    transmission before disabling the USART or entering Stop mode. Software must
    wait until TC=1. The TC flag remains cleared during all data transfers and
    it is set by hardware at the end of transmission of the last frame.
     */
    while ((usart->ISR & USART_ISR_TC) == 0)
    {
    }
 
    return ret;
}
}  // namespace Boardcore