Test program done

2026-03-20 11:41:44 -07:00
parent 303a554595
commit 1b29371fff
20 changed files with 1037 additions and 114 deletions
--- a/SHAL/Src/STM32H7xx/Peripheral/ADC/SHAL_ADC.cpp
+++ b/SHAL/Src/STM32H7xx/Peripheral/ADC/SHAL_ADC.cpp
@@ -0,0 +1,271 @@
+//
+// Created by Luca on 9/21/2025.
+//
+
+#include "SHAL_ADC.h"
+#include "SHAL_GPIO.h"
+#include "SHAL_UART.h"
+#include <cstdio>
+
+bool SHAL_ADC::isValid() const {
+    if(m_ADCKey == ADC_Key::INVALID || m_ADCKey == ADC_Key::NUM_ADC){
+        return false;
+    }
+    return true;
+}
+
+SHAL_Result SHAL_ADC::init(const ADC_Key key, SHAL_ADC_Sample_Mode mode){
+
+    m_ADCKey = key;
+
+    if(!isValid()){
+        return SHAL_Result::ERROR;
+    }
+
+    auto rcc = getADCRCCEnableRegister(m_ADCKey); //Clock enable
+    auto ccr = getADCCommonControl(m_ADCKey);
+
+    SHAL_apply_bitmask(rcc.reg,rcc.mask); //Enable clock
+
+    wakeFromDeepSleep(); //Wake and enable LDO
+
+    //Configure clock source
+    SHAL_set_bits(ccr.reg,2,static_cast<uint32_t>(SHAL_ADC_Clock_Mode::SYNC_BY_2),ccr.clock_mode_position); //TODO take as param?
+
+    configureResolution(SHAL_ADC_Resolution::B12); //Configure resolution
+    SHAL_apply_bitmask(ccr.reg,ccr.VoltageRefEnable);
+
+    if(calibrate(mode) != SHAL_Result::OKAY){ //Calibrate
+        return SHAL_Result::ERROR;
+    }
+
+    if(enable() != SHAL_Result::OKAY){
+        return SHAL_Result::ERROR;
+    }
+
+    return SHAL_Result::OKAY;
+}
+
+SHAL_Result SHAL_ADC::calibrate(SHAL_ADC_Sample_Mode sampleMode) const {
+    const SHAL_ADC_Control_Reg control_reg = getADCControlReg(m_ADCKey);
+
+    if(disable() != SHAL_Result::OKAY){
+        return SHAL_Result::ERROR;
+    }
+
+    SHAL_set_bits(control_reg.reg, 1, static_cast<uint32_t>(sampleMode), control_reg.sample_mode_offset); //Set sample mode (differential or single ended)
+
+    SHAL_apply_bitmask(control_reg.reg, control_reg.calibration_mask); //Start calibration
+
+    if (!SHAL_wait_for_bit_clear_us(control_reg.reg, control_reg.calibration_mask, 100)) { //Wait for calibration
+        return SHAL_Result::ERROR;
+    }
+
+    return SHAL_Result::OKAY;
+}
+
+uint32_t SHAL_ADC::singleConvertSingle(SHAL_ADC_Channel channel, SHAL_ADC_SampleTime time) const {
+
+    auto data_reg = getADCDataReg(m_ADCKey);
+    auto ISR_reg = getADCISRReg(m_ADCKey);
+
+    auto sampleTimeReg = getADCChannelSamplingTimeRegister(m_ADCKey, channel);
+
+    SHAL_set_bits(sampleTimeReg.reg, 3, static_cast<uint8_t>(time), sampleTimeReg.channel_offset); //Set sample time
+
+    if(setADCSequenceAmount(1) == SHAL_Result::ERROR) { return 0; } //Set sequence amount to 1
+
+    addADCChannelToSequence(channel, 1);
+
+    startConversion();
+
+    if (!SHAL_wait_for_bit_set_us(ISR_reg.reg, ISR_reg.end_of_conversion_mask, 200)) {
+        return 0;
+    }
+
+    uint32_t result = *data_reg.reg;
+
+    return result;
+}
+
+SHAL_Result SHAL_ADC::multiConvertSingle(SHAL_ADC_Channel* channels, int numChannels, uint16_t* result, SHAL_ADC_SampleTime time) {
+    auto data_reg = getADCDataReg(m_ADCKey); //Where our output will be stored
+
+    setADCSequenceAmount(numChannels); //Convert the correct amount of channels
+
+    for(int i = 0; i < numChannels; i++){
+        auto channel = channels[i];
+
+        auto sampleTimeReg = getADCChannelSamplingTimeRegister(m_ADCKey,channel);
+
+        SHAL_set_bits(sampleTimeReg.reg,3,static_cast<uint8_t>(time),sampleTimeReg.channel_offset); //Set sample time register TODO un-hardcode bit width?
+
+        addADCChannelToSequence(channel,i); //Use index 0 to convert channel
+    }
+
+    startConversion(); //Start ADC conversion
+
+    auto ISR_reg = getADCISRReg(m_ADCKey);
+
+    for(int i = 0; i < numChannels; i++) {
+        if (!SHAL_WAIT_FOR_CONDITION_US(((*ISR_reg.reg & ISR_reg.end_of_conversion_mask) != 0),500)) { //Wait for conversion
+            return SHAL_Result::ERROR; //Failed conversion
+        }
+        result[i] = *data_reg.reg;
+    }
+
+    if (!SHAL_WAIT_FOR_CONDITION_US(((*ISR_reg.reg & ISR_reg.end_of_sequence_mask) != 0),500)) { //Wait for conversion
+        return SHAL_Result::ERROR; //Failed sequence
+    }
+
+    return SHAL_Result::OKAY;
+}
+
+SHAL_Result SHAL_ADC::enable() const {
+
+    if(!isValid()){
+        return SHAL_Result::ERROR;
+    }
+
+    const SHAL_ADC_Control_Reg control_reg = getADCControlReg(m_ADCKey);
+    const SHAL_ADC_ISR_Reg ISR_reg = getADCISRReg(m_ADCKey);
+
+    SHAL_apply_bitmask(ISR_reg.reg, ISR_reg.ready_mask); //Clear ready flag (write is correct as per datasheet)
+
+    SHAL_apply_bitmask(control_reg.reg, control_reg.enable_mask); //Enable mask
+
+    if (!SHAL_wait_for_bit_set_us(ISR_reg.reg,ISR_reg.ready_mask,500)) { //Check ADRDY
+        return SHAL_Result::ERROR;
+    }
+
+    return SHAL_Result::OKAY;
+}
+
+/// Disables the ADC
+/// @return
+SHAL_Result SHAL_ADC::disable() const {
+    if(!isValid()){
+        return SHAL_Result::ERROR;
+    }
+
+    const auto control_reg = getADCControlReg(m_ADCKey);
+
+    //Stop any ongoing conversion
+    if (SHAL_check_bit(control_reg.reg, control_reg.start_mask)) {
+        SHAL_apply_bitmask(control_reg.reg, control_reg.stop_mask);
+    }
+
+    if (SHAL_check_bit(control_reg.reg,control_reg.enable_mask)) { //DO NOT disable if the ADC is already disabled
+        SHAL_apply_bitmask(control_reg.reg, control_reg.disable_mask);
+
+        if (!SHAL_wait_for_bit_clear_ms(control_reg.reg,(control_reg.enable_mask | control_reg.disable_mask),50)) {
+            return SHAL_Result::ERROR;
+        }
+    }
+
+    return SHAL_Result::OKAY;
+}
+
+SHAL_Result SHAL_ADC::wakeFromDeepSleep() const {
+
+    const SHAL_ADC_Control_Reg control_reg = getADCControlReg(m_ADCKey); //ADC Control register
+    const SHAL_ADC_ISR_Reg ISR_reg = getADCISRReg(m_ADCKey);
+
+    SHAL_clear_bitmask(control_reg.reg,control_reg.deep_power_down_mask); //Wake ADC from sleep
+
+    SHAL_apply_bitmask(control_reg.reg,control_reg.voltage_regulator_mask);
+
+    if (!SHAL_wait_for_bit_set_us(ISR_reg.reg,ISR_reg.ldo_ready_mask,50)) { //Wait for LDO
+        return SHAL_Result::ERROR;
+    }
+
+    return SHAL_Result::OKAY;
+}
+
+SHAL_Result SHAL_ADC::startConversion() const {
+    auto control_reg = getADCControlReg(m_ADCKey);
+
+    SHAL_apply_bitmask(control_reg.reg,control_reg.start_mask);
+
+    return SHAL_Result::OKAY;
+}
+
+
+SHAL_Result SHAL_ADC::configureResolution(SHAL_ADC_Resolution resolution) const {
+    if(!isValid()){
+        return SHAL_Result::ERROR;
+    }
+
+    const SHAL_ADC_Config_Reg config_reg = getADCConfigReg(m_ADCKey);
+    /*
+    SHAL_set_bits(config_reg.reg,2,static_cast<uint8_t>(resolution),config_reg.resolution_offset);
+
+    char buff[20];
+    snprintf(buff,16,"CFGR: %lu\r\n", (unsigned long)ADC1->CFGR);
+    SHAL_UART3.sendString(buff);
+    */
+
+    SHAL_set_register_value(config_reg.reg, 0);
+
+    return SHAL_Result::OKAY;
+}
+
+SHAL_Result SHAL_ADC::configureAlignment(SHAL_ADC_Alignment alignment) const {
+    if(!isValid()){
+        return SHAL_Result::ERROR;
+    }
+
+    const SHAL_ADC_Config_Reg config_reg = getADCConfigReg(m_ADCKey);
+
+    //TODO check if this needs to be abstracted (Do other platforms have >2 resolution possibilities?
+    SHAL_set_bits(config_reg.reg,1,static_cast<uint8_t>(alignment),config_reg.alignment_offset);
+
+    return SHAL_Result::OKAY;
+}
+
+SHAL_Result SHAL_ADC::preselectChannel(SHAL_ADC_Channel channel) const {
+    auto preselect = getADCPreselectRegister(ADC_Key::S_ADC1, SHAL_ADC_Channel::CH15);
+    SHAL_set_bits(preselect.reg,1,1,preselect.channel_offset);
+
+    return SHAL_Result::OKAY;
+}
+
+SHAL_Result SHAL_ADC::setADCSequenceAmount(uint32_t amount) const {
+    if(!isValid()){return SHAL_Result::ERROR;}
+
+    if(amount == 0){
+        return SHAL_Result::ERROR;
+    }
+
+    SHAL_ADC_Sequence_Amount_Reg sequence_amount_reg = getADCSequenceAmountRegister(m_ADCKey);
+
+    SHAL_set_bits(sequence_amount_reg.reg, 4, amount - 1, 0); //Sequence amount reg is just SQR1 least significant 4 bits, this offset should always be 0
+
+    return SHAL_Result::OKAY;
+}
+
+SHAL_Result SHAL_ADC::addADCChannelToSequence(SHAL_ADC_Channel channel, const uint32_t conversionNumber) const {
+    if(!isValid()) { return SHAL_Result::ERROR; }
+
+    auto sqr = getADCSequenceRegister(m_ADCKey, conversionNumber);
+    auto channelNum = static_cast<uint8_t>(channel);
+
+
+    SHAL_wait_for_bit_clear_ms(&ADC1->CR,ADC_CR_ADSTART,50);
+
+    SHAL_set_bits(sqr.reg, 5, channelNum, sqr.sequence_offset); //Set regular sequence register
+
+    return SHAL_Result::OKAY;
+}
+
+SHAL_ADC &ADCManager::get(ADC_Key key) {
+    return m_ADCs[static_cast<uint8_t>(key)];
+}
+
+SHAL_ADC& ADCManager::getByIndex(int index) {
+
+    if(index < static_cast<int>(ADC_Key::NUM_ADC)){
+        return m_ADCs[index];
+    }
+    return m_ADCs[0];
+}
--- a/SHAL/Src/STM32H7xx/Peripheral/GPIO/SHAL_GPIO.cpp
+++ b/SHAL/Src/STM32H7xx/Peripheral/GPIO/SHAL_GPIO.cpp
@@ -18,44 +18,46 @@ SHAL_GPIO::SHAL_GPIO(GPIO_Key key) : m_GPIO_KEY(key) {
    SHAL_apply_bitmask(GPIORCCEnable.reg,GPIORCCEnable.mask);
 }

-void SHAL_GPIO::setLow() {
+void SHAL_GPIO::setLow() const {
    auto outputDataReg = getGPIOOutputDataRegister(m_GPIO_KEY);
    SHAL_set_bits(outputDataReg.reg,1,0,outputDataReg.offset);
 }

-void SHAL_GPIO::setHigh() {
+void SHAL_GPIO::setHigh() const {
    auto outputDataReg = getGPIOOutputDataRegister(m_GPIO_KEY);
    SHAL_set_bits(outputDataReg.reg,1,1,outputDataReg.offset);
 }

-void SHAL_GPIO::toggle() volatile {
+void SHAL_GPIO::toggle() const volatile {
    auto outputDataReg = getGPIOOutputDataRegister(m_GPIO_KEY);
    SHAL_flip_bits(outputDataReg.reg,1,outputDataReg.offset);
 }

-void SHAL_GPIO::setOutputType(PinType type) volatile {
+void SHAL_GPIO::setOutputType(PinType type) const volatile {
    auto outputTypeReg = getGPIOOutputTypeRegister(m_GPIO_KEY);
    SHAL_set_bits(outputTypeReg.reg,2,static_cast<uint8_t>(type),outputTypeReg.offset);
 }

-void SHAL_GPIO::setOutputSpeed(OutputSpeed speed) volatile {
+void SHAL_GPIO::setOutputSpeed(OutputSpeed speed) const volatile {
    auto outputSpeedReg = getGPIOOutputSpeedRegister(m_GPIO_KEY);
    SHAL_set_bits(outputSpeedReg.reg,2,static_cast<uint8_t>(speed),outputSpeedReg.offset);
 }

-void SHAL_GPIO::setInternalResistor(InternalResistorType type) volatile {
+void SHAL_GPIO::setInternalResistor(InternalResistorType type) const volatile {
    auto pupdreg = getGPIOPUPDRegister(m_GPIO_KEY);
    SHAL_set_bits(pupdreg.reg,2,static_cast<uint8_t>(type),pupdreg.offset);
 }

-void SHAL_GPIO::setAlternateFunction(GPIO_Alternate_Function AF) volatile {
+void SHAL_GPIO::setAlternateFunction(GPIO_Alternate_Function AF) const volatile {
    auto alternateFunctionReg = getGPIOAlternateFunctionRegister(m_GPIO_KEY);
    SHAL_set_bits(alternateFunctionReg.reg,4,static_cast<uint8_t>(AF),alternateFunctionReg.offset);
 }

-SHAL_Result SHAL_GPIO::setPinMode(PinMode mode) volatile {
+SHAL_Result SHAL_GPIO::setPinMode(PinMode mode) const volatile {
    auto pinModeReg = getGPIOModeRegister(m_GPIO_KEY);

+    GPIOManager::initGPIO(m_GPIO_KEY);
+
    /*
    if(mode == PinMode::ANALOG_MODE && getGPIOPortInfo(m_GPIO_KEY).ADCChannel == SHAL_ADC_Channel::NO_ADC_MAPPING){
        char buff[100];
@@ -69,6 +71,12 @@ SHAL_Result SHAL_GPIO::setPinMode(PinMode mode) volatile {
    return SHAL_Result::OKAY;
 }

+uint16_t SHAL_GPIO::digitalRead() const {
+    auto offset = getGPIOPinNumber(m_GPIO_KEY);
+
+    return (SHAL_check_bit(getGPIOInputDataRegister(m_GPIO_KEY).reg, 1 << offset)) ? 1 : 0;
+}
+
 /* TODO Fix implementation for STM32F072
 void SHAL_GPIO::useAsExternalInterrupt(TriggerMode mode, EXTICallback callback) {

@@ -116,10 +124,20 @@ uint16_t SHAL_GPIO::analogRead(SHAL_ADC_SampleTime sampleTime) {
    SHAL_ADC_Channel channel = getGPIOPortInfo(m_GPIO_KEY).ADCChannel;

    return GPIOManager::getGPIOADC().singleConvertSingle(channel,sampleTime);
+    return GPIOManager::getGPIOADC().singleConvertSingle(channel,sampleTime);
 }
 */

-SHAL_GPIO& GPIOManager::get(GPIO_Key key) {
+SHAL_GPIO& GPIOManager::get(const uint8_t portNum, const uint8_t pinNum) {
+
+    uint8_t pinIndex = (PINS_PER_PORT * portNum) + pinNum;
+
+    initGPIO(static_cast<GPIO_Key>(pinIndex));
+
+    return m_gpios[portNum][pinNum];
+}
+
+void GPIOManager::initGPIO(GPIO_Key key) {

    unsigned int gpioPort = getGPIOPortNumber(key);
    uint8_t gpioPin = getGPIOPinNumber(key);
@@ -127,6 +145,12 @@ SHAL_GPIO& GPIOManager::get(GPIO_Key key) {
    if (m_gpios[gpioPort][gpioPin].m_GPIO_KEY == GPIO_Key::INVALID){
        m_gpios[gpioPort][gpioPin] = SHAL_GPIO(key);
    }
-
-    return m_gpios[gpioPort][gpioPin];
+}
+
+
+SHAL_GPIO& GPIOManager::get(const GPIO_Key key) {
+
+    initGPIO(key);
+
+    return m_gpios[getGPIOPortNumber(key)][getGPIOPinNumber(key)];
 }
--- a/SHAL/Src/STM32H7xx/Peripheral/Timer/SHAL_TIM.cpp
+++ b/SHAL/Src/STM32H7xx/Peripheral/Timer/SHAL_TIM.cpp
@@ -14,49 +14,56 @@ Timer::Timer() : m_key(Timer_Key::S_TIM_INVALID){
 }

 void Timer::start() {
+    auto control_reg  = getTimerControlRegister1(m_key);
+    auto event_reg    = getTimerEventGenerationRegister(m_key);
+    auto status_reg   = getTimerStatusRegister(m_key);
+    auto bdtr_reg     = getTimerBreakDeadTimeRegister(m_key);
+    auto rcc_reg      = getTimerRCC(m_key);

-    auto control_reg = getTimerControlRegister1(m_key);
-    auto event_generation_reg = getTimerEventGenerationRegister(m_key);
-    auto status_reg = getTimerStatusRegister(m_key);
-    auto break_time_dead_reg = getTimerBreakDeadTimeRegister(m_key);
+    SHAL_apply_bitmask(event_reg.reg, event_reg.update_generation_mask);
+    SHAL_clear_bitmask(status_reg.reg, status_reg.update_interrupt_flag_mask);

-    auto rcc_reg = getTimerRCC(m_key);
+    SHAL_apply_bitmask(control_reg.reg, control_reg.auto_reload_preload_enable_mask);

-    SHAL_apply_bitmask(control_reg.reg, control_reg.counter_enable_mask); //Enable counter
-    SHAL_apply_bitmask(control_reg.reg, control_reg.auto_reload_preload_enable_mask); //Preload enable (buffer)
-    SHAL_apply_bitmask(event_generation_reg.reg, event_generation_reg.update_generation_mask);
-
-    SHAL_clear_bitmask(status_reg.reg,status_reg.update_interrupt_flag_mask);
-
-    SHAL_apply_bitmask(rcc_reg.reg,rcc_reg.enable_mask);
-    SHAL_apply_bitmask(break_time_dead_reg.reg,break_time_dead_reg.main_output_enable_mask);
+    SHAL_apply_bitmask(rcc_reg.reg, rcc_reg.enable_mask);
+    SHAL_apply_bitmask(control_reg.reg, control_reg.counter_enable_mask);
+    SHAL_apply_bitmask(bdtr_reg.reg, bdtr_reg.main_output_enable_mask);

    enableInterrupt();
 }

 void Timer::stop() const {
-    auto rcc_reg = getTimerRCC(m_key);
+    auto control_reg = getTimerControlRegister1(m_key);

-    SHAL_clear_bitmask(rcc_reg.reg,rcc_reg.enable_mask);
+    SHAL_clear_bitmask(control_reg.reg, control_reg.counter_enable_mask);
+    getTimerRegister(m_key)->CNT = 0;
+
+    // Force an update event to flush shadow registers NOW
+    auto event_reg = getTimerEventGenerationRegister(m_key);
+    SHAL_apply_bitmask(event_reg.reg, event_reg.update_generation_mask);
+
+    auto rcc_reg = getTimerRCC(m_key);
+    SHAL_clear_bitmask(rcc_reg.reg, rcc_reg.enable_mask);
 }

-void Timer::setPrescaler(const uint16_t presc) const {
+void Timer::setPrescaler(const uint32_t presc) const {
    auto prescalerReg = getTimerPrescalerRegister(m_key);

-    SHAL_set_bits(prescalerReg.reg,16,presc,0);
+    SHAL_set_register_value(prescalerReg.reg,presc);
 }

-void Timer::setARR(const uint16_t arr) const {
+void Timer::setARR(const uint32_t arr) const {
    auto autoReloadReg = getTimerAutoReloadRegister(m_key);

-    SHAL_set_bits(autoReloadReg.reg,16,arr,0);}
+    SHAL_set_register_value(autoReloadReg.reg,arr);
+}

 void Timer::enableInterrupt() {
    getTimerRegister(m_key)->DIER |= TIM_DIER_UIE;
    NVIC_EnableIRQ(getIRQn(m_key));
 }

-void Timer::init(uint16_t prescaler, uint16_t autoReload) {
+void Timer::init(uint32_t prescaler, uint32_t autoReload) {
    SHAL_TIM_RCC_Register rcc = getTimerRCC(m_key);

    SHAL_apply_bitmask(rcc.reg,rcc.enable_mask);
@@ -65,18 +72,23 @@ void Timer::init(uint16_t prescaler, uint16_t autoReload) {
    setARR(autoReload);
 }

-void Timer::configurePWM(SHAL_Timer_Channel channel, uint16_t prescaler, uint16_t autoReload, uint16_t captureCompareThreshold) {
+void Timer::init() const {
+    SHAL_TIM_RCC_Register rcc = getTimerRCC(m_key);
+
+    SHAL_apply_bitmask(rcc.reg,rcc.enable_mask);
+}
+
+void Timer::configurePWM(SHAL_Timer_Channel channel, uint32_t prescaler, uint32_t autoReload, uint32_t captureCompareThreshold) {

    setPrescaler(prescaler);
    setARR(autoReload);
+    setCaptureCompareValue(channel, captureCompareThreshold);

    setOutputCompareMode(channel, SHAL_TIM_Output_Compare_Mode::PWMMode1);
    enableChannel(channel,SHAL_Timer_Channel_Main_Output_Mode::Polarity_Normal,SHAL_Timer_Channel_Complimentary_Output_Mode::Disabled);
-
-    setCaptureCompareValue(channel, captureCompareThreshold);
 }

-void Timer::configureOneshot(SHAL_Timer_Channel channel, uint16_t prescaler, uint16_t autoReload, uint16_t captureCompareThreshold) {
+void Timer::configureOneshot(SHAL_Timer_Channel channel, uint32_t prescaler, uint32_t autoReload, uint32_t captureCompareThreshold) {

    setPrescaler(prescaler);
    setARR(autoReload);
@@ -111,13 +123,13 @@ void Timer::enableChannel(SHAL_Timer_Channel channel, SHAL_Timer_Channel_Main_Ou
    setValue |= (static_cast<uint8_t>(mainOutputMode) << ((channelNum - 1) * channelStride)); //xxBB shifted by c - 1
    setValue |= (static_cast<uint8_t>(complimentaryOutputMode) << (((channelNum - 1) * channelStride) + 2)); //BBxx shifted by c - 1

-    SHAL_set_bits(captureCompareEnableReg.reg,16,setValue,0);
+    SHAL_set_register_value(captureCompareEnableReg.reg,setValue);
 }

-void Timer::setCaptureCompareValue(SHAL_Timer_Channel channel, uint16_t value) {
+void Timer::setCaptureCompareValue(SHAL_Timer_Channel channel, uint32_t value) const {
    auto captureCompareReg = getTimerCaptureCompareRegister(m_key,channel);

-    SHAL_set_bits(captureCompareReg.reg,16,value,0);
+    SHAL_set_register_value(captureCompareReg.reg,value);
 }


--- a/SHAL/Src/STM32H7xx/Peripheral/UART/SHAL_UART.cpp
+++ b/SHAL/Src/STM32H7xx/Peripheral/UART/SHAL_UART.cpp
@@ -45,6 +45,9 @@ void SHAL_UART::begin(uint32_t baudRate, SHAL_USART_Word_Length wordLength) cons
    SHAL_apply_bitmask(CR.reg, static_cast<uint32_t>(wordLength));
    SHAL_apply_bitmask(CR.reg, CR.receive_enable_mask);
    SHAL_apply_bitmask(CR.reg, CR.transmit_enable_mask);
+    SHAL_apply_bitmask(CR.reg, CR.Rx_interrupt_enable_mask);
+
+    SHAL_set_register_value(getUARTTransmitDataRegister(m_key).reg,0); //Clear TDR

    uint16_t baud = SystemCoreClock / (1 * baudRate);
    SHAL_set_bits_16(BRR.reg, 16, baud, 0);
@@ -52,34 +55,23 @@ void SHAL_UART::begin(uint32_t baudRate, SHAL_USART_Word_Length wordLength) cons
    SHAL_apply_bitmask(CR.reg, CR.usart_enable_mask); //Enable
 }

-void SHAL_UART::sendString(const char *s) volatile {
+void SHAL_UART::sendString(const char *s) const volatile {
    const auto ISR = getUARTISR(m_key);

    while (*s) sendChar(*s++); //Send chars while we haven't reached end of s

-    if(!SHAL_WAIT_FOR_CONDITION_US((*ISR.reg & ISR.transmission_complete_mask) != 0, 1000)){
+    if (!SHAL_wait_for_bit_set_us(ISR.reg, ISR.transmit_data_register_empty_mask, 500)) {
        assert(false);
    }
 }

-void SHAL_UART::sendChar(char c) volatile {
+void SHAL_UART::sendChar(const char c) const volatile {

-    /* TODO fix old
-    auto ISR = getUARTISR(m_key);
+    const auto ISR = getUARTISR(m_key);

-    if(!SHAL_WAIT_FOR_CONDITION_US((*ISR.reg & ISR.transmit_data_register_empty_mask) == 0, 20)){ //TODO check if this is too slow for this? Need to check busy reg
-        assert(false);
-    }
+    SHAL_wait_for_bit_set_us(ISR.reg,ISR.transmit_data_register_empty_mask,500);

-
-    *getUARTTransmitDataRegister(m_key).reg = c; //Send character
-    */
-    /* Wait until TXE (Transmit Data Register Empty) */
-    while (!(USART3->ISR & USART_ISR_TXE_TXFNF))
-    {
-        /* spin */
-    }
-    USART3->TDR = (uint32_t)(uint8_t)c;
+    SHAL_set_register_value(getUARTTransmitDataRegister(m_key).reg,static_cast<uint32_t>(c));
 }