diff --git a/.github/workflows/macos.yml b/.github/workflows/macos.yml index ed2a042e9e..e0e492bda5 100644 --- a/.github/workflows/macos.yml +++ b/.github/workflows/macos.yml @@ -67,7 +67,7 @@ jobs: - name: Hosted Examples if: always() run: | - (cd examples && ../tools/scripts/examples_compile.py linux) + (cd examples/linux && ../../tools/scripts/examples_compile.py block_device assert build_info etl fiber git logger printf threads) - name: Compile STM32 Examples if: always() diff --git a/examples/avr/display/hd44780/project.xml b/examples/avr/display/hd44780/project.xml index 37e48bec39..a926d56242 100644 --- a/examples/avr/display/hd44780/project.xml +++ b/examples/avr/display/hd44780/project.xml @@ -8,6 +8,7 @@ modm:driver:hd44780 modm:platform:gpio modm:platform:core + modm:platform:clock modm:build:scons diff --git a/examples/avr/qmc5883l/main.cpp b/examples/avr/qmc5883l/main.cpp index edcee2a985..db58fe0a09 100644 --- a/examples/avr/qmc5883l/main.cpp +++ b/examples/avr/qmc5883l/main.cpp @@ -17,7 +17,7 @@ using namespace Board; using namespace std::chrono_literals; -using Compass = Qmc5883l; +using Compass = modm::Qmc5883l; Compass::Data data; Compass compass(data); modm::atomic::Flag dataReady(true); @@ -54,7 +54,7 @@ main() << modm::endl; } else { - MODM_LOG_INFO << "readDataBlocking(): Error: " << uint8_t(I2cMaster::getErrorState()) + MODM_LOG_INFO << "readData(): Error: " << uint8_t(I2cMaster::getErrorState()) << modm::endl; } } diff --git a/examples/generic/ros/environment/thread_display.cpp b/examples/generic/ros/environment/thread_display.cpp index d96d565f9f..4fba8787fc 100644 --- a/examples/generic/ros/environment/thread_display.cpp +++ b/examples/generic/ros/environment/thread_display.cpp @@ -32,7 +32,7 @@ DisplayThread::update() // Wait for 100 msec unitl display powered up. modm::this_fiber::sleep_for(100ms); - display.initializeBlocking(); + display.initialize(); display.setFont(modm::font::Assertion); display.clear(); display << "Hello World!"; diff --git a/examples/nucleo_f042k6/spi_dma/main.cpp b/examples/nucleo_f042k6/spi_dma/main.cpp index 611068fe38..a82b7b025f 100644 --- a/examples/nucleo_f042k6/spi_dma/main.cpp +++ b/examples/nucleo_f042k6/spi_dma/main.cpp @@ -33,10 +33,10 @@ int main() uint8_t receiveBuffer[13]; // send out 12 bytes, don't care about response - Spi::transferBlocking(sendBuffer, nullptr, 12); + Spi::transfer(sendBuffer, nullptr, 12); // send out 12 bytes, read in 12 bytes - Spi::transferBlocking(sendBuffer, receiveBuffer, 12); + Spi::transfer(sendBuffer, receiveBuffer, 12); } return 0; diff --git a/examples/nucleo_f303re/spi_dma/main.cpp b/examples/nucleo_f303re/spi_dma/main.cpp index 5e60bf438f..6e3f0ac8f7 100644 --- a/examples/nucleo_f303re/spi_dma/main.cpp +++ b/examples/nucleo_f303re/spi_dma/main.cpp @@ -33,10 +33,10 @@ int main() uint8_t receiveBuffer[13]; // send out 12 bytes, don't care about response - Spi::transferBlocking(sendBuffer, nullptr, 12); + Spi::transfer(sendBuffer, nullptr, 12); // send out 12 bytes, read in 12 bytes - Spi::transferBlocking(sendBuffer, receiveBuffer, 12); + Spi::transfer(sendBuffer, receiveBuffer, 12); } return 0; diff --git a/examples/nucleo_f401re/distance_vl53l0/project.xml b/examples/nucleo_f401re/distance_vl53l0/project.xml index ebb54c6ac7..47779d3be6 100644 --- a/examples/nucleo_f401re/distance_vl53l0/project.xml +++ b/examples/nucleo_f401re/distance_vl53l0/project.xml @@ -7,6 +7,7 @@ modm:driver:vl53l0 modm:platform:i2c:1 modm:processing:fiber + modm:processing:timer modm:build:scons diff --git a/examples/nucleo_g071rb/amnb/main.cpp b/examples/nucleo_g071rb/amnb/main.cpp index f51990a325..5cd952c360 100644 --- a/examples/nucleo_g071rb/amnb/main.cpp +++ b/examples/nucleo_g071rb/amnb/main.cpp @@ -36,16 +36,14 @@ Listener listeners[] = }; Action actions[] = { - {1, []() -> Response + {1, [counter=uint8_t{}]() mutable -> Response { - static uint8_t counter{0}; MODM_LOG_INFO << "Node1 or Node3 received Action 1" << modm::endl; return counter++; } }, - {2, [](const uint32_t& data) -> Response + {2, [counter=uint8_t{}](const uint32_t& data) mutable -> Response { - static uint8_t counter{0}; MODM_LOG_INFO << "Node1 or Node3 received Action 2 with argument: " << data << modm::endl; return ErrorResponse(counter++); } @@ -82,15 +80,17 @@ modm::Fiber fiber_demo([] node1.broadcast(1, counter++); node3.broadcast(2); - auto res1 = node2.request(1, 1); + auto res1 = node1.request(1, 1); MODM_LOG_INFO << "Node1 responded with: " << res1.error(); - if (res1) { MODM_LOG_INFO << " " << *res1 << modm::endl; } + if (res1) { MODM_LOG_INFO << " " << *res1; } + MODM_LOG_INFO << modm::endl; auto res2 = node1.request(3, 2, counter); - MODM_LOG_INFO << "Node3 responded with: " << res2.error(); + MODM_LOG_INFO << "Node1 responded with: " << res2.error(); if (res2.hasUserError()) { - MODM_LOG_INFO << " " << *res2.userError() << modm::endl; + MODM_LOG_INFO << " " << *res2.userError(); } + MODM_LOG_INFO << modm::endl; if (counter % 10 == 0) { diff --git a/examples/nucleo_l432kc/spi_dma/main.cpp b/examples/nucleo_l432kc/spi_dma/main.cpp index 3c5b45d0f1..af84cd8741 100644 --- a/examples/nucleo_l432kc/spi_dma/main.cpp +++ b/examples/nucleo_l432kc/spi_dma/main.cpp @@ -34,10 +34,10 @@ int main() uint8_t receiveBuffer[13]; // send out 12 bytes, don't care about response - Spi::transferBlocking(sendBuffer, nullptr, 12); + Spi::transfer(sendBuffer, nullptr, 12); // send out 12 bytes, read in 12 bytes - Spi::transferBlocking(sendBuffer, receiveBuffer, 12); + Spi::transfer(sendBuffer, receiveBuffer, 12); } return 0; diff --git a/examples/nucleo_l432kc/uart_spi/main.cpp b/examples/nucleo_l432kc/uart_spi/main.cpp index 9c3a98385d..83a09dcf72 100644 --- a/examples/nucleo_l432kc/uart_spi/main.cpp +++ b/examples/nucleo_l432kc/uart_spi/main.cpp @@ -25,7 +25,7 @@ main() while (true) { - UartSpiMaster1::transferBlocking(0xF0); + UartSpiMaster1::transfer(0xF0); } return 0; diff --git a/examples/rp_pico/spi_dma/main.cpp b/examples/rp_pico/spi_dma/main.cpp index e019436b3a..f3ba220ec0 100644 --- a/examples/rp_pico/spi_dma/main.cpp +++ b/examples/rp_pico/spi_dma/main.cpp @@ -42,12 +42,12 @@ int main() usb_stream0 << "Spi tx only" << modm::endl; tud_task(); // send out 12 bytes, don't care about response - Spi::transferBlocking(sendBuffer, nullptr, 12); + Spi::transfer(sendBuffer, nullptr, 12); usb_stream0 << "Spi send \"" << reinterpret_cast(sendBuffer) << "\"" << modm::endl; tud_task(); // send out 12 bytes, read in 12 bytes - Spi::transferBlocking(sendBuffer, receiveBuffer, 12); + Spi::transfer(sendBuffer, receiveBuffer, 12); receiveBuffer[12] = 0; usb_stream0 << "Spi received \"" << reinterpret_cast(receiveBuffer) << "\"" << modm::endl; tud_task(); diff --git a/examples/samg55_xplained_pro/spi-loopback/main.cpp b/examples/samg55_xplained_pro/spi-loopback/main.cpp index 98c20ebf6e..dce76f59ad 100644 --- a/examples/samg55_xplained_pro/spi-loopback/main.cpp +++ b/examples/samg55_xplained_pro/spi-loopback/main.cpp @@ -29,7 +29,7 @@ int main() uint8_t tx[] = {0xa5, 0x21}; uint8_t rx[2]; - SpiMaster0::transferBlocking(tx, rx, 2); + SpiMaster0::transfer(tx, rx, 2); if(rx[0] == 0xa5 && rx[1] == 0x21) { flash_time_ms = 500; diff --git a/examples/stm32f4_discovery/display/ssd1306/main.cpp b/examples/stm32f4_discovery/display/ssd1306/main.cpp index aeea40c8a4..633ffbca41 100644 --- a/examples/stm32f4_discovery/display/ssd1306/main.cpp +++ b/examples/stm32f4_discovery/display/ssd1306/main.cpp @@ -35,7 +35,7 @@ main() MyI2cMaster::connect(); MyI2cMaster::initialize(); - display.initializeBlocking(); + display.initialize(); display.setFont(modm::font::Assertion); display << "Hello World!"; display.update(); diff --git a/examples/stm32f4_discovery/spi/main.cpp b/examples/stm32f4_discovery/spi/main.cpp index c20a834f29..a17412fd97 100644 --- a/examples/stm32f4_discovery/spi/main.cpp +++ b/examples/stm32f4_discovery/spi/main.cpp @@ -36,7 +36,7 @@ main() while (true) { // Connect Mosi to Miso to create a loopback - data = SpiMaster::transferBlocking(data); + data = SpiMaster::transfer(data); data++; // This will be spamming the bus } diff --git a/examples/stm32f4_discovery/uart_spi/main.cpp b/examples/stm32f4_discovery/uart_spi/main.cpp index 7168244355..ed1a49c668 100644 --- a/examples/stm32f4_discovery/uart_spi/main.cpp +++ b/examples/stm32f4_discovery/uart_spi/main.cpp @@ -24,7 +24,7 @@ main() while (true) { - UartSpiMaster2::transferBlocking(0xF0); + UartSpiMaster2::transfer(0xF0); } return 0; diff --git a/examples/stm32f746g_discovery/tmp102/main.cpp b/examples/stm32f746g_discovery/tmp102/main.cpp index 362011ca5b..557f3529c9 100644 --- a/examples/stm32f746g_discovery/tmp102/main.cpp +++ b/examples/stm32f746g_discovery/tmp102/main.cpp @@ -13,7 +13,6 @@ #include #include -#include #include typedef I2cMaster1 MyI2cMaster; diff --git a/src/modm/architecture/interface/block_device.hpp b/src/modm/architecture/interface/block_device.hpp index 78b6312ecf..eebe04aae3 100644 --- a/src/modm/architecture/interface/block_device.hpp +++ b/src/modm/architecture/interface/block_device.hpp @@ -14,7 +14,6 @@ #define MODM_INTERFACE_BLOCK_DEVICE_HPP #include -#include namespace modm { @@ -40,11 +39,11 @@ class BlockDevice #ifdef __DOXYGEN__ public: /// Initializes the storage hardware - modm::ResumableResult + bool initialize(); /// Deinitializes the storage hardware - modm::ResumableResult + bool deinitialize(); /** Read data from one or more blocks @@ -54,7 +53,7 @@ class BlockDevice * @param size Size to read in bytes (multiple of read block size) * @return True on success */ - modm::ResumableResult + bool read(uint8_t *buffer, bd_address_t address, bd_size_t size); /** Program blocks with data @@ -66,7 +65,7 @@ class BlockDevice * @param size Size to write in bytes (multiple of read block size) * @return True on success */ - modm::ResumableResult + bool program(const uint8_t *buffer, bd_address_t address, bd_size_t size); /** Erase blocks @@ -77,7 +76,7 @@ class BlockDevice * @param size Size to erase in bytes (multiple of read block size) * @return True on success */ - modm::ResumableResult + bool erase(bd_address_t address, bd_size_t size); /** Writes data to one or more blocks after erasing them @@ -89,7 +88,7 @@ class BlockDevice * @param size Size to write in bytes (multiple of read block size) * @return True on success */ - modm::ResumableResult + bool write(const uint8_t *buffer, bd_address_t address, bd_size_t size); public: diff --git a/src/modm/architecture/interface/gpio_expander.hpp b/src/modm/architecture/interface/gpio_expander.hpp index cad784cbf7..58fd6013ba 100644 --- a/src/modm/architecture/interface/gpio_expander.hpp +++ b/src/modm/architecture/interface/gpio_expander.hpp @@ -14,7 +14,6 @@ #include #include -#include #include namespace modm @@ -73,27 +72,27 @@ class GpioExpander public: /// Sets one or more pins to output - modm::ResumableResult + bool setOutput(Pins pins); /// Sets one or more pins to logic high /// @warning only modifies pins that have previously been set to output! - modm::ResumableResult + bool set(Pins pins); /// Resets one or more pins to logic low /// @warning only modifies pins that have previously been set to output! - modm::ResumableResult + bool reset(Pins pins); /// Toggles one or more pins /// @warning only modifies pins that have previously been set to output! - modm::ResumableResult + bool toggle(Pins pins); /// sets one or more pins to high or low level /// @warning only modifies pins that have previously been set to output! - modm::ResumableResult + bool set(Pins pins, bool value); /// Returns the set logical output state of the pin. @@ -106,7 +105,7 @@ class GpioExpander public: /// Sets one or more pins to input - modm::ResumableResult + bool setInput(Pins pins); /// Returns true if **all** pins have a high level @@ -116,17 +115,17 @@ class GpioExpander read(Pins pins) const; /// Reads the inputs and buffers them - modm::ResumableResult + bool readInput(); public: /// Writes data to the entire port /// @warning only modifies pins that have previously been set to output! - modm::ResumableResult + bool writePort(PortType data); /// Reads the entire port, buffers them and outputs the result to data. - modm::ResumableResult + bool readPort(PortType &data); public: @@ -208,7 +207,7 @@ class GpioExpanderPin : public modm::GpioIO static void setOutput() { - RF_CALL_BLOCKING(expander.setOutput(pin)); + expander.setOutput(pin); } static void inline @@ -220,25 +219,25 @@ class GpioExpanderPin : public modm::GpioIO static void set() { - RF_CALL_BLOCKING(expander.set(pin)); + expander.set(pin); } static void set(bool value) { - RF_CALL_BLOCKING(expander.set(pin, value)); + expander.set(pin, value); } static void reset() { - RF_CALL_BLOCKING(expander.reset(pin)); + expander.reset(pin); } static void toggle() { - RF_CALL_BLOCKING(expander.toggle(pin)); + expander.toggle(pin); } static bool inline @@ -250,13 +249,13 @@ class GpioExpanderPin : public modm::GpioIO static void setInput() { - RF_CALL_BLOCKING(expander.setInput(pin)); + expander.setInput(pin); } static bool read() { - RF_CALL_BLOCKING(expander.readInput()); + expander.readInput(); return expander.read(pin); } @@ -370,19 +369,19 @@ class GpioExpanderPort : public modm::GpioPort static void setOutput() { - RF_CALL_BLOCKING(expander.setOutput(Pins(portMask))); + expander.setOutput(Pins(portMask)); } static void setInput() { - RF_CALL_BLOCKING(expander.setInput(Pins(portMask))); + expander.setInput(Pins(portMask)); } static PortType read() { - RF_CALL_BLOCKING(expander.readInput()); + expander.readInput(); return (expander.getInputs().value & portMask) >> StartIndex; } @@ -392,13 +391,13 @@ class GpioExpanderPort : public modm::GpioPort { data = (data & dataMask) << StartIndex; data = (expander.getOutputs().value & ~portMask) | data; - RF_CALL_BLOCKING( expander.writePort(data) ); + expander.writePort(data); } static void toggle() { - RF_CALL_BLOCKING( expander.toggle(Pins(portMask)) ); + expander.toggle(Pins(portMask)); } }; @@ -439,19 +438,19 @@ class GpioExpanderPort> StartIndexReversed; } @@ -461,13 +460,13 @@ class GpioExpanderPort #include -#include +#include namespace modm { @@ -116,13 +116,13 @@ struct I2c static void resetDevices(uint32_t baudrate = 100'000) { - const auto delay = 500'000ul / baudrate; + const std::chrono::microseconds delay{500'000ul / baudrate}; for (uint_fast8_t ii = 0; ii < 9; ++ii) { Scl::reset(); - modm::delay_us(delay); + modm::this_fiber::sleep_for(delay); Scl::set(); - modm::delay_us(delay); + modm::this_fiber::sleep_for(delay); } } diff --git a/src/modm/architecture/interface/i2c_device.hpp b/src/modm/architecture/interface/i2c_device.hpp index 8c2a3d3b56..8fe4510342 100644 --- a/src/modm/architecture/interface/i2c_device.hpp +++ b/src/modm/architecture/interface/i2c_device.hpp @@ -15,7 +15,7 @@ #include "i2c.hpp" #include "i2c_master.hpp" #include "i2c_transaction.hpp" -#include +#include namespace modm { @@ -41,7 +41,7 @@ namespace modm * @ingroup modm_architecture_i2c_device */ template < class I2cMaster, uint8_t NestingLevels = 10, class Transaction = I2cWriteReadTransaction > -class I2cDevice : protected modm::NestedResumable< NestingLevels + 1 > +class I2cDevice { public: /// @param address the slave address not yet shifted left (address < 128). @@ -68,82 +68,36 @@ class I2cDevice : protected modm::NestedResumable< NestingLevels + 1 > /// @retval true device responds to address /// @retval false no device with address found - modm::ResumableResult + bool ping() { - RF_BEGIN(); - - RF_WAIT_UNTIL( transaction.configurePing() and startTransaction() ); - - RF_WAIT_WHILE( isTransactionRunning() ); - - RF_END_RETURN( wasTransactionSuccessful() ); + modm::this_fiber::poll([&]{ return transaction.configurePing(); }); + return runTransaction(); } /// Starts a write-read transaction and waits until finished. - modm::ResumableResult + bool writeRead(const uint8_t *writeBuffer, std::size_t writeSize, uint8_t *readBuffer, std::size_t readSize) { - RF_BEGIN(); - - RF_WAIT_UNTIL( startWriteRead(writeBuffer, writeSize, readBuffer, readSize) ); - - RF_WAIT_WHILE( isTransactionRunning() ); - - RF_END_RETURN( wasTransactionSuccessful() ); + modm::this_fiber::poll([&]{ return transaction.configureWriteRead(writeBuffer, writeSize, readBuffer, readSize); }); + return runTransaction(); } /// Starts a write transaction and waits until finished. - modm::ResumableResult + bool write(const uint8_t *buffer, std::size_t size) { - RF_BEGIN(); - - RF_WAIT_UNTIL( startWrite(buffer, size) ); - - RF_WAIT_WHILE( isTransactionRunning() ); - - RF_END_RETURN( wasTransactionSuccessful() ); + modm::this_fiber::poll([&]{ return transaction.configureWrite(buffer, size); }); + return runTransaction(); } /// Starts a write transaction and waits until finished. - modm::ResumableResult + bool read(uint8_t *buffer, std::size_t size) { - RF_BEGIN(); - - RF_WAIT_UNTIL( startRead(buffer, size) ); - - RF_WAIT_WHILE( isTransactionRunning() ); - - RF_END_RETURN( wasTransactionSuccessful() ); - } - -protected: - /// Configures the transaction with a write/read operation and starts it. - bool inline - startWriteRead(const uint8_t *writeBuffer, std::size_t writeSize, - uint8_t *readBuffer, std::size_t readSize) - { - return ( transaction.configureWriteRead(writeBuffer, writeSize, readBuffer, readSize) and - startTransaction() ); - } - - /// Configures the transaction with a write operation and starts it. - bool inline - startWrite(const uint8_t *buffer, std::size_t size) - { - return ( transaction.configureWrite(buffer, size) and - startTransaction() ); - } - - /// Configures the transaction with a read operation and starts it. - bool inline - startRead(uint8_t *buffer, std::size_t size) - { - return ( transaction.configureRead(buffer, size) and - startTransaction() ); + modm::this_fiber::poll([&]{ return transaction.configureRead(buffer, size); }); + return runTransaction(); } protected: @@ -176,16 +130,12 @@ class I2cDevice : protected modm::NestedResumable< NestingLevels + 1 > } /// Starts our own transaction and waits until finished. - modm::ResumableResult + bool runTransaction() { - RF_BEGIN(); - - RF_WAIT_UNTIL( startTransaction() ); - - RF_WAIT_WHILE( isTransactionRunning() ); - - RF_END_RETURN( wasTransactionSuccessful() ); + modm::this_fiber::poll([&]{ return startTransaction(); }); + modm::this_fiber::poll([&]{ return isTransactionRunning(); }); + return wasTransactionSuccessful(); } protected: diff --git a/src/modm/architecture/interface/i2c_multiplexer.hpp b/src/modm/architecture/interface/i2c_multiplexer.hpp index 65e6b6171d..31725d7272 100644 --- a/src/modm/architecture/interface/i2c_multiplexer.hpp +++ b/src/modm/architecture/interface/i2c_multiplexer.hpp @@ -12,7 +12,8 @@ #ifndef MODM_I2C_MULTIPLEXER_HPP #define MODM_I2C_MULTIPLEXER_HPP -#include +#include +#include "i2c_transaction.hpp" namespace modm { @@ -99,10 +100,9 @@ bool modm::I2cMultiplexerChannel::start(modm::I2cTransaction *transaction, ConfigurationHandler handler) { // If call to multiplexer failed, return without doing the actual transaction - if (RF_CALL_BLOCKING(multiplexer.multiplexerDevice.setActiveChannel(static_cast(channel))) == false) { + if (not multiplexer.multiplexerDevice.setActiveChannel(uint8_t(channel))) { return false; } - return multiplexer.start(transaction, handler); } diff --git a/src/modm/architecture/interface/spi_master.hpp b/src/modm/architecture/interface/spi_master.hpp index cbf571483a..9e32b121cb 100644 --- a/src/modm/architecture/interface/spi_master.hpp +++ b/src/modm/architecture/interface/spi_master.hpp @@ -15,7 +15,6 @@ #ifndef MODM_INTERFACE_SPI_MASTER_HPP #define MODM_INTERFACE_SPI_MASTER_HPP -#include #include "spi.hpp" namespace modm @@ -105,7 +104,7 @@ class SpiMaster : public ::modm::PeripheralDriver, public Spi * data to be sent * @return received data */ - static modm::ResumableResult + static uint8_t transfer(uint8_t data); /** @@ -123,7 +122,7 @@ class SpiMaster : public ::modm::PeripheralDriver, public Spi * @param length * number of bytes to be shifted out */ - static modm::ResumableResult + static void transfer(const uint8_t *tx, uint8_t *rx, std::size_t length); #endif }; diff --git a/src/modm/architecture/interface/uart_device.hpp b/src/modm/architecture/interface/uart_device.hpp index 1be7ff47b6..5410e9fb5e 100644 --- a/src/modm/architecture/interface/uart_device.hpp +++ b/src/modm/architecture/interface/uart_device.hpp @@ -26,8 +26,8 @@ namespace modm * @author Rasmus Kleist Hørlyck Sørensen * @ingroup modm_architecture_uart_device */ -template < class Uart, uint8_t NestingLevels = 10 > -class UartDevice : protected modm::NestedResumable< NestingLevels + 1 > +template < class Uart > +class UartDevice { public: UartDevice() : @@ -55,28 +55,24 @@ class UartDevice : protected modm::NestedResumable< NestingLevels + 1 > } protected: - modm::ResumableResult + bool write(uint8_t data) { - RF_BEGIN(0); - timeout.restart(txTimeout); - RF_WAIT_UNTIL(Uart::write(data) or timeout.isExpired() or Uart::hasError()); + modm::this_fiber::poll([&]{ return Uart::write(data) or timeout.isExpired() or Uart::hasError(); }); if (timeout.isExpired() or Uart::hasError()) { Uart::discardTransmitBuffer(); Uart::discardReceiveBuffer(); Uart::clearError(); - RF_RETURN(false); + return false; } - RF_END_RETURN(true); + return true; } - modm::ResumableResult + bool write(const uint8_t *data, std::size_t length) { - RF_BEGIN(0); - writeIndex = 0; timeout.restart(txTimeout); while (writeIndex < length) @@ -91,35 +87,31 @@ class UartDevice : protected modm::NestedResumable< NestingLevels + 1 > { Uart::discardReceiveBuffer(); Uart::clearError(); - RF_RETURN(false); + return false; } - RF_YIELD(); + modm::this_fiber::yield(); } - RF_END_RETURN(true); + return true; } - modm::ResumableResult + bool read(uint8_t &data) { - RF_BEGIN(1); - timeout.restart(rxTimeout); - RF_WAIT_UNTIL(Uart::read(data) or timeout.isExpired() or Uart::hasError()); + modm::this_fiber::poll([&]{ return Uart::read(data) or timeout.isExpired() or Uart::hasError(); }); if (timeout.isExpired() or Uart::hasError()) { Uart::discardReceiveBuffer(); Uart::clearError(); - RF_RETURN(false); + return false; } - RF_END_RETURN(true); + return true; } - modm::ResumableResult + bool read(uint8_t *buffer, std::size_t length) { - RF_BEGIN(1); - readIndex = 0; timeout.restart(rxTimeout); while (readIndex < length) @@ -134,12 +126,12 @@ class UartDevice : protected modm::NestedResumable< NestingLevels + 1 > { Uart::discardReceiveBuffer(); Uart::clearError(); - RF_RETURN(false); + return false; } - RF_YIELD(); + modm::this_fiber::yield(); } - RF_END_RETURN(true); + return true; } private: diff --git a/src/modm/architecture/module.lb b/src/modm/architecture/module.lb index f6bd40ac13..f4eda182fd 100644 --- a/src/modm/architecture/module.lb +++ b/src/modm/architecture/module.lb @@ -94,7 +94,6 @@ class BlockDevice(Module): module.description = "Block Devices" def prepare(self, module, options): - module.depends(":processing:resumable") return True def build(self, env): @@ -203,8 +202,7 @@ class GpioExpander(Module): module.description = "GPIO Expanders" def prepare(self, module, options): - module.depends(":architecture:gpio", ":architecture:register", - ":processing:resumable", ":math:utils") + module.depends(":architecture:gpio", ":architecture:register", ":math:utils") return True def build(self, env): @@ -218,7 +216,7 @@ class I2c(Module): module.description = "Inter-Integrated Circuit (I²C)" def prepare(self, module, options): - module.depends(":architecture:gpio", ":architecture:delay") + module.depends(":architecture:gpio", ":architecture:fiber") return True def build(self, env): @@ -234,7 +232,7 @@ class I2cDevice(Module): module.description = "I²C Devices" def prepare(self, module, options): - module.depends(":architecture:i2c", ":processing:resumable") + module.depends(":architecture:i2c", ":architecture:fiber") return True def build(self, env): @@ -248,8 +246,7 @@ class I2cMultiplexer(Module): module.description = "I²C Multiplexer" def prepare(self, module, options): - module.depends(":architecture:i2c", ":architecture:register", - ":processing:resumable", ":math:utils") + module.depends(":architecture:i2c") return True def build(self, env): @@ -317,7 +314,6 @@ class Spi(Module): module.description = "Serial Peripheral Interface (SPI)" def prepare(self, module, options): - module.depends(":processing:resumable") return True def build(self, env): @@ -360,7 +356,7 @@ class UartDevice(Module): module.description = "UART Devices" def prepare(self, module, options): - module.depends(":architecture:uart") + module.depends(":architecture:uart", ":processing:timer") return True def build(self, env): diff --git a/src/modm/communication/amnb/interface.hpp b/src/modm/communication/amnb/interface.hpp index cf9c3f84bc..41c781d6ff 100644 --- a/src/modm/communication/amnb/interface.hpp +++ b/src/modm/communication/amnb/interface.hpp @@ -14,7 +14,6 @@ #include "message.hpp" #include #include -#include namespace modm::amnb { @@ -47,15 +46,15 @@ class Device virtual bool hasReceived() = 0; - virtual modm::ResumableResult + virtual bool write(uint8_t data) = 0; - virtual modm::ResumableResult + virtual bool read(uint8_t *data) = 0; }; template< class Uart, uint16_t TimeoutUsTx = 1000, uint16_t TimeoutUsRx = 10'000 > -class DeviceWrapper : public Device, modm::Resumable<2> +class DeviceWrapper : public Device { public: bool @@ -64,37 +63,35 @@ class DeviceWrapper : public Device, modm::Resumable<2> return Uart::receiveBufferSize() > 0; } - modm::ResumableResult + bool write(uint8_t data) final { - RF_BEGIN(0); - RF_WAIT_UNTIL(Uart::write(data)); + modm::this_fiber::poll([&]{ return Uart::write(data); }); timeout.restart(std::chrono::microseconds(TimeoutUsTx)); - RF_WAIT_UNTIL(Uart::read(rx_data) or Uart::hasError() or timeout.isExpired()); + modm::this_fiber::poll([&]{ return Uart::read(rx_data) or Uart::hasError() or timeout.isExpired(); }); if (timeout.isExpired() or Uart::hasError() or rx_data != data) { Uart::discardTransmitBuffer(); Uart::discardReceiveBuffer(); Uart::clearError(); - RF_RETURN(false); + return false; } - RF_END_RETURN(true); + return true; } - modm::ResumableResult + bool read(uint8_t *data) final { - RF_BEGIN(1); timeout.restart(std::chrono::microseconds(TimeoutUsRx)); - RF_WAIT_UNTIL(Uart::read(*data) or Uart::hasError() or timeout.isExpired()); + modm::this_fiber::poll([&]{ return Uart::read(*data) or Uart::hasError() or timeout.isExpired(); }); if (timeout.isExpired() or Uart::hasError()) { Uart::discardReceiveBuffer(); Uart::clearError(); - RF_RETURN(false); + return false; } - RF_END_RETURN(true); + return true; } protected: @@ -103,7 +100,7 @@ class DeviceWrapper : public Device, modm::Resumable<2> }; template< size_t MaxHeapAllocation = 0 > -class Interface : modm::Resumable<6> +class Interface { public: Interface(Device &device) @@ -113,140 +110,130 @@ class Interface : modm::Resumable<6> isMediumBusy() const { return isReceiving or device.hasReceived(); } - modm::ResumableResult + InterfaceStatus transmit(const Message *message) { - RF_BEGIN(0); - if (isMediumBusy()) - RF_RETURN(InterfaceStatus::MediumBusy); + return InterfaceStatus::MediumBusy; isTransmitting = true; tx_data = STX; - if (not RF_CALL(write())) RF_RETURN(InterfaceStatus::SyncWriteFailed); - if (not RF_CALL(write())) RF_RETURN(InterfaceStatus::SyncWriteFailed); + if (not write()) return InterfaceStatus::SyncWriteFailed; + if (not write()) return InterfaceStatus::SyncWriteFailed; for (tx_index = 0; tx_index < message->headerLength(); tx_index++) - if (not RF_CALL(write_escaped(message->self()[tx_index]))) - RF_RETURN(InterfaceStatus::HeaderWriteFailed); + if (not write_escaped(message->self()[tx_index])) + return InterfaceStatus::HeaderWriteFailed; for (tx_index = 0; tx_index < message->dataLength(); tx_index++) - if (not RF_CALL(write_escaped(message->get()[tx_index]))) - RF_RETURN(InterfaceStatus::DataWriteFailed); + if (not write_escaped(message->get()[tx_index])) + return InterfaceStatus::DataWriteFailed; isTransmitting = false; - RF_END_RETURN(InterfaceStatus::Ok); + return InterfaceStatus::Ok; } - modm::ResumableResult + InterfaceStatus receiveHeader(Message *message) { - RF_BEGIN(1); - if (isTransmitting) - RF_RETURN(InterfaceStatus::MediumBusy); + return InterfaceStatus::MediumBusy; if (not device.hasReceived()) - RF_RETURN(InterfaceStatus::MediumEmpty); + return InterfaceStatus::MediumEmpty; - if (not RF_CALL(read())) RF_RETURN(InterfaceStatus::SyncReadFailed); - if (rx_data != STX) RF_RETURN(InterfaceStatus::SyncReadFailed); + if (not read()) return InterfaceStatus::SyncReadFailed; + if (rx_data != STX) return InterfaceStatus::SyncReadFailed; isReceiving = true; - if (not RF_CALL(read())) { + if (not read()) { isReceiving = false; - RF_RETURN(InterfaceStatus::SyncReadFailed); + return InterfaceStatus::SyncReadFailed; } if (rx_data != STX) { isReceiving = false; - RF_RETURN(InterfaceStatus::SyncReadFailed); + return InterfaceStatus::SyncReadFailed; } for(rx_index = 0; rx_index < message->SMALL_HEADER_SIZE; rx_index++) { - if (not RF_CALL(read_escaped())) RF_RETURN(InterfaceStatus::HeaderReadFailed); + if (not read_escaped()) return InterfaceStatus::HeaderReadFailed; message->self()[rx_index] = rx_data; } for(; rx_index < message->headerLength(); rx_index++) { - if (not RF_CALL(read_escaped())) RF_RETURN(InterfaceStatus::HeaderReadFailed); + if (not read_escaped()) return InterfaceStatus::HeaderReadFailed; message->self()[rx_index] = rx_data; } if (not message->isHeaderValid()) { isReceiving = false; - RF_RETURN(InterfaceStatus::HeaderInvalid); + return InterfaceStatus::HeaderInvalid; } if (not message->isLarge()) isReceiving = false; - RF_END_RETURN(InterfaceStatus::Ok); + return InterfaceStatus::Ok; } - modm::ResumableResult + InterfaceStatus receiveData(Message *message, bool allocate=true) { - RF_BEGIN(1); // same index as receiveHeader!!! - - if (not message->isLarge()) RF_RETURN(InterfaceStatus::Ok); + if (not message->isLarge()) return InterfaceStatus::Ok; if ( (rx_allocated = allocate and (message->dataLength() <= MaxHeapAllocation)) ) rx_allocated = message->allocate(); for(rx_index = 0; rx_index < message->dataLength(); rx_index++) { - if (not RF_CALL(read_escaped())) RF_RETURN(InterfaceStatus::DataReadFailed); + if (not read_escaped()) return InterfaceStatus::DataReadFailed; if (rx_allocated) message->get()[rx_index] = rx_data; } isReceiving = false; - if (allocate and not rx_allocated) RF_RETURN(InterfaceStatus::AllocationFailed); + if (allocate and not rx_allocated) return InterfaceStatus::AllocationFailed; - RF_END_RETURN(message->isDataValid() ? InterfaceStatus::Ok : InterfaceStatus::DataInvalid); + return message->isDataValid() ? InterfaceStatus::Ok : InterfaceStatus::DataInvalid; } protected: - modm::ResumableResult + bool write_escaped(uint8_t data) { - RF_BEGIN(2); if (data == STX or data == DLE) { tx_data = DLE; - if (not RF_CALL(write())) RF_RETURN(false); + if (not write()) return false; tx_data = data ^ 0x20; } else { tx_data = data; } - RF_END_RETURN_CALL(write()); + return write(); } - modm::ResumableResult + bool read_escaped() { - RF_BEGIN(3); - if (not RF_CALL(read())) RF_RETURN(false); + if (not read()) return false; if (rx_data == DLE) { - if (not RF_CALL(read())) RF_RETURN(false); + if (not read()) return false; rx_data ^= 0x20; } - RF_END_RETURN(true); + return true; } - modm::ResumableResult + bool write() { - RF_BEGIN(4); - if (RF_CALL(device.write(tx_data))) - RF_RETURN(true); + if (device.write(tx_data)) + return true; isTransmitting = false; - RF_END_RETURN(false); + return false; } - modm::ResumableResult + bool read() { - RF_BEGIN(5); - if (RF_CALL(device.read(&rx_data))) - RF_RETURN(true); + if (device.read(&rx_data)) + return true; isReceiving = false; - RF_END_RETURN(false); + return false; } protected: diff --git a/src/modm/communication/amnb/module.lb b/src/modm/communication/amnb/module.lb index 520c1f64b4..48120274ce 100644 --- a/src/modm/communication/amnb/module.lb +++ b/src/modm/communication/amnb/module.lb @@ -17,7 +17,7 @@ def init(module): def prepare(module, options): module.depends( ":math:utils", - ":processing:resumable", + ":architecture:fiber", ":processing:timer", ":container", ":utils") diff --git a/src/modm/communication/amnb/node.hpp b/src/modm/communication/amnb/node.hpp index 893348b4db..63847caae2 100644 --- a/src/modm/communication/amnb/node.hpp +++ b/src/modm/communication/amnb/node.hpp @@ -22,7 +22,7 @@ namespace modm::amnb /// @author Niklas Hauser /// @ingroup modm_communication_amnb template < size_t TxBufferSize = 2, size_t MaxHeapAllocation = 0 > -class Node : public modm::Resumable<6> +class Node { static_assert(2 <= TxBufferSize, "TxBuffer must have at least two messages!"); public: @@ -90,122 +90,113 @@ class Node : public modm::Resumable<6> } template< class ReturnType = void, class ErrorType = void > - modm::ResumableResult< Result > + Result request(uint8_t from, uint8_t command) { - RF_BEGIN(0); request_msg = Message(from, command, Type::Request); - RF_CALL(request()); - RF_END_RETURN(request_msg); + request(); + return request_msg; } template< class ReturnType = void, class ErrorType = void > - modm::ResumableResult< Result > + Result request(uint8_t from, uint8_t command, const uint8_t *data, size_t length) { - RF_BEGIN(1); { request_msg = Message(from, command, length, Type::Request); uint8_t* arg = request_msg.get(); if (arg == nullptr) { request_msg = Response(Error::RequestAllocationFailed).msg; - RF_RETURN(request_msg); + return request_msg; } std::memcpy(arg, data, length); } - RF_CALL(request()); - RF_END_RETURN(request_msg); + request(); + return request_msg; } template< class ReturnType = void, class ErrorType = void, class T > - modm::ResumableResult< Result > + Result request(uint8_t from, uint8_t command, const T &argument) { return request(from, command, (uint8_t*)&argument, sizeof(T)); } public: - modm::ResumableResult + void update_transmit() { - RF_BEGIN(2); while(1) { if (not tx_queue.isEmpty()) { - RF_WAIT_WHILE(isResumableRunning(3)); - RF_CALL(send(tx_queue.get())); + modm::this_fiber::poll([&]{ return not is_sending; }); + send(tx_queue.get()); tx_queue.pop(); } - RF_YIELD(); + modm::this_fiber::yield(); } - RF_END(); } - modm::ResumableResult + void update_receive() { - RF_BEGIN(5); while(1) { rx_msg.deallocate(); // deallocates previous message - if (RF_CALL(interface.receiveHeader(&rx_msg)) == InterfaceStatus::Ok) + if (interface.receiveHeader(&rx_msg) == InterfaceStatus::Ok) { // Check lists if we are interested in this message is_rx_msg_for_us = handleRxMessage(false); // Receive the message data, only allocate if it's for us - if (RF_CALL(interface.receiveData(&rx_msg, is_rx_msg_for_us)) == InterfaceStatus::Ok) + if (interface.receiveData(&rx_msg, is_rx_msg_for_us) == InterfaceStatus::Ok) { // Only handle message *with* data if it's for us if (is_rx_msg_for_us) handleRxMessage(true); } } - RF_YIELD(); + modm::this_fiber::yield(); } - RF_END(); } protected: - modm::ResumableResult + void send(Message &msg) { - RF_BEGIN(3); - + is_sending = true; msg.setValid(); tx_counter = std::min(MIN_TX_TRIES, uint8_t(msg.command() >> (8 - PRIORITY_BITS))); while(1) { while (interface.isMediumBusy()) { - RF_WAIT_WHILE(interface.isMediumBusy()); + modm::this_fiber::poll([&]{ return not interface.isMediumBusy(); }); reschedule(RESCHEDULE_MASK_SHORT); - RF_WAIT_UNTIL(tx_timer.isExpired()); + modm::this_fiber::poll([&]{ return tx_timer.isExpired(); }); } - if (RF_CALL(interface.transmit(&msg)) == InterfaceStatus::Ok) + if (interface.transmit(&msg) == InterfaceStatus::Ok) break; if (--tx_counter == 0) break; // a collision or other write issue occurred - RF_WAIT_WHILE(interface.isMediumBusy()); + modm::this_fiber::poll([&]{ return not interface.isMediumBusy(); }); reschedule(RESCHEDULE_MASK_LONG); - RF_WAIT_UNTIL(tx_timer.isExpired()); + modm::this_fiber::poll([&]{ return tx_timer.isExpired(); }); } - RF_END(); + is_sending = false; } - modm::ResumableResult + void request() { - RF_BEGIN(4); - - RF_WAIT_WHILE(isResumableRunning(3)); + modm::this_fiber::poll([&]{ return not is_sending; }); response_status = ResponseStatus::Waiting; - RF_CALL(send(request_msg)); + send(request_msg); response_timer.restart(1s); - RF_WAIT_UNTIL((response_status == ResponseStatus::Received) or response_timer.isExpired()); + modm::this_fiber::poll([&]{ return (response_status == ResponseStatus::Received) or response_timer.isExpired(); }); response_status = ResponseStatus::NotWaiting; if (response_timer.isExpired()) { @@ -213,7 +204,6 @@ class Node : public modm::Resumable<6> *request_msg.get() = Error::RequestTimeout; } - RF_END(); } protected: @@ -306,6 +296,7 @@ class Node : public modm::Resumable<6> const uint8_t actionCount{0}; const uint8_t listenerCount{0}; uint8_t address; + bool is_sending{false}; uint8_t tx_counter; bool is_rx_msg_for_us; diff --git a/src/modm/driver/adc/ad7280a.hpp b/src/modm/driver/adc/ad7280a.hpp index f0bee5e4e9..a5b7370eb5 100644 --- a/src/modm/driver/adc/ad7280a.hpp +++ b/src/modm/driver/adc/ad7280a.hpp @@ -15,8 +15,9 @@ #include +#include #include -#include +#include #include @@ -145,38 +146,38 @@ namespace modm * * @ingroup modm_driver_ad7280a */ - template - class Ad7280a + template + class Ad7280a: public modm::SpiDevice { // used for Unittests friend class ::Ad7280aTest; public: - static void + void initialize(ad7280a::Average average = ad7280a::NO_AVERAGE); /* * Initialize daisy chain. */ - static bool + bool chainSetup(); /** * Enable/Disable the six cell balance outputs. */ - static void + void enableBalancer(uint8_t device, uint8_t cells); - static bool + bool performSelftest(); - static void + void softwareReset(); /** * Read a single channel */ - static bool + bool readChannel(uint8_t device, ad7280a::Channel channel, uint16_t *value); /** @@ -184,14 +185,14 @@ namespace modm * * \param[out] values Array containing the results */ - static bool + bool readAllChannels(uint16_t *values); private: /** * Calculate the CRC for one byte */ - static uint8_t + uint8_t updateCrc(uint8_t data); /** @@ -206,30 +207,30 @@ namespace modm * uint8_t crc = calculateCrc(reg >> 10); * \endcode */ - static uint8_t + uint8_t calculateCrc(uint32_t data); - static bool + bool write(uint8_t device, ad7280a::Register reg, bool addressAll, uint8_t value); - static bool + bool read(uint32_t *value); - static bool + bool readRegister(ad7280a::RegisterValue* result); - static bool + bool readConversionResult(ad7280a::ConversionValue* result); /*static void dumpRegisterRead(uint32_t value); - static void + void dumpConversion(uint32_t value);*/ - static uint8_t controlHighByte; + uint8_t controlHighByte{}; }; #if __has_include() diff --git a/src/modm/driver/adc/ad7280a.lb b/src/modm/driver/adc/ad7280a.lb index 1c1164de6b..488c6c164c 100644 --- a/src/modm/driver/adc/ad7280a.lb +++ b/src/modm/driver/adc/ad7280a.lb @@ -36,7 +36,7 @@ be lower than 1 MHz because delays introduced in each stage of the chain. def prepare(module, options): module.depends( - ":architecture:delay", + ":architecture:fiber", ":architecture:gpio", ":architecture:spi", ":math:utils") diff --git a/src/modm/driver/adc/ad7280a_impl.hpp b/src/modm/driver/adc/ad7280a_impl.hpp index b5ddc0f834..18bb97d80c 100644 --- a/src/modm/driver/adc/ad7280a_impl.hpp +++ b/src/modm/driver/adc/ad7280a_impl.hpp @@ -131,13 +131,9 @@ #define AD7280A_READ_TXVAL 0xF800030A // ---------------------------------------------------------------------------- -template -uint8_t modm::Ad7280a::controlHighByte = 0; - -// ---------------------------------------------------------------------------- -template +template void -modm::Ad7280a::initialize(ad7280a::Average average) +modm::Ad7280a::initialize(ad7280a::Average average) { static_assert(N == 1, "Daisy chain length is currently limited to 1!"); @@ -148,9 +144,9 @@ modm::Ad7280a::initialize(ad7280a::Average average) } // ---------------------------------------------------------------------------- -template +template bool -modm::Ad7280a::chainSetup() +modm::Ad7280a::chainSetup() { // Set reset bit for all devices write(ad7280a::MASTER, ad7280a::CTRL_LB, true, @@ -200,17 +196,17 @@ modm::Ad7280a::chainSetup() } // ---------------------------------------------------------------------------- -template +template void -modm::Ad7280a::enableBalancer(uint8_t device, uint8_t cells) +modm::Ad7280a::enableBalancer(uint8_t device, uint8_t cells) { write(device, ad7280a::CELL_BALANCE, false, cells); } // ---------------------------------------------------------------------------- -template +template bool -modm::Ad7280a::performSelftest() +modm::Ad7280a::performSelftest() { // Set Bit D0 of the control register to 1 on all parts. This // setting enables the daisy-chain register read operation on @@ -229,7 +225,7 @@ modm::Ad7280a::performSelftest() // Allow sufficient time for the self-test conversions to be // completed plus tWAIT. - modm::delay_ms(50); // TODO + modm::this_fiber::sleep_for(50ms); // TODO // The register address corresponding to the self-test // conversion should be written to the read register of all @@ -256,9 +252,9 @@ modm::Ad7280a::performSelftest() } // ---------------------------------------------------------------------------- -template +template void -modm::Ad7280a::softwareReset() +modm::Ad7280a::softwareReset() { // Set reset bit for all devices write(ad7280a::MASTER, ad7280a::CTRL_LB, true, @@ -275,9 +271,9 @@ modm::Ad7280a::softwareReset() } // ---------------------------------------------------------------------------- -template +template bool -modm::Ad7280a::readChannel(uint8_t device, +modm::Ad7280a::readChannel(uint8_t device, ad7280a::Channel channel, uint16_t *value) { write(ad7280a::MASTER, ad7280a::CTRL_HB, true, @@ -290,7 +286,7 @@ modm::Ad7280a::readChannel(uint8_t device, AD7280A_CTRL_HB_CONV_START_CS); // Wait for the conversion to finish - modm::delay_ms(5); + modm::this_fiber::sleep_for(5ms); // Read one channel write(device, ad7280a::READ, false, channel); @@ -308,9 +304,9 @@ modm::Ad7280a::readChannel(uint8_t device, } // ---------------------------------------------------------------------------- -template +template bool -modm::Ad7280a::readAllChannels(uint16_t *values) +modm::Ad7280a::readAllChannels(uint16_t *values) { // Write Register Address 0x00 to the read register on all // parts. A device address of 0x00 is used when computing @@ -323,7 +319,7 @@ modm::Ad7280a::readAllChannels(uint16_t *values) AD7280A_CTRL_HB_CONV_START_CS); // Allow sufficient time for all conversions to be completed plus tWAIT. - modm::delay_ms(5); + modm::this_fiber::sleep_for(5ms); // Apply a CS low pulse that frames 32 SCLKs to read back // the desired voltage. This frame should simultaneously @@ -350,9 +346,9 @@ modm::Ad7280a::readAllChannels(uint16_t *values) /* * P(x) = x^8 + x^5 + x^3 + x^2 + x^1 + x^0 = 0b100101111 => 0x2F */ -template +template uint8_t -modm::Ad7280a::updateCrc(uint8_t data) +modm::Ad7280a::updateCrc(uint8_t data) { for (uint_fast8_t i = 0; i < 8; i++) { uint8_t bit = data & 0x80; @@ -366,9 +362,9 @@ modm::Ad7280a::updateCrc(uint8_t data) } // ---------------------------------------------------------------------------- -template +template uint8_t -modm::Ad7280a::calculateCrc(uint32_t data) +modm::Ad7280a::calculateCrc(uint32_t data) { uint8_t crc; @@ -379,9 +375,9 @@ modm::Ad7280a::calculateCrc(uint32_t data) } // ---------------------------------------------------------------------------- -template +template bool -modm::Ad7280a::write(uint8_t device, ad7280a::Register reg, +modm::Ad7280a::write(uint8_t device, ad7280a::Register reg, bool addressAll, uint8_t value) { // The device address is send with LSB first @@ -393,29 +389,34 @@ modm::Ad7280a::write(uint8_t device, ad7280a::Register reg, t |= calculateCrc(t >> 11) << 3 | 0x2; + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); - Spi::write((t >> 24) & 0xff); - Spi::write((t >> 16) & 0xff); - Spi::write((t >> 8) & 0xff); - Spi::write((t >> 0) & 0xff); - Cs::set(); + SpiMaster::transfer((t >> 24) & 0xff); + SpiMaster::transfer((t >> 16) & 0xff); + SpiMaster::transfer((t >> 8) & 0xff); + SpiMaster::transfer((t >> 0) & 0xff); + + if (this->releaseMaster()) Cs::set(); // TODO remove this - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); return true; } // ---------------------------------------------------------------------------- -template +template bool -modm::Ad7280a::read(uint32_t *value) +modm::Ad7280a::read(uint32_t *value) { + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); - *value = static_cast(Spi::write(0xF8)) << 24; - *value |= static_cast(Spi::write(0x00)) << 16; - *value |= static_cast(Spi::write(0x03)) << 8; - *value |= static_cast(Spi::write(0x0A)); - Cs::set(); + + *value = static_cast(SpiMaster::transfer(0xF8)) << 24; + *value |= static_cast(SpiMaster::transfer(0x00)) << 16; + *value |= static_cast(SpiMaster::transfer(0x03)) << 8; + *value |= static_cast(SpiMaster::transfer(0x0A)); + + if (this->releaseMaster()) Cs::set(); //MODM_LOG_DEBUG << "read = " << modm::hex << *value << modm::ascii << modm::endl; @@ -430,9 +431,9 @@ modm::Ad7280a::read(uint32_t *value) } // ---------------------------------------------------------------------------- -template +template bool -modm::Ad7280a::readRegister(ad7280a::RegisterValue* result) +modm::Ad7280a::readRegister(ad7280a::RegisterValue* result) { uint32_t value; if (read(&value)) @@ -449,9 +450,9 @@ modm::Ad7280a::readRegister(ad7280a::RegisterValue* result) } // ---------------------------------------------------------------------------- -template +template bool -modm::Ad7280a::readConversionResult(ad7280a::ConversionValue* result) +modm::Ad7280a::readConversionResult(ad7280a::ConversionValue* result) { uint32_t value; if (read(&value)) diff --git a/src/modm/driver/adc/ad7928.hpp b/src/modm/driver/adc/ad7928.hpp index 0cb440e4e0..0ada4ee813 100644 --- a/src/modm/driver/adc/ad7928.hpp +++ b/src/modm/driver/adc/ad7928.hpp @@ -18,8 +18,7 @@ #include #include #include -#include -#include +#include namespace modm { @@ -153,30 +152,30 @@ struct ad7928 * @ingroup modm_driver_ad7928 */ template -class Ad7928 : public ad7928, public modm::SpiDevice, protected modm::NestedResumable<3> +class Ad7928 : public ad7928, public modm::SpiDevice { public: Ad7928(); /// Call this function once before using the device - modm::ResumableResult + void initialize(); /// Initiate a single conversion and return the result of the previous conversion /// A running sequence will be aborted. /// If the device is in full shutdown, it will be woken up. - modm::ResumableResult + Data singleConversion(InputChannel channel); /// Start a conversion sequence. /// The device will automatically cycle through the specified channels, starting /// with the lowest channel index in sequence1, when nextSequenceConversion() is called. - modm::ResumableResult + void startSequence(SequenceChannels_t channels1, SequenceChannels_t channels2 = SequenceChannels_t(0)); /// Perform the next sequence conversion /// The result is undefined if the device is not in sequence mode or not in normal power mode. - modm::ResumableResult + Data nextSequenceConversion(); /// Enable extended range mode (0V < input < 2*Vref) @@ -199,15 +198,15 @@ class Ad7928 : public ad7928, public modm::SpiDevice, protected modm: /// Shutdown device /// Calling wakeup() or initiating a conversion will wake up the device - modm::ResumableResult + void fullShutdown(); /// Wake up the device from full shutdown mode - modm::ResumableResult + void wakeup(); private: - modm::ResumableResult + void transfer(Register_t reg); ControlRegister_t config; diff --git a/src/modm/driver/adc/ad7928.lb b/src/modm/driver/adc/ad7928.lb index 783694b6ee..f5e8921f48 100644 --- a/src/modm/driver/adc/ad7928.lb +++ b/src/modm/driver/adc/ad7928.lb @@ -24,11 +24,10 @@ of 20 Mhz is supported. def prepare(module, options): module.depends( - ":architecture:delay", + ":architecture:fiber", ":architecture:gpio", ":architecture:register", - ":architecture:spi.device", - ":processing:resumable") + ":architecture:spi.device") return True def build(env): diff --git a/src/modm/driver/adc/ad7928_impl.hpp b/src/modm/driver/adc/ad7928_impl.hpp index d4fe3651d2..f68cfe5680 100644 --- a/src/modm/driver/adc/ad7928_impl.hpp +++ b/src/modm/driver/adc/ad7928_impl.hpp @@ -31,17 +31,15 @@ Ad7928::Ad7928() : currentPowerMode{PowerMode::Normal} // ---------------------------------------------------------------------------- template -ResumableResult +void Ad7928::initialize() { - RF_BEGIN(); - Cs::setOutput(modm::Gpio::High); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); // reset device - RF_CALL(transfer(Register_t(0xFF))); - RF_CALL(transfer(Register_t(0xFF))); + transfer(Register_t(0xFF)); + transfer(Register_t(0xFF)); config |= ControlRegister::WriteControlReg; config |= ControlRegister::UnsignedOutput; @@ -51,87 +49,77 @@ Ad7928::initialize() InputChannel_t::reset(config); // write configuration - RF_CALL(transfer(config)); + transfer(config); currentPowerMode = PowerMode::Normal; - - RF_END(); } // ---------------------------------------------------------------------------- template -ResumableResult +ad7928::Data Ad7928::singleConversion(ad7928::InputChannel channel) { - RF_BEGIN(); - SequenceMode_t::set(config, SequenceMode::NoSequence); if(currentPowerMode == PowerMode::FullShutdown) { // power up device, will be in normal mode afterwards - RF_CALL(wakeup()); + wakeup(); } else if(currentPowerMode == PowerMode::AutoShutdown) { // dummy conversion to power up device // does not alter control register (write bit = 0) - RF_CALL(transfer(ControlRegister(0))); + transfer(ControlRegister(0)); } InputChannel_t::set(config, channel); - RF_CALL(transfer(config)); + transfer(config); InputChannel_t::reset(config); currentPowerMode = PowerMode_t::get(config); - RF_END_RETURN(data); + return data; } // ---------------------------------------------------------------------------- template -ResumableResult +void Ad7928::startSequence(ad7928::SequenceChannels_t channels1, ad7928::SequenceChannels_t channels2) { - RF_BEGIN(); - if(currentPowerMode == PowerMode::FullShutdown) { // power up device, will be in normal mode afterwards - RF_CALL(wakeup()); + wakeup(); } else if(currentPowerMode == PowerMode::AutoShutdown) { // dummy conversion to power up device // does not alter control register (write bit = 0) - RF_CALL(transfer(ControlRegister_t(0))); + transfer(ControlRegister_t(0)); } PowerMode_t::set(config, PowerMode::Normal); SequenceMode_t::set(config, SequenceMode::ProgramShadowRegister); - RF_CALL(transfer(config)); + transfer(config); currentPowerMode = PowerMode::Normal; // The next transfer writes to the shadow register Sequence1Channels_t::set(sequenceChannels, static_cast(channels1.value)); Sequence2Channels_t::set(sequenceChannels, static_cast(channels2.value)); - RF_CALL(transfer(sequenceChannels)); - - RF_END(); + transfer(sequenceChannels); } // ---------------------------------------------------------------------------- template -ResumableResult +ad7928::Data Ad7928::nextSequenceConversion() { - RF_BEGIN(); - SequenceMode_t::set(config, SequenceMode::ContinueSequence); // invalid if device is not in normal mode if(currentPowerMode != PowerMode::Normal) { - RF_RETURN(data); + return data; } - RF_CALL(transfer(config)); + transfer(config); currentPowerMode = PowerMode_t::get(config); - RF_END_RETURN(data); + return data; } // ---------------------------------------------------------------------------- @@ -176,7 +164,7 @@ Ad7928::isAutoShutdownEnabled() // ---------------------------------------------------------------------------- template -ResumableResult +void Ad7928::fullShutdown() { SequenceMode_t::set(config, SequenceMode::NoSequence); @@ -191,31 +179,26 @@ Ad7928::fullShutdown() // ---------------------------------------------------------------------------- template -ResumableResult +void Ad7928::wakeup() { - RF_BEGIN(); SequenceMode_t::set(config, SequenceMode::NoSequence); PowerMode_t::set(config, PowerMode::Normal); - RF_CALL(transfer(config)); + transfer(config); // Wait for the device to power up - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); currentPowerMode = PowerMode::Normal; - - RF_END(); } // ---------------------------------------------------------------------------- template -ResumableResult +void Ad7928::transfer(Register_t reg) { - RF_BEGIN(); - - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); SpiMaster::setDataMode(SpiMaster::DataMode::Mode1); @@ -224,12 +207,10 @@ Ad7928::transfer(Register_t reg) outBuffer[0] = (reg.value & 0xFF00) >> 8; outBuffer[1] = reg.value & 0xFF; - RF_CALL(SpiMaster::transfer(outBuffer, data.data, 2)); + SpiMaster::transfer(outBuffer, data.data, 2); if (this->releaseMaster()) Cs::set(); - - RF_END(); } } // namespace modm diff --git a/src/modm/driver/adc/ads101x.hpp b/src/modm/driver/adc/ads101x.hpp index 741d1bbc58..a249e60854 100644 --- a/src/modm/driver/adc/ads101x.hpp +++ b/src/modm/driver/adc/ads101x.hpp @@ -187,7 +187,7 @@ struct ads101x * @ingroup modm_driver_ads101x */ template -class Ads101x : public ads101x, public modm::I2cDevice +class Ads101x : public ads101x, public modm::I2cDevice { public: /** @@ -197,52 +197,52 @@ class Ads101x : public ads101x, public modm::I2cDevice Ads101x(Data &data, uint8_t address = 0x49); /// Call this function before using the device - modm::ResumableResult + bool initialize(); /// Determine if the device is currently performing a conversion - modm::ResumableResult + bool isBusy(); /// Start a single conversion with the specified input - modm::ResumableResult + bool startSingleShotConversion(); /// Start continuous conversions with the specified datarate and input - modm::ResumableResult + bool startContinuousConversion(DataRate dataRate = DataRate::Sps1600); /// Start a single conversion with the specified input /// @warning ADS1015 only - modm::ResumableResult + bool startSingleShotConversion(InputMultiplexer input = InputMultiplexer::Input0); /// Start continuous conversions with the specified datarate and input /// @warning ADS1015 only - modm::ResumableResult + bool startContinuousConversion(InputMultiplexer input = InputMultiplexer::Input0, DataRate dataRate = DataRate::Sps1600); /// Read the last conversion result /// @attention Following power-up, the conversion result remains zero until the first conversion is completed - modm::ResumableResult + bool readConversionResult(); /// Enable the conversion-ready function of the ALERT/RDY pin /// @attention enabling the conversion-ready function disables the comparator and sets the ComparatorQue value to one conversion - modm::ResumableResult + bool enableConversionReadyFunction(); /// Enable the comparator /// @warning ADS1014 and ADS1015 only /// @warning To use the comparator-function the high threshold must be greater than the low threshold - modm::ResumableResult + bool enableComparator(ComparatorMode mode, ComparatorPolarity polarity, ComparatorLatch latch, ComparatorQueue queue); /// Set the low threshold used by the comparator queue /// @warning ADS1014 and ADS1015 only /// @warning The low threshold value must be smaller than the high threshold value /// @attention The high threshold value must be updated whenever the PGA settings are changed - modm::ResumableResult + bool setLowThreshold(uint16_t threshold) { return writeRegister(Register::LowThreshold, (threshold << 4) & 0xFFF0); @@ -252,18 +252,18 @@ class Ads101x : public ads101x, public modm::I2cDevice /// @warning ADS1014 and ADS1015 only /// @warning The high threshold value must be greater than the low threshold value /// @attention The high threshold value must be updated whenever the PGA settings are changed - modm::ResumableResult + bool setHighThreshold(uint16_t threshold) { return writeRegister(Register::HighThreshold, (threshold << 4) | 0x0F); } /// Set the full scale range by programming the PGA and corresponding LSB size - modm::ResumableResult + bool setFullScaleRange(FullScaleRange fullScaleRange); private: - modm::ResumableResult + bool writeRegister(Register reg, uint16_t data); Data &data; diff --git a/src/modm/driver/adc/ads101x_impl.hpp b/src/modm/driver/adc/ads101x_impl.hpp index 9ed84f2364..49c6d16f8b 100644 --- a/src/modm/driver/adc/ads101x_impl.hpp +++ b/src/modm/driver/adc/ads101x_impl.hpp @@ -18,18 +18,16 @@ namespace modm { template -Ads101x::Ads101x(Data &data, uint8_t address) : I2cDevice(address), data(data) +Ads101x::Ads101x(Data &data, uint8_t address) : I2cDevice(address), data(data) { } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool Ads101x::initialize() { - RF_BEGIN(); - InputMultiplexer_t::set(config, InputMultiplexer::Input0); FullScaleRange_t::set(config, FullScaleRange::V2_048); DeviceOperatingMode_t::set(config, DeviceOperatingMode::SingleShot); @@ -37,168 +35,141 @@ Ads101x::initialize() ComparatorQueue_t::set(config, ComparatorQueue::Disable); data.lsbSizeIndex = i(FullScaleRange::V2_048) >> 9; - RF_END_RETURN_CALL(writeRegister(Register::Config, config.value)); + return writeRegister(Register::Config, config.value); } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool Ads101x::isBusy() { - RF_BEGIN(); - buffer[0] = i(Register::Config); - this->transaction.configureWriteRead(buffer, 1, buffer, 2); - - if (RF_CALL(this->runTransaction())) + if (I2cDevice::writeRead(buffer, 1, buffer, 2)) { - RF_RETURN((static_cast(buffer[0] << 8) & i(ConfigRegister::OS)) == 0); + return (static_cast(buffer[0] << 8) & i(ConfigRegister::OS)) == 0; } - RF_END_RETURN(false); + return false; } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool Ads101x::startSingleShotConversion() { - RF_BEGIN(); - DeviceOperatingMode_t::set(config, DeviceOperatingMode::SingleShot); - RF_END_RETURN_CALL(writeRegister(Register::Config, i(ConfigRegister::OS) | config.value)); + return writeRegister(Register::Config, i(ConfigRegister::OS) | config.value); } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool Ads101x::startContinuousConversion(DataRate dataRate) { - RF_BEGIN(); - DataRate_t::set(config, dataRate); DeviceOperatingMode_t::set(config, DeviceOperatingMode::Continuous); - RF_END_RETURN_CALL(writeRegister(Register::Config, config.value)); + return writeRegister(Register::Config, config.value); } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool Ads101x::startSingleShotConversion(InputMultiplexer input) { - RF_BEGIN(); - InputMultiplexer_t::set(config, input); DeviceOperatingMode_t::set(config, DeviceOperatingMode::SingleShot); - RF_END_RETURN_CALL(writeRegister(Register::Config, i(ConfigRegister::OS) | config.value)); + return writeRegister(Register::Config, i(ConfigRegister::OS) | config.value); } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool Ads101x::startContinuousConversion(InputMultiplexer input, DataRate dataRate) { - RF_BEGIN(); - DataRate_t::set(config, dataRate); InputMultiplexer_t::set(config, input); DeviceOperatingMode_t::set(config, DeviceOperatingMode::Continuous); - RF_END_RETURN_CALL(writeRegister(Register::Config, config.value)); + return writeRegister(Register::Config, config.value); } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool Ads101x::readConversionResult() { - RF_BEGIN(); - buffer[0] = i(Register::Conversion); - this->transaction.configureWriteRead(buffer, 1, data.data, 2); - - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::writeRead(buffer, 1, data.data, 2); } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool Ads101x::enableConversionReadyFunction() { - RF_BEGIN(); - - if (not RF_CALL(writeRegister(Register::LowThreshold, 0x0000))) + if (not writeRegister(Register::LowThreshold, 0x0000)) { - RF_RETURN(false); + return false; } - if (not RF_CALL(writeRegister(Register::HighThreshold, 0xFFFF))) + if (not writeRegister(Register::HighThreshold, 0xFFFF)) { - RF_RETURN(false); + return false; } ComparatorQueue_t::set(config, ComparatorQueue::OneConversion); - if (not RF_CALL(writeRegister(Register::Config, config.value))) + if (not writeRegister(Register::Config, config.value)) { - RF_RETURN(false); + return false; } - RF_END_RETURN(true); + return true; } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool Ads101x::enableComparator(ComparatorMode mode, ComparatorPolarity polarity, ComparatorLatch latch, ComparatorQueue queue) { - RF_BEGIN(); - ComparatorMode_t::set(config, mode); ComparatorPolarity_t::set(config, polarity); ComparatorLatch_t::set(config, latch); ComparatorQueue_t::set(config, queue); - RF_END_RETURN_CALL(writeRegister(Register::Config, config.value)); + return writeRegister(Register::Config, config.value); } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool Ads101x::setFullScaleRange(FullScaleRange fullScaleRange) { - RF_BEGIN(); - FullScaleRange_t::set(config, fullScaleRange); data.lsbSizeIndex = i(fullScaleRange) >> 9; - RF_END_RETURN_CALL(writeRegister(Register::Config, config.value)); + return writeRegister(Register::Config, config.value); } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool Ads101x::writeRegister(Register reg, uint16_t data) { - RF_BEGIN(); - buffer[0] = i(reg); buffer[1] = (data >> 8) & 0xFF; buffer[2] = data & 0xFF; - - this->transaction.configureWrite(buffer, 3); - - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::write(buffer, 3); } -} // modm namespace \ No newline at end of file +} // modm namespace diff --git a/src/modm/driver/adc/ads7828.hpp b/src/modm/driver/adc/ads7828.hpp index 8f03fd222b..ae38a33f85 100644 --- a/src/modm/driver/adc/ads7828.hpp +++ b/src/modm/driver/adc/ads7828.hpp @@ -105,7 +105,7 @@ struct ads7828 * @ingroup modm_driver_ads101x */ template -class Ads7828 : public ads7828, public modm::I2cDevice +class Ads7828 : public ads7828, public modm::I2cDevice { public: /** @@ -126,7 +126,7 @@ class Ads7828 : public ads7828, public modm::I2cDevice * The ads7828 has an internal reference which can be on or off, and the adc itself can be on or off. * INTERNAL_REFERENCE_mode_ADC_mode, where mode is either ON or OFF. Does not have to be the same */ - modm::ResumableResult + bool startMeasurement(InputChannel input); /** @@ -136,13 +136,13 @@ class Ads7828 : public ads7828, public modm::I2cDevice * PD1 = 1 to have internal reference ON * PD0 = 1 to have A/D converter ON */ - modm::ResumableResult + bool setPowerDownSelection(PowerDown powerDownSelection); /** * @brief Reads the latest measurement result **/ - modm::ResumableResult + bool readConversionResult(); inline Data & @@ -161,4 +161,4 @@ class Ads7828 : public ads7828, public modm::I2cDevice #include "ads7828_impl.hpp" -#endif // MODM_ADS7828_HPP \ No newline at end of file +#endif // MODM_ADS7828_HPP diff --git a/src/modm/driver/adc/ads7828.lb b/src/modm/driver/adc/ads7828.lb index a8e2075575..b2c0939c0f 100644 --- a/src/modm/driver/adc/ads7828.lb +++ b/src/modm/driver/adc/ads7828.lb @@ -21,10 +21,10 @@ The ADS7828 is a single-supply, low-power, 12-bit data acquisition device def prepare(module, options): module.depends( ":architecture:i2c.device", - ":processing:resumable") + ":architecture:fiber") return True def build(env): env.outbasepath = "modm/src/modm/driver/adc" env.copy("ads7828.hpp") - env.copy("ads7828_impl.hpp") \ No newline at end of file + env.copy("ads7828_impl.hpp") diff --git a/src/modm/driver/adc/ads7828_impl.hpp b/src/modm/driver/adc/ads7828_impl.hpp index 6a912029cc..5ca8bb54a6 100644 --- a/src/modm/driver/adc/ads7828_impl.hpp +++ b/src/modm/driver/adc/ads7828_impl.hpp @@ -21,43 +21,37 @@ namespace modm { template -Ads7828::Ads7828(Data &data, uint8_t address) : modm::I2cDevice(address), data(data) +Ads7828::Ads7828(Data &data, uint8_t address) : modm::I2cDevice(address), data(data) { } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool Ads7828::startMeasurement(InputChannel channel) { - RF_BEGIN(); InputChannel_t::set(commandByte, channel); - this->transaction.configureWrite(&commandByte.value, 1); - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::write(&commandByte.value, 1); } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool Ads7828::setPowerDownSelection(PowerDown powerDownSelection) { - RF_BEGIN(); PowerDown_t::set(commandByte, powerDownSelection); - this->transaction.configureWrite(&commandByte.value, 1); - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::write(&commandByte.value, 1); } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool Ads7828::readConversionResult() { - RF_BEGIN(); - this->transaction.configureRead(data.data, 2); - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::read(data.data, 2); } } // modm namespace diff --git a/src/modm/driver/adc/ads816x.hpp b/src/modm/driver/adc/ads816x.hpp index faaf64ce94..ef0ab5c351 100644 --- a/src/modm/driver/adc/ads816x.hpp +++ b/src/modm/driver/adc/ads816x.hpp @@ -17,8 +17,7 @@ #include #include #include -#include -#include +#include namespace modm { @@ -95,21 +94,21 @@ struct ads816x * @ingroup modm_driver_ads816x */ template -class Ads816x : public ads816x, public modm::SpiDevice, protected modm::NestedResumable<3> +class Ads816x : public ads816x, public modm::SpiDevice { public: Ads816x() = default; /// Call this function before using the device or to change operation mode /// \warning Only Mode::Manual is currently supported! - modm::ResumableResult + void initialize(Mode mode = Mode::Manual); /// Initiate a single conversion and return the result of the conversion. /// Simultanously the channel for the after next conversion will be set, /// i.e. before reading the first valid data two dummy conversions have to /// be executed. - modm::ResumableResult + uint16_t manualModeConversion(uint8_t afterNextChannel); /* @@ -118,7 +117,7 @@ class Ads816x : public ads816x, public modm::SpiDevice, protected mod /// The device will automatically cycle through the specified channels in /// the bitmask. template - modm::ResumableResult + void autoSequenceConversion(uint8_t channelsBitmask, std::span result); */ @@ -127,20 +126,19 @@ class Ads816x : public ads816x, public modm::SpiDevice, protected mod /// for ADS8167 and 2500ns (3us) for ADS8166 are the minimal values. /// Defaults to 3us, which works for all variants of the chip. void - setTConv(modm::ShortPreciseDuration t) + setTConv(std::chrono::microseconds t) { tConv = t; } private: - modm::ResumableResult + uint8_t registerAccess(Command command, Register reg, uint8_t value = 0); uint8_t buffer[3]; uint8_t buffer2[3]; - modm::ShortPreciseTimeout timeout; - modm::ShortPreciseDuration tConv = std::chrono::microseconds(3); + std::chrono::microseconds tConv{3us}; }; } // namespace modm diff --git a/src/modm/driver/adc/ads816x.lb b/src/modm/driver/adc/ads816x.lb index 84f75ece22..ad27196674 100644 --- a/src/modm/driver/adc/ads816x.lb +++ b/src/modm/driver/adc/ads816x.lb @@ -33,8 +33,7 @@ def prepare(module, options): ":architecture:gpio", ":architecture:spi.device", ":io", - ":processing:resumable", - ":processing:timer") + ":architecture:fiber") return True def build(env): diff --git a/src/modm/driver/adc/ads816x_impl.hpp b/src/modm/driver/adc/ads816x_impl.hpp index cd7f055927..5dbf05631e 100644 --- a/src/modm/driver/adc/ads816x_impl.hpp +++ b/src/modm/driver/adc/ads816x_impl.hpp @@ -22,88 +22,78 @@ namespace modm { template -ResumableResult +void Ads816x::initialize(Mode mode) { - RF_BEGIN(); - // Unlock register access by writing magic byte - RF_CALL(registerAccess(Command::Write, Register::REG_ACCESS, 0b1010'1010)); + registerAccess(Command::Write, Register::REG_ACCESS, 0b1010'1010); // Power up everything except the internal REFby2 buffer - RF_CALL(registerAccess(Command::Write, Register::PD_CNTL, 0b0001'0000)); + registerAccess(Command::Write, Register::PD_CNTL, 0b0001'0000); // Data type: Only ADC value - RF_CALL(registerAccess(Command::Write, Register::DATA_CNTL, uint8_t(DataFormat::OnlyResult))); + registerAccess(Command::Write, Register::DATA_CNTL, uint8_t(DataFormat::OnlyResult)); // V_ref = 4.096V (internal reference) - RF_CALL(registerAccess(Command::Write, Register::OFST_CAL, 0b010)); + registerAccess(Command::Write, Register::OFST_CAL, 0b010); // AIN0 ... AIN7 are separate channels - RF_CALL(registerAccess(Command::Write, Register::AIN_CFG, 0x00)); + registerAccess(Command::Write, Register::AIN_CFG, 0x00); // All individual channels are single-ended inputs; connect the AIN-COM pin to GND - RF_CALL(registerAccess(Command::Write, Register::COM_CFG, 0x00)); + registerAccess(Command::Write, Register::COM_CFG, 0x00); // Set mode - RF_CALL(registerAccess(Command::Write, Register::DEVICE_CFG, uint8_t(mode))); - - RF_END(); + registerAccess(Command::Write, Register::DEVICE_CFG, uint8_t(mode)); } template -ResumableResult +uint16_t Ads816x::manualModeConversion(uint8_t afterNextChannel) { - RF_BEGIN(); - - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); buffer[0] = uint8_t(Command::Write) | (uint16_t(Register::CHANNEL_ID) >> 8); buffer[1] = uint16_t(Register::CHANNEL_ID); buffer[2] = (afterNextChannel & 0b0000'0111); - RF_CALL(SpiMaster::transfer(buffer, buffer2, 3)); + SpiMaster::transfer(buffer, buffer2, 3); if (this->releaseMaster()) { Cs::set(); } - RF_END_RETURN( static_cast((static_cast(buffer2[0]) << 8) | buffer2[1]) ); + return static_cast((static_cast(buffer2[0]) << 8) | buffer2[1]); } /* // FIXME: Does not work correctly... template template -ResumableResult +void Ads816x::autoSequenceConversion(uint8_t channelsBitmask, std::span result) { - RF_BEGIN(); - if (std::popcount(channelsBitmask) > N) { - RF_RETURN(); + return; } // Write channel bitmask config (AUTO_SEQ_CH) - RF_CALL(registerAccess(Command::Write, Register::AUTO_SEQ_CFG1, channelsBitmask)); + registerAccess(Command::Write, Register::AUTO_SEQ_CFG1, channelsBitmask); - timeout.restart(tConv); - RF_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(tConv); // Set SEQ_START (0b1 << 0) bit - RF_CALL(registerAccess(Command::SetBits, Register::SEQ_START, 0b1)); + registerAccess(Command::SetBits, Register::SEQ_START, 0b1); // Read conversion results for (buffer2[0] = 0; buffer2[0] < std::popcount(channelsBitmask); buffer2[0]++) { - timeout.restart(tConv); - RF_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(tConv); - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); - RF_CALL(SpiMaster::transfer(nullptr, buffer, 2)); + SpiMaster::transfer(nullptr, buffer, 2); if (this->releaseMaster()) { Cs::set(); @@ -111,42 +101,37 @@ Ads816x::autoSequenceConversion(uint8_t channelsBitmask, std::spa result[buffer2[0]] = buffer[0] << 8 | buffer[1]; } - - RF_END(); } */ template -modm::ResumableResult +uint8_t Ads816x::registerAccess(Command command, Register reg, uint8_t value) { - RF_BEGIN(); - buffer[0] = uint8_t(command) | (uint16_t(reg) >> 8); buffer[1] = uint16_t(reg); buffer[2] = value; - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); - RF_CALL(SpiMaster::transfer(buffer, nullptr, 3)); + SpiMaster::transfer(buffer, nullptr, 3); if (command == Command::Read) { Cs::set(); - timeout.restart(tConv); - RF_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(tConv); // To retrieve the result of the read command we issue another 3 byte // transfer, nullptr instead of manual NoOperation (= 0b000...) command Cs::reset(); - RF_CALL(SpiMaster::transfer(nullptr, buffer, 3)); + SpiMaster::transfer(nullptr, buffer, 3); } if (this->releaseMaster()) { Cs::set(); } - RF_END_RETURN(buffer[0]); + return buffer[0]; } } // namespace modm diff --git a/src/modm/driver/adc/hx711.hpp b/src/modm/driver/adc/hx711.hpp index d180a90ad7..ededed8cf8 100644 --- a/src/modm/driver/adc/hx711.hpp +++ b/src/modm/driver/adc/hx711.hpp @@ -12,9 +12,9 @@ #ifndef MODM_HX711_HPP #define MODM_HX711_HPP -#include +#include #include -#include +#include namespace modm { @@ -40,13 +40,13 @@ struct hx711 /// @ingroup modm_driver_hx711 template -class Hx711 : public hx711, protected modm::NestedResumable<2> +class Hx711 : public hx711 { using Sck = typename Cfg::Sck; using Data = typename Cfg::Data; public: - modm::ResumableResult + int32_t singleConversion(); private: diff --git a/src/modm/driver/adc/hx711.lb b/src/modm/driver/adc/hx711.lb index ec553ac80c..342b8098a5 100644 --- a/src/modm/driver/adc/hx711.lb +++ b/src/modm/driver/adc/hx711.lb @@ -25,9 +25,8 @@ In a complete system the SCK pin's timing while reading must be met. def prepare(module, options): module.depends( - ":architecture:delay", - ":architecture:gpio", - ":processing:resumable") + ":architecture:fiber", + ":architecture:gpio") return True def build(env): diff --git a/src/modm/driver/adc/hx711_impl.hpp b/src/modm/driver/adc/hx711_impl.hpp index cf837ac6cd..91b63d9fd4 100644 --- a/src/modm/driver/adc/hx711_impl.hpp +++ b/src/modm/driver/adc/hx711_impl.hpp @@ -17,32 +17,30 @@ namespace modm { template -ResumableResult +int32_t Hx711::singleConversion() { - RF_BEGIN(); + modm::this_fiber::poll([&]{ return Data::read() == modm::Gpio::Low; }); - RF_WAIT_UNTIL(Data::read() == modm::Gpio::Low); - - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); data = 0; for (uint8_t ii = 0; ii < 24; ++ii) { Sck::set(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); data = (data << 1) | Data::read(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); Sck::reset(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); } // Additional pulses for mode of next conversion for (uint8_t ii = 0; ii < static_cast(Cfg::mode); ++ii) { Sck::set(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); Sck::reset(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); } // Fill up MSBs for negative numbers @@ -50,7 +48,7 @@ Hx711::singleConversion() data |= 0xff000000; } - RF_END_RETURN(data); + return data; } } // modm namespace diff --git a/src/modm/driver/adc/mcp3008.hpp b/src/modm/driver/adc/mcp3008.hpp index 644328ec95..94e34c6cc6 100644 --- a/src/modm/driver/adc/mcp3008.hpp +++ b/src/modm/driver/adc/mcp3008.hpp @@ -14,7 +14,7 @@ #include #include -#include +#include namespace modm { @@ -56,7 +56,7 @@ struct mcp3008 * @ingroup modm_driver_mcp3008 */ template -class Mcp3008 : public mcp3008, public modm::SpiDevice, protected modm::NestedResumable<1> +class Mcp3008 : public mcp3008, public modm::SpiDevice { public: Mcp3008() = default; @@ -65,7 +65,7 @@ class Mcp3008 : public mcp3008, public modm::SpiDevice, protected mod void initialize(); /// Read ADC channel - modm::ResumableResult + uint16_t read(Channel channel); private: diff --git a/src/modm/driver/adc/mcp3008.lb b/src/modm/driver/adc/mcp3008.lb index ddf5916c41..cc29d03b13 100644 --- a/src/modm/driver/adc/mcp3008.lb +++ b/src/modm/driver/adc/mcp3008.lb @@ -21,7 +21,7 @@ MCP3004/3008 are 10-bit 200 ksps SAR ADCs with SPI interface and 4/8 channels. def prepare(module, options): module.depends( ":architecture:spi.device", - ":processing:resumable") + ":architecture:fiber") return True def build(env): diff --git a/src/modm/driver/adc/mcp3008_impl.hpp b/src/modm/driver/adc/mcp3008_impl.hpp index 881dad70cf..ddbc9bf064 100644 --- a/src/modm/driver/adc/mcp3008_impl.hpp +++ b/src/modm/driver/adc/mcp3008_impl.hpp @@ -28,23 +28,21 @@ Mcp3008::initialize() } template -modm::ResumableResult +uint16_t Mcp3008::read(Channel channel) { - RF_BEGIN(); - - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); txBuffer_[1] = static_cast(channel) << 4; - RF_CALL(SpiMaster::transfer(txBuffer_, rxBuffer_, 3)); + SpiMaster::transfer(txBuffer_, rxBuffer_, 3); if (this->releaseMaster()) { Cs::set(); } - RF_END_RETURN(uint16_t(((rxBuffer_[1] & 0b11) << 8) | rxBuffer_[2])); + return uint16_t(((rxBuffer_[1] & 0b11) << 8) | rxBuffer_[2]); } } // namespace modm diff --git a/src/modm/driver/bus/bitbang_memory_interface.hpp b/src/modm/driver/bus/bitbang_memory_interface.hpp index c68ab2590b..146a1a3c1b 100644 --- a/src/modm/driver/bus/bitbang_memory_interface.hpp +++ b/src/modm/driver/bus/bitbang_memory_interface.hpp @@ -14,7 +14,7 @@ #define MODM_BITBANG_MEMORY_INTERFACE_HPP #include -#include +#include namespace modm { diff --git a/src/modm/driver/bus/bitbang_memory_interface_impl.hpp b/src/modm/driver/bus/bitbang_memory_interface_impl.hpp index cfa3cc859e..95a5850f2c 100644 --- a/src/modm/driver/bus/bitbang_memory_interface_impl.hpp +++ b/src/modm/driver/bus/bitbang_memory_interface_impl.hpp @@ -47,17 +47,17 @@ void modm::BitbangMemoryInterface::writeRegister(uint8_t reg) WR::reset(); PORT::write(0); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); WR::set(); // Low-to-High strobe - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); WR::reset(); PORT::write(reg); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); WR::set(); // Low-to-High strobe - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); CS::set(); @@ -73,17 +73,17 @@ void modm::BitbangMemoryInterface::writeData(const uint16_t da WR::reset(); PORT::write(data >> 8); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); WR::set(); // Low-to-High strobe - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); WR::reset(); PORT::write(data); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); WR::set(); // Low-to-High strobe - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); CS::set(); } @@ -99,15 +99,15 @@ void modm::BitbangMemoryInterface::writeDataMult(const uint16_ { WR::reset(); PORT::write(data >> 8); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); WR::set(); // Low-to-High strobe WR::reset(); PORT::write(data); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); WR::set(); // Low-to-High strobe - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); } @@ -134,9 +134,9 @@ void modm::BitbangMemoryInterface::writeRam(uint8_t * addr, co WR::reset(); PORT::write( *(addr++) ); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); WR::set(); // Low-to-high strobe - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); } CS::set(); diff --git a/src/modm/driver/bus/memory_bus.lb b/src/modm/driver/bus/memory_bus.lb index 1f05d4634d..4998dd0153 100644 --- a/src/modm/driver/bus/memory_bus.lb +++ b/src/modm/driver/bus/memory_bus.lb @@ -17,7 +17,7 @@ def init(module): def prepare(module, options): - module.depends(":architecture:delay", ":architecture:gpio") + module.depends(":architecture:fiber", ":architecture:gpio") return True def build(env): diff --git a/src/modm/driver/bus/tft_memory_bus.hpp b/src/modm/driver/bus/tft_memory_bus.hpp index 6f536f7ec1..1dce26f37f 100644 --- a/src/modm/driver/bus/tft_memory_bus.hpp +++ b/src/modm/driver/bus/tft_memory_bus.hpp @@ -14,7 +14,7 @@ #define TFT_MEMORY_BUS_HPP_ #include -#include +#include namespace modm { @@ -153,13 +153,13 @@ class MemoryBus PORT::write(data); // t_AS - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); WR::reset(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); WR::set(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); PORT::setInput(); CS::set(); @@ -172,11 +172,11 @@ class MemoryBus CS::reset(); WR::set(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); RD::reset(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); ret = PORT::read(); RD::set(); diff --git a/src/modm/driver/can/mcp2515.hpp b/src/modm/driver/can/mcp2515.hpp index aef8149df0..649bebf20b 100644 --- a/src/modm/driver/can/mcp2515.hpp +++ b/src/modm/driver/can/mcp2515.hpp @@ -18,8 +18,8 @@ #include #include -#include #include +#include #include #include "mcp2515_definitions.hpp" diff --git a/src/modm/driver/can/mcp2515.lb b/src/modm/driver/can/mcp2515.lb index 845784d7b1..1495df300a 100644 --- a/src/modm/driver/can/mcp2515.lb +++ b/src/modm/driver/can/mcp2515.lb @@ -30,7 +30,7 @@ def prepare(module, options): ":architecture:assert", ":architecture:can", ":architecture:clock", - ":architecture:delay", + ":architecture:fiber", ":debug") return True diff --git a/src/modm/driver/can/mcp2515_impl.hpp b/src/modm/driver/can/mcp2515_impl.hpp index 9b705897b5..1d716212d4 100644 --- a/src/modm/driver/can/mcp2515_impl.hpp +++ b/src/modm/driver/can/mcp2515_impl.hpp @@ -70,22 +70,22 @@ modm::Mcp2515::initializeWithPrescaler( // software reset for the mcp2515, after this the chip is back in the // configuration mode chipSelect.reset(); - spi.transferBlocking(RESET); - modm::delay_ms(1); + spi.transfer(RESET); + modm::this_fiber::sleep_for(1ms); chipSelect.set(); // wait a bit to give the MCP2515 some time to restart - modm::delay_ms(30); + modm::this_fiber::sleep_for(30ms); chipSelect.reset(); - spi.transferBlocking(WRITE); - spi.transferBlocking(CNF3); + spi.transfer(WRITE); + spi.transfer(CNF3); // load CNF1..3 - spi.transferBlocking(cnf, nullptr, 3); + spi.transfer(cnf, nullptr, 3); // enable interrupts - spi.transferBlocking(RX1IE | RX0IE); + spi.transfer(RX1IE | RX0IE); chipSelect.set(); // set TXnRTS pins as inwrites @@ -146,15 +146,15 @@ modm::Mcp2515::setFilter(accessor::Flash filter) for (i = 0; i < 0x30; i += 0x10) { chipSelect.reset(); - spi.transferBlocking(WRITE); - spi.transferBlocking(i); + spi.transfer(WRITE); + spi.transfer(i); for (j = 0; j < 12; j++) { if (i == 0x20 && j >= 0x08) break; - spi.transferBlocking(*filter++); + spi.transfer(*filter++); } chipSelect.set(); } @@ -213,7 +213,7 @@ modm::Mcp2515::getMessage(can::Message& message) } chipSelect.reset(); - spi.transferBlocking(address); + spi.transfer(address); message.flags.extended = readIdentifier(message.identifier); if (status & FLAG_RTR) { @@ -222,10 +222,10 @@ modm::Mcp2515::getMessage(can::Message& message) else { message.flags.rtr = false; } - message.length = spi.transferBlocking(0xff) & 0x0f; + message.length = spi.transfer(0xff) & 0x0f; for (uint8_t i = 0; i < message.length; ++i) { - message.data[i] = spi.transferBlocking(0xff); + message.data[i] = spi.transfer(0xff); } chipSelect.set(); @@ -279,28 +279,28 @@ modm::Mcp2515::sendMessage(const can::Message& message) } chipSelect.reset(); - spi.transferBlocking(WRITE_TX | address); + spi.transfer(WRITE_TX | address); writeIdentifier(message.identifier, message.flags.extended); // if the message is a rtr-frame, is has a length but no attached data if (message.flags.rtr) { - spi.transferBlocking(MCP2515_RTR | message.length); + spi.transfer(MCP2515_RTR | message.length); } else { - spi.transferBlocking(message.length); + spi.transfer(message.length); for (uint8_t i = 0; i < message.length; ++i) { - spi.transferBlocking(message.data[i]); + spi.transfer(message.data[i]); } } chipSelect.set(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); // send message via RTS command chipSelect.reset(); address = (address == 0) ? 1 : address; // 0 2 4 => 1 2 4 - spi.transferBlocking(RTS | address); + spi.transfer(RTS | address); chipSelect.set(); return address; @@ -314,9 +314,9 @@ modm::Mcp2515::writeRegister(uint8_t address, uint8_t data) { chipSelect.reset(); - spi.transferBlocking(WRITE); - spi.transferBlocking(address); - spi.transferBlocking(data); + spi.transfer(WRITE); + spi.transfer(address); + spi.transfer(data); chipSelect.set(); } @@ -327,9 +327,9 @@ modm::Mcp2515::readRegister(uint8_t address) { chipSelect.reset(); - spi.transferBlocking(READ); - spi.transferBlocking(address); - uint8_t data = spi.transferBlocking(0xff); + spi.transfer(READ); + spi.transfer(address); + uint8_t data = spi.transfer(0xff); chipSelect.set(); @@ -342,10 +342,10 @@ modm::Mcp2515::bitModify(uint8_t address, uint8_t mask, uint8_t da { chipSelect.reset(); - spi.transferBlocking(BIT_MODIFY); - spi.transferBlocking(address); - spi.transferBlocking(mask); - spi.transferBlocking(data); + spi.transfer(BIT_MODIFY); + spi.transfer(address); + spi.transfer(mask); + spi.transfer(data); chipSelect.set(); } @@ -356,8 +356,8 @@ modm::Mcp2515::readStatus(uint8_t type) { chipSelect.reset(); - spi.transferBlocking(type); - uint8_t data = spi.transferBlocking(0xff); + spi.transfer(type); + uint8_t data = spi.transfer(0xff); chipSelect.set(); @@ -377,7 +377,7 @@ modm::Mcp2515::writeIdentifier(const uint32_t& identifier, if (isExtendedFrame) { - spi.transferBlocking(*((uint16_t *) ptr + 1) >> 5); + spi.transfer(*((uint16_t *) ptr + 1) >> 5); // calculate the next values uint8_t temp; @@ -385,16 +385,16 @@ modm::Mcp2515::writeIdentifier(const uint32_t& identifier, temp |= MCP2515_IDE; temp |= (*((uint8_t *) ptr + 2)) & 0x03; - spi.transferBlocking(temp); - spi.transferBlocking(*((uint8_t *) ptr + 1)); - spi.transferBlocking(*((uint8_t *) ptr)); + spi.transfer(temp); + spi.transfer(*((uint8_t *) ptr + 1)); + spi.transfer(*((uint8_t *) ptr)); } else { - spi.transferBlocking(*((uint16_t *) ptr) >> 3); - spi.transferBlocking(*((uint8_t *) ptr) << 5); - spi.transferBlocking(0); - spi.transferBlocking(0); + spi.transfer(*((uint16_t *) ptr) >> 3); + spi.transfer(*((uint8_t *) ptr) << 5); + spi.transfer(0); + spi.transfer(0); } } @@ -406,31 +406,31 @@ modm::Mcp2515::readIdentifier(uint32_t& identifier) uint32_t *ptr = &identifier; - uint8_t first = spi.transferBlocking(0xff); - uint8_t second = spi.transferBlocking(0xff); + uint8_t first = spi.transfer(0xff); + uint8_t second = spi.transfer(0xff); if (second & MCP2515_IDE) { *((uint16_t *) ptr + 1) = (uint16_t) first << 5; - *((uint8_t *) ptr + 1) = spi.transferBlocking(0xff); + *((uint8_t *) ptr + 1) = spi.transfer(0xff); *((uint8_t *) ptr + 2) |= (second >> 3) & 0x1C; *((uint8_t *) ptr + 2) |= second & 0x03; - *((uint8_t *) ptr) = spi.transferBlocking(0xff); + *((uint8_t *) ptr) = spi.transfer(0xff); return true; } else { - spi.transferBlocking(0xff); + spi.transfer(0xff); *((uint8_t *) ptr + 3) = 0; *((uint8_t *) ptr + 2) = 0; *((uint16_t *) ptr) = (uint16_t) first << 3; - spi.transferBlocking(0xff); + spi.transfer(0xff); *((uint8_t *) ptr) |= second >> 5; diff --git a/src/modm/driver/color/tcs3414.hpp b/src/modm/driver/color/tcs3414.hpp index 6505957ed6..79a91f900e 100644 --- a/src/modm/driver/color/tcs3414.hpp +++ b/src/modm/driver/color/tcs3414.hpp @@ -166,46 +166,46 @@ struct tcs3414 * @ingroup modm_driver_tcs3414 */ template < typename I2cMaster > -class Tcs3414 : public tcs3414, public modm::I2cDevice< I2cMaster, 2 > +class Tcs3414 : public tcs3414, public modm::I2cDevice< I2cMaster > { public: Tcs3414(Data &data, uint8_t address = addr()); - modm::ResumableResult + bool initialize() { return writeRegister(RegisterAddress::CONTROL, 0b11); }; - modm::ResumableResult + bool configure(Gain gain = Gain::X1, Prescaler prescaler = Prescaler::D1) { return setGain(gain, prescaler); } public: /// The gain can be used to adjust the sensitivity of all ADC output channels. - modm::ResumableResult + bool setGain(Gain gain = Gain::X1, Prescaler prescaler = Prescaler::D1) { return writeRegister(RegisterAddress::GAIN, uint8_t(gain) | uint8_t(prescaler)); } /// Sets the integration time for the ADCs. - modm::ResumableResult + bool setIntegrationTime(IntegrationMode mode, NominalIntegrationTime time) { return writeRegister(RegisterAddress::TIMING, uint8_t(mode) | uint8_t(time)); } /// Sets the integration time for the ADCs. - modm::ResumableResult + bool setIntegrationTime(IntegrationMode mode, SyncPulseCount time) { return writeRegister(RegisterAddress::TIMING, uint8_t(mode) | uint8_t(time)); } public: /// Read current samples of ADC conversions for all channels. - modm::ResumableResult + bool readColor() { return readRegisters(RegisterAddress::DATA1LOW, data.data+1, sizeof(data.data)); } public: - modm::ResumableResult + bool readRegisters(RegisterAddress address, uint8_t *values, uint8_t count = 1); - modm::ResumableResult + bool writeRegister(RegisterAddress address, uint8_t value); protected: diff --git a/src/modm/driver/color/tcs3414_impl.hpp b/src/modm/driver/color/tcs3414_impl.hpp index e3c39099ec..60b65f3b78 100644 --- a/src/modm/driver/color/tcs3414_impl.hpp +++ b/src/modm/driver/color/tcs3414_impl.hpp @@ -18,39 +18,29 @@ template < typename I2cMaster > modm::Tcs3414::Tcs3414(Data &data, uint8_t address) - : I2cDevice(address), data(data) + : I2cDevice(address), data(data) {} // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::Tcs3414::writeRegister(RegisterAddress address, uint8_t value) { - RF_BEGIN(); - buffer[0] = 0x80 | // write command 0x40 | // with SMB read/write block protocol uint8_t(address); // at this address // buffer[1] contains ignored byte count buffer[2] = value; - - this->transaction.configureWrite(buffer, 2); - - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::write(buffer, 2); } template -modm::ResumableResult +bool modm::Tcs3414::readRegisters(RegisterAddress address, uint8_t* values, uint8_t count) { - RF_BEGIN(); - buffer[0] = 0x80 | // write command 0x40 | // with SMB read/write block protocol uint8_t(address); // at this address - - this->transaction.configureWriteRead(buffer, 1, values, count); - - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::writeRead(buffer, 1, values, count); } diff --git a/src/modm/driver/color/tcs3472.hpp b/src/modm/driver/color/tcs3472.hpp index 1a20c34ba7..5795797cea 100644 --- a/src/modm/driver/color/tcs3472.hpp +++ b/src/modm/driver/color/tcs3472.hpp @@ -181,7 +181,7 @@ struct tcs3472 * @ingroup modm_driver_tcs3472 */ template -class Tcs3472 : public tcs3472, public modm::I2cDevice +class Tcs3472 : public tcs3472, public modm::I2cDevice { public: Tcs3472(Data &data, uint8_t address = addr()); @@ -192,57 +192,57 @@ class Tcs3472 : public tcs3472, public modm::I2cDevice static constexpr Enable_t Enable_InterruptMode_Waittime = Enable_InterruptMode | Enable::INTERRUPT_ENABLE; /// Power up sensor and start conversions - modm::ResumableResult + bool initialize(Enable_t flags = Enable_PollingMode) { return writeRegister(RegisterAddress::ENABLE, flags.value); } - modm::ResumableResult + bool configure(Gain gain = Gain::X1, IntegrationTime int_time = IntegrationTime::MSEC_2_4); public: /// The gain can be used to adjust the sensitivity of all ADC output channels. - modm::ResumableResult + bool setGain(Gain gain = Gain::X1) { return writeRegister(RegisterAddress::GAIN, uint8_t(gain)); } /// Sets the integration time for the ADCs. - modm::ResumableResult + bool setIntegrationTime(IntegrationTime int_time = tcs3472::IntegrationTime::MSEC_2_4) { return writeRegister(RegisterAddress::INTEGRATION_TIME, uint8_t(int_time)); } /// Sets the wait time for the ADCs. - modm::ResumableResult + bool setWaitTime(WaitTime wait_time, bool wait_long = false); /// Sets the low threshold for the interrupt-comparator - modm::ResumableResult + bool setInterruptLowThreshold(uint16_t threshold); /// Sets the high threshold for the interrupt-comparator - modm::ResumableResult + bool setInterruptHighThreshold(uint16_t threshold); /// The gain can be used to adjust the sensitivity of all ADC output channels. - modm::ResumableResult + bool setInterruptPersistenceFilter(InterruptPersistence value) { return writeRegister(RegisterAddress::INTERRUPT_PERSIST_FILTER, uint8_t(value)); } public: /// Resets the interrupt output. - modm::ResumableResult + bool reloadInterrupt(); /// Read current samples of ADC conversions for all channels. - modm::ResumableResult + bool readColor() { return readRegisters(RegisterAddress::CDATALOW, data.data, sizeof(data.data)); } public: - modm::ResumableResult + bool readRegisters(RegisterAddress address, uint8_t *values, uint8_t count = 1); - modm::ResumableResult + bool writeRegister(RegisterAddress address, uint8_t value); Data& getData() { return data; } diff --git a/src/modm/driver/color/tcs3472_impl.hpp b/src/modm/driver/color/tcs3472_impl.hpp index 3c870b7cdb..fa084f41d8 100644 --- a/src/modm/driver/color/tcs3472_impl.hpp +++ b/src/modm/driver/color/tcs3472_impl.hpp @@ -20,121 +20,98 @@ template modm::Tcs3472::Tcs3472(Data &data, uint8_t address) - : I2cDevice(address), data(data) + : I2cDevice(address), data(data) {} // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::Tcs3472::setInterruptLowThreshold(uint16_t threshold) { - RF_BEGIN(); - - if (RF_CALL(writeRegister(RegisterAddress::LOW_THRESH_LOW_BYTE, threshold))) + if (writeRegister(RegisterAddress::LOW_THRESH_LOW_BYTE, threshold)) { - modm::delay(20us); - if (RF_CALL(writeRegister(RegisterAddress::LOW_THRESH_HIGH_BYTE, threshold >> 8))) + modm::this_fiber::sleep_for(20us); + if (writeRegister(RegisterAddress::LOW_THRESH_HIGH_BYTE, threshold >> 8)) { - RF_RETURN(true); + return true; } } - RF_END_RETURN(false); + return false; } template -modm::ResumableResult +bool modm::Tcs3472::setInterruptHighThreshold(uint16_t threshold) { - RF_BEGIN(); - - if (RF_CALL(writeRegister(RegisterAddress::HIGH_THRESH_LOW_BYTE, threshold))) + if (writeRegister(RegisterAddress::HIGH_THRESH_LOW_BYTE, threshold)) { - if (RF_CALL(writeRegister(RegisterAddress::HIGH_THRESH_HIGH_BYTE, threshold >> 8))) + if (writeRegister(RegisterAddress::HIGH_THRESH_HIGH_BYTE, threshold >> 8)) { - RF_RETURN(true); + return true; } } - RF_END_RETURN(false); + return false; } template -modm::ResumableResult +bool modm::Tcs3472::setWaitTime(WaitTime wait_time, bool wait_long) { - RF_BEGIN(); - - if (RF_CALL(writeRegister(RegisterAddress::CONFIGURATION, wait_long ? 1 << 1 : 0))) + if (writeRegister(RegisterAddress::CONFIGURATION, wait_long ? 1 << 1 : 0)) { - if (RF_CALL(writeRegister(RegisterAddress::WAIT_TIME, uint8_t(wait_time)))) + if (writeRegister(RegisterAddress::WAIT_TIME, uint8_t(wait_time))) { - RF_RETURN(true); + return true; } } - RF_END_RETURN(false); + return false; } template -modm::ResumableResult +bool modm::Tcs3472::configure(Gain gain, IntegrationTime int_time) { - RF_BEGIN(); - - if (RF_CALL(setGain(gain))) + if (setGain(gain)) { - if (RF_CALL(setIntegrationTime(int_time))) + if (setIntegrationTime(int_time)) { - RF_RETURN(true); + return true; } } - RF_END_RETURN(false); + return false; } // ---------------------------------------------------------------------------- // MARK: - Tasks template -modm::ResumableResult +bool modm::Tcs3472::reloadInterrupt() { - RF_BEGIN(); - // Only send command, don't append data! otherwise the reload is not working! buffer[0] = 0x80 | uint8_t(RegisterAddress::RELOAD_INTERRUPT); - - this->transaction.configureWrite(buffer, 1); - - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::write(buffer, 1); } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::Tcs3472::writeRegister(RegisterAddress address, uint8_t value) { - RF_BEGIN(); - buffer[0] = 0x80 | uint8_t(address); buffer[1] = value; - - this->transaction.configureWrite(buffer, 2); - - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::write(buffer, 2); } template -modm::ResumableResult +bool modm::Tcs3472::readRegisters(RegisterAddress address, uint8_t *const values, uint8_t count) { - RF_BEGIN(); - buffer[0] = 0x80 | 0x20 | // read command auto increment uint8_t(address); // at this address - - this->transaction.configureWriteRead(buffer, 1, values, count); - - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::writeRead(buffer, 1, values, count); } diff --git a/src/modm/driver/dac/mcp4922_impl.hpp b/src/modm/driver/dac/mcp4922_impl.hpp index cf6ec032d1..fafa4a4b87 100644 --- a/src/modm/driver/dac/mcp4922_impl.hpp +++ b/src/modm/driver/dac/mcp4922_impl.hpp @@ -58,9 +58,9 @@ template void modm::Mcp4922::update() { - modm::delay_us(1); // 40 nanoseconds + modm::this_fiber::sleep_for(1us); // 40 nanoseconds Ldac::reset(); - modm::delay_us(1); // 100 nanoseconds + modm::this_fiber::sleep_for(1us); // 100 nanoseconds Ldac::set(); } diff --git a/src/modm/driver/display/ea_dog.lb b/src/modm/driver/display/ea_dog.lb index dc26d0d707..130814d74c 100644 --- a/src/modm/driver/display/ea_dog.lb +++ b/src/modm/driver/display/ea_dog.lb @@ -25,7 +25,7 @@ def prepare(module, options): module.depends( ":architecture:accessor", - ":architecture:delay", + ":architecture:fiber", ":ui:display") return True diff --git a/src/modm/driver/display/hd44780.lb b/src/modm/driver/display/hd44780.lb index 4517f67c5e..3bf3ed8b4c 100644 --- a/src/modm/driver/display/hd44780.lb +++ b/src/modm/driver/display/hd44780.lb @@ -18,8 +18,9 @@ def init(module): def prepare(module, options): module.depends( - ":architecture:delay", + ":architecture:fiber", ":architecture:gpio", + ":architecture:delay", ":ui:display") return True diff --git a/src/modm/driver/display/hd44780_base.hpp b/src/modm/driver/display/hd44780_base.hpp index a05f38cf5f..277a969211 100644 --- a/src/modm/driver/display/hd44780_base.hpp +++ b/src/modm/driver/display/hd44780_base.hpp @@ -13,6 +13,8 @@ #ifndef MODM_HD44780_BASE_HPP #define MODM_HD44780_BASE_HPP +#include + namespace modm { diff --git a/src/modm/driver/display/hd44780_base_impl.hpp b/src/modm/driver/display/hd44780_base_impl.hpp index f93e9b6c33..117582fdc7 100644 --- a/src/modm/driver/display/hd44780_base_impl.hpp +++ b/src/modm/driver/display/hd44780_base_impl.hpp @@ -16,7 +16,7 @@ #error "Don't include this file directly, use 'hd44780_base.hpp' instead!" #endif -#include +#include // ---------------------------------------------------------------------------- template @@ -29,11 +29,11 @@ modm::Hd44780Base::initialize(LineMode lineMode) Bus::writeHighNibble(Set8BitBus); - modm::delay_ms(5); + modm::this_fiber::sleep_for(5ms); Bus::writeHighNibble(Set8BitBus); - modm::delay_us(100); + modm::this_fiber::sleep_for(100us); Bus::writeHighNibble(Set8BitBus); @@ -164,7 +164,7 @@ modm::Hd44780Base::isBusy() if (read() & BusyFlagMask) { - modm::delay_us(2); + modm::this_fiber::sleep_for(2us); return true; } return false; @@ -177,7 +177,7 @@ modm::Hd44780Base::writeCGRAM(uint8_t character, const uint8_t while(not writeCommand(SetCGRAM_Address | (character << 3))) ; for (std::size_t ii = 0; ii < 8; ++ii) { - modm::delay(50us); + modm::this_fiber::sleep_for(50us); writeRAM(cg[ii]); } return true; @@ -194,14 +194,14 @@ modm::Hd44780Base::Bus::write(uint8_t data) DATA::write(data >> 4); E::set(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); E::reset(); modm::delay_ns(10); DATA::write(data); E::set(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); E::reset(); modm::delay_ns(10); } @@ -223,7 +223,7 @@ modm::Hd44780Base::Bus::read() DATA::setInput(); E::set(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); data = DATA::read(); E::reset(); modm::delay_ns(10); @@ -231,7 +231,7 @@ modm::Hd44780Base::Bus::read() data <<= 4; E::set(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); data |= DATA::read(); E::reset(); modm::delay_ns(10); @@ -249,7 +249,7 @@ modm::Hd44780Base::Bus::write(uint8_t data) DATA::write(data); E::set(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); E::reset(); modm::delay_ns(500); } @@ -271,7 +271,7 @@ modm::Hd44780Base::Bus::read() DATA::setInput(); E::set(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); data = DATA::read(); E::reset(); modm::delay_ns(500); diff --git a/src/modm/driver/display/ili9341.hpp b/src/modm/driver/display/ili9341.hpp index 76ac5b18c0..8ad2de71d9 100644 --- a/src/modm/driver/display/ili9341.hpp +++ b/src/modm/driver/display/ili9341.hpp @@ -12,7 +12,7 @@ #define MODM_ILI9341_HPP #include -#include +#include #include #include #include diff --git a/src/modm/driver/display/ili9341.lb b/src/modm/driver/display/ili9341.lb index 63333894e3..5037ef966e 100644 --- a/src/modm/driver/display/ili9341.lb +++ b/src/modm/driver/display/ili9341.lb @@ -17,7 +17,7 @@ def init(module): def prepare(module, options): module.depends( - ":architecture:delay", + ":architecture:fiber", ":architecture:spi.device", ":ui:display") return True diff --git a/src/modm/driver/display/ili9341_impl.hpp b/src/modm/driver/display/ili9341_impl.hpp index d44ce85c69..ba3432fd0a 100644 --- a/src/modm/driver/display/ili9341_impl.hpp +++ b/src/modm/driver/display/ili9341_impl.hpp @@ -70,7 +70,7 @@ Ili9341::initialize() sizeof(negativeGammaCorr)); this->writeCommand(Command::LeaveSleep); - modm::delay_ms(120); + modm::this_fiber::sleep_for(120ms); this->writeCommand(Command::InversionOff); this->writeCommand(Command::DisplayOn); @@ -85,16 +85,16 @@ Ili9341::reset(bool hardReset /* = fals if (hardReset) { Reset::set(); - modm::delay_ms(5); + modm::this_fiber::sleep_for(5ms); Reset::reset(); - modm::delay_ms(5); + modm::this_fiber::sleep_for(5ms); Reset::set(); - modm::delay_ms(5); + modm::this_fiber::sleep_for(5ms); } else { BatchHandle h(*this); this->writeCommand(Command::SwReset); - modm::delay_ms(5); + modm::this_fiber::sleep_for(5ms); } } diff --git a/src/modm/driver/display/ili9341_spi.hpp b/src/modm/driver/display/ili9341_spi.hpp index 59fe7a5856..ecd98d67b6 100644 --- a/src/modm/driver/display/ili9341_spi.hpp +++ b/src/modm/driver/display/ili9341_spi.hpp @@ -36,24 +36,24 @@ class Ili9341SPIInterface: public ili9341, public modm::SpiDevice writeCommand(Command command) { Dc::reset(); // enable command - SPI::transferBlocking(i(command)); + SPI::transfer(i(command)); Dc::set(); // reset to data } modm_noinline void writeCommand(Command command, uint8_t const *args, std::size_t length) { Dc::reset(); // enable command - SPI::transferBlocking(i(command)); + SPI::transfer(i(command)); Dc::set(); // reset to data if (length != 0) { - SPI::transferBlocking(const_cast(args), nullptr, length); + SPI::transfer(const_cast(args), nullptr, length); } } void writeData(uint8_t const *data, std::size_t length) { - SPI::transferBlocking(const_cast(data), nullptr, length); + SPI::transfer(const_cast(data), nullptr, length); } void writeCommandValue8(Command command, uint8_t value) @@ -69,16 +69,16 @@ class Ili9341SPIInterface: public ili9341, public modm::SpiDevice Dc::reset(); // enable command // SPI::Hal::setDataSize(SpiBase::DataSize::Bit9); - SPI::transferBlocking(i(command) << 1); + SPI::transfer(i(command) << 1); SPI::Hal::setDataSize(SpiBase::DataSize::Bit8); Dc::set(); // reset to data - SPI::transferBlocking(b /*nullptr*/, buffer, length); + SPI::transfer(b /*nullptr*/, buffer, length); } uint8_t readData(Command command) { writeCommand(command); - return SPI::transferBlocking(0x00); + return SPI::transfer(0x00); } public: diff --git a/src/modm/driver/display/is31fl3733.hpp b/src/modm/driver/display/is31fl3733.hpp index 07c4968fbb..c89c7499d7 100644 --- a/src/modm/driver/display/is31fl3733.hpp +++ b/src/modm/driver/display/is31fl3733.hpp @@ -13,7 +13,7 @@ #include #include -#include +#include #include namespace modm @@ -109,11 +109,11 @@ struct is31fl3733 * @author Niklas Hauser */ template < class I2cMaster > -class Is31fl3733 : public is31fl3733, public modm::I2cDevice +class Is31fl3733 : public is31fl3733, public modm::I2cDevice { public: Is31fl3733(uint8_t address=addr()): - I2cDevice(address) + I2cDevice(address) {} bool @@ -164,116 +164,100 @@ class Is31fl3733 : public is31fl3733, public modm::I2cDevice { return led_short; } public: - modm::ResumableResult + bool reset() { return readRegister(Register::RESET, buffer); } - modm::ResumableResult + bool setGlobalCurrent(uint8_t current) { return writeRegister(Register::GLOBAL_CURRENT_CONTROL, current); } - modm::ResumableResult + bool clearSoftwareShutdown() { return writeRegister(Register::CONFIGURATION, 0x01); } - modm::ResumableResult + bool setSwPullUp(Resistor value) { return writeRegister(Register::SW_PULL_UP, uint8_t(value)); } - modm::ResumableResult + bool setCsPullDown(Resistor value) { return writeRegister(Register::CS_PULL_DOWN, uint8_t(value)); } - modm::ResumableResult + bool triggerOpenShortDetection() { return writeRegister(Register::CONFIGURATION, 0b101); } - modm::ResumableResult + bool readOpenShort() { - RF_BEGIN(); - if (not RF_CALL(setPage(Register::LED_OPEN))) RF_RETURN(false); + if (not setPage(Register::LED_OPEN)) return false; buffer[0] = uint8_t(Register::LED_OPEN); - this->transaction.configureWriteRead(buffer, 1, (uint8_t*)&led_open, LED_OPEN_size + LED_SHORT_size); - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::writeRead(buffer, 1, (uint8_t*)&led_open, LED_OPEN_size + LED_SHORT_size); } - modm::ResumableResult + bool writeOnOff() { - RF_BEGIN(); - if (not RF_CALL(setPage(Register::LED_ON_OFF))) RF_RETURN(false); - - this->transaction.configureWrite(&data.addr_led_on_off, LED_ON_OFF_size+1); - RF_END_RETURN_CALL(this->runTransaction()); + if (not setPage(Register::LED_ON_OFF)) return false; + return I2cDevice::write(&data.addr_led_on_off, LED_ON_OFF_size+1); } - modm::ResumableResult + bool writePwm() { - RF_BEGIN(); - if (not RF_CALL(setPage(Register::PWM))) RF_RETURN(false); - - this->transaction.configureWrite(&data.addr_led_pwm, PWM_size+1); - RF_END_RETURN_CALL(this->runTransaction()); + if (not setPage(Register::PWM)) return false; + return I2cDevice::write(&data.addr_led_pwm, PWM_size+1); } public: - modm::ResumableResult + bool writeRegister(Register reg, uint8_t value, uint8_t offset=0) { - RF_BEGIN(); - if (not RF_CALL(setPage(reg))) RF_RETURN(false); + if (not setPage(reg)) return false; buffer[0] = uint8_t(reg) + offset; buffer[1] = value; - - this->transaction.configureWrite(buffer, 2); - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::write(buffer, 2); } - modm::ResumableResult + bool readRegister(Register reg, uint8_t *const value, uint8_t offset=0) { - RF_BEGIN(); - if (not RF_CALL( setPage(reg) )) RF_RETURN(false); + if (not setPage(reg)) return false; buffer[0] = uint8_t(reg) + offset; - this->transaction.configureWriteRead(buffer, 1, value, 1); - - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::writeRead(buffer, 1, value, 1); } protected: - modm::ResumableResult + bool setPage(Register reg) { - RF_BEGIN(); - if (hasPage(reg) and (getPage(reg) != current_page)) { buffer[0] = uint8_t(Register::COMMAND_WRITE_LOCK); buffer[1] = 0xC5; // command write key - this->transaction.configureWrite(buffer, 2); - if (not RF_CALL(this->runTransaction())) RF_RETURN(false); + + if (not I2cDevice::write(buffer, 2)) return false; buffer[0] = uint8_t(Register::COMMAND); buffer[1] = getPage(reg); - if (not RF_CALL(this->runTransaction())) RF_RETURN(false); + if (not I2cDevice::write(buffer, 2)) return false; current_page = getPage(reg); } - RF_END_RETURN(true); + return true; } protected: struct LedData { - const uint8_t addr_led_on_off{uint8_t(Register::LED_ON_OFF)}; + const uint8_t addr_led_on_off{uint8_t(Register::LED_ON_OFF)}; uint16_t led_on_off[SizeX]; - const uint8_t addr_led_pwm{uint8_t(Register::PWM)}; + const uint8_t addr_led_pwm{uint8_t(Register::PWM)}; uint8_t led_pwm[SizeX][SizeY]; } modm_packed; diff --git a/src/modm/driver/display/max7219.hpp b/src/modm/driver/display/max7219.hpp index 5aa64ed2bb..c4ed84e6ad 100644 --- a/src/modm/driver/display/max7219.hpp +++ b/src/modm/driver/display/max7219.hpp @@ -84,8 +84,8 @@ class Max7219 CS::reset(); for (uint8_t i = 0; i < MODULES; ++i) { - SPI::transferBlocking(col + 1); - SPI::transferBlocking(0); + SPI::transfer(col + 1); + SPI::transfer(0); } CS::set(); }; @@ -102,8 +102,8 @@ class Max7219 CS::reset(); for (uint8_t i = 0; i < MODULES; ++i) { - SPI::transferBlocking(col + 1); - SPI::transferBlocking(*data++); + SPI::transfer(col + 1); + SPI::transfer(*data++); } CS::set(); } @@ -127,8 +127,8 @@ class Max7219 // Write the command multiple times, for each MODULES for (uint8_t i = 0; i < MODULES; ++i) { - SPI::transferBlocking(uint8_t(reg)); - SPI::transferBlocking(data); + SPI::transfer(uint8_t(reg)); + SPI::transfer(data); } CS::set(); } diff --git a/src/modm/driver/display/nokia5110_impl.hpp b/src/modm/driver/display/nokia5110_impl.hpp index 8622ff9875..42097febbd 100644 --- a/src/modm/driver/display/nokia5110_impl.hpp +++ b/src/modm/driver/display/nokia5110_impl.hpp @@ -50,7 +50,7 @@ Nokia5110< Spi, Ce, Dc, Reset >::update() Dc::set(); // high = data for (uint8_t xx = 0; xx < this->getWidth(); ++xx) { for (uint8_t yy = 0; yy < this->getHeight() / 8; ++yy) { - Spi::transferBlocking(this->buffer[xx][yy]); + Spi::transfer(this->buffer[xx][yy]); } } Ce::set(); @@ -62,7 +62,7 @@ Nokia5110< Spi, Ce, Dc, Reset >::writeCommand(uint8_t data) { Dc::reset(); // low = command Ce::reset(); - Spi::transferBlocking(data); + Spi::transfer(data); Ce::set(); } diff --git a/src/modm/driver/display/nokia6610.hpp b/src/modm/driver/display/nokia6610.hpp index d7d217f46f..0f72018e77 100644 --- a/src/modm/driver/display/nokia6610.hpp +++ b/src/modm/driver/display/nokia6610.hpp @@ -15,7 +15,7 @@ #ifndef MODM_NOKIA6610_HPP #define MODM_NOKIA6610_HPP -#include +#include #include namespace modm diff --git a/src/modm/driver/display/nokia6610_impl.hpp b/src/modm/driver/display/nokia6610_impl.hpp index fa10b7bdf9..f034fa1550 100644 --- a/src/modm/driver/display/nokia6610_impl.hpp +++ b/src/modm/driver/display/nokia6610_impl.hpp @@ -31,9 +31,9 @@ modm::Nokia6610::initialize() // Reset pin Reset::setOutput(); Reset::reset(); - modm::delay_ms(1); + modm::this_fiber::sleep_for(1ms); Reset::set(); - modm::delay_ms(10); + modm::this_fiber::sleep_for(10ms); lcdSettings(); @@ -108,9 +108,9 @@ modm::Nokia6610::lcdSettings() else{ // Hardware reset Reset::reset(); - modm::delay_ms(50); + modm::this_fiber::sleep_for(50ms); Reset::set(); - modm::delay_ms(50); + modm::this_fiber::sleep_for(50ms); // Display vontrol writeSpiCommand(nokia::NOKIA_GE8_DISCTL); @@ -129,7 +129,7 @@ modm::Nokia6610::lcdSettings() CS::set(); // wait aproximetly 100ms - modm::delay_ms(100); + modm::this_fiber::sleep_for(100ms); CS::reset(); // Sleep out @@ -221,7 +221,7 @@ modm::Nokia6610::update() else { // wait approximately 100ms - modm::delay_ms(100); + modm::this_fiber::sleep_for(100ms); // Display On writeSpiCommand(nokia::NOKIA_GE8_DISON); diff --git a/src/modm/driver/display/parallel_tft.hpp b/src/modm/driver/display/parallel_tft.hpp index e4d98a28e9..a7f6691908 100644 --- a/src/modm/driver/display/parallel_tft.hpp +++ b/src/modm/driver/display/parallel_tft.hpp @@ -15,7 +15,6 @@ #ifndef MODM_PARALLEL_TFT_HPP #define MODM_PARALLEL_TFT_HPP -#include #include namespace modm diff --git a/src/modm/driver/display/parallel_tft.lb b/src/modm/driver/display/parallel_tft.lb index c705f55c99..a35b3426f1 100644 --- a/src/modm/driver/display/parallel_tft.lb +++ b/src/modm/driver/display/parallel_tft.lb @@ -16,9 +16,7 @@ def init(module): module.description = "Parallel Bus TFT Display" def prepare(module, options): - module.depends( - ":architecture:delay", - ":ui:display") + module.depends(":ui:display") return True def build(env): diff --git a/src/modm/driver/display/sh1106.hpp b/src/modm/driver/display/sh1106.hpp index 92120d7c80..f835869dcc 100644 --- a/src/modm/driver/display/sh1106.hpp +++ b/src/modm/driver/display/sh1106.hpp @@ -32,11 +32,9 @@ class Sh1106 : public Ssd1306 Sh1106(uint8_t address = 0x3C) : Ssd1306(address) {} protected: - modm::ResumableResult + void startWriteDisplay() override { - RF_BEGIN(); - this->transaction_success = true; this->commandBuffer[0] = ssd1306::AdressingCommands::HigherColumnStartAddress; @@ -45,28 +43,23 @@ class Sh1106 : public Ssd1306 for (page = 0; page < Height / 8; page++) { this->commandBuffer[2] = 0xB0 | page; - this->transaction_success &= RF_CALL(this->writeCommands(3)); + this->transaction_success &= this->writeCommands(3); - RF_WAIT_UNTIL( - this->transaction.configureDisplayWrite((uint8_t*)&this->buffer[page], 128)); - RF_WAIT_UNTIL(this->startTransaction()); - RF_WAIT_WHILE(this->isTransactionRunning()); - this->transaction_success &= this->wasTransactionSuccessful(); + modm::this_fiber::poll([&]{ return this->transaction.configureDisplayWrite((uint8_t*)&this->buffer[page], 128); }); + this->transaction_success &= this->runTransaction(); }; - RF_END_RETURN(this->transaction_success); + return this->transaction_success; } - modm::ResumableResult + void initializeMemoryMode() override { - RF_BEGIN(); // Default on Power-up - can be omitted this->commandBuffer[0] = ssd1306::AdressingCommands::MemoryMode; this->commandBuffer[1] = ssd1306::MemoryMode::PAGE; - this->transaction_success &= RF_CALL(this->writeCommands(2)); - RF_END(); - } + this->transaction_success &= this->writeCommands(2); +} private: size_t page; diff --git a/src/modm/driver/display/siemens_m55.hpp b/src/modm/driver/display/siemens_m55.hpp index 4acd9571a1..3d1415c887 100644 --- a/src/modm/driver/display/siemens_m55.hpp +++ b/src/modm/driver/display/siemens_m55.hpp @@ -15,7 +15,7 @@ #ifndef MODM_SIEMENS_M55_HPP #define MODM_SIEMENS_M55_HPP -#include +#include #include namespace modm diff --git a/src/modm/driver/display/siemens_m55_impl.hpp b/src/modm/driver/display/siemens_m55_impl.hpp index 41f3ad0417..0e36c76a83 100644 --- a/src/modm/driver/display/siemens_m55_impl.hpp +++ b/src/modm/driver/display/siemens_m55_impl.hpp @@ -62,9 +62,9 @@ void modm::SiemensM55::lcdSettings() { // Hardware reset is low from initialize - modm::delay_ms(10); + modm::this_fiber::sleep_for(10ms); Reset::set(); - modm::delay_ms(10); + modm::this_fiber::sleep_for(10ms); RS::set(); // command mode CS::reset(); // select display @@ -73,10 +73,10 @@ modm::SiemensM55::lcdSettings() { SPI::write(initData_lm15[ii]); // send initialization data } - modm::delay_ms(1); + modm::this_fiber::sleep_for(1ms); CS::set(); // deactivate LCD CS - modm::delay_ms(1); + modm::this_fiber::sleep_for(1ms); CS::reset(); // activate LCD CS SPI::write(0xF0); @@ -99,7 +99,7 @@ modm::SiemensM55::lcdSettings() CS::set(); // deactivate LCD CS RS::reset(); // set LCD to data mode - modm::delay_ms(10); + modm::this_fiber::sleep_for(10ms); static uint8_t contrast = 22; diff --git a/src/modm/driver/display/siemens_s65.hpp b/src/modm/driver/display/siemens_s65.hpp index cde174975a..5d77cbe230 100644 --- a/src/modm/driver/display/siemens_s65.hpp +++ b/src/modm/driver/display/siemens_s65.hpp @@ -15,7 +15,7 @@ #ifndef MODM_SIEMENS_S65_HPP #define MODM_SIEMENS_S65_HPP -#include +#include #include // to enable this define add it to your ``project.cfg`` and diff --git a/src/modm/driver/display/siemens_s65.lb b/src/modm/driver/display/siemens_s65.lb index 8cedc61b54..5251bf41d5 100644 --- a/src/modm/driver/display/siemens_s65.lb +++ b/src/modm/driver/display/siemens_s65.lb @@ -17,7 +17,7 @@ def init(module): def prepare(module, options): module.depends( - ":architecture:delay", + ":architecture:fiber", ":architecture:spi", ":ui:display") return True diff --git a/src/modm/driver/display/siemens_s65_impl.hpp b/src/modm/driver/display/siemens_s65_impl.hpp index 94eeddc200..6953536543 100644 --- a/src/modm/driver/display/siemens_s65_impl.hpp +++ b/src/modm/driver/display/siemens_s65_impl.hpp @@ -78,9 +78,9 @@ void modm::SiemensS65Common::writeReg(uint8_t reg) { CS::reset(); - SPI::transferBlocking(0x74); // start byte, RS = 0, R/W = 0, write index register - SPI::transferBlocking(0x00); - SPI::transferBlocking(reg); + SPI::transfer(0x74); // start byte, RS = 0, R/W = 0, write index register + SPI::transfer(0x00); + SPI::transfer(reg); CS::set(); } @@ -89,9 +89,9 @@ void modm::SiemensS65Common::writeData(uint16_t data) { CS::reset(); - SPI::transferBlocking(0x76); // start byte, RS = 1, R/W = 0, write instruction or RAM data - SPI::transferBlocking(data>>8); - SPI::transferBlocking(data); + SPI::transfer(0x76); // start byte, RS = 1, R/W = 0, write instruction or RAM data + SPI::transfer(data>>8); + SPI::transfer(data); CS::set(); } @@ -100,12 +100,12 @@ void modm::SiemensS65Common::lcdSettings(bool landscape) { // Hardware reset is low from initialize - modm::delay_ms(50); + modm::this_fiber::sleep_for(50ms); Reset::set(); - modm::delay_ms(50); + modm::this_fiber::sleep_for(50ms); writeCmd(0x07, 0x0000); //display off - modm::delay_ms(10); + modm::this_fiber::sleep_for(10ms); //power on sequence writeCmd(0x02, 0x0400); //lcd drive control @@ -118,11 +118,11 @@ modm::SiemensS65Common::lcdSettings(bool landscape) writeCmd(0x03, 0x0000); //power control 1: BT //step 2 writeCmd(0x03, 0x0000); //power control 1: DC writeCmd(0x03, 0x000C); //power control 1: AP - modm::delay_ms(40); + modm::this_fiber::sleep_for(40ms); writeCmd(0x0E, 0x2D1F); //power control 5: VCOMG //step 3 - modm::delay_ms(40); + modm::this_fiber::sleep_for(40ms); writeCmd(0x0D, 0x0616); //power control 4: PON //step 4 - modm::delay_ms(100); + modm::this_fiber::sleep_for(100ms); //display options if (landscape) { @@ -142,7 +142,7 @@ modm::SiemensS65Common::lcdSettings(bool landscape) writeCmd(0x07, 0x0025); //display control: GON writeCmd(0x07, 0x0027); //display control: D1 writeCmd(0x07, 0x0037); //display control: DTE - modm::delay_ms(10); + modm::this_fiber::sleep_for(10ms); lcdCls(0x03e0); } @@ -156,7 +156,7 @@ modm::SiemensS65Common::lcdCls(uint16_t colour) { writeReg(0x22); CS::reset(); - SPI::transferBlocking(0x76); // start byte + SPI::transfer(0x76); // start byte // start data transmission @@ -183,8 +183,8 @@ modm::SiemensS65Common::lcdCls(uint16_t colour) { uint8_t c1 = colour >> 8; uint8_t c2 = colour & 0xff; for (uint16_t i = 0; i < (132 * 176); ++i) { - SPI::transferBlocking(c1); - SPI::transferBlocking(c2); + SPI::transfer(c1); + SPI::transfer(c2); } #endif @@ -202,7 +202,7 @@ modm::SiemensS65Portrait::update() { // WRITE MEMORY CS::reset(); - SPI::transferBlocking(0x76); // start byte + SPI::transfer(0x76); // start byte const uint16_t maskBlank = 0x0000; // RRRR RGGG GGGB BBBB const uint16_t maskFilled = 0x37e0; // RRRR RGGG GGGB BBBB @@ -280,7 +280,7 @@ modm::SiemensS65Portrait::update() { } // pix // use transfer() of SPI to transfer spiBuffer - SPI::transferBlocking(spiBuffer, nullptr, 16); + SPI::transfer(spiBuffer, nullptr, 16); } // y } // x #endif @@ -299,7 +299,7 @@ modm::SiemensS65Landscape::update() { // WRITE MEMORY CS::reset(); - SPI::transferBlocking(0x76); // start byte + SPI::transfer(0x76); // start byte const uint16_t maskBlank = 0x0000; // RRRR RGGG GGGB BBBB const uint16_t maskFilled = 0x37e0; // RRRR RGGG GGGB BBBB @@ -383,7 +383,7 @@ modm::SiemensS65Landscape::update() { } // pix // use transfer() of SPI to transfer spiBuffer - SPI::transferBlocking(spiBuffer, nullptr, bufSize); + SPI::transfer(spiBuffer, nullptr, bufSize); } // y } // x #endif diff --git a/src/modm/driver/display/siemens_s75.hpp b/src/modm/driver/display/siemens_s75.hpp index 901552e937..6a8f04e359 100644 --- a/src/modm/driver/display/siemens_s75.hpp +++ b/src/modm/driver/display/siemens_s75.hpp @@ -15,7 +15,7 @@ #ifndef MODM_SIEMENS_S75_HPP #define MODM_SIEMENS_S75_HPP -#include +#include #include #include diff --git a/src/modm/driver/display/siemens_s75.lb b/src/modm/driver/display/siemens_s75.lb index 542327ee3e..a02a3321dd 100644 --- a/src/modm/driver/display/siemens_s75.lb +++ b/src/modm/driver/display/siemens_s75.lb @@ -17,7 +17,7 @@ def init(module): def prepare(module, options): module.depends( - ":architecture:delay", + ":architecture:fiber", ":driver:memory_bus", ":ui:display") return True diff --git a/src/modm/driver/display/siemens_s75_impl.hpp b/src/modm/driver/display/siemens_s75_impl.hpp index af13983898..2c02f8b9b2 100644 --- a/src/modm/driver/display/siemens_s75_impl.hpp +++ b/src/modm/driver/display/siemens_s75_impl.hpp @@ -135,12 +135,12 @@ void modm::SiemensS75Common::lcdSettings() { // Hardware reset is low from initialize - modm::delay_ms(50); + modm::this_fiber::sleep_for(50ms); RESET::set(); - modm::delay_ms(50); + modm::this_fiber::sleep_for(50ms); interface.writeRegister(0x00, 0x0001); // R00: Start oscillation - modm::delay_ms(10); + modm::this_fiber::sleep_for(10ms); //power on sequence interface.writeRegister(0x10, 0x1f92); // R10: Power Control 1 @@ -152,7 +152,7 @@ modm::SiemensS75Common::lcdSettings() interface.writeRegister(0x02, 0x0000); // R02: LCD drive AC control interface.writeRegister(0x12, 0x040f); // R12: Power Control 2 - modm::delay_ms(100); + modm::this_fiber::sleep_for(100ms); // R03: Entry mode switch(ORIENTATION) @@ -190,7 +190,7 @@ modm::SiemensS75Common::lcdSettings() interface.writeRegister(0x44, 0x8300); // Horizontal RAM Address interface.writeRegister(0x45, 0xaf00); // Vertical RAM Address - modm::delay_ms(10); + modm::this_fiber::sleep_for(10ms); // colourful test lcdCls(0x0000); // black diff --git a/src/modm/driver/display/ssd1306.hpp b/src/modm/driver/display/ssd1306.hpp index c13ac0819a..c77e75cfe0 100644 --- a/src/modm/driver/display/ssd1306.hpp +++ b/src/modm/driver/display/ssd1306.hpp @@ -105,17 +105,19 @@ class Ssd1306 : public ssd1306, Ssd1306(uint8_t address = 0x3C); /// Pings the display + [[deprecated("Use ping() instead!")]] // DEPRECATE: 2026q3 bool inline pingBlocking() - { return RF_CALL_BLOCKING(this->ping()); } + { return this->ping(); } /// initializes for 3V3 with charge-pump + [[deprecated("Use initialize() instead!")]] // DEPRECATE: 2026q3 bool inline initializeBlocking() - { return RF_CALL_BLOCKING(initialize()); } + { return initialize(); } /// Update the display with the content of the RAM buffer. void update() override - { RF_CALL_BLOCKING(startWriteDisplay()); } + { startWriteDisplay(); } /// Use this method to synchronize writing to the displays buffer /// to avoid tearing. @@ -125,21 +127,21 @@ class Ssd1306 : public ssd1306, // MARK: - TASKS /// initializes for 3V3 with charge-pump asynchronously - modm::ResumableResult + bool initialize(); // starts a frame transfer and waits for completion - virtual modm::ResumableResult + virtual bool writeDisplay(); - modm::ResumableResult + bool setDisplayMode(DisplayMode mode = DisplayMode::Normal) { commandBuffer[0] = uint8_t(mode); return writeCommands(1); } - modm::ResumableResult + bool setContrast(uint8_t contrast = 0xCE) { commandBuffer[0] = FundamentalCommands::ContrastControl; @@ -150,20 +152,20 @@ class Ssd1306 : public ssd1306, /** * \param orientation glcd::Orientation::Landscape0 or glcd::Orientation::Landscape180 */ - modm::ResumableResult + bool setOrientation(glcd::Orientation orientation); - modm::ResumableResult + bool configureScroll(uint8_t origin, uint8_t size, ScrollDirection direction, ScrollStep steps); - modm::ResumableResult + bool enableScroll() { commandBuffer[0] = ScrollingCommands::EnableScroll; return writeCommands(1); } - modm::ResumableResult + bool disableScroll() { commandBuffer[0] = ScrollingCommands::DisableScroll; @@ -171,13 +173,13 @@ class Ssd1306 : public ssd1306, } protected: - modm::ResumableResult + bool writeCommands(std::size_t length); - virtual modm::ResumableResult + virtual void initializeMemoryMode(); - virtual modm::ResumableResult + virtual void startWriteDisplay(); uint8_t commandBuffer[7]; diff --git a/src/modm/driver/display/ssd1306_impl.hpp b/src/modm/driver/display/ssd1306_impl.hpp index cde8634f40..995d1a13cd 100644 --- a/src/modm/driver/display/ssd1306_impl.hpp +++ b/src/modm/driver/display/ssd1306_impl.hpp @@ -21,10 +21,9 @@ modm::Ssd1306::Ssd1306(uint8_t address) // ---------------------------------------------------------------------------- // MARK: - Tasks template -modm::ResumableResult +bool modm::Ssd1306::initialize() { - RF_BEGIN(); transaction_success = true; commandBuffer[0] = FundamentalCommands::DisplayOff; @@ -35,9 +34,9 @@ modm::Ssd1306::initialize() commandBuffer[4] = 63; // Range 0-63 commandBuffer[5] = HardwareConfigCommands::DisplayOffset; commandBuffer[6] = 0; // Range 0-63 - transaction_success &= RF_CALL(writeCommands(7)); + transaction_success &= writeCommands(7); - RF_CALL(initializeMemoryMode()); + initializeMemoryMode(); commandBuffer[0] = TimingAndDrivingCommands::ChargePump; commandBuffer[1] = ChargePump::V7_5; @@ -45,7 +44,7 @@ modm::Ssd1306::initialize() commandBuffer[3] = HardwareConfigCommands::ComOutputScanDirectionDecrement; commandBuffer[4] = HardwareConfigCommands::DisplayStartLine; commandBuffer[4] |= 0; // Range 0-63 - transaction_success &= RF_CALL(writeCommands(5)); + transaction_success &= writeCommands(5); commandBuffer[0] = HardwareConfigCommands::ComPinsOrder; commandBuffer[1] = Height == 64 ? 0x12 : 0x02; @@ -54,19 +53,19 @@ modm::Ssd1306::initialize() commandBuffer[4] = TimingAndDrivingCommands::PreChargePeriod; commandBuffer[5] = 1; // [3:0] Phase 1 period commandBuffer[5] |= 15 << 4;// [7:4] Phase 2 period - transaction_success &= RF_CALL(writeCommands(6)); + transaction_success &= writeCommands(6); commandBuffer[0] = TimingAndDrivingCommands::V_DeselectLevel; commandBuffer[1] = 4 << 4; // [7:4] See Datasheet commandBuffer[2] = ScrollingCommands::DisableScroll; commandBuffer[3] = FundamentalCommands::EntireDisplayResumeToRam; commandBuffer[4] = FundamentalCommands::NormalDisplay; - transaction_success &= RF_CALL(writeCommands(5)); + transaction_success &= writeCommands(5); commandBuffer[0] = FundamentalCommands::DisplayOn; - transaction_success &= RF_CALL(writeCommands(1)); + transaction_success &= writeCommands(1); - RF_END_RETURN(transaction_success); + return transaction_success; } /** @@ -75,13 +74,12 @@ modm::Ssd1306::initialize() * is send in one transaction. */ template -modm::ResumableResult +void modm::Ssd1306::initializeMemoryMode() { - RF_BEGIN(); commandBuffer[0] = AdressingCommands::MemoryMode; commandBuffer[1] = MemoryMode::HORIZONTAL; - transaction_success &= RF_CALL(writeCommands(2)); + transaction_success &= writeCommands(2); // Default on Power-up - can be omitted commandBuffer[0] = AdressingCommands::ColumnAddress; @@ -90,44 +88,33 @@ modm::Ssd1306::initializeMemoryMode() commandBuffer[3] = AdressingCommands::PageAddress; commandBuffer[4] = 0; commandBuffer[5] = 7; - transaction_success &= RF_CALL(writeCommands(6)); - - RF_END(); + transaction_success &= writeCommands(6); } // ---------------------------------------------------------------------------- template -modm::ResumableResult +void modm::Ssd1306::startWriteDisplay() { - RF_BEGIN(); - - RF_WAIT_UNTIL( - this->transaction.configureDisplayWrite((uint8_t*)(&this->buffer), sizeof(this->buffer)) and - this->startTransaction()); - - RF_END(); + modm::this_fiber::poll([&]{ return this->transaction.configureDisplayWrite((uint8_t*)(&this->buffer), sizeof(this->buffer)); }); + modm::this_fiber::poll([&]{ return this->startTransaction(); }); } template -modm::ResumableResult +bool modm::Ssd1306::writeDisplay() { - RF_BEGIN(); + startWriteDisplay(); - RF_CALL(startWriteDisplay()); + modm::this_fiber::poll([&]{ return not this->isTransactionRunning(); }); - RF_WAIT_WHILE(this->isTransactionRunning()); - - RF_END_RETURN(this->wasTransactionSuccessful()); + return this->wasTransactionSuccessful(); } template -modm::ResumableResult +bool modm::Ssd1306::setOrientation(glcd::Orientation orientation) { - RF_BEGIN(); - if (orientation == glcd::Orientation::Landscape0) { commandBuffer[0] = HardwareConfigCommands::SegmentRemap127; @@ -139,18 +126,16 @@ modm::Ssd1306::setOrientation(glcd::Orientation orientation) commandBuffer[1] = HardwareConfigCommands::ComOutputScanDirectionIncrement; } - RF_END_RETURN_CALL(writeCommands(2)); + return writeCommands(2); } template -modm::ResumableResult +bool modm::Ssd1306::configureScroll(uint8_t origin, uint8_t size, ScrollDirection direction, ScrollStep steps) { - RF_BEGIN(); - - if (!RF_CALL(disableScroll())) - RF_RETURN(false); + if (!disableScroll()) + return false; { uint8_t beginY = (origin > 7) ? 7 : origin; @@ -167,20 +152,15 @@ modm::Ssd1306::configureScroll(uint8_t origin, uint8_t size, commandBuffer[6] = 0xFF; } - RF_END_RETURN_CALL(writeCommands(7)); + return writeCommands(7); } // ---------------------------------------------------------------------------- // MARK: write command template -modm::ResumableResult +bool modm::Ssd1306::writeCommands(std::size_t length) { - RF_BEGIN(); - - RF_WAIT_UNTIL(this->startWrite(commandBuffer, length)); - - RF_WAIT_WHILE(this->isTransactionRunning()); - - RF_END_RETURN(this->wasTransactionSuccessful()); + modm::this_fiber::poll([&]{ return this->transaction.configureWrite(commandBuffer, length); }); + return this->runTransaction(); } diff --git a/src/modm/driver/display/st7036.hpp b/src/modm/driver/display/st7036.hpp index fa3c2b4fde..76982fb83f 100644 --- a/src/modm/driver/display/st7036.hpp +++ b/src/modm/driver/display/st7036.hpp @@ -15,6 +15,7 @@ #define MODM_ST7036_HPP #include +#include namespace modm { diff --git a/src/modm/driver/display/st7036_impl.hpp b/src/modm/driver/display/st7036_impl.hpp index 7d06e2c1c9..ccd349c5a5 100644 --- a/src/modm/driver/display/st7036_impl.hpp +++ b/src/modm/driver/display/st7036_impl.hpp @@ -15,8 +15,6 @@ #error "Don't include this file directly, use 'st7036.hpp' instead!" #endif -#include - // ---------------------------------------------------------------------------- namespace modm { @@ -53,7 +51,7 @@ modm::St7036::writeRaw(char c) RS::set(); CS::reset(); - SPI::transferBlocking(c); + SPI::transfer(c); CS::set(); } @@ -84,15 +82,15 @@ modm::St7036::writeCommand(uint8_t inCommand) RS::reset(); CS::reset(); - SPI::transferBlocking(inCommand); + SPI::transfer(inCommand); CS::set(); // check if the command is 'clear display' oder 'return home', these // commands take a bit longer until they are finished. if ((inCommand & 0xfc) == 0) { - modm::delay_us(1200); + modm::this_fiber::sleep_for(1200us); } else { - modm::delay_us(27); + modm::this_fiber::sleep_for(27us); } } diff --git a/src/modm/driver/display/st7565.hpp b/src/modm/driver/display/st7565.hpp index 2f7f7846d3..99a190005c 100644 --- a/src/modm/driver/display/st7565.hpp +++ b/src/modm/driver/display/st7565.hpp @@ -15,7 +15,7 @@ #define MODM_ST7565_HPP #include -#include +#include #include diff --git a/src/modm/driver/display/st7565_impl.hpp b/src/modm/driver/display/st7565_impl.hpp index 1ac77d16e7..2c6781cfaf 100644 --- a/src/modm/driver/display/st7565_impl.hpp +++ b/src/modm/driver/display/st7565_impl.hpp @@ -28,20 +28,20 @@ modm::St7565::update() { // command mode a0.reset(); - spi.transferBlocking(ST7565_PAGE_ADDRESS | y); // Row select - spi.transferBlocking(ST7565_COL_ADDRESS_MSB); // Column select high + spi.transfer(ST7565_PAGE_ADDRESS | y); // Row select + spi.transfer(ST7565_COL_ADDRESS_MSB); // Column select high if (TopView) { - spi.transferBlocking(ST7565_COL_ADDRESS_LSB | 4); // Column select low + spi.transfer(ST7565_COL_ADDRESS_LSB | 4); // Column select low } else { - spi.transferBlocking(ST7565_COL_ADDRESS_LSB); // Column select low + spi.transfer(ST7565_COL_ADDRESS_LSB); // Column select low } // switch to data mode a0.set(); for(uint8_t x = 0; x < Width; ++x) { - spi.transferBlocking(this->buffer[x][y]); + spi.transfer(this->buffer[x][y]); } } cs.set(); @@ -55,10 +55,10 @@ modm::St7565::setInvert(bool invert) a0.reset(); if (invert) { - spi.transferBlocking(ST7565_REVERSE); + spi.transfer(ST7565_REVERSE); } else { - spi.transferBlocking(ST7565_NORMAL); + spi.transfer(ST7565_NORMAL); } cs.set(); } @@ -78,7 +78,7 @@ modm::St7565::initialize( // reset the controller reset.setOutput(); reset.reset(); - modm::delay_ms(50); + modm::this_fiber::sleep_for(50ms); reset.set(); cs.reset(); @@ -86,16 +86,16 @@ modm::St7565::initialize( // View direction if (TopView) { - spi.transferBlocking(ST7565_ADC_NORMAL); // ADC normal - spi.transferBlocking(ST7565_SCAN_DIR_REVERSE); // reverse COM0~COM63 + spi.transfer(ST7565_ADC_NORMAL); // ADC normal + spi.transfer(ST7565_SCAN_DIR_REVERSE); // reverse COM0~COM63 } else { - spi.transferBlocking(ST7565_ADC_REVERSE); - spi.transferBlocking(ST7565_SCAN_DIR_NORMAL); + spi.transfer(ST7565_ADC_REVERSE); + spi.transfer(ST7565_SCAN_DIR_NORMAL); } for (uint8_t i = 0; i < size; ++i) { - spi.transferBlocking(configuration[i]); + spi.transfer(configuration[i]); } cs.set(); diff --git a/src/modm/driver/display/st7586s.lb b/src/modm/driver/display/st7586s.lb index f5e7ce778f..59db05f789 100644 --- a/src/modm/driver/display/st7586s.lb +++ b/src/modm/driver/display/st7586s.lb @@ -17,7 +17,7 @@ def init(module): def prepare(module, options): module.depends( - ":architecture:delay", + ":architecture:fiber", ":ui:display") return True diff --git a/src/modm/driver/display/st7586s_impl.hpp b/src/modm/driver/display/st7586s_impl.hpp index e29336c6d2..31e0e9c489 100644 --- a/src/modm/driver/display/st7586s_impl.hpp +++ b/src/modm/driver/display/st7586s_impl.hpp @@ -24,10 +24,10 @@ St7586s::sendCommand(Command cmd, const void *d { CS::reset(); DC::reset(); // command mode - SPI::transferBlocking(static_cast(cmd)); + SPI::transfer(static_cast(cmd)); DC::set(); // data mode if (len > 0) { - SPI::transferBlocking(reinterpret_cast(data), nullptr, len); + SPI::transfer(reinterpret_cast(data), nullptr, len); } CS::set(); // exit with data mode on @@ -45,14 +45,14 @@ St7586s::initialize() DC::setOutput(); // Reset display - modm::delay(10us); + modm::this_fiber::sleep_for(10us); RST::set(); - modm::delay(120ms); + modm::this_fiber::sleep_for(120ms); // Power ON operation flow (see datasheet) sendCommand(Command::SleepOff); sendCommand(Command::DisplayOff); - modm::delay(50ms); // t_{ON-V2} + modm::this_fiber::sleep_for(50ms); // t_{ON-V2} sendCommand(Command::SetVop, payload::SetVop(13.52f)); sendCommand(Command::SetBias, payload::SetBias::Ratio_1_11); sendCommand(Command::SetBooster, payload::SetBooster::x8); @@ -117,7 +117,7 @@ St7586s::update() validBits -= 3; currentByte >>= 3; - SPI::transferBlocking(cell); + SPI::transfer(cell); } } CS::set(); diff --git a/src/modm/driver/display/st7789.lb b/src/modm/driver/display/st7789.lb index fb1146d498..e238e2f3cb 100644 --- a/src/modm/driver/display/st7789.lb +++ b/src/modm/driver/display/st7789.lb @@ -16,7 +16,7 @@ def init(module): def prepare(module, options): module.depends( - ":architecture:delay", + ":architecture:fiber", ":ui:display") return True diff --git a/src/modm/driver/display/st7789/st7789_driver_impl.hpp b/src/modm/driver/display/st7789/st7789_driver_impl.hpp index caa6a3a3d6..8eee3088dc 100644 --- a/src/modm/driver/display/st7789/st7789_driver_impl.hpp +++ b/src/modm/driver/display/st7789/st7789_driver_impl.hpp @@ -26,10 +26,10 @@ St7789Driver::initialize() hardReset(); softReset(); - modm::delay(10ms); + modm::this_fiber::sleep_for(10ms); Interface::sendCommand(Command::SleepOff); - modm::delay(10ms); + modm::this_fiber::sleep_for(10ms); using detail::st7789::InterfacePixelFormat; InterfacePixelFormat ipf; @@ -42,7 +42,7 @@ St7789Driver::initialize() Interface::sendCommand(Command::NormalDisplayModeOn); Interface::sendCommand(Command::DisplayOn); - modm::delay(10ms); + modm::this_fiber::sleep_for(10ms); } template @@ -122,11 +122,11 @@ void St7789Driver::hardReset() { Interface::Reset::set(); - modm::delay(10ms); + modm::this_fiber::sleep_for(10ms); Interface::Reset::reset(); - modm::delay(100ms); + modm::this_fiber::sleep_for(100ms); Interface::Reset::set(); - modm::delay(10ms); + modm::this_fiber::sleep_for(10ms); } template diff --git a/src/modm/driver/display/st7789/st7789_spi_interface.hpp b/src/modm/driver/display/st7789/st7789_spi_interface.hpp index bcf204e83a..152c0001bf 100644 --- a/src/modm/driver/display/st7789/st7789_spi_interface.hpp +++ b/src/modm/driver/display/st7789/st7789_spi_interface.hpp @@ -43,7 +43,7 @@ struct St7789SPIInterface { DataCommands::reset(); Cs::reset(); - Spi::transferBlocking(command); + Spi::transfer(command); } template @@ -100,7 +100,7 @@ struct St7789SPIInterface sendData(uint8_t data) { switchToDataMode(); - Spi::transferBlocking(data); + Spi::transfer(data); end(); } @@ -108,7 +108,7 @@ struct St7789SPIInterface sendData(data_t data) { switchToDataMode(); - Spi::transferBlocking(data.data(), nullptr, data.size()); + Spi::transfer(data.data(), nullptr, data.size()); end(); } @@ -117,7 +117,7 @@ struct St7789SPIInterface sendData(const Data &data) { switchToDataMode(); - Spi::transferBlocking(reinterpret_cast(&data), nullptr, sizeof(data)); + Spi::transfer(reinterpret_cast(&data), nullptr, sizeof(data)); end(); } @@ -125,20 +125,20 @@ struct St7789SPIInterface static void continueData(uint8_t data) { - Spi::transferBlocking(data); + Spi::transfer(data); } static void continueData(data_t data) { - Spi::transferBlocking(data.data(), nullptr, data.size()); + Spi::transfer(data.data(), nullptr, data.size()); } template static void continueData(const Data &data) { - Spi::transferBlocking(reinterpret_cast(&data), nullptr, sizeof(data)); + Spi::transfer(reinterpret_cast(&data), nullptr, sizeof(data)); } //-- diff --git a/src/modm/driver/encoder/as5047.hpp b/src/modm/driver/encoder/as5047.hpp index ded1a0d7c4..2d60313a91 100644 --- a/src/modm/driver/encoder/as5047.hpp +++ b/src/modm/driver/encoder/as5047.hpp @@ -17,7 +17,7 @@ #include #include #include -#include +#include #include namespace modm @@ -91,7 +91,7 @@ struct as5047 * @ingroup modm_driver_as5047 */ template -class As5047 : public as5047, public modm::SpiDevice, protected modm::NestedResumable<5> +class As5047 : public as5047, public modm::SpiDevice { public: using Data = as5047::Data; @@ -102,7 +102,7 @@ class As5047 : public as5047, public modm::SpiDevice, protected modm: As5047(Data &data); /// Read the raw data from the sensor - modm::ResumableResult + void read(); /// Get the data object for this sensor diff --git a/src/modm/driver/encoder/as5047.lb b/src/modm/driver/encoder/as5047.lb index 14f4f4eea5..29256aa7d0 100644 --- a/src/modm/driver/encoder/as5047.lb +++ b/src/modm/driver/encoder/as5047.lb @@ -22,7 +22,7 @@ def prepare(module, options): module.depends( ":architecture:gpio", ":architecture:spi.device", - ":processing:resumable", + ":architecture:fiber", ":math:geometry" ) return True diff --git a/src/modm/driver/encoder/as5047_impl.hpp b/src/modm/driver/encoder/as5047_impl.hpp index 0107333a53..4b6b3c260e 100644 --- a/src/modm/driver/encoder/as5047_impl.hpp +++ b/src/modm/driver/encoder/as5047_impl.hpp @@ -22,29 +22,26 @@ As5047::As5047(Data &data) : data(data) } template -modm::ResumableResult +void As5047::read() { - RF_BEGIN(); - - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); outBuffer[1] = static_cast(Register::ReadAngleunc); outBuffer[0] = static_cast(static_cast(Register::ReadAngleunc) >> 8); - RF_CALL(SpiMaster::transfer(outBuffer, inBuffer, 2)); + SpiMaster::transfer(outBuffer, inBuffer, 2); Cs::set(); - modm::delay(1us); + modm::this_fiber::sleep_for(1us); Cs::reset(); outBuffer[1] = 0; outBuffer[0] = 0; - RF_CALL(SpiMaster::transfer(outBuffer, inBuffer, 2)); + SpiMaster::transfer(outBuffer, inBuffer, 2); data.data = static_cast(inBuffer[1]) | (static_cast(inBuffer[0]) << 8); if (this->releaseMaster()) { Cs::set(); } - RF_END(); } -} // namespace modm \ No newline at end of file +} // namespace modm diff --git a/src/modm/driver/encoder/as5600.hpp b/src/modm/driver/encoder/as5600.hpp index 6708d11dfd..6c0d411690 100644 --- a/src/modm/driver/encoder/as5600.hpp +++ b/src/modm/driver/encoder/as5600.hpp @@ -147,22 +147,20 @@ struct as5600 /// @ingroup modm_driver_as5600 template -class As5600 : public as5600, public modm::I2cDevice +class As5600 : public as5600, public modm::I2cDevice { public: /** AS5600 has hardwired address 0x36 * AS4500L has default address 0x40 but supports programming a different one */ - As5600(Data &data, uint8_t address = 0x36) : I2cDevice(address), data(data) {} + As5600(Data &data, uint8_t address = 0x36) : I2cDevice(address), data(data) {} /** Reset to Power up state. * Useful for developement, not required in production. */ - modm::ResumableResult + bool reset() { - RF_BEGIN(); - bool success = true; /// The config registers span from 0x00 to 0x08 @@ -172,42 +170,39 @@ class As5600 : public as5600, public modm::I2cDevice buffer[1] = 0; buffer[2] = 0; - this->transaction.configureWrite(buffer, 3); - - RF_CALL(this->runTransaction()); - success &= this->wasTransactionSuccessful(); + success &= I2cDevice::write(buffer, 3); } - RF_END_RETURN(success); + return success; } - modm::ResumableResult + bool configure(Config_t config) { return write(Register::CONF, config.value); } - modm::ResumableResult + bool setI2cAddress(uint8_t address) { return write(Register::I2C_ADDR, address); } /// Wait 1ms after setting the lower limit - modm::ResumableResult + bool setLowerLimit(Data data) { return write(Register::ZPOS, data.data); } /// Wait 1ms after setting the upper limit - modm::ResumableResult + bool setUpperLimit(Data data) { return write(Register::MPOS, data.data); } - modm::ResumableResult + bool setMaxAngle(Data data) { return write(Register::MANG, data.data); @@ -215,31 +210,29 @@ class As5600 : public as5600, public modm::I2cDevice /// Permanently burn configurations /// @warning As5600 can be burned only 3 times! - modm::ResumableResult + bool burn(Burn flags) { buffer[0] = static_cast(flags); return write(Register::BURN, buffer, 1); } - modm::ResumableResult + Data getRawValue() { - RF_BEGIN(); - - const uint16_t result = RF_CALL(read(Register::ANGLE_RAW)); + const uint16_t result = read(Register::ANGLE_RAW); // Raw value requires masking - RF_END_RETURN(Data(result & Data::max)); + return Data(result & Data::max); } - modm::ResumableResult + Status getStatus() { return static_cast(read(Register::STATUS)); } - modm::ResumableResult + uint16_t getMagnitude() { return read(Register::MAGNITUDE); @@ -257,20 +250,18 @@ class As5600 : public as5600, public modm::I2cDevice * In 5V operation, range is 0-255 * In 3.3V operation, range is reduced to 0-128 */ - modm::ResumableResult + uint8_t getAgcValue() { return read(Register::AGC); } - modm::ResumableResult + bool read() { - RF_BEGIN(); + data.data = read(Register::ANGLE); - data.data = RF_CALL(read(Register::ANGLE)); - - RF_END_RETURN(this->wasTransactionSuccessful()); + return this->wasTransactionSuccessful(); } inline Data & @@ -281,11 +272,9 @@ class As5600 : public as5600, public modm::I2cDevice private: template - modm::ResumableResult + bool write(Register reg, T value) { - RF_BEGIN(); - buffer[0] = reg; if constexpr (std::is_same_v) @@ -296,22 +285,16 @@ class As5600 : public as5600, public modm::I2cDevice buffer[1] = value >> 8; buffer[2] = value; } - - this->transaction.configureWrite(buffer, 1 + sizeof(T)); - - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::write(buffer, 1 + sizeof(T)); } template - modm::ResumableResult + T read(Register reg) { - RF_BEGIN(); - buffer[0] = reg; - this->transaction.configureWriteRead(buffer, 1, buffer + 1, sizeof(T)); - - RF_CALL(this->runTransaction()); + if (not I2cDevice::writeRead(buffer, 1, buffer + 1, sizeof(T))) + return 0; T result; @@ -323,10 +306,10 @@ class As5600 : public as5600, public modm::I2cDevice result = buffer[1] << 8 | buffer[2]; } - RF_END_RETURN(result); + return result; } Data &data; uint8_t buffer[3]; }; -} // namespace modm \ No newline at end of file +} // namespace modm diff --git a/src/modm/driver/encoder/as5600.lb b/src/modm/driver/encoder/as5600.lb index 4d327a9006..a43025cf7d 100644 --- a/src/modm/driver/encoder/as5600.lb +++ b/src/modm/driver/encoder/as5600.lb @@ -22,7 +22,7 @@ def prepare(module, options): module.depends( ":architecture:gpio", ":architecture:i2c.device", - ":processing:resumable", + ":architecture:fiber", ":math:geometry" ) return True diff --git a/src/modm/driver/encoder/bitbang_encoder_input.hpp b/src/modm/driver/encoder/bitbang_encoder_input.hpp index 1ded3f55ee..27ce10c0f7 100644 --- a/src/modm/driver/encoder/bitbang_encoder_input.hpp +++ b/src/modm/driver/encoder/bitbang_encoder_input.hpp @@ -14,6 +14,7 @@ #include #include +#include #include namespace modm diff --git a/src/modm/driver/encoder/bitbang_encoder_input.lb b/src/modm/driver/encoder/bitbang_encoder_input.lb index 7d5084f84f..6276446aa7 100644 --- a/src/modm/driver/encoder/bitbang_encoder_input.lb +++ b/src/modm/driver/encoder/bitbang_encoder_input.lb @@ -22,7 +22,7 @@ https://www.mikrocontroller.net/articles/Drehgeber. def prepare(module, options): - module.depends(":architecture:atomic") + module.depends(":architecture:atomic", ":architecture:fiber", ":platform:gpio") return True diff --git a/src/modm/driver/encoder/bitbang_encoder_input_impl.hpp b/src/modm/driver/encoder/bitbang_encoder_input_impl.hpp index c87a882a16..b9a46de41e 100644 --- a/src/modm/driver/encoder/bitbang_encoder_input_impl.hpp +++ b/src/modm/driver/encoder/bitbang_encoder_input_impl.hpp @@ -36,7 +36,7 @@ modm::BitBangEncoderInput::initialize( Signals::setInput(inputType); // Tare power-on state - modm::delay(10us); + modm::this_fiber::sleep_for(10us); raw_last = getRaw(); } diff --git a/src/modm/driver/fpga/xilinx_spartan3_impl.hpp b/src/modm/driver/fpga/xilinx_spartan3_impl.hpp index 3c9f921589..1f269e3143 100644 --- a/src/modm/driver/fpga/xilinx_spartan3_impl.hpp +++ b/src/modm/driver/fpga/xilinx_spartan3_impl.hpp @@ -71,7 +71,7 @@ modm::XilinxSpartan3::configure(const while (InitB::read() == modm::Gpio::High || Done::read() == modm::Gpio::High) { - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); if (counter++ > 1000) { // Timeout (1ms) reached, FPGA is not responding abort configuration MODM_LOG_ERROR << MODM_FILE_INFO; @@ -82,14 +82,14 @@ modm::XilinxSpartan3::configure(const } // Led1::reset(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); ProgB::set(); // Wait until INIT_B goes high uint32_t counter = 0; while (InitB::read() == modm::Gpio::Low) { - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); if (counter++ > 1000) { // Timeout (1ms) reached, FPGA is not responding abort configuration MODM_LOG_ERROR << MODM_FILE_INFO; @@ -100,7 +100,7 @@ modm::XilinxSpartan3::configure(const // Led2::reset(); // wait 0.5..4us before starting the configuration - modm::delay_us(4); + modm::this_fiber::sleep_for(4us); uint8_t buffer[256]; diff --git a/src/modm/driver/fpga/xilinx_spartan6_impl.hpp b/src/modm/driver/fpga/xilinx_spartan6_impl.hpp index 5b1a54cffe..de41e7756f 100644 --- a/src/modm/driver/fpga/xilinx_spartan6_impl.hpp +++ b/src/modm/driver/fpga/xilinx_spartan6_impl.hpp @@ -55,7 +55,7 @@ modm::XilinxSpartan6Parallel 1000) { // Timeout (1ms) reached, FPGA is not responding abort configuration MODM_LOG_ERROR << MODM_FILE_INFO; @@ -74,7 +74,7 @@ modm::XilinxSpartan6Parallel 1000) { // Timeout (1ms) reached, FPGA is not responding abort configuration MODM_LOG_ERROR << MODM_FILE_INFO; diff --git a/src/modm/driver/gpio/mcp23_transport.hpp b/src/modm/driver/gpio/mcp23_transport.hpp index 5507b52570..a7afc7d34f 100644 --- a/src/modm/driver/gpio/mcp23_transport.hpp +++ b/src/modm/driver/gpio/mcp23_transport.hpp @@ -14,7 +14,7 @@ #include #include -#include +#include namespace modm { @@ -33,7 +33,7 @@ namespace modm * @author Niklas Hauser */ template < class I2cMaster > -class Mcp23TransportI2c : public modm::I2cDevice< I2cMaster, 2 > +class Mcp23TransportI2c : public modm::I2cDevice< I2cMaster > { public: Mcp23TransportI2c(uint8_t address); @@ -41,22 +41,22 @@ class Mcp23TransportI2c : public modm::I2cDevice< I2cMaster, 2 > protected: // RAW REGISTER ACCESS /// write a 8bit value - modm::ResumableResult + bool write(uint8_t reg, uint8_t value); /// write a 16bit value - modm::ResumableResult + bool write16(uint8_t reg, uint16_t value); /// read a 8bit value - modm::ResumableResult + bool read(uint8_t reg, uint8_t &value) { return read(reg, &value, 1); } /// read multiple 8bit values from a start register - modm::ResumableResult + bool read(uint8_t reg, uint8_t *buffer, uint8_t length); private: @@ -78,34 +78,34 @@ class Mcp23TransportI2c : public modm::I2cDevice< I2cMaster, 2 > * @author Niklas Hauser */ template < class SpiMaster, class Cs > -class Mcp23TransportSpi : public modm::SpiDevice< SpiMaster >, protected modm::NestedResumable<2> +class Mcp23TransportSpi : public modm::SpiDevice< SpiMaster > { public: Mcp23TransportSpi(uint8_t address); /// pings the sensor - modm::ResumableResult + bool ping(); protected: // RAW REGISTER ACCESS /// write a 8bit value - modm::ResumableResult + bool write(uint8_t reg, uint8_t value); /// write a 16bit value - modm::ResumableResult + bool write16(uint8_t reg, uint16_t value); /// read a 8bit value - modm::ResumableResult + bool read(uint8_t reg, uint8_t &value) { return read(reg, &value, 1); } /// read multiple 8bit values from a start register - modm::ResumableResult + bool read(uint8_t reg, uint8_t *buffer, uint8_t length); private: diff --git a/src/modm/driver/gpio/mcp23_transport_impl.hpp b/src/modm/driver/gpio/mcp23_transport_impl.hpp index 12137c6c42..03b70d1eda 100644 --- a/src/modm/driver/gpio/mcp23_transport_impl.hpp +++ b/src/modm/driver/gpio/mcp23_transport_impl.hpp @@ -17,52 +17,38 @@ // MARK: I2C TRANSPORT template < class I2cMaster > modm::Mcp23TransportI2c::Mcp23TransportI2c(uint8_t address) -: I2cDevice(address) +: I2cDevice(address) { } // MARK: - register access // MARK: write register template < class I2cMaster > -modm::ResumableResult +bool modm::Mcp23TransportI2c::write(uint8_t reg, uint8_t value) { - RF_BEGIN(); - buffer[0] = reg; buffer[1] = value; - - this->transaction.configureWrite(buffer, 2); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(buffer, 2); } template < class I2cMaster > -modm::ResumableResult +bool modm::Mcp23TransportI2c::write16(uint8_t reg, uint16_t value) { - RF_BEGIN(); - buffer[0] = reg; buffer[1] = value; buffer[2] = (value >> 8); - - this->transaction.configureWrite(buffer, 3); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(buffer, 3); } // MARK: read register template < class I2cMaster > -modm::ResumableResult +bool modm::Mcp23TransportI2c::read(uint8_t reg, uint8_t *buffer, uint8_t length) { - RF_BEGIN(); - this->buffer[0] = reg; - this->transaction.configureWriteRead(this->buffer, 1, buffer, length); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::writeRead(this->buffer, 1, buffer, length); } // ============================================================================ @@ -76,72 +62,65 @@ modm::Mcp23TransportSpi::Mcp23TransportSpi(uint8_t address) // MARK: ping template < class SpiMaster, class Cs > -modm::ResumableResult +bool modm::Mcp23TransportSpi::ping() { - RF_BEGIN(); - RF_END_RETURN(true); + return true; } // MARK: - register access // MARK: write register template < class SpiMaster, class Cs > -modm::ResumableResult +bool modm::Mcp23TransportSpi::write(uint8_t reg, uint8_t value) { - RF_BEGIN(); - - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); - RF_CALL(SpiMaster::transfer(address | Write)); - RF_CALL(SpiMaster::transfer(reg)); - RF_CALL(SpiMaster::transfer(value)); + SpiMaster::transfer(address | Write); + SpiMaster::transfer(reg); + SpiMaster::transfer(value); if (this->releaseMaster()) Cs::set(); - RF_END_RETURN(true); + return true; } template < class SpiMaster, class Cs > -modm::ResumableResult +bool modm::Mcp23TransportSpi::write16(uint8_t reg, uint16_t value) { - RF_BEGIN(); - - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); - RF_CALL(SpiMaster::transfer(address | Write)); - RF_CALL(SpiMaster::transfer(reg)); - RF_CALL(SpiMaster::transfer(value)); - RF_CALL(SpiMaster::transfer(value >> 8)); + SpiMaster::transfer(address | Write); + SpiMaster::transfer(reg); + SpiMaster::transfer(value); + SpiMaster::transfer(value >> 8); if (this->releaseMaster()) Cs::set(); - RF_END_RETURN(true); + return true; } // MARK: read register template < class SpiMaster, class Cs > -modm::ResumableResult +bool modm::Mcp23TransportSpi::read(uint8_t reg, uint8_t *buffer, uint8_t length) { - RF_BEGIN(); - - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); - RF_CALL(SpiMaster::transfer(address | Read)); - RF_CALL(SpiMaster::transfer(reg)); + SpiMaster::transfer(address | Read); + SpiMaster::transfer(reg); - RF_CALL(SpiMaster::transfer(nullptr, buffer, length)); + SpiMaster::transfer(nullptr, buffer, length); if (this->releaseMaster()) Cs::set(); - RF_END_RETURN(true); + return true; } diff --git a/src/modm/driver/gpio/mcp23s08.hpp b/src/modm/driver/gpio/mcp23s08.hpp index 73fadf517d..e455be2549 100644 --- a/src/modm/driver/gpio/mcp23s08.hpp +++ b/src/modm/driver/gpio/mcp23s08.hpp @@ -16,6 +16,7 @@ #include #include +#include namespace modm { diff --git a/src/modm/driver/gpio/mcp23s08_impl.hpp b/src/modm/driver/gpio/mcp23s08_impl.hpp index 2a526ad339..b07319bd3d 100644 --- a/src/modm/driver/gpio/mcp23s08_impl.hpp +++ b/src/modm/driver/gpio/mcp23s08_impl.hpp @@ -36,7 +36,7 @@ modm::Mcp23s08::initialize() cs.setOutput(); interrupt.setInput(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); // disable address pins (as they are by default) and enable the // open-drain output from the interrupt line @@ -46,7 +46,7 @@ modm::Mcp23s08::initialize() spi.write(1 << 2); cs.set(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); } template @@ -59,7 +59,7 @@ modm::Mcp23s08::configure(uint8_t inputMask, uint8_t pullupMask) spi.write(inputMask); cs.set(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); cs.reset(); spi.write(deviceAddress | WRITE); @@ -67,7 +67,7 @@ modm::Mcp23s08::configure(uint8_t inputMask, uint8_t pullupMask) spi.write(pullupMask); cs.set(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); } //void @@ -85,7 +85,7 @@ modm::Mcp23s08::read() uint8_t value = spi.write(0x00); cs.set(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); return value; } @@ -100,5 +100,5 @@ modm::Mcp23s08::write(uint8_t output) spi.write(output); cs.set(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); } diff --git a/src/modm/driver/gpio/mcp23x17.hpp b/src/modm/driver/gpio/mcp23x17.hpp index 0909bcf9f6..c29e9b9d11 100644 --- a/src/modm/driver/gpio/mcp23x17.hpp +++ b/src/modm/driver/gpio/mcp23x17.hpp @@ -114,22 +114,22 @@ class Mcp23x17 : public mcp23x17, public Transport, public modm::GpioExpander Mcp23x17(uint8_t address=0x20); public: - modm::ResumableResult + bool initialize(); - modm::ResumableResult + bool setOutput(Pins pins); - modm::ResumableResult + bool set(Pins pins); - modm::ResumableResult + bool reset(Pins pins); - modm::ResumableResult + bool toggle(Pins pins); - modm::ResumableResult + bool set(Pins pins, bool value); bool @@ -149,19 +149,19 @@ class Mcp23x17 : public mcp23x17, public Transport, public modm::GpioExpander } public: - modm::ResumableResult + bool setInput(Pins pins); - modm::ResumableResult + bool setPullUp(Pins pins); - modm::ResumableResult + bool resetPullUp(Pins pins); - modm::ResumableResult + bool setInvertInput(Pins pins); - modm::ResumableResult + bool resetInvertInput(Pins pins); bool @@ -171,19 +171,19 @@ class Mcp23x17 : public mcp23x17, public Transport, public modm::GpioExpander return memory.gpio.any(pin); } - modm::ResumableResult inline + bool inline readInput() { return Transport::read(i(Register::GPIO), buffer + 18, 2); } - modm::ResumableResult inline + bool inline readAllInput() { return Transport::read(i(Register::INTF), buffer + 14, 8); } public: - modm::ResumableResult + bool writePort(PortType data); - modm::ResumableResult + bool readPort(PortType &data); public: diff --git a/src/modm/driver/gpio/mcp23x17.lb b/src/modm/driver/gpio/mcp23x17.lb index 5359233e88..a525035aba 100644 --- a/src/modm/driver/gpio/mcp23x17.lb +++ b/src/modm/driver/gpio/mcp23x17.lb @@ -34,7 +34,7 @@ def prepare(module, options): ":architecture:i2c.device", ":architecture:register", ":architecture:spi.device", - ":processing:resumable") + ":architecture:fiber") return True def build(env): diff --git a/src/modm/driver/gpio/mcp23x17_impl.hpp b/src/modm/driver/gpio/mcp23x17_impl.hpp index 870b33e6e2..6d74e50973 100644 --- a/src/modm/driver/gpio/mcp23x17_impl.hpp +++ b/src/modm/driver/gpio/mcp23x17_impl.hpp @@ -21,159 +21,135 @@ modm::Mcp23x17::Mcp23x17(uint8_t address) : } template < class Transport > -modm::ResumableResult +bool modm::Mcp23x17::initialize() { - RF_BEGIN(); - memory = Memory(); // reset local register cache - RF_CALL(this->write(i(Register::IOCON), memory.controlA.value)); - - RF_CALL(this->write16(i(Register::IODIR), memory.direction.value)); - RF_CALL(this->write16(i(Register::IPOL), memory.polarity.value)); - RF_CALL(this->write16(i(Register::GPINTEN), memory.interruptEnable.value)); - RF_CALL(this->write16(i(Register::DEFVAL), memory.interruptDefault.value)); - RF_CALL(this->write16(i(Register::INTCON), memory.interruptControl.value)); - RF_CALL(this->write16(i(Register::GPPU), memory.pullup.value)); - RF_CALL(this->write16(i(Register::GPIO), memory.gpio.value)); - RF_CALL(this->write16(i(Register::OLAT), memory.outputLatch.value)); - RF_CALL(Transport::read(i(Register::INTF), buffer + 14, 4)); - - RF_END_RETURN_CALL( Transport::read(i(Register::IODIR), buffer, sizeof(buffer)) ); + this->write(i(Register::IOCON), memory.controlA.value); + + this->write16(i(Register::IODIR), memory.direction.value); + this->write16(i(Register::IPOL), memory.polarity.value); + this->write16(i(Register::GPINTEN), memory.interruptEnable.value); + this->write16(i(Register::DEFVAL), memory.interruptDefault.value); + this->write16(i(Register::INTCON), memory.interruptControl.value); + this->write16(i(Register::GPPU), memory.pullup.value); + this->write16(i(Register::GPIO), memory.gpio.value); + this->write16(i(Register::OLAT), memory.outputLatch.value); + Transport::read(i(Register::INTF), buffer + 14, 4); + + return Transport::read(i(Register::IODIR), buffer, sizeof(buffer)); } // MARK: - Tasks template < class Transport > -modm::ResumableResult +bool modm::Mcp23x17::setOutput(Pins pins) { - RF_BEGIN(); - // output is 0, input is 1 memory.direction.reset(pins); - RF_END_RETURN_CALL( this->write16(i(Register::IODIR), memory.direction.value) ); + return this->write16(i(Register::IODIR), memory.direction.value); } template < class Transport > -modm::ResumableResult +bool modm::Mcp23x17::set(Pins pins) { - RF_BEGIN(); - // high is 1, low is 0 // set output latches locally, but only those that are output memory.outputLatch.set(pins & ~memory.direction); - RF_END_RETURN_CALL( this->write16(i(Register::GPIO), memory.outputLatch.value) ); + return this->write16(i(Register::GPIO), memory.outputLatch.value); } template < class Transport > -modm::ResumableResult +bool modm::Mcp23x17::reset(Pins pins) { - RF_BEGIN(); - // high is 1, low is 0 // reset reset output latches locally, but only those that are output memory.outputLatch.reset(pins & ~memory.direction); - RF_END_RETURN_CALL( this->write16(i(Register::GPIO), memory.outputLatch.value) ); + return this->write16(i(Register::GPIO), memory.outputLatch.value); } template < class Transport > -modm::ResumableResult +bool modm::Mcp23x17::toggle(Pins pins) { - RF_BEGIN(); - // high is 1, low is 0 // toggle output latches locally, but only those that are output memory.outputLatch.toggle(pins & ~memory.direction); - RF_END_RETURN_CALL( this->write16(i(Register::GPIO), memory.outputLatch.value) ); + return this->write16(i(Register::GPIO), memory.outputLatch.value); } template < class Transport > -modm::ResumableResult +bool modm::Mcp23x17::set(Pins pins, bool value) { - RF_BEGIN(); - // high is 1, low is 0 // update output latches locally, but only those that are output memory.outputLatch.update(pins & ~memory.direction, value); - RF_END_RETURN_CALL( this->write16(i(Register::GPIO), memory.outputLatch.value) ); + return this->write16(i(Register::GPIO), memory.outputLatch.value); } template < class Transport > -modm::ResumableResult +bool modm::Mcp23x17::setInput(Pins pins) { - RF_BEGIN(); - // output is 0, input is 1 memory.direction.set(pins); memory.outputLatch.reset(pins); - RF_END_RETURN_CALL( this->write16(i(Register::IODIR), memory.direction.value) ); + return this->write16(i(Register::IODIR), memory.direction.value); } template < class Transport > -modm::ResumableResult +bool modm::Mcp23x17::setPullUp(Pins pins) { - RF_BEGIN(); - // inverted is 1, normal is 0 memory.pullup.set(pins); - RF_END_RETURN_CALL( this->write16(i(Register::GPPU), memory.pullup.value) ); + return this->write16(i(Register::GPPU), memory.pullup.value); } template < class Transport > -modm::ResumableResult +bool modm::Mcp23x17::resetPullUp(Pins pins) { - RF_BEGIN(); - // inverted is 1, normal is 0 memory.pullup.reset(pins); - RF_END_RETURN_CALL( this->write16(i(Register::GPPU), memory.pullup.value) ); + return this->write16(i(Register::GPPU), memory.pullup.value); } template < class Transport > -modm::ResumableResult +bool modm::Mcp23x17::setInvertInput(Pins pins) { - RF_BEGIN(); - // inverted is 1, normal is 0 memory.polarity.set(pins); - RF_END_RETURN_CALL( this->write16(i(Register::IPOL), memory.polarity.value) ); + return this->write16(i(Register::IPOL), memory.polarity.value); } template < class Transport > -modm::ResumableResult +bool modm::Mcp23x17::resetInvertInput(Pins pins) { - RF_BEGIN(); - // inverted is 1, normal is 0 memory.polarity.reset(pins); - RF_END_RETURN_CALL( this->write16(i(Register::IPOL), memory.polarity.value) ); + return this->write16(i(Register::IPOL), memory.polarity.value); } template < class Transport > -modm::ResumableResult +bool modm::Mcp23x17::writePort(uint16_t data) { - RF_BEGIN(); - // high is 1, low is 0 // output is 0, input is 1 // set output latches locally, but only those that are output @@ -182,20 +158,18 @@ modm::Mcp23x17::writePort(uint16_t data) // set masked output values memory.outputLatch.set(Pins(data) & ~memory.direction); - RF_END_RETURN_CALL( this->write16(i(Register::GPIO), memory.outputLatch.value) ); + return this->write16(i(Register::GPIO), memory.outputLatch.value); } template < class Transport > -modm::ResumableResult +bool modm::Mcp23x17::readPort(uint16_t &data) { - RF_BEGIN(); - - if (RF_CALL( readInput() )) + if (readInput()) { data = memory.gpio.value; - RF_RETURN( true ); + return true; } - RF_END_RETURN( false ); + return false; } diff --git a/src/modm/driver/gpio/pca8574.hpp b/src/modm/driver/gpio/pca8574.hpp index 4a6cdd8641..f7099513f5 100644 --- a/src/modm/driver/gpio/pca8574.hpp +++ b/src/modm/driver/gpio/pca8574.hpp @@ -16,7 +16,7 @@ #include #include #include -#include +#include namespace modm { @@ -52,7 +52,7 @@ struct pca8574 * @author Niklas Hauser */ template < class I2cMaster > -class Pca8574 : public pca8574, public modm::I2cDevice< I2cMaster, 2 >, public modm::GpioExpander +class Pca8574 : public pca8574, public modm::I2cDevice< I2cMaster >, public modm::GpioExpander { public: static constexpr uint8_t width = 8; @@ -70,24 +70,23 @@ class Pca8574 : public pca8574, public modm::I2cDevice< I2cMaster, 2 >, public m Pca8574(uint8_t address=0x27); public: - modm::ResumableResult inline + bool inline setOutput(Pins pins) { - RF_BEGIN(); direction.set(pins); - RF_END_RETURN(true); + return true; } - modm::ResumableResult + bool set(Pins pins); - modm::ResumableResult + bool reset(Pins pins); - modm::ResumableResult + bool toggle(Pins pins); - modm::ResumableResult + bool set(Pins pins, bool value); bool inline @@ -105,7 +104,7 @@ class Pca8574 : public pca8574, public modm::I2cDevice< I2cMaster, 2 >, public m } public: - modm::ResumableResult + bool setInput(Pins pins); bool inline @@ -116,15 +115,15 @@ class Pca8574 : public pca8574, public modm::I2cDevice< I2cMaster, 2 >, public m } public: - modm::ResumableResult inline + bool inline readInput() { return readPort(input.value); } public: - modm::ResumableResult + bool writePort(PortType value); - modm::ResumableResult + bool readPort(PortType &value); public: diff --git a/src/modm/driver/gpio/pca8574.lb b/src/modm/driver/gpio/pca8574.lb index 9c95175b77..edaee920e7 100644 --- a/src/modm/driver/gpio/pca8574.lb +++ b/src/modm/driver/gpio/pca8574.lb @@ -20,7 +20,7 @@ def prepare(module, options): ":architecture:gpio.expander", ":architecture:i2c.device", ":architecture:register", - ":processing:resumable") + ":architecture:fiber") return True def build(env): diff --git a/src/modm/driver/gpio/pca8574_impl.hpp b/src/modm/driver/gpio/pca8574_impl.hpp index 3a0fba7bc1..df4cb82ebf 100644 --- a/src/modm/driver/gpio/pca8574_impl.hpp +++ b/src/modm/driver/gpio/pca8574_impl.hpp @@ -16,101 +16,85 @@ template < class I2cMaster > modm::Pca8574::Pca8574(uint8_t address): - I2cDevice(address), + I2cDevice(address), direction(Pins(0xff)), output(Pins(0xff)), input(Pins(0xff)) { } template < class I2cMaster > -modm::ResumableResult +bool modm::Pca8574::set(Pins pins) { - RF_BEGIN(); - // high is 1, low is 0 // we can _always_ set the pins high output.set(pins); - RF_END_RETURN_CALL( writePort(output.value) ); + return writePort(output.value); } template < class I2cMaster > -modm::ResumableResult +bool modm::Pca8574::reset(Pins pins) { - RF_BEGIN(); - // high is 1, low is 0 // only reset those that are actually configured as output output.reset(pins & direction); - RF_END_RETURN_CALL( writePort(output.value) ); + return writePort(output.value); } template < class I2cMaster > -modm::ResumableResult +bool modm::Pca8574::toggle(Pins pins) { - RF_BEGIN(); - // high is 1, low is 0 // only toggle those that are actually configured as output output.toggle(pins & direction); - RF_END_RETURN_CALL( writePort(output.value) ); + return writePort(output.value); } template < class I2cMaster > -modm::ResumableResult +bool modm::Pca8574::set(Pins pins, bool value) { - RF_BEGIN(); - // high is 1, low is 0 // only update those that are actually configured as output output.update(pins & direction, value); - RF_END_RETURN_CALL( writePort(output.value) ); + return writePort(output.value); } template < class I2cMaster > -modm::ResumableResult +bool modm::Pca8574::setInput(Pins pins) { - RF_BEGIN(); - // reset direction bits direction.reset(pins); // set pins high, which means input (open-drain, remember?) output.set(pins); - RF_END_RETURN_CALL( writePort(output.value) ); + return writePort(output.value); } template < class I2cMaster > -modm::ResumableResult +bool modm::Pca8574::writePort(uint8_t value) { - RF_BEGIN(); - output.value = value; - this->transaction.configureWrite(&output.value, 1); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(&output.value, 1); }; template < class I2cMaster > -modm::ResumableResult +bool modm::Pca8574::readPort(uint8_t &value) { - RF_BEGIN(); - this->transaction.configureRead(&input.value, 1); - if (RF_CALL(this->runTransaction())) + if (I2cDevice::read(&input.value, 1)) { value = input.value; - RF_RETURN( true ); + return true; } - RF_END_RETURN( false ); + return false; }; diff --git a/src/modm/driver/gpio/pca9535.hpp b/src/modm/driver/gpio/pca9535.hpp index 04a0627c0e..813546777c 100644 --- a/src/modm/driver/gpio/pca9535.hpp +++ b/src/modm/driver/gpio/pca9535.hpp @@ -71,7 +71,7 @@ struct pca9535 * @ingroup modm_driver_pca9535 */ template < typename I2cMaster > -class Pca9535 : public pca9535, public modm::I2cDevice< I2cMaster, 2 >, public modm::GpioExpander +class Pca9535 : public pca9535, public modm::I2cDevice< I2cMaster >, public modm::GpioExpander { enum class Index : uint8_t @@ -98,19 +98,19 @@ class Pca9535 : public pca9535, public modm::I2cDevice< I2cMaster, 2 >, public m Pca9535(uint8_t address=0x20); public: - modm::ResumableResult + bool setOutput(Pins pins); - modm::ResumableResult + bool set(Pins pins); - modm::ResumableResult + bool reset(Pins pins); - modm::ResumableResult + bool toggle(Pins pins); - modm::ResumableResult + bool set(Pins pins, bool value); bool inline @@ -130,13 +130,13 @@ class Pca9535 : public pca9535, public modm::I2cDevice< I2cMaster, 2 >, public m } public: - modm::ResumableResult + bool setInput(Pins pins); - modm::ResumableResult + bool setInvertInput(Pins pins); - modm::ResumableResult + bool resetInvertInput(Pins pins); bool inline @@ -146,15 +146,15 @@ class Pca9535 : public pca9535, public modm::I2cDevice< I2cMaster, 2 >, public m return memory.input.all(pins); } - modm::ResumableResult inline + bool inline readInput() { return readMemory(Index::Input); } public: - modm::ResumableResult + bool writePort(PortType data); - modm::ResumableResult + bool readPort(PortType &data); @@ -218,10 +218,10 @@ class Pca9535 : public pca9535, public modm::I2cDevice< I2cMaster, 2 >, public m using Port = GpioExpanderPort< Pca9535, object, StartPin, Width, DataOrder >; private: - modm::ResumableResult + bool writeMemory(Index index); - modm::ResumableResult + bool readMemory(Index index); /// @cond diff --git a/src/modm/driver/gpio/pca9535.lb b/src/modm/driver/gpio/pca9535.lb index df528f88d2..a76dc80022 100644 --- a/src/modm/driver/gpio/pca9535.lb +++ b/src/modm/driver/gpio/pca9535.lb @@ -31,12 +31,12 @@ getters. using Pin = expander.Pin; using Pins = expander.Pins; -RF_CALL_BLOCKING(expander.setOutput(Pins(0xff)); // set all lower 8 pins to output -RF_CALL_BLOCKING(expander.set(Pin::P0_0)); // set only pin 00 high -RF_CALL_BLOCKING(expander.reset(Pin::P0_1 | Pin::P0_2)); // reset 2 pins -RF_CALL_BLOCKING(expander.toggle(Pin::P0_3 | Pin::P0_4 | Pin::P0_5)); // toggle 3 pins +expander.setOutput(Pins(0xff); // set all lower 8 pins to output +expander.set(Pin::P0_0); // set only pin 00 high +expander.reset(Pin::P0_1 | Pin::P0_2); // reset 2 pins +expander.toggle(Pin::P0_3 | Pin::P0_4 | Pin::P0_5); // toggle 3 pins -RF_CALL_BLOCKING(expander.readInput()); // transfer input states from the external chip +expander.readInput(); // transfer input states from the external chip bool high = expander.read(Pin::P1_0); // check a single pin Pins input = expander.getInputs(); // get all 16 input states @@ -49,7 +49,7 @@ def prepare(module, options): ":architecture:gpio.expander", ":architecture:i2c.device", ":architecture:register", - ":processing:resumable", + ":architecture:fiber", ":math:utils") return True diff --git a/src/modm/driver/gpio/pca9535_impl.hpp b/src/modm/driver/gpio/pca9535_impl.hpp index ac048f7cbd..3c6d582162 100644 --- a/src/modm/driver/gpio/pca9535_impl.hpp +++ b/src/modm/driver/gpio/pca9535_impl.hpp @@ -16,157 +16,129 @@ // ---------------------------------------------------------------------------- template < typename I2cMaster > modm::Pca9535::Pca9535(uint8_t address) -: I2cDevice(address), memory() +: I2cDevice(address), memory() { } // MARK: - Tasks template < typename I2cMaster > -modm::ResumableResult +bool modm::Pca9535::setOutput(Pins pins) { - RF_BEGIN(); - // output is 0, input is 1 memory.configuration.reset(pins); - RF_END_RETURN_CALL( writeMemory(Index::Configuration) ); + return writeMemory(Index::Configuration); } template < typename I2cMaster > -modm::ResumableResult +bool modm::Pca9535::set(Pins pins) { - RF_BEGIN(); - // high is 1, low is 0 memory.output.set(pins); - RF_END_RETURN_CALL( writeMemory(Index::Output) ); + return writeMemory(Index::Output); } template < typename I2cMaster > -modm::ResumableResult +bool modm::Pca9535::reset(Pins pins) { - RF_BEGIN(); - // high is 1, low is 0 memory.output.reset(pins); - RF_END_RETURN_CALL( writeMemory(Index::Output) ); + return writeMemory(Index::Output); } template < typename I2cMaster > -modm::ResumableResult +bool modm::Pca9535::toggle(Pins pins) { - RF_BEGIN(); - // high is 1, low is 0 memory.output.toggle(pins); - RF_END_RETURN_CALL( writeMemory(Index::Output) ); + return writeMemory(Index::Output); } template < typename I2cMaster > -modm::ResumableResult +bool modm::Pca9535::set(Pins pins, bool value) { - RF_BEGIN(); - // high is 1, low is 0 memory.output.update(pins, value); - RF_END_RETURN_CALL( writeMemory(Index::Output) ); + return writeMemory(Index::Output); } template < typename I2cMaster > -modm::ResumableResult +bool modm::Pca9535::setInput(Pins pins) { - RF_BEGIN(); - // output is 0, input is 1 memory.configuration.set(pins); - RF_END_RETURN_CALL( writeMemory(Index::Configuration) ); + return writeMemory(Index::Configuration); } template < typename I2cMaster > -modm::ResumableResult +bool modm::Pca9535::setInvertInput(Pins pins) { - RF_BEGIN(); - // inverted is 1, normal is 0 memory.polarity.set(pins); - RF_END_RETURN_CALL( writeMemory(Index::Polarity) ); + return writeMemory(Index::Polarity); } template < typename I2cMaster > -modm::ResumableResult +bool modm::Pca9535::resetInvertInput(Pins pins) { - RF_BEGIN(); - // inverted is 1, normal is 0 memory.polarity.reset(pins); - RF_END_RETURN_CALL( writeMemory(Index::Polarity) ); + return writeMemory(Index::Polarity); } template < typename I2cMaster > -modm::ResumableResult +bool modm::Pca9535::writePort(uint16_t data) { - RF_BEGIN(); - // high is 1, low is 0 memory.output.value = data; - RF_END_RETURN_CALL( writeMemory(Index::Output) ); + return writeMemory(Index::Output); } template < typename I2cMaster > -modm::ResumableResult +bool modm::Pca9535::readPort(uint16_t &data) { - RF_BEGIN(); - - if ( RF_CALL(readInput()) ) + if ( readInput() ) { // high is 1, low is 0 data = memory.input.value; - RF_RETURN( true ); + return true; } - RF_END_RETURN( false ); + return false; } // MARK: write multilength register template < class I2cMaster > -modm::ResumableResult +bool modm::Pca9535::writeMemory(Index index) { - RF_BEGIN(); - - this->transaction.configureWrite(buffer + uint8_t(index), 3); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(buffer + uint8_t(index), 3); } // MARK: read multilength register template < class I2cMaster > -modm::ResumableResult +bool modm::Pca9535::readMemory(Index index) { - RF_BEGIN(); - - this->transaction.configureWriteRead( + retur I2cDevice::writeRead( buffer + uint8_t(index) , 1, buffer + uint8_t(index) + 1, 2); - - RF_END_RETURN_CALL( this->runTransaction() ); } diff --git a/src/modm/driver/gpio/pca9548a.hpp b/src/modm/driver/gpio/pca9548a.hpp index 641fedeab6..8f3ff1bee6 100644 --- a/src/modm/driver/gpio/pca9548a.hpp +++ b/src/modm/driver/gpio/pca9548a.hpp @@ -28,18 +28,18 @@ namespace modm * @author Sascha Schade */ template < typename I2cMaster > -class Pca9548a : public modm::I2cDevice< I2cMaster, 2 > +class Pca9548a : public modm::I2cDevice< I2cMaster > { public: Pca9548a(uint8_t address=0b1110000); - modm::ResumableResult + bool setActiveChannel(uint8_t channel); - modm::ResumableResult + bool readCommandRegister(uint8_t &command_register); - modm::ResumableResult + bool writeCommandRegister(uint8_t command_register); private: diff --git a/src/modm/driver/gpio/pca9548a.lb b/src/modm/driver/gpio/pca9548a.lb index f865dab2e6..03aeab4f3d 100644 --- a/src/modm/driver/gpio/pca9548a.lb +++ b/src/modm/driver/gpio/pca9548a.lb @@ -24,7 +24,7 @@ def prepare(module, options): module.depends( ":architecture:i2c.device", ":architecture:i2c.multiplexer", - ":processing:resumable") + ":architecture:fiber") return True def build(env): diff --git a/src/modm/driver/gpio/pca9548a_impl.hpp b/src/modm/driver/gpio/pca9548a_impl.hpp index 825d58b770..6b0bbe4406 100644 --- a/src/modm/driver/gpio/pca9548a_impl.hpp +++ b/src/modm/driver/gpio/pca9548a_impl.hpp @@ -18,51 +18,41 @@ template < typename I2cMaster > modm::Pca9548a::Pca9548a(uint8_t address) -: I2cDevice{address} +: I2cDevice{address} { } template < typename I2cMaster > -modm::ResumableResult +bool modm::Pca9548a::setActiveChannel(uint8_t channel) { - RF_BEGIN(); - if (channel >= CHANNELS) { - RF_RETURN(false); + return false; } // Channel to mask - RF_END_RETURN_CALL( writeCommandRegister(1 << channel) ); + return writeCommandRegister(1 << channel); } template < typename I2cMaster > -modm::ResumableResult +bool modm::Pca9548a::readCommandRegister(uint8_t &command_register) { - RF_BEGIN(); - - this->transaction.configureRead(buffer, 1); command_register = buffer[0]; current_command_register = command_register; - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::read(buffer, 1); } template < typename I2cMaster > -modm::ResumableResult +bool modm::Pca9548a::writeCommandRegister(uint8_t command_register) { - RF_BEGIN(); - if (current_command_register == command_register) { // No change, return without bus operation - RF_RETURN(true); + return true; } current_command_register = command_register; buffer[0] = command_register; - this->transaction.configureWrite(buffer, 1); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(buffer, 1); } diff --git a/src/modm/driver/inertial/adis16470.hpp b/src/modm/driver/inertial/adis16470.hpp index 9928fb8868..9beff1ee56 100644 --- a/src/modm/driver/inertial/adis16470.hpp +++ b/src/modm/driver/inertial/adis16470.hpp @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include namespace modm @@ -184,9 +184,7 @@ struct adis16470 * \author Raphael Lehmann */ template -class Adis16470 : public adis16470, - public modm::SpiDevice, - protected modm::NestedResumable<2> +class Adis16470 : public adis16470, public modm::SpiDevice { public: /** @@ -198,7 +196,7 @@ class Adis16470 : public adis16470, * @warning The SPI frequency must not exceed 2 MHz for this chip, * or 1 MHz with burst mode. */ - modm::ResumableResult + void initialize(); /** @@ -208,7 +206,7 @@ class Adis16470 : public adis16470, * @return The register value in case of a read access, or std::nullopt if * an error occured, e.g. if some register access is not permitted. */ - modm::ResumableResult> + std::optional readRegister(Register reg); /** @@ -216,7 +214,7 @@ class Adis16470 : public adis16470, * * @return The register value */ - modm::ResumableResult + modm::adis16470::DiagStat_t readDiagStat(); /** @@ -224,7 +222,7 @@ class Adis16470 : public adis16470, * * @return The register value */ - modm::ResumableResult + modm::adis16470::MscCtrl_t readMscCtrl(); /** @@ -235,7 +233,7 @@ class Adis16470 : public adis16470, * @return False in case of any error, e.g. if some register acces is not * permitted. */ - modm::ResumableResult + bool writeRegister(Register reg, uint16_t value); /** @@ -243,7 +241,7 @@ class Adis16470 : public adis16470, * * @param value The value to be written to the MSC_CTRL register. */ - modm::ResumableResult + void writeMscCtrl(modm::adis16470::MscCtrl_t value); /** @@ -251,7 +249,7 @@ class Adis16470 : public adis16470, * * @param value The value to be written to the MSC_CTRL register. */ - modm::ResumableResult + void writeGlobCmd(modm::adis16470::GlobCmd_t value); /** @@ -261,7 +259,7 @@ class Adis16470 : public adis16470, * @tparam tolerance: acceptable tolerance, default 1% */ template - modm::ResumableResult + void setDataOutputFrequency(); /** @@ -274,7 +272,7 @@ class Adis16470 : public adis16470, * @return False in case of any error, e.g. if some register access is not * permitted. */ - modm::ResumableResult + bool readRegisterSequence(std::span sequence, std::span values); /** @@ -302,7 +300,7 @@ class Adis16470 : public adis16470, * has already been checked by this driver. * @return False in case of a checksum mismatch. */ - modm::ResumableResult + bool readRegisterBurst(std::array& data); private: diff --git a/src/modm/driver/inertial/adis16470.lb b/src/modm/driver/inertial/adis16470.lb index f7c0e3ec8c..dfe91e0aa1 100644 --- a/src/modm/driver/inertial/adis16470.lb +++ b/src/modm/driver/inertial/adis16470.lb @@ -25,7 +25,7 @@ def prepare(module, options): ":architecture:register", ":architecture:spi.device", ":math:utils", - ":processing:resumable", + ":architecture:fiber", ":processing:timer") return True diff --git a/src/modm/driver/inertial/adis16470_impl.hpp b/src/modm/driver/inertial/adis16470_impl.hpp index b7db925420..9efbd93e93 100644 --- a/src/modm/driver/inertial/adis16470_impl.hpp +++ b/src/modm/driver/inertial/adis16470_impl.hpp @@ -20,11 +20,9 @@ namespace modm { template -modm::ResumableResult +void Adis16470::initialize() { - RF_BEGIN(); - this->attachConfigurationHandler([]() { SpiMaster::setDataMode(SpiMaster::DataMode::Mode3); SpiMaster::setDataOrder(SpiMaster::DataOrder::MsbFirst); @@ -32,135 +30,119 @@ Adis16470::initialize() Cs::setOutput(modm::Gpio::High); timeout.restart(tStall); - - RF_END(); } template -modm::ResumableResult> +std::optional Adis16470::readRegister(Register reg) { - RF_BEGIN(); - if (getRegisterAccess(reg) == AccessMethod::Write) { // Reading this register is not permitted - RF_RETURN(std::nullopt); + return std::nullopt; } // Ensure CS was not asserted for T_stall - RF_WAIT_UNTIL(timeout.isExpired()); + timeout.wait(); - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); buffer[0] = uint8_t(reg) & 0b0111'1111; buffer[1] = 0; - RF_CALL(SpiMaster::transfer(buffer.data(), nullptr, 2)); + SpiMaster::transfer(buffer.data(), nullptr, 2); if (this->releaseMaster()) { Cs::set(); } - timeout.restart(tStall); - RF_WAIT_UNTIL(timeout.isExpired()); - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::sleep_for(tStall); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); - RF_CALL(SpiMaster::transfer(buffer.data(), &buffer[2], 2)); + SpiMaster::transfer(buffer.data(), &buffer[2], 2); if (this->releaseMaster()) { Cs::set(); } timeout.restart(tStall); - RF_END_RETURN((static_cast(buffer[2]) << 8) | buffer[3]); + return (static_cast(buffer[2]) << 8) | buffer[3]; } template -modm::ResumableResult +modm::adis16470::DiagStat_t Adis16470::readDiagStat() { - RF_BEGIN(); - tmp = RF_CALL(readRegister(Register::DIAG_STAT)); - RF_END_RETURN(DiagStat_t(*tmp)); + tmp = readRegister(Register::DIAG_STAT); + return DiagStat_t(*tmp); } template -modm::ResumableResult +modm::adis16470::MscCtrl_t Adis16470::readMscCtrl() { - RF_BEGIN(); - tmp = RF_CALL(readRegister(Register::MSC_CTRL)); - RF_END_RETURN(MscCtrl_t(*tmp)); + tmp = readRegister(Register::MSC_CTRL); + return MscCtrl_t(*tmp); } template -modm::ResumableResult +bool Adis16470::writeRegister(Register reg, uint16_t value) { - RF_BEGIN(); - if (getRegisterAccess(reg) == AccessMethod::Read) { // Writing to this register is not permitted - RF_RETURN(false); + return false; } // Ensure CS was not asserted for T_stall - RF_WAIT_UNTIL(timeout.isExpired()); + timeout.wait(); - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); buffer[0] = (uint8_t(reg) & 0b0111'1111) | 0b1000'0000; buffer[1] = static_cast(value); - RF_CALL(SpiMaster::transfer(buffer.data(), nullptr, 2)); + SpiMaster::transfer(buffer.data(), nullptr, 2); if (this->releaseMaster()) { Cs::set(); } - timeout.restart(tStall); - RF_WAIT_UNTIL(timeout.isExpired()); - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::sleep_for(tStall); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); buffer[0] = ((uint8_t(reg) + 1) & 0b0111'1111) | 0b1000'0000; buffer[1] = static_cast(value >> 8); - RF_CALL(SpiMaster::transfer(buffer.data(), nullptr, 2)); + SpiMaster::transfer(buffer.data(), nullptr, 2); if (this->releaseMaster()) { Cs::set(); } timeout.restart(tStall); - RF_END_RETURN(true); + return true; } template -modm::ResumableResult +void Adis16470::writeMscCtrl(modm::adis16470::MscCtrl_t value) { - RF_BEGIN(); - - RF_CALL(writeRegister(Register::MSC_CTRL, value.value)); + writeRegister(Register::MSC_CTRL, value.value); // Writing to MSC_CTRL take approx. 3ms - timeout.restart(std::chrono::milliseconds(3)); - - RF_END(); + timeout.restart(3ms); } template -modm::ResumableResult +void Adis16470::writeGlobCmd(modm::adis16470::GlobCmd_t value) { - RF_BEGIN(); - RF_CALL(writeRegister(Register::GLOB_CMD, value.value)); - RF_END(); + writeRegister(Register::GLOB_CMD, value.value); } template template -modm::ResumableResult +void Adis16470::setDataOutputFrequency() { // Output data rate R = 2000SPS / (DEC_RATE + 1) @@ -169,27 +151,22 @@ Adis16470::setDataOutputFrequency() static_assert(frequency < 2000, "Maximum data output rate is 2000Hz"); modm::PeripheralDriver::assertBaudrateInTolerance(); - - RF_BEGIN(); - RF_CALL(writeRegister(Register::DEC_RATE, decRate)); - RF_END(); + writeRegister(Register::DEC_RATE, decRate); } template -modm::ResumableResult +bool Adis16470::readRegisterSequence(std::span sequence, std::span values) { - RF_BEGIN(); - if(sequence.size() != values.size()) { // Mismatching std::span sizes - RF_RETURN(false); + return false; } // Ensure CS was not asserted for T_stall - RF_WAIT_UNTIL(timeout.isExpired()); + timeout.wait(); - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); for (i = 0; i < sequence.size(); i++) { @@ -199,21 +176,20 @@ Adis16470::readRegisterSequence(std::span sequenc Cs::set(); } timeout.restart(tStall); - RF_RETURN(false); + return false; } buffer[0] = uint8_t(sequence[i]) & 0b0111'1111; buffer[1] = 0; - RF_CALL(SpiMaster::transfer(buffer.data(), &buffer[2], 2)); + SpiMaster::transfer(buffer.data(), &buffer[2], 2); if (this->releaseMaster()) { Cs::set(); } - timeout.restart(tStall); - RF_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(tStall); - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); if (i != 0) { @@ -224,7 +200,7 @@ Adis16470::readRegisterSequence(std::span sequenc // one additionl transfer to retrieve value of last register buffer[0] = uint8_t(sequence[0]) & 0b0111'1111; buffer[1] = 0; - RF_CALL(SpiMaster::transfer(buffer.data(), &buffer[2], 2)); + SpiMaster::transfer(buffer.data(), &buffer[2], 2); values[values.size()-1] = (static_cast(buffer[2]) << 8) | buffer[3]; if (this->releaseMaster()) { @@ -232,22 +208,20 @@ Adis16470::readRegisterSequence(std::span sequenc } timeout.restart(tStall); - RF_END_RETURN(true); + return true; } template -modm::ResumableResult +bool Adis16470::readRegisterBurst(std::array& data) { - RF_BEGIN(); - buffer.fill(0); buffer[0] = 0x68; - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); - RF_CALL(SpiMaster::transfer(buffer.data(), reinterpret_cast(data.data()), 22)); + SpiMaster::transfer(buffer.data(), reinterpret_cast(data.data()), 22); if (this->releaseMaster()) { Cs::set(); @@ -265,7 +239,7 @@ Adis16470::readRegisterBurst(std::array& data) data[i] = modm::fromBigEndian(data[i]); } - RF_END_RETURN(checksum == data[10]); + return checksum == data[10]; } } // namespace modm diff --git a/src/modm/driver/inertial/adxl345.hpp b/src/modm/driver/inertial/adxl345.hpp index 7be2b482c9..0d1e3a618e 100644 --- a/src/modm/driver/inertial/adxl345.hpp +++ b/src/modm/driver/inertial/adxl345.hpp @@ -14,7 +14,7 @@ #ifndef MODM_ADXL345_HPP #define MODM_ADXL345_HPP -#include +#include namespace modm { @@ -158,7 +158,7 @@ namespace modm * \tparam I2cMaster Asynchronous Two Wire interface */ template < typename I2cMaster > - class Adxl345 : protected modm::I2cWriteReadTransaction + class Adxl345 : protected modm::I2cDevice { public: /** diff --git a/src/modm/driver/inertial/adxl345_impl.hpp b/src/modm/driver/inertial/adxl345_impl.hpp index 87425617e5..ea23f048f6 100644 --- a/src/modm/driver/inertial/adxl345_impl.hpp +++ b/src/modm/driver/inertial/adxl345_impl.hpp @@ -19,7 +19,7 @@ // ---------------------------------------------------------------------------- template < typename I2cMaster > modm::Adxl345::Adxl345(uint8_t* data, uint8_t address) -: I2cWriteReadTransaction(address), status(0), data(data) +: I2cDevice(address), status(0), data(data) { configureWriteRead(buffer, 0, data, 0); } @@ -69,19 +69,11 @@ template < typename I2cMaster > void modm::Adxl345::update() { - if (status & READ_ACCELEROMETER_RUNNING && - getAdapterState() == modm::I2c::AdapterState::Idle) { - status &= ~READ_ACCELEROMETER_RUNNING; - status |= NEW_ACCELEROMETER_DATA; - } - else if (status & READ_ACCELEROMETER_PENDING) + if (status & READ_ACCELEROMETER_PENDING) { buffer[0] = adxl345::REGISTER_DATA_X0; - configureWriteRead(buffer, 1, data, 6); - - if (I2cMaster::start(this)) { + if (I2cDevice::writeRead(buffer, 1, data, 6);) { status &= ~READ_ACCELEROMETER_PENDING; - status |= READ_ACCELEROMETER_RUNNING; } } } @@ -91,26 +83,17 @@ template < typename I2cMaster > bool modm::Adxl345::writeRegister(adxl345::Register reg, uint8_t value) { - while (getAdapterState() == modm::I2c::AdapterState::Busy) - ; buffer[0] = reg; buffer[1] = value; - configureWriteRead(buffer, 2, data, 0); - - return I2cMaster::startBlocking(this); + return I2cDevice::writeRead(buffer, 2, data, 0); } template < typename I2cMaster > uint8_t modm::Adxl345::readRegister(adxl345::Register reg) { - while (getAdapterState() == modm::I2c::AdapterState::Busy) - ; buffer[0] = reg; - configureWriteRead(buffer, 1, buffer, 1); - - while (!I2cMaster::startBlocking(this)) - ; + if (not I2cDevice::writeRead(buffer, 1, buffer, 1)) return 0; return buffer[0]; } diff --git a/src/modm/driver/inertial/bma180.hpp b/src/modm/driver/inertial/bma180.hpp index 312033f5a6..4a6bf4b230 100644 --- a/src/modm/driver/inertial/bma180.hpp +++ b/src/modm/driver/inertial/bma180.hpp @@ -13,7 +13,7 @@ #ifndef MODM_BMA180_HPP #define MODM_BMA180_HPP -#include +#include namespace modm { @@ -392,7 +392,7 @@ namespace modm * \tparam I2cMaster Asynchronous Two Wire interface */ template < typename I2cMaster > - class Bma180 : protected modm::I2cWriteReadTransaction + class Bma180 : protected modm::I2cDevice< I2cMaster > { public: /** diff --git a/src/modm/driver/inertial/bma180_impl.hpp b/src/modm/driver/inertial/bma180_impl.hpp index 5e65ea312e..4e3cc7f019 100644 --- a/src/modm/driver/inertial/bma180_impl.hpp +++ b/src/modm/driver/inertial/bma180_impl.hpp @@ -16,9 +16,8 @@ // ---------------------------------------------------------------------------- template < typename I2cMaster > modm::Bma180::Bma180(uint8_t* data, uint8_t address) -: I2cWriteReadTransaction(address), status(0), data(data) +: I2cDevice(address), status(0), data(data) { - configureWriteRead(buffer, 0, data, 0); } template < typename I2cMaster > @@ -66,19 +65,11 @@ template < typename I2cMaster > void modm::Bma180::update() { - if (status & READ_ACCELEROMETER_RUNNING && - getAdapterState() == modm::I2c::AdapterState::Idle) { - status &= ~READ_ACCELEROMETER_RUNNING; - status |= NEW_ACCELEROMETER_DATA; - } - else if (status & READ_ACCELEROMETER_PENDING) + if (status & READ_ACCELEROMETER_PENDING) { buffer[0] = bma180::REGISTER_DATA_X0; - configureWriteRead(buffer, 1, data, 7); - - if (I2cMaster::start(this)) { + if (I2cDevice::writeRead(buffer, 1, data, 7)) { status &= ~READ_ACCELEROMETER_PENDING; - status |= READ_ACCELEROMETER_RUNNING; } } } @@ -103,26 +94,17 @@ template < typename I2cMaster > bool modm::Bma180::writeRegister(bma180::Register reg, uint8_t value) { - while (getAdapterState() == modm::I2c::AdapterState::Busy) - ; buffer[0] = reg; buffer[1] = value; - configureWriteRead(buffer, 2, data, 0); - - return I2cMaster::startBlocking(this); + return I2cDevice::writeRead(buffer, 2, data, 0); } template < typename I2cMaster > uint8_t modm::Bma180::readRegister(bma180::Register reg) { - while (getAdapterState() == modm::I2c::AdapterState::Busy) - ; buffer[0] = reg; - configureWriteRead(buffer, 1, buffer, 1); - - while (!I2cMaster::startBlocking(this)) - ; + if (not I2cDevice::writeRead(buffer, 1, buffer, 1)) return 0; return buffer[0]; } diff --git a/src/modm/driver/inertial/bmi088.hpp b/src/modm/driver/inertial/bmi088.hpp index 10156921c4..121fd48677 100644 --- a/src/modm/driver/inertial/bmi088.hpp +++ b/src/modm/driver/inertial/bmi088.hpp @@ -311,9 +311,6 @@ class Bmi088 : public bmi088, public Transport bool enableAccelerometer(); - void - timerWait(); - std::optional readRegister(auto reg); diff --git a/src/modm/driver/inertial/bmi088_impl.hpp b/src/modm/driver/inertial/bmi088_impl.hpp index 9f47d75fb1..e0b7335321 100644 --- a/src/modm/driver/inertial/bmi088_impl.hpp +++ b/src/modm/driver/inertial/bmi088_impl.hpp @@ -40,7 +40,7 @@ Bmi088::initialize(bool runSelfTest) return false; } - timerWait(); + timer_.wait(); const bool ok = this->writeRegister(GyroRegister::InterruptControl, uint8_t(GyroInterruptControl::DataReady)); timer_.restart(WriteTimeout); @@ -87,7 +87,7 @@ template bool Bmi088::setAccRate(AccRate config) { - timerWait(); + timer_.wait(); const bool ok = this->writeRegister(AccRegister::Config, uint8_t(config)); timer_.restart(ResetTimeout); return ok; @@ -97,7 +97,7 @@ template bool Bmi088::setAccRange(AccRange range) { - timerWait(); + timer_.wait(); const bool ok = this->writeRegister(AccRegister::Range, uint8_t(range)); timer_.restart(ResetTimeout); if (ok) { @@ -111,7 +111,7 @@ template bool Bmi088::setAccInt1GpioConfig(AccGpioConfig_t config) { - timerWait(); + timer_.wait(); const bool ok = this->writeRegister(AccRegister::Int1Control, config.value); timer_.restart(ResetTimeout); return ok; @@ -121,7 +121,7 @@ template bool Bmi088::setAccInt2GpioConfig(AccGpioConfig_t config) { - timerWait(); + timer_.wait(); const bool ok = this->writeRegister(AccRegister::Int2Control, config.value); timer_.restart(ResetTimeout); return ok; @@ -131,7 +131,7 @@ template bool Bmi088::setAccGpioMap(AccGpioMap_t map) { - timerWait(); + timer_.wait(); const bool ok = this->writeRegister(AccRegister::IntMap, map.value); timer_.restart(ResetTimeout); return ok; @@ -169,7 +169,7 @@ template bool Bmi088::setGyroRate(GyroRate rate) { - timerWait(); + timer_.wait(); const bool ok = this->writeRegister(GyroRegister::Bandwidth, uint8_t(rate)); timer_.restart(ResetTimeout); return ok; @@ -179,7 +179,7 @@ template bool Bmi088::setGyroRange(GyroRange range) { - timerWait(); + timer_.wait(); const bool ok = this->writeRegister(GyroRegister::Range, uint8_t(range)); timer_.restart(ResetTimeout); if (ok) { @@ -193,7 +193,7 @@ template bool Bmi088::setGyroGpioConfig(GyroGpioConfig_t config) { - timerWait(); + timer_.wait(); const bool ok = this->writeRegister(GyroRegister::Int3Int4Conf, config.value); timer_.restart(ResetTimeout); return ok; @@ -203,7 +203,7 @@ template bool Bmi088::setGyroGpioMap(GyroGpioMap_t map) { - timerWait(); + timer_.wait(); const bool ok = this->writeRegister(GyroRegister::Int3Int4Map, map.value); timer_.restart(ResetTimeout); return ok; @@ -220,15 +220,6 @@ Bmi088::checkChipId() return gyroIdValid and accIdValid; } -template -void -Bmi088::timerWait() -{ - while (timer_.isArmed()) { - modm::this_fiber::yield(); - } -} - template std::optional Bmi088::readRegister(auto reg) @@ -255,7 +246,7 @@ Bmi088::reset() gyroRange_ = GyroRange::Range2000dps; timer_.restart(ResetTimeout); - timerWait(); + timer_.wait(); // required to switch the accelerometer to SPI mode if SPI is used Transport::initialize(); @@ -273,13 +264,13 @@ Bmi088::selfTest() return false; } timer_.restart(2ms); - timerWait(); + timer_.wait(); ok = this->writeRegister(AccRegister::SelfTest, uint8_t(AccSelfTest::Positive)); if (!ok) { return false; } timer_.restart(50ms); - timerWait(); + timer_.wait(); const auto positiveData = readAccData(); if (!positiveData) { return false; @@ -289,7 +280,7 @@ Bmi088::selfTest() return false; } timer_.restart(50ms); - timerWait(); + timer_.wait(); const auto negativeData = readAccData(); if (!negativeData) { return false; @@ -305,7 +296,7 @@ Bmi088::selfTest() return false; } timer_.restart(30ms); - timerWait(); + timer_.wait(); const auto gyroTest = GyroSelfTest_t(readRegister(GyroRegister::SelfTest).value_or(0)); if (gyroTest != (GyroSelfTest::Ready | GyroSelfTest::RateOk)) { return false; @@ -317,13 +308,13 @@ template bool Bmi088::enableAccelerometer() { - timerWait(); + timer_.wait(); bool ok = this->writeRegister(AccRegister::PowerConfig, uint8_t(AccPowerConf::Active)); timer_.restart(AccSuspendTimeout); if (!ok) { return false; } - timerWait(); + timer_.wait(); ok = this->writeRegister(AccRegister::PowerControl, uint8_t(AccPowerControl::On)); timer_.restart(WriteTimeout); diff --git a/src/modm/driver/inertial/bmi088_transport.hpp b/src/modm/driver/inertial/bmi088_transport.hpp index 569bdd71dc..e32346ad8c 100644 --- a/src/modm/driver/inertial/bmi088_transport.hpp +++ b/src/modm/driver/inertial/bmi088_transport.hpp @@ -16,12 +16,12 @@ #include #include #include +#include #include #include #include #include #include -#include namespace modm { diff --git a/src/modm/driver/inertial/bmi088_transport_impl.hpp b/src/modm/driver/inertial/bmi088_transport_impl.hpp index da44d22738..8f13e2390e 100644 --- a/src/modm/driver/inertial/bmi088_transport_impl.hpp +++ b/src/modm/driver/inertial/bmi088_transport_impl.hpp @@ -13,8 +13,6 @@ #error "Don't include this file directly, use 'bmi088_transport.hpp' instead!" #endif -#include - namespace modm { @@ -56,9 +54,7 @@ Bmi088SpiTransport::readRegisters(uint8_t startReg, return {}; } - while (!this->acquireMaster()) { - modm::this_fiber::yield(); - } + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); const uint8_t dataOffset = (dummyByte ? 2 : 1); @@ -94,9 +90,7 @@ template bool Bmi088SpiTransport::writeRegister(uint8_t reg, uint8_t data) { - while (!this->acquireMaster()) { - modm::this_fiber::yield(); - } + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); txBuffer_[0] = reg; @@ -148,10 +142,7 @@ Bmi088I2cTransport::readRegisters(uint8_t startReg, uint8_t count) return {}; } - this->transaction.configureWriteRead(&startReg, 1, &buffer_[0], count); - const bool success = this->runTransaction(); - - if (success) { + if (I2cDevice::writeRead(&startReg, 1, &buffer_[0], count)) { return std::span{&buffer_[0], count}; } else { return {}; @@ -180,9 +171,7 @@ Bmi088I2cTransport::writeRegister(uint8_t reg, uint8_t data) { buffer_[0] = reg; buffer_[1] = data; - this->transaction.configureWrite(&buffer_[0], 2); - - return this->runTransaction(); + return I2cDevice::write(&buffer_[0], 2); } } // namespace modm diff --git a/src/modm/driver/inertial/bno055.hpp b/src/modm/driver/inertial/bno055.hpp index 2cf1f4cfca..8d15a82ccf 100644 --- a/src/modm/driver/inertial/bno055.hpp +++ b/src/modm/driver/inertial/bno055.hpp @@ -13,7 +13,7 @@ #include #include -#include +#include #include #include @@ -586,57 +586,52 @@ struct bno055 * @author Niklas Hauser */ template < class I2cMaster > -class Bno055 : public bno055, public modm::I2cDevice +class Bno055 : public bno055, public modm::I2cDevice { public: /// Constructor, requires a bno055::Data object. inline Bno055(Data &data, uint8_t address=addr()): - I2cDevice(address), data(data) {} + I2cDevice(address), data(data) {} - inline modm::ResumableResult + inline bool configure(OperationMode mode=OperationMode::NDOF) { return updateRegister(Register::OPR_MODE, mode); } - inline modm::ResumableResult + inline bool readData() { return readRegister(Register::ACCEL_DATA_X_LSB, (uint8_t*)&data.r, Data::size); } - inline modm::ResumableResult + inline bool enableExternalClock() { return updateRegister(Register::SYS_TRIGGER, SystemTrigger::CLK_SEL, Registers_t(0)); } public: - inline modm::ResumableResult + inline bool updateRegister(Register reg, Registers_t setMask, Registers_t clearMask = Registers_t(0xff)) { - RF_BEGIN(); - RF_CALL(setPageId(reg)); + setPageId(reg); buffer[0] = uint8_t(reg); - this->transaction.configureWriteRead(buffer, 1, buffer, 1); - RF_CALL( this->runTransaction() ); + if (not I2cDevice::writeRead(buffer, 1, buffer, 1)) return false; buffer[1] = (buffer[0] & ~clearMask.value) | setMask.value; buffer[0] = uint8_t(reg); - this->transaction.configureWrite(buffer, 2); - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(buffer, 2); } - inline modm::ResumableResult + inline bool readRegister(Register reg, uint8_t *output, size_t length=1) { - RF_BEGIN(); - RF_CALL(setPageId(reg)); + setPageId(reg); buffer[0] = uint8_t(reg); - this->transaction.configureWriteRead(buffer, 1, output, length); - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::writeRead(buffer, 1, output, length); } public: @@ -645,22 +640,20 @@ class Bno055 : public bno055, public modm::I2cDevice { return data; } protected: - inline modm::ResumableResult + inline bool setPageId(Register regi) { const uint8_t reg = uint8_t(regi); - RF_BEGIN(); if ((reg ^ prev_reg) & 0x80) { buffer[0] = uint8_t(Register::PAGE_ID); buffer[1] = reg >> 7; - this->transaction.configureWrite(buffer, 2); - buffer[2] = RF_CALL( this->runTransaction() ); + buffer[2] = I2cDevice::write(buffer, 2); if (buffer[2]) prev_reg = reg; - RF_RETURN((bool)buffer[2]); + return (bool)buffer[2]; } - RF_END_RETURN(true); + return true; } private: diff --git a/src/modm/driver/inertial/bno055.lb b/src/modm/driver/inertial/bno055.lb index 49105af46d..576c5bc95d 100644 --- a/src/modm/driver/inertial/bno055.lb +++ b/src/modm/driver/inertial/bno055.lb @@ -27,7 +27,7 @@ def prepare(module, options): ":architecture:register", ":architecture:i2c.device", ":math:utils", - ":processing:resumable", + ":architecture:fiber", ":processing:timer", ":math:geometry") return True diff --git a/src/modm/driver/inertial/hmc5843.hpp b/src/modm/driver/inertial/hmc5843.hpp index a45426b9e0..d6286f02c8 100644 --- a/src/modm/driver/inertial/hmc5843.hpp +++ b/src/modm/driver/inertial/hmc5843.hpp @@ -87,15 +87,15 @@ class Hmc5843 : public hmc5843, public Hmc58x3< I2cMaster > { } - modm::ResumableResult inline + bool inline configure(MeasurementRate rate=MeasurementRate::Hz10, Gain gain=Gain::Ga1_0) { return this->configureRaw(uint8_t(rate), uint8_t(gain), gainValues); } - modm::ResumableResult inline + bool inline setMeasurementRate(MeasurementRate rate) { return this->updateConfigA(ConfigA_t(uint8_t(rate)), ConfigA::DO_Mask); } - modm::ResumableResult inline + bool inline setGain(Gain gain) { return this->setGainRaw(uint8_t(gain), gainValues); } }; diff --git a/src/modm/driver/inertial/hmc5883l.hpp b/src/modm/driver/inertial/hmc5883l.hpp index d8fe5095ae..a9c8d594c0 100644 --- a/src/modm/driver/inertial/hmc5883l.hpp +++ b/src/modm/driver/inertial/hmc5883l.hpp @@ -95,21 +95,21 @@ class Hmc5883 : public hmc5883, public Hmc58x3< I2cMaster > { } - modm::ResumableResult inline + bool inline configure(MeasurementRate rate=MeasurementRate::Hz15, Gain gain=Gain::Ga1_3, MeasurementAverage average=MeasurementAverage::Average1) { return this->configureRaw(uint8_t(rate), uint8_t(gain), gainValues, uint8_t(average)); } - modm::ResumableResult inline + bool inline setMeasurementAverage(MeasurementAverage average) { return this->updateConfigA(ConfigA_t(uint8_t(average)), ConfigA::MA_Mask); } - modm::ResumableResult inline + bool inline setMeasurementRate(MeasurementRate rate) { return this->updateConfigA(ConfigA_t(uint8_t(rate)), ConfigA::DO_Mask); } - modm::ResumableResult inline + bool inline setGain(Gain gain) { return this->setGainRaw(uint8_t(gain), gainValues); } }; diff --git a/src/modm/driver/inertial/hmc58x3.hpp b/src/modm/driver/inertial/hmc58x3.hpp index 39e65a5535..51fb99bbe4 100644 --- a/src/modm/driver/inertial/hmc58x3.hpp +++ b/src/modm/driver/inertial/hmc58x3.hpp @@ -185,44 +185,44 @@ struct hmc58x3 * @author Niklas Hauser */ template < typename I2cMaster > -class Hmc58x3 : public hmc58x3, public modm::I2cDevice< I2cMaster, 2 > +class Hmc58x3 : public hmc58x3, public modm::I2cDevice< I2cMaster > { protected: /// Constructor, requires a hmc58x3::Data object, sets address to default of 0x1e Hmc58x3(Data &data, uint8_t address=0x1e); public: - modm::ResumableResult inline + bool inline setOperationMode(OperationMode mode) { return updateMode(Mode_t(uint8_t(mode)), Mode::MD_Mask); } // MARK: Read access - modm::ResumableResult + bool readMagneticField(); protected: /// @cond - modm::ResumableResult + bool configureRaw(uint8_t rate, uint8_t gain, const uint8_t* gainValues, uint8_t average=0); - modm::ResumableResult + bool setGainRaw(uint8_t gain, const uint8_t* gainValues); /// @endcond protected: // MARK: Control Registers - modm::ResumableResult inline + bool inline updateConfigA(ConfigA_t setMask, ConfigA_t clearMask = ConfigA_t(0x7f)) { return updateRegister(0, setMask.value, clearMask.value); } - modm::ResumableResult inline + bool inline updateConfigB(ConfigB_t setMask, ConfigB_t clearMask = ConfigB_t(0xe0)) { return updateRegister(1, setMask.value, clearMask.value); } - modm::ResumableResult inline + bool inline updateMode(Mode_t setMask, Mode_t clearMask = Mode::MD_Mask) { return updateRegister(2, setMask.value, clearMask.value); } @@ -242,7 +242,7 @@ class Hmc58x3 : public hmc58x3, public modm::I2cDevice< I2cMaster, 2 > Status_t getStatus() { return Status_t(rawBuffer[9]); } - modm::ResumableResult + bool readStatus(); public: @@ -254,25 +254,25 @@ class Hmc58x3 : public hmc58x3, public modm::I2cDevice< I2cMaster, 2 > protected: /// @cond /// write a 8bit value a register - modm::ResumableResult + bool write(Register reg, uint8_t &value) { return write(reg, &value, 1); } /// write multiple 8bit values from a start register - modm::ResumableResult + bool write(Register reg, uint8_t *buffer, uint8_t length); /// read a 8bit value from a register - modm::ResumableResult + bool read(Register reg, uint8_t &value) { return read(reg, &value, 1); } /// read multiple 8bit values from a start register - modm::ResumableResult + bool read(Register reg, uint8_t *buffer, uint8_t length); private: - modm::ResumableResult + bool updateRegister(uint8_t index, uint8_t setMask, uint8_t clearMask = 0xff); /// @endcond diff --git a/src/modm/driver/inertial/hmc58x3_impl.hpp b/src/modm/driver/inertial/hmc58x3_impl.hpp index 96047eecd3..a9999b217e 100644 --- a/src/modm/driver/inertial/hmc58x3_impl.hpp +++ b/src/modm/driver/inertial/hmc58x3_impl.hpp @@ -18,71 +18,65 @@ // ---------------------------------------------------------------------------- template < typename I2cMaster > modm::Hmc58x3::Hmc58x3(Data &data, uint8_t address) -: I2cDevice(address), data(data), +: I2cDevice(address), data(data), rawBuffer{0x10, 0x20, 0x01, 0,0,0,0,0,0, 0x00} { } // MARK: - Tasks template < typename I2cMaster > -modm::ResumableResult +bool modm::Hmc58x3::readMagneticField() { - RF_BEGIN(); - - if (RF_CALL(read(Register::DataX_Msb, rawBuffer+3, 7))) + if (read(Register::DataX_Msb, rawBuffer+3, 7)) { std::memcpy(data.data, rawBuffer+3, 6); - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } // ---------------------------------------------------------------------------- // MARK: - base methods template < typename I2cMaster > -modm::ResumableResult +bool modm::Hmc58x3::configureRaw(uint8_t rate, uint8_t gain, const uint8_t* gainValues, uint8_t average) { - RF_BEGIN(); - rawBuffer[0] = rate | average; rawBuffer[1] = gain; rawBuffer[2] = uint8_t(OperationMode::ContinousConversion); - if (RF_CALL(write(Register::ConfigA, rawBuffer, 3))) + if (write(Register::ConfigA, rawBuffer, 3)) { // look-up the gain for the data object data.gain = gainValues[((gain >> 5) & 0x07)]; - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } template < typename I2cMaster > -modm::ResumableResult +bool modm::Hmc58x3::setGainRaw(uint8_t gain, const uint8_t* gainValues) { - RF_BEGIN(); - rawBuffer[1] = (rawBuffer[1] & ~uint8_t(ConfigB::GN_Mask)) | gain; - if (RF_CALL(write(Register::ConfigB, rawBuffer[1]))) + if (write(Register::ConfigB, rawBuffer[1])) { // look-up the gain for the data object data.gain = gainValues[((gain >> 5) & 0x07)]; - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } // ---------------------------------------------------------------------------- // MARK: - register access template < typename I2cMaster > -modm::ResumableResult +bool modm::Hmc58x3::readStatus() { return read(Register::Status, rawBuffer[9]); @@ -90,43 +84,32 @@ modm::Hmc58x3::readStatus() // MARK: update register template < typename I2cMaster > -modm::ResumableResult +bool modm::Hmc58x3::updateRegister(uint8_t index, uint8_t setMask, uint8_t clearMask) { - RF_BEGIN(); - rawBuffer[index] = (rawBuffer[index] & ~clearMask) | setMask; - RF_END_RETURN_CALL(write(Register(index), rawBuffer[index])); + return write(Register(index), rawBuffer[index]); } // MARK: write multilength register template < class I2cMaster > -modm::ResumableResult +bool modm::Hmc58x3::write(Register reg, uint8_t *buffer, uint8_t length) { - RF_BEGIN(); - if (length > 5) - RF_RETURN(false); + return false; rawBuffer[3] = uint8_t(reg); std::memcpy(rawBuffer+4, buffer, length); - - this->transaction.configureWrite(rawBuffer+3, length+1); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(rawBuffer+3, length+1); } // MARK: read multilength register template < class I2cMaster > -modm::ResumableResult +bool modm::Hmc58x3::read(Register reg, uint8_t *buffer, uint8_t length) { - RF_BEGIN(); - rawBuffer[3] = uint8_t(reg); - this->transaction.configureWriteRead(rawBuffer+3, 1, buffer, length); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::writeRead(rawBuffer+3, 1, buffer, length); } diff --git a/src/modm/driver/inertial/hmc6343.hpp b/src/modm/driver/inertial/hmc6343.hpp index 700465b85f..fb12bbd454 100644 --- a/src/modm/driver/inertial/hmc6343.hpp +++ b/src/modm/driver/inertial/hmc6343.hpp @@ -206,7 +206,7 @@ struct hmc6343 * @author Niklas Hauser */ template < class I2cMaster > -class Hmc6343 : public hmc6343, public modm::I2cDevice< I2cMaster, 2 > +class Hmc6343 : public hmc6343, public modm::I2cDevice< I2cMaster > { public: /// Constructor, requires a hmc6343::Data object, sets address to default of 0x19 @@ -215,7 +215,7 @@ class Hmc6343 : public hmc6343, public modm::I2cDevice< I2cMaster, 2 > // READING RAM /// read operation mode register 2 - modm::ResumableResult + bool readOperationMode() { return readPostData(Command::PostOperationMode, 20, 1); } @@ -223,22 +223,22 @@ class Hmc6343 : public hmc6343, public modm::I2cDevice< I2cMaster, 2 > // WRITING EEPROM /// Configures the sensor to normal orientation mode with 10Hz data rate. - modm::ResumableResult + bool setMeasurementRate(MeasurementRate measurementRate=MeasurementRate::Hz10) { return writeRegister(Register::OperationMode2, i(measurementRate)); } /// sets a new deviation angle in eeprom - modm::ResumableResult inline + bool inline setDeviationAngle(int16_t angle) { return writeRegister(Register16::DeviationAngle, static_cast(angle)); } /// sets a new variation angle in eeprom - modm::ResumableResult inline + bool inline setVariationAngle(int16_t angle) { return writeRegister(Register16::VariationAngle, static_cast(angle)); } /// sets a new IIR filter in eeprom - modm::ResumableResult + bool setIIR_Filter(uint8_t filter) { return writeRegister(Register::Filter, filter & 0x0f); } @@ -246,12 +246,12 @@ class Hmc6343 : public hmc6343, public modm::I2cDevice< I2cMaster, 2 > // READING EEPROM /// reads the device id from eeprom - modm::ResumableResult + bool getDeviceId(uint16_t &value) { return readRegister(Register16::DeviceSerial, value); } /// sets a new IIR filter in eeprom - modm::ResumableResult + bool getIIR_Filter(uint8_t &value) { return readRegister(Register::Filter, value); } @@ -259,42 +259,42 @@ class Hmc6343 : public hmc6343, public modm::I2cDevice< I2cMaster, 2 > // COMMANDS /// Sets the specified orientation - modm::ResumableResult + bool setOrientation(Orientation orientation) { return writeCommand(static_cast(orientation)); } /// enters run mode - modm::ResumableResult + bool enterRunMode() { return writeCommand(Command::EnterRunMode); } /// enters standby mode - modm::ResumableResult + bool enterStandbyMode() { return writeCommand(Command::EnterStandbyMode); } /// enters sleep mode - modm::ResumableResult + bool enterSleepMode() { return writeCommand(Command::EnterSleepMode); } /// exit sleep mode - modm::ResumableResult + bool exitSleepMode() { return writeCommand(Command::ExitSleepMode, 20); } /// enters user calibration mode - modm::ResumableResult + bool enterUserCalibrationMode() { return writeCommand(Command::EnterUserCalibrationMode); } /// exit user calibration mode - modm::ResumableResult + bool exitUserCalibrationMode() { return writeCommand(Command::ExitUserCalibrationMode, 50); } /// resets the processor, any new command is delayed by 500ms - modm::ResumableResult + bool resetProcessor() { return writeCommand(Command::ResetProcessor, 500); } @@ -302,22 +302,22 @@ class Hmc6343 : public hmc6343, public modm::I2cDevice< I2cMaster, 2 > // DATA REQUESTS /// reads the Acceleration registers and buffer the results - modm::ResumableResult + bool readAcceleration() { return readPostData(Command::PostAccelData, 0, 6); } /// reads the Magnetometer registers and buffer the results - modm::ResumableResult + bool readMagneticField() { return readPostData(Command::PostMagData, 6, 6); } /// reads the Heading registers and buffer the results - modm::ResumableResult + bool readHeading() { return readPostData(Command::PostHeadingData, 12, 6); } /// reads the Tilt registers and buffer the results - modm::ResumableResult + bool readTilt() { return readPostData(Command::PostTiltData, 14, 6); } @@ -328,19 +328,19 @@ class Hmc6343 : public hmc6343, public modm::I2cDevice< I2cMaster, 2 > // RAW REGISTER ACCESS /// write a 8bit value into the eeprom - modm::ResumableResult + bool writeRegister(Register reg, uint8_t value); /// write a 16bit value into the eeprom - modm::ResumableResult + bool writeRegister(Register16 reg, uint16_t value); /// read a 8bit value from the eeprom - modm::ResumableResult + bool readRegister(Register reg, uint8_t &value); /// read a 16bit value from the eeprom - modm::ResumableResult + bool readRegister(Register16 reg, uint16_t &value); /// @} @@ -352,10 +352,10 @@ class Hmc6343 : public hmc6343, public modm::I2cDevice< I2cMaster, 2 > { return data; } private: - modm::ResumableResult + bool writeCommand(Command command, uint16_t timeout = 1); - modm::ResumableResult + bool readPostData(Command command, uint8_t offset, uint8_t readSize); private: diff --git a/src/modm/driver/inertial/hmc6343_impl.hpp b/src/modm/driver/inertial/hmc6343_impl.hpp index bc2269604f..478f601ccb 100644 --- a/src/modm/driver/inertial/hmc6343_impl.hpp +++ b/src/modm/driver/inertial/hmc6343_impl.hpp @@ -17,7 +17,7 @@ // ---------------------------------------------------------------------------- template < class I2cMaster > modm::Hmc6343::Hmc6343(Data &data, uint8_t address) -: I2cDevice(address), data(data), timeout(500ms) +: I2cDevice(address), data(data), timeout(500ms) { } @@ -25,125 +25,105 @@ modm::Hmc6343::Hmc6343(Data &data, uint8_t address) // MARK: - register access // MARK: write command template < class I2cMaster > -modm::ResumableResult +bool modm::Hmc6343::writeCommand(Command command, uint16_t timeout) { - RF_BEGIN(); + timeout.wait(); buffer[0] = i(command); - RF_WAIT_UNTIL( this->timeout.isExpired() and this->startWrite(buffer, 1) ); - - this->timeout.restart(std::chrono::milliseconds(timeout)); - RF_WAIT_WHILE( this->isTransactionRunning() ); - - RF_END_RETURN( this->wasTransactionSuccessful() ); + if (I2cDevice::write(buffer, 1)) + { + timeout.restart(std::chrono::milliseconds(timeout)); + return true; + } + return false; } // MARK: write register template < class I2cMaster > -modm::ResumableResult +bool modm::Hmc6343::writeRegister(Register reg, uint8_t value) { - RF_BEGIN(); + timeout.wait(); buffer[0] = i(Command::WriteEeprom); buffer[1] = i(reg); buffer[2] = value; - - RF_WAIT_UNTIL( timeout.isExpired() and this->startWrite(buffer, 3) ); - - timeout.restart(10ms); - RF_WAIT_WHILE( this->isTransactionRunning() ); - - RF_END_RETURN( this->wasTransactionSuccessful() ); + if (I2cDevice::write(buffer, 3)) + { + timeout.restart(10ms); + return true; + } + return false; } // MARK: write 16bit register template < class I2cMaster > -modm::ResumableResult +bool modm::Hmc6343::writeRegister(Register16 reg, uint16_t value) { - RF_BEGIN(); - // for little endian machines this endianness "conversion" does nothing *reinterpret_cast(buffer+2) = modm::toLittleEndian(value); // for big endian machines, the bytes are swapped, so that the following is always true! // buffer[2] has LSB, buffer[3] has MSB // LSB - if ( RF_CALL( writeRegister(static_cast(reg), buffer[2]) ) ) + if ( writeRegister(static_cast(reg), buffer[2]) ) { // MSB - RF_RETURN_CALL( writeRegister(static_cast(i(reg)+1), buffer[3]) ); + return writeRegister(static_cast(i(reg)+1), buffer[3]); } - RF_END_RETURN(false); + return false; } // MARK: read register template < class I2cMaster > -modm::ResumableResult +bool modm::Hmc6343::readRegister(Register reg, uint8_t &value) { - RF_BEGIN(); + timeout.wait(); buffer[0] = i(Command::ReadEeprom); buffer[1] = i(reg); - RF_WAIT_UNTIL( timeout.isExpired() and this->startWrite(buffer, 2) ); - - timeout.restart(10ms); - RF_WAIT_WHILE( this->isTransactionRunning() ); - - if( this->wasTransactionSuccessful() ) + if(I2cDevice::write(buffer, 2)) { - RF_WAIT_UNTIL( timeout.isExpired() and this->startRead(&value, 1) ); - - RF_WAIT_WHILE( this->isTransactionRunning() ); - - RF_RETURN( this->wasTransactionSuccessful() ); + modm::this_fiber::sleep_for(20ms); + return I2cDevice::read(&value, 1); } - - RF_END_RETURN(false); + return false; } // MARK: read 16bit register template < class I2cMaster > -modm::ResumableResult +bool modm::Hmc6343::readRegister(Register16 reg, uint16_t &value) { - RF_BEGIN(); - // LSB - if ( RF_CALL( readRegister(static_cast(reg), buffer[2]) ) ) + if ( readRegister(static_cast(reg), buffer[2]) ) { // MSB - if ( RF_CALL( readRegister(static_cast(i(reg)+1), buffer[3]) ) ) + if ( readRegister(static_cast(i(reg)+1), buffer[3]) ) { // buffer[2] has LSB, buffer[3] has MSB // bytes get swapped on big endian machines value = modm::fromLittleEndian(*reinterpret_cast(buffer+2)); - RF_RETURN(true); + return true; } } - RF_END_RETURN(false); + return false; } // MARK: read 6 or 1 bytes of data template < class I2cMaster > -modm::ResumableResult +bool modm::Hmc6343::readPostData(Command command, uint8_t offset, uint8_t readSize) { - RF_BEGIN(); - - if (RF_CALL(writeCommand(command, 1))) + if (writeCommand(command, 1)) { - RF_WAIT_UNTIL( timeout.isExpired() and this->startRead(data.data + offset, readSize) ); - - RF_WAIT_WHILE( this->isTransactionRunning() ); - - RF_RETURN( this->wasTransactionSuccessful() ); + timeout.wait(); + return I2cDevice::read(data.data + offset, readSize); } - - RF_END_RETURN(false); + return false; } diff --git a/src/modm/driver/inertial/itg3200.hpp b/src/modm/driver/inertial/itg3200.hpp index c06ac70c22..a4284baf46 100644 --- a/src/modm/driver/inertial/itg3200.hpp +++ b/src/modm/driver/inertial/itg3200.hpp @@ -190,34 +190,34 @@ struct itg3200 * @ingroup modm_driver_itg3200 */ template < typename I2cMaster > -class Itg3200 : public itg3200, public modm::I2cDevice< I2cMaster, 2 > +class Itg3200 : public itg3200, public modm::I2cDevice< I2cMaster > { public: /// Constructor, requires an itg3200::Data object, sets address to default of 0x68 (AD0 low: 0x69) Itg3200(Data &data, uint8_t address=0x68); - modm::ResumableResult + bool configure(LowPassFilter filter=LowPassFilter::Hz20, uint8_t divider=0); /// reads the temperature and gyro registers and buffer the results - modm::ResumableResult + bool readRotation(); - modm::ResumableResult inline + bool inline setLowPassFilter(LowPassFilter filter) { return updateFilter(filter, Filter::DLPF_CFG_Mask); } - modm::ResumableResult inline + bool inline setSampleRateDivider(uint8_t divider); - modm::ResumableResult inline + bool inline updateInterrupt(Interrupt_t setMask, Interrupt_t clearMask = Interrupt_t(0xf5)) { return updateRegister(1, setMask.value, clearMask.value); } - modm::ResumableResult inline + bool inline updatePower(Power_t setMask, Power_t clearMask = Power_t(0xff)) { return updateRegister(11, setMask.value, clearMask.value); } @@ -236,12 +236,12 @@ class Itg3200 : public itg3200, public modm::I2cDevice< I2cMaster, 2 > Status_t getStatus() { return Status_t(rawBuffer[2]); } - modm::ResumableResult + bool readStatus(); protected: /// @cond - modm::ResumableResult inline + bool inline updateFilter(Filter_t setMask, Filter_t clearMask = Filter_t(0x1f)) { return updateRegister(0, setMask.value, clearMask.value); } @@ -259,25 +259,25 @@ class Itg3200 : public itg3200, public modm::I2cDevice< I2cMaster, 2 > protected: /// @cond /// write a 8bit value a register - modm::ResumableResult + bool write(Register reg, uint8_t &value) { return write(reg, &value, 1); } /// write multiple 8bit values from a start register - modm::ResumableResult + bool write(Register reg, uint8_t *buffer, uint8_t length, bool copyBuffer=true); /// read a 8bit value from a register - modm::ResumableResult + bool read(Register reg, uint8_t &value) { return read(reg, &value, 1); } /// read multiple 8bit values from a start register - modm::ResumableResult + bool read(Register reg, uint8_t *buffer, uint8_t length); private: - modm::ResumableResult + bool updateRegister(uint8_t index, uint8_t setMask, uint8_t clearMask = 0xff); /// @endcond diff --git a/src/modm/driver/inertial/itg3200_impl.hpp b/src/modm/driver/inertial/itg3200_impl.hpp index 5ae5ee567d..044cfcac83 100644 --- a/src/modm/driver/inertial/itg3200_impl.hpp +++ b/src/modm/driver/inertial/itg3200_impl.hpp @@ -16,55 +16,49 @@ // ---------------------------------------------------------------------------- template < typename I2cMaster > modm::Itg3200::Itg3200(Data &data, uint8_t address) -: I2cDevice(address), data(data), +: I2cDevice(address), data(data), rawBuffer{0x00, 0x00, 0x00, 0,0,0,0,0,0,0,0, 0x00} { } // MARK: - Tasks template < typename I2cMaster > -modm::ResumableResult +bool modm::Itg3200::configure(LowPassFilter filter, uint8_t divider) { - RF_BEGIN(); - rawBuffer[4] = divider; rawBuffer[5] = rawBuffer[0] = LowPassFilter_t(filter).value | uint8_t(Filter::FullScale); rawBuffer[6] = rawBuffer[1] = 0; - RF_END_RETURN_CALL(write(Register::SMPLRT_DIV, rawBuffer, 3, false)); + return write(Register::SMPLRT_DIV, rawBuffer, 3, false); } template < typename I2cMaster > -modm::ResumableResult +bool modm::Itg3200::readRotation() { - RF_BEGIN(); - - if (RF_CALL(read(Register::INT_STATUS, rawBuffer+2, 9))) + if (read(Register::INT_STATUS, rawBuffer+2, 9)) { std::memcpy(data.data, rawBuffer+3, 8); - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } template < typename I2cMaster > -modm::ResumableResult +bool modm::Itg3200::setSampleRateDivider(uint8_t divider) { - RF_BEGIN(); - rawBuffer[4] = divider; - RF_END_RETURN_CALL(write(Register::SMPLRT_DIV, rawBuffer+4, 1, false)); + return write(Register::SMPLRT_DIV, rawBuffer+4, 1, false); } // ---------------------------------------------------------------------------- // MARK: - register access template < typename I2cMaster > -modm::ResumableResult +bool modm::Itg3200::readStatus() { return read(Register::INT_STATUS, rawBuffer[2]); @@ -72,43 +66,32 @@ modm::Itg3200::readStatus() // MARK: update register template < typename I2cMaster > -modm::ResumableResult +bool modm::Itg3200::updateRegister(uint8_t index, uint8_t setMask, uint8_t clearMask) { - RF_BEGIN(); - rawBuffer[index] = (rawBuffer[index] & ~clearMask) | setMask; - RF_END_RETURN_CALL(write(Register(index), rawBuffer[index])); + return write(Register(index), rawBuffer[index]); } // MARK: write multilength register template < class I2cMaster > -modm::ResumableResult +bool modm::Itg3200::write(Register reg, uint8_t *buffer, uint8_t length, bool copyBuffer) { - RF_BEGIN(); - if (length > 7) - RF_RETURN(false); + return false; rawBuffer[3] = uint8_t(reg); if (copyBuffer) std::memcpy(rawBuffer+4, buffer, length); - - this->transaction.configureWrite(rawBuffer+3, length+1); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(rawBuffer+3, length+1); } // MARK: read multilength register template < class I2cMaster > -modm::ResumableResult +bool modm::Itg3200::read(Register reg, uint8_t *buffer, uint8_t length) { - RF_BEGIN(); - rawBuffer[3] = uint8_t(reg); - this->transaction.configureWriteRead(rawBuffer+3, 1, buffer, length); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::writeRead(rawBuffer+3, 1, buffer, length); } diff --git a/src/modm/driver/inertial/ixm42xxx.hpp b/src/modm/driver/inertial/ixm42xxx.hpp index fdbd6c25ed..a3d467cb11 100644 --- a/src/modm/driver/inertial/ixm42xxx.hpp +++ b/src/modm/driver/inertial/ixm42xxx.hpp @@ -17,6 +17,7 @@ #include "ixm42xxx_data.hpp" #include "ixm42xxx_definitions.hpp" #include "ixm42xxx_transport.hpp" +#include namespace modm { @@ -48,7 +49,7 @@ class Ixm42xxx : public ixm42xxx, public Transport * @brief Initialize the device to use default settings and the endianess of the microcontroller * @warning Calling this functions resets the device and blocks for 1 ms */ - modm::ResumableResult + void initialize(); /** @@ -56,7 +57,7 @@ class Ixm42xxx : public ixm42xxx, public Transport * @return False in case of any error, e.g. if some register access is not * permitted. */ - modm::ResumableResult + bool readTempData(); /** @@ -64,7 +65,7 @@ class Ixm42xxx : public ixm42xxx, public Transport * @return False in case of any error, e.g. if some register access is not * permitted. */ - modm::ResumableResult + bool readAccelData(); /** @@ -72,7 +73,7 @@ class Ixm42xxx : public ixm42xxx, public Transport * @return False in case of any error, e.g. if some register access is not * permitted. */ - modm::ResumableResult + bool readGyroData(); /** @@ -80,7 +81,7 @@ class Ixm42xxx : public ixm42xxx, public Transport * @return False in case of any error, e.g. if some register access is not * permitted. */ - modm::ResumableResult + bool readSensorData(); /** @@ -88,7 +89,7 @@ class Ixm42xxx : public ixm42xxx, public Transport * @return False in case of any error, e.g. if some register access is not * permitted. */ - modm::ResumableResult + bool readFsyncTimestamp(uint16_t *timestamp); /** @@ -96,7 +97,7 @@ class Ixm42xxx : public ixm42xxx, public Transport * @return False in case of any error, e.g. if some register access is not * permitted. */ - modm::ResumableResult + bool readFifoCount(uint16_t *count); /** @@ -104,7 +105,7 @@ class Ixm42xxx : public ixm42xxx, public Transport * @return False in case of any error, e.g. if some register access is not * permitted. */ - modm::ResumableResult + bool readFifoData(); /** @@ -113,7 +114,7 @@ class Ixm42xxx : public ixm42xxx, public Transport * @return False in case of any error, e.g. if some register access is not * permitted. */ - modm::ResumableResult + bool writeFifoWatermark(uint16_t watermark); public: @@ -129,7 +130,7 @@ class Ixm42xxx : public ixm42xxx, public Transport * @warning Only the registers GYRO_CONFIG0, ACCEL_CONFIG0 and PWR_MGMT0 * can be modified during sensor operation */ - modm::ResumableResult + bool updateRegister(Register reg, Register_t setMask, Register_t clearMask = Register_t(0xff)); /** @@ -143,7 +144,7 @@ class Ixm42xxx : public ixm42xxx, public Transport * @warning Only the registers GYRO_CONFIG0, ACCEL_CONFIG0 and PWR_MGMT0 * can be modified during sensor operation */ - modm::ResumableResult + bool writeRegister(Register reg, uint8_t value); /** @@ -154,7 +155,7 @@ class Ixm42xxx : public ixm42xxx, public Transport * @return False in case of any error, e.g. if some register access is not * permitted. */ - modm::ResumableResult + bool readRegister(Register reg, uint8_t *value); /** @@ -165,7 +166,7 @@ class Ixm42xxx : public ixm42xxx, public Transport * @return False in case of any error, e.g. if some register access is not * permitted. */ - modm::ResumableResult + bool readRegister(Register reg, uint8_t *buffer, std::size_t length); public: @@ -175,7 +176,7 @@ class Ixm42xxx : public ixm42xxx, public Transport { return data; } protected: - inline modm::ResumableResult + inline bool setRegisterBank(Register regi); private: @@ -188,4 +189,4 @@ class Ixm42xxx : public ixm42xxx, public Transport #include "ixm42xxx_impl.hpp" -#endif // MODM_IXM42XXX_HPP \ No newline at end of file +#endif // MODM_IXM42XXX_HPP diff --git a/src/modm/driver/inertial/ixm42xxx.lb b/src/modm/driver/inertial/ixm42xxx.lb index 4246abc773..55e4d0b092 100644 --- a/src/modm/driver/inertial/ixm42xxx.lb +++ b/src/modm/driver/inertial/ixm42xxx.lb @@ -34,7 +34,7 @@ def prepare(module, options): ":architecture:spi.device", ":math:geometry", ":math:utils", - ":processing:resumable") + ":architecture:fiber") return True def build(env): diff --git a/src/modm/driver/inertial/ixm42xxx_impl.hpp b/src/modm/driver/inertial/ixm42xxx_impl.hpp index 574841d362..a153cdf178 100644 --- a/src/modm/driver/inertial/ixm42xxx_impl.hpp +++ b/src/modm/driver/inertial/ixm42xxx_impl.hpp @@ -28,39 +28,35 @@ Ixm42xxx< Transport >::Ixm42xxx(Data &data, uint8_t address) : Transport(address // ----------------------------------------------------------------------------- template < class Transport > -modm::ResumableResult +void Ixm42xxx< Transport >::initialize() { - RF_BEGIN(); - /// Synchronize the register bank selection - RF_CALL(readRegister(Register::REG_BANK_SEL, &prevBank)); + readRegister(Register::REG_BANK_SEL, &prevBank); /// Reset the device and wait 1 ms for the reset to be effective - RF_CALL(writeRegister(Register::DEVICE_CONFIG, uint8_t(DeviceConfig::SOFT_RESET_CONFIG))); - modm::delay_ms(1); + writeRegister(Register::DEVICE_CONFIG, uint8_t(DeviceConfig::SOFT_RESET_CONFIG)); + modm::this_fiber::sleep_for(1ms); /// Configure the device to use the endianess of the microcontroller if (isBigEndian()) { - RF_CALL(writeRegister(Register::INTF_CONFIG0, 0x30)); + writeRegister(Register::INTF_CONFIG0, 0x30); } else { - RF_CALL(writeRegister(Register::INTF_CONFIG0, 0x0)); + writeRegister(Register::INTF_CONFIG0, 0x0); } /// Configure the device to use default full scale and odr for gyro and accel - RF_CALL(updateRegister(Register::GYRO_CONFIG0, GyroFs_t(GyroFs::dps1000) | GyroOdr_t(GyroOdr::kHz1))); - RF_CALL(updateRegister(Register::ACCEL_CONFIG0, AccelFs_t(AccelFs::g16) | AccelOdr_t(AccelOdr::kHz1))); - - RF_END(); + updateRegister(Register::GYRO_CONFIG0, GyroFs_t(GyroFs::dps1000) | GyroOdr_t(GyroOdr::kHz1)); + updateRegister(Register::ACCEL_CONFIG0, AccelFs_t(AccelFs::g16) | AccelOdr_t(AccelOdr::kHz1)); } // ----------------------------------------------------------------------------- template < class Transport > -modm::ResumableResult +bool Ixm42xxx< Transport >::readTempData() { return readRegister(Register::TEMP_DATA1, data.sensorData.temp, 2); @@ -69,7 +65,7 @@ Ixm42xxx< Transport >::readTempData() // ----------------------------------------------------------------------------- template < class Transport > -modm::ResumableResult +bool Ixm42xxx< Transport >::readAccelData() { return readRegister(Register::ACCEL_DATA_X1, data.sensorData.accel, 6); @@ -78,7 +74,7 @@ Ixm42xxx< Transport >::readAccelData() // ----------------------------------------------------------------------------- template < class Transport > -modm::ResumableResult +bool Ixm42xxx< Transport >::readGyroData() { return readRegister(Register::GYRO_DATA_X1, data.sensorData.gyro, 6); @@ -87,7 +83,7 @@ Ixm42xxx< Transport >::readGyroData() // ----------------------------------------------------------------------------- template < class Transport > -modm::ResumableResult +bool Ixm42xxx< Transport >::readSensorData() { return readRegister(Register::TEMP_DATA1, reinterpret_cast(&data.sensorData), 14); @@ -96,7 +92,7 @@ Ixm42xxx< Transport >::readSensorData() // ----------------------------------------------------------------------------- template < class Transport > -modm::ResumableResult +bool Ixm42xxx< Transport >::readFsyncTimestamp(uint16_t *timestamp) { /// TODO: Verify endianess @@ -106,73 +102,65 @@ Ixm42xxx< Transport >::readFsyncTimestamp(uint16_t *timestamp) // ----------------------------------------------------------------------------- template < class Transport > -modm::ResumableResult +bool Ixm42xxx< Transport >::readFifoCount(uint16_t *count) { - RF_BEGIN(); - /// TODO: Figure out why the endianess seem to be wrong (possibly like FIFO_CONFIG2-3) /// Read FIFO_COUNTL to latch new data for both FIFO_COUNTH and FIFO_COUNTL - if (RF_CALL(readRegister(Register::FIFO_COUNTL, &readByte))) + if (readRegister(Register::FIFO_COUNTL, &readByte)) { *count = uint16_t(readByte) << 8; - if (RF_CALL(readRegister(Register::FIFO_COUNTH, &readByte))) + if (readRegister(Register::FIFO_COUNTH, &readByte)) { *count |= uint16_t(readByte); - RF_RETURN(true); + return true; } } - RF_END_RETURN(false); + return false; } // ----------------------------------------------------------------------------- template < class Transport > -modm::ResumableResult +bool Ixm42xxx< Transport >::readFifoData() { - RF_BEGIN(); - - if (RF_CALL(readFifoCount(&data.fifoCount))) + if (readFifoCount(&data.fifoCount)) { data.fifoCount = std::min(data.fifoCount, data.fifoBuffer.size()); if (data.fifoCount > 0) { - RF_RETURN_CALL(readRegister(Register::FIFO_DATA, data.fifoBuffer.data(), data.fifoCount)); + return readRegister(Register::FIFO_DATA, data.fifoBuffer.data(), data.fifoCount); } } - RF_END_RETURN(false); + return false; } // ----------------------------------------------------------------------------- template < class Transport > -modm::ResumableResult +bool Ixm42xxx< Transport >::writeFifoWatermark(uint16_t watermark) { - RF_BEGIN(); - /// TODO: Determine what endianess to use - if (RF_CALL(writeRegister(Register::FIFO_CONFIG2, watermark & 0xFF)) && - RF_CALL(writeRegister(Register::FIFO_CONFIG3, (watermark >> 8) & 0xFF))) + if (writeRegister(Register::FIFO_CONFIG2, watermark & 0xFF) && + writeRegister(Register::FIFO_CONFIG3, (watermark >> 8) & 0xFF)) { - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } // ----------------------------------------------------------------------------- template < class Transport > -modm::ResumableResult +bool modm::Ixm42xxx< Transport >::updateRegister(Register reg, Register_t setMask, Register_t clearMask) { - RF_BEGIN(); - - if (RF_CALL(setRegisterBank(reg)) && RF_CALL(this->read(i(reg), readByte))) + if (setRegisterBank(reg) && this->read(i(reg), readByte)) { readByte = (readByte & ~clearMask.value) | setMask.value; if (reg == Register::GYRO_CONFIG0) @@ -185,78 +173,70 @@ modm::Ixm42xxx< Transport >::updateRegister(Register reg, Register_t setMask, Re AccelConfig0_t accelConfig0 = AccelConfig0_t(readByte); data.accelScale = 16.0f / float(1 << uint8_t(AccelFs_t::get(accelConfig0))); } - RF_RETURN_CALL( this->write(i(reg), readByte) ); + return this->write(i(reg), readByte); } - RF_END_RETURN(false); + return false; } // ----------------------------------------------------------------------------- template < class Transport > -modm::ResumableResult +bool modm::Ixm42xxx< Transport >::writeRegister(Register reg, uint8_t value) { - RF_BEGIN(); - - if (RF_CALL(setRegisterBank(reg))) + if (setRegisterBank(reg)) { - RF_RETURN_CALL( this->write(i(reg), value) ); + return this->write(i(reg), value); } - RF_END_RETURN(false); + return false; } // ----------------------------------------------------------------------------- template < class Transport > -modm::ResumableResult +bool modm::Ixm42xxx< Transport >::readRegister(Register reg, uint8_t *value) { - RF_BEGIN(); - - if (RF_CALL(setRegisterBank(reg))) + if (setRegisterBank(reg)) { - RF_RETURN_CALL( this->read(i(reg), value, 1) ); + return this->read(i(reg), value, 1); } - RF_END_RETURN(false); + return false; } // ----------------------------------------------------------------------------- template < class Transport > -modm::ResumableResult +bool modm::Ixm42xxx< Transport >::readRegister(Register reg, uint8_t *buffer, std::size_t length) { - RF_BEGIN(); - - if (RF_CALL(setRegisterBank(reg))) + if (setRegisterBank(reg)) { - RF_RETURN_CALL( this->read(i(reg), buffer, length) ); + return this->read(i(reg), buffer, length); } - RF_END_RETURN(false); + return false; } // ----------------------------------------------------------------------------- template < class Transport > -modm::ResumableResult +bool modm::Ixm42xxx< Transport >::setRegisterBank(Register regi) { const uint8_t bank = (uint16_t(regi) >> 8) & 0xFF; - RF_BEGIN(); - if (bank != prevBank) { - RF_CALL( this->write(i(Register::REG_BANK_SEL), bank) ); - RF_CALL( this->read(i(Register::REG_BANK_SEL), prevBank) ); - RF_RETURN(bank == prevBank); + this->write(i(Register::REG_BANK_SEL), bank); + this->read(i(Register::REG_BANK_SEL), prevBank); + return bank == prevBank; } - RF_END_RETURN(true); + return true; } -} // namespace modm \ No newline at end of file +} // namespace modm diff --git a/src/modm/driver/inertial/ixm42xxx_transport.hpp b/src/modm/driver/inertial/ixm42xxx_transport.hpp index caed0ef485..367aacaecf 100644 --- a/src/modm/driver/inertial/ixm42xxx_transport.hpp +++ b/src/modm/driver/inertial/ixm42xxx_transport.hpp @@ -16,7 +16,7 @@ #include #include -#include +#include namespace modm { @@ -32,7 +32,7 @@ namespace modm * @author Rasmus Kleist Hørlyck Sørensen */ template < class I2cMaster > -class Ixm42xxxTransportI2c : public modm::I2cDevice< I2cMaster, 4 > +class Ixm42xxxTransportI2c : public modm::I2cDevice< I2cMaster > { public: Ixm42xxxTransportI2c(uint8_t address); @@ -40,15 +40,15 @@ class Ixm42xxxTransportI2c : public modm::I2cDevice< I2cMaster, 4 > protected: // RAW REGISTER ACCESS /// write a 8bit value - modm::ResumableResult + bool write(uint8_t reg, uint8_t value); /// read a 8bit value - modm::ResumableResult + bool read(uint8_t reg, uint8_t &value); /// read multiple 8bit values from a start register - modm::ResumableResult + bool read(uint8_t reg, uint8_t *buffer, std::size_t length); private: @@ -67,27 +67,27 @@ class Ixm42xxxTransportI2c : public modm::I2cDevice< I2cMaster, 4 > * @author Rasmus Kleist Hørlyck Sørensen */ template < class SpiMaster, class Cs > -class Ixm42xxxTransportSpi : public modm::SpiDevice< SpiMaster >, protected modm::NestedResumable< 4 > +class Ixm42xxxTransportSpi : public modm::SpiDevice< SpiMaster > { public: Ixm42xxxTransportSpi(uint8_t /*address*/); /// pings the sensor - modm::ResumableResult + bool ping(); protected: // RAW REGISTER ACCESS /// write a 8bit value - modm::ResumableResult + bool write(uint8_t reg, uint8_t value); /// read a 8bit value - modm::ResumableResult + bool read(uint8_t reg, uint8_t &value); /// read multiple 8bit values from a start register - modm::ResumableResult + bool read(uint8_t reg, uint8_t *buffer, std::size_t length); private: diff --git a/src/modm/driver/inertial/ixm42xxx_transport_impl.hpp b/src/modm/driver/inertial/ixm42xxx_transport_impl.hpp index bc3853de5e..4016565a6d 100644 --- a/src/modm/driver/inertial/ixm42xxx_transport_impl.hpp +++ b/src/modm/driver/inertial/ixm42xxx_transport_impl.hpp @@ -19,30 +19,25 @@ template < class I2cMaster > modm::Ixm42xxxTransportI2c::Ixm42xxxTransportI2c(uint8_t address) - : I2cDevice(address) + : I2cDevice(address) { } // ---------------------------------------------------------------------------- template < class I2cMaster > -modm::ResumableResult +bool modm::Ixm42xxxTransportI2c::write(uint8_t reg, uint8_t value) { - RF_BEGIN(); - buffer[0] = reg; buffer[1] = value; - - this->transaction.configureWrite(buffer, 2); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(buffer, 2); } // ---------------------------------------------------------------------------- template < class I2cMaster > -modm::ResumableResult +bool modm::Ixm42xxxTransportI2c::read(uint8_t reg, uint8_t &value) { return read(reg, &value, 1); @@ -51,15 +46,11 @@ modm::Ixm42xxxTransportI2c::read(uint8_t reg, uint8_t &value) // ---------------------------------------------------------------------------- template < class I2cMaster > -modm::ResumableResult +bool modm::Ixm42xxxTransportI2c::read(uint8_t reg, uint8_t *buffer, std::size_t length) { - RF_BEGIN(); - this->buffer[0] = reg; - this->transaction.configureWriteRead(this->buffer, 1, buffer, length); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::writeRead(this->buffer, 1, buffer, length); } // ---------------------------------------------------------------------------- @@ -74,41 +65,37 @@ modm::Ixm42xxxTransportSpi::Ixm42xxxTransportSpi(uint8_t /*addres // ---------------------------------------------------------------------------- template < class SpiMaster, class Cs > -modm::ResumableResult +bool modm::Ixm42xxxTransportSpi::ping() { - RF_BEGIN(); - whoAmI = 0; - RF_CALL(read(0x75, whoAmI)); + read(0x75, whoAmI); - RF_END_RETURN(whoAmI != 0); + return whoAmI != 0; } // ---------------------------------------------------------------------------- template < class SpiMaster, class Cs > -modm::ResumableResult +bool modm::Ixm42xxxTransportSpi::write(uint8_t reg, uint8_t value) { - RF_BEGIN(); - - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); - RF_CALL(SpiMaster::transfer(reg | Write)); - RF_CALL(SpiMaster::transfer(value)); + SpiMaster::transfer(reg | Write); + SpiMaster::transfer(value); if (this->releaseMaster()) Cs::set(); - RF_END_RETURN(true); + return true; } // ---------------------------------------------------------------------------- template < class SpiMaster, class Cs > -modm::ResumableResult +bool modm::Ixm42xxxTransportSpi::read(uint8_t reg, uint8_t &value) { return read(reg, &value, 1); @@ -117,19 +104,17 @@ modm::Ixm42xxxTransportSpi::read(uint8_t reg, uint8_t &value) // ---------------------------------------------------------------------------- template < class SpiMaster, class Cs > -modm::ResumableResult +bool modm::Ixm42xxxTransportSpi::read(uint8_t reg, uint8_t *buffer, std::size_t length) { - RF_BEGIN(); - - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); - RF_CALL(SpiMaster::transfer(reg | Read)); - RF_CALL(SpiMaster::transfer(nullptr, buffer, length)); + SpiMaster::transfer(reg | Read); + SpiMaster::transfer(nullptr, buffer, length); if (this->releaseMaster()) Cs::set(); - RF_END_RETURN(true); + return true; } diff --git a/src/modm/driver/inertial/l3gd20.hpp b/src/modm/driver/inertial/l3gd20.hpp index efb3d942ec..f7ce4fe1b0 100644 --- a/src/modm/driver/inertial/l3gd20.hpp +++ b/src/modm/driver/inertial/l3gd20.hpp @@ -13,7 +13,7 @@ #define MODM_L3GD20_HPP #include -#include +#include #include #include "lis3_transport.hpp" @@ -335,53 +335,54 @@ class L3gd20 : public l3gd20, public Transport /// For I2c this also sets the address to 0b110101 (alternative: 0x1C). L3gd20(Data &data, uint8_t address=0x35); + [[deprecated("Use configure() instead!")]] // DEPRECATE: 2026q3 bool inline configureBlocking(Scale scale, MeasurementRate rate = MeasurementRate::Hz380) { - return RF_CALL_BLOCKING(configure(scale, rate)); + return configure(scale, rate); } - modm::ResumableResult + bool configure(Scale scale, MeasurementRate rate = MeasurementRate::Hz380); // MARK: Control Registers - modm::ResumableResult inline + bool inline updateControl(Control1_t setMask, Control1_t clearMask = Control1_t(0xff)) { return updateControlRegister(1, setMask, clearMask); } - modm::ResumableResult inline + bool inline updateControl(Control2_t setMask, Control2_t clearMask = Control2_t(0xff)) { return updateControlRegister(2, setMask, clearMask); } - modm::ResumableResult inline + bool inline updateControl(Control3_t setMask, Control3_t clearMask = Control3_t(0xff)) { return updateControlRegister(3, setMask, clearMask); } - modm::ResumableResult inline + bool inline updateControl(Control4_t setMask, Control4_t clearMask = Control4_t(0xff)) { return updateControlRegister(0, setMask, clearMask); } - modm::ResumableResult inline + bool inline updateControl(Control5_t setMask, Control5_t clearMask = Control5_t(0xff)) { return updateControlRegister(4, setMask, clearMask); } - modm::ResumableResult inline + bool inline updateFifoControl(FifoControl_t setMask, FifoControl_t clearMask = FifoControl_t(0xff)) { return updateControlRegister(14, setMask, clearMask); } - modm::ResumableResult inline + bool inline updateInterruptConfiguration(IntConfig_t setMask, IntConfig_t clearMask = IntConfig_t(0xff)) { return updateControlRegister(16, setMask, clearMask); @@ -389,7 +390,7 @@ class L3gd20 : public l3gd20, public Transport // MARK: Read access - modm::ResumableResult + bool readRotation(); // MARK: Registers with instant access @@ -434,10 +435,10 @@ class L3gd20 : public l3gd20, public Transport { return data; } private: - modm::ResumableResult + bool updateControlRegister(uint8_t index, Control_t setMask, Control_t clearMask = static_cast(0xff)); - modm::ResumableResult + bool updateRegister(uint8_t reg, uint8_t setMask, uint8_t clearMask = 0xff); Data &data; diff --git a/src/modm/driver/inertial/l3gd20.lb b/src/modm/driver/inertial/l3gd20.lb index 8325ebae16..3763b1d44b 100644 --- a/src/modm/driver/inertial/l3gd20.lb +++ b/src/modm/driver/inertial/l3gd20.lb @@ -26,7 +26,7 @@ def prepare(module, options): ":architecture:register", ":driver:lis3.transport", ":math:utils", - ":processing:resumable") + ":architecture:fiber") return True def build(env): diff --git a/src/modm/driver/inertial/l3gd20_impl.hpp b/src/modm/driver/inertial/l3gd20_impl.hpp index a44a7e6005..e1f2d71873 100644 --- a/src/modm/driver/inertial/l3gd20_impl.hpp +++ b/src/modm/driver/inertial/l3gd20_impl.hpp @@ -24,11 +24,9 @@ modm::L3gd20::L3gd20(Data &data, uint8_t address) } template < class Transport > -modm::ResumableResult +bool modm::L3gd20::configure(Scale scale, MeasurementRate rate) { - RF_BEGIN(); - // MeasurementRate must be set in Control1 rawBuffer[0] = i(rate) | 0x0F; rawBuffer[0] = 0x0F; @@ -36,19 +34,17 @@ modm::L3gd20::configure(Scale scale, MeasurementRate rate) rawBuffer[3] = i(scale); data.scale = (i(scale) >> 4) + 1; - if (RF_CALL(this->write(i(Register::CTRL_REG1), rawBuffer[0]))) + if (this->write(i(Register::CTRL_REG1), rawBuffer[0])) { - RF_RETURN_CALL(this->write(i(Register::CTRL_REG4), rawBuffer[3])); + return this->write(i(Register::CTRL_REG4), rawBuffer[3]); } - RF_END_RETURN(false); + return false; } template < class Transport > -modm::ResumableResult +bool modm::L3gd20::updateControlRegister(uint8_t index, Control_t setMask, Control_t clearMask) { - RF_BEGIN(); - rawBuffer[index] = (rawBuffer[index] & ~clearMask.value) | setMask.value; // update the scale in the data object, if we update CTRL_REG4 (index 3) if (index == 3) @@ -56,38 +52,34 @@ modm::L3gd20::updateControlRegister(uint8_t index, Control_t setMask, data.scale = ((Control4_t(rawBuffer[3]) & (Control4::FS1 | Control4::FS0)).value >> 4) + 1; } - RF_END_RETURN_CALL(this->write(0x20 + index, rawBuffer[index])); + return this->write(0x20 + index, rawBuffer[index]); } template < class Transport > -modm::ResumableResult +bool modm::L3gd20::readRotation() { - RF_BEGIN(); - - if (RF_CALL(this->read(i(Register::OUT_TEMP) | Transport::AddressIncrement, rawBuffer + 6, 12))) + if (this->read(i(Register::OUT_TEMP) | Transport::AddressIncrement, rawBuffer + 6, 12)) { // copy the memory std::memcpy(data.data, rawBuffer + 8, 6); data.temperature = rawBuffer[6]; - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } // ---------------------------------------------------------------------------- template < class Transport > -modm::ResumableResult +bool modm::L3gd20::updateRegister(uint8_t reg, uint8_t setMask, uint8_t clearMask) { - RF_BEGIN(); - - if (RF_CALL(this->read(reg, rawBuffer[6]))) + if (this->read(reg, rawBuffer[6])) { rawBuffer[6] = (rawBuffer[6] & ~clearMask) | setMask; - RF_RETURN_CALL(this->write(reg, rawBuffer[6])); + return this->write(reg, rawBuffer[6]); } - RF_END_RETURN(false); + return false; } diff --git a/src/modm/driver/inertial/lis302dl.hpp b/src/modm/driver/inertial/lis302dl.hpp index e088f63990..128cfef197 100644 --- a/src/modm/driver/inertial/lis302dl.hpp +++ b/src/modm/driver/inertial/lis302dl.hpp @@ -13,7 +13,7 @@ #define MODM_LIS302DL_HPP #include -#include +#include #include "lis3_transport.hpp" namespace modm @@ -313,35 +313,36 @@ class Lis302dl : public lis302dl, public Transport /// For I2c this also sets the address to 0x1D (alternative: 0x1C). Lis302dl(Data &data, uint8_t address=0x1D); + [[deprecated("Use configure() instead!")]] // DEPRECATE: 2026q3 bool inline configureBlocking(Scale scale, MeasurementRate rate = MeasurementRate::Hz100) { - return RF_CALL_BLOCKING(configure(scale, rate)); + return configure(scale, rate); } - modm::ResumableResult + bool configure(Scale scale, MeasurementRate rate = MeasurementRate::Hz100); // MARK: Control Registers - modm::ResumableResult inline + bool inline updateControlRegister(Control1_t setMask, Control1_t clearMask = Control1_t(0xff)) { return updateControlRegister(0, setMask, clearMask); } - modm::ResumableResult inline + bool inline updateControlRegister(Control2_t setMask, Control2_t clearMask = Control2_t(0xff)) { return updateControlRegister(1, setMask, clearMask); } - modm::ResumableResult inline + bool inline updateControlRegister(Control3_t setMask, Control3_t clearMask = Control3_t(0xff)) { return updateControlRegister(2, setMask, clearMask); } - modm::ResumableResult inline + bool inline writeInterruptSource(Interrupt interrupt, InterruptSource source) { if (interrupt == Interrupt::One) @@ -351,65 +352,65 @@ class Lis302dl : public lis302dl, public Transport } // MARK: Free Fall Registers - modm::ResumableResult inline + bool inline updateFreeFallConfiguration(Interrupt interrupt, FreeFallConfig_t setMask, FreeFallConfig_t clearMask = FreeFallConfig_t(0xff)) { return updateRegister(i(Register::FfWuCfg1) | i(interrupt), setMask.value, clearMask.value); } - modm::ResumableResult inline + bool inline readFreeFallSource(Interrupt interrupt, FreeFallSource_t &source) { return this->read(i(Register::FfWuSrc1) | i(interrupt), source.value); } - modm::ResumableResult inline + bool inline setFreeFallThreshold(Interrupt interrupt, uint8_t threshold) { return this->write(i(Register::FfWuThs1) | i(interrupt), threshold); } - modm::ResumableResult inline + bool inline setFreeFallDuration(Interrupt interrupt, uint8_t duration) { return this->write(i(Register::FfWuDuration1) | i(interrupt), duration); } // MARK: Clock Registers - modm::ResumableResult inline + bool inline updateClickConfiguration(ClickConfig_t setMask, ClickConfig_t clearMask) { return updateRegister(i(Register::ClickCfg), setMask, clearMask); } - modm::ResumableResult inline + bool inline readClickSource(ClickSource_t &source) { return this->read(i(Register::ClickSrc), source); } - modm::ResumableResult inline + bool inline setClickThreshold(Axis axis, uint8_t threshold); - modm::ResumableResult inline + bool inline setClickTimeLimit(uint8_t limit) { return this->write(i(Register::ClickTimeLimit), limit); } - modm::ResumableResult inline + bool inline setClickLatency(uint8_t latency) { return this->write(i(Register::ClickLatency), latency); } - modm::ResumableResult inline + bool inline setClickWindow(uint8_t window) { return this->write(i(Register::ClickWindow), window); } - modm::ResumableResult + bool readAcceleration(); Status_t @@ -435,10 +436,10 @@ class Lis302dl : public lis302dl, public Transport { return data; } private: - modm::ResumableResult + bool updateControlRegister(uint8_t index, Control_t setMask, Control_t clearMask = Control_t(0xff)); - modm::ResumableResult + bool updateRegister(uint8_t reg, uint8_t setMask, uint8_t clearMask = 0xff); Data &data; diff --git a/src/modm/driver/inertial/lis302dl.lb b/src/modm/driver/inertial/lis302dl.lb index 2966990989..a2b6928d70 100644 --- a/src/modm/driver/inertial/lis302dl.lb +++ b/src/modm/driver/inertial/lis302dl.lb @@ -25,7 +25,7 @@ def prepare(module, options): module.depends( ":architecture:register", ":driver:lis3.transport", - ":processing:resumable") + ":architecture:fiber") return True def build(env): diff --git a/src/modm/driver/inertial/lis302dl_impl.hpp b/src/modm/driver/inertial/lis302dl_impl.hpp index e9dbe8587e..05fb6dbc53 100644 --- a/src/modm/driver/inertial/lis302dl_impl.hpp +++ b/src/modm/driver/inertial/lis302dl_impl.hpp @@ -22,27 +22,25 @@ modm::Lis302dl::Lis302dl(Data &data, uint8_t address) } template < class Transport > -modm::ResumableResult +bool modm::Lis302dl::configure(Scale scale, MeasurementRate rate) { return updateControlRegister(r(scale) | r(rate) | Control1_t(0x47)); } template < class Transport > -modm::ResumableResult +bool modm::Lis302dl::updateControlRegister(uint8_t index, Control_t setMask, Control_t clearMask) { - RF_BEGIN(); - rawBuffer[index] = (rawBuffer[index] & ~clearMask.value) | setMask.value; if (index == 0) data.meta = bool(Control1_t(rawBuffer[0]) & Control1::FS); - RF_END_RETURN_CALL(this->write(i(Register::CtrlReg1) + index, rawBuffer[index])); + return this->write(i(Register::CtrlReg1) + index, rawBuffer[index]); } template < class Transport > -modm::ResumableResult +bool modm::Lis302dl::setClickThreshold(Axis axis, uint8_t threshold) { switch(axis) @@ -60,34 +58,30 @@ modm::Lis302dl::setClickThreshold(Axis axis, uint8_t threshold) } template < class Transport > -modm::ResumableResult +bool modm::Lis302dl::readAcceleration() { - RF_BEGIN(); - - if (RF_CALL(this->read(i(Register::Status) | Transport::AddressIncrement, rawBuffer + 3, 7))) + if (this->read(i(Register::Status) | Transport::AddressIncrement, rawBuffer + 3, 7)) { data.data[0] = rawBuffer[5]; data.data[1] = rawBuffer[7]; data.data[2] = rawBuffer[9]; - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } // ---------------------------------------------------------------------------- template < class Transport > -modm::ResumableResult +bool modm::Lis302dl::updateRegister(uint8_t reg, uint8_t setMask, uint8_t clearMask) { - RF_BEGIN(); - - if (RF_CALL(this->read(reg, rawBuffer[4]))) + if (this->read(reg, rawBuffer[4])) { rawBuffer[4] = (rawBuffer[4] & ~clearMask) | setMask; - RF_RETURN_CALL(this->write(reg, rawBuffer[4])); + return this->write(reg, rawBuffer[4]); } - RF_END_RETURN(false); + return false; } diff --git a/src/modm/driver/inertial/lis3_transport.hpp b/src/modm/driver/inertial/lis3_transport.hpp index 500dbffa75..6774aecddd 100644 --- a/src/modm/driver/inertial/lis3_transport.hpp +++ b/src/modm/driver/inertial/lis3_transport.hpp @@ -14,7 +14,7 @@ #include #include -#include +#include namespace modm { @@ -36,7 +36,7 @@ namespace modm * @author Niklas Hauser */ template < class I2cMaster > -class Lis3TransportI2c : public modm::I2cDevice< I2cMaster, 2 > +class Lis3TransportI2c : public modm::I2cDevice< I2cMaster > { public: Lis3TransportI2c(uint8_t address); @@ -44,18 +44,18 @@ class Lis3TransportI2c : public modm::I2cDevice< I2cMaster, 2 > protected: // RAW REGISTER ACCESS /// write a 8bit value - modm::ResumableResult + bool write(uint8_t reg, uint8_t value); /// read a 8bit value - modm::ResumableResult + bool read(uint8_t reg, uint8_t &value) { return read(reg, &value, 1); } /// read multiple 8bit values from a start register - modm::ResumableResult + bool read(uint8_t reg, uint8_t *buffer, uint8_t length); // increment address or not? @@ -83,30 +83,30 @@ class Lis3TransportI2c : public modm::I2cDevice< I2cMaster, 2 > * @author Niklas Hauser */ template < class SpiMaster, class Cs > -class Lis3TransportSpi : public modm::SpiDevice< SpiMaster >, protected modm::NestedResumable<2> +class Lis3TransportSpi : public modm::SpiDevice< SpiMaster > { public: Lis3TransportSpi(uint8_t /*address*/); /// pings the sensor - modm::ResumableResult + bool ping(); protected: // RAW REGISTER ACCESS /// write a 8bit value - modm::ResumableResult + bool write(uint8_t reg, uint8_t value); /// read a 8bit value - modm::ResumableResult + bool read(uint8_t reg, uint8_t &value) { return read(reg, &value, 1); } /// read multiple 8bit values from a start register - modm::ResumableResult + bool read(uint8_t reg, uint8_t *buffer, uint8_t length); // increment address or not? diff --git a/src/modm/driver/inertial/lis3_transport.lb b/src/modm/driver/inertial/lis3_transport.lb index bfcb044751..8c61d0f4c8 100644 --- a/src/modm/driver/inertial/lis3_transport.lb +++ b/src/modm/driver/inertial/lis3_transport.lb @@ -19,7 +19,7 @@ def prepare(module, options): module.depends( ":architecture:i2c.device", ":architecture:spi.device", - ":processing:resumable") + ":architecture:fiber") return True def build(env): diff --git a/src/modm/driver/inertial/lis3_transport_impl.hpp b/src/modm/driver/inertial/lis3_transport_impl.hpp index d68ca82c6a..498767433b 100644 --- a/src/modm/driver/inertial/lis3_transport_impl.hpp +++ b/src/modm/driver/inertial/lis3_transport_impl.hpp @@ -17,37 +17,28 @@ // MARK: I2C TRANSPORT template < class I2cMaster > modm::Lis3TransportI2c::Lis3TransportI2c(uint8_t address) -: I2cDevice(address) +: I2cDevice(address) { } // MARK: - register access // MARK: write register template < class I2cMaster > -modm::ResumableResult +bool modm::Lis3TransportI2c::write(uint8_t reg, uint8_t value) { - RF_BEGIN(); - buffer[0] = reg; buffer[1] = value; - - this->transaction.configureWrite(buffer, 2); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(buffer, 2); } // MARK: read register template < class I2cMaster > -modm::ResumableResult +bool modm::Lis3TransportI2c::read(uint8_t reg, uint8_t *buffer, uint8_t length) { - RF_BEGIN(); - this->buffer[0] = reg; - this->transaction.configureWriteRead(this->buffer, 1, buffer, length); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::writeRead(this->buffer, 1, buffer, length); } // ============================================================================ @@ -61,55 +52,49 @@ modm::Lis3TransportSpi::Lis3TransportSpi(uint8_t /*address*/) // MARK: ping template < class SpiMaster, class Cs > -modm::ResumableResult +bool modm::Lis3TransportSpi::ping() { - RF_BEGIN(); - whoAmI = 0; - RF_CALL(read(0x0F, whoAmI)); + read(0x0F, whoAmI); - RF_END_RETURN(whoAmI != 0); + return whoAmI != 0; } // MARK: - register access // MARK: write register template < class SpiMaster, class Cs > -modm::ResumableResult +bool modm::Lis3TransportSpi::write(uint8_t reg, uint8_t value) { - RF_BEGIN(); - - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); - RF_CALL(SpiMaster::transfer(reg | Write)); - RF_CALL(SpiMaster::transfer(value)); + SpiMaster::transfer(reg | Write); + SpiMaster::transfer(value); if (this->releaseMaster()) Cs::set(); - RF_END_RETURN(true); + return true; } // MARK: read register template < class SpiMaster, class Cs > -modm::ResumableResult +bool modm::Lis3TransportSpi::read(uint8_t reg, uint8_t *buffer, uint8_t length) { - RF_BEGIN(); - - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); - RF_CALL(SpiMaster::transfer(reg | Read)); + SpiMaster::transfer(reg | Read); - RF_CALL(SpiMaster::transfer(nullptr, buffer, length)); + SpiMaster::transfer(nullptr, buffer, length); if (this->releaseMaster()) Cs::set(); - RF_END_RETURN(true); + return true; } diff --git a/src/modm/driver/inertial/lis3dsh.hpp b/src/modm/driver/inertial/lis3dsh.hpp index bf424cb571..deab08b99d 100644 --- a/src/modm/driver/inertial/lis3dsh.hpp +++ b/src/modm/driver/inertial/lis3dsh.hpp @@ -13,7 +13,7 @@ #define MODM_LIS3DSH_HPP #include -#include +#include #include #include "lis3_transport.hpp" @@ -519,47 +519,48 @@ class Lis3dsh : public lis3dsh, public Transport /// For I2c this also sets the address to 0x1D (alternative: 0x1C). Lis3dsh(Data &data, uint8_t address=0x1D); + [[deprecated("Use configure() instead!")]] // DEPRECATE: 2026q3 bool inline configureBlocking(Scale scale, MeasurementRate rate = MeasurementRate::Hz100) { - return RF_CALL_BLOCKING(configure(scale, rate)); + return configure(scale, rate); } - modm::ResumableResult + bool configure(Scale scale, MeasurementRate rate = MeasurementRate::Hz100); // MARK: Control Registers - modm::ResumableResult inline + bool inline updateSmControl1(SmControl_t setMask, SmControl_t clearMask = SmControl_t(0xff)) { return updateControlRegister(1, setMask, clearMask); } - modm::ResumableResult inline + bool inline updateSmControl2(SmControl_t setMask, SmControl_t clearMask = SmControl_t(0xff)) { return updateControlRegister(2, setMask, clearMask); } - modm::ResumableResult inline + bool inline updateControl(Control3_t setMask, Control3_t clearMask = Control3_t(0xff)) { return updateControlRegister(3, setMask, clearMask); } - modm::ResumableResult inline + bool inline updateControl(Control4_t setMask, Control4_t clearMask = Control4_t(0xff)) { return updateControlRegister(0, setMask, clearMask); } - modm::ResumableResult inline + bool inline updateControl(Control5_t setMask, Control5_t clearMask = Control5_t(0xff)) { return updateControlRegister(4, setMask, clearMask); } - modm::ResumableResult inline + bool inline updateControl(Control6_t setMask, Control6_t clearMask = Control6_t(0xff)) { return updateControlRegister(5, setMask, clearMask); @@ -567,7 +568,7 @@ class Lis3dsh : public lis3dsh, public Transport // MARK: Read access - modm::ResumableResult + bool readAcceleration(); // MARK: Registers with instant access @@ -606,10 +607,10 @@ class Lis3dsh : public lis3dsh, public Transport { return data; } private: - modm::ResumableResult + bool updateControlRegister(uint8_t index, Control_t setMask, Control_t clearMask = static_cast(0xff)); - modm::ResumableResult + bool updateRegister(uint8_t reg, uint8_t setMask, uint8_t clearMask = 0xff); Data &data; diff --git a/src/modm/driver/inertial/lis3dsh_impl.hpp b/src/modm/driver/inertial/lis3dsh_impl.hpp index 5cef69e99f..9be38de550 100644 --- a/src/modm/driver/inertial/lis3dsh_impl.hpp +++ b/src/modm/driver/inertial/lis3dsh_impl.hpp @@ -24,11 +24,9 @@ modm::Lis3dsh::Lis3dsh(Data &data, uint8_t address) } template < class Transport > -modm::ResumableResult +bool modm::Lis3dsh::configure(Scale scale, MeasurementRate rate) { - RF_BEGIN(); - // MeasurementRate must be set in Control4 rawBuffer[0] = i(rate) | 0x07; // Scale must be set in Control5 @@ -41,22 +39,20 @@ modm::Lis3dsh::configure(Scale scale, MeasurementRate rate) rawBuffer[5] = uint8_t(Control6::ADD_INC); - if ( RF_CALL(this->write(i(Register::CTRL_REG4), rawBuffer[0])) ) + if ( this->write(i(Register::CTRL_REG4), rawBuffer[0]) ) { - if ( RF_CALL(this->write(i(Register::CTRL_REG5), rawBuffer[4])) ) + if ( this->write(i(Register::CTRL_REG5), rawBuffer[4]) ) { - RF_RETURN_CALL(this->write(i(Register::CTRL_REG6), rawBuffer[5])); + return this->write(i(Register::CTRL_REG6), rawBuffer[5]); } } - RF_END_RETURN(false); + return false; } template < class Transport > -modm::ResumableResult +bool modm::Lis3dsh::updateControlRegister(uint8_t index, Control_t setMask, Control_t clearMask) { - RF_BEGIN(); - rawBuffer[index] = (rawBuffer[index] & ~clearMask.value) | setMask.value; // update the scale in the data object, if we update CTRL_REG5 (index 4) if (index == 4) @@ -68,37 +64,33 @@ modm::Lis3dsh::updateControlRegister(uint8_t index, Control_t setMask data.scale = (scale == 8) ? scale + 8 : scale + 2; } - RF_END_RETURN_CALL(this->write(0x20 + index, rawBuffer[index])); + return this->write(0x20 + index, rawBuffer[index]); } template < class Transport > -modm::ResumableResult +bool modm::Lis3dsh::readAcceleration() { - RF_BEGIN(); - - if (RF_CALL(this->read(i(Register::STATUS), rawBuffer + 6, 9))) + if (this->read(i(Register::STATUS), rawBuffer + 6, 9)) { // copy the memory std::memcpy(data.data, rawBuffer + 7, 6); - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } // ---------------------------------------------------------------------------- template < class Transport > -modm::ResumableResult +bool modm::Lis3dsh::updateRegister(uint8_t reg, uint8_t setMask, uint8_t clearMask) { - RF_BEGIN(); - - if (RF_CALL(this->read(reg, rawBuffer[7]))) + if (this->read(reg, rawBuffer[7])) { rawBuffer[7] = (rawBuffer[7] & ~clearMask) | setMask; - RF_RETURN_CALL(this->write(reg, rawBuffer[7])); + return this->write(reg, rawBuffer[7]); } - RF_END_RETURN(false); + return false; } diff --git a/src/modm/driver/inertial/lis3mdl.hpp b/src/modm/driver/inertial/lis3mdl.hpp index b042f664af..bf7494df5d 100644 --- a/src/modm/driver/inertial/lis3mdl.hpp +++ b/src/modm/driver/inertial/lis3mdl.hpp @@ -13,7 +13,7 @@ #define MODM_LIS3MDL_HPP #include -#include +#include #include #include #include "lis3_transport.hpp" @@ -229,7 +229,7 @@ class Lis3mdl : public lis3mdl, public Lis3TransportI2c * \param scale The full scale of the output data * \return Whether the configuration was successful */ - modm::ResumableResult + bool configure(DataRate rate, Scale scale); /** @@ -238,7 +238,7 @@ class Lis3mdl : public lis3mdl, public Lis3TransportI2c * @param mode The mode on which the magnetometer operates * @return Whether the mode has been set */ - modm::ResumableResult + bool setMode(OperationMode mode); @@ -256,7 +256,7 @@ class Lis3mdl : public lis3mdl, public Lis3TransportI2c * @param data A reference to a Vector3i object the data will be written to * @return Whether the sensor data have been read */ - modm::ResumableResult + bool readMagnetometerRaw(Vector3i& data); /** @@ -265,7 +265,7 @@ class Lis3mdl : public lis3mdl, public Lis3TransportI2c * \param data A reference to a Vector3f object the data will be written to * \return Whether the sensor data have been read */ - modm::ResumableResult + bool readMagnetometer(Vector3f& data); private: diff --git a/src/modm/driver/inertial/lis3mdl_impl.hpp b/src/modm/driver/inertial/lis3mdl_impl.hpp index 6e038c4e22..cb0a100bd0 100644 --- a/src/modm/driver/inertial/lis3mdl_impl.hpp +++ b/src/modm/driver/inertial/lis3mdl_impl.hpp @@ -28,11 +28,9 @@ modm::Lis3mdl::Lis3mdl( uint8_t address) } template < class I2cMaster > -modm::ResumableResult +bool modm::Lis3mdl::configure(DataRate rate, Scale scale) { - RF_BEGIN(); - DataRate_t::set(control1Shadow,rate); // if FAST_ODR is requested, copy the highsped modes for the Z axis @@ -44,26 +42,25 @@ modm::Lis3mdl::configure(DataRate rate, Scale scale) Scale_t::set(control2Shadow,scale); - success = RF_CALL(this->write(static_cast(Register::CTRL1),control1Shadow.value)); + success = this->write(static_cast(Register::CTRL1),control1Shadow.value); if (success) { - success = RF_CALL(this->write(static_cast(Register::CTRL2),control2Shadow.value)); + success = this->write(static_cast(Register::CTRL2),control2Shadow.value); if(success) { - success = RF_CALL(this->write(static_cast(Register::CTRL4),control4Shadow.value)); + success = this->write(static_cast(Register::CTRL4),control4Shadow.value); } } - RF_END_RETURN(success); + return success; } template < class I2cMaster > -modm::ResumableResult +bool modm::Lis3mdl::setMode(OperationMode mode) { - RF_BEGIN(); OperationMode_t::set(control3Shadow,mode); - RF_END_RETURN_CALL(this->write(static_cast(Register::CTRL3),control3Shadow.value)); + return this->write(static_cast(Register::CTRL3),control3Shadow.value); } template < class I2cMaster > @@ -74,27 +71,25 @@ modm::Lis3mdl::getScale() } template < class I2cMaster > -modm::ResumableResult +bool modm::Lis3mdl::readMagnetometerRaw(Vector3i& data) { - RF_BEGIN(); - success = RF_CALL(this->read(static_cast(Register::OUT_X_L),reinterpret_cast(readBuffer),6)); + success = this->read(static_cast(Register::OUT_X_L),reinterpret_cast(readBuffer),6); if(success) { data.x = readBuffer[0]; data.y = readBuffer[1]; data.z = readBuffer[2]; } - RF_END_RETURN(success); + return success; } template < class I2cMaster > -modm::ResumableResult +bool modm::Lis3mdl::readMagnetometer(Vector3f& data) { - RF_BEGIN(); - success = RF_CALL(this->read(static_cast(Register::OUT_X_L),reinterpret_cast(readBuffer),6)); + success = this->read(static_cast(Register::OUT_X_L),reinterpret_cast(readBuffer),6); if(success) { @@ -107,5 +102,5 @@ modm::Lis3mdl::readMagnetometer(Vector3f& data) data.z = static_cast(readBuffer[2]) * conversionValue; } - RF_END_RETURN(success); -} \ No newline at end of file + return success; +} diff --git a/src/modm/driver/inertial/lsm303a.hpp b/src/modm/driver/inertial/lsm303a.hpp index 2fb93e6a28..3143d8e71c 100644 --- a/src/modm/driver/inertial/lsm303a.hpp +++ b/src/modm/driver/inertial/lsm303a.hpp @@ -13,7 +13,7 @@ #define MODM_LSM303A_HPP #include -#include +#include #include #include "lis3_transport.hpp" @@ -382,53 +382,54 @@ class Lsm303a : public lsm303a, public Lis3TransportI2c /// Constructor, requires a lsm303a::Data object. Lsm303a(Data &data, uint8_t address = 0b0011001); + [[deprecated("Use configure() instead!")]] // DEPRECATE: 2026q3 bool inline configureBlocking(Scale scale, MeasurementRate rate = MeasurementRate::Hz100) { - return RF_CALL_BLOCKING(configure(scale, rate)); + return configure(scale, rate); } - modm::ResumableResult + bool configure(Scale scale, MeasurementRate rate = MeasurementRate::Hz100); // MARK: Control Registers - modm::ResumableResult inline + bool inline updateControl(Control1_t setMask, Control1_t clearMask = Control1_t(0xff)) { return updateControlRegister(1, setMask, clearMask); } - modm::ResumableResult inline + bool inline updateControl(Control2_t setMask, Control2_t clearMask = Control2_t(0xff)) { return updateControlRegister(2, setMask, clearMask); } - modm::ResumableResult inline + bool inline updateControl(Control3_t setMask, Control3_t clearMask = Control3_t(0xff)) { return updateControlRegister(3, setMask, clearMask); } - modm::ResumableResult inline + bool inline updateControl(Control4_t setMask, Control4_t clearMask = Control4_t(0xff)) { return updateControlRegister(0, setMask, clearMask); } - modm::ResumableResult inline + bool inline updateControl(Control5_t setMask, Control5_t clearMask = Control5_t(0xff)) { return updateControlRegister(4, setMask, clearMask); } - modm::ResumableResult inline + bool inline updateControl(Control6_t setMask, Control6_t clearMask = Control6_t(0xff)) { return updateControlRegister(5, setMask, clearMask); } - modm::ResumableResult inline + bool inline updateFifoControl(FifoControl_t setMask, FifoControl_t clearMask = FifoControl_t(0xff)) { return updateControlRegister(14, setMask, clearMask); @@ -436,7 +437,7 @@ class Lsm303a : public lsm303a, public Lis3TransportI2c // MARK: Read access - modm::ResumableResult + bool readAcceleration(); // MARK: Registers with instant access @@ -478,10 +479,10 @@ class Lsm303a : public lsm303a, public Lis3TransportI2c { return data; } private: - modm::ResumableResult + bool updateControlRegister(uint8_t index, Control_t setMask, Control_t clearMask = static_cast(0xff)); - modm::ResumableResult + bool updateRegister(uint8_t reg, uint8_t setMask, uint8_t clearMask = 0xff); Data &data; diff --git a/src/modm/driver/inertial/lsm303a_impl.hpp b/src/modm/driver/inertial/lsm303a_impl.hpp index c2442c8883..388760d4e1 100644 --- a/src/modm/driver/inertial/lsm303a_impl.hpp +++ b/src/modm/driver/inertial/lsm303a_impl.hpp @@ -24,11 +24,9 @@ modm::Lsm303a::Lsm303a(Data &data, uint8_t address) } template < class I2cMaster > -modm::ResumableResult +bool modm::Lsm303a::configure(Scale scale, MeasurementRate rate) { - RF_BEGIN(); - // MeasurementRate must be set in Control1 rawBuffer[0] = i(rate) | 0x07; // Scale must be set in Control4 @@ -39,19 +37,17 @@ modm::Lsm303a::configure(Scale scale, MeasurementRate rate) else if (scale == Scale::G8) data.scale = 4; else data.scale = 12; - if ( RF_CALL(this->write(i(Register::CTRL1), rawBuffer[0])) ) + if ( this->write(i(Register::CTRL1), rawBuffer[0]) ) { - RF_RETURN_CALL( this->write(i(Register::CTRL4), rawBuffer[3]) ); + return this->write(i(Register::CTRL4), rawBuffer[3]); } - RF_END_RETURN(false); + return false; } template < class I2cMaster > -modm::ResumableResult +bool modm::Lsm303a::updateControlRegister(uint8_t index, Control_t setMask, Control_t clearMask) { - RF_BEGIN(); - rawBuffer[index] = (rawBuffer[index] & ~clearMask.value) | setMask.value; // update the scale in the data object, if we update CTRL_REG4 (index 3) if (index == 3) @@ -63,37 +59,33 @@ modm::Lsm303a::updateControlRegister(uint8_t index, Control_t setMask else data.scale = 12; } - RF_END_RETURN_CALL(this->write(0x20 + index, rawBuffer[index])); + return this->write(0x20 + index, rawBuffer[index]); } template < class I2cMaster > -modm::ResumableResult +bool modm::Lsm303a::readAcceleration() { - RF_BEGIN(); - - if (RF_CALL(this->read(i(Register::STATUS) | 0x80, rawBuffer + 7, 9))) + if (this->read(i(Register::STATUS) | 0x80, rawBuffer + 7, 9)) { // copy the memory std::memcpy(data.data, rawBuffer + 7, 6); - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } // ---------------------------------------------------------------------------- template < class I2cMaster > -modm::ResumableResult +bool modm::Lsm303a::updateRegister(uint8_t reg, uint8_t setMask, uint8_t clearMask) { - RF_BEGIN(); - - if (RF_CALL(this->read(reg, rawBuffer[8]))) + if (this->read(reg, rawBuffer[8])) { rawBuffer[8] = (rawBuffer[8] & ~clearMask) | setMask; - RF_RETURN_CALL(this->write(reg, rawBuffer[8])); + return this->write(reg, rawBuffer[8]); } - RF_END_RETURN(false); + return false; } diff --git a/src/modm/driver/inertial/lsm6ds33.hpp b/src/modm/driver/inertial/lsm6ds33.hpp index 8cff8be54a..61b1612b0b 100644 --- a/src/modm/driver/inertial/lsm6ds33.hpp +++ b/src/modm/driver/inertial/lsm6ds33.hpp @@ -13,7 +13,7 @@ #define MODM_LSM6DS33_HPP #include -#include +#include #include #include #include "lis3_transport.hpp" @@ -357,7 +357,7 @@ class Lsm6ds33 : public lsm6ds33, public Lis3TransportI2c * \param accScale The full scale of the acceleration data * \return Whether the configuration was successful */ - modm::ResumableResult + bool configureAccelerationSensor(AccDataRate accRate, AccScale accScale); /** @@ -369,7 +369,7 @@ class Lsm6ds33 : public lsm6ds33, public Lis3TransportI2c * \param accScale The full scale of the spin rate data * \return Whether the configuration was successful */ - modm::ResumableResult + bool configureGyroscope(GyroDataRate gyroRate, GyroScale gyroScale); /** @@ -378,7 +378,7 @@ class Lsm6ds33 : public lsm6ds33, public Lis3TransportI2c * @param acceleration A reference to a Vector3i object the data will be written to * @return Whether the sensor data have been read */ - modm::ResumableResult + bool readAccelerationRaw(Vector3i& acceleration); /** @@ -387,7 +387,7 @@ class Lsm6ds33 : public lsm6ds33, public Lis3TransportI2c * @param spinRates A reference to a Vector3i object the data will be written to * @return Whether the sensor data have been read */ - modm::ResumableResult + bool readGyroscopeRaw(Vector3i& spinRates); /** @@ -413,7 +413,7 @@ class Lsm6ds33 : public lsm6ds33, public Lis3TransportI2c * \param acceleration A reference to a Vector3f object the data will be written to * \return Whether the sensor data have been read */ - modm::ResumableResult + bool readAcceleration(Vector3f& acceleration); /** @@ -422,7 +422,7 @@ class Lsm6ds33 : public lsm6ds33, public Lis3TransportI2c * \param spinRates A reference to a Vector3f object the data will be written to * \return Whether the sensor data have been read */ - modm::ResumableResult + bool readGyroscope(Vector3f& spinRates); private: diff --git a/src/modm/driver/inertial/lsm6ds33_impl.hpp b/src/modm/driver/inertial/lsm6ds33_impl.hpp index 5e6e458202..a07e4e8987 100644 --- a/src/modm/driver/inertial/lsm6ds33_impl.hpp +++ b/src/modm/driver/inertial/lsm6ds33_impl.hpp @@ -26,53 +26,49 @@ modm::Lsm6ds33::Lsm6ds33( uint8_t address) } template < class I2cMaster > -modm::ResumableResult +bool modm::Lsm6ds33::configureAccelerationSensor(AccDataRate accRate, AccScale accScale) { - RF_BEGIN(); AccDataRate_t::set(control1Shadow,accRate); AccScale_t::set(control1Shadow,accScale); - RF_END_RETURN_CALL(this->write(static_cast(Register::CTRL1),control1Shadow.value)); + return this->write(static_cast(Register::CTRL1),control1Shadow.value); } template < class I2cMaster > -modm::ResumableResult +bool modm::Lsm6ds33::configureGyroscope(GyroDataRate gyroRate, GyroScale gyroScale) { - RF_BEGIN(); GyroDataRate_t::set(control2Shadow,gyroRate); GyroScale_t::set(control2Shadow,gyroScale); - RF_END_RETURN_CALL( this->write(static_cast(Register::CTRL2),control2Shadow.value)); + return this->write(static_cast(Register::CTRL2),control2Shadow.value); } template < class I2cMaster > -modm::ResumableResult +bool modm::Lsm6ds33::readAccelerationRaw(Vector3i& acceleration) { - RF_BEGIN(); - success = RF_CALL(this->read(static_cast(Register::OUT_X_L_XL),reinterpret_cast(readBuffer),6)); + success = this->read(static_cast(Register::OUT_X_L_XL),reinterpret_cast(readBuffer),6); if(success) { acceleration.x = readBuffer[0]; acceleration.y = readBuffer[1]; acceleration.z = readBuffer[2]; } - RF_END_RETURN(success); + return success; } template < class I2cMaster > -modm::ResumableResult +bool modm::Lsm6ds33::readGyroscopeRaw(Vector3i& spinRates) { - RF_BEGIN(); - success = RF_CALL(this->read(static_cast(Register::OUT_X_L_G),reinterpret_cast(readBuffer),6)); + success = this->read(static_cast(Register::OUT_X_L_G),reinterpret_cast(readBuffer),6); if(success) { spinRates.x = readBuffer[0]; spinRates.y = readBuffer[1]; spinRates.z = readBuffer[2]; } - RF_END_RETURN(success); + return success; } template < class I2cMaster > @@ -90,11 +86,10 @@ modm::Lsm6ds33::getGyroscopeScale() } template < class I2cMaster > -modm::ResumableResult +bool modm::Lsm6ds33::readAcceleration(Vector3f& acceleration) { - RF_BEGIN(); - success = RF_CALL(this->read(static_cast(Register::OUT_X_L_XL),reinterpret_cast(readBuffer),6)); + success = this->read(static_cast(Register::OUT_X_L_XL),reinterpret_cast(readBuffer),6); if(success) { @@ -107,15 +102,14 @@ modm::Lsm6ds33::readAcceleration(Vector3f& acceleration) acceleration.z = static_cast(readBuffer[2]) * conversionValue; } - RF_END_RETURN(success); + return success; } template < class I2cMaster > -modm::ResumableResult +bool modm::Lsm6ds33::readGyroscope(Vector3f& acceleration) { - RF_BEGIN(); - success = RF_CALL(this->read(static_cast(Register::OUT_X_L_G),reinterpret_cast(readBuffer),6)); + success = this->read(static_cast(Register::OUT_X_L_G),reinterpret_cast(readBuffer),6); if(success) { @@ -138,5 +132,5 @@ modm::Lsm6ds33::readGyroscope(Vector3f& acceleration) acceleration.z = static_cast(readBuffer[2]) * conversionValue; } - RF_END_RETURN(success); -} \ No newline at end of file + return success; +} diff --git a/src/modm/driver/inertial/lsm6dso.hpp b/src/modm/driver/inertial/lsm6dso.hpp index 75dfa877b5..a5ad2dbe1e 100644 --- a/src/modm/driver/inertial/lsm6dso.hpp +++ b/src/modm/driver/inertial/lsm6dso.hpp @@ -15,7 +15,7 @@ #include #include #include -#include +#include #include // LSM6DSO uses same I2C/SPI transport as the older LIS3 sensors @@ -204,7 +204,7 @@ class Lsm6dso : public lsm6dso, public Transport * * @return False in case of any error */ - modm::ResumableResult + bool initialize(); /** @@ -214,7 +214,7 @@ class Lsm6dso : public lsm6dso, public Transport * @return The register value in case of a read access, or std::nullopt if * an error occured */ - modm::ResumableResult> + std::optional readRegister(Register reg); /** @@ -224,7 +224,7 @@ class Lsm6dso : public lsm6dso, public Transport * @param values Results of the read operations * @return False in case of any error */ - modm::ResumableResult + bool readRegisters(Register reg, std::span values); /** @@ -234,7 +234,7 @@ class Lsm6dso : public lsm6dso, public Transport * @param value The value to be written to the register. * @return False in case of any error */ - modm::ResumableResult + bool writeRegister(Register reg, uint8_t value); /** @@ -247,7 +247,7 @@ class Lsm6dso : public lsm6dso, public Transport * @return False in case of any error */ template - modm::ResumableResult + bool setOutputDataRate(); /** @@ -258,7 +258,7 @@ class Lsm6dso : public lsm6dso, public Transport * ±1000dps, ±2000dps) * @return False in case of any error */ - modm::ResumableResult + bool setRange(LinearRange lr, AngularRange ar); /** @@ -275,7 +275,7 @@ class Lsm6dso : public lsm6dso, public Transport * @return False in case of any error */ template - modm::ResumableResult + bool setOutputDataRateAndRange(LinearRange lr, AngularRange ar); private: diff --git a/src/modm/driver/inertial/lsm6dso_impl.hpp b/src/modm/driver/inertial/lsm6dso_impl.hpp index afb78df819..99b8d313dd 100644 --- a/src/modm/driver/inertial/lsm6dso_impl.hpp +++ b/src/modm/driver/inertial/lsm6dso_impl.hpp @@ -24,35 +24,33 @@ Lsm6dso::Lsm6dso(uint8_t address) } template -modm::ResumableResult +bool Lsm6dso::initialize() { - RF_BEGIN(); // nothing - RF_END_RETURN(true); + return true; } template -modm::ResumableResult> +std::optional Lsm6dso::readRegister(Register reg) { - RF_BEGIN(); - if (!RF_CALL(this->read(i(reg), buffer[0]))) + if (!this->read(i(reg), buffer[0])) { - RF_RETURN(std::nullopt); + return std::nullopt; } - RF_END_RETURN(buffer[0]); + return buffer[0]; } template -modm::ResumableResult +bool Lsm6dso::readRegisters(Register reg, std::span values) { return this->read(i(reg), &values[0], values.size()); } template -modm::ResumableResult +bool Lsm6dso::writeRegister(Register reg, uint8_t value) { return this->write(i(reg), value); @@ -60,63 +58,59 @@ Lsm6dso::writeRegister(Register reg, uint8_t value) template template -modm::ResumableResult +bool Lsm6dso::setOutputDataRate() { constexpr auto actualOutputDataRate = getClosestOdr(); modm::PeripheralDriver::assertBaudrateInTolerance(); // Set linear and angular ODR - RF_BEGIN(); - if (! RF_CALL(this->read(i(Register::CTRL1_XL), buffer, 2))) + if (! this->read(i(Register::CTRL1_XL), buffer, 2)) { - RF_RETURN(false); + return false; } buffer[0] = (actualOutputDataRate.second << 4) | (buffer[0] & ~Ctrl1XlOutputDataRateMask); buffer[1] = (actualOutputDataRate.second << 4) | (buffer[1] & ~Ctrl2GOutputDataRateMask); - if (!RF_CALL(writeRegister(Register::CTRL1_XL, buffer[0]))) + if (!writeRegister(Register::CTRL1_XL, buffer[0])) { - RF_RETURN(false); + return false; } - RF_END_RETURN_CALL(writeRegister(Register::CTRL2_G, buffer[1])); + return writeRegister(Register::CTRL2_G, buffer[1]); } template -modm::ResumableResult +bool Lsm6dso::setRange(LinearRange lr, AngularRange ar) { - RF_BEGIN(); - if (! RF_CALL(this->read(i(Register::CTRL1_XL), buffer, 2))) + if (! this->read(i(Register::CTRL1_XL), buffer, 2)) { - RF_RETURN(false); + return false; } buffer[0] = i(lr) | (buffer[0] & ~Ctrl1XlLinearRangeMask); buffer[1] = i(ar) | (buffer[1] & ~Ctrl2GAngularRangeMask); - if (!RF_CALL(writeRegister(Register::CTRL1_XL, buffer[0]))) + if (!writeRegister(Register::CTRL1_XL, buffer[0])) { - RF_RETURN(false); + return false; } - RF_END_RETURN_CALL(writeRegister(Register::CTRL2_G, buffer[1])); + return writeRegister(Register::CTRL2_G, buffer[1]); } template template -modm::ResumableResult +bool Lsm6dso::setOutputDataRateAndRange(LinearRange lr, AngularRange ar) { constexpr auto actualOutputDataRate = getClosestOdr(); modm::PeripheralDriver::assertBaudrateInTolerance(); - - RF_BEGIN(); - if (!RF_CALL(writeRegister(Register::CTRL1_XL, (actualOutputDataRate.second << 4) | i(lr)))) + if (!writeRegister(Register::CTRL1_XL, (actualOutputDataRate.second << 4) | i(lr))) { - RF_RETURN(false); + return false; } - RF_END_RETURN_CALL(writeRegister(Register::CTRL2_G, (actualOutputDataRate.second << 4) | i(ar))); + return writeRegister(Register::CTRL2_G, (actualOutputDataRate.second << 4) | i(ar)); } } // namespace modm diff --git a/src/modm/driver/inertial/mmc5603.hpp b/src/modm/driver/inertial/mmc5603.hpp index 816b2085da..f793e4b8bc 100644 --- a/src/modm/driver/inertial/mmc5603.hpp +++ b/src/modm/driver/inertial/mmc5603.hpp @@ -12,7 +12,6 @@ #pragma once #include -#include #include namespace modm @@ -268,39 +267,37 @@ struct mmc5603 * @author Niklas Hauser */ template < class I2cMaster > -class Mmc5603 : public mmc5603, public modm::I2cDevice< I2cMaster, 3 > +class Mmc5603 : public mmc5603, public modm::I2cDevice< I2cMaster > { public: /// Constructor, requires a mmc5603::Data object Mmc5603(Data &data, uint8_t address=addr()) - : I2cDevice(address), data(data) + : I2cDevice(address), data(data) {} - modm::ResumableResult + bool ping() { - RF_BEGIN(); buffer[1] = 0; - RF_CALL(read(Register::ProductID, buffer[1])); - RF_END_RETURN(buffer[1] == ProductID); + read(Register::ProductID, buffer[1]); + return buffer[1] == ProductID; } - modm::ResumableResult + bool startTemperatureMeasurement() { return update(Register::InternalControl0, Control0::Take_meas_T); } /// Continuous Mode up to 250Hz with Automatic Set/Reset, 2ms Measurement Time - modm::ResumableResult + bool configureContinuousMode(uint8_t frequency, Bandwidth bandwidth=Bandwidth::Ms2_0) { - RF_BEGIN(); - if (not RF_CALL(write(Register::ODR, frequency))) - RF_RETURN(false); - if (not RF_CALL(update(Register::InternalControl1, Register_t(i(bandwidth))))) - RF_RETURN(false); - if (not RF_CALL(update(Register::InternalControl0, Control0::Cmm_freq_en | Control0::Auto_SR_en))) - RF_RETURN(false); - RF_END_RETURN_CALL(update(Register::InternalControl2, Control2::Cmm_en)); + if (not write(Register::ODR, frequency)) + return false; + if (not update(Register::InternalControl1, Register_t(i(bandwidth)))) + return false; + if (not update(Register::InternalControl0, Control0::Cmm_freq_en | Control0::Auto_SR_en)) + return false; + return update(Register::InternalControl2, Control2::Cmm_en); } public: @@ -312,44 +309,39 @@ class Mmc5603 : public mmc5603, public modm::I2cDevice< I2cMaster, 3 > Control2_t getControl2() { return rb(Register::InternalControl2); } public: - modm::ResumableResult + bool readProductId(uint8_t &value) { return read(Register::ProductID, value); } - modm::ResumableResult + bool readMagneticField() { return read(Register::Xout0, data.data, 10); } public: - modm::ResumableResult + bool update(Register reg, Register_t setMask, Register_t clearMask = Register_t(0)) { - RF_BEGIN(); rb(reg) = (rb(reg) & ~clearMask.value) | setMask.value; - RF_END_RETURN_CALL(write(reg, rb(reg))); + return write(reg, rb(reg)); } - modm::ResumableResult + bool write(Register reg, uint8_t value) { - RF_BEGIN(); buffer[0] = i(reg); buffer[1] = value; - this->transaction.configureWrite(buffer, 2); - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::write(buffer, 2); } - modm::ResumableResult + bool read(Register reg, uint8_t &value) { return read(reg, &value, 1); } - modm::ResumableResult + bool read(Register reg, uint8_t *data, uint8_t size) { - RF_BEGIN(); buffer[0] = i(reg); - this->transaction.configureWriteRead(buffer, 1, data, size); - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::writeRead(buffer, 1, data, size); } protected: diff --git a/src/modm/driver/inertial/qmc5883l.hpp b/src/modm/driver/inertial/qmc5883l.hpp index 18d4ececcd..07f76db435 100644 --- a/src/modm/driver/inertial/qmc5883l.hpp +++ b/src/modm/driver/inertial/qmc5883l.hpp @@ -1,3 +1,14 @@ +/* + * Copyright (c) 2023, Alexander Solovets + * + * This file is part of the modm project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ +// ---------------------------------------------------------------------------- + #pragma once #include @@ -5,6 +16,9 @@ #include #include +namespace modm +{ + template class Qmc5883l; @@ -124,23 +138,19 @@ struct Qmc5883lRegisters }; template -class Qmc5883l : public Qmc5883lRegisters, public modm::I2cDevice +class Qmc5883l : public Qmc5883lRegisters, public I2cDevice { /// @cond Data &data; /// @endcond uint8_t buffer[sizeof data.data]; - modm::ResumableResult + bool writeRegister(Register reg, uint8_t value) { - RF_BEGIN(); - buffer[0] = uint8_t(reg); buffer[1] = value; - this->transaction.configureWrite(buffer, 2); - - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::write(buffer, 2); } public: @@ -156,7 +166,7 @@ class Qmc5883l : public Qmc5883lRegisters, public modm::I2cDevice auto status() { return Status_t(data.data[uint8_t(Register::Status)]); } public: - modm::ResumableResult + bool initialize() { // Per datasheet: @@ -164,33 +174,31 @@ class Qmc5883l : public Qmc5883lRegisters, public modm::I2cDevice return writeRegister(Register::SetReset, 0x01); } - modm::ResumableResult + bool configure(Mode_t mode, Control1_t control) { control |= mode; return writeRegister(Register::Control1, control.value); } - modm::ResumableResult + bool configure(Control2_t control) { return writeRegister(Register::Control2, control.value); } - modm::ResumableResult + bool readData() { - RF_BEGIN(); - buffer[0] = uint8_t(Register::DataX_Lsb); - this->transaction.configureWriteRead(buffer, 1, buffer, sizeof buffer); - - if (RF_CALL(this->runTransaction())) + if (I2cDevice::writeRead(buffer, 1, buffer, sizeof buffer)) { std::copy_n(buffer, sizeof data.data, data.data); - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } }; + +} // namespace modm diff --git a/src/modm/driver/motion/adns9800.lb b/src/modm/driver/motion/adns9800.lb index d22b64bc3b..c4194ebc2b 100644 --- a/src/modm/driver/motion/adns9800.lb +++ b/src/modm/driver/motion/adns9800.lb @@ -29,7 +29,7 @@ def prepare(module, options): default="a6")) module.depends( - ":architecture:delay", + ":architecture:fiber", ":architecture:spi", ":debug") return True diff --git a/src/modm/driver/motion/adns9800_impl.hpp b/src/modm/driver/motion/adns9800_impl.hpp index 1456202df0..c6d6daee24 100644 --- a/src/modm/driver/motion/adns9800_impl.hpp +++ b/src/modm/driver/motion/adns9800_impl.hpp @@ -20,7 +20,7 @@ #endif #include -#include +#include #include "adns9800.hpp" #include "adns9800_srom.hpp" @@ -43,18 +43,18 @@ void modm::Adns9800< Spi, Cs>::getDeltaXY(int16_t &delta_x, int16_t &delta_y) { Cs::reset(); - modm::delay_us(100); // tSRAD + modm::this_fiber::sleep_for(100us); // tSRAD - Spi::transferBlocking(static_cast(Register::Motion_Burst)); + Spi::transfer(static_cast(Register::Motion_Burst)); // Delay tSRAD - modm::delay_us(100); + modm::this_fiber::sleep_for(100us); static constexpr uint8_t buf_size = 6; uint8_t tx_buf[buf_size]; uint8_t rx_buf[buf_size]; - Spi::transferBlocking(tx_buf, rx_buf, buf_size); + Spi::transfer(tx_buf, rx_buf, buf_size); modm::delay_ns(120); // tSCLK-NCS for read operation is 120ns Cs::set(); @@ -77,15 +77,15 @@ modm::Adns9800< Spi, Cs>::readReg(Register const reg) uint8_t address = static_cast(reg); // send adress of the register, with MSBit = 0 to indicate it's a read - Spi::transferBlocking(address & 0x7f); - modm::delay_us(100); // tSRAD + Spi::transfer(address & 0x7f); + modm::this_fiber::sleep_for(100us); // tSRAD // read data - uint8_t data = Spi::transferBlocking(0); + uint8_t data = Spi::transfer(0); modm::delay_ns(120); // tSCLK-NCS for read operation is 120ns Cs::set(); - modm::delay_us(19); // tSRW/tSRR (=20us) minus tSCLK-NCS + modm::this_fiber::sleep_for(19us); // tSRW/tSRR (=20us) minus tSCLK-NCS return data; } @@ -99,14 +99,14 @@ modm::Adns9800< Spi, Cs>::writeReg(Register const reg, uint8_t const data) uint8_t address = static_cast(reg); //send adress of the register, with MSBit = 1 to indicate it's a write - Spi::transferBlocking(address | 0x80); + Spi::transfer(address | 0x80); //send data - Spi::transferBlocking(data); + Spi::transfer(data); - modm::delay_us(20); // tSCLK-NCS for write operation + modm::this_fiber::sleep_for(20us); // tSCLK-NCS for write operation Cs::set(); - modm::delay_us(100); // tSWW/tSWR (=120us) minus tSCLK-NCS. Could be shortened, but is looks like a safe lower bound + modm::this_fiber::sleep_for(100us); // tSWW/tSWR (=120us) minus tSCLK-NCS. Could be shortened, but is looks like a safe lower bound } template < typename Spi, typename Cs > @@ -120,7 +120,7 @@ modm::Adns9800< Spi, Cs >::uploadFirmware() writeReg(Register::SROM_Enable, 0x1d); // wait for more than one frame period - modm::delay_ms(10); // assume that the frame rate is as low as 100fps... even if it should never be that low + modm::this_fiber::sleep_for(10ms); // assume that the frame rate is as low as 100fps... even if it should never be that low // write 0x18 to SROM_enable to start SROM download writeReg(Register::SROM_Enable, 0x18); @@ -130,14 +130,14 @@ modm::Adns9800< Spi, Cs >::uploadFirmware() // write burst destination address uint8_t address = static_cast(Register::SROM_Load_Burst) | 0x80; - Spi::transferBlocking(address); - modm::delay_us(15); + Spi::transfer(address); + modm::this_fiber::sleep_for(15us); // send all bytes of the firmware for(int ii = 0; ii < firmware_length; ++ii) { - Spi::transferBlocking(firmware_data[ii]); - modm::delay_us(15); + Spi::transfer(firmware_data[ii]); + modm::this_fiber::sleep_for(15us); } Cs::set(); @@ -155,7 +155,7 @@ modm::Adns9800< Spi, Cs >::initialise() Cs::set(); writeReg(Register::Power_Up_Reset, 0x5a); // force reset - modm::delay_ms(50); // wait for it to reboot + modm::this_fiber::sleep_for(50ms); // wait for it to reboot // Read Product ID uint8_t id = readReg(Register::Product_ID); @@ -191,7 +191,7 @@ modm::Adns9800< Spi, Cs >::initialise() readReg(Register::Delta_Y_H); uploadFirmware(); - modm::delay_ms(10); + modm::this_fiber::sleep_for(10ms); // enable laser(bit 0 = 0b), in normal mode (bits 3,2,1 = 000b) // reading the actual value of the register is important because the real @@ -214,7 +214,7 @@ modm::Adns9800< Spi, Cs >::initialise() writeReg(Register::Frame_Period_Max_Bound_Lower, 0x20); writeReg(Register::Frame_Period_Max_Bound_Upper, 0x1b); - modm::delay_ms(1); + modm::this_fiber::sleep_for(1ms); return success; } diff --git a/src/modm/driver/motion/pat9125el.hpp b/src/modm/driver/motion/pat9125el.hpp index 24d2fad6df..106c0e3c4d 100644 --- a/src/modm/driver/motion/pat9125el.hpp +++ b/src/modm/driver/motion/pat9125el.hpp @@ -14,8 +14,8 @@ #include #include -#include #include +#include "pat9125el_transport.hpp" namespace modm { @@ -95,13 +95,13 @@ class Pat9125el : public pat9125el, public Transport * @param xResolution x resolution in unit of 5 counts per inch * @param yResolution y resolution in unit of 5 counts per inch */ - modm::ResumableResult + bool configure(uint8_t xResolution = 0x14, uint8_t yResolution = 0x14); /** * Check if the device is available */ - modm::ResumableResult + bool ping(); /** @@ -113,7 +113,7 @@ class Pat9125el : public pat9125el, public Transport /** * Read movement measurement from the device */ - modm::ResumableResult + bool readData(); /** @@ -126,13 +126,13 @@ class Pat9125el : public pat9125el, public Transport resetMoved(); private: - modm::ResumableResult + bool writeRegister(Register reg, uint8_t data); - modm::ResumableResult + bool readRegister(Register reg, uint8_t& data); - modm::ResumableResult + bool readRegister(Register reg, uint8_t* data, size_t size); void diff --git a/src/modm/driver/motion/pat9125el.lb b/src/modm/driver/motion/pat9125el.lb index 0f09b9c08d..dbdb9ad326 100644 --- a/src/modm/driver/motion/pat9125el.lb +++ b/src/modm/driver/motion/pat9125el.lb @@ -17,8 +17,6 @@ def init(module): def prepare(module, options): module.depends( - ":architecture:delay", - ":architecture:spi", ":architecture:i2c.device", ":math:geometry", ":debug") diff --git a/src/modm/driver/motion/pat9125el_impl.hpp b/src/modm/driver/motion/pat9125el_impl.hpp index 35766fe3eb..85108f4871 100644 --- a/src/modm/driver/motion/pat9125el_impl.hpp +++ b/src/modm/driver/motion/pat9125el_impl.hpp @@ -24,40 +24,37 @@ Pat9125el::Pat9125el(TransportParams&&... params) } template -modm::ResumableResult +bool Pat9125el::configure(uint8_t xResolution, uint8_t yResolution) { - RF_BEGIN(); - - success = RF_CALL(writeRegister(Register::WriteProtect, uint8_t(WriteProtect::Disabled))); + success = writeRegister(Register::WriteProtect, uint8_t(WriteProtect::Disabled)); if(!success) { - RF_RETURN(false); + return false; } - success = RF_CALL(writeRegister(Register::ResolutionX, xResolution)); + success = writeRegister(Register::ResolutionX, xResolution); if(!success) { - RF_RETURN(false); + return false; } - success = RF_CALL(writeRegister(Register::ResolutionY, yResolution)); + success = writeRegister(Register::ResolutionY, yResolution); if(!success) { - RF_RETURN(false); + return false; } - RF_END_RETURN_CALL(writeRegister(Register::WriteProtect, uint8_t(WriteProtect::Enabled))); + return writeRegister(Register::WriteProtect, uint8_t(WriteProtect::Enabled)); } template -modm::ResumableResult +bool Pat9125el::ping() { - RF_BEGIN(); - success = RF_CALL(readRegister(Register::ProductId1, readBuffer[0])); + success = readRegister(Register::ProductId1, readBuffer[0]); if(!success) { - RF_RETURN(false); + return false; } - RF_END_RETURN(readBuffer[0] == ProductId1); + return readBuffer[0] == ProductId1; } template @@ -68,39 +65,37 @@ Pat9125el::getData() const } template -modm::ResumableResult +bool Pat9125el::readData() { - RF_BEGIN(); - if constexpr(UseInterruptPin) { if(IntPin::read()) { - RF_RETURN(false); + return false; } } - success = RF_CALL(readRegister(Register::MotionStatus, status)); + success = readRegister(Register::MotionStatus, status); if(!success) { - RF_RETURN(false); + return false; } if(MotionStatus_t{status} & MotionStatus::DataAvailable) { // read x and y low data registers - success = RF_CALL(readRegister(Register::DeltaXLow, &readBuffer[0], 2)); + success = readRegister(Register::DeltaXLow, &readBuffer[0], 2); if(!success) { - RF_RETURN(false); + return false; } // read x/y high data register - success = RF_CALL(readRegister(Register::DeltaXYHigh, &readBuffer[2], 1)); + success = readRegister(Register::DeltaXYHigh, &readBuffer[2], 1); if(!success) { - RF_RETURN(false); + return false; } updateData(); } - RF_END_RETURN(true); + return true; } template @@ -131,21 +126,21 @@ Pat9125el::resetMoved() } template -modm::ResumableResult +bool Pat9125el::writeRegister(Register reg, uint8_t data) { return this->write(static_cast(reg), data); } template -modm::ResumableResult +bool Pat9125el::readRegister(Register reg, uint8_t& data) { return this->read(static_cast(reg), data); } template -modm::ResumableResult +bool Pat9125el::readRegister(Register reg, uint8_t* data, size_t size) { return this->read(static_cast(reg), data, size); diff --git a/src/modm/driver/motion/pat9125el_transport.hpp b/src/modm/driver/motion/pat9125el_transport.hpp index 37734d79b0..e143476564 100644 --- a/src/modm/driver/motion/pat9125el_transport.hpp +++ b/src/modm/driver/motion/pat9125el_transport.hpp @@ -13,7 +13,6 @@ #define MODM_PAT9125EL_TRANSPORT_HPP #include -#include #include namespace modm @@ -26,18 +25,18 @@ namespace modm * @ingroup modm_driver_pat9125el */ template -class Pat9125elI2cTransport : public I2cDevice +class Pat9125elI2cTransport : public I2cDevice { protected: Pat9125elI2cTransport(uint8_t address); - modm::ResumableResult + bool write(uint8_t reg, uint8_t value); - modm::ResumableResult + bool read(uint8_t reg, uint8_t& value); - modm::ResumableResult + bool read(uint8_t reg, uint8_t* buffer, uint8_t length); private: diff --git a/src/modm/driver/motion/pat9125el_transport_impl.hpp b/src/modm/driver/motion/pat9125el_transport_impl.hpp index f85d215a89..c2ceaf5b56 100644 --- a/src/modm/driver/motion/pat9125el_transport_impl.hpp +++ b/src/modm/driver/motion/pat9125el_transport_impl.hpp @@ -18,41 +18,32 @@ namespace modm template Pat9125elI2cTransport::Pat9125elI2cTransport(uint8_t address) - : I2cDevice{address} + : I2cDevice{address} { } template -modm::ResumableResult +bool Pat9125elI2cTransport::write(uint8_t reg, uint8_t value) { - RF_BEGIN(); - buffer[0] = reg; buffer[1] = value; - - this->transaction.configureWrite(&buffer[0], 2); - - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::write(&buffer[0], 2); } template -modm::ResumableResult +bool Pat9125elI2cTransport::read(uint8_t reg, uint8_t& value) { return read(reg, &value, 1); } template -modm::ResumableResult +bool Pat9125elI2cTransport::read(uint8_t reg, uint8_t* buffer, uint8_t length) { - RF_BEGIN(); - buffer[0] = reg; - this->transaction.configureWriteRead(&buffer[0], 1, buffer, length); - - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::writeRead(&buffer[0], 1, buffer, length); } } diff --git a/src/modm/driver/motor/drv832x_spi.hpp b/src/modm/driver/motor/drv832x_spi.hpp index 391e6ad2c0..9fc5d7f393 100644 --- a/src/modm/driver/motor/drv832x_spi.hpp +++ b/src/modm/driver/motor/drv832x_spi.hpp @@ -16,7 +16,7 @@ #include #include #include -#include +#include namespace modm { @@ -297,7 +297,7 @@ struct drv832xSpi * \author Raphael Lehmann */ template < class SpiMaster, class Cs > -class Drv832xSpi : public drv832xSpi, public modm::SpiDevice< SpiMaster >, protected modm::NestedResumable<3> +class Drv832xSpi : public drv832xSpi, public modm::SpiDevice< SpiMaster > { public: /** @@ -307,31 +307,31 @@ class Drv832xSpi : public drv832xSpi, public modm::SpiDevice< SpiMaster >, prote */ Drv832xSpi(); - modm::ResumableResult + void readFaultStatus1(); - modm::ResumableResult + void readVgsStatus2(); - modm::ResumableResult + void readDriverControl(); - modm::ResumableResult + void readGateDriveHS(); - modm::ResumableResult + void readGateDriveLS(); - modm::ResumableResult + void readOcpControl(); - modm::ResumableResult + void readCsaControl(); - modm::ResumableResult + void readAll(); - modm::ResumableResult + void initialize(); private: @@ -373,17 +373,17 @@ class Drv832xSpi : public drv832xSpi, public modm::SpiDevice< SpiMaster >, prote return _csaControl; } - modm::ResumableResult + void commit(); protected: - modm::ResumableResult + void writeData(Register address, uint16_t data); - modm::ResumableResult + uint16_t readData(Register address); - modm::ResumableResult + void readData(Register address, uint16_t& data); private: diff --git a/src/modm/driver/motor/drv832x_spi.lb b/src/modm/driver/motor/drv832x_spi.lb index e3d8bd7586..fe675977aa 100644 --- a/src/modm/driver/motor/drv832x_spi.lb +++ b/src/modm/driver/motor/drv832x_spi.lb @@ -25,7 +25,7 @@ def prepare(module, options): ":architecture:gpio", ":architecture:register", ":architecture:spi.device", - ":processing:resumable") + ":architecture:fiber") return True def build(env): diff --git a/src/modm/driver/motor/drv832x_spi_impl.hpp b/src/modm/driver/motor/drv832x_spi_impl.hpp index 47905abd69..3067e89581 100644 --- a/src/modm/driver/motor/drv832x_spi_impl.hpp +++ b/src/modm/driver/motor/drv832x_spi_impl.hpp @@ -27,108 +27,102 @@ modm::Drv832xSpi::Drv832xSpi() } template < class SpiMaster, class Cs > -modm::ResumableResult +void modm::Drv832xSpi::initialize() { return readAll(); } template < class SpiMaster, class Cs > -modm::ResumableResult +void modm::Drv832xSpi::readAll() { - RF_BEGIN(); - RF_CALL(readFaultStatus1()); - RF_CALL(readVgsStatus2()); - RF_CALL(readDriverControl()); - RF_CALL(readGateDriveHS()); - RF_CALL(readGateDriveLS()); - RF_CALL(readOcpControl()); - RF_CALL(readCsaControl()); - RF_END(); + readFaultStatus1(); + readVgsStatus2(); + readDriverControl(); + readGateDriveHS(); + readGateDriveLS(); + readOcpControl(); + readCsaControl(); } template < class SpiMaster, class Cs > -modm::ResumableResult +void modm::Drv832xSpi::readFaultStatus1() { return readData(Register::FaultStatus1, _faultStatus1.value); } template < class SpiMaster, class Cs > -modm::ResumableResult +void modm::Drv832xSpi::readVgsStatus2() { return readData(Register::VgsStatus2, _vgsStatus2.value); } template < class SpiMaster, class Cs > -modm::ResumableResult +void modm::Drv832xSpi::readDriverControl() { return readData(Register::DriverControl, _driverControl.value); } template < class SpiMaster, class Cs > -modm::ResumableResult +void modm::Drv832xSpi::readGateDriveHS() { return readData(Register::GateDriveHS, _gateDriveHS.value); } template < class SpiMaster, class Cs > -modm::ResumableResult +void modm::Drv832xSpi::readGateDriveLS() { return readData(Register::GateDriveLS, _gateDriveLS.value); } template < class SpiMaster, class Cs > -modm::ResumableResult +void modm::Drv832xSpi::readOcpControl() { return readData(Register::OcpControl, _ocpControl.value); } template < class SpiMaster, class Cs > -modm::ResumableResult +void modm::Drv832xSpi::readCsaControl() { return readData(Register::CsaControl, _csaControl.value); } template < class SpiMaster, class Cs > -modm::ResumableResult +void modm::Drv832xSpi::commit() { - RF_BEGIN(); if(accessBitmap & 0b0000100) { - RF_CALL(writeData(Register::DriverControl, _driverControl.value)); + writeData(Register::DriverControl, _driverControl.value); } if(accessBitmap & 0b0001000) { - RF_CALL(writeData(Register::GateDriveHS, _gateDriveHS.value)); + writeData(Register::GateDriveHS, _gateDriveHS.value); } if(accessBitmap & 0b0010000) { - RF_CALL(writeData(Register::GateDriveLS, _gateDriveLS.value)); + writeData(Register::GateDriveLS, _gateDriveLS.value); } if(accessBitmap & 0b0100000) { - RF_CALL(writeData(Register::OcpControl, _ocpControl.value)); + writeData(Register::OcpControl, _ocpControl.value); } if(accessBitmap & 0b1000000) { - RF_CALL(writeData(Register::CsaControl, _csaControl.value)); + writeData(Register::CsaControl, _csaControl.value); } accessBitmap = 0; - RF_END(); } template < class SpiMaster, class Cs > -modm::ResumableResult +void modm::Drv832xSpi::writeData(Register address, uint16_t data) { - RF_BEGIN(); - - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); inBuffer[0] = 0; @@ -139,22 +133,18 @@ modm::Drv832xSpi::writeData(Register address, uint16_t data) outBuffer[0] = writeBit | (static_cast(address) << 3) | ((data >> 8) & 0b111); outBuffer[1] = data & 0xff; - RF_CALL(SpiMaster::transfer(outBuffer, inBuffer, 2)); + SpiMaster::transfer(outBuffer, inBuffer, 2); if (this->releaseMaster()) { Cs::set(); } - - RF_END(); } template < class SpiMaster, class Cs > -modm::ResumableResult +void modm::Drv832xSpi::readData(Register address, uint16_t& data) { - RF_BEGIN(); - - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); inBuffer[0] = 0x00; @@ -165,12 +155,11 @@ modm::Drv832xSpi::readData(Register address, uint16_t& data) outBuffer[0] = writeBit | (static_cast(address) << 3); outBuffer[1] = 0; - RF_CALL(SpiMaster::transfer(outBuffer, inBuffer, 2)); + SpiMaster::transfer(outBuffer, inBuffer, 2); if (this->releaseMaster()) { Cs::set(); } data = static_cast(inBuffer[1]) | (static_cast(inBuffer[0] & 0b111) << 8); - RF_END(); } diff --git a/src/modm/driver/other/ad840x.hpp b/src/modm/driver/other/ad840x.hpp index 1b5566f242..d9f4c6e992 100644 --- a/src/modm/driver/other/ad840x.hpp +++ b/src/modm/driver/other/ad840x.hpp @@ -15,7 +15,7 @@ #include #include -#include +#include namespace modm { diff --git a/src/modm/driver/other/ad840x_impl.hpp b/src/modm/driver/other/ad840x_impl.hpp index ab7bc06cfe..c02a17798c 100644 --- a/src/modm/driver/other/ad840x_impl.hpp +++ b/src/modm/driver/other/ad840x_impl.hpp @@ -33,7 +33,7 @@ inline void modm::AD840x::reset() { Rs::reset(); - modm::delay_us(1); // wait at least 50ns + modm::this_fiber::sleep_for(1us); // wait at least 50ns Rs::set(); } diff --git a/src/modm/driver/position/vl53l0.hpp b/src/modm/driver/position/vl53l0.hpp index 642222eab3..9657fff560 100644 --- a/src/modm/driver/position/vl53l0.hpp +++ b/src/modm/driver/position/vl53l0.hpp @@ -17,7 +17,6 @@ #include #include #include -#include namespace modm { @@ -338,7 +337,7 @@ operator << (IOStream& os, const vl53l0::RangeErrorCode& c); * @ingroup modm_driver_vl53l0 */ template < typename I2cMaster > -class Vl53l0 : public vl53l0, public modm::I2cDevice< I2cMaster, 5 > +class Vl53l0 : public vl53l0, public modm::I2cDevice< I2cMaster > { public: /// Constructor, requires an vl53l0::Data object, sets address to default of 0x29 @@ -346,23 +345,23 @@ class Vl53l0 : public vl53l0, public modm::I2cDevice< I2cMaster, 5 > /// Ping the I2C device. /// This overwrites the build-in ping method of I2cDevice with a read of the model number. - modm::ResumableResult + bool ping(); /// Soft reset of device. - modm::ResumableResult + bool reset(); - modm::ResumableResult + bool initialize(); /// Set a new I2C address (< 128) for this device. /// The address is not permanent and must be set again after every device boot. - modm::ResumableResult + bool setDeviceAddress(uint8_t address); /// Reads the distance and buffers the result - inline modm::ResumableResult + inline bool readDistance() { return readSensor(); } @@ -371,7 +370,7 @@ class Vl53l0 : public vl53l0, public modm::I2cDevice< I2cMaster, 5 > { return data.error; } template - modm::ResumableResult + bool updateRegister(Register reg, T setMask, T clearMask = T(0xff)) { return updateControlRegister(reg, Control_t(setMask), Control_t(clearMask)); @@ -388,7 +387,7 @@ class Vl53l0 : public vl53l0, public modm::I2cDevice< I2cMaster, 5 > /// The default is ~30ms. ST recommends 2'000'000us = 200ms for high precision. /// /// useful range of values: ~20'000us - 2'000'000us (20ms - 2s) - modm::ResumableResult + bool setMaxMeasurementTime(uint32_t timeUs); /// Get the configured maximum measurement time. @@ -398,38 +397,38 @@ class Vl53l0 : public vl53l0, public modm::I2cDevice< I2cMaster, 5 > protected: /// @cond /// read multiple 8bit values from subsequent registers - modm::ResumableResult + bool read(Register reg, uint8_t *buffer, uint8_t length); /// @endcond /// @cond /// read an 8bit value from a register - modm::ResumableResult + bool read(Register reg, uint8_t& value) { return read(reg, &value, 1); } /// @endcond /// @cond /// write to a control register - modm::ResumableResult + bool write(Register reg, Control_t value) { return write(reg, value.value); } /// @endcond /// @cond /// write an 8bit value to a register - modm::ResumableResult + bool write(Register reg, uint8_t value); /// @endcond /// @cond /// write a 16bit value to 2 subsequent registers in big endian format - modm::ResumableResult + bool writeUInt16(Register reg, uint16_t value); /// @endcond /// @cond - modm::ResumableResult + bool updateControlRegister(Register reg, Control_t setMask, Control_t clearMask = Control_t(0xff)); /// @endcond @@ -439,7 +438,7 @@ class Vl53l0 : public vl53l0, public modm::I2cDevice< I2cMaster, 5 > /// /// the predicate must take a uint8_t argument and return a bool template < typename Predicate > - modm::ResumableResult + bool poll(Register reg, Predicate pred, uint16_t timeoutMs = 20, uint16_t stepMs = 4); /// @endcond private: @@ -449,30 +448,30 @@ class Vl53l0 : public vl53l0, public modm::I2cDevice< I2cMaster, 5 > /// /// The maximum data length is 6 since i2cBuffer[0] /// will contain the register address. - modm::ResumableResult + bool writeI2CBuffer(Register reg, uint8_t dataLength); /// @endcond - modm::ResumableResult + bool readSensor(); - modm::ResumableResult + bool checkModelID(); - modm::ResumableResult + bool checkRevisionID(); - modm::ResumableResult + bool loadTuningSettings(); // SPAD = "single photon avalanche diode" - modm::ResumableResult + bool initializeSpadConfig(); - modm::ResumableResult + bool performReferenceCalibration(Start_t modeFlags = Start_t(0)); - modm::ResumableResult + bool readSequenceInfo(); private: @@ -501,8 +500,6 @@ class Vl53l0 : public vl53l0, public modm::I2cDevice< I2cMaster, 5 > Data &data; - modm::ShortTimeout timeout; - // Internal I2C write buffer // 0: Index[7:0] // 1: Data[0] diff --git a/src/modm/driver/position/vl53l0.lb b/src/modm/driver/position/vl53l0.lb index 20799f08d5..71bd3fbc1d 100644 --- a/src/modm/driver/position/vl53l0.lb +++ b/src/modm/driver/position/vl53l0.lb @@ -20,8 +20,7 @@ def prepare(module, options): ":architecture:accessor", ":architecture:i2c.device", ":architecture:register", - ":debug", - ":processing:timer") + ":debug") return True def build(env): diff --git a/src/modm/driver/position/vl53l0_impl.hpp b/src/modm/driver/position/vl53l0_impl.hpp index 2c7b8050fc..fb1fd8b1d8 100644 --- a/src/modm/driver/position/vl53l0_impl.hpp +++ b/src/modm/driver/position/vl53l0_impl.hpp @@ -17,12 +17,10 @@ #include #include -#define VL53L0_RF_CALL(rf) if(not RF_CALL(rf)) { RF_RETURN(false); } - // ---------------------------------------------------------------------------- template < typename I2cMaster > modm::Vl53l0::Vl53l0(Data &data, uint8_t address) -: I2cDevice{address}, data{data}, +: I2cDevice{address}, data{data}, i2cBuffer{0,0,0,0,0,0,0}, index{0}, measurementTimeUs{DefaultMeasurementTime} { } @@ -30,44 +28,37 @@ modm::Vl53l0::Vl53l0(Data &data, uint8_t address) // MARK: - i2cTasks // MARK: ping template < class I2cMaster > -modm::ResumableResult +bool modm::Vl53l0::ping() { - RF_BEGIN(); + if (not checkModelID()) return false; - VL53L0_RF_CALL(checkModelID()); - - RF_END_RETURN_CALL(checkRevisionID()); + return checkRevisionID(); } template < typename I2cMaster > -modm::ResumableResult +bool modm::Vl53l0::reset() { - RF_BEGIN(); - // Set Reset - VL53L0_RF_CALL(write(Register::SOFT_RESET__GO2_SOFT_RESET_N, 0x00)); + if (not write(Register::SOFT_RESET__GO2_SOFT_RESET_N, 0x00)) return false; // Wait until the device is in reset - VL53L0_RF_CALL(poll(Register::IDENTIFICATION__MODEL_ID, [](uint8_t value) { + if (not poll(Register::IDENTIFICATION__MODEL_ID, [](uint8_t value) { return value == 0x00; - })); + })) return false; // Release Reset - VL53L0_RF_CALL(write(Register::SOFT_RESET__GO2_SOFT_RESET_N, 0x01)); + if (not write(Register::SOFT_RESET__GO2_SOFT_RESET_N, 0x01)) return false; // Wait until the device responds again // After releasing reset it does not accept any I2C transactions // for some time and will respond with NACKs - timeout.restart(500ms); - RF_WAIT_UNTIL(RF_CALL(ping()) or timeout.isExpired()); - - RF_END_RETURN(not timeout.isExpired()); + return modm::this_fiber::poll_for(500ms, [&]{ return ping(); }); } template < typename I2cMaster > -modm::ResumableResult +bool modm::Vl53l0::initialize() { using namespace vl53l0_private; @@ -82,245 +73,229 @@ modm::Vl53l0::initialize() constexpr MSRCConfig_t msrcConfig = MSRCConfig::DisableRateCheck | MSRCConfig::DisablePreRangeCheck; - RF_BEGIN(); - // read the measurement sequence configuration into this->sequenceInfo - VL53L0_RF_CALL(readSequenceInfo()); + if (not readSequenceInfo()) return false; // "Set I2C standard mode" - VL53L0_RF_CALL(write(Register::UNDOCUMENTED__I2C_MODE, 0x00)); + if (not write(Register::UNDOCUMENTED__I2C_MODE, 0x00)) return false; // magic is happening here ... - VL53L0_RF_CALL(write(Register(0x80), 0x01)); - VL53L0_RF_CALL(write(Register(0xFF), 0x01)); - VL53L0_RF_CALL(write(Register(0x00), 0x00)); - VL53L0_RF_CALL(read(Register(0x91), stopMode)); - VL53L0_RF_CALL(write(Register(0x00), 0x01)); - VL53L0_RF_CALL(write(Register(0xFF), 0x00)); - VL53L0_RF_CALL(write(Register(0x80), 0x00)); + if (not write(Register(0x80), 0x01)) return false; + if (not write(Register(0xFF), 0x01)) return false; + if (not write(Register(0x00), 0x00)) return false; + if (not read(Register(0x91), stopMode)) return false; + if (not write(Register(0x00), 0x01)) return false; + if (not write(Register(0xFF), 0x00)) return false; + if (not write(Register(0x80), 0x00)) return false; - VL53L0_RF_CALL(updateControlRegister(Register::MSRC__CONFIG_CONTROL, msrcConfig, Control_t(0))); + if (not updateControlRegister(Register::MSRC__CONFIG_CONTROL, msrcConfig, Control_t(0))) return false; - VL53L0_RF_CALL(writeUInt16(Register::FINAL_RANGE__CONFIG_MIN_COUNT_RATE_RTN_LIMIT, DefaultSignalRateLimit)); + if (not writeUInt16(Register::FINAL_RANGE__CONFIG_MIN_COUNT_RATE_RTN_LIMIT, DefaultSignalRateLimit)) return false; // finish "data init" phase - VL53L0_RF_CALL(write(Register::SYSTEM__SEQUENCE_CONFIG, 0xFF)); + if (not write(Register::SYSTEM__SEQUENCE_CONFIG, 0xFF)) return false; // load SPAD calibration data from NVM and initialize dynamic SPAD configuration - VL53L0_RF_CALL(initializeSpadConfig()); + if (not initializeSpadConfig()) return false; - VL53L0_RF_CALL(loadTuningSettings()); + if (not loadTuningSettings()) return false; // enable "new sample ready" interrupt flag - VL53L0_RF_CALL(write(Register::SYSTEM__INTERRUPT_CONFIG_GPIO, InterruptConfig::NewSampleReady)); - VL53L0_RF_CALL(updateControlRegister(Register::GPIO__HV_MUX_ACTIVE_HIGH, - Control_t(0), GpioConfig::InterruptPolarityHigh)); - VL53L0_RF_CALL(write(Register::SYSTEM__INTERRUPT_CLEAR, InterruptClear::Range)); + if (not write(Register::SYSTEM__INTERRUPT_CONFIG_GPIO, InterruptConfig::NewSampleReady)) return false; + if (not updateControlRegister(Register::GPIO__HV_MUX_ACTIVE_HIGH, + Control_t(0), GpioConfig::InterruptPolarityHigh)) return false; + if (not write(Register::SYSTEM__INTERRUPT_CLEAR, InterruptClear::Range)) return false; // set default measurement sequence (TCC and MSRC disabled) - VL53L0_RF_CALL(write(Register::SYSTEM__SEQUENCE_CONFIG, standardSequence)); + if (not write(Register::SYSTEM__SEQUENCE_CONFIG, standardSequence)) return false; // recalculate measurement timings - VL53L0_RF_CALL(setMaxMeasurementTime(this->measurementTimeUs)); + if (not setMaxMeasurementTime(this->measurementTimeUs)) return false; // VHV calibration - VL53L0_RF_CALL(write(Register::SYSTEM__SEQUENCE_CONFIG, MeasurementSequenceStep::VhvCalibration)); - if(not RF_CALL(performReferenceCalibration(Start::VhvCalibrationMode))) { + if (not write(Register::SYSTEM__SEQUENCE_CONFIG, MeasurementSequenceStep::VhvCalibration)) return false; + if(not performReferenceCalibration(Start::VhvCalibrationMode)) { MODM_LOG_ERROR << "VHV calibration failed." << modm::endl; - RF_RETURN(false); + return false; } // phase calibration - VL53L0_RF_CALL(write(Register::SYSTEM__SEQUENCE_CONFIG, MeasurementSequenceStep::PhaseCalibration)); - if(not RF_CALL(performReferenceCalibration())) { + if (not write(Register::SYSTEM__SEQUENCE_CONFIG, MeasurementSequenceStep::PhaseCalibration)) return false; + if(not performReferenceCalibration()) { MODM_LOG_ERROR << "Phase calibration failed." << modm::endl; - RF_RETURN(false); + return false; } // restore measurement sequence settings - VL53L0_RF_CALL(write(Register::SYSTEM__SEQUENCE_CONFIG, standardSequence)); + if (not write(Register::SYSTEM__SEQUENCE_CONFIG, standardSequence)) return false; - RF_END_RETURN(true); + return true; } template < typename I2cMaster > -modm::ResumableResult +bool modm::Vl53l0::loadTuningSettings() { using namespace vl53l0_private; - RF_BEGIN(); - // write the configuration for(index = 0; index < 80; index++) { i2cBuffer[1] = configuration[index].value; - VL53L0_RF_CALL( write(Register(configuration[index].reg), i2cBuffer[1]) ); + if (not write(Register(configuration[index].reg), i2cBuffer[1])) return false; } - RF_END_RETURN(true); + return true; } template < typename I2cMaster > -modm::ResumableResult +bool modm::Vl53l0::readSensor() { static_assert(MaxMeasurementTimeUs / 1000 * 2 <= std::numeric_limits::max(), "MaxMeasurementTimeUs out of range"); - RF_BEGIN(); - data.reset(); // undocumented black magic - VL53L0_RF_CALL(write(Register(0x80), 0x01)); - VL53L0_RF_CALL(write(Register(0xFF), 0x01)); - VL53L0_RF_CALL(write(Register(0x00), 0x00)); - VL53L0_RF_CALL(write(Register(0x91), stopMode)); - VL53L0_RF_CALL(write(Register(0x00), 0x01)); - VL53L0_RF_CALL(write(Register(0xFF), 0x00)); - VL53L0_RF_CALL(write(Register(0x80), 0x00)); + if (not write(Register(0x80), 0x01)) return false; + if (not write(Register(0xFF), 0x01)) return false; + if (not write(Register(0x00), 0x00)) return false; + if (not write(Register(0x91), stopMode)) return false; + if (not write(Register(0x00), 0x01)) return false; + if (not write(Register(0xFF), 0x00)) return false; + if (not write(Register(0x80), 0x00)) return false; // start range measurement - VL53L0_RF_CALL(write(Register::SYSRANGE__START, Start::StartStop)); + if (not write(Register::SYSRANGE__START, Start::StartStop)) return false; // wait for the start flag to be cleared, use default timeout - VL53L0_RF_CALL(poll(Register::SYSRANGE__START, [](uint8_t value) { + if (not poll(Register::SYSRANGE__START, [](uint8_t value) { return not (value & uint8_t(Start::StartStop)); - })); + })) return false; // wait for the measurement to finish, timeout of 2 * this->measurementTimeUs - VL53L0_RF_CALL(poll(Register::RESULT__INTERRUPT_STATUS, [](uint8_t value) { + if (not poll(Register::RESULT__INTERRUPT_STATUS, [](uint8_t value) { return value & (InterruptStatus::NewSampleReady | InterruptStatus::OutOfWindow).value; - }, measurementTimeUs / 1000 * 2)); + }, measurementTimeUs / 1000 * 2)) return false; // read 16 bit range value - VL53L0_RF_CALL(read(Register::RESULT__RANGE_VALUE0, &i2cBuffer[1], 2)); + if (not read(Register::RESULT__RANGE_VALUE0, &i2cBuffer[1], 2)) return false; data.distanceBuffer[0] = i2cBuffer[1]; data.distanceBuffer[1] = i2cBuffer[2]; // clear interrupt flag - VL53L0_RF_CALL(write(Register::SYSTEM__INTERRUPT_CLEAR, InterruptClear::Range)); + if (not write(Register::SYSTEM__INTERRUPT_CLEAR, InterruptClear::Range)) return false; // read error code - VL53L0_RF_CALL(read(Register::RESULT__RANGE_STATUS, i2cBuffer[1])); + if (not read(Register::RESULT__RANGE_STATUS, i2cBuffer[1])) return false; data.error = RangeErrorCode_t::get(RangeStatus_t(i2cBuffer[1])); - RF_END_RETURN(true); + return true; } // MARK: setMaxMeasurementTime template < class I2cMaster > -modm::ResumableResult +bool modm::Vl53l0::setMaxMeasurementTime(uint32_t timeUs) { - RF_BEGIN(); - if(timeUs > MaxMeasurementTimeUs) { MODM_LOG_ERROR << "Measurement time out of range." << modm::endl; - RF_RETURN(false); + return false; } // read the measurement sequence configuration into this->sequenceInfo - VL53L0_RF_CALL(readSequenceInfo()); + if (not readSequenceInfo()) return false; if(not calculateFinalRangeTimeout(timeUs, sequenceInfo.finalRangeTimeout)) { MODM_LOG_ERROR << "Invalid timing requested, aborting." << modm::endl; - RF_RETURN(false); + return false; } - VL53L0_RF_CALL(writeUInt16(Register::FINAL_RANGE__CONFIG_TIMEOUT_MACROP_HI, sequenceInfo.finalRangeTimeout)); + if (not writeUInt16(Register::FINAL_RANGE__CONFIG_TIMEOUT_MACROP_HI, sequenceInfo.finalRangeTimeout)) return false; this->measurementTimeUs = timeUs; - RF_END_RETURN(true); + return true; } // MARK: checkModelID template < class I2cMaster > -modm::ResumableResult +bool modm::Vl53l0::checkModelID() { - RF_BEGIN(); + if (not read(Register::IDENTIFICATION__MODEL_ID, &i2cBuffer[1], 2)) return false; - VL53L0_RF_CALL(read(Register::IDENTIFICATION__MODEL_ID, &i2cBuffer[1], 2)); - - RF_END_RETURN((i2cBuffer[1] == ModelID[0]) and (i2cBuffer[2] == ModelID[1])); + return (i2cBuffer[1] == ModelID[0]) and (i2cBuffer[2] == ModelID[1]); } // MARK: checkRevisionID template < class I2cMaster > -modm::ResumableResult +bool modm::Vl53l0::checkRevisionID() { - RF_BEGIN(); + if (not read(Register::IDENTIFICATION__REVISION_ID, i2cBuffer[1])) return false; - VL53L0_RF_CALL(read(Register::IDENTIFICATION__REVISION_ID, i2cBuffer[1])); - - RF_END_RETURN(i2cBuffer[1] == RevisionID); + return i2cBuffer[1] == RevisionID; } template < typename I2cMaster > -modm::ResumableResult +bool modm::Vl53l0::setDeviceAddress(uint8_t address) { - RF_BEGIN(); - - if(RF_CALL(write(Register::I2C_SLAVE__DEVICE_ADDRESS, (address & 0x7F)))) + if(write(Register::I2C_SLAVE__DEVICE_ADDRESS, (address & 0x7F))) { this->setAddress(address); - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } template < typename I2cMaster > -modm::ResumableResult +bool modm::Vl53l0::initializeSpadConfig() { - RF_BEGIN(); - // read number and type of SPADs to be used for calibration - VL53L0_RF_CALL(write(Register(0x80), 0x01)); - VL53L0_RF_CALL(write(Register(0xFF), 0x01)); - VL53L0_RF_CALL(write(Register(0x00), 0x00)); - VL53L0_RF_CALL(write(Register(0xFF), 0x06)); - VL53L0_RF_CALL(updateControlRegister(Register(0x83), Control_t(4), Control_t(0))); - VL53L0_RF_CALL(write(Register(0xFF), 0x07)); - VL53L0_RF_CALL(write(Register(0x81), 0x01)); - VL53L0_RF_CALL(write(Register(0x80), 0x01)); - - VL53L0_RF_CALL(write(Register(0x94), 0x6b)); - VL53L0_RF_CALL(write(Register(0x83), 0x00)); - - VL53L0_RF_CALL(poll(Register(0x83), [](uint8_t value) { + if (not write(Register(0x80), 0x01)) return false; + if (not write(Register(0xFF), 0x01)) return false; + if (not write(Register(0x00), 0x00)) return false; + if (not write(Register(0xFF), 0x06)) return false; + if (not updateControlRegister(Register(0x83), Control_t(4), Control_t(0))) return false; + if (not write(Register(0xFF), 0x07)) return false; + if (not write(Register(0x81), 0x01)) return false; + if (not write(Register(0x80), 0x01)) return false; + + if (not write(Register(0x94), 0x6b)) return false; + if (not write(Register(0x83), 0x00)) return false; + + if (not poll(Register(0x83), [](uint8_t value) { return value != 0x00; - })); + })) return false; - VL53L0_RF_CALL(write(Register(0x83), 0x01)); - VL53L0_RF_CALL(read(Register(0x92), i2cBuffer[1])); + if (not write(Register(0x83), 0x01)) return false; + if (not read(Register(0x92), i2cBuffer[1])) return false; spadInfo.referenceSpadCount = i2cBuffer[1] & 0x7F; spadInfo.useApertureSpads = (i2cBuffer[1] & (1 << 7)) != 0; - VL53L0_RF_CALL(write(Register(0x81), 0x00)); - VL53L0_RF_CALL(write(Register(0xFF), 0x06)); - VL53L0_RF_CALL(updateControlRegister(Register(0x83), Control_t(0), Control_t(4))); - VL53L0_RF_CALL(write(Register(0xFF), 0x01)); - VL53L0_RF_CALL(write(Register(0x00), 0x01)); - VL53L0_RF_CALL(write(Register(0xFF), 0x00)); - VL53L0_RF_CALL(write(Register(0x80), 0x00)); + if (not write(Register(0x81), 0x00)) return false; + if (not write(Register(0xFF), 0x06)) return false; + if (not updateControlRegister(Register(0x83), Control_t(0), Control_t(4))) return false; + if (not write(Register(0xFF), 0x01)) return false; + if (not write(Register(0x00), 0x01)) return false; + if (not write(Register(0xFF), 0x00)) return false; + if (not write(Register(0x80), 0x00)) return false; // read map of SPADs available for reference calibration - VL53L0_RF_CALL(read(Register::GLOBAL__CONFIG_SPAD_ENABLES_REF_0, spadInfo.map, 6)); + if (not read(Register::GLOBAL__CONFIG_SPAD_ENABLES_REF_0, spadInfo.map, 6)) return false; // prepare setting SPAD config - VL53L0_RF_CALL(write(Register(0xFF), 0x01)); - VL53L0_RF_CALL(write(Register::DYNAMIC_SPAD__REF_EN_START_OFFSET, 0x00)); - VL53L0_RF_CALL(write(Register::DYNAMIC_SPAD__NUM_REQUESTED_REF_SPAD, 0x2C)); - VL53L0_RF_CALL(write(Register(0xFF), 0x00)); - VL53L0_RF_CALL(write(Register::GLOBAL__CONFIG_REF_EN_START_SELECT, 0xB4)); + if (not write(Register(0xFF), 0x01)) return false; + if (not write(Register::DYNAMIC_SPAD__REF_EN_START_OFFSET, 0x00)) return false; + if (not write(Register::DYNAMIC_SPAD__NUM_REQUESTED_REF_SPAD, 0x2C)) return false; + if (not write(Register(0xFF), 0x00)) return false; + if (not write(Register::GLOBAL__CONFIG_REF_EN_START_SELECT, 0xB4)) return false; if(not setupReferenceSpadMap(spadInfo.map, &i2cBuffer[1])) { @@ -329,37 +304,35 @@ modm::Vl53l0::initializeSpadConfig() MODM_LOG_ERROR << "This procedure is not implemented in this driver.\n"; MODM_LOG_ERROR << "Please use the VL53L0X API provided by ST." << modm::endl; - RF_RETURN(false); + return false; } // Write SPAD config to the device. This will not be written to NVM. - VL53L0_RF_CALL(writeI2CBuffer(Register::GLOBAL__CONFIG_SPAD_ENABLES_REF_0, 6)); + if (not writeI2CBuffer(Register::GLOBAL__CONFIG_SPAD_ENABLES_REF_0, 6)) return false; - RF_END_RETURN(true); + return true; } template < typename I2cMaster > -modm::ResumableResult +bool modm::Vl53l0::performReferenceCalibration(Start_t mode) { static_assert(MaxMeasurementTimeUs / 1000 * 2 <= std::numeric_limits::max(), "MaxMeasurementTimeUs out of range"); - RF_BEGIN(); - // start measurement - VL53L0_RF_CALL(write(Register::SYSRANGE__START, Start::StartStop | mode)); + if (not write(Register::SYSRANGE__START, Start::StartStop | mode)) return false; // wait for the measurement to finish - VL53L0_RF_CALL(poll(Register::RESULT__INTERRUPT_STATUS, [](uint8_t value) { + if (not poll(Register::RESULT__INTERRUPT_STATUS, [](uint8_t value) { return value & (InterruptStatus::NewSampleReady | InterruptStatus::OutOfWindow).value; - }, measurementTimeUs / 1000 * 2)); + }, measurementTimeUs / 1000 * 2)) return false; // clear interrupt flags - VL53L0_RF_CALL(write(Register::SYSTEM__INTERRUPT_CLEAR, InterruptClear::Range)); - VL53L0_RF_CALL(write(Register::SYSRANGE__START, Start::SingleShotMode)); + if (not write(Register::SYSTEM__INTERRUPT_CLEAR, InterruptClear::Range)) return false; + if (not write(Register::SYSRANGE__START, Start::SingleShotMode)) return false; - RF_END_RETURN(true); + return true; } template < typename I2cMaster > @@ -400,14 +373,12 @@ modm::Vl53l0::setupReferenceSpadMap(const uint8_t availableSpadMap[6] } template < typename I2cMaster > -modm::ResumableResult +bool modm::Vl53l0::readSequenceInfo() { - RF_BEGIN(); - // read vcsel periods - VL53L0_RF_CALL(read(Register::PRE_RANGE__CONFIG_VCSEL_PERIOD, sequenceInfo.vcselPeriodPreRange)); - VL53L0_RF_CALL(read(Register::FINAL_RANGE__CONFIG_VCSEL_PERIOD, sequenceInfo.vcselPeriodFinalRange)); + if (not read(Register::PRE_RANGE__CONFIG_VCSEL_PERIOD, sequenceInfo.vcselPeriodPreRange)) return false; + if (not read(Register::FINAL_RANGE__CONFIG_VCSEL_PERIOD, sequenceInfo.vcselPeriodFinalRange)) return false; // decode vcsel period values sequenceInfo.vcselPeriodPreRange += 1; @@ -415,19 +386,19 @@ modm::Vl53l0::readSequenceInfo() sequenceInfo.vcselPeriodFinalRange += 1; sequenceInfo.vcselPeriodFinalRange <<= 1; - VL53L0_RF_CALL(read(Register::SYSTEM__SEQUENCE_CONFIG, i2cBuffer[1])); + if (not read(Register::SYSTEM__SEQUENCE_CONFIG, i2cBuffer[1])) return false; sequenceInfo.enabledSteps = static_cast(i2cBuffer[1]); - VL53L0_RF_CALL(read(Register::MSRC__CONFIG_TIMEOUT_MACROP, sequenceInfo.msrcDssTccTimeout)); + if (not read(Register::MSRC__CONFIG_TIMEOUT_MACROP, sequenceInfo.msrcDssTccTimeout)) return false; sequenceInfo.msrcDssTccTimeout += 1; - VL53L0_RF_CALL(read(Register::PRE_RANGE__CONFIG_TIMEOUT_MACROP_HI, &i2cBuffer[1], 2)); + if (not read(Register::PRE_RANGE__CONFIG_TIMEOUT_MACROP_HI, &i2cBuffer[1], 2)) return false; sequenceInfo.preRangeTimeout = decodeTimeout((i2cBuffer[1] << 8) + i2cBuffer[2]); - VL53L0_RF_CALL(read(Register::FINAL_RANGE__CONFIG_TIMEOUT_MACROP_HI, &i2cBuffer[1], 2)); + if (not read(Register::FINAL_RANGE__CONFIG_TIMEOUT_MACROP_HI, &i2cBuffer[1], 2)) return false; sequenceInfo.finalRangeTimeout = decodeTimeout((i2cBuffer[1] << 8) + i2cBuffer[2]); - RF_END_RETURN(true); + return true; } template < typename I2cMaster > @@ -563,95 +534,76 @@ modm::Vl53l0::decodeTimeout(uint16_t registerValue) // ---------------------------------------------------------------------------- // MARK: update register template < typename I2cMaster > -modm::ResumableResult +bool modm::Vl53l0::updateControlRegister(Register reg, Control_t setMask, Control_t clearMask) { - RF_BEGIN(); - if(clearMask.value != 0xFF) { - if(!RF_CALL(read(reg, i2cBuffer[1]))) { - RF_RETURN(false); + if(!read(reg, i2cBuffer[1])) { + return false; } } i2cBuffer[1] = (i2cBuffer[1] & ~clearMask.value) | setMask.value; - RF_END_RETURN_CALL(write(reg, i2cBuffer[1])); + return write(reg, i2cBuffer[1]); } // MARK: write multilength register template < class I2cMaster > -modm::ResumableResult +bool modm::Vl53l0::write(Register reg, uint8_t value) { - RF_BEGIN(); i2cBuffer[1] = value; - RF_END_RETURN_CALL(writeI2CBuffer(reg, 1)); + return writeI2CBuffer(reg, 1); } // MARK: write multilength register template < class I2cMaster > -modm::ResumableResult +bool modm::Vl53l0::writeI2CBuffer(Register reg, uint8_t dataLength) { - RF_BEGIN(); - if(dataLength >= sizeof(i2cBuffer)) { - RF_RETURN(false); + return false; } i2cBuffer[0] = uint8_t(reg); - this->transaction.configureWrite(i2cBuffer, dataLength + 1); - - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::write(i2cBuffer, dataLength + 1); } template < class I2cMaster > -modm::ResumableResult +bool modm::Vl53l0::writeUInt16(Register reg, uint16_t value) { - RF_BEGIN(); - i2cBuffer[0] = uint8_t(reg); i2cBuffer[1] = (value & 0xFF00) >> 8; i2cBuffer[2] = value & 0xFF; - - this->transaction.configureWrite(i2cBuffer, 3); - - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::write(i2cBuffer, 3); } // MARK: read multilength register template < class I2cMaster > -modm::ResumableResult +bool modm::Vl53l0::read(Register reg, uint8_t *buffer, uint8_t length) { - RF_BEGIN(); - i2cBuffer[0] = uint8_t(reg); - - this->transaction.configureWriteRead(i2cBuffer, 1, buffer, length); - - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::writeRead(i2cBuffer, 1, buffer, length); } template < class I2cMaster > template < typename Predicate > -modm::ResumableResult +bool modm::Vl53l0::poll(Register reg, Predicate pred, const uint16_t timeoutMs, const uint16_t stepMs) { - RF_BEGIN(); - index = timeoutMs; while(true) { - if(not RF_CALL(read(reg, i2cBuffer[1]))) { + if(not read(reg, i2cBuffer[1])) { break; } if(pred(i2cBuffer[1])) { - RF_RETURN(true); + return true; } if(index == 0) { @@ -659,8 +611,7 @@ modm::Vl53l0::poll(Register reg, Predicate pred, const uint16_t timeo break; } - timeout.restart(std::chrono::milliseconds(stepMs)); - RF_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(std::chrono::milliseconds(stepMs)); if(index >= stepMs) { index -= stepMs; @@ -669,5 +620,5 @@ modm::Vl53l0::poll(Register reg, Predicate pred, const uint16_t timeo } } - RF_END_RETURN(false); + return false; } diff --git a/src/modm/driver/position/vl6180.hpp b/src/modm/driver/position/vl6180.hpp index d3b6dfa2aa..170a5e4df5 100644 --- a/src/modm/driver/position/vl6180.hpp +++ b/src/modm/driver/position/vl6180.hpp @@ -18,7 +18,6 @@ #include #include #include -#include namespace modm { @@ -353,7 +352,7 @@ struct vl6180 * @ingroup modm_driver_vl6180 */ template < typename I2cMaster > -class Vl6180 : public vl6180, public modm::I2cDevice< I2cMaster, 2 > +class Vl6180 : public vl6180, public modm::I2cDevice< I2cMaster > { public: /// Constructor, requires an vl6180::Data object, sets address to default of 0x29 @@ -361,34 +360,34 @@ class Vl6180 : public vl6180, public modm::I2cDevice< I2cMaster, 2 > /// Ping the I2C device. /// This overwrites the build-in ping method of I2cDevice with a read of the model number. - modm::ResumableResult + bool ping(); - modm::ResumableResult + bool initialize(); /// Set a new I2C address (< 128) for this device. /// The address is not permanent and must be set again after every device boot. - modm::ResumableResult + bool setDeviceAddress(uint8_t address); /// Sets a new analog gain for ALS. - modm::ResumableResult + bool setGain(AnalogGain gain); /// Sets a new integration time for ALS. /// @param time integration time in ms, max ~500ms. - modm::ResumableResult + bool setIntegrationTime(uint16_t time); /// Reads the distance and buffer the results (can take 10-55ms). - modm::ResumableResult + bool readDistance() { return readSensor(true); } /// Reads the ambient light and buffer the results. /// This takes as long as the chosen integration time (100ms default). - modm::ResumableResult + bool readAmbientLight() { return readSensor(false); } @@ -402,7 +401,7 @@ class Vl6180 : public vl6180, public modm::I2cDevice< I2cMaster, 2 > template - modm::ResumableResult + bool updateRegister(Register reg, T setMask, T clearMask = T(0xff)) { return updateControlRegister(reg, Control_t(setMask), Control_t(clearMask)); @@ -410,14 +409,14 @@ class Vl6180 : public vl6180, public modm::I2cDevice< I2cMaster, 2 > public: /// write a 8bit value a register - modm::ResumableResult + bool write(Register reg, uint8_t value) { return write(reg, value, 1); } protected: /// @cond /// read a 8bit value from a register - modm::ResumableResult + bool read(Register reg, uint8_t &value) { return read(reg, &value, 1); } /// @endcond @@ -431,29 +430,28 @@ class Vl6180 : public vl6180, public modm::I2cDevice< I2cMaster, 2 > protected: /// @cond /// write multiple 8bit values from a start register - modm::ResumableResult + bool write(Register reg, uint8_t value, uint8_t length); /// @endcond public: /// read multiple 8bit values from a start register - modm::ResumableResult + bool read(Register reg, uint8_t *buffer, uint8_t length); protected: /// @cond - modm::ResumableResult + bool updateControlRegister(Register reg, Control_t setMask, Control_t clearMask = Control_t(0xff)); /// @endcond private: - modm::ResumableResult + bool readSensor(bool isDistance = true); private: Data &data; - modm::ShortTimeout timeout; RangeErrorCode rangeError; ALS_ErrorCode alsError; diff --git a/src/modm/driver/position/vl6180.lb b/src/modm/driver/position/vl6180.lb index 39356d144a..9694156be5 100644 --- a/src/modm/driver/position/vl6180.lb +++ b/src/modm/driver/position/vl6180.lb @@ -21,8 +21,7 @@ def prepare(module, options): ":architecture:i2c.device", ":architecture:register", ":architecture:unaligned", - ":math:utils", - ":processing:timer") + ":math:utils") return True def build(env): diff --git a/src/modm/driver/position/vl6180_impl.hpp b/src/modm/driver/position/vl6180_impl.hpp index 7d91c973ff..478f670eec 100644 --- a/src/modm/driver/position/vl6180_impl.hpp +++ b/src/modm/driver/position/vl6180_impl.hpp @@ -16,7 +16,7 @@ // ---------------------------------------------------------------------------- template < typename I2cMaster > modm::Vl6180::Vl6180(Data &data, uint8_t address) -: I2cDevice(address), data(data), +: I2cDevice(address), data(data), i2cBuffer{0,0,0,0}, logicBuffer{Register(0)} { } @@ -24,25 +24,21 @@ modm::Vl6180::Vl6180(Data &data, uint8_t address) // MARK: - i2cTasks // MARK: ping template < class I2cMaster > -modm::ResumableResult +bool modm::Vl6180::ping() { - RF_BEGIN(); + if (not read(Register::IDENTIFICATION_MODEL_ID, i2cBuffer[2])) + return false; - if (not RF_CALL(read(Register::IDENTIFICATION_MODEL_ID, i2cBuffer[2]))) - RF_RETURN(false); - - RF_END_RETURN(i2cBuffer[2] == 0xB4); + return i2cBuffer[2] == 0xB4; } template < typename I2cMaster > -modm::ResumableResult +bool modm::Vl6180::initialize() { using namespace vl6180_private; - RF_BEGIN(); - // logicBuffer[0] => success of config logicBuffer.byte[0] = true; // logicBuffer[1] => index of array @@ -52,32 +48,30 @@ modm::Vl6180::initialize() for (; logicBuffer.byte[1] < 40; logicBuffer.byte[1]++) { i2cBuffer[2] = configuration[logicBuffer.byte[1]].value; - logicBuffer.byte[0] &= RF_CALL( write(Register(configuration[logicBuffer.byte[1]].reg), i2cBuffer[2]) ); + logicBuffer.byte[0] &= write(Register(configuration[logicBuffer.byte[1]].reg), i2cBuffer[2]); // prematurely abort if something failed - if (!logicBuffer.byte[0]) RF_RETURN(false); + if (!logicBuffer.byte[0]) return false; } - RF_END_RETURN((bool)logicBuffer.byte[0]); + return (bool)logicBuffer.byte[0]; } template < typename I2cMaster > -modm::ResumableResult +bool modm::Vl6180::readSensor(bool isDistance) { using namespace std::chrono; // for this relatively complicated sequence, see the datasheet page 17 - RF_BEGIN(); // Write 0x01 to the SYSRANGE_START or SYSALS_START register. // The start bit auto-clears after completion. logicBuffer.reg = isDistance ? Register::SYSRANGE_START : Register::SYSALS_START; - if ( RF_CALL(write(logicBuffer.reg, uint8_t(Start::StartStop))) ) + if ( write(logicBuffer.reg, uint8_t(Start::StartStop)) ) { // Measurement will take 7.5ms + convergence time (< ~10ms) for ranging // or the analog integration time for ALS - timeout.restart(milliseconds(isDistance ? 10 : data.time)); - RF_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(milliseconds(isDistance ? 10 : data.time)); // When the measurement is completed, the interrupt source of ALS or range // in RESULT_INTERRUPT_STATUS_GPIO will set to New Sample Ready. @@ -85,8 +79,8 @@ modm::Vl6180::readSensor(bool isDistance) while(true) { // read the byte - if ( !RF_CALL(read(Register::RESULT_INTERRUPT_STATUS_GPIO, i2cBuffer[2])) ) - RF_RETURN(false); + if ( !read(Register::RESULT_INTERRUPT_STATUS_GPIO, i2cBuffer[2]) ) + return false; // break if the correct interrupt source is set { @@ -99,19 +93,18 @@ modm::Vl6180::readSensor(bool isDistance) } // otherwise wait 2ms longer on every try - timeout.restart(milliseconds(logicBuffer.byte[0])); logicBuffer.byte[0] += 2; // 168ms timeout if (logicBuffer.byte[0] > 25) - RF_RETURN(false); + return false; - RF_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(milliseconds(logicBuffer.byte[0])); } // The range result is read from RESULT_RANGE_VAL or RESULT_ALS_VAL. logicBuffer.reg = isDistance ? Register::RESULT_RANGE_VAL : Register::RESULT_ALS_VAL; - if ( RF_CALL(read(logicBuffer.reg, i2cBuffer+2, isDistance ? 1 : 2)) ) + if ( read(logicBuffer.reg, i2cBuffer+2, isDistance ? 1 : 2) ) { if (isDistance) data.data[0] = i2cBuffer[2]; @@ -121,7 +114,7 @@ modm::Vl6180::readSensor(bool isDistance) } // Interrupt status flags are cleared by writing a 0x07 to SYSTEM_INTERRUPT_CLEAR. - if ( RF_CALL(write(Register::SYSTEM_INTERRUPT_CLEAR, (InterruptClear::Range | InterruptClear::ALS | InterruptClear::Error).value)) ) + if ( write(Register::SYSTEM_INTERRUPT_CLEAR, (InterruptClear::Range | InterruptClear::ALS | InterruptClear::Error).value) ) { // Bit 0 of RESULT_RANGE_STATUS or RESULT_ALS_STATUS indicates when either sensor is ready for the next operation. logicBuffer.reg = isDistance ? Register::RESULT_RANGE_STATUS : Register::RESULT_ALS_STATUS; @@ -129,8 +122,8 @@ modm::Vl6180::readSensor(bool isDistance) while(true) { // read the byte - if ( !RF_CALL(read(logicBuffer.reg, i2cBuffer[2])) ) - RF_RETURN(false); + if ( !read(logicBuffer.reg, i2cBuffer[2]) ) + return false; // Error codes are indicated in bits [7:4] of the status registers if ( i2cBuffer[2] & uint8_t(RangeStatus::DeviceReady) ) @@ -141,118 +134,99 @@ modm::Vl6180::readSensor(bool isDistance) } // otherwise wait 4ms and try again - timeout.restart(4ms); - RF_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(4ms); if (i2cBuffer[3]++ > 15) - RF_RETURN(false); + return false; } // the sequence was executed successfully - RF_RETURN(true); + return true; } } } - RF_END_RETURN(false); + return false; } template < typename I2cMaster > -modm::ResumableResult +bool modm::Vl6180::setDeviceAddress(uint8_t address) { - RF_BEGIN(); - - if ( RF_CALL(write(Register::I2C_SLAVE_DEVICE_ADDRESS, (address & 0x7f))) ) + if ( write(Register::I2C_SLAVE_DEVICE_ADDRESS, (address & 0x7f)) ) { this->setAddress(address); - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } template < typename I2cMaster > -modm::ResumableResult +bool modm::Vl6180::setGain(AnalogGain gain) { - RF_BEGIN(); - - if ( RF_CALL(write(Register::SYSALS_ANALOGUE_GAIN, uint8_t(gain))) ) + if ( write(Register::SYSALS_ANALOGUE_GAIN, uint8_t(gain)) ) { data.gain = uint8_t(gain); - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } template < typename I2cMaster > -modm::ResumableResult +bool modm::Vl6180::setIntegrationTime(uint16_t time) { - RF_BEGIN(); - // 0 is 1ms, we need to substract 1 UNLESS the time is 0! if (time > 0) time -= 1; if (time > 0x1ff) time = 0x1ff; i2cBuffer[2] = time >> 8; i2cBuffer[3] = time; - if ( RF_CALL(write(Register::SYSALS_INTEGRATION_PERIOD, i2cBuffer[2], 2)) ) + if ( write(Register::SYSALS_INTEGRATION_PERIOD, i2cBuffer[2], 2) ) { // data.time must never be zero! data.time = time + 1; - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } // ---------------------------------------------------------------------------- // MARK: update register template < typename I2cMaster > -modm::ResumableResult +bool modm::Vl6180::updateControlRegister(Register reg, Control_t setMask, Control_t clearMask) { - RF_BEGIN(); - if (clearMask.value != 0xff) - if (!RF_CALL(read(reg, i2cBuffer[2]))) - RF_RETURN(false); + if (!read(reg, i2cBuffer[2])) + return false; i2cBuffer[2] = (i2cBuffer[2] & ~clearMask.value) | setMask.value; - RF_END_RETURN_CALL(write(reg, i2cBuffer[2])); + return write(reg, i2cBuffer[2]); } // MARK: write multilength register template < class I2cMaster > -modm::ResumableResult +bool modm::Vl6180::write(Register reg, uint8_t value, uint8_t length) { - RF_BEGIN(); - i2cBuffer[0] = uint16_t(reg) >> 8; i2cBuffer[1] = uint8_t(reg); i2cBuffer[2] = value; - - this->transaction.configureWrite(i2cBuffer, length+2); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(i2cBuffer, length+2); } // MARK: read multilength register template < class I2cMaster > -modm::ResumableResult +bool modm::Vl6180::read(Register reg, uint8_t *buffer, uint8_t length) { - RF_BEGIN(); - i2cBuffer[0] = uint16_t(reg) >> 8; i2cBuffer[1] = uint8_t(reg); - - this->transaction.configureWriteRead(i2cBuffer, 2, buffer, length); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::writeRead(i2cBuffer, 2, buffer, length); } diff --git a/src/modm/driver/pressure/ams5915.hpp b/src/modm/driver/pressure/ams5915.hpp index b9e6bf88ce..57ff6998b9 100644 --- a/src/modm/driver/pressure/ams5915.hpp +++ b/src/modm/driver/pressure/ams5915.hpp @@ -96,38 +96,21 @@ struct ams5915 * @author Raphael Lehman, Niklas Hauser */ template < typename I2cMaster > -class Ams5915 : public ams5915, public modm::I2cDevice +class Ams5915 : public ams5915, public modm::I2cDevice { public: /** * @param data a ams5915::Data object */ Ams5915(Data &data, uint8_t i2cAddress = 0x28) - : I2cDevice(i2cAddress), data(data) - { - this->transaction.configureRead(data.data, 4); - } - - /// pings the sensor - modm::ResumableResult - ping() - { - RF_BEGIN(); - - RF_WAIT_UNTIL(this->transaction.configurePing() and this->startTransaction()); - - RF_WAIT_WHILE( this->isTransactionRunning() ); - - this->transaction.configureRead(data.data, 4); - - RF_END_RETURN( this->wasTransactionSuccessful() ); - } + : I2cDevice(i2cAddress), data(data) + {} /// reads the Pressure registers and buffers the results - inline modm::ResumableResult + inline bool readPressure() { - return this->runTransaction(); + return I2cDevice::read(data.data, 4); } public: diff --git a/src/modm/driver/pressure/bme280.hpp b/src/modm/driver/pressure/bme280.hpp index 701ab75fac..448f509be4 100644 --- a/src/modm/driver/pressure/bme280.hpp +++ b/src/modm/driver/pressure/bme280.hpp @@ -198,7 +198,7 @@ using DataDouble = modm::bme280data::DataDouble; * @tparam I2cMaster I2C interface */ template < typename I2cMaster > -class Bme280 : public bme280, public modm::I2cDevice +class Bme280 : public bme280, public modm::I2cDevice { public: /** @@ -209,7 +209,7 @@ class Bme280 : public bme280, public modm::I2cDevice // MARK: - TASKS /// Reads out and stores the calibration bytes - modm::ResumableResult + bool initialize( Mode mode = Mode::Normal, Oversampling temperature = Oversampling::Single, @@ -217,13 +217,13 @@ class Bme280 : public bme280, public modm::I2cDevice Oversampling humidity = Oversampling::Single); /// Read the raw data from the sensor. Conversion must be freerunning. - modm::ResumableResult + bool readout(); /// Start a single measurement in forced mode. Sensor will go to standby after this. /// To reduce bus traffic only the absolute necessary register (CTRL_MEAS) is written. /// If oversampling of humidity shall be changed, another method must be used. - modm::ResumableResult + bool startMeasurement( Oversampling temperature = Oversampling::Single, Oversampling pressure = Oversampling::Single); diff --git a/src/modm/driver/pressure/bme280_impl.hpp b/src/modm/driver/pressure/bme280_impl.hpp index 522f2c634f..5b10ee7afe 100644 --- a/src/modm/driver/pressure/bme280_impl.hpp +++ b/src/modm/driver/pressure/bme280_impl.hpp @@ -23,34 +23,29 @@ // ---------------------------------------------------------------------------- template < typename I2cMaster > modm::Bme280::Bme280(Data &data, uint8_t address) : - I2cDevice(address), data(data) + I2cDevice(address), data(data) { } // ---------------------------------------------------------------------------- // MARK: - Tasks template < typename I2cMaster > -modm::ResumableResult +bool modm::Bme280::initialize(Mode mode, Oversampling pressure, Oversampling temperature, Oversampling humidity) { - RF_BEGIN(); - // Very first verify Chip Id { buffer[0] = i(Register::CHIP_ID); uint8_t chid; - - this->transaction.configureWriteRead(buffer, 1, &chid, 1); - - if (RF_CALL( this->runTransaction() )) + if (I2cDevice::writeRead(buffer, 1, &chid, 1)) { MODM_LOG_DEBUG.printf("BME280 Chip Id check. Read %02x, expected %02x\n", chid, ChipId); if (chid != ChipId) { MODM_LOG_ERROR.printf("BME280 Chip Id mismatch. Read %02x, expected %02x\n", chid, ChipId); - RF_RETURN(false); + return false; } } else { - RF_RETURN(false); + return false; } } @@ -62,9 +57,8 @@ modm::Bme280::initialize(Mode mode, Oversampling pressure, Oversampli buffer[0] = i(Register::CTRL_HUM); buffer[1] = ctrl_hum.value; } - this->transaction.configureWrite(buffer, 2); - if (not RF_CALL( this->runTransaction() )) { - RF_RETURN(false); + if (not I2cDevice::write(buffer, 2)) { + return false; } @@ -77,10 +71,8 @@ modm::Bme280::initialize(Mode mode, Oversampling pressure, Oversampli buffer[0] = i(Register::CTRL_MEAS); buffer[1] = ctrl_meas.value; } - - this->transaction.configureWrite(buffer, 2); - if (not RF_CALL( this->runTransaction() )) { - RF_RETURN(false); + if (not I2cDevice::write(buffer, 2)) { + return false; } @@ -91,18 +83,14 @@ modm::Bme280::initialize(Mode mode, Oversampling pressure, Oversampli buffer[0] = i(Register::CONFIG); buffer[1] = config.value; } - this->transaction.configureWrite(buffer, 2); - if (not RF_CALL( this->runTransaction() )) { - RF_RETURN(false); + if (not I2cDevice::write(buffer, 2)) { + return false; } // Read 26 bytes of Calib00 to Calib25 buffer[0] = i(Register::CALIB00); - - this->transaction.configureWriteRead(buffer, 1, reinterpret_cast(&data.calibration), 26); - - if (RF_CALL( this->runTransaction() )) + if (I2cDevice::writeRead(buffer, 1, reinterpret_cast(&data.calibration), 26)) { { MODM_LOG_DEBUG << "Raw calibration data: "; @@ -130,7 +118,7 @@ modm::Bme280::initialize(Mode mode, Oversampling pressure, Oversampli element[11] = modm::fromLittleEndian(element[11]); element[12] = modm::fromLittleEndian(element[12]); } else { - RF_RETURN(false); + return false; } // Read 7 bytes of Calib26 to Calib32 @@ -142,10 +130,7 @@ modm::Bme280::initialize(Mode mode, Oversampling pressure, Oversampli // H2=359, H3=0, H4=326, H5=0, H6=30 (decimal) buffer[0] = i(Register::CALIB26); - - this->transaction.configureWriteRead(buffer, 1, calBuffer, 7); - - if (RF_CALL( this->runTransaction() )) + if (I2cDevice::writeRead(buffer, 1, calBuffer, 7)) { data.calibration.H2 = (calBuffer[1] << 8) | calBuffer[0]; data.calibration.H3 = calBuffer[2]; @@ -153,44 +138,41 @@ modm::Bme280::initialize(Mode mode, Oversampling pressure, Oversampli data.calibration.H5 = (uint16_t(calBuffer[5]) << 4) | (calBuffer[4] >> 4); data.calibration.H6 = calBuffer[6]; - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } template < typename I2cMaster > -modm::ResumableResult +bool modm::Bme280::readout() { - RF_BEGIN(); - // Get the raw data from sensor // It is advised by the datasheet to readout the complete sensor // data at once to avoid mixing old and new data. // And a single 8-byte transaction is even faster than multiple 3-byte // transactions. buffer[0] = i(Register::PRESS_MSB); - this->transaction.configureWriteRead(buffer, 1, data.raw, 8); - - MODM_LOG_DEBUG.printf("RAW: %02x %02x %02x %02x %02x %02x %02x %02x\n", - data.raw[0], data.raw[1], data.raw[2], data.raw[3], - data.raw[4], data.raw[5], data.raw[6], data.raw[7]); - - // There will come new data, so new calculation necessary - data.rawTemperatureTouched(); - data.rawPressureTouched(); - data.rawHumidityTouched(); - - RF_END_RETURN_CALL( this->runTransaction() ); + if (I2cDevice::writeRead(buffer, 1, data.raw, 8)) + { + MODM_LOG_DEBUG.printf("RAW: %02x %02x %02x %02x %02x %02x %02x %02x\n", + data.raw[0], data.raw[1], data.raw[2], data.raw[3], + data.raw[4], data.raw[5], data.raw[6], data.raw[7]); + + // There will come new data, so new calculation necessary + data.rawTemperatureTouched(); + data.rawPressureTouched(); + data.rawHumidityTouched(); + return true; + } + return false; } template < typename I2cMaster > -modm::ResumableResult +bool modm::Bme280::startMeasurement(Oversampling pressure, Oversampling temperature) { - RF_BEGIN(); - { Mode mode = Mode::Forced; @@ -201,7 +183,5 @@ modm::Bme280::startMeasurement(Oversampling pressure, Oversampling te buffer[0] = i(Register::CTRL_MEAS); buffer[1] = ctrl_meas.value; } - - this->transaction.configureWrite(buffer, 2); - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(buffer, 2); } diff --git a/src/modm/driver/pressure/bmp085.hpp b/src/modm/driver/pressure/bmp085.hpp index 407fbe8937..2060d4821d 100644 --- a/src/modm/driver/pressure/bmp085.hpp +++ b/src/modm/driver/pressure/bmp085.hpp @@ -14,7 +14,6 @@ #define MODM_BMP085_HPP #include -#include #include "bmp085_data.hpp" @@ -102,7 +101,7 @@ using DataDouble = modm::bmp085data::DataDouble; * @tparam I2cMaster I2C interface */ template < typename I2cMaster > -class Bmp085 : public bmp085, public modm::I2cDevice +class Bmp085 : public bmp085, public modm::I2cDevice { public: /** @@ -113,11 +112,11 @@ class Bmp085 : public bmp085, public modm::I2cDevice // MARK: - TASKS /// Reads out and stores the calibration bytes - modm::ResumableResult + bool initialize(Mode mode = Mode::Standard); /// Do a readout sequence to convert and read temperature and then pressure from sensor - modm::ResumableResult + bool readout(); /// Configures the sensor @@ -136,7 +135,6 @@ class Bmp085 : public bmp085, public modm::I2cDevice private: DataBase &data; - modm::ShortTimeout timeout; /** * Maximum conversion time for pressure from datasheet for diff --git a/src/modm/driver/pressure/bmp085.lb b/src/modm/driver/pressure/bmp085.lb index 4e4dffc968..6b1242e809 100644 --- a/src/modm/driver/pressure/bmp085.lb +++ b/src/modm/driver/pressure/bmp085.lb @@ -32,8 +32,7 @@ def prepare(module, options): module.depends( ":architecture:i2c.device", ":math:utils", - ":debug", - ":processing:timer") + ":debug") return True def build(env): diff --git a/src/modm/driver/pressure/bmp085_data.hpp b/src/modm/driver/pressure/bmp085_data.hpp index 8c075d9e55..6c49179fc5 100644 --- a/src/modm/driver/pressure/bmp085_data.hpp +++ b/src/modm/driver/pressure/bmp085_data.hpp @@ -14,6 +14,7 @@ #include #include +#include #ifndef MODM_BMP085_DATA_HPP #define MODM_BMP085_DATA_HPP diff --git a/src/modm/driver/pressure/bmp085_impl.hpp b/src/modm/driver/pressure/bmp085_impl.hpp index 0cbf40992c..dd8b44dda0 100644 --- a/src/modm/driver/pressure/bmp085_impl.hpp +++ b/src/modm/driver/pressure/bmp085_impl.hpp @@ -20,24 +20,20 @@ // ---------------------------------------------------------------------------- template < typename I2cMaster > modm::Bmp085::Bmp085(DataBase &data, uint8_t address) : - I2cDevice(address), data(data) + I2cDevice(address), data(data) { } // ---------------------------------------------------------------------------- // MARK: - Tasks template < typename I2cMaster > -modm::ResumableResult +bool modm::Bmp085::initialize(Mode mode) { - RF_BEGIN(); - setMode(mode); buffer[0] = i(Register::CAL_AC1); - this->transaction.configureWriteRead(buffer, 1, reinterpret_cast(&data.calibration), 22); - - if (RF_CALL( this->runTransaction() )) + if (I2cDevice::writeRead(buffer, 1, reinterpret_cast(&data.calibration), 22)) { uint16_t* element = reinterpret_cast(&data.calibration); element[ 0] = modm::fromBigEndian(element[0]); @@ -54,64 +50,53 @@ modm::Bmp085::initialize(Mode mode) element[ 9] = modm::fromBigEndian(element[9]); element[10] = modm::fromBigEndian(element[10]); - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } template < typename I2cMaster > -modm::ResumableResult +bool modm::Bmp085::readout() { - RF_BEGIN(); - // Start temperature reading buffer[0] = i(Register::CONTROL); buffer[1] = i(Conversion::Temperature); - - this->transaction.configureWrite(buffer, 2); - - if (not RF_CALL( this->runTransaction() )) - RF_RETURN(false); + if (not I2cDevice::write(buffer, 2)) + return false; // Wait until temperature reading is succeeded - timeout.restart(5ms); - RF_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(5ms); // Get the temperature from sensor buffer[0] = i(Register::MSB); - this->transaction.configureWriteRead(buffer, 1, data.raw, 2); + if (not I2cDevice::writeRead(buffer, 1, data.raw, 2)) + return false; // Notify data class about changed buffer. data.rawTemperatureTouched(); - if (not RF_CALL( this->runTransaction() )) - RF_RETURN(false); - // buffer the mode for the timer later bufferedMode = data.meta & i(Mode::Mask); // Now start converting the pressure buffer[0] = i(Register::CONTROL); buffer[1] = i(Conversion::Pressure) | bufferedMode; - - this->transaction.configureWrite(buffer, 2); - - if (not RF_CALL( this->runTransaction() )) - RF_RETURN(false); + if (not I2cDevice::write(buffer, 2)) + return false; // Wait until sensor has converted the pressure - timeout.restart(std::chrono::milliseconds(conversionDelay[bufferedMode >> 6])); - RF_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(std::chrono::milliseconds(conversionDelay[bufferedMode >> 6])); // Get the pressure from sensor buffer[0] = i(Register::MSB); - this->transaction.configureWriteRead(buffer, 1, data.raw + 2, 3); + if (not I2cDevice::writeRead(buffer, 1, data.raw + 2, 3)) + return false; // Notify data class about changed buffer. data.rawPressureTouched(); - RF_END_RETURN_CALL( this->runTransaction() ); + return true; } template < typename I2cMaster > diff --git a/src/modm/driver/pressure/hclax.hpp b/src/modm/driver/pressure/hclax.hpp index 76f36d89c9..6e4c7ae91f 100644 --- a/src/modm/driver/pressure/hclax.hpp +++ b/src/modm/driver/pressure/hclax.hpp @@ -64,7 +64,7 @@ struct hclax * @author Niklas Hauser */ template < typename I2cMaster > -class HclaX : public hclax, public modm::I2cDevice +class HclaX : public hclax, public modm::I2cDevice { public: /** @@ -74,33 +74,14 @@ class HclaX : public hclax, public modm::I2cDevice(0x78), data(data) - { - this->transaction.configureRead(data.data, 2); - } - - /// pings the sensor - modm::ResumableResult - ping() - { - RF_BEGIN(); - - RF_WAIT_UNTIL(this->transaction.configurePing() and this->startTransaction()); - - RF_WAIT_WHILE( this->isTransactionRunning() ); - - this->transaction.configureRead(data.data, 2); - - RF_END_RETURN( this->wasTransactionSuccessful() ); - } + : I2cDevice(0x78), data(data) + {} /// reads the Pressure registers and buffers the results - modm::ResumableResult + bool readPressure() { - RF_BEGIN(); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::read(data.data, 2); } public: diff --git a/src/modm/driver/pressure/ms5611.hpp b/src/modm/driver/pressure/ms5611.hpp index 5fd61a36bf..d3b568a0ae 100644 --- a/src/modm/driver/pressure/ms5611.hpp +++ b/src/modm/driver/pressure/ms5611.hpp @@ -16,8 +16,7 @@ #include "ms5611_data.hpp" -#include -#include +#include #include namespace modm @@ -80,7 +79,7 @@ using DataBase = modm::ms5611data::DataBase; * @ingroup modm_driver_ms5611 */ template -class Ms5611 : public ms5611, public modm::SpiDevice, protected modm::NestedResumable<2> +class Ms5611 : public ms5611, public modm::SpiDevice { public: /** @@ -90,11 +89,11 @@ class Ms5611 : public ms5611, public modm::SpiDevice, protected modm: /// Call this function before using the device to read the factory calibration /// @warning calls to this function resets the device - modm::ResumableResult + bool initialize(); /// Do a readout sequence to convert and read temperature and then pressure from sensor - modm::ResumableResult + void readout(OversamplingRatio osrPressure = OversamplingRatio::Osr256, OversamplingRatio osrTemperature = OversamplingRatio::Osr256); @@ -105,12 +104,11 @@ class Ms5611 : public ms5611, public modm::SpiDevice, protected modm: private: /// Read the PROM register at the address - modm::ResumableResult + uint16_t readProm(uint8_t addr); private: DataBase &data; - modm::ShortTimeout timeout; /** * Conversion time of the Analog Digital Convert for different oversampling ratios @@ -126,4 +124,4 @@ class Ms5611 : public ms5611, public modm::SpiDevice, protected modm: #include "ms5611_impl.hpp" -#endif // MODM_MS5611_HPP \ No newline at end of file +#endif // MODM_MS5611_HPP diff --git a/src/modm/driver/pressure/ms5611.lb b/src/modm/driver/pressure/ms5611.lb index 0ff00f9fa6..0d46b3a57a 100644 --- a/src/modm/driver/pressure/ms5611.lb +++ b/src/modm/driver/pressure/ms5611.lb @@ -22,8 +22,7 @@ def prepare(module, options): ":architecture:gpio", ":architecture:spi.device", ":math:utils", - ":processing:resumable", - ":processing:timer") + ":architecture:fiber") return True def build(env): @@ -31,4 +30,4 @@ def build(env): env.copy("ms5611.hpp") env.copy("ms5611_impl.hpp") env.copy("ms5611_data.hpp") - env.copy("ms5611_data_impl.hpp") \ No newline at end of file + env.copy("ms5611_data_impl.hpp") diff --git a/src/modm/driver/pressure/ms5611_impl.hpp b/src/modm/driver/pressure/ms5611_impl.hpp index a7e3412be7..54388246d7 100644 --- a/src/modm/driver/pressure/ms5611_impl.hpp +++ b/src/modm/driver/pressure/ms5611_impl.hpp @@ -28,11 +28,9 @@ Ms5611::Ms5611(DataBase &data) : data(data) // ----------------------------------------------------------------------------- template -modm::ResumableResult +bool Ms5611::initialize() { - RF_BEGIN(); - // Command sequence must be clocked out with MSB first this->attachConfigurationHandler([]() { SpiMaster::setDataMode(SpiMaster::DataMode::Mode0); @@ -42,11 +40,11 @@ Ms5611::initialize() Cs::setOutput(modm::Gpio::High); // Reset sensor once after power-on to load calibration PROM into the internal registers - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); buffer[0] = i(Command::Reset); - RF_CALL(SpiMaster::transfer(buffer, nullptr, 1)); + SpiMaster::transfer(buffer, nullptr, 1); if(this->releaseMaster()) { @@ -54,37 +52,34 @@ Ms5611::initialize() } // 2.8 ms reload - timeout.restart(std::chrono::milliseconds(3)); - RF_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(3ms); // Read the factory calibration from PROM - data.prom.data[0] = modm::fromBigEndian(RF_CALL(readProm(0))); - data.prom.data[1] = modm::fromBigEndian(RF_CALL(readProm(1))); - data.prom.data[2] = modm::fromBigEndian(RF_CALL(readProm(2))); - data.prom.data[3] = modm::fromBigEndian(RF_CALL(readProm(3))); - data.prom.data[4] = modm::fromBigEndian(RF_CALL(readProm(4))); - data.prom.data[5] = modm::fromBigEndian(RF_CALL(readProm(5))); - data.prom.data[6] = modm::fromBigEndian(RF_CALL(readProm(6))); - data.prom.data[7] = modm::fromBigEndian(RF_CALL(readProm(7))); - - RF_END_RETURN(data.prom.calculateCrc() == (data.prom.data[7] & 0x000F)); + data.prom.data[0] = modm::fromBigEndian(readProm(0)); + data.prom.data[1] = modm::fromBigEndian(readProm(1)); + data.prom.data[2] = modm::fromBigEndian(readProm(2)); + data.prom.data[3] = modm::fromBigEndian(readProm(3)); + data.prom.data[4] = modm::fromBigEndian(readProm(4)); + data.prom.data[5] = modm::fromBigEndian(readProm(5)); + data.prom.data[6] = modm::fromBigEndian(readProm(6)); + data.prom.data[7] = modm::fromBigEndian(readProm(7)); + + return data.prom.calculateCrc() == (data.prom.data[7] & 0x000F); } // ----------------------------------------------------------------------------- template -modm::ResumableResult +void Ms5611::readout(OversamplingRatio osrPressure, OversamplingRatio osrTemperature) { - RF_BEGIN(); - // start a pressure conversion buffer[0] = i(Command::Convert) | i(Conversion::Pressure) | i(osrPressure); - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); - RF_CALL(SpiMaster::transfer(buffer, nullptr, 1)); + SpiMaster::transfer(buffer, nullptr, 1); if (this->releaseMaster()) { @@ -92,17 +87,16 @@ Ms5611::readout(OversamplingRatio osrPressure, OversamplingRatio o } // Wait until pressure conversion has finished - timeout.restart(std::chrono::milliseconds(conversionDelay[i(osrPressure) >> 1])); - RF_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(std::chrono::milliseconds(conversionDelay[i(osrPressure) >> 1])); // Get the pressure conversion result from sensor buffer[0] = i(Command::AdcRead); - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); - RF_CALL(SpiMaster::transfer(buffer, nullptr, 1)); - RF_CALL(SpiMaster::transfer(nullptr, data.raw, 3)); + SpiMaster::transfer(buffer, nullptr, 1); + SpiMaster::transfer(nullptr, data.raw, 3); if (this->releaseMaster()) { @@ -115,10 +109,10 @@ Ms5611::readout(OversamplingRatio osrPressure, OversamplingRatio o // start a temperature conversion buffer[0] = i(Command::Convert) | i(Conversion::Temperature) | i(osrTemperature); - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); - RF_CALL(SpiMaster::transfer(buffer, nullptr, 1)); + SpiMaster::transfer(buffer, nullptr, 1); if (this->releaseMaster()) { @@ -126,17 +120,16 @@ Ms5611::readout(OversamplingRatio osrPressure, OversamplingRatio o } // Wait until temperature conversion has finished - timeout.restart(std::chrono::milliseconds(conversionDelay[i(osrTemperature) >> 1])); - RF_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(std::chrono::milliseconds(conversionDelay[i(osrTemperature) >> 1])); // Get the temperature conversion result from sensor buffer[0] = i(Command::AdcRead); - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); - RF_CALL(SpiMaster::transfer(buffer, nullptr, 1)); - RF_CALL(SpiMaster::transfer(nullptr, data.raw + 3, 3)); + SpiMaster::transfer(buffer, nullptr, 1); + SpiMaster::transfer(nullptr, data.raw + 3, 3); if (this->releaseMaster()) { @@ -145,32 +138,28 @@ Ms5611::readout(OversamplingRatio osrPressure, OversamplingRatio o // Notify data class about changed buffer data.rawTemperatureTouched(); - - RF_END(); } // ----------------------------------------------------------------------------- template -modm::ResumableResult +uint16_t Ms5611::readProm(uint8_t address) { - RF_BEGIN(); - buffer[0] = i(Command::PromRead) | ((address & 0b111) << 1); - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); - RF_CALL(SpiMaster::transfer(buffer, nullptr, 1)); - RF_CALL(SpiMaster::transfer(nullptr, buffer, 2)); + SpiMaster::transfer(buffer, nullptr, 1); + SpiMaster::transfer(nullptr, buffer, 2); if (this->releaseMaster()) { Cs::set(); } - RF_END_RETURN(static_cast((buffer[1] << 8) | buffer[0])); + return static_cast((buffer[1] << 8) | buffer[0]); } -} // modm namespace \ No newline at end of file +} // modm namespace diff --git a/src/modm/driver/pressure/ms5837.hpp b/src/modm/driver/pressure/ms5837.hpp index 6be261124d..1ef2dec1d2 100644 --- a/src/modm/driver/pressure/ms5837.hpp +++ b/src/modm/driver/pressure/ms5837.hpp @@ -17,10 +17,9 @@ #include "ms5837_data.hpp" -#include +#include #include #include -#include namespace modm @@ -82,7 +81,7 @@ using DataBase = modm::ms5837data::DataBase; * @ingroup modm_driver_ms5837 */ template < typename I2cMaster > -class Ms5837 : public ms5837, public modm::I2cDevice +class Ms5837 : public ms5837, public modm::I2cDevice { public: /** @@ -92,11 +91,11 @@ class Ms5837 : public ms5837, public modm::I2cDevice /// Call this function before using the device to read the factory calibration /// @warning calls to this function resets the device - modm::ResumableResult + bool initialize(); /// Do a readout sequence to convert and read temperature and then pressure from sensor - modm::ResumableResult + bool readout(OversamplingRatio osrPressure = OversamplingRatio::Osr256, OversamplingRatio osrTemperature = OversamplingRatio::Osr256); @@ -107,12 +106,11 @@ class Ms5837 : public ms5837, public modm::I2cDevice private: /// Read the PROM register at the address - modm::ResumableResult + uint16_t readProm(uint8_t addr); private: DataBase &data; - modm::ShortTimeout timeout; /** * Conversion time of the Analog Digital Convert for different oversampling ratios diff --git a/src/modm/driver/pressure/ms5837.lb b/src/modm/driver/pressure/ms5837.lb index 0cea778e66..f6f1636ba8 100644 --- a/src/modm/driver/pressure/ms5837.lb +++ b/src/modm/driver/pressure/ms5837.lb @@ -24,7 +24,7 @@ def prepare(module, options): ":architecture:i2c.device", ":architecture:register", ":math:utils", - ":processing:resumable", + ":architecture:fiber", ":processing:timer") return True @@ -33,4 +33,4 @@ def build(env): env.copy("ms5837.hpp") env.copy("ms5837_impl.hpp") env.copy("ms5837_data.hpp") - env.copy("ms5837_data_impl.hpp") \ No newline at end of file + env.copy("ms5837_data_impl.hpp") diff --git a/src/modm/driver/pressure/ms5837_impl.hpp b/src/modm/driver/pressure/ms5837_impl.hpp index d3a7e5ff0c..6be513df5a 100644 --- a/src/modm/driver/pressure/ms5837_impl.hpp +++ b/src/modm/driver/pressure/ms5837_impl.hpp @@ -22,69 +22,59 @@ namespace modm { template < typename I2cMaster > -Ms5837::Ms5837(DataBase &data, uint8_t address) : I2cDevice(address), data(data) +Ms5837::Ms5837(DataBase &data, uint8_t address) : I2cDevice(address), data(data) { } // ----------------------------------------------------------------------------- template < typename I2cMaster > -modm::ResumableResult +bool Ms5837::initialize() { - RF_BEGIN(); - // Reset sensor once after power-on to load calibration PROM into the internal registers buffer[0] = i(Command::Reset); - this->transaction.configureWrite(buffer, 1); - if (!RF_CALL( this->runTransaction() )) + if (!I2cDevice::write(buffer, 1)) { - RF_RETURN(false); + return false; } // 4 ms reload (?: from MS5611) - timeout.restart(std::chrono::milliseconds(4)); - RF_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(4us); // Read the factory calibration from PROM - data.prom.data[0] = RF_CALL(readProm(0)); - data.prom.data[1] = RF_CALL(readProm(1)); - data.prom.data[2] = RF_CALL(readProm(2)); - data.prom.data[3] = RF_CALL(readProm(3)); - data.prom.data[4] = RF_CALL(readProm(4)); - data.prom.data[5] = RF_CALL(readProm(5)); - data.prom.data[6] = RF_CALL(readProm(6)); + data.prom.data[0] = readProm(0); + data.prom.data[1] = readProm(1); + data.prom.data[2] = readProm(2); + data.prom.data[3] = readProm(3); + data.prom.data[4] = readProm(4); + data.prom.data[5] = readProm(5); + data.prom.data[6] = readProm(6); factory_crc = (data.prom.data[0] >> 12) & 0xF; - RF_END_RETURN(factory_crc == data.prom.calculateCrc()); + return factory_crc == data.prom.calculateCrc(); } // ----------------------------------------------------------------------------- template < typename I2cMaster > -modm::ResumableResult +bool Ms5837::readout(OversamplingRatio osrPressure, OversamplingRatio osrTemperature) { - RF_BEGIN(); - // start a pressure conversion buffer[0] = i(Command::Convert) | i(Conversion::Pressure) | i(osrPressure); - this->transaction.configureWrite(buffer, 1); - if (!RF_CALL( this->runTransaction() )){ - RF_RETURN(false); + if (!I2cDevice::write(buffer, 1)){ + return false; } // Wait until pressure conversion has finished - timeout.restart(std::chrono::milliseconds(conversionDelay[i(osrPressure) >> 1])); - RF_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(std::chrono::milliseconds(conversionDelay[i(osrPressure) >> 1])); // Get the pressure conversion result from sensor buffer[0] = i(Command::AdcRead); - - this->transaction.configureWriteRead(buffer, 1, data.raw, 3); - if (!RF_CALL( this->runTransaction() )){ - RF_RETURN(false); + if (!I2cDevice::writeRead(buffer, 1, data.raw, 3)){ + return false; } // Notify data class about changed buffer @@ -93,50 +83,43 @@ Ms5837::readout(OversamplingRatio osrPressure, OversamplingRatio osrT // start a temperature conversion buffer[0] = i(Command::Convert) | i(Conversion::Temperature) | i(osrTemperature); - this->transaction.configureWrite(buffer, 1); - if (!RF_CALL( this->runTransaction() )){ - RF_RETURN(false); + if (!I2cDevice::write(buffer, 1)){ + return false; } // Wait until temperature conversion has finished - timeout.restart(std::chrono::milliseconds(conversionDelay[i(osrTemperature) >> 1])); - RF_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(std::chrono::milliseconds(conversionDelay[i(osrTemperature) >> 1])); // Get the temperature conversion result from sensor buffer[0] = i(Command::AdcRead); - - this->transaction.configureWriteRead(buffer, 1, data.raw + 3, 3); - if (!RF_CALL( this->runTransaction() )){ - RF_RETURN(false); + if (!I2cDevice::writeRead(buffer, 1, data.raw + 3, 3)){ + return false; } // Notify data class about changed buffer data.rawTemperatureTouched(); - RF_END_RETURN(true); + return true; } // ----------------------------------------------------------------------------- template < typename I2cMaster > -modm::ResumableResult +uint16_t Ms5837::readProm(uint8_t address) { - RF_BEGIN(); - // MODM_LOG_DEBUG.printf("MS5837 readProm(%02x)\n", address); buffer[0] = i(Command::PromRead) | ((address & 0b111) << 1); - this->transaction.configureWriteRead(buffer, 1, buffer, 2); - if (! RF_CALL( this->runTransaction() )) + if (! I2cDevice::writeRead(buffer, 1, buffer, 2)) { // MODM_LOG_DEBUG.printf("MS5837 readProm(%02x) Failed\n", address); - RF_RETURN(0); + return 0; } // MODM_LOG_DEBUG.printf("MS5837 readProm(%02x) Success: %d (%04x)\n", address, (buffer[0] << 8) | buffer[1], (buffer[0] << 8) | buffer[1]); - RF_END_RETURN(static_cast((buffer[0] << 8) | buffer[1])); + return static_cast((buffer[0] << 8) | buffer[1]); } } // modm namespace diff --git a/src/modm/driver/pressure/scp1000.hpp b/src/modm/driver/pressure/scp1000.hpp index 2406e0631e..f2092b44f9 100644 --- a/src/modm/driver/pressure/scp1000.hpp +++ b/src/modm/driver/pressure/scp1000.hpp @@ -14,7 +14,7 @@ #define MODM_SCP1000_HPP #include -#include +#include namespace modm { diff --git a/src/modm/driver/pressure/scp1000_impl.hpp b/src/modm/driver/pressure/scp1000_impl.hpp index 857f09f665..a8f43f636b 100644 --- a/src/modm/driver/pressure/scp1000_impl.hpp +++ b/src/modm/driver/pressure/scp1000_impl.hpp @@ -101,7 +101,7 @@ modm::Scp1000::setOperation(scp1000::Operation opMode) uint8_t retries = 16; // wait for the sensor to complete setting the operation while (--retries && (readStatus(true) & scp1000::OPERATION_STATUS_RUNNING)) { - modm::delay_ms(1); + modm::this_fiber::sleep_for(1ms); } // The sensor took too long to complete the operation @@ -129,12 +129,12 @@ modm::Scp1000::reset(uint8_t timeout=50) writeRegister(scp1000::REGISTER_RSTR, scp1000::RESET); // wait a bit to give the Scp1000 some time to restart - modm::delay_ms(151); + modm::this_fiber::sleep_for(151ms); uint8_t retries = timeout; // wait for the sensor to complete start up, this should take 160ms while (--retries && (readStatus() & scp1000::STATUS_STARTUP_RUNNING_bm)) { - modm::delay_ms(1); + modm::this_fiber::sleep_for(1ms); } if (retries > 0) { diff --git a/src/modm/driver/pwm/apa102.hpp b/src/modm/driver/pwm/apa102.hpp index b9b5e79d32..1e5b448075 100644 --- a/src/modm/driver/pwm/apa102.hpp +++ b/src/modm/driver/pwm/apa102.hpp @@ -87,7 +87,7 @@ class Apa102 return data[4 + index*4] & ~0xe0; } - modm::ResumableResult + void write() { return SpiMaster::transfer(data, nullptr, length); diff --git a/src/modm/driver/pwm/apa102.lb b/src/modm/driver/pwm/apa102.lb index 0dd4bbc959..ed2fcf7dd1 100644 --- a/src/modm/driver/pwm/apa102.lb +++ b/src/modm/driver/pwm/apa102.lb @@ -36,7 +36,7 @@ References: def prepare(module, options): module.depends( ":architecture:spi", - ":processing:resumable", + ":architecture:fiber", ":ui:color") return True diff --git a/src/modm/driver/pwm/lp503x.hpp.in b/src/modm/driver/pwm/lp503x.hpp.in index b973d81166..aa5bb88890 100644 --- a/src/modm/driver/pwm/lp503x.hpp.in +++ b/src/modm/driver/pwm/lp503x.hpp.in @@ -90,7 +90,7 @@ struct lp503x * @ingroup modm_driver_lp503x */ template -class Lp503x : public lp503x, public modm::I2cDevice +class Lp503x : public lp503x, public modm::I2cDevice { public: static_assert(Channels == 30 || Channels == 36); @@ -104,20 +104,20 @@ public: Lp503x(uint8_t i2cAddress); /// Initialize device, call before using any other function - modm::ResumableResult + bool initialize(); /// Enable device - modm::ResumableResult + bool enable(); /// Disable device - modm::ResumableResult + bool disable(); /// Set device configuration /// AutoIncrement is always set - modm::ResumableResult + bool setConfiguration(DeviceConfig1_t configuration); /** @@ -131,7 +131,7 @@ public: * @param channel output channel index * @param value 8-bit brightness value */ - modm::ResumableResult + bool setChannelBrightness(uint8_t channel, uint8_t value); %% if options.multichannel_write @@ -145,7 +145,7 @@ public: * @param numChannels number of values to write * @param value 8-bit brightness value */ - modm::ResumableResult + bool setChannelBrightnessValues(uint8_t startChannel, const uint8_t* values, uint8_t numChannels); %% endif @@ -166,7 +166,7 @@ public: * * @param value 8-bit brightness value */ - modm::ResumableResult + bool setLedGroupBrightness(uint8_t index, uint8_t value); /** @@ -180,46 +180,46 @@ public: * and the BankBrightness register. * The inidividual channel settings are ignored then. */ - modm::ResumableResult + bool setBankModeEnabled(LedBankMode_t bankMode); /** * Set bank brightness multiplicator. This controls * all channels in bank mode. */ - modm::ResumableResult + bool setBankBrightness(uint8_t value); /** * Set brightness for all channels 3n * configured in bank mode. */ - modm::ResumableResult + bool setBankABrightness(uint8_t value); /** * Set brightness for all channels 3n+1 * configured in bank mode. */ - modm::ResumableResult + bool setBankBBrightness(uint8_t value); /** * Set brightness for all channels 3n+2 * configured in bank mode. */ - modm::ResumableResult + bool setBankCBrightness(uint8_t value); private: - modm::ResumableResult + bool writeRegister(Register reg, uint8_t value); - modm::ResumableResult + bool writeRegister(Register reg, RegisterValue_t value); %% if options.multichannel_write - modm::ResumableResult + bool writeRegisters(Register startRegister, const uint8_t* values, uint8_t numValues); %% endif diff --git a/src/modm/driver/pwm/lp503x_impl.hpp.in b/src/modm/driver/pwm/lp503x_impl.hpp.in index c393bdf249..d0cb73bbf8 100644 --- a/src/modm/driver/pwm/lp503x_impl.hpp.in +++ b/src/modm/driver/pwm/lp503x_impl.hpp.in @@ -20,164 +20,151 @@ namespace modm template Lp503x::Lp503x(uint8_t i2cAddress) - : I2cDevice(i2cAddress) + : I2cDevice(i2cAddress) { } template -ResumableResult +bool Lp503x::initialize() { - RF_BEGIN(); - success = RF_CALL(writeRegister(Register::Reset, Reset::ResetDevice)); + success = writeRegister(Register::Reset, Reset::ResetDevice); if(!success) { - RF_RETURN(false); + return false; } - RF_END_RETURN_CALL(setConfiguration(DefaultConfiguration)); + return setConfiguration(DefaultConfiguration); } template -ResumableResult +bool Lp503x::enable() { return writeRegister(Register::DeviceConfig0, DeviceConfig0::ChipEnable); } template -ResumableResult +bool Lp503x::disable() { return writeRegister(Register::DeviceConfig0, DeviceConfig0_t{}); } template -ResumableResult +bool Lp503x::setConfiguration(DeviceConfig1_t configuration) { return writeRegister(Register::DeviceConfig1, configuration | DeviceConfig1::AutoIncrement); } template -ResumableResult +bool Lp503x::setChannelBrightness(uint8_t channel, uint8_t value) { const Register reg = static_cast(uint8_t(Register::Out0Color) + channel); - - RF_BEGIN(); if(channel >= Channels) { - RF_RETURN(false); + return false; } - RF_END_RETURN_CALL(writeRegister(reg, value)); + return writeRegister(reg, value); } %% if options.multichannel_write template -ResumableResult +bool Lp503x::setChannelBrightnessValues(uint8_t startChannel, const uint8_t* values, uint8_t numChannels) { - RF_BEGIN(); if(numChannels == 0 || (startChannel + numChannels) > Channels) { - RF_RETURN(false); + return false; } - RF_END_RETURN_CALL(writeRegisters( + return writeRegisters( static_cast(uint8_t(Register::Out0Color) + startChannel), - values, numChannels)); + values, numChannels); } %% endif template -ResumableResult +bool Lp503x::setLedGroupBrightness(uint8_t index, uint8_t value) { const Register reg = static_cast(uint8_t(Register::Led0Brightness) + index); - - RF_BEGIN(); if(index >= (Channels / 3)) { - RF_RETURN(false); + return false; } - RF_END_RETURN_CALL(writeRegister(reg, value)); + return writeRegister(reg, value); } template -ResumableResult +bool Lp503x::setBankModeEnabled(LedBankMode_t bankMode) { - RF_BEGIN(); buffer[0] = static_cast(Register::LedConfig0); buffer[1] = bankMode.value & 0xff; buffer[2] = (bankMode.value & 0xf00) >> 8; - this->transaction.configureWrite(buffer, 3); - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::write(buffer, 3); } template -ResumableResult +bool Lp503x::setBankABrightness(uint8_t value) { return writeRegister(Register::BankAColor, value); } template -ResumableResult +bool Lp503x::setBankBBrightness(uint8_t value) { return writeRegister(Register::BankBColor, value); } template -ResumableResult +bool Lp503x::setBankCBrightness(uint8_t value) { return writeRegister(Register::BankCColor, value); } template -ResumableResult +bool Lp503x::setBankBrightness(uint8_t value) { return writeRegister(Register::BankBrightness, value); } template -ResumableResult +bool Lp503x::writeRegister(Register reg, RegisterValue_t value) { return writeRegister(reg, value.value); } template -ResumableResult +bool Lp503x::writeRegister(Register reg, uint8_t value) { - RF_BEGIN(); buffer[0] = static_cast(reg); buffer[1] = value; - this->transaction.configureWrite(buffer, 2); - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::write(buffer, 2); } %% if options.multichannel_write template -ResumableResult +bool Lp503x::writeRegisters(Register startRegister, const uint8_t* values, uint8_t numValues) { - RF_BEGIN(); - if((numValues + 1u) > sizeof(buffer)) { - RF_RETURN(false); + return false; } buffer[0] = static_cast(startRegister); std::copy_n(values, numValues, &buffer[0] + 1); - this->transaction.configureWrite(buffer, numValues + 1u); - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::write(buffer, numValues + 1u); } %% endif diff --git a/src/modm/driver/pwm/max6966.hpp b/src/modm/driver/pwm/max6966.hpp index 476a60ca56..51c261afe9 100644 --- a/src/modm/driver/pwm/max6966.hpp +++ b/src/modm/driver/pwm/max6966.hpp @@ -16,7 +16,7 @@ #include #include -#include +#include namespace modm { diff --git a/src/modm/driver/pwm/max6966.lb b/src/modm/driver/pwm/max6966.lb index 596dbda74d..7a5233f5f6 100644 --- a/src/modm/driver/pwm/max6966.lb +++ b/src/modm/driver/pwm/max6966.lb @@ -21,7 +21,7 @@ def init(module): def prepare(module, options): module.depends( - ":architecture:delay", + ":architecture:fiber", ":architecture:gpio", ":architecture:spi") return True diff --git a/src/modm/driver/pwm/max6966_impl.hpp b/src/modm/driver/pwm/max6966_impl.hpp index a14694fbb7..b85b068928 100644 --- a/src/modm/driver/pwm/max6966_impl.hpp +++ b/src/modm/driver/pwm/max6966_impl.hpp @@ -33,8 +33,8 @@ modm::MAX6966::setAllConfiguration(uint8_t config) Cs::reset(); for (uint_fast8_t i=0; i < DRIVERS; ++i) { - Spi::transferBlocking(max6966::REGISTER_CONFIGURATION); - Spi::transferBlocking(config); + Spi::transfer(max6966::REGISTER_CONFIGURATION); + Spi::transfer(config); } Cs::set(); } @@ -64,8 +64,8 @@ modm::MAX6966::setChannels(uint8_t * values) // for all drivers for (uint_fast8_t dr = 0; dr < DRIVERS; ++dr) { - Spi::transferBlocking(max6966::REGISTER_PORT0 + ch); - Spi::transferBlocking(values[ch + dr * 10]); + Spi::transfer(max6966::REGISTER_PORT0 + ch); + Spi::transfer(values[ch + dr * 10]); } Cs::set(); } @@ -78,8 +78,8 @@ modm::MAX6966::setAllChannels(uint8_t value) Cs::reset(); for (uint_fast8_t i=0; i < DRIVERS; ++i) { - Spi::transferBlocking(max6966::REGISTER_PORT0_9); - Spi::transferBlocking(value); + Spi::transfer(max6966::REGISTER_PORT0_9); + Spi::transfer(value); } Cs::set(); } @@ -130,8 +130,8 @@ modm::MAX6966::setAllCurrent(max6966::Current current) Cs::reset(); for (uint_fast8_t i=0; i < DRIVERS; ++i) { - Spi::transferBlocking(max6966::REGISTER_GLOBAL_CURRENT); - Spi::transferBlocking(current); + Spi::transfer(max6966::REGISTER_GLOBAL_CURRENT); + Spi::transfer(current); } Cs::set(); } @@ -144,8 +144,8 @@ modm::MAX6966::writeToDriver(uint8_t driver, max6966::Register Cs::reset(); for (uint_fast8_t i=0; i < DRIVERS; ++i) { - Spi::transferBlocking((i == driver) ? (reg | max6966::WRITE) : max6966::REGISTER_NO_OP); - Spi::transferBlocking(data); + Spi::transfer((i == driver) ? (reg | max6966::WRITE) : max6966::REGISTER_NO_OP); + Spi::transfer(data); } Cs::set(); } @@ -158,13 +158,13 @@ modm::MAX6966::readFromDriver(uint8_t driver, max6966::Registe Cs::reset(); for (uint_fast8_t i=0; i < DRIVERS; ++i) { - Spi::transferBlocking((i == driver) ? (reg | max6966::READ) : max6966::REGISTER_NO_OP); - Spi::transferBlocking(0xff); + Spi::transfer((i == driver) ? (reg | max6966::READ) : max6966::REGISTER_NO_OP); + Spi::transfer(0xff); } Cs::set(); // TODO: Add delay here? -// modm::delay_ms(1); +// modm::this_fiber::sleep_for(1ms); // send dummy data and get the right register uint8_t data=0; @@ -172,8 +172,8 @@ modm::MAX6966::readFromDriver(uint8_t driver, max6966::Registe Cs::reset(); for (uint_fast8_t i=0; i < DRIVERS; ++i) { - Spi::transferBlocking(max6966::REGISTER_NO_OP); - buffer = Spi::transferBlocking(0xff); + Spi::transfer(max6966::REGISTER_NO_OP); + buffer = Spi::transfer(0xff); if (i == driver) data = buffer; } Cs::set(); diff --git a/src/modm/driver/pwm/pca9685.hpp b/src/modm/driver/pwm/pca9685.hpp index 803c04f5eb..97fc5ef90f 100644 --- a/src/modm/driver/pwm/pca9685.hpp +++ b/src/modm/driver/pwm/pca9685.hpp @@ -146,7 +146,7 @@ struct pca9685 * @ingroup modm_driver_pca9685 */ template -class Pca9685 : public pca9685, public modm::I2cDevice< I2cMaster, 1, I2cWriteTransaction > +class Pca9685 : public pca9685, public modm::I2cDevice< I2cMaster > { uint8_t buffer[3]; @@ -167,7 +167,7 @@ class Pca9685 : public pca9685, public modm::I2cDevice< I2cMaster, 1, I2cWriteTr * @param mode1 value to be written to MODE1 register * @param mode2 value to be written to MODE2 register */ - modm::ResumableResult + bool initialize(uint8_t mode1 = 0, uint8_t mode2 = 0); /** @@ -180,7 +180,7 @@ class Pca9685 : public pca9685, public modm::I2cDevice< I2cMaster, 1, I2cWriteTr * @param channel one of the 16 channels (0-15) * @param value 12-bit PWM value to be written */ - modm::ResumableResult + bool setChannel(uint8_t channel, uint16_t value); /** @@ -191,7 +191,7 @@ class Pca9685 : public pca9685, public modm::I2cDevice< I2cMaster, 1, I2cWriteTr * * @param value 12-bit PWM value to be written */ - modm::ResumableResult + bool setAllChannels(uint16_t value); }; diff --git a/src/modm/driver/pwm/pca9685_impl.hpp b/src/modm/driver/pwm/pca9685_impl.hpp index 393b0d82a3..6a4aa2aef8 100644 --- a/src/modm/driver/pwm/pca9685_impl.hpp +++ b/src/modm/driver/pwm/pca9685_impl.hpp @@ -17,31 +17,25 @@ // ---------------------------------------------------------------------------- template modm::Pca9685::Pca9685(uint8_t address) : - I2cDevice(address) + I2cDevice(address) {} template -modm::ResumableResult +bool modm::Pca9685::initialize(uint8_t mode1, uint8_t mode2) { - RF_BEGIN(); - // set the first mode register buffer[0] = REG_MODE1; buffer[1] = mode1 | MODE1_AI; // ensure that auto increment is enabled - this->transaction.configureWrite(buffer, 2); - - if (not RF_CALL( this->runTransaction() )) - RF_RETURN(false); + if (not I2cDevice::write(buffer, 2)) + return false; // set the second mode register buffer[0] = REG_MODE2; buffer[1] = mode2; - this->transaction.configureWrite(buffer, 2); - - if (not RF_CALL( this->runTransaction() )) - RF_RETURN(false); + if (not I2cDevice::write(buffer, 2)) + return false; // Always turn on all LEDs at tick 0 and switch them of later according // to the current value @@ -49,43 +43,33 @@ modm::Pca9685::initialize(uint8_t mode1, uint8_t mode2) buffer[0] = REG_ALL_LED_ON_L; buffer[1] = 0x00; buffer[2] = 0x00; - this->transaction.configureWrite(buffer, 3); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(buffer, 3); } template -modm::ResumableResult +bool modm::Pca9685::setChannel(uint8_t channel, uint16_t value) { - RF_BEGIN(); - // there are only 16 channels if (channel >= 16) - RF_RETURN(false); + return false; buffer[0] = REG_LED0_OFF_L + 4 * channel; // The Controller turns all LEDs on at tick 0 // and turns this LED of at value buffer[1] = uint8_t(value); buffer[2] = uint8_t(value >> 8) & 0x0f; - this->transaction.configureWrite(buffer, 3); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(buffer, 3); } template -modm::ResumableResult +bool modm::Pca9685::setAllChannels(uint16_t value) { - RF_BEGIN(); - buffer[0] = REG_ALL_LED_OFF_L; // The Controller turns all LEDs on at tick 0 // and turns this LED of at tick $value buffer[1] = uint8_t(value); buffer[2] = uint8_t(value >> 8) & 0x0f; - this->transaction.configureWrite(buffer, 3); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(buffer, 3); } diff --git a/src/modm/driver/pwm/tlc594x.hpp b/src/modm/driver/pwm/tlc594x.hpp index 439e044c05..9308b94e0f 100644 --- a/src/modm/driver/pwm/tlc594x.hpp +++ b/src/modm/driver/pwm/tlc594x.hpp @@ -15,7 +15,7 @@ #define MODM_TLC594X_HPP #include -#include +#include #include #include @@ -64,7 +64,7 @@ template< typename Xblank=modm::platform::GpioUnused, typename Vprog=modm::platform::GpioUnused, typename Xerr=modm::platform::GpioUnused > -class TLC594X : private modm::NestedResumable< 1 > +class TLC594X { public: static_assert(CHANNELS % 4 == 0, "Template parameter CHANNELS must be a multiple of 4."); @@ -110,11 +110,11 @@ class TLC594X : private modm::NestedResumable< 1 > getDotCorrection(uint16_t channel); /// transfer channel data to driver chip - modm::ResumableResult + void writeChannels(bool flush=true); /// transfer dot correction data to driver chip - modm::ResumableResult + void writeDotCorrection(bool flush=true); /// writes data from the input shift register to either GS or DC register. diff --git a/src/modm/driver/pwm/tlc594x.lb b/src/modm/driver/pwm/tlc594x.lb index 257e36e550..c3a7d8a604 100644 --- a/src/modm/driver/pwm/tlc594x.lb +++ b/src/modm/driver/pwm/tlc594x.lb @@ -21,7 +21,7 @@ TLC594* multi-channel, daisy-chainable, constant-current sink, 12bit PWM LED dri def prepare(module, options): module.depends( - ":architecture:delay", + ":architecture:fiber", ":architecture:gpio", ":architecture:spi") return True diff --git a/src/modm/driver/pwm/tlc594x_impl.hpp b/src/modm/driver/pwm/tlc594x_impl.hpp index ea20a410f5..c307806e1e 100644 --- a/src/modm/driver/pwm/tlc594x_impl.hpp +++ b/src/modm/driver/pwm/tlc594x_impl.hpp @@ -28,8 +28,8 @@ modm::TLC594X::initialize(uint16_t cha if (channels != 0xffff) setAllChannels(channels); if (dots != 0xff) setAllDotCorrection(dots); - if (writeCH) RF_CALL_BLOCKING(writeChannels()); - if (writeDC) RF_CALL_BLOCKING(writeDotCorrection()); + if (writeCH) writeChannels(); + if (writeDC) writeDotCorrection(); if (enable) this->enable(); } @@ -42,7 +42,7 @@ modm::TLC594X::latch() Xlat::set(); // datasheet says 20ns but that is unreliable // => wait for at least 1000ns - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); Xlat::reset(); if (enabled) Xblank::reset(); } @@ -205,30 +205,22 @@ modm::TLC594X::getDotCorrection(uint16 } template -modm::ResumableResult +void modm::TLC594X::writeChannels(bool flush) { - RF_BEGIN(); - - RF_CALL(Spi::transfer(gs, status, CHANNELS*3/2)); + Spi::transfer(gs, status, CHANNELS*3/2); if (flush) latch(); - - RF_END(); } template -modm::ResumableResult +void modm::TLC594X::writeDotCorrection(bool flush) { - RF_BEGIN(); - Vprog::set(); // transfer - RF_CALL(Spi::transfer(dc, nullptr, CHANNELS*3/4)); + Spi::transfer(dc, nullptr, CHANNELS*3/4); if (flush) latch(); Vprog::reset(); - - RF_END(); } diff --git a/src/modm/driver/radio/dw3110/dw3110.lb b/src/modm/driver/radio/dw3110/dw3110.lb index d33c5a8348..83b8240c3c 100644 --- a/src/modm/driver/radio/dw3110/dw3110.lb +++ b/src/modm/driver/radio/dw3110/dw3110.lb @@ -23,7 +23,7 @@ def prepare(module, options): ":architecture:spi.device", ":debug", ":processing:timer", - ":processing:resumable") + ":architecture:fiber") return True def build(env): diff --git a/src/modm/driver/radio/dw3110/dw3110_phy.hpp b/src/modm/driver/radio/dw3110/dw3110_phy.hpp index 880e465d76..3da81ba20a 100644 --- a/src/modm/driver/radio/dw3110/dw3110_phy.hpp +++ b/src/modm/driver/radio/dw3110/dw3110_phy.hpp @@ -17,7 +17,7 @@ #include #include #include -#include +#include #include #include @@ -37,7 +37,7 @@ namespace modm * @author Michael Jossen */ template -class Dw3110Phy : public modm::SpiDevice, protected modm::NestedResumable<6> +class Dw3110Phy : public modm::SpiDevice { public: enum class TXMode @@ -72,32 +72,32 @@ class Dw3110Phy : public modm::SpiDevice, protected modm::NestedResum Dw3110Phy(); /// Set the UWB channel used - modm::ResumableResult + void setChannel(Dw3110::Channel channel); /// Change Header format to non-standard to allow 1021 Byte payloads instead of /// default 125 - modm::ResumableResult + void setEnableLongFrames(bool value); /// Send the PHR at 6.8Mbps. \n /// By default the PHR is always sent at 850kb/s - modm::ResumableResult + void setSendHeaderFast(bool value); /// Set the timeout to wait on arriving packets after startReceive() \n /// 0 = No Timeout \n /// Register resolution is ~1µs - modm::ResumableResult + void setReceiveWaitTimeout(modm::chrono::micro_clock::duration duration); /// Set whether to stay in receive mode after receive failure - modm::ResumableResult + void setReenableOnRxFailure(bool value); /// Set the time spent listening for competing transmissions on CCA commands \n /// Unit is in counts of PAC symbols - modm::ResumableResult + void setCCATimeout(uint16_t timeout); /// Enable a faster TX/RX turnaround. \n @@ -105,152 +105,152 @@ class Dw3110Phy : public modm::SpiDevice, protected modm::NestedResum /// any ACKs will be sent. \n /// Time of Arrival may not be ready when the frame is /// made available - modm::ResumableResult + void setEnableFastTurnaround(bool value); /// Read the value of the internal chip clock - modm::ResumableResult + uint32_t readChipTime(); /// Set the number of preamble symbols sent with each transmission \n /// Each symbol takes ~1µs to send \n /// @warning While further fine tuning of preamble length is possible it is currently /// unsupported by this driver - modm::ResumableResult + void setPreambleLength(Dw3110::PreambleLength len); /// Get the timestamp of the last arrived packet. \n /// This timestamp already has various correction factors applied to it. \n /// It is given in ~15.65 picoseconds per unit - modm::ResumableResult + uint64_t getReceiveTimestamp(); /// Analogous to getReceiveTimestamp - modm::ResumableResult + uint64_t getTransmitTimestamp(); /// Set the time between RX of a packet and the TX of the acknowledgement \n /// Specified in number of preamble symbols, so time depends on the PRF - modm::ResumableResult + void setAcknowledgeTurnaround(uint8_t time); /// Set the time between a transmission and the start of RX on any of the RX /// after TX commands \n /// This can be used to delay turning on of the receiver after transmission /// to save on power. - modm::ResumableResult + void setWaitForResponseTime(modm::PreciseClock::duration time); /// Set the start frame delimiter used by the chip - modm::ResumableResult + void setSFD(Dw3110::StartFrameDelimiter sfd); /// Set the preamble code used and looked for. /// @warning This implicitly sets the PRF as codes >8 use the 64MHz PRF /// instead of 16MHz - modm::ResumableResult + void setPreambleCode(Dw3110::PreambleCode rx, Dw3110::PreambleCode tx); /// Get devices unique id - modm::ResumableResult + uint64_t getDeviceUID(); /// Set devices unique id - modm::ResumableResult + void setDeviceUID(uint64_t uid); /// Get devices pan id - modm::ResumableResult + uint16_t getPanUID(); /// Get devices short id - modm::ResumableResult + uint16_t getShortUID(); /// Set devices pan id - modm::ResumableResult + void setPanUID(uint16_t pid); /// Set devices short id - modm::ResumableResult + void setShortUID(uint16_t pid); /// Enable or disable the frame filtering - modm::ResumableResult + void setFrameFilterEnabled(bool value); /// Get current filtering config - modm::ResumableResult + Dw3110::FilterConfig_t getFilterConfig(); /// Set frame filtering config - modm::ResumableResult + void setFilterConfig(Dw3110::FilterConfig_t fc); /// Enable auto acknowledgment \n /// Will automatically respond to valid 802.5.14 Frames with the ACK request /// bit set \n /// Only active when Frame filtering is enabled - modm::ResumableResult + void setAutoAckEnabled(bool value); /// Read the reported state of the chip - modm::ResumableResult + Dw3110::SystemState getChipState(); /// Set both the RX and TX antenna delay \n /// Unit is approx 15.65ps - modm::ResumableResult + void setAntennaDelay(uint16_t delay); /// Get the currently programmed RX antenna delay \n /// Unit is approx 15.65ps - modm::ResumableResult + uint16_t getRXAntennaDelay(); /// Get the currently programmed TX antenna delay \n /// Unit is approx 15.65ps - modm::ResumableResult + uint16_t getTXAntennaDelay(); /// Get the Clock offset multiplier for the received packet \n /// Can be used to recover the clock drift compared to a packets sender \n /// Unit is in parts per million (ppm) - modm::ResumableResult + float getReceiverClockOffset(); /// Runs the RX calibration. \n /// Needs to be done after startup, wake and after 20C temperature change \n /// Needs to be run when chip is in IDLE_PLL state \n /// Automatically run by @ref initialize() - modm::ResumableResult + bool calibrate(); /// Initialize the DW3000, make ready to receive/transmit \n /// Call ideally after the IRQ Pin has gone high, signalling SPIRDY - modm::ResumableResult + bool initialize(Dw3110::Channel channel = Dw3110::Channel::Channel5, Dw3110::PreambleCode pcode = Dw3110::PreambleCode::Code_64Mhz_9, Dw3110::PreambleLength plen = Dw3110::PreambleLength::Preamble_64, Dw3110::StartFrameDelimiter sfd = Dw3110::StartFrameDelimiter::IEEE802_15_4z_8); /// Set the chip into receive mode - modm::ResumableResult + bool startReceive(); /// Check if a packet has been successfully received - modm::ResumableResult + bool packetReady(); /// Check if the chip is currently in RX mode - modm::ResumableResult + bool isReceiving(); /// Copy received packet into the provided payload buffer, clear packet /// received flags /// @param payload Region of memory the payload will be written to /// @param payload_len Contains the length of the received payload - modm::ResumableResult + bool fetchPacket(std::span payload, size_t &payload_len); /// Transmit a given package using the current configuration using a given transmission mode @@ -258,13 +258,13 @@ class Dw3110Phy : public modm::SpiDevice, protected modm::NestedResum /// @param ranging Decides whether or not to set the ranging bit in the header /// @param fast Decides if the data portion is sent at 850kbps or 6.8Mbps template - modm::ResumableResult + Error transmit(const std::span payload, bool ranging = true, bool fast = true); /// Set the reference time value \n /// Used as the reference in @ref transmitDelayed() when using DelayTXMode::DelayWRTRef and /// DelayTXMode::DelayWRTRefAndReceive - modm::ResumableResult + void setReferenceTime(uint32_t time); /// Transmit a given package using the current configuration using a given delayed transmission @@ -276,16 +276,16 @@ class Dw3110Phy : public modm::SpiDevice, protected modm::NestedResum /// DelayTXMode provided /// @warning This transmission mode does not respect the clear channel assessment template - modm::ResumableResult + Error transmitDelayed(uint32_t time, const std::span payload, bool ranging = true, bool fast = true); /// Read the current system status register - modm::ResumableResult + Dw3110::SystemStatus_t getStatus(); /// Clear the given bits in the status register - modm::ResumableResult + void clearStatusBits(Dw3110::SystemStatus_t mask); /// Set interrupt bits \n @@ -295,64 +295,64 @@ class Dw3110Phy : public modm::SpiDevice, protected modm::NestedResum /// Polarity of the line can be changed with \ref setIRQPolarity() \n /// @warning Dw3110::SystemStatus::IRQS will be ignored since it does not correspond to an /// interrupt - modm::ResumableResult + void setInterruptsEnabled(Dw3110::SystemStatus_t mask); /// Set the polarity of the interrupt line \n /// Default is active high \n /// @param high If true sets the line to active high - modm::ResumableResult + void setIRQPolarity(bool high); /// Set the length of the generated Secure timestamp ///@param len Length in units of 8 chips (~1µs), minimum supported is 32 chips(e.g a value of 3) - modm::ResumableResult + bool setSTSLength(uint8_t len); /// Set the STS mode \n /// @param mode Defines where to place the STS inside the packet /// @param sdc If true IV and Key are ignored and a deterministic code is used - modm::ResumableResult + void setSTSMode(Dw3110::STSMode mode, bool sdc); /// Return the 12-bit quality assessment of the last received STS - modm::ResumableResult + uint16_t getSTSQuality(); /// Return whether or not the STS quality is in an acceptable range - modm::ResumableResult + bool getSTSGood(); /// Set the Key to use for AES generation of the STS \n /// Ignored if SDC is set in \ref setSTSMode() - modm::ResumableResult + void setSTSKey(std::span key); /// Set the IV to use for AES generation of the STS \n /// Ignored if SDC is set in \ref setSTSMode() - modm::ResumableResult + void setSTSIV(std::span iv); /// Get the Key to use for AES generation of the STS - modm::ResumableResult + void getSTSKey(std::span key); /// Get the IV to use for AES generation of the STS /// @warning Will only return the programmed IV, to get the incremented value use \ref /// getCurrentCounter() - modm::ResumableResult + void getSTSIV(std::span iv); /// Get lower 32 bits of the currently used STS IV - modm::ResumableResult + uint32_t getCurrentCounter(); /// Reload the STS IV from the STSIV registers - modm::ResumableResult + void reloadSTSIV(); /// Don't increment the STS IV for the next RX/TX - modm::ResumableResult + void reuseLastSTSIV(); protected: @@ -362,78 +362,78 @@ class Dw3110Phy : public modm::SpiDevice, protected modm::NestedResum /// @param fast Decides if the data portion is sent at 850kbps or 6.8Mbps /// command template - modm::ResumableResult + Error transmitGeneric(std::span payload, bool ranging, bool fast); /// Only load configuration independent stuff, everything else should be /// initialized when changing those parts - modm::ResumableResult + void loadOTP(); /// Check the device ID over SPI - modm::ResumableResult + bool checkDevID(); /// Send a command to the chip template - modm::ResumableResult + void sendCommand(); /// Recompute the SFD TOC - modm::ResumableResult + void setRX_SFD_TOC(); /// Update the local system_status variable - modm::ResumableResult + void fetchSystemStatus(); /// Update the local chip_state variable - modm::ResumableResult + void fetchChipState(); /// Due to an errata the check is a bit more complicated \n /// Relies on system_status and chip_state being up to date - modm::ResumableResult + Error checkTXFailed(); /// Read a variable from the OTP memory template - modm::ResumableResult + void readOTPMemory(std::span out); /// Read a variable from a register template - modm::ResumableResult + void readRegister(std::span out); /// Read a number of bytes from a register bank, useful for RX buffers and /// other large read transfers template - modm::ResumableResult + void readRegisterBank(std::span out, size_t len); /// Write a variable to a register template - modm::ResumableResult + void writeRegister(std::span val); /// Simple implementation of read modify write using and and or masks \n /// Use on registers that do not support native write register masked template - modm::ResumableResult + void readModifyWriteRegister(std::span or_mask, std::span and_mask); /// Do not use to clear "write 1 to clear" bits (2.3.1.2 Table 3) template - modm::ResumableResult + void writeRegisterMasked(std::span or_mask, std::span and_mask); /// Write a number of bytes to a register bank, useful for TX buffers and other /// large write transfers template - modm::ResumableResult + void writeRegisterBank(std::span val, size_t len); template @@ -464,4 +464,4 @@ class Dw3110Phy : public modm::SpiDevice, protected modm::NestedResum #include "dw3110_phy_impl.hpp" -#endif \ No newline at end of file +#endif diff --git a/src/modm/driver/radio/dw3110/dw3110_phy_impl.hpp b/src/modm/driver/radio/dw3110/dw3110_phy_impl.hpp index ddb37fdacb..28d914ce1b 100644 --- a/src/modm/driver/radio/dw3110/dw3110_phy_impl.hpp +++ b/src/modm/driver/radio/dw3110/dw3110_phy_impl.hpp @@ -31,19 +31,18 @@ modm::Dw3110Phy::Dw3110Phy() } template -modm::ResumableResult +bool modm::Dw3110Phy::initialize(Dw3110::Channel channel, Dw3110::PreambleCode pcode, Dw3110::PreambleLength plen, Dw3110::StartFrameDelimiter sfd) { using namespace std::chrono_literals; - RF_BEGIN(); - RF_CALL(fetchChipState()); + fetchChipState(); if (chip_state == Dw3110::SystemState::TX || chip_state == Dw3110::SystemState::RX || chip_state == Dw3110::SystemState::TX_WAIT || chip_state == Dw3110::SystemState::RX_WAIT) { - RF_CALL(sendCommand()); + sendCommand(); } // Loop until valid state @@ -54,294 +53,268 @@ modm::Dw3110Phy::initialize(Dw3110::Channel channel, Dw3110::Prea if (timeout.execute()) { MODM_LOG_ERROR << "Timeout waiting for IDLE State!" << (int)chip_state << modm::endl; - RF_RETURN(false); + return false; } - RF_YIELD(); - RF_CALL(fetchChipState()); + modm::this_fiber::yield(); + fetchChipState(); } - if (!RF_CALL(checkDevID())) { RF_RETURN(false); } + if (!checkDevID()) { return false; } // Initialize factory programmed defaults - RF_CALL(loadOTP()); + loadOTP(); // Load magic constants mentioned in various places in the manual constexpr static uint8_t rf_tx_ctrl_1_magic[] = {0x0E}; - RF_CALL(writeRegister(rf_tx_ctrl_1_magic)); + writeRegister(rf_tx_ctrl_1_magic); constexpr static uint8_t ldo_rload_magic[] = {0x14}; - RF_CALL(writeRegister(ldo_rload_magic)); + writeRegister(ldo_rload_magic); constexpr static uint8_t res_b0_magic[] = {0x9B}; - RF_CALL(writeRegister(res_b0_magic)); + writeRegister(res_b0_magic); // Magic value from the user manual (8.2.7.4) // constexpr static uint8_t dtune3_magic[] = {0xCC, 0x35, 0x5F, 0xAF}; - // RF_CALL(writeRegister(dtune3_magic)); + // writeRegister(dtune3_magic); // TODO maybe only when sending packets with no payload, // investigate (github.com/egnor/DW3000_notes.md)) // Change to IDLE_PLL by setting SEQ_CTRL:AINIT2IDLE constexpr static uint8_t and_mask[] = {0xFF}; constexpr static uint8_t or_mask[] = {0x01}; - RF_CALL(writeRegisterMasked(or_mask, and_mask)); + writeRegisterMasked(or_mask, and_mask); // Loop until we are in IDLE_PLL timeout.restart(1ms); - RF_CALL(fetchChipState()); + fetchChipState(); while (chip_state != Dw3110::SystemState::IDLE_PLL) { - RF_YIELD(); + modm::this_fiber::yield(); if (timeout.execute()) { MODM_LOG_ERROR << "Failed to reach IDLE_PLL State!" << modm::endl; - RF_RETURN(false); + return false; } fetchChipState(); } - if (!RF_CALL(calibrate())) { RF_RETURN(false); } - RF_CALL(setChannel(channel)); - RF_CALL(setPreambleCode(pcode, pcode)); - RF_CALL(setPreambleLength(plen)); - RF_CALL(setSFD(sfd)); - RF_CALL(setEnableLongFrames(false)); - RF_CALL(setSendHeaderFast(false)); - RF_CALL(setCCATimeout(256)); - RF_CALL(reloadSTSIV()); - RF_END_RETURN(true); + if (!calibrate()) { return false; } + setChannel(channel); + setPreambleCode(pcode, pcode); + setPreambleLength(plen); + setSFD(sfd); + setEnableLongFrames(false); + setSendHeaderFast(false); + setCCATimeout(256); + reloadSTSIV(); + return true; } template -modm::ResumableResult +bool modm::Dw3110Phy::checkDevID() { - RF_BEGIN(); - // Check we actually read registers correctly by checking for the device type constexpr static uint8_t DEV_ID_MATCH[] = {0x03, 0xCA, 0xDE}; - RF_CALL(readRegister(std::span(scratch).first<4>())); + readRegister(std::span(scratch).first<4>()); if (!std::ranges::equal(DEV_ID_MATCH, std::span(scratch).subspan<1, 3>())) { MODM_LOG_ERROR << "Device did not return valid Dw3000 device ID!" << modm::endl; - RF_RETURN(false); + return false; } - RF_END_RETURN(true); + return true; } template -modm::ResumableResult +void modm::Dw3110Phy::setEnableLongFrames(bool value) { - RF_BEGIN() long_frames = value; if (value) { constexpr static uint8_t or_mask_true[] = {0x10}; constexpr static uint8_t and_mask_true[] = {0xFF}; - RF_CALL(writeRegisterMasked(or_mask_true, and_mask_true)); + writeRegisterMasked(or_mask_true, and_mask_true); } else { constexpr static uint8_t or_mask_false[] = {0x00}; constexpr static uint8_t and_mask_false[] = {0xEF}; - RF_CALL(writeRegisterMasked(or_mask_false, and_mask_false)); + writeRegisterMasked(or_mask_false, and_mask_false); } - RF_END(); } // TODO Write timeout to RX_FWTO template -modm::ResumableResult +void modm::Dw3110Phy::setReceiveWaitTimeout(modm::chrono::micro_clock::duration duration) { - RF_BEGIN() if (duration.count() == 0) { constexpr static uint8_t or_mask[] = {0x00}; constexpr static uint8_t and_mask[] = {0xFD}; - RF_CALL(writeRegisterMasked(or_mask, and_mask)); + writeRegisterMasked(or_mask, and_mask); } else { scratch[0] = (duration.count() >> 0) & 0xFF; scratch[1] = (duration.count() >> 8) & 0xFF; scratch[2] = (duration.count() >> 16) & 0xFF; - RF_CALL(writeRegister(std::span(scratch).first<3>())); + writeRegister(std::span(scratch).first<3>()); constexpr static uint8_t or_mask[] = {0x02}; constexpr static uint8_t and_mask[] = {0xFF}; - RF_CALL(writeRegisterMasked(or_mask, and_mask)); + writeRegisterMasked(or_mask, and_mask); } - RF_END(); } template -modm::ResumableResult +void modm::Dw3110Phy::setSendHeaderFast(bool value) { - RF_BEGIN() if (value) { constexpr static uint8_t or_mask_true[] = {0x20}; constexpr static uint8_t and_mask_true[] = {0xFF}; - RF_CALL(writeRegisterMasked(or_mask_true, and_mask_true)); + writeRegisterMasked(or_mask_true, and_mask_true); } else { constexpr static uint8_t or_mask_false[] = {0x00}; constexpr static uint8_t and_mask_false[] = {0xDF}; - RF_CALL(writeRegisterMasked(or_mask_false, and_mask_false)); + writeRegisterMasked(or_mask_false, and_mask_false); } - RF_END(); } template -modm::ResumableResult +void modm::Dw3110Phy::setReenableOnRxFailure(bool value) { - RF_BEGIN() if (value) { constexpr static uint8_t or_mask_true[] = {0x04}; constexpr static uint8_t and_mask_true[] = {0xFF}; - RF_CALL(writeRegisterMasked(or_mask_true, and_mask_true)); + writeRegisterMasked(or_mask_true, and_mask_true); } else { constexpr static uint8_t or_mask_false[] = {0x00}; constexpr static uint8_t and_mask_false[] = {0xFB}; - RF_CALL(writeRegisterMasked(or_mask_false, and_mask_false)); + writeRegisterMasked(or_mask_false, and_mask_false); } - RF_END(); } template -modm::ResumableResult +void modm::Dw3110Phy::setCCATimeout(uint16_t timeout) { - RF_BEGIN(); scratch[0] = (timeout >> 0) & 0xFF; scratch[1] = (timeout >> 8) & 0xFF; - RF_CALL(writeRegister(std::span(scratch).first<2>())); - RF_END(); + writeRegister(std::span(scratch).first<2>()); } template -modm::ResumableResult +void modm::Dw3110Phy::setEnableFastTurnaround(bool value) { - RF_BEGIN() if (value) { constexpr static uint8_t or_mask_true[] = {0x04}; constexpr static uint8_t and_mask_true[] = {0xFF}; - RF_CALL(writeRegisterMasked(or_mask_true, and_mask_true)); + writeRegisterMasked(or_mask_true, and_mask_true); } else { constexpr static uint8_t or_mask_false[] = {0x00}; constexpr static uint8_t and_mask_false[] = {0xFB}; - RF_CALL(writeRegisterMasked(or_mask_false, and_mask_false)); + writeRegisterMasked(or_mask_false, and_mask_false); } - RF_END(); } template -modm::ResumableResult +uint32_t modm::Dw3110Phy::readChipTime() { - RF_BEGIN(); const static uint8_t zero[] = {0x0, 0x0, 0x0, 0x0}; - RF_CALL(writeRegister(zero)); - RF_CALL(readRegister(std::span(scratch).first<4>())); - RF_END_RETURN((uint32_t)scratch[0] | ((uint32_t)scratch[1] << 8) | - ((uint32_t)scratch[2] << 16) | ((uint32_t)scratch[3] << 24)); + writeRegister(zero); + readRegister(std::span(scratch).first<4>()); + return (uint32_t)scratch[0] | ((uint32_t)scratch[1] << 8) | + ((uint32_t)scratch[2] << 16) | ((uint32_t)scratch[3] << 24); } template -modm::ResumableResult +uint64_t modm::Dw3110Phy::getReceiveTimestamp() { - RF_BEGIN(); - RF_CALL(readRegister(std::span(scratch).first<5>())); - RF_END_RETURN((uint64_t)scratch[0] | ((uint64_t)scratch[1] << 8) | + readRegister(std::span(scratch).first<5>()); + return (uint64_t)scratch[0] | ((uint64_t)scratch[1] << 8) | ((uint64_t)scratch[2] << 16) | ((uint64_t)scratch[3] << 24) | - ((uint64_t)scratch[4] << 32)); + ((uint64_t)scratch[4] << 32); } template -modm::ResumableResult +uint64_t modm::Dw3110Phy::getTransmitTimestamp() { - RF_BEGIN(); - RF_CALL(readRegister(std::span(scratch).first<5>())); - RF_END_RETURN((uint64_t)scratch[0] | ((uint64_t)scratch[1] << 8) | + readRegister(std::span(scratch).first<5>()); + return (uint64_t)scratch[0] | ((uint64_t)scratch[1] << 8) | ((uint64_t)scratch[2] << 16) | ((uint64_t)scratch[3] << 24) | - ((uint64_t)scratch[4] << 32)); + ((uint64_t)scratch[4] << 32); } template -modm::ResumableResult +void modm::Dw3110Phy::setAcknowledgeTurnaround(uint8_t time) { - RF_BEGIN(); scratch[0] = time; - RF_CALL(writeRegister(std::span(scratch).first<1>())); - RF_END(); + writeRegister(std::span(scratch).first<1>()); } template -modm::ResumableResult +void modm::Dw3110Phy::setWaitForResponseTime(modm::PreciseClock::duration time) { - RF_BEGIN(); scratch[0] = time.count() & 0xFF; scratch[1] = (time.count() >> 8) & 0xFF; scratch[2] = (time.count() >> 16) & 0x0F; - RF_CALL(writeRegister(std::span(scratch).first<3>())); - RF_END(); + writeRegister(std::span(scratch).first<3>()); } template -modm::ResumableResult +modm::Dw3110::SystemStatus_t modm::Dw3110Phy::getStatus() { - RF_BEGIN(); - RF_CALL(fetchSystemStatus()); - RF_END_RETURN(system_status); + fetchSystemStatus(); + return system_status; } template -modm::ResumableResult +void modm::Dw3110Phy::clearStatusBits(Dw3110::SystemStatus_t mask) { - RF_BEGIN(); scratch[0] = (mask.value >> 0) & 0xFF; scratch[1] = (mask.value >> 8) & 0xFF; scratch[2] = (mask.value >> 16) & 0xFF; scratch[3] = (mask.value >> 24) & 0xFF; scratch[4] = (mask.value >> 32) & 0xFF; scratch[5] = (mask.value >> 40) & 0xFF; - RF_CALL(writeRegister(std::span(scratch).first<6>())); - RF_END(); + writeRegister(std::span(scratch).first<6>()); } template -modm::ResumableResult +modm::Dw3110::SystemState modm::Dw3110Phy::getChipState() { - RF_BEGIN(); - RF_CALL(fetchChipState()); - RF_END_RETURN(chip_state); + fetchChipState(); + return chip_state; } template -modm::ResumableResult +void modm::Dw3110Phy::loadOTP() { - RF_BEGIN(); // Check LDO and kick if not 0 - RF_CALL(readOTPMemory(otp_read)); + readOTPMemory(otp_read); if (otp_read[0] == 0 && otp_read[1] == 0 && otp_read[2] == 0 && otp_read[3] == 0) { - RF_CALL(readOTPMemory(otp_read)); + readOTPMemory(otp_read); } if (otp_read[0] != 0 || otp_read[1] != 0 || otp_read[2] != 0 || otp_read[3] != 0) // TODO find default if 0 @@ -349,249 +322,237 @@ modm::Dw3110Phy::loadOTP() // Set LDO_KICK constexpr static uint8_t or_mask[] = {0xC0}; constexpr static uint8_t and_mask[] = {0xF0}; - RF_CALL(writeRegisterMasked(or_mask, and_mask)); + writeRegisterMasked(or_mask, and_mask); } // Load from OTP according to (github.com/egnor/DW3000_notes.md)) // XTAL_TRIM = 0x13 | Len = 2, need bits 0-5 -> XTAL:0-5 - RF_CALL(readOTPMemory(otp_read)); - RF_CALL(readRegister(xtal)); + readOTPMemory(otp_read); + readRegister(xtal); if (xtal[0] & 0x1F) // TODO find default if 0 { xtal[0] = (xtal[0] & 0xE0) | (otp_read[0] & 0x1F); - RF_CALL(writeRegister(xtal)); + writeRegister(xtal); } // BIASTUNE_CAL = 0x0A | Len = ?? need bits 16-20 -> BIAS_CTRL:0-5 - RF_CALL(readOTPMemory(otp_read)); + readOTPMemory(otp_read); if (otp_read[0] != 0 || otp_read[1] != 0 || otp_read[2] != 0 || otp_read[3] != 0) // TODO find default if 0 { // Set BIAS_KICK constexpr static uint8_t or_mask[] = {0x01}; constexpr static uint8_t and_mask[] = {0xFF}; - RF_CALL(writeRegisterMasked(or_mask, and_mask)); + writeRegisterMasked(or_mask, and_mask); } // Fix incomplete bias initialization - RF_CALL(readRegister(bias_ctrl)); + readRegister(bias_ctrl); if (otp_read[2] & 0x1F) { bias_ctrl[0] = (bias_ctrl[0] & 0xE0) | (otp_read[2] & 0x1F); - RF_CALL(writeRegister(bias_ctrl)); + writeRegister(bias_ctrl); } // TODO find default if 0 - - RF_END(); } template template -modm::ResumableResult +void modm::Dw3110Phy::readOTPMemory(std::span out) { - RF_BEGIN(); constexpr static uint8_t or_mask_one[] = {0x01}; constexpr static uint8_t and_mask_one[] = {0xF1}; - RF_CALL(writeRegisterMasked(or_mask_one, and_mask_one)); - RF_CALL(writeRegisterMasked(Addr.or_mask, Addr.and_mask)); + writeRegisterMasked(or_mask_one, and_mask_one); + writeRegisterMasked(Addr.or_mask, Addr.and_mask); constexpr static uint8_t or_mask_read_2[] = {0x02}; constexpr static uint8_t and_mask_read_2[] = {0xF2}; - RF_CALL(writeRegisterMasked(or_mask_read_2, and_mask_read_2)); - RF_CALL(readRegister(out)); + writeRegisterMasked(or_mask_read_2, and_mask_read_2); + readRegister(out); constexpr static uint8_t or_mask_zero[] = {0x00}; constexpr static uint8_t and_mask_zero[] = {0xF0}; - RF_CALL(writeRegisterMasked(or_mask_zero, and_mask_zero)); - RF_END(); + writeRegisterMasked(or_mask_zero, and_mask_zero); } template -modm::ResumableResult +bool modm::Dw3110Phy::calibrate() { - RF_BEGIN(); - // Save LDO configuration - RF_CALL(readRegister(ldo_config)); + readRegister(ldo_config); // Setup calibration powersupply constexpr static uint8_t or_mask_ldo[] = {0x5, 0x1}; constexpr static uint8_t and_mask_ldo[] = {0xFF, 0xFF}; - RF_CALL(writeRegisterMasked(or_mask_ldo, and_mask_ldo)); + writeRegisterMasked(or_mask_ldo, and_mask_ldo); // Reset calibration done flag constexpr static uint8_t one[] = {0x1}; - RF_CALL(writeRegister(one)); + writeRegister(one); // Set calibration modes and set COMP_DLY to 0x2 constexpr static uint8_t rx_cal_1[] = {0x1, 0x0, 0x2, 0x0}; - RF_CALL(writeRegister(rx_cal_1)); + writeRegister(rx_cal_1); // Clear result registers constexpr static uint8_t zeros[] = {0, 0, 0, 0}; - RF_CALL(writeRegister(zeros)); - RF_CALL(writeRegister(zeros)); + writeRegister(zeros); + writeRegister(zeros); // Enable calibration constexpr static uint8_t cal_enable_or[] = {0x10}; constexpr static uint8_t cal_enable_and[] = {0xFF}; - RF_CALL(writeRegisterMasked(cal_enable_or, cal_enable_and)); + writeRegisterMasked(cal_enable_or, cal_enable_and); // Wait until calibration is done timeout.restart(10ms); - RF_CALL(readRegister(rx_cal_sts)); + readRegister(rx_cal_sts); while (rx_cal_sts[0] == 0) { - RF_YIELD(); - if (timeout.execute()) { RF_RETURN(false); } - RF_CALL(readRegister(rx_cal_sts)); + modm::this_fiber::yield(); + if (timeout.execute()) { return false; } + readRegister(rx_cal_sts); } // Setting COMP_DLY Bit 0 constexpr static uint8_t comp_dly_1_or[] = {0x1}; constexpr static uint8_t comp_dly_1_and[] = {0xFF}; - RF_CALL(writeRegisterMasked(comp_dly_1_or, comp_dly_1_and)); + writeRegisterMasked(comp_dly_1_or, comp_dly_1_and); - RF_CALL(readRegister(rx_cal_res)); + readRegister(rx_cal_res); if (rx_cal_res[0] == 0xFF && rx_cal_res[1] == 0xFF && rx_cal_res[2] == 0xFF && (rx_cal_res[3] & 0x1F) == 0x1F) { - RF_RETURN(false); + return false; } - RF_CALL(readRegister(rx_cal_res)); + readRegister(rx_cal_res); if (rx_cal_res[0] == 0xFF && rx_cal_res[1] == 0xFF && rx_cal_res[2] == 0xFF && (rx_cal_res[3] & 0x1F) == 0x1F) { - RF_RETURN(false); + return false; } // Restore LDO config - RF_CALL(writeRegister(ldo_config)); + writeRegister(ldo_config); // Reset RX_CAL constexpr static uint8_t reset_val[] = {0x0, 0x0, 0x2, 0x0}; - RF_CALL(writeRegister(reset_val)); + writeRegister(reset_val); - RF_END_RETURN(true); + return true; } template -modm::ResumableResult +void modm::Dw3110Phy::setChannel(Dw3110::Channel channel) { - RF_BEGIN(); - if (channel == Dw3110::Channel::Channel9) { // Set DGC_KICK and DGC_SEL appropriately constexpr static uint8_t or_mask[] = {0x40, 0x20}; constexpr static uint8_t and_mask[] = {0xF0, 0xFF}; - RF_CALL(writeRegisterMasked(or_mask, and_mask)); + writeRegisterMasked(or_mask, and_mask); // TODO check if 0 and load magic instead (see 8.2.4) // Magic constexpr static uint8_t rf_tx_ctrl_2_magic[] = {0x34, 0x00, 0x01, 0x1c}; - RF_CALL(writeRegister(rf_tx_ctrl_2_magic)); + writeRegister(rf_tx_ctrl_2_magic); constexpr static uint8_t pll_cfg_magic[] = {0x3c, 0x0f}; - RF_CALL(writeRegister(pll_cfg_magic)); + writeRegister(pll_cfg_magic); // TODO Set RF_RX_CTRL_HI to 0x08B5A833 for ch9 // Actually set Channel constexpr static uint8_t chan_ctrl_or[] = {0x01}; constexpr static uint8_t chan_ctrl_and[] = {0xFF}; - RF_CALL(writeRegisterMasked(chan_ctrl_or, chan_ctrl_and)); + writeRegisterMasked(chan_ctrl_or, chan_ctrl_and); } else if (channel == Dw3110::Channel::Channel5) { // Set DGC_KICK and DGC_SEL appropriately constexpr static uint8_t or_mask[] = {0x40, 0x00}; constexpr static uint8_t and_mask[] = {0xF0, 0xDF}; - RF_CALL(writeRegisterMasked(or_mask, and_mask)); + writeRegisterMasked(or_mask, and_mask); // TODO check if 0 and load magic instead (see 8.2.4) // Magic constexpr static uint8_t rf_tx_ctrl_2_magic[] = {0x34, 0x11, 0x07, 0x1c}; - RF_CALL(writeRegister(rf_tx_ctrl_2_magic)); + writeRegister(rf_tx_ctrl_2_magic); constexpr static uint8_t pll_cfg_magic[] = {0x3c, 0x1f}; - RF_CALL(writeRegister(pll_cfg_magic)); + writeRegister(pll_cfg_magic); // TODO Find default value of RF_RX_CTRL_HI for setting to ch5 // Actually set Channel constexpr static uint8_t chan_ctrl_or[] = {0x00}; constexpr static uint8_t chan_ctrl_and[] = {0xFE}; - RF_CALL(writeRegisterMasked(chan_ctrl_or, chan_ctrl_and)); + writeRegisterMasked(chan_ctrl_or, chan_ctrl_and); } constexpr static uint8_t pll_cfg_ld_magic[] = {0x81}; - RF_CALL(writeRegister(pll_cfg_ld_magic)); + writeRegister(pll_cfg_ld_magic); constexpr static uint8_t cal_enable_or[] = {0x01}; constexpr static uint8_t cal_enable_and[] = {0xFF}; - RF_CALL(writeRegisterMasked(cal_enable_or, cal_enable_and)); - RF_END_RETURN(); + writeRegisterMasked(cal_enable_or, cal_enable_and); + return; } template -modm::ResumableResult +void modm::Dw3110Phy::setPreambleCode(Dw3110::PreambleCode rx, Dw3110::PreambleCode tx) { - RF_BEGIN(); - // Read modify write pcodes into CHAN_CTRL - RF_CALL(readRegister(chan_ctrl)); + readRegister(chan_ctrl); chan_ctrl[1] = ((uint8_t)rx & 0x1F) | (chan_ctrl[1] & 0xE0); chan_ctrl[0] = (((uint8_t)tx & 0x1F) << 3) | (chan_ctrl[0] & 0x07); - RF_CALL(writeRegister(chan_ctrl)); + writeRegister(chan_ctrl); // Set RX_TUNE_EN and a magic value in THR_64 if ((uint8_t)rx > 8) { constexpr static uint8_t rx_tune_en_or[] = {0x01, 0xE4}; constexpr static uint8_t rx_tune_en_and[] = {0xFF, 0xE5}; - RF_CALL(writeRegisterMasked(rx_tune_en_or, rx_tune_en_and)); + writeRegisterMasked(rx_tune_en_or, rx_tune_en_and); } else { constexpr static uint8_t rx_tune_en_or[] = {0x00, 0xE4}; constexpr static uint8_t rx_tune_en_and[] = {0xFE, 0xE5}; - RF_CALL(writeRegisterMasked(rx_tune_en_or, rx_tune_en_and)); + writeRegisterMasked(rx_tune_en_or, rx_tune_en_and); } - RF_END_RETURN(); + return; } template -modm::ResumableResult +void modm::Dw3110Phy::setPreambleLength(Dw3110::PreambleLength plen) { - RF_BEGIN(); - // Write length to TX_FCTRL - RF_CALL(readRegister(tx_info)); + readRegister(tx_info); tx_info[1] = (tx_info[1] & 0x0F) | (((uint8_t)plen << 4) & 0xF0); - RF_CALL(writeRegister(tx_info)); + writeRegister(tx_info); // Set Preamble acquisition window to expect packets of the same configuration if (plen == Dw3110::PreambleLength::Preamble_32) { // Set PAC to be 4, also clear DTOB4 constexpr static uint8_t pac[] = {0x0F}; - RF_CALL(writeRegister(pac)); + writeRegister(pac); preamble_len = 32; pac_len = 4; } else if (plen == Dw3110::PreambleLength::Preamble_64) { //// Set PAC to be 8, also clear DTOB4 constexpr static uint8_t pac[] = {0x0C}; - RF_CALL(writeRegister(pac)); + writeRegister(pac); preamble_len = 64; pac_len = 8; } else { // Set PAC to be 16, also clear DTOB4 constexpr static uint8_t pac[] = {0x0D}; - RF_CALL(writeRegister(pac)); + writeRegister(pac); pac_len = 16; if (plen == Dw3110::PreambleLength::Preamble_128) preamble_len = 128; @@ -608,71 +569,67 @@ modm::Dw3110Phy::setPreambleLength(Dw3110::PreambleLength plen) else if (plen == Dw3110::PreambleLength::Preamble_4096) preamble_len = 4096; } - RF_CALL(setRX_SFD_TOC()); + setRX_SFD_TOC(); // Load the appropriate OPS values from OTP using OPS_KICK if (preamble_len >= 256) { constexpr static uint8_t otp_val_or[] = {0x04}; constexpr static uint8_t otp_val_and[] = {0xE7}; - RF_CALL(writeRegisterMasked(otp_val_or, otp_val_and)); + writeRegisterMasked(otp_val_or, otp_val_and); } else { constexpr static uint8_t otp_val_or[] = {0x14}; constexpr static uint8_t otp_val_and[] = {0xF7}; - RF_CALL(writeRegisterMasked(otp_val_or, otp_val_and)); + writeRegisterMasked(otp_val_or, otp_val_and); } - RF_END_RETURN(); + return; } template -modm::ResumableResult +void modm::Dw3110Phy::setSFD(Dw3110::StartFrameDelimiter sfd) { - RF_BEGIN(); - RF_CALL(readRegister(chan_ctrl)); + readRegister(chan_ctrl); chan_ctrl[0] = (chan_ctrl[0] & 0xF9) | (((uint8_t)sfd & 0x03) << 1); - RF_CALL(writeRegister(chan_ctrl)); + writeRegister(chan_ctrl); if (sfd == Dw3110::StartFrameDelimiter::Decawave_16) sfd_len = 16; else sfd_len = 8; - RF_CALL(setRX_SFD_TOC()); - RF_END_RETURN(); + setRX_SFD_TOC(); + return; } template -modm::ResumableResult +void modm::Dw3110Phy::setRX_SFD_TOC() { - RF_BEGIN(); - if (pac_len == 0 || preamble_len == 0 || sfd_len == 0) RF_RETURN(); + if (pac_len == 0 || preamble_len == 0 || sfd_len == 0) return; sfd_toc_val = preamble_len + 1 - pac_len + sfd_len; rx_sfd_toc[0] = (uint8_t)(sfd_toc_val & 0xFF); rx_sfd_toc[1] = (uint8_t)((sfd_toc_val >> 8) & 0xFF); - RF_CALL(writeRegister(rx_sfd_toc)); - RF_END_RETURN(); + writeRegister(rx_sfd_toc); + return; } template -modm::ResumableResult +uint64_t modm::Dw3110Phy::getDeviceUID() { - RF_BEGIN(); - RF_CALL(readRegister(std::span(scratch).first<8>())); - RF_END_RETURN((uint64_t)scratch[0] | ((uint64_t)scratch[1] << 8) | + readRegister(std::span(scratch).first<8>()); + return (uint64_t)scratch[0] | ((uint64_t)scratch[1] << 8) | ((uint64_t)scratch[2] << 16) | ((uint64_t)scratch[3] << 24) | ((uint64_t)scratch[4] << 32) | ((uint64_t)scratch[5] << 40) | - ((uint64_t)scratch[6] << 48) | ((uint64_t)scratch[7] << 56)); + ((uint64_t)scratch[6] << 48) | ((uint64_t)scratch[7] << 56); } template -modm::ResumableResult +void modm::Dw3110Phy::setDeviceUID(uint64_t uid) { - RF_BEGIN(); scratch[0] = (uid >> 0) & 0xFF; scratch[1] = (uid >> 8) & 0xFF; scratch[2] = (uid >> 16) & 0xFF; @@ -681,146 +638,127 @@ modm::Dw3110Phy::setDeviceUID(uint64_t uid) scratch[5] = (uid >> 40) & 0xFF; scratch[6] = (uid >> 48) & 0xFF; scratch[7] = (uid >> 56) & 0xFF; - RF_CALL(writeRegister(std::span(scratch).first<8>())); - RF_END(); + writeRegister(std::span(scratch).first<8>()); } template -modm::ResumableResult +uint16_t modm::Dw3110Phy::getPanUID() { - RF_BEGIN(); - RF_CALL(readRegister(std::span(scratch).first<2>())); - RF_END_RETURN((uint64_t)scratch[0] | ((uint64_t)scratch[1] << 8)); + readRegister(std::span(scratch).first<2>()); + return (uint64_t)scratch[0] | ((uint64_t)scratch[1] << 8); } template -modm::ResumableResult +uint16_t modm::Dw3110Phy::getShortUID() { - RF_BEGIN(); - RF_CALL(readRegister(std::span(scratch).first<2>())); - RF_END_RETURN((uint64_t)scratch[0] | ((uint64_t)scratch[1] << 8)); + readRegister(std::span(scratch).first<2>()); + return (uint64_t)scratch[0] | ((uint64_t)scratch[1] << 8); } template -modm::ResumableResult +void modm::Dw3110Phy::setPanUID(uint16_t pid) { - RF_BEGIN(); scratch[0] = (pid >> 0) & 0xFF; scratch[1] = (pid >> 8) & 0xFF; - RF_CALL(writeRegister(std::span(scratch).first<2>())); - RF_END(); + writeRegister(std::span(scratch).first<2>()); } template -modm::ResumableResult +void modm::Dw3110Phy::setShortUID(uint16_t sid) { - RF_BEGIN(); scratch[0] = (sid >> 0) & 0xFF; scratch[1] = (sid >> 8) & 0xFF; - RF_CALL(writeRegister(std::span(scratch).first<2>())); - RF_END(); + writeRegister(std::span(scratch).first<2>()); } // Enable or disable the frame filtering template -modm::ResumableResult +void modm::Dw3110Phy::setFrameFilterEnabled(bool value) { - RF_BEGIN(); if (value) { constexpr static uint8_t FFEN_or[] = {0x01}; constexpr static uint8_t FFEN_and[] = {0xFF}; - RF_CALL(writeRegisterMasked(FFEN_or, FFEN_and)); + writeRegisterMasked(FFEN_or, FFEN_and); } else { constexpr static uint8_t FFEN_or[] = {0x00}; constexpr static uint8_t FFEN_and[] = {0xFE}; - RF_CALL(writeRegisterMasked(FFEN_or, FFEN_and)); + writeRegisterMasked(FFEN_or, FFEN_and); } - RF_END(); } template -modm::ResumableResult +modm::Dw3110::FilterConfig_t modm::Dw3110Phy::getFilterConfig() { - RF_BEGIN(); - RF_CALL(readRegister(std::span(scratch).first<2>())); - RF_END_RETURN(Dw3110::FilterConfig_t(scratch[0] | scratch[1] << 8)); + readRegister(std::span(scratch).first<2>()); + return Dw3110::FilterConfig_t(scratch[0] | scratch[1] << 8); } template -modm::ResumableResult +void modm::Dw3110Phy::setFilterConfig(Dw3110::FilterConfig_t fc) { - RF_BEGIN(); scratch[0] = (fc.value & 0xFF) >> 0; scratch[1] = (fc.value & 0xFF) >> 8; - RF_CALL(writeRegister(std::span(scratch).first<2>())); - RF_END(); + writeRegister(std::span(scratch).first<2>()); } template -modm::ResumableResult +uint16_t modm::Dw3110Phy::getRXAntennaDelay() { - RF_BEGIN(); - RF_CALL(readRegister(std::span(scratch).first<2>())); - RF_END_RETURN(uint16_t(scratch[0] << 0 | scratch[1] << 8)); + readRegister(std::span(scratch).first<2>()); + return uint16_t(scratch[0] << 0 | scratch[1] << 8); } template -modm::ResumableResult +uint16_t modm::Dw3110Phy::getTXAntennaDelay() { - RF_BEGIN(); - RF_CALL(readRegister(std::span(scratch).first<2>())); - RF_END_RETURN(uint16_t(scratch[0] << 0 | scratch[1] << 8)); + readRegister(std::span(scratch).first<2>()); + return uint16_t(scratch[0] << 0 | scratch[1] << 8); } template -modm::ResumableResult +float modm::Dw3110Phy::getReceiverClockOffset() { - RF_BEGIN(); - RF_CALL(readRegister(std::span(scratch).first<2>())); - RF_END_RETURN(float(scratch[0] << 0 | (scratch[1] & 0x1F) << 8) / 67108864.0f * 1000000.0f); + readRegister(std::span(scratch).first<2>()); + return float(scratch[0] << 0 | (scratch[1] & 0x1F) << 8) / 67108864.0f * 1000000.0f; } template -modm::ResumableResult +void modm::Dw3110Phy::setAntennaDelay(uint16_t delay) { - RF_BEGIN(); scratch[0] = (delay >> 0) & 0xFF; scratch[1] = (delay >> 8) & 0xFF; - RF_CALL(writeRegister(std::span(scratch).first<2>())); - RF_CALL(writeRegister(std::span(scratch).first<2>())); - RF_END(); + writeRegister(std::span(scratch).first<2>()); + writeRegister(std::span(scratch).first<2>()); } template -modm::ResumableResult +void modm::Dw3110Phy::setAutoAckEnabled(bool value) { - RF_BEGIN(); if (value) { constexpr static uint8_t AUTO_ACK_or[] = {0x08}; constexpr static uint8_t AUTO_ACK_and[] = {0xFF}; - RF_CALL(writeRegisterMasked(AUTO_ACK_or, AUTO_ACK_and)); + writeRegisterMasked(AUTO_ACK_or, AUTO_ACK_and); } else { constexpr static uint8_t AUTO_ACK_or[] = {0x00}; constexpr static uint8_t AUTO_ACK_and[] = {0xF7}; - RF_CALL(writeRegisterMasked(AUTO_ACK_or, AUTO_ACK_and)); + writeRegisterMasked(AUTO_ACK_or, AUTO_ACK_and); } - RF_END(); } template @@ -887,22 +825,21 @@ modm::Dw3110Phy::txModeToCmdDelay() template template::DelayTXMode dmode> -modm::ResumableResult::Error> +typename modm::Dw3110Phy::Error modm::Dw3110Phy::transmitDelayed(uint32_t time, std::span payload, bool ranging, bool fast) { - RF_BEGIN(); scratch[0] = (time >> 0) & 0xFF; scratch[1] = (time >> 8) & 0xFF; scratch[2] = (time >> 16) & 0xFF; scratch[3] = (time >> 24) & 0xFF; - RF_CALL(writeRegister(std::span(scratch).first<4>())); - RF_END_RETURN_CALL(transmitGeneric()>(payload, ranging, fast)); + writeRegister(std::span(scratch).first<4>()); + return transmitGeneric()>(payload, ranging, fast); } template template::TXMode tmode> -modm::ResumableResult::Error> +typename modm::Dw3110Phy::Error modm::Dw3110Phy::transmit(std::span payload, bool ranging, bool fast) { return transmitGeneric()>(payload, ranging, fast); @@ -910,24 +847,23 @@ modm::Dw3110Phy::transmit(std::span payload, bool template template -modm::ResumableResult::Error> +typename modm::Dw3110Phy::Error modm::Dw3110Phy::transmitGeneric(std::span payload, bool ranging, bool fast) { - RF_BEGIN(); if ((long_frames && payload.size() > 1021) || (!long_frames && payload.size() > 125)) { MODM_LOG_ERROR << "Payload is too long to transmit!" << modm::endl; - RF_RETURN(Error::PayloadTooLarge); + return Error::PayloadTooLarge; } // Go idle - RF_CALL(sendCommand()); + sendCommand(); // Write payload to buffer - RF_CALL(writeRegisterBank(payload, payload.size())); + writeRegisterBank(payload, payload.size()); - RF_CALL(readRegister(tx_info)); + readRegister(tx_info); tx_info[0] = (uint8_t)(0xFF & (payload.size() + fcs_len)); // Set Payload length tx_info[1] = (tx_info[1] & 0xF0) | ((uint8_t)((payload.size() + fcs_len) >> 8) & 0x03); @@ -937,158 +873,146 @@ modm::Dw3110Phy::transmitGeneric(std::span payload tx_info[2] = 0; // Clear TXB_OFFSET tx_info[3] &= 0x3; - RF_CALL(writeRegister(tx_info)); - RF_CALL(sendCommand()); + writeRegister(tx_info); + sendCommand(); - RF_CALL(fetchSystemStatus()); - RF_CALL(fetchChipState()); + fetchSystemStatus(); + fetchChipState(); while (system_status.none(Dw3110::SystemStatus::TXFRS)) { - last_err = RF_CALL(checkTXFailed()); + last_err = checkTXFailed(); if (last_err != Error::None) { - RF_CALL(sendCommand()); - RF_CALL( - clearStatusBits(Dw3110::SystemStatus::CCA_FAIL | Dw3110::SystemStatus::HPDWARN)); - RF_RETURN(last_err); + sendCommand(); + clearStatusBits(Dw3110::SystemStatus::CCA_FAIL | Dw3110::SystemStatus::HPDWARN); + return last_err; } - RF_YIELD(); - RF_CALL(fetchSystemStatus()); - RF_CALL(fetchChipState()); + modm::this_fiber::yield(); + fetchSystemStatus(); + fetchChipState(); } // Clear TX related flags - RF_CALL(clearStatusBits(Dw3110::SystemStatus::TXFRS | Dw3110::SystemStatus::TXFRB | + clearStatusBits(Dw3110::SystemStatus::TXFRS | Dw3110::SystemStatus::TXFRB | Dw3110::SystemStatus::TXPHS | Dw3110::SystemStatus::TXPRS | - Dw3110::SystemStatus::CCA_FAIL | Dw3110::SystemStatus::HPDWARN)); - RF_END_RETURN(Error::None); + Dw3110::SystemStatus::CCA_FAIL | Dw3110::SystemStatus::HPDWARN); + return Error::None; } template -modm::ResumableResult::Error> +typename modm::Dw3110Phy::Error modm::Dw3110Phy::checkTXFailed() { - RF_BEGIN(); - if (system_status.any(Dw3110::SystemStatus::CCA_FAIL)) { RF_RETURN(Error::ChannelBusy); } + if (system_status.any(Dw3110::SystemStatus::CCA_FAIL)) { return Error::ChannelBusy; } - if (system_status.any(Dw3110::SystemStatus::HPDWARN)) { RF_RETURN(Error::DelayTooShort); } + if (system_status.any(Dw3110::SystemStatus::HPDWARN)) { return Error::DelayTooShort; } if (sys_state[0] == 0x0 && sys_state[1] == 0x0 && sys_state[2] == 0x0D && sys_state[3] == 0x0) { - RF_CALL(fetchChipState()); + fetchChipState(); if (sys_state[0] == 0x0 && sys_state[1] == 0x0 && sys_state[2] == 0x0D && sys_state[3] == 0x0) { - RF_RETURN(Error::DelayTooShort); + return Error::DelayTooShort; } } - RF_END_RETURN(Error::None); + return Error::None; } template -modm::ResumableResult +void modm::Dw3110Phy::setReferenceTime(uint32_t time) { - RF_BEGIN(); scratch[0] = (time >> 0) & 0xFF; scratch[1] = (time >> 8) & 0xFF; scratch[2] = (time >> 16) & 0xFF; scratch[3] = (time >> 24) & 0xFF; - RF_CALL(writeRegister(std::span(scratch).first<4>())); - RF_END(); + writeRegister(std::span(scratch).first<4>()); } template -modm::ResumableResult +bool modm::Dw3110Phy::startReceive() { - RF_BEGIN(); - if (RF_CALL(this->packetReady())) RF_RETURN(true); + if (this->packetReady()) return true; - RF_CALL(fetchChipState()); + fetchChipState(); if (chip_state == Dw3110::SystemState::RX || chip_state == Dw3110::SystemState::RX_WAIT) { - RF_RETURN(true); + return true; } // Go idle - RF_CALL(sendCommand()); + sendCommand(); // Clear RXFR RXPHE RXFCG and RXFCE flag - RF_CALL(clearStatusBits(Dw3110::SystemStatus::RXFR | Dw3110::SystemStatus::RXPHE | - Dw3110::SystemStatus::RXFCG | Dw3110::SystemStatus::RXFCE)); + clearStatusBits(Dw3110::SystemStatus::RXFR | Dw3110::SystemStatus::RXPHE | + Dw3110::SystemStatus::RXFCG | Dw3110::SystemStatus::RXFCE); - RF_CALL(sendCommand()); - RF_CALL(fetchChipState()); + sendCommand(); + fetchChipState(); timeout.restart(10ms); while (chip_state != Dw3110::SystemState::RX && chip_state != Dw3110::SystemState::RX_WAIT) { - RF_YIELD(); - RF_CALL(fetchChipState()); - if (timeout.execute()) { RF_RETURN(false); } + modm::this_fiber::yield(); + fetchChipState(); + if (timeout.execute()) { return false; } } - RF_END_RETURN(true); + return true; } template -modm::ResumableResult +bool modm::Dw3110Phy::fetchPacket(std::span payload, size_t &payload_len) { - RF_BEGIN(); - - RF_CALL(readRegister(rx_finfo)); + readRegister(rx_finfo); payload_len = (rx_finfo[0] | ((rx_finfo[1] & 0x03) << 8)); if (payload_len >= fcs_len) payload_len -= fcs_len; - if (payload.size() < payload_len) { RF_RETURN(false); } - RF_CALL(readRegisterBank(payload, payload_len)); + if (payload.size() < payload_len) { return false; } + readRegisterBank(payload, payload_len); // Clear most rx flags - RF_CALL(clearStatusBits(Dw3110::SystemStatus::RXFR | Dw3110::SystemStatus::RXPHE | + clearStatusBits(Dw3110::SystemStatus::RXFR | Dw3110::SystemStatus::RXPHE | Dw3110::SystemStatus::RXFCG | Dw3110::SystemStatus::RXFCE | Dw3110::SystemStatus::RXSFDD | Dw3110::SystemStatus::RXPRD | - Dw3110::SystemStatus::RXPHD | Dw3110::SystemStatus::RXFSL)); - RF_END_RETURN(true); + Dw3110::SystemStatus::RXPHD | Dw3110::SystemStatus::RXFSL); + return true; } template -modm::ResumableResult +bool modm::Dw3110Phy::packetReady() { - RF_BEGIN(); - RF_CALL(fetchSystemStatus()); - RF_END_RETURN(system_status.all(Dw3110::SystemStatus::RXFR | Dw3110::SystemStatus::RXFCG)); + fetchSystemStatus(); + return system_status.all(Dw3110::SystemStatus::RXFR | Dw3110::SystemStatus::RXFCG); } template -modm::ResumableResult +bool modm::Dw3110Phy::isReceiving() { - RF_BEGIN(); - RF_CALL(fetchChipState()); - RF_END_RETURN(chip_state == Dw3110::SystemState::RX); + fetchChipState(); + return chip_state == Dw3110::SystemState::RX; } template -modm::ResumableResult +void modm::Dw3110Phy::fetchSystemStatus() { - RF_BEGIN(); - RF_CALL(readRegister(sys_status)); + readRegister(sys_status); system_status = Dw3110::SystemStatus_t((uint64_t)sys_status[0] | ((uint64_t)sys_status[1] << 8) | ((uint64_t)sys_status[2] << 16) | ((uint64_t)sys_status[3] << 24) | ((uint64_t)sys_status[4] << 32) | ((uint64_t)sys_status[5] << 40)); - RF_END(); } template -modm::ResumableResult +void modm::Dw3110Phy::fetchChipState() { - RF_BEGIN(); - RF_CALL(readRegister(sys_state)); + readRegister(sys_state); if (sys_state[2] == 0x00) { chip_state = Dw3110::SystemState::WAKEUP; @@ -1108,75 +1032,67 @@ modm::Dw3110Phy::fetchChipState() { chip_state = Dw3110::SystemState::INVALID; } - RF_END(); } template template -modm::ResumableResult +void modm::Dw3110Phy::sendCommand() { - RF_BEGIN(); - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); tx_buffer[0] = 0x80 | (uint8_t)Cmd; Cs::setOutput(false); - RF_CALL(SpiMaster::transfer(tx_buffer.data(), nullptr, 1)); + SpiMaster::transfer(tx_buffer.data(), nullptr, 1); Cs::setOutput(true); this->releaseMaster(); - RF_END(); } template template -modm::ResumableResult +void modm::Dw3110Phy::readRegister(std::span out) { static_assert(Len <= Reg.length + Offset, "Size of read is too large for this register!"); - RF_BEGIN(); - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); tx_buffer[0] = (uint8_t)(0x40 | ((Reg.bank.addr << 1) & 0x3E) | (((Reg.offset + Offset) >> 6) & 0x01)); tx_buffer[1] = (uint8_t)(0x00 | ((Reg.offset + Offset) << 2)); Cs::setOutput(false); - RF_CALL(SpiMaster::transfer(tx_buffer.data(), nullptr, 2)); - RF_CALL(SpiMaster::transfer(nullptr, out.data(), out.size())); + SpiMaster::transfer(tx_buffer.data(), nullptr, 2); + SpiMaster::transfer(nullptr, out.data(), out.size()); Cs::setOutput(true); this->releaseMaster(); - RF_END(); } template template -modm::ResumableResult +void modm::Dw3110Phy::writeRegister(std::span val) { static_assert(Len + Offset <= Reg.length, "Size of write is too large for this register!"); - - RF_BEGIN(); - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); tx_buffer[0] = (uint8_t)(0xC0 | ((Reg.bank.addr << 1) & 0x3E) | (((Reg.offset + Offset) >> 6) & 0x01)); tx_buffer[1] = (uint8_t)(0x00 | ((Reg.offset + Offset) << 2)); Cs::setOutput(false); - RF_CALL(SpiMaster::transfer(tx_buffer.data(), nullptr, 2)); - RF_CALL(SpiMaster::transfer(val.data(), nullptr, Len)); + SpiMaster::transfer(tx_buffer.data(), nullptr, 2); + SpiMaster::transfer(val.data(), nullptr, Len); Cs::setOutput(true); this->releaseMaster(); - RF_END(); } template template -modm::ResumableResult +void modm::Dw3110Phy::readModifyWriteRegister(std::span or_mask, std::span and_mask) { @@ -1184,21 +1100,18 @@ modm::Dw3110Phy::readModifyWriteRegister(std::span(std::span(temp_rw).first())); + readRegister(std::span(temp_rw).first()); for (size_t i = 0; i < Len; i++) { temp_rw[i] |= or_mask[i]; temp_rw[i] &= and_mask[i]; } - RF_CALL(writeRegister(std::span(temp_rw).first())); - RF_END(); + writeRegister(std::span(temp_rw).first()); } template template -modm::ResumableResult +void modm::Dw3110Phy::writeRegisterMasked(std::span or_mask, std::span and_mask) { @@ -1206,9 +1119,7 @@ modm::Dw3110Phy::writeRegisterMasked(std::spanacquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); tx_buffer[0] = (uint8_t)(0xC0 | ((Reg.bank.addr << 1) & 0x3E) | (((Reg.offset + Offset) >> 6) & 0x01)); @@ -1221,169 +1132,154 @@ modm::Dw3110Phy::writeRegisterMasked(std::spanreleaseMaster(); - RF_END(); } template template -modm::ResumableResult +void modm::Dw3110Phy::readRegisterBank(std::span out, size_t len) { - RF_BEGIN(); - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); tx_buffer[0] = (uint8_t)((Reg.addr << 1) & 0x3E); Cs::setOutput(false); - RF_CALL(SpiMaster::transfer(tx_buffer.data(), nullptr, 1)); - RF_CALL(SpiMaster::transfer(nullptr, out.data(), len)); + SpiMaster::transfer(tx_buffer.data(), nullptr, 1); + SpiMaster::transfer(nullptr, out.data(), len); Cs::setOutput(true); this->releaseMaster(); - RF_END(); } template template -modm::ResumableResult +void modm::Dw3110Phy::writeRegisterBank(std::span val, size_t len) { - RF_BEGIN(); - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); tx_buffer[0] = (uint8_t)(0x80 | ((Reg.addr << 1) & 0x3E)); Cs::setOutput(false); - RF_CALL(SpiMaster::transfer(tx_buffer.data(), nullptr, 1)); - RF_CALL(SpiMaster::transfer(val.data(), nullptr, len)); + SpiMaster::transfer(tx_buffer.data(), nullptr, 1); + SpiMaster::transfer(val.data(), nullptr, len); Cs::setOutput(true); this->releaseMaster(); - RF_END(); } template -modm::ResumableResult +void modm::Dw3110Phy::setInterruptsEnabled(Dw3110::SystemStatus_t mask) { - RF_BEGIN(); scratch[0] = (mask.value >> 0) & 0xFF; scratch[1] = (mask.value >> 8) & 0xFF; scratch[2] = (mask.value >> 16) & 0xFF; scratch[3] = (mask.value >> 24) & 0xFF; scratch[4] = (mask.value >> 32) & 0xFF; scratch[5] = (mask.value >> 40) & 0xFF; - RF_CALL(writeRegister(std::span(scratch).first<6>())); - RF_END(); + writeRegister(std::span(scratch).first<6>()); } template -modm::ResumableResult +void modm::Dw3110Phy::setIRQPolarity(bool high) { - RF_BEGIN(); if (high) { constexpr static uint8_t or_mask[] = {0x20}; constexpr static uint8_t and_mask[] = {0xFF}; - RF_CALL(writeRegisterMasked(or_mask, and_mask)); + writeRegisterMasked(or_mask, and_mask); } else { constexpr static uint8_t or_mask[] = {0x00}; constexpr static uint8_t and_mask[] = {0xDF}; - RF_CALL(writeRegisterMasked(or_mask, and_mask)); + writeRegisterMasked(or_mask, and_mask); } - RF_END(); } template -modm::ResumableResult +bool modm::Dw3110Phy::setSTSLength(uint8_t len) { - RF_BEGIN(); - if (len < 3) { RF_RETURN(false); } + if (len < 3) { return false; } scratch[0] = len; - RF_CALL(writeRegister(std::span(scratch).first<1>())); - RF_END_RETURN(true); + writeRegister(std::span(scratch).first<1>()); + return true; } template -modm::ResumableResult +void modm::Dw3110Phy::setSTSMode(Dw3110::STSMode mode, bool sdc) { - RF_BEGIN(); scratch[0] = ((uint8_t)mode << 4) | (sdc ? 0x80 : 0x00); scratch[1] = 0x4F | scratch[0]; - RF_CALL(writeRegisterMasked( + writeRegisterMasked( std::span(scratch).first<1>(), - std::span(scratch).subspan<1, 1>())); - RF_END(); + std::span(scratch).subspan<1, 1>()); } template -modm::ResumableResult +uint16_t modm::Dw3110Phy::getSTSQuality() { - RF_BEGIN(); - RF_CALL(readRegister(std::span(scratch).first<2>())); - RF_END_RETURN((uint16_t)(scratch[0] << 0 | (scratch[1] & 0x0F) << 8)); + readRegister(std::span(scratch).first<2>()); + return (uint16_t)(scratch[0] << 0 | (scratch[1] & 0x0F) << 8); } template -modm::ResumableResult +bool modm::Dw3110Phy::getSTSGood() { - RF_BEGIN(); - RF_CALL(readRegister(std::span(scratch).first<1>())); - RF_CALL(readRegister(std::span(scratch).subspan<1, 2>())); - RF_END_RETURN((scratch[0] + 1) * 8.0f * 0.6f < (scratch[1] | (scratch[2] << 8))); + readRegister(std::span(scratch).first<1>()); + readRegister(std::span(scratch).subspan<1, 2>()); + return (scratch[0] + 1) * 8.0f * 0.6f < (scratch[1] | (scratch[2] << 8)); } template -modm::ResumableResult +void modm::Dw3110Phy::setSTSKey(std::span key) { return writeRegister(key); } template -modm::ResumableResult +void modm::Dw3110Phy::setSTSIV(std::span iv) { return writeRegister(iv); } template -modm::ResumableResult +void modm::Dw3110Phy::getSTSKey(std::span key) { return readRegister(key); } template -modm::ResumableResult +void modm::Dw3110Phy::getSTSIV(std::span iv) { return readRegister(iv); } template -modm::ResumableResult +uint32_t modm::Dw3110Phy::getCurrentCounter() { - RF_BEGIN(); - RF_CALL(readRegister(std::span(scratch).first<4>())); - RF_END_RETURN((uint32_t)scratch[0] | (uint32_t)scratch[1] << 8 | (uint32_t)scratch[2] << 16 | - (uint32_t)scratch[3] << 24); + readRegister(std::span(scratch).first<4>()); + return (uint32_t)scratch[0] | (uint32_t)scratch[1] << 8 | (uint32_t)scratch[2] << 16 | + (uint32_t)scratch[3] << 24; } template -modm::ResumableResult +void modm::Dw3110Phy::reloadSTSIV() { constexpr static uint8_t sts_ctrl[] = {0x01}; @@ -1391,10 +1287,10 @@ modm::Dw3110Phy::reloadSTSIV() } template -modm::ResumableResult +void modm::Dw3110Phy::reuseLastSTSIV() { constexpr static uint8_t sts_ctrl[] = {0x02}; return writeRegister(sts_ctrl); -} \ No newline at end of file +} diff --git a/src/modm/driver/radio/nrf24/nrf24.lb b/src/modm/driver/radio/nrf24/nrf24.lb index a9ac028dc5..5f056c8732 100644 --- a/src/modm/driver/radio/nrf24/nrf24.lb +++ b/src/modm/driver/radio/nrf24/nrf24.lb @@ -17,7 +17,7 @@ def init(module): def prepare(module, options): module.depends( - ":architecture:delay", + ":architecture:fiber", ":architecture:register", ":architecture:spi", ":architecture:clock", diff --git a/src/modm/driver/radio/nrf24/nrf24_config_impl.hpp b/src/modm/driver/radio/nrf24/nrf24_config_impl.hpp index aa3e5fd850..394f073b72 100644 --- a/src/modm/driver/radio/nrf24/nrf24_config_impl.hpp +++ b/src/modm/driver/radio/nrf24/nrf24_config_impl.hpp @@ -33,7 +33,7 @@ modm::Nrf24Config::setMode(Mode mode) case Mode::Rx: // MODM_LOG_DEBUG << "[nrf24] Set mode Rx" << modm::endl; // Nrf24Phy::resetCe(); -// modm::delay_us(30); +// modm::this_fiber::sleep_for(30us); Nrf24Phy::flushRxFifo(); Nrf24Phy::setBits(NrfRegister::CONFIG, Config::PRIM_RX); diff --git a/src/modm/driver/radio/nrf24/nrf24_phy.hpp b/src/modm/driver/radio/nrf24/nrf24_phy.hpp index 6c0d3c3d93..b57b17ea6c 100644 --- a/src/modm/driver/radio/nrf24/nrf24_phy.hpp +++ b/src/modm/driver/radio/nrf24/nrf24_phy.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include "nrf24_definitions.hpp" diff --git a/src/modm/driver/radio/nrf24/nrf24_phy_impl.hpp b/src/modm/driver/radio/nrf24/nrf24_phy_impl.hpp index e75123e5eb..d851496570 100644 --- a/src/modm/driver/radio/nrf24/nrf24_phy_impl.hpp +++ b/src/modm/driver/radio/nrf24/nrf24_phy_impl.hpp @@ -26,7 +26,7 @@ modm::Nrf24Phy::writeCommandNoData(Command_t cmd) { Csn::reset(); - status = Spi::transferBlocking(cmd.value); + status = Spi::transfer(cmd.value); Csn::set(); } @@ -39,9 +39,9 @@ modm::Nrf24Phy::writeCommandSingleData(Command_t cmd, uint8_t data { Csn::reset(); - status = Spi::transferBlocking(cmd.value); + status = Spi::transfer(cmd.value); - uint8_t ret = Spi::transferBlocking(data); + uint8_t ret = Spi::transfer(data); Csn::set(); @@ -56,11 +56,11 @@ modm::Nrf24Phy::writeCommandMultiData(Command_t cmd, uint8_t* argv { Csn::reset(); - status = Spi::transferBlocking(cmd.value); + status = Spi::transfer(cmd.value); for(uint8_t i = 0; i < argc; i++) { uint8_t arg = (argv == nullptr) ? 0 : argv[i]; - uint8_t data = Spi::transferBlocking(arg); + uint8_t data = Spi::transfer(arg); if(retv != nullptr) { retv[i] = data; @@ -320,7 +320,7 @@ modm::Nrf24Phy::pulseCe() Ce::toggle(); // delay might not be precise enough - modm::delay_us(15); + modm::this_fiber::sleep_for(15us); Ce::toggle(); } diff --git a/src/modm/driver/radio/sx1276.hpp b/src/modm/driver/radio/sx1276.hpp index 2854d2fb5a..23c7eb373e 100644 --- a/src/modm/driver/radio/sx1276.hpp +++ b/src/modm/driver/radio/sx1276.hpp @@ -28,7 +28,7 @@ namespace modm * @ingroup modm_driver_sx1276 */ template -class Sx1276 : public sx1276, public modm::SpiDevice, protected modm::NestedResumable<6> +class Sx1276 : public sx1276, public modm::SpiDevice { public: /** @@ -39,7 +39,7 @@ class Sx1276 : public sx1276, public modm::SpiDevice, protected modm: /** * \brief Initialize the modem into Lora mode */ - modm::ResumableResult + void initialize(); /** @@ -51,7 +51,7 @@ class Sx1276 : public sx1276, public modm::SpiDevice, protected modm: * \param implicitHeader Run the modem in implicit header mode * \param payloadCrc Append CRC checksums to validate the packages */ - modm::ResumableResult + void setModemParams( Bandwidth bw, SpreadingFactor sf, CodingRate cr, @@ -63,7 +63,7 @@ class Sx1276 : public sx1276, public modm::SpiDevice, protected modm: * * \param freq The Carrier frequency the modem will be tuned to */ - modm::ResumableResult + void setCarrierFrequency(frequency_t freq); /** @@ -71,7 +71,7 @@ class Sx1276 : public sx1276, public modm::SpiDevice, protected modm: * * \param syncWord The new sync word of upcoming lora packages */ - modm::ResumableResult + void setSyncWord(uint8_t syncWord); /** @@ -81,19 +81,19 @@ class Sx1276 : public sx1276, public modm::SpiDevice, protected modm: * \param length The length of the payload data in bytes * \return Returns true if transmission has started. False if the buffer was already sending */ - modm::ResumableResult + bool transmit(uint8_t* data, uint8_t length); /** * \brief Enable the Rx mode and start listening for packages */ - modm::ResumableResult + void enableListening(); /** * \brief Disable Rx mode */ - modm::ResumableResult + void disableListening(); /** @@ -106,7 +106,7 @@ class Sx1276 : public sx1276, public modm::SpiDevice, protected modm: * \param maxLength The maximum amount of bytes that can be read * \return The amount of bytes in the last package */ - modm::ResumableResult + uint8_t readPacket(uint8_t* data, uint8_t maxLength); /** @@ -114,7 +114,7 @@ class Sx1276 : public sx1276, public modm::SpiDevice, protected modm: * * \return The raw signal to noise ratio in dB (has to be divided by 4 for the actual value) */ - modm::ResumableResult + int8_t getPacketSnr(); /** @@ -122,7 +122,7 @@ class Sx1276 : public sx1276, public modm::SpiDevice, protected modm: * * \return The RSSI value of the last received packet */ - modm::ResumableResult + int16_t getPacketRssi(); /** @@ -130,7 +130,7 @@ class Sx1276 : public sx1276, public modm::SpiDevice, protected modm: * * \return The RSSI value of the last received packet */ - modm::ResumableResult + int16_t getCurrentRssi(); private: @@ -160,19 +160,19 @@ class Sx1276 : public sx1276, public modm::SpiDevice, protected modm: int16_t tempRssi; /// Reads a single register - modm::ResumableResult + uint8_t readRegister(Sx1276Register reg); /// Writes a single register with a given value - modm::ResumableResult + void writeRegister(Sx1276Register reg, uint8_t value); /// Transfers the buffer to the SPI bus - modm::ResumableResult + void transferBuffer(uint8_t length); /// change the operation mode of the modem - modm::ResumableResult + void changeMode(ModemMode mode); }; diff --git a/src/modm/driver/radio/sx1276.lb b/src/modm/driver/radio/sx1276.lb index 156f21f588..5004d45bad 100644 --- a/src/modm/driver/radio/sx1276.lb +++ b/src/modm/driver/radio/sx1276.lb @@ -17,13 +17,9 @@ def init(module): def prepare(module, options): module.depends( - ":architecture:delay", ":architecture:register", - ":architecture:spi", ":architecture:spi.device", - ":architecture:clock", - ":debug", - ":processing:timer") + ":debug") return True def build(env): diff --git a/src/modm/driver/radio/sx1276_impl.hpp b/src/modm/driver/radio/sx1276_impl.hpp index ef41310150..a7380ce75a 100644 --- a/src/modm/driver/radio/sx1276_impl.hpp +++ b/src/modm/driver/radio/sx1276_impl.hpp @@ -36,39 +36,33 @@ Sx1276::Sx1276() // ----------------------------------------------------------------------------- template -ResumableResult +void Sx1276::initialize() { - RF_BEGIN(); - // set the mode to sleep - RF_CALL(changeMode(ModemMode::SLEEP)); + changeMode(ModemMode::SLEEP); // change to the Lora mode opModeShadow.set(OpModeRegister::LoraMode); opModeShadow.reset(OpModeRegister::AccessSharedReg); opModeShadow.set(OpModeRegister::LowFrequencyMode); - RF_CALL(writeRegister(Sx1276Register::OpMode,opModeShadow.value)); + writeRegister(Sx1276Register::OpMode,opModeShadow.value); //mask out all interrupts irqMaskShadow.value = 0xFF; - RF_CALL(writeRegister(Sx1276Register::IrqFlagsMask,irqMaskShadow.value)); + writeRegister(Sx1276Register::IrqFlagsMask,irqMaskShadow.value); // set the mode back to standby - RF_CALL(changeMode(ModemMode::STDBY)); - - RF_END(); + changeMode(ModemMode::STDBY); } // ----------------------------------------------------------------------------- template -modm::ResumableResult +void Sx1276::setCarrierFrequency(frequency_t freq) { - RF_BEGIN(); - frfShadow = static_cast(freq * (static_cast(1<<19) / static_cast(osc_freq))); // do a burst write to the three frequency registers instead of seperate register write calls @@ -77,22 +71,19 @@ Sx1276::setCarrierFrequency(frequency_t freq) buffer[2] = static_cast((frfShadow >> 8) & 0xFF); buffer[3] = static_cast(frfShadow & 0xFF); - RF_CALL(transferBuffer(4)); - - RF_END(); + transferBuffer(4); } // ----------------------------------------------------------------------------- template -modm::ResumableResult +void Sx1276::setModemParams( Bandwidth bw, SpreadingFactor sf, CodingRate cr, bool implicitHeader, bool payloadCrc) { - RF_BEGIN(); //configure the modem config shadow registers Bandwidth_t::set(modemConf1Shadow,bw); CodingRate_t::set(modemConf1Shadow,cr); @@ -119,148 +110,134 @@ Sx1276::setModemParams( Bandwidth bw, } // Write out the config registers - RF_CALL(writeRegister(Sx1276Register::ModemConfig1,modemConf1Shadow.value)); - RF_CALL(writeRegister(Sx1276Register::ModemConfig2,modemConf2Shadow.value)); - RF_CALL(writeRegister(Sx1276Register::ModemConfig3,modemConf3Shadow.value)); + writeRegister(Sx1276Register::ModemConfig1,modemConf1Shadow.value); + writeRegister(Sx1276Register::ModemConfig2,modemConf2Shadow.value); + writeRegister(Sx1276Register::ModemConfig3,modemConf3Shadow.value); //Configure the optimizations if(sf == SpreadingFactor::SF_6) { - RF_CALL(writeRegister(Sx1276Register::DetectOptimize,0x05)); - RF_CALL(writeRegister(Sx1276Register::DetectionThreshold,0x0C)); + writeRegister(Sx1276Register::DetectOptimize,0x05); + writeRegister(Sx1276Register::DetectionThreshold,0x0C); } else { - RF_CALL(writeRegister(Sx1276Register::DetectOptimize,0x03)); - RF_CALL(writeRegister(Sx1276Register::DetectionThreshold,0x0A)); + writeRegister(Sx1276Register::DetectOptimize,0x03); + writeRegister(Sx1276Register::DetectionThreshold,0x0A); } - - RF_END(); } // ----------------------------------------------------------------------------- template -modm::ResumableResult +void Sx1276::setSyncWord(uint8_t syncWord) { - RF_BEGIN(); - RF_CALL(writeRegister(Sx1276Register::SyncWord,syncWord)); - RF_END(); + writeRegister(Sx1276Register::SyncWord,syncWord); } // ----------------------------------------------------------------------------- template -modm::ResumableResult +bool Sx1276::transmit(uint8_t* data, uint8_t length) { - RF_BEGIN(); - lastTransmitResult = false; //read the mode register and check if we're in the RX Mode - opModeShadow.value = RF_CALL(readRegister(Sx1276Register::OpMode)); + opModeShadow.value = readRegister(Sx1276Register::OpMode); if(ModemMode_t::get(opModeShadow) == ModemMode::STDBY) { - RF_CALL(writeRegister(Sx1276Register::PayloadLength,length)); - RF_CALL(writeRegister(Sx1276Register::FifoTxBaseAddr,0x00)); - RF_CALL(writeRegister(Sx1276Register::FifoAddrPtr,0x00)); + writeRegister(Sx1276Register::PayloadLength,length); + writeRegister(Sx1276Register::FifoTxBaseAddr,0x00); + writeRegister(Sx1276Register::FifoAddrPtr,0x00); - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); buffer[0] = 0x80 | static_cast(Sx1276Register::Fifo); Cs::reset(); - RF_CALL(SpiMaster::transfer(buffer,nullptr,1)); - RF_CALL(SpiMaster::transfer(data,nullptr,length)); + SpiMaster::transfer(buffer,nullptr,1); + SpiMaster::transfer(data,nullptr,length); if(this->releaseMaster()) { Cs::set(); } - RF_CALL(changeMode(ModemMode::TX)); + changeMode(ModemMode::TX); } else { MODM_LOG_ERROR<<"SX1276: Transmission failed as the modem is busy"< -modm::ResumableResult +void Sx1276::enableListening() { - RF_BEGIN(); //enable the rx interrupts irqMaskShadow.reset(Interrupts::RX_DONE); - RF_CALL(writeRegister(Sx1276Register::FifoAddrPtr,0x00)); - RF_CALL(writeRegister(Sx1276Register::FifoRxBaseAddr,0x00)); + writeRegister(Sx1276Register::FifoAddrPtr,0x00); + writeRegister(Sx1276Register::FifoRxBaseAddr,0x00); - RF_CALL(writeRegister(Sx1276Register::IrqFlagsMask,irqMaskShadow.value)); - RF_CALL(writeRegister(Sx1276Register::IrqFlags, 0xFF)); + writeRegister(Sx1276Register::IrqFlagsMask,irqMaskShadow.value); + writeRegister(Sx1276Register::IrqFlags, 0xFF); - RF_CALL(changeMode(ModemMode::RXCONTINOUS)); - RF_END(); + changeMode(ModemMode::RXCONTINOUS); } // ----------------------------------------------------------------------------- template -modm::ResumableResult +void Sx1276::disableListening() { - RF_BEGIN(); - irqMaskShadow.set(Interrupts::RX_DONE); - RF_CALL(writeRegister(Sx1276Register::IrqFlagsMask,irqMaskShadow.value)); + writeRegister(Sx1276Register::IrqFlagsMask,irqMaskShadow.value); - RF_CALL(changeMode(ModemMode::SLEEP)); - - RF_END(); + changeMode(ModemMode::SLEEP); } // ----------------------------------------------------------------------------- template -modm::ResumableResult +uint8_t Sx1276::readPacket(uint8_t* data, uint8_t maxLength) { - RF_BEGIN(); - lastPacketSize = 0; //read the mode register and check if we're in the RX Mode - opModeShadow.value = RF_CALL(readRegister(Sx1276Register::OpMode)); + opModeShadow.value = readRegister(Sx1276Register::OpMode); if(ModemMode_t::get(opModeShadow) == ModemMode::RXCONTINOUS) { - irqFlags.value = RF_CALL(readRegister(Sx1276Register::IrqFlags)); + irqFlags.value = readRegister(Sx1276Register::IrqFlags); if(irqFlags & Interrupts::RX_DONE) { - lastPacketSize = RF_CALL(readRegister(Sx1276Register::RxNbBytes)); - RF_WAIT_UNTIL(this->acquireMaster()); + lastPacketSize = readRegister(Sx1276Register::RxNbBytes); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); buffer[0] = static_cast(Sx1276Register::Fifo); Cs::reset(); - RF_CALL(SpiMaster::transfer(buffer,nullptr,1)); + SpiMaster::transfer(buffer,nullptr,1); if(lastPacketSize > maxLength) { MODM_LOG_ERROR<<"SX1276: Read buffer is too small, packet discarded!"<releaseMaster()) @@ -268,7 +245,7 @@ Sx1276::readPacket(uint8_t* data, uint8_t maxLength) Cs::set(); } - RF_CALL(writeRegister(Sx1276Register::IrqFlags, static_cast(Interrupts::RX_DONE))); + writeRegister(Sx1276Register::IrqFlags, static_cast(Interrupts::RX_DONE)); } } @@ -277,108 +254,91 @@ Sx1276::readPacket(uint8_t* data, uint8_t maxLength) MODM_LOG_ERROR<<"SX1276: ReadPacket can only called when enabling listening first!"< -modm::ResumableResult +int8_t Sx1276::getPacketSnr() { - RF_BEGIN(); - RF_CALL(readRegister(Sx1276Register::PktSnrValue)); - RF_END_RETURN(static_cast(buffer[1])); + readRegister(Sx1276Register::PktSnrValue); + return static_cast(buffer[1]); } // ----------------------------------------------------------------------------- template -modm::ResumableResult +int16_t Sx1276::getPacketRssi() { - RF_BEGIN(); - tempRssi = static_cast(RF_CALL(readRegister(Sx1276Register::PktRssiValue))); + tempRssi = static_cast(readRegister(Sx1276Register::PktRssiValue)); tempRssi += rssiOffsetLF; - RF_END_RETURN(tempRssi); + return tempRssi; } // ----------------------------------------------------------------------------- template -modm::ResumableResult +int16_t Sx1276::getCurrentRssi() { - RF_BEGIN(); - tempRssi = static_cast(RF_CALL(readRegister(Sx1276Register::RssiValue))); + tempRssi = static_cast(readRegister(Sx1276Register::RssiValue)); tempRssi += rssiOffsetLF; - RF_END_RETURN(tempRssi); + return tempRssi; } // ----------------------------------------------------------------------------- template -modm::ResumableResult +void Sx1276::transferBuffer(uint8_t length) { - RF_BEGIN(); - - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); - RF_CALL(SpiMaster::transfer(buffer,buffer,length)); + SpiMaster::transfer(buffer,buffer,length); if(this->releaseMaster()) { Cs::set(); } - - RF_END(); } // ----------------------------------------------------------------------------- template -modm::ResumableResult +void Sx1276::writeRegister(Sx1276Register reg, uint8_t value) { - RF_BEGIN(); - buffer[0] = 0x80 | static_cast(reg); buffer[1] = value; - RF_CALL(transferBuffer(2)); - - RF_END(); + transferBuffer(2); } // ----------------------------------------------------------------------------- template -modm::ResumableResult +uint8_t Sx1276::readRegister(Sx1276Register reg) { - RF_BEGIN(); - buffer[0] = static_cast(reg); buffer[1] = 0x00; - RF_CALL(transferBuffer(2)); + transferBuffer(2); - RF_END_RETURN(buffer[1]); + return buffer[1]; } // ----------------------------------------------------------------------------- template -modm::ResumableResult +void Sx1276::changeMode(ModemMode mode) { - RF_BEGIN(); - ModemMode_t::set(opModeShadow,mode); - RF_CALL(writeRegister(Sx1276Register::OpMode,opModeShadow.value)); - - RF_END(); + writeRegister(Sx1276Register::OpMode,opModeShadow.value); } -} \ No newline at end of file +} diff --git a/src/modm/driver/radio/sx128x.hpp b/src/modm/driver/radio/sx128x.hpp index 4e81862a13..242e170ef3 100644 --- a/src/modm/driver/radio/sx128x.hpp +++ b/src/modm/driver/radio/sx128x.hpp @@ -16,8 +16,6 @@ #include "sx128x_definitions.hpp" #include "sx128x_transport.hpp" -#include - namespace modm { @@ -42,193 +40,193 @@ class Sx128x : public sx128x, public Transport /// Reset the transceiver /// @attention It is necessary to reset each radio on a shared bus prior to starting the first SPI communicatio - modm::ResumableResult + void reset(); public: /// Get the transceiver status directly. /// @attention this command can be issued at any time as long as it is not the very first command send over the interface - modm::ResumableResult + bool getStatus(Status *status); /// Writes a single byte in a data memory space at the specified address - modm::ResumableResult + bool writeRegister(Register reg, uint8_t data); /// Writes a block of bytes in a data memory space starting at a specific address - modm::ResumableResult + bool writeRegister(Register reg, std::span data); /// Read a single byte of data at the given address - modm::ResumableResult + bool readRegister(Register reg, uint8_t *data); /// Read a block of data starting at a given address - modm::ResumableResult + bool readRegister(Register reg, std::span data); /// This function is used to write the data payload to be transmitted /// @attention When the address exceeds 255 it wraps back to 0 due to the circular nature of data buffer - modm::ResumableResult + bool writeBuffer(uint8_t offset, std::span data); /// This function is used to read the received data payload - modm::ResumableResult + bool readBuffer(uint8_t offset, std::span data); /// Set the transceiver to Sleep mode with the lowest current consumption possible /// @warning Depending on the specified level of memory retention not all instruction will be retained - modm::ResumableResult + bool setSleep(SleepConfig_t sleepConfig); /// Set the device in either STDBY_RC or STDBY_XOSC mode which are intermediate levels of power consumption /// By default in this state, the system is clocked by the 13 MHz RC oscillator to reduce power consumption. /// However if the application is time critical, the XOSC block can be turned or left ON. /// @attention In this Standby mode, the transceiver may be configured for future RF operations - modm::ResumableResult + bool setStandby(StandbyConfig standbyConfig = StandbyConfig::StdbyRc); /// Set the device in Frequency Synthesizer mode where the PLL is locked to the carrier frequency /// @attention In normal operation of the transceiver, the user does not normally need to use FS mode - modm::ResumableResult + bool setFs(); /// Set the device in Transmit mode /// @warning The IRQ status must be cleared before using this command - modm::ResumableResult + bool setTx(PeriodBase periodBase, uint16_t periodBaseCount); /// Set the device in Receiver mode /// @warning The IRQ status must be cleared before using this command /// @attention Setting periodBaseCount = 0 puts the transceiver in RX Single Mode /// @attention Setting periodBaseCount = 0xFFFF puts the transceiver in Rx Continuous mode - modm::ResumableResult + bool setRx(PeriodBase periodBase, uint16_t periodBaseCount); /// Set the transceiver in sniff mode, so that it regularly looks for new packets /// @warning RxDone interrupt must be configured prior to enter Duty cycled operations /// @warning SetLongPreamble must be issued prior to setRxDutyCycle - modm::ResumableResult + bool setRxDutyCycle(PeriodBase periodBase, uint16_t rxPeriodBaseCount, uint16_t sleepPeriodBaseCount); /// Set the transceiver to use CAD (Channel Activity Detection) mode /// @warning The Channel Activity Detection is a LoRa specific mode of operation - modm::ResumableResult + bool setCad(); /// Set the transceiver to generate a Continuous Wave (RF tone) at a selected frequency and output power /// @attention The device remains in Tx Continuous Wave until the host sends a mode confiquration command. - modm::ResumableResult + bool setTxContinuousWave(); /// Set the transceiver to generate an infinite sequence of alternating 'O's and '1's in GFSK modulation and symbol 0 in LoRa /// @attention The device remains in transmit until the host sends a mode confiquration command - modm::ResumableResult + bool setTxContinuousPreamble(); /// Set the transceiver radio frame out of a choice of 5 different packet types /// @attention the packet type must be set first in the radio configuration sequence - modm::ResumableResult + bool setPacketType(PacketType packetType); /// Get the current operating packet type of the radio - modm::ResumableResult + bool getPacketType(PacketType *packetType); /// Set the frequency of the RF frequency mode. /// @warning This must be called after SetPacket type. /// @attention The LSB of rfFrequency is equal to the PLL step i.e. 52e6/2^18 Hz, where 52e6 is the crystal frequency in Hz - modm::ResumableResult + bool setRfFrequency(uint32_t rfFrequency); /// Set the Tx output power and the Tx ramp time /// @attention The output power (Pout) is defined by the parameter power. Pout = -18 + power /// @attention PoutMin = -18 dBm (power = 0) and PoutMax = 13 dBm (power = 31) - modm::ResumableResult + bool setTxParams(uint8_t power, RampTime rampTime); /// Set the number of symbols on which which Channel Activity Detected (CAD) operates - modm::ResumableResult + bool setCadParams(CadSymbolNumber cadSymbolNumber); /// Set the base address for the packet handling operation in Tx and Rx mode for all packet types - modm::ResumableResult + bool setBufferBaseAddress(uint8_t txBaseAddress, uint8_t rxBaseAddress); /// Set the modulation parameters of the radio /// @attention The modulation parameters will be interpreted depending on the frame type - modm::ResumableResult + bool setModulationParams(ModulationParams modulationParams); /// Set the parameters of the packet handling block /// @warning Interpretation by the transceiver of the packet parameters depends upon the chosen packet type - modm::ResumableResult + bool setPacketParams(PacketParams packetParams); /// Get length of the last received packet and the address of the first byte received - modm::ResumableResult + bool getRxBufferStatus(RxBufferStatus *rxBufferStatus); /// Retrieve information about the last received packet /// @attention The returned parameters are frame-dependent /// @attention Actual signal power is -(rssiSync) / 2 (dBm) /// @attention Actual SNR is (snr) / 4 (dBm) - modm::ResumableResult + bool getPacketStatus(PacketStatus *packetStatus); /// Get the instantaneous RSSI value during reception of the packet /// @attention Actual signal power is -(rssiSync) / 2 (dBm) - modm::ResumableResult + bool getRssiInst(uint8_t *rssiInst); /// Enable IRQs and to route IRQs to DIO pins. - modm::ResumableResult + bool setDioIrqParams(Irq_t irqMask, Irq_t dio1Mask = Irq_t(), Irq_t dio2Mask = Irq_t(), Irq_t dio3Mask = Irq_t()); /// Get the value of the IRQ register - modm::ResumableResult + bool getIrqStatus(Irq_t *irqStatus); /// Clear IRQ flags in the IRQ register - modm::ResumableResult + bool clearIrqStatus(Irq_t irqMask); /// Specify if DC-DC or LDO is used for power regulation - modm::ResumableResult + bool setRegulatorMode(RegulatorMode regModeParam); /// Save the present context of the radio register values to the Data RAM - modm::ResumableResult + bool setSaveContext(); /// Set the chip so that the state following a Rx or Tx operation is FS and not STDBY /// This feature is to be used to reduce the switching time between consecutive Rx and/or Tx operations - modm::ResumableResult + bool setAutoFs(bool enable = true); /// Set the device to be able to send back a response 150 us after a packet reception /// @attention The delay between the packet reception end and the next packet transmission start is defined by TxDelay = time + 33 us - modm::ResumableResult + bool setAutoTx(uint16_t time); /// Set the transceiver into Long Preamble mode /// @warning Long Preamble mode can only be used with either LoRa mode and GFSK mode - modm::ResumableResult + bool setLongPreamble(bool enable = true); /// Set UART speed /// @warning UART only /// @attention UART communication must be initiated with 115.2 kbps /// @attention The UartDividerRatio is given by UartDividerRatio = (Baud Rate * 2^20) / fClk - modm::ResumableResult + bool setUartSpeed(UartDividerRatio uartDividerRatio); /// Set the role of the radio in ranging operation - modm::ResumableResult + bool setRangingRole(RangingRole rangingRole); /// Enable advanced ranging - modm::ResumableResult + bool setAdvancedRanging(bool enable = true); public: @@ -236,7 +234,6 @@ class Sx128x : public sx128x, public Transport static constexpr float frequencyLsb = float(52_MHz) / 262144.f; private: - ShortPreciseTimeout timeout; uint8_t buffer[8]; }; @@ -244,4 +241,4 @@ class Sx128x : public sx128x, public Transport #include "sx128x_impl.hpp" -#endif \ No newline at end of file +#endif diff --git a/src/modm/driver/radio/sx128x.lb b/src/modm/driver/radio/sx128x.lb index f54c22ba18..f9604f2487 100644 --- a/src/modm/driver/radio/sx128x.lb +++ b/src/modm/driver/radio/sx128x.lb @@ -27,8 +27,7 @@ def prepare(module, options): ":architecture:spi.device", ":architecture:uart.device", ":math:utils", - ":processing:resumable", - ":processing:timer") + ":architecture:fiber") return True def build(env): diff --git a/src/modm/driver/radio/sx128x_impl.hpp b/src/modm/driver/radio/sx128x_impl.hpp index 251041eea8..600e341f5a 100644 --- a/src/modm/driver/radio/sx128x_impl.hpp +++ b/src/modm/driver/radio/sx128x_impl.hpp @@ -31,26 +31,20 @@ Sx128x< Transport, Reset, Busy >::isBusy() // ---------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +void Sx128x< Transport, Reset, Busy >::reset() { - RF_BEGIN(); - Reset::setOutput(modm::Gpio::Low); - timeout.restart(std::chrono::milliseconds(50)); - RF_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(50ms); Reset::setOutput(modm::Gpio::High); - timeout.restart(std::chrono::milliseconds(20)); - RF_WAIT_UNTIL(timeout.isExpired()); - - RF_END(); + modm::this_fiber::sleep_for(20ms); } // ---------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::getStatus(sx128x::Status *status) { // The GetStatus command can be issued at any time @@ -60,7 +54,7 @@ Sx128x< Transport, Reset, Busy >::getStatus(sx128x::Status *status) // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::writeRegister(Register reg, uint8_t data) { buffer[0] = data; @@ -70,22 +64,20 @@ Sx128x< Transport, Reset, Busy >::writeRegister(Register reg, uint8_t data) // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::writeRegister(Register reg, std::span data) { - RF_BEGIN(); - buffer[0] = (i(reg) >> 8) & 0xFF; buffer[1] = i(reg) & 0xFF; - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommand(Command(Opcode::WriteRegister, std::span{buffer, 2}), data)); + return this->writeCommand(Command(Opcode::WriteRegister, std::span{buffer, 2}), data); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::readRegister(Register reg, uint8_t *value) { return this->readRegister(reg, std::span{value, 1}); @@ -94,358 +86,310 @@ Sx128x< Transport, Reset, Busy >::readRegister(Register reg, uint8_t *value) // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::readRegister(Register reg, std::span data) { - RF_BEGIN(); - buffer[0] = (i(reg) >> 8) & 0xFF; buffer[1] = i(reg) & 0xFF; - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->readCommand(Command(Opcode::ReadRegister, std::span{buffer, 2}), data)); + return this->readCommand(Command(Opcode::ReadRegister, std::span{buffer, 2}), data); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::writeBuffer(uint8_t offset, std::span data) { - RF_BEGIN(); - buffer[0] = offset; - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommand(Command(Opcode::WriteBuffer, std::span{buffer, 1}), data)); + return this->writeCommand(Command(Opcode::WriteBuffer, std::span{buffer, 1}), data); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::readBuffer(uint8_t offset, std::span data) { - RF_BEGIN(); - buffer[0] = offset; - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->readCommand(Command(Opcode::ReadBuffer, std::span{buffer, 1}), data)); + return this->readCommand(Command(Opcode::ReadBuffer, std::span{buffer, 1}), data); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setSleep(SleepConfig_t sleepConfig) { - RF_BEGIN(); - buffer[0] = sleepConfig.value; - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommand(Opcode::SetSleep, std::span{buffer, 1})); + return this->writeCommand(Opcode::SetSleep, std::span{buffer, 1}); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setStandby(StandbyConfig standbyConfig) { - RF_BEGIN(); - buffer[0] = i(standbyConfig); - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommand(Opcode::SetStandby, std::span{buffer, 1})); + return this->writeCommand(Opcode::SetStandby, std::span{buffer, 1}); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setFs() { - RF_BEGIN(); - - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommandSingleData(Opcode::SetFs)); + return this->writeCommandSingleData(Opcode::SetFs); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setTx(PeriodBase periodBase, uint16_t periodBaseCount) { - RF_BEGIN(); - buffer[0] = i(periodBase); buffer[1] = (periodBaseCount >> 8) & 0xFF; buffer[2] = periodBaseCount & 0xFF; - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommand(Opcode::SetTx, std::span{buffer, 3})); + return this->writeCommand(Opcode::SetTx, std::span{buffer, 3}); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setRx(PeriodBase periodBase, uint16_t periodBaseCount) { - RF_BEGIN(); - buffer[0] = i(periodBase); buffer[1] = (periodBaseCount >> 8) & 0xFF; buffer[2] = periodBaseCount & 0xFF; - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommand(Opcode::SetRx, std::span{buffer, 3}) ); + return this->writeCommand(Opcode::SetRx, std::span{buffer, 3}); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setRxDutyCycle(PeriodBase periodBase, uint16_t rxPeriodBaseCount, uint16_t sleepPeriodBaseCount) { - RF_BEGIN(); - buffer[0] = i(periodBase); buffer[1] = (rxPeriodBaseCount >> 8) & 0xFF; buffer[2] = rxPeriodBaseCount & 0xFF; buffer[3] = (sleepPeriodBaseCount >> 8) & 0xFF; buffer[4] = sleepPeriodBaseCount & 0xFF; - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommand(Opcode::SetRxDutyCycle, std::span{buffer, 5})); + return this->writeCommand(Opcode::SetRxDutyCycle, std::span{buffer, 5}); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setCad() { - RF_BEGIN(); - - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommandSingleData(Opcode::SetCad)); + return this->writeCommandSingleData(Opcode::SetCad); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setTxContinuousWave() { - RF_BEGIN(); - - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommandSingleData(Opcode::SetTxContinuousWave)); + return this->writeCommandSingleData(Opcode::SetTxContinuousWave); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setTxContinuousPreamble() { - RF_BEGIN(); - - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommandSingleData(Opcode::SetTxContinuousPreamble)); + return this->writeCommandSingleData(Opcode::SetTxContinuousPreamble); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setPacketType(PacketType packetType) { - RF_BEGIN(); - buffer[0] = i(packetType); - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommand(Opcode::SetPacketType, std::span{buffer, 1})); + return this->writeCommand(Opcode::SetPacketType, std::span{buffer, 1}); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::getPacketType(PacketType *packetType) { - RF_BEGIN(); - - RF_WAIT_WHILE(isBusy()); - if (RF_CALL(this->readCommand(Opcode::GetPacketType, std::span{(uint8_t *) packetType, 1}))) + modm::this_fiber::poll([&]{ return not isBusy(); }); + if (this->readCommand(Opcode::GetPacketType, std::span{(uint8_t *) packetType, 1})) { *packetType = PacketType(buffer[0]); - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setRfFrequency(uint32_t rfFrequency) { - RF_BEGIN(); - buffer[0] = (rfFrequency >> 16) & 0xFF; buffer[1] = (rfFrequency >> 8) & 0xFF; buffer[2] = rfFrequency & 0xFF; - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommand(Opcode::SetRfFrequency, std::span{buffer, 3})); + return this->writeCommand(Opcode::SetRfFrequency, std::span{buffer, 3}); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setTxParams(uint8_t power, RampTime rampTime) { - RF_BEGIN(); - buffer[0] = power; buffer[1] = i(rampTime); - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommand(Opcode::SetTxParams, std::span{buffer, 2})); + return this->writeCommand(Opcode::SetTxParams, std::span{buffer, 2}); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setCadParams(CadSymbolNumber cadSymbolNumber) { - RF_BEGIN(); - buffer[0] = uint8_t(cadSymbolNumber); - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommand(Opcode::SetCadParams, std::span{buffer, 1})); + return this->writeCommand(Opcode::SetCadParams, std::span{buffer, 1}); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setBufferBaseAddress(uint8_t txBaseAddress, uint8_t rxBaseAddress) { - RF_BEGIN(); - buffer[0] = txBaseAddress; buffer[1] = rxBaseAddress; - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommand(Opcode::SetBufferBaseAddress, std::span{buffer, 2})); + return this->writeCommand(Opcode::SetBufferBaseAddress, std::span{buffer, 2}); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setModulationParams(ModulationParams modulationParams) { - RF_BEGIN(); - std::memcpy(buffer, (uint8_t *) &modulationParams, 3); - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommand(Opcode::SetModulationParams, std::span{buffer, 3})); + return this->writeCommand(Opcode::SetModulationParams, std::span{buffer, 3}); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setPacketParams(PacketParams packetParams) { - RF_BEGIN(); - std::memcpy(buffer, (uint8_t *) &packetParams, 7); - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommand(Opcode::SetPacketParams, std::span{buffer, 7})); + return this->writeCommand(Opcode::SetPacketParams, std::span{buffer, 7}); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::getRxBufferStatus(RxBufferStatus *rxBufferStatus) { - RF_BEGIN(); - - RF_WAIT_WHILE(isBusy()); - if (RF_CALL(this->readCommand(Opcode::GetRxBufferStatus, std::span{buffer, 2}))) + modm::this_fiber::poll([&]{ return not isBusy(); }); + if (this->readCommand(Opcode::GetRxBufferStatus, std::span{buffer, 2})) { std::memcpy((uint8_t *) rxBufferStatus, buffer, 2); - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::getPacketStatus(PacketStatus *packetStatus) { - RF_BEGIN(); - - RF_WAIT_WHILE(isBusy()); - if (RF_CALL(this->readCommand(Opcode::GetPacketStatus, std::span{buffer, 5}))) + modm::this_fiber::poll([&]{ return not isBusy(); }); + if (this->readCommand(Opcode::GetPacketStatus, std::span{buffer, 5})) { std::memcpy((uint8_t *) packetStatus, buffer, 5); - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::getRssiInst(uint8_t *rssiInst) { - RF_BEGIN(); - - RF_WAIT_WHILE(isBusy()); - if (RF_CALL(this->readCommand(Opcode::GetRssiInst, std::span{buffer, 1}))) + modm::this_fiber::poll([&]{ return not isBusy(); }); + if (this->readCommand(Opcode::GetRssiInst, std::span{buffer, 1})) { *rssiInst = buffer[0]; - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setDioIrqParams(Irq_t irqMask, Irq_t dio1Mask, Irq_t dio2Mask, Irq_t dio3Mask) { - RF_BEGIN(); - buffer[0] = (irqMask.value >> 8) & 0xFF; buffer[1] = irqMask.value & 0xFF; buffer[2] = (dio1Mask.value >> 8) & 0xFF; @@ -454,155 +398,135 @@ Sx128x< Transport, Reset, Busy >::setDioIrqParams(Irq_t irqMask, Irq_t dio1Mask, buffer[5] = dio2Mask.value & 0xFF; buffer[6] = (dio3Mask.value >> 8) & 0xFF; buffer[7] = dio3Mask.value & 0xFF; - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommand(Opcode::SetDioIrqParams, std::span{buffer, 8})); + return this->writeCommand(Opcode::SetDioIrqParams, std::span{buffer, 8}); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::getIrqStatus(Irq_t *irqStatus) { - RF_BEGIN(); - - RF_WAIT_WHILE(isBusy()); - if (RF_CALL(this->readCommand(Opcode::GetIrqStatus, std::span{buffer, 2}))) + modm::this_fiber::poll([&]{ return not isBusy(); }); + if (this->readCommand(Opcode::GetIrqStatus, std::span{buffer, 2})) { *irqStatus = Irq_t((buffer[0] << 8) | buffer[1]); - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::clearIrqStatus(Irq_t irqMask) { - RF_BEGIN(); - buffer[0] = (irqMask.value >> 8) & 0xFF; buffer[1] = irqMask.value & 0xFF; - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommand(Opcode::ClrIrqStatus, std::span{buffer, 2})); + return this->writeCommand(Opcode::ClrIrqStatus, std::span{buffer, 2}); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setRegulatorMode(RegulatorMode regModeParam) { - RF_BEGIN(); - buffer[0] = i(regModeParam); - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommand(Opcode::SetRegulatorMode, std::span{buffer, 1})); + return this->writeCommand(Opcode::SetRegulatorMode, std::span{buffer, 1}); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setSaveContext() { - RF_BEGIN(); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_WAIT_WHILE(isBusy()); - - RF_END_RETURN_CALL(this->writeCommandSingleData(Opcode::SetSaveContext)); + return this->writeCommandSingleData(Opcode::SetSaveContext); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setAutoFs(bool enable) { - RF_BEGIN(); - buffer[0] = enable ? 0x01 : 0x00; - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommand(Opcode::SetAutoFs, std::span{buffer, 1})); + return this->writeCommand(Opcode::SetAutoFs, std::span{buffer, 1}); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setAutoTx(uint16_t time) { - RF_BEGIN(); - buffer[0] = (time >> 8) & 0xFF; buffer[1] = time & 0xFF; - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommand(Opcode::SetAutoTx, std::span{buffer, 2})); + return this->writeCommand(Opcode::SetAutoTx, std::span{buffer, 2}); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setLongPreamble(bool enable) { - RF_BEGIN(); - buffer[0] = enable ? 0x01 : 0x00; - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommand(Opcode::SetLongPreamble, std::span{buffer, 1})); + return this->writeCommand(Opcode::SetLongPreamble, std::span{buffer, 1}); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setUartSpeed(UartDividerRatio uartDividerRatio) { - RF_BEGIN(); - buffer[0] = (i(uartDividerRatio) >> 8) & 0xFF; buffer[1] = i(uartDividerRatio) & 0xFF; - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommand(Opcode::SetLongPreamble, std::span{buffer, 1})); + return this->writeCommand(Opcode::SetLongPreamble, std::span{buffer, 1}); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setRangingRole(RangingRole rangingRole) { - RF_BEGIN(); - buffer[0] = i(rangingRole); - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommand(Opcode::SetLongPreamble, std::span{buffer, 1})); + return this->writeCommand(Opcode::SetLongPreamble, std::span{buffer, 1}); } // ----------------------------------------------------------------------------- template < class Transport, class Reset, class Busy > -modm::ResumableResult +bool Sx128x< Transport, Reset, Busy >::setAdvancedRanging(bool enable) { - RF_BEGIN(); - buffer[0] = enable ? 0x01 : 0x00; - RF_WAIT_WHILE(isBusy()); + modm::this_fiber::poll([&]{ return not isBusy(); }); - RF_END_RETURN_CALL(this->writeCommand(Opcode::SetAdvancedRanging, std::span{buffer, 1})); + return this->writeCommand(Opcode::SetAdvancedRanging, std::span{buffer, 1}); } -} \ No newline at end of file +} diff --git a/src/modm/driver/radio/sx128x_transport.hpp b/src/modm/driver/radio/sx128x_transport.hpp index 8cc98bcf2c..d80e71fee3 100644 --- a/src/modm/driver/radio/sx128x_transport.hpp +++ b/src/modm/driver/radio/sx128x_transport.hpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include namespace modm @@ -84,19 +84,19 @@ class Sx128xTransport * @author Rasmus Kleist Hørlyck Sørensen */ template < class SpiMaster, class Cs > -class Sx128xTransportSpi : public Sx128xTransport, public SpiDevice< SpiMaster >, protected NestedResumable<2> +class Sx128xTransportSpi : public Sx128xTransport, public SpiDevice< SpiMaster > { public: Sx128xTransportSpi() = default; protected: - modm::ResumableResult + bool writeCommandSingleData(Command command, uint8_t *data = nullptr); - modm::ResumableResult + bool writeCommand(Command command, std::span data); - modm::ResumableResult + bool readCommand(Command command, std::span data); private: @@ -112,19 +112,19 @@ class Sx128xTransportSpi : public Sx128xTransport, public SpiDevice< SpiMaster > * @author Rasmus Kleist Hørlyck Sørensen */ template < class Uart > -class Sx128xTransportUart : public Sx128xTransport, public UartDevice< Uart, 2 > +class Sx128xTransportUart : public Sx128xTransport, public UartDevice< Uart > { public: Sx128xTransportUart() = default; protected: - modm::ResumableResult + bool writeCommandSingleData(Command command, uint8_t *data = nullptr); - modm::ResumableResult + bool writeCommand(Command command, std::span data); - modm::ResumableResult + bool readCommand(Command command, std::span data); private: diff --git a/src/modm/driver/radio/sx128x_transport_impl.hpp b/src/modm/driver/radio/sx128x_transport_impl.hpp index 86967b1f46..8c98027895 100644 --- a/src/modm/driver/radio/sx128x_transport_impl.hpp +++ b/src/modm/driver/radio/sx128x_transport_impl.hpp @@ -19,32 +19,28 @@ namespace modm { template < class SpiMaster, class Cs > -modm::ResumableResult +bool Sx128xTransportSpi::writeCommandSingleData(Command command, uint8_t *data) { - RF_BEGIN(); - - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); command.getOpcode(commandBuffer); - RF_CALL(SpiMaster::transfer(commandBuffer, data, 1)); + SpiMaster::transfer(commandBuffer, data, 1); if (this->releaseMaster()) Cs::set(); - RF_END_RETURN(true); + return true; } // ---------------------------------------------------------------------------- template < class SpiMaster, class Cs > -modm::ResumableResult +bool Sx128xTransportSpi::writeCommand(Command command, std::span data) { - RF_BEGIN(); - - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); command.getOpcode(commandBuffer); @@ -53,24 +49,22 @@ Sx128xTransportSpi::writeCommand(Command command, std::spanreleaseMaster()) Cs::set(); - RF_END_RETURN(true); + return true; } // ---------------------------------------------------------------------------- template < class SpiMaster, class Cs > -modm::ResumableResult +bool Sx128xTransportSpi::readCommand(Command command, std::span data) { - RF_BEGIN(); - - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); command.getOpcode(commandBuffer); @@ -82,43 +76,39 @@ Sx128xTransportSpi::readCommand(Command command, std::spanreleaseMaster()) Cs::set(); - RF_END_RETURN(true); + return true; } // ---------------------------------------------------------------------------- template < class Uart > -modm::ResumableResult +bool Sx128xTransportUart::writeCommandSingleData(Command command, uint8_t *data) { - RF_BEGIN(); - command.getOpcode(commandBuffer); - if (RF_CALL(this->write(commandBuffer[0]))) { + if (this->write(commandBuffer[0])) { if (data != nullptr) { - RF_RETURN_CALL(this->read(data)); + return this->read(data); } else { - RF_RETURN(true); + return true; } } - RF_END_RETURN(false); + return false; } // ---------------------------------------------------------------------------- template < class Uart > -modm::ResumableResult +bool Sx128xTransportUart::writeCommand(Command command, std::span data) { - RF_BEGIN(); - command.getOpcode(commandBuffer); if (command.hasVargs()) { auto vargs = command.getVargs(); @@ -128,21 +118,19 @@ Sx128xTransportUart::writeCommand(Command command, std::spanwrite(commandBuffer, 2 + command.getVargsCount()))) { - RF_RETURN_CALL(this->write(&data[0], data.size())); + if (this->write(commandBuffer, 2 + command.getVargsCount())) { + return this->write(&data[0], data.size()); } - RF_END_RETURN(false); + return false; } // ---------------------------------------------------------------------------- template < class Uart > -modm::ResumableResult +bool Sx128xTransportUart::readCommand(Command command, std::span data) { - RF_BEGIN(); - command.getOpcode(commandBuffer); if (command.hasVargs()) { auto vargs = command.getVargs(); @@ -152,11 +140,11 @@ Sx128xTransportUart::readCommand(Command command, std::span data) commandBuffer[1] = data.size(); } - if (RF_CALL(this->write(commandBuffer, 2 + command.getVargsCount()))) { - RF_RETURN_CALL(this->read(&data[0], data.size())); + if (this->write(commandBuffer, 2 + command.getVargsCount())) { + return this->read(&data[0], data.size()); } - RF_END_RETURN(false); + return false; } -} // namespace modm \ No newline at end of file +} // namespace modm diff --git a/src/modm/driver/rtc/ds1302.hpp b/src/modm/driver/rtc/ds1302.hpp index 3b9e7753c1..53946525f3 100644 --- a/src/modm/driver/rtc/ds1302.hpp +++ b/src/modm/driver/rtc/ds1302.hpp @@ -13,7 +13,7 @@ #ifndef MODM_DS1302_HPP #define MODM_DS1302_HPP -#include +#include namespace modm { diff --git a/src/modm/driver/rtc/ds1302.lb b/src/modm/driver/rtc/ds1302.lb index a0a3a697cb..b1a6b51041 100644 --- a/src/modm/driver/rtc/ds1302.lb +++ b/src/modm/driver/rtc/ds1302.lb @@ -17,7 +17,7 @@ def init(module): def prepare(module, options): module.depends( - ":architecture:delay", + ":architecture:fiber", ":architecture:gpio") return True diff --git a/src/modm/driver/rtc/ds1302_impl.hpp b/src/modm/driver/rtc/ds1302_impl.hpp index 1b4449b4cd..87846be68d 100644 --- a/src/modm/driver/rtc/ds1302_impl.hpp +++ b/src/modm/driver/rtc/ds1302_impl.hpp @@ -27,20 +27,20 @@ template< class PinSet > void modm::Ds1302< PinSet >::writeByte(uint8_t byte) { - modm::delay(DELAY_CE); + modm::this_fiber::sleep_for(DELAY_CE); Io::setOutput(); for (uint8_t ii = 8; ii > 0; --ii) { - modm::delay(DELAY_CLK); + modm::this_fiber::sleep_for(DELAY_CLK); Sclk::reset(); Io::set(byte & 0x01); - modm::delay(DELAY_CLK); + modm::this_fiber::sleep_for(DELAY_CLK); Sclk::set(); byte >>= 1; } - modm::delay(DELAY_CLK); + modm::this_fiber::sleep_for(DELAY_CLK); } template< class PinSet > @@ -53,10 +53,10 @@ modm::Ds1302< PinSet >::write(const uint8_t addr, const uint8_t data) // Cleanup Sclk::reset(); - modm::delay(DELAY_CLK); + modm::this_fiber::sleep_for(DELAY_CLK); Io::setInput(); Ce::reset(); - modm::delay(DELAY_CE); + modm::this_fiber::sleep_for(DELAY_CE); } template< class PinSet > @@ -69,16 +69,16 @@ modm::Ds1302< PinSet >::read(const uint8_t addr) uint8_t ret = 0; Io::setInput(); - modm::delay(DELAY_CLK); + modm::this_fiber::sleep_for(DELAY_CLK); for (uint8_t ii = 8; ii > 0; --ii) { bool rr = Io::read(); ret >>= 1; ret |= (rr << 7); Sclk::set(); - modm::delay(DELAY_CLK); + modm::this_fiber::sleep_for(DELAY_CLK); Sclk::reset(); - modm::delay(DELAY_CLK); + modm::this_fiber::sleep_for(DELAY_CLK); } Ce::reset(); @@ -99,7 +99,7 @@ modm::Ds1302< PinSet >::readRtc(ds1302::Data &storage) Sclk::reset(); // Wait for stable output - modm::delay(DELAY_CE); + modm::this_fiber::sleep_for(DELAY_CE); for (uint8_t jj = 0; jj < MODM_ARRAY_SIZE(storage.data); ++jj) { @@ -110,11 +110,11 @@ modm::Ds1302< PinSet >::readRtc(ds1302::Data &storage) ret >>= 1; ret |= (rr << 7); Sclk::set(); - modm::delay(DELAY_CLK); + modm::this_fiber::sleep_for(DELAY_CLK); // Falling edge of SCLK will trigger DS1302 output Sclk::reset(); - modm::delay(DELAY_CLK); + modm::this_fiber::sleep_for(DELAY_CLK); } storage.data[jj] = ret; } diff --git a/src/modm/driver/rtc/mcp7941x.hpp b/src/modm/driver/rtc/mcp7941x.hpp index cc87f0896a..2368a7576e 100644 --- a/src/modm/driver/rtc/mcp7941x.hpp +++ b/src/modm/driver/rtc/mcp7941x.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include namespace modm { @@ -46,18 +46,18 @@ struct mcp7941x */ template < class I2cMaster > class Mcp7941x : public mcp7941x, - public modm::I2cDevice + public modm::I2cDevice { public: Mcp7941x(uint8_t address = 0x6f); - modm::ResumableResult> + std::optional getDateTime(); - modm::ResumableResult + bool setDateTime(DateTime); - modm::ResumableResult + bool oscillatorRunning(); private: @@ -93,12 +93,12 @@ class Mcp7941x : public mcp7941x, * @author Raphael Lehmann */ template < class I2cMaster > -class Mcp7941xEeprom : public modm::I2cDevice +class Mcp7941xEeprom : public modm::I2cDevice { public: Mcp7941xEeprom(uint8_t address = 0x57); - modm::ResumableResult>> + std::optional> getUniqueId(); private: diff --git a/src/modm/driver/rtc/mcp7941x.lb b/src/modm/driver/rtc/mcp7941x.lb index fc098a350b..5e702a8f4b 100644 --- a/src/modm/driver/rtc/mcp7941x.lb +++ b/src/modm/driver/rtc/mcp7941x.lb @@ -27,7 +27,7 @@ def prepare(module, options): module.depends( ":architecture:i2c.device", ":architecture:register", - ":processing:resumable") + ":architecture:fiber") return True diff --git a/src/modm/driver/rtc/mcp7941x_impl.hpp b/src/modm/driver/rtc/mcp7941x_impl.hpp index a3474ed5a1..1a935846ed 100644 --- a/src/modm/driver/rtc/mcp7941x_impl.hpp +++ b/src/modm/driver/rtc/mcp7941x_impl.hpp @@ -17,17 +17,15 @@ // ---------------------------------------------------------------------------- template < typename I2cMaster > modm::Mcp7941x::Mcp7941x(uint8_t address) -: I2cDevice(address), dateTime{} +: I2cDevice(address), dateTime{} { } template < typename I2cMaster > -modm::ResumableResult> +std::optional modm::Mcp7941x::getDateTime() { - RF_BEGIN(); - this->transaction.configureWriteRead(&addr_seconds, 1, scratch, 7); - if (RF_CALL(this->runTransaction())) { + if (I2cDevice::writeRead(&addr_seconds, 1, scratch, 7)) { dateTime.seconds = decodeBcd(scratch[0] & 0b0111'1111); dateTime.minutes = decodeBcd(scratch[1]); dateTime.hours = decodeBcd(scratch[2] & 0b0011'1111); @@ -35,16 +33,15 @@ modm::Mcp7941x::getDateTime() dateTime.days = decodeBcd(scratch[4]); dateTime.months = decodeBcd(scratch[5] & 0b0001'1111); dateTime.years = decodeBcd(scratch[6]); - RF_RETURN(dateTime); + return dateTime; } - RF_END_RETURN(std::nullopt); + return std::nullopt; } template < typename I2cMaster > -modm::ResumableResult +bool modm::Mcp7941x::setDateTime(DateTime dt) { - RF_BEGIN(); scratch[0] = addr_seconds; scratch[1] = 0x00; // stop oscillator scratch[2] = encodeBcd(dt.minutes); @@ -53,43 +50,37 @@ modm::Mcp7941x::setDateTime(DateTime dt) scratch[5] = encodeBcd(dt.days); scratch[6] = encodeBcd(dt.months); scratch[7] = encodeBcd(dt.years); - this->transaction.configureWrite(scratch, 8); - if (not RF_CALL(this->runTransaction())) { - RF_RETURN(false); + if (not I2cDevice::write(scratch, 8)) { + return false; } scratch[0] = addr_seconds; scratch[1] = encodeBcd(dt.seconds) | 0b1000'0000 /* Start oscillator bit */; - this->transaction.configureWrite(scratch, 2); - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::write(scratch, 2); } template < typename I2cMaster > -modm::ResumableResult +bool modm::Mcp7941x::oscillatorRunning() { - RF_BEGIN(); - this->transaction.configureWriteRead(&addr_weekday, 1, scratch, 1); - if (RF_CALL(this->runTransaction())) { - RF_RETURN((scratch[0] | 0b0010'0000 /* OSCRUN bit */) > 0); + if (I2cDevice::writeRead(&addr_weekday, 1, scratch, 1)) { + return (scratch[0] | 0b0010'0000 /* OSCRUN bit */) > 0; } - RF_END_RETURN(false); + return false; } template < typename I2cMaster > modm::Mcp7941xEeprom::Mcp7941xEeprom(uint8_t address) -: I2cDevice(address) +: I2cDevice(address) { } template < typename I2cMaster > -modm::ResumableResult>> +std::optional> modm::Mcp7941xEeprom::getUniqueId() { - RF_BEGIN(); - this->transaction.configureWriteRead(&addr_unique_id, 1, data.data(), 8); - if (not RF_CALL(this->runTransaction())) { - RF_RETURN(std::nullopt); + if (not I2cDevice::writeRead(&addr_unique_id, 1, data.data(), 8)) { + return std::nullopt; } - RF_END_RETURN(data); + return data; } diff --git a/src/modm/driver/storage/at24mac402.hpp b/src/modm/driver/storage/at24mac402.hpp index 09aef4fe23..c297da6b05 100644 --- a/src/modm/driver/storage/at24mac402.hpp +++ b/src/modm/driver/storage/at24mac402.hpp @@ -49,34 +49,34 @@ class At24Mac402 : protected I2cEeprom setAddress(uint8_t address); // Read 48 bit pre-programmed MAC - modm::ResumableResult + bool readMac(std::span data); // Read 128 bit pre-programmed unique id - modm::ResumableResult + bool readUniqueId(std::span data); - modm::ResumableResult + bool ping(); - modm::ResumableResult + bool writeByte(uint32_t address, uint8_t data); - modm::ResumableResult + bool write(uint32_t address, const uint8_t* data, std::size_t length); template - modm::ResumableResult + bool write(uint32_t address, const T& data); - modm::ResumableResult + bool readByte(uint32_t address, uint8_t& data); - modm::ResumableResult + bool read(uint32_t address, uint8_t* data, std::size_t length); template - modm::ResumableResult + bool read(uint32_t address, T& data); private: uint8_t address_{}; diff --git a/src/modm/driver/storage/at24mac402_impl.hpp b/src/modm/driver/storage/at24mac402_impl.hpp index 3f084236e2..4fddd06012 100644 --- a/src/modm/driver/storage/at24mac402_impl.hpp +++ b/src/modm/driver/storage/at24mac402_impl.hpp @@ -30,7 +30,7 @@ modm::At24Mac402::setAddress(uint8_t address) } template -modm::ResumableResult +bool modm::At24Mac402::readMac(std::span data) { Base::setAddress(address_ | 0b1'000); @@ -38,7 +38,7 @@ modm::At24Mac402::readMac(std::span data) } template -modm::ResumableResult +bool modm::At24Mac402::readUniqueId(std::span data) { Base::setAddress(address_ | 0b1'000); @@ -46,7 +46,7 @@ modm::At24Mac402::readUniqueId(std::span data) } template -modm::ResumableResult +bool modm::At24Mac402::ping() { Base::setAddress(address_); @@ -54,7 +54,7 @@ modm::At24Mac402::ping() } template -modm::ResumableResult +bool modm::At24Mac402::writeByte(uint32_t address, uint8_t data) { Base::setAddress(address_); @@ -62,7 +62,7 @@ modm::At24Mac402::writeByte(uint32_t address, uint8_t data) } template -modm::ResumableResult +bool modm::At24Mac402::write(uint32_t address, const uint8_t* data, std::size_t length) { Base::setAddress(address_); @@ -71,7 +71,7 @@ modm::At24Mac402::write(uint32_t address, const uint8_t* data, std::s template template -modm::ResumableResult +bool modm::At24Mac402::write(uint32_t address, const T& data) { Base::setAddress(address_); @@ -79,7 +79,7 @@ modm::At24Mac402::write(uint32_t address, const T& data) } template -modm::ResumableResult +bool modm::At24Mac402::readByte(uint32_t address, uint8_t& data) { Base::setAddress(address_); @@ -87,7 +87,7 @@ modm::At24Mac402::readByte(uint32_t address, uint8_t& data) } template -modm::ResumableResult +bool modm::At24Mac402::read(uint32_t address, uint8_t* data, std::size_t length) { Base::setAddress(address_); @@ -96,7 +96,7 @@ modm::At24Mac402::read(uint32_t address, uint8_t* data, std::size_t l template template -modm::ResumableResult +bool modm::At24Mac402::read(uint32_t address, T& data) { Base::setAddress(address_); diff --git a/src/modm/driver/storage/at45db0x1d_impl.hpp b/src/modm/driver/storage/at45db0x1d_impl.hpp index aff016d24e..975b0d4a96 100644 --- a/src/modm/driver/storage/at45db0x1d_impl.hpp +++ b/src/modm/driver/storage/at45db0x1d_impl.hpp @@ -37,10 +37,10 @@ modm::At45db0x1d::initialize() Cs::reset(); // send page size change sequence (fixed sequence) - Spi::transferBlocking(0x3d); - Spi::transferBlocking(0x2a); - Spi::transferBlocking(0x80); - Spi::transferBlocking(0xa6); + Spi::transfer(0x3d); + Spi::transfer(0x2a); + Spi::transfer(0x80); + Spi::transfer(0xa6); Cs::set(); } @@ -58,16 +58,16 @@ modm::At45db0x1d::copyPageToBuffer(uint16_t pageAddress, at45db::Buffer Cs::reset(); if (buffer == at45db::BUFFER_0) { - Spi::transferBlocking(MAIN_MEMORY_PAGE_TO_BUFFER_1_TRANSFER); + Spi::transfer(MAIN_MEMORY_PAGE_TO_BUFFER_1_TRANSFER); } else { - Spi::transferBlocking(MAIN_MEMORY_PAGE_TO_BUFFER_2_TRANSFER); + Spi::transfer(MAIN_MEMORY_PAGE_TO_BUFFER_2_TRANSFER); } // set address - Spi::transferBlocking(pageAddress >> 8); - Spi::transferBlocking(pageAddress & 0xff); - Spi::transferBlocking(0); + Spi::transfer(pageAddress >> 8); + Spi::transfer(pageAddress & 0xff); + Spi::transfer(0); Cs::set(); } @@ -80,16 +80,16 @@ modm::At45db0x1d::comparePageToBuffer(uint16_t pageAddress, at45db::Buf Cs::reset(); if (buffer == at45db::BUFFER_0) { - Spi::transferBlocking(MAIN_MEMORY_PAGE_TO_BUFFER_1_COMPARE); + Spi::transfer(MAIN_MEMORY_PAGE_TO_BUFFER_1_COMPARE); } else { - Spi::transferBlocking(MAIN_MEMORY_PAGE_TO_BUFFER_2_COMPARE); + Spi::transfer(MAIN_MEMORY_PAGE_TO_BUFFER_2_COMPARE); } // set address - Spi::transferBlocking(pageAddress >> 8); - Spi::transferBlocking(pageAddress & 0xff); - Spi::transferBlocking(0); + Spi::transfer(pageAddress >> 8); + Spi::transfer(pageAddress & 0xff); + Spi::transfer(0); Cs::set(); } @@ -110,16 +110,16 @@ modm::At45db0x1d::copyBufferToPage(at45db::Buffer buffer, uint16_t page Cs::reset(); if (buffer == at45db::BUFFER_0) { - Spi::transferBlocking(BUFFER_1_TO_MAIN_MEMORY_PAGE_PROGRAM_WITH_ERASE); + Spi::transfer(BUFFER_1_TO_MAIN_MEMORY_PAGE_PROGRAM_WITH_ERASE); } else { - Spi::transferBlocking(BUFFER_2_TO_MAIN_MEMORY_PAGE_PROGRAM_WITH_ERASE); + Spi::transfer(BUFFER_2_TO_MAIN_MEMORY_PAGE_PROGRAM_WITH_ERASE); } // set address - Spi::transferBlocking(pageAddress >> 8); - Spi::transferBlocking(pageAddress & 0xff); - Spi::transferBlocking(0); + Spi::transfer(pageAddress >> 8); + Spi::transfer(pageAddress & 0xff); + Spi::transfer(0); Cs::set(); } @@ -132,16 +132,16 @@ modm::At45db0x1d::copyBufferToPageWithoutErase(at45db::Buffer buffer, u Cs::reset(); if (buffer == at45db::BUFFER_0) { - Spi::transferBlocking(BUFFER_1_TO_MAIN_MEMORY_PAGE_PROGRAM_WITHOUT_ERASE); + Spi::transfer(BUFFER_1_TO_MAIN_MEMORY_PAGE_PROGRAM_WITHOUT_ERASE); } else { - Spi::transferBlocking(BUFFER_2_TO_MAIN_MEMORY_PAGE_PROGRAM_WITHOUT_ERASE); + Spi::transfer(BUFFER_2_TO_MAIN_MEMORY_PAGE_PROGRAM_WITHOUT_ERASE); } // set address - Spi::transferBlocking(pageAddress >> 8); - Spi::transferBlocking(pageAddress & 0xff); - Spi::transferBlocking(0); + Spi::transfer(pageAddress >> 8); + Spi::transfer(pageAddress & 0xff); + Spi::transfer(0); Cs::set(); } @@ -154,22 +154,22 @@ modm::At45db0x1d::readFromBuffer(at45db::Buffer buffer, { Cs::reset(); if (buffer == at45db::BUFFER_0) { - Spi::transferBlocking(BUFFER_1_READ); + Spi::transfer(BUFFER_1_READ); } else { - Spi::transferBlocking(BUFFER_2_READ); + Spi::transfer(BUFFER_2_READ); } // set address - Spi::transferBlocking(0); - Spi::transferBlocking(0); - Spi::transferBlocking(address); + Spi::transfer(0); + Spi::transfer(0); + Spi::transfer(address); // don't care byte - Spi::transferBlocking(0); + Spi::transfer(0); for (std::size_t i = 0; i < size; ++i) { - *data++ = Spi::transferBlocking(0); + *data++ = Spi::transfer(0); } Cs::set(); } @@ -182,19 +182,19 @@ modm::At45db0x1d::writeToBuffer(at45db::Buffer buffer, { Cs::reset(); if (buffer == at45db::BUFFER_0) { - Spi::transferBlocking(BUFFER_1_WRITE); + Spi::transfer(BUFFER_1_WRITE); } else { - Spi::transferBlocking(BUFFER_2_WRITE); + Spi::transfer(BUFFER_2_WRITE); } // set address - Spi::transferBlocking(0); - Spi::transferBlocking(0); - Spi::transferBlocking(address); + Spi::transfer(0); + Spi::transfer(0); + Spi::transfer(address); for (std::size_t i = 0; i < size; ++i) { - Spi::transferBlocking(*data++); + Spi::transfer(*data++); } Cs::set(); } @@ -205,15 +205,15 @@ void modm::At45db0x1d::readFromMemory(uint32_t address, uint8_t *data, std::size_t size) { Cs::reset(); - Spi::transferBlocking(CONTINOUS_ARRAY_READ); + Spi::transfer(CONTINOUS_ARRAY_READ); // set address - Spi::transferBlocking(address >> 16); - Spi::transferBlocking(address >> 8); - Spi::transferBlocking(address); + Spi::transfer(address >> 16); + Spi::transfer(address >> 8); + Spi::transfer(address); for (std::size_t i = 0; i < size; ++i) { - *data++ = Spi::transferBlocking(0); + *data++ = Spi::transfer(0); } Cs::set(); } @@ -224,21 +224,21 @@ void modm::At45db0x1d::readPageFromMemory(uint32_t address, uint8_t *data, std::size_t size) { Cs::reset(); - Spi::transferBlocking(MAIN_MEMORY_PAGE_READ); + Spi::transfer(MAIN_MEMORY_PAGE_READ); // set address - Spi::transferBlocking(address >> 16); - Spi::transferBlocking(address >> 8); - Spi::transferBlocking(address); + Spi::transfer(address >> 16); + Spi::transfer(address >> 8); + Spi::transfer(address); // don't care - Spi::transferBlocking(0); - Spi::transferBlocking(0); - Spi::transferBlocking(0); - Spi::transferBlocking(0); + Spi::transfer(0); + Spi::transfer(0); + Spi::transfer(0); + Spi::transfer(0); for (std::size_t i = 0; i < size; ++i) { - *data++ = Spi::transferBlocking(0); + *data++ = Spi::transfer(0); } Cs::set(); } @@ -253,9 +253,9 @@ modm::At45db0x1d::pageErase(uint16_t pageAddress) Cs::reset(); // set address - Spi::transferBlocking(pageAddress >> 8); - Spi::transferBlocking(pageAddress & 0xff); - Spi::transferBlocking(0); + Spi::transfer(pageAddress >> 8); + Spi::transfer(pageAddress & 0xff); + Spi::transfer(0); Cs::set(); } @@ -268,16 +268,16 @@ modm::At45db0x1d::pageRewrite(uint16_t pageAddress, at45db::Buffer buff Cs::reset(); if (buffer == at45db::BUFFER_0) { - Spi::transferBlocking(BUFFER_1_PAGE_REWRITE); + Spi::transfer(BUFFER_1_PAGE_REWRITE); } else { - Spi::transferBlocking(BUFFER_2_PAGE_REWRITE); + Spi::transfer(BUFFER_2_PAGE_REWRITE); } // set address - Spi::transferBlocking(pageAddress >> 8); - Spi::transferBlocking(pageAddress & 0xff); - Spi::transferBlocking(0); + Spi::transfer(pageAddress >> 8); + Spi::transfer(pageAddress & 0xff); + Spi::transfer(0); Cs::set(); } @@ -292,9 +292,9 @@ modm::At45db0x1d::blockErase(uint16_t blockAddress) Cs::reset(); // set address - Spi::transferBlocking(blockAddress >> 8); - Spi::transferBlocking(blockAddress & 0xf8); - Spi::transferBlocking(0); + Spi::transfer(blockAddress >> 8); + Spi::transfer(blockAddress & 0xf8); + Spi::transfer(0); Cs::set(); } @@ -309,10 +309,10 @@ modm::At45db0x1d::chipErase() Cs::reset(); // send chip erase sequence (fixed sequence) - Spi::transferBlocking(0xc7); - Spi::transferBlocking(0x94); - Spi::transferBlocking(0x80); - Spi::transferBlocking(0x9a); + Spi::transfer(0xc7); + Spi::transfer(0x94); + Spi::transfer(0x80); + Spi::transfer(0x9a); Cs::set(); } @@ -341,8 +341,8 @@ uint8_t modm::At45db0x1d::readStatus() { Cs::reset(); - Spi::transferBlocking(READ_STATUS_REGISTER); - uint8_t result = Spi::transferBlocking(0); // dummy write to get result + Spi::transfer(READ_STATUS_REGISTER); + uint8_t result = Spi::transfer(0); // dummy write to get result Cs::set(); return result; diff --git a/src/modm/driver/storage/block_device_file.hpp b/src/modm/driver/storage/block_device_file.hpp index 67e08d9254..16bf182304 100644 --- a/src/modm/driver/storage/block_device_file.hpp +++ b/src/modm/driver/storage/block_device_file.hpp @@ -15,7 +15,7 @@ #include -#include +#include #include #include @@ -32,15 +32,15 @@ namespace modm * \author Raphael Lehmann */ template -class BdFile : public modm::BlockDevice, protected modm::NestedResumable<3> +class BdFile : public modm::BlockDevice { public: /// Initializes the storage hardware - modm::ResumableResult + bool initialize(); /// Deinitializes the storage hardware - modm::ResumableResult + bool deinitialize(); /** Read data from one or more blocks @@ -50,7 +50,7 @@ class BdFile : public modm::BlockDevice, protected modm::NestedResumable<3> * @param size Size to read in bytes (multiple of read block size) * @return True on success */ - modm::ResumableResult + bool read(uint8_t* buffer, bd_address_t address, bd_size_t size); /** Program blocks with data @@ -62,7 +62,7 @@ class BdFile : public modm::BlockDevice, protected modm::NestedResumable<3> * @param size Size to write in bytes (multiple of read block size) * @return True on success */ - modm::ResumableResult + bool program(const uint8_t* buffer, bd_address_t address, bd_size_t size); /** Erase blocks @@ -73,7 +73,7 @@ class BdFile : public modm::BlockDevice, protected modm::NestedResumable<3> * @param size Size to erase in bytes (multiple of read block size) * @return True on success */ - modm::ResumableResult + bool erase(bd_address_t address, bd_size_t size); /** Writes data to one or more blocks after erasing them @@ -85,7 +85,7 @@ class BdFile : public modm::BlockDevice, protected modm::NestedResumable<3> * @param size Size to write in bytes (multiple of read block size) * @return True on success */ - modm::ResumableResult + bool write(const uint8_t* buffer, bd_address_t address, bd_size_t size); public: diff --git a/src/modm/driver/storage/block_device_file_impl.hpp b/src/modm/driver/storage/block_device_file_impl.hpp index 4290063c28..a4bec07f80 100644 --- a/src/modm/driver/storage/block_device_file_impl.hpp +++ b/src/modm/driver/storage/block_device_file_impl.hpp @@ -17,13 +17,12 @@ // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdFile::initialize() { - RF_BEGIN(); file.open(Filename::name, std::fstream::binary | std::fstream::in | std::fstream::out | std::fstream::ate); if(!file.is_open()) - RF_RETURN(false); + return false; if(file.tellg() != DeviceSize) { if(file.tellg() == 0) { // create empty file with size of DeviceSize @@ -32,89 +31,80 @@ modm::BdFile::initialize() } } else { - RF_RETURN(false); + return false; } } - RF_END_RETURN(true); + return true; } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdFile::deinitialize() { - RF_BEGIN(); file.close(); - RF_END_RETURN(true); + return true; } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdFile::read(uint8_t* buffer, bd_address_t address, bd_size_t size) { - RF_BEGIN(); - if((size == 0) || (size % BlockSizeRead != 0) || (address + size > DeviceSize)) { - RF_RETURN(false); + return false; } file.seekg(address); file.read(reinterpret_cast(buffer), size); - RF_END_RETURN(true); + return true; } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdFile::program(const uint8_t* buffer, bd_address_t address, bd_size_t size) { - RF_BEGIN(); - if((size == 0) || (size % BlockSizeWrite != 0) || (address + size > DeviceSize)) { - RF_RETURN(false); + return false; } file.seekp(address); file.write(reinterpret_cast(const_cast(buffer)), size); - RF_END_RETURN(true); + return true; } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdFile::erase(bd_address_t address, bd_size_t size) { - RF_BEGIN(); - if((size == 0) || (size % BlockSizeErase != 0) || (address + size > DeviceSize)) { - RF_RETURN(false); + return false; } // erasing does nothing, memory is undefined after erase and has to be programed first - RF_END_RETURN(true); + return true; } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdFile::write(const uint8_t* buffer, bd_address_t address, bd_size_t size) { - RF_BEGIN(); - if((size == 0) || (size % BlockSizeErase != 0) || (size % BlockSizeWrite != 0) || (address + size > DeviceSize)) { - RF_RETURN(false); + return false; } - if(!RF_CALL(this->erase(address, size))) { - RF_RETURN(false); + if(!this->erase(address, size)) { + return false; } - RF_END_RETURN_CALL(this->program(buffer, address, size)); + return this->program(buffer, address, size); } diff --git a/src/modm/driver/storage/block_device_heap.hpp b/src/modm/driver/storage/block_device_heap.hpp index b0c8c6e53c..42ef8d3617 100644 --- a/src/modm/driver/storage/block_device_heap.hpp +++ b/src/modm/driver/storage/block_device_heap.hpp @@ -15,7 +15,7 @@ #include #include -#include +#include namespace modm { @@ -33,19 +33,19 @@ namespace modm * \author Raphael Lehmann */ template -class BdHeap : public modm::BlockDevice, protected modm::NestedResumable<3> +class BdHeap : public modm::BlockDevice { public: /// Initializes the memory with zeros - modm::ResumableResult + bool initialize(); /// Data will be stored at externalMemoryAddress - modm::ResumableResult + bool initialize(uint8_t* externalMemoryAddress); /// Deinitializes nothing - modm::ResumableResult + bool deinitialize(); /** Read data from one or more blocks @@ -55,7 +55,7 @@ class BdHeap : public modm::BlockDevice, protected modm::NestedResumable<3> * @param size Size to read in bytes (multiple of read block size) * @return True on success */ - modm::ResumableResult + bool read(uint8_t* buffer, bd_address_t address, bd_size_t size); /** Program blocks with data @@ -67,7 +67,7 @@ class BdHeap : public modm::BlockDevice, protected modm::NestedResumable<3> * @param size Size to write in bytes (multiple of read block size) * @return True on success */ - modm::ResumableResult + bool program(const uint8_t* buffer, bd_address_t address, bd_size_t size); /** Erase blocks @@ -78,7 +78,7 @@ class BdHeap : public modm::BlockDevice, protected modm::NestedResumable<3> * @param size Size to erase in bytes (multiple of read block size) * @return True on success */ - modm::ResumableResult + bool erase(bd_address_t address, bd_size_t size); /** Writes data to one or more blocks after erasing them @@ -90,7 +90,7 @@ class BdHeap : public modm::BlockDevice, protected modm::NestedResumable<3> * @param size Size to write in bytes (multiple of read block size) * @return True on success */ - modm::ResumableResult + bool write(const uint8_t* buffer, bd_address_t address, bd_size_t size); public: diff --git a/src/modm/driver/storage/block_device_heap_impl.hpp b/src/modm/driver/storage/block_device_heap_impl.hpp index 7f4abcf07e..86ef0622c1 100644 --- a/src/modm/driver/storage/block_device_heap_impl.hpp +++ b/src/modm/driver/storage/block_device_heap_impl.hpp @@ -17,100 +17,89 @@ // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdHeap::initialize() { static_assert(externalMemory == false, "Use modm::BdHeap::initialize(uint8_t* memory) for externalMemory==true"); - RF_BEGIN(); std::memset(data, 0, DeviceSize); - RF_END_RETURN(true); + return true; } template -modm::ResumableResult +bool modm::BdHeap::initialize(uint8_t* memory) { static_assert(externalMemory == true, "modm::BdHeap::initialize(uint8_t* memory) is only allowed for externalMemory==true"); - RF_BEGIN(); data = memory; - RF_END_RETURN(true); + return true; } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdHeap::deinitialize() { - RF_BEGIN(); - RF_END_RETURN(true); + return true; } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdHeap::read(uint8_t* buffer, bd_address_t address, bd_size_t size) { - RF_BEGIN(); - if((size == 0) || (size % BlockSizeRead != 0) || (address + size > DeviceSize)) { - RF_RETURN(false); + return false; } std::memcpy(buffer, &data[address], size); - RF_END_RETURN(true); + return true; } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdHeap::program(const uint8_t* buffer, bd_address_t address, bd_size_t size) { - RF_BEGIN(); - if((size == 0) || (size % BlockSizeWrite != 0) || (address + size > DeviceSize)) { - RF_RETURN(false); + return false; } std::memcpy(&data[address], buffer, size); - RF_END_RETURN(true); + return true; } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdHeap::erase(bd_address_t address, bd_size_t size) { - RF_BEGIN(); - if((size == 0) || (size % BlockSizeErase != 0) || (address + size > DeviceSize)) { - RF_RETURN(false); + return false; } // erasing does nothing, memory is undefined after erase and has to be programed first - RF_END_RETURN(true); + return true; } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdHeap::write(const uint8_t* buffer, bd_address_t address, bd_size_t size) { - RF_BEGIN(); - if((size == 0) || (size % BlockSizeErase != 0) || (size % BlockSizeWrite != 0) || (address + size > DeviceSize)) { - RF_RETURN(false); + return false; } - if(!RF_CALL(this->erase(address, size))) { - RF_RETURN(false); + if(!this->erase(address, size)) { + return false; } - RF_END_RETURN_CALL(this->program(buffer, address, size)); + return this->program(buffer, address, size); } diff --git a/src/modm/driver/storage/block_device_mirror.hpp b/src/modm/driver/storage/block_device_mirror.hpp index 888fb16191..b26be187ae 100644 --- a/src/modm/driver/storage/block_device_mirror.hpp +++ b/src/modm/driver/storage/block_device_mirror.hpp @@ -14,7 +14,7 @@ #define MODM_BLOCK_DEVICE_MIRROR_HPP #include -#include +#include #include namespace modm @@ -34,15 +34,15 @@ namespace modm * \author Raphael Lehmann */ template -class BdMirror : public modm::BlockDevice, protected NestedResumable<3> +class BdMirror : public modm::BlockDevice { public: /// Initializes the storage hardware - modm::ResumableResult + bool initialize(); /// Deinitializes the storage hardware - modm::ResumableResult + bool deinitialize(); /** Read data from one or more blocks @@ -52,7 +52,7 @@ class BdMirror : public modm::BlockDevice, protected NestedResumable<3> * @param size Size to read in bytes (multiple of read block size) * @return True on success */ - modm::ResumableResult + bool read(uint8_t* buffer, bd_address_t address, bd_size_t size); /** Program blocks with data @@ -64,7 +64,7 @@ class BdMirror : public modm::BlockDevice, protected NestedResumable<3> * @param size Size to write in bytes (multiple of read block size) * @return True on success */ - modm::ResumableResult + bool program(const uint8_t* buffer, bd_address_t address, bd_size_t size); /** Erase blocks @@ -75,7 +75,7 @@ class BdMirror : public modm::BlockDevice, protected NestedResumable<3> * @param size Size to erase in bytes (multiple of read block size) * @return True on success */ - modm::ResumableResult + bool erase(bd_address_t address, bd_size_t size); /** Writes data to one or more blocks after erasing them @@ -87,7 +87,7 @@ class BdMirror : public modm::BlockDevice, protected NestedResumable<3> * @param size Size to write in bytes (multiple of read block size) * @return True on success */ - modm::ResumableResult + bool write(const uint8_t* buffer, bd_address_t address, bd_size_t size); public: @@ -112,11 +112,6 @@ class BdMirror : public modm::BlockDevice, protected NestedResumable<3> private: BlockDeviceA blockDeviceA; BlockDeviceB blockDeviceB; - -private: - bool resultA; - bool resultB; - }; } diff --git a/src/modm/driver/storage/block_device_mirror_impl.hpp b/src/modm/driver/storage/block_device_mirror_impl.hpp index a5ecd76457..2777c26d58 100644 --- a/src/modm/driver/storage/block_device_mirror_impl.hpp +++ b/src/modm/driver/storage/block_device_mirror_impl.hpp @@ -18,33 +18,23 @@ // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdMirror::initialize() { - RF_BEGIN(); - - resultA = RF_CALL(blockDeviceA.initialize()); - resultB = RF_CALL(blockDeviceB.initialize()); - - RF_END_RETURN(resultA && resultA); + return blockDeviceA.initialize() and blockDeviceB.initialize(); } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdMirror::deinitialize() { - RF_BEGIN(); - - resultA = RF_CALL(blockDeviceA.deinitialize()); - resultB = RF_CALL(blockDeviceB.deinitialize()); - - RF_END_RETURN(resultA && resultA); + return blockDeviceA.deinitialize() and blockDeviceB.deinitialize(); } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdMirror::read(uint8_t* buffer, bd_address_t address, bd_size_t size) { return blockDeviceA.read(buffer, address, size); @@ -52,53 +42,38 @@ modm::BdMirror::read(uint8_t* buffer, bd_address_t a // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdMirror::program(const uint8_t* buffer, bd_address_t address, bd_size_t size) { - RF_BEGIN(); - if((size == 0) || (size % BlockSizeWrite != 0)) { - RF_RETURN(false); + return false; } - resultA = RF_CALL(blockDeviceA.program(buffer, address, size)); - resultB = RF_CALL(blockDeviceB.program(buffer, address, size)); - - RF_END_RETURN(resultA && resultA); + return blockDeviceA.program(buffer, address, size) and blockDeviceB.program(buffer, address, size); } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdMirror::erase(bd_address_t address, bd_size_t size) { - RF_BEGIN(); - if((size == 0) || (size % BlockSizeErase != 0)) { - RF_RETURN(false); + return false; } - resultA = RF_CALL(blockDeviceA.erase(address, size)); - resultB = RF_CALL(blockDeviceB.erase(address, size)); - - RF_END_RETURN(resultA && resultA); + return blockDeviceA.erase(address, size) and blockDeviceB.erase(address, size); } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdMirror::write(const uint8_t* buffer, bd_address_t address, bd_size_t size) { - RF_BEGIN(); - if((size == 0) || (size % BlockSizeErase != 0) || (size % BlockSizeWrite != 0)) { - RF_RETURN(false); + return false; } - resultA = RF_CALL(blockDeviceA.write(buffer, address, size)); - resultB = RF_CALL(blockDeviceB.write(buffer, address, size)); - - RF_END_RETURN(resultA && resultA); + return blockDeviceA.write(buffer, address, size) and blockDeviceB.write(buffer, address, size); } diff --git a/src/modm/driver/storage/block_device_spiflash.hpp b/src/modm/driver/storage/block_device_spiflash.hpp index 019544f0b1..e90895c76a 100644 --- a/src/modm/driver/storage/block_device_spiflash.hpp +++ b/src/modm/driver/storage/block_device_spiflash.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include namespace modm { @@ -39,15 +39,15 @@ namespace modm * \author Rasmus Kleist Hørlyck Sørensen */ template -class BdSpiFlash : public modm::BlockDevice, public modm::SpiDevice< Spi >, protected NestedResumable<6> +class BdSpiFlash : public modm::BlockDevice, public modm::SpiDevice< Spi > { public: /// Initializes the storage hardware - modm::ResumableResult + bool initialize(); /// Deinitializes the storage hardware - modm::ResumableResult + bool deinitialize(); /** Read data from one or more blocks @@ -57,7 +57,7 @@ class BdSpiFlash : public modm::BlockDevice, public modm::SpiDevice< Spi >, prot * @param size Size to read in bytes (multiple of read block size) * @return True on success */ - modm::ResumableResult + bool read(uint8_t* buffer, bd_address_t address, bd_size_t size); /** Program blocks with data @@ -69,7 +69,7 @@ class BdSpiFlash : public modm::BlockDevice, public modm::SpiDevice< Spi >, prot * @param size Size to write in bytes (multiple of read block size) * @return True on success */ - modm::ResumableResult + bool program(const uint8_t* buffer, bd_address_t address, bd_size_t size); /** Erase blocks @@ -80,7 +80,7 @@ class BdSpiFlash : public modm::BlockDevice, public modm::SpiDevice< Spi >, prot * @param size Size to erase in bytes (multiple of read block size) * @return True on success */ - modm::ResumableResult + bool erase(bd_address_t address, bd_size_t size); /** Writes data to one or more blocks after erasing them @@ -92,7 +92,7 @@ class BdSpiFlash : public modm::BlockDevice, public modm::SpiDevice< Spi >, prot * @param size Size to write in bytes (multiple of read block size) * @return True on success */ - modm::ResumableResult + bool write(const uint8_t* buffer, bd_address_t address, bd_size_t size); public: @@ -120,7 +120,7 @@ class BdSpiFlash : public modm::BlockDevice, public modm::SpiDevice< Spi >, prot * * @return The Jedec Id returned from the flash chip */ - modm::ResumableResult + JedecId readId(); public: @@ -142,7 +142,7 @@ class BdSpiFlash : public modm::BlockDevice, public modm::SpiDevice< Spi >, prot * * @return Status register */ - modm::ResumableResult + StatusRegister readStatus(); public: @@ -151,7 +151,7 @@ class BdSpiFlash : public modm::BlockDevice, public modm::SpiDevice< Spi >, prot * * @param die The pre-assigned “Die ID#” of the die to select */ - modm::ResumableResult + void selectDie(uint8_t die); public: @@ -221,24 +221,24 @@ class BdSpiFlash : public modm::BlockDevice, public modm::SpiDevice< Spi >, prot * * @return True if device is busy. */ - modm::ResumableResult + bool isBusy(); /** This function can be used in another resumable function * to wait until the flash operation is finished. */ - modm::ResumableResult + void waitWhileBusy(); private: /** Send a non-addressed operation instruction to the spi flash chip */ - modm::ResumableResult + void spiOperation(Instruction instruction, const uint8_t* tx = nullptr, uint8_t* rx = nullptr, std::size_t length = 0, uint8_t dummyCycles = 0); /** Send an addressed operation instruction to the spi flash chip */ - modm::ResumableResult + void spiOperation(Instruction instruction, bd_address_t address, const uint8_t* tx = nullptr, uint8_t* rx = nullptr, std::size_t length = 0, uint8_t dummyCycles = 0); }; diff --git a/src/modm/driver/storage/block_device_spiflash_impl.hpp b/src/modm/driver/storage/block_device_spiflash_impl.hpp index 56ffb55fe1..5758c4e473 100644 --- a/src/modm/driver/storage/block_device_spiflash_impl.hpp +++ b/src/modm/driver/storage/block_device_spiflash_impl.hpp @@ -18,238 +18,211 @@ // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdSpiFlash::initialize() { - RF_BEGIN(); this->attachConfigurationHandler([]() { Spi::setDataMode(Spi::DataMode::Mode0); Spi::setDataOrder(Spi::DataOrder::MsbFirst); }); Cs::setOutput(modm::Gpio::High); - RF_CALL(spiOperation(Instruction::RstEn)); + spiOperation(Instruction::RstEn); // Wait T_CPH = 25ns - RF_CALL(spiOperation(Instruction::Rst)); - RF_CALL(waitWhileBusy()); + spiOperation(Instruction::Rst); + waitWhileBusy(); - RF_CALL(spiOperation(Instruction::WE)); - RF_CALL(spiOperation(Instruction::GBU)); - RF_CALL(waitWhileBusy()); + spiOperation(Instruction::WE); + spiOperation(Instruction::GBU); + waitWhileBusy(); // Enter 4-Byte Address mode for serial flash memory that support 256M-bit or more if (DeviceSize > ExtendedAddressThreshold) { - RF_CALL(spiOperation(Instruction::En4BAM)); - RF_CALL(waitWhileBusy()); + spiOperation(Instruction::En4BAM); + waitWhileBusy(); } - RF_END_RETURN(true); + return true; } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdSpiFlash::deinitialize() { - RF_BEGIN(); // nothing - RF_END_RETURN(true); + return true; } // ---------------------------------------------------------------------------- template -modm::ResumableResult::JedecId> +typename modm::BdSpiFlash::JedecId modm::BdSpiFlash::readId() { - RF_BEGIN(); + spiOperation(Instruction::RJI, nullptr, resultBuffer, 3); - RF_CALL(spiOperation(Instruction::RJI, nullptr, resultBuffer, 3)); - - RF_END_RETURN(JedecId(resultBuffer[0], resultBuffer[1], resultBuffer[2])); + return JedecId(resultBuffer[0], resultBuffer[1], resultBuffer[2]); } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdSpiFlash::read(uint8_t* buffer, bd_address_t address, bd_size_t size) { - RF_BEGIN(); - if((size == 0) || (size % BlockSizeRead != 0) || (address + size > flashSize)) { - RF_RETURN(false); + return false; } - RF_CALL(waitWhileBusy()); - RF_CALL(spiOperation(Instruction::FR, address, nullptr, buffer, size, 1)); + waitWhileBusy(); + spiOperation(Instruction::FR, address, nullptr, buffer, size, 1); - RF_END_RETURN(true); + return true; } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdSpiFlash::program(const uint8_t* buffer, bd_address_t address, bd_size_t size) { - RF_BEGIN(); - if((size == 0) || (size % BlockSizeWrite != 0) || (address + size > flashSize)) { - RF_RETURN(false); + return false; } index = 0; while(index < size) { - RF_CALL(waitWhileBusy()); - RF_CALL(spiOperation(Instruction::WE)); - RF_CALL(spiOperation(Instruction::PP, address + index, &buffer[index], nullptr, BlockSizeWrite)); + waitWhileBusy(); + spiOperation(Instruction::WE); + spiOperation(Instruction::PP, address + index, &buffer[index], nullptr, BlockSizeWrite); index += BlockSizeWrite; } - RF_END_RETURN(true); + return true; } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdSpiFlash::erase(bd_address_t address, bd_size_t size) { - RF_BEGIN(); - if((size == 0) || (size % BlockSizeErase != 0) || (address + size > flashSize)) { - RF_RETURN(false); + return false; } if (address == 0 && size == flashSize) { - RF_CALL(waitWhileBusy()); - RF_CALL(spiOperation(Instruction::CE)); + waitWhileBusy(); + spiOperation(Instruction::CE); } else { index = 0; while(index < size) { - RF_CALL(waitWhileBusy()); - RF_CALL(spiOperation(Instruction::WE)); - RF_CALL(spiOperation(Instruction::SE, address + index)); + waitWhileBusy(); + spiOperation(Instruction::WE); + spiOperation(Instruction::SE, address + index); index += BlockSizeErase; } } - RF_END_RETURN(true); + return true; } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdSpiFlash::write(const uint8_t* buffer, bd_address_t address, bd_size_t size) { - RF_BEGIN(); - if((size == 0) || (size % BlockSizeErase != 0) || (size % BlockSizeWrite != 0) || (address + size > flashSize)) { - RF_RETURN(false); + return false; } - if(!RF_CALL(this->erase(address, size))) { - RF_RETURN(false); + if(!this->erase(address, size)) { + return false; } - if(!RF_CALL(this->program(buffer, address, size))) { - RF_RETURN(false); + if(!this->program(buffer, address, size)) { + return false; } - RF_END_RETURN(true); + return true; } // ============================================================================ template -modm::ResumableResult::StatusRegister> +typename modm::BdSpiFlash::StatusRegister modm::BdSpiFlash::readStatus() { - RF_BEGIN(); - RF_CALL(spiOperation(Instruction::RSR1, nullptr, resultBuffer, 1)); - RF_END_RETURN(static_cast(resultBuffer[0])); + spiOperation(Instruction::RSR1, nullptr, resultBuffer, 1); + return static_cast(resultBuffer[0]); } template -modm::ResumableResult +void modm::BdSpiFlash::selectDie(uint8_t die) { - RF_BEGIN(); - - RF_CALL(spiOperation(Instruction::SDS, &die, nullptr, 1)); - RF_CALL(waitWhileBusy()); + spiOperation(Instruction::SDS, &die, nullptr, 1); + waitWhileBusy(); - RF_CALL(spiOperation(Instruction::WE)); - RF_CALL(spiOperation(Instruction::GBU)); - RF_CALL(waitWhileBusy()); + spiOperation(Instruction::WE); + spiOperation(Instruction::GBU); + waitWhileBusy(); // Enter 4-Byte Address mode for serial flash memory that support 256M-bit or more if (DeviceSize > ExtendedAddressThreshold) { - RF_CALL(spiOperation(Instruction::En4BAM)); - RF_CALL(waitWhileBusy()); + spiOperation(Instruction::En4BAM); + waitWhileBusy(); } - - RF_END(); } template -modm::ResumableResult +bool modm::BdSpiFlash::isBusy() { - RF_BEGIN(); - - if(RF_CALL(readStatus()) & StatusRegister::Busy) { - RF_RETURN(true); + if(readStatus() & StatusRegister::Busy) { + return true; } - RF_END_RETURN(false); + return false; } template -modm::ResumableResult +void modm::BdSpiFlash::waitWhileBusy() { - RF_BEGIN(); - while(RF_CALL(isBusy())) { - RF_YIELD(); + while(isBusy()) { + modm::this_fiber::yield(); } - RF_END(); } template -modm::ResumableResult +void modm::BdSpiFlash::spiOperation(Instruction instruction, const uint8_t* tx, uint8_t* rx, std::size_t length, uint8_t nrDummyCycles) { - RF_BEGIN(); - i = 0; instructionBuffer[i++] = static_cast(instruction); for(uint8_t j = 0; j < nrDummyCycles; j++) { instructionBuffer[i++] = 0x00; } - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); - RF_CALL(Spi::transfer(instructionBuffer, nullptr, i)); + Spi::transfer(instructionBuffer, nullptr, i); if(length > 0) { - RF_CALL(Spi::transfer(const_cast(tx), rx, length)); + Spi::transfer(const_cast(tx), rx, length); } if (this->releaseMaster()) { Cs::set(); } - - RF_END(); } template -modm::ResumableResult +void modm::BdSpiFlash::spiOperation(Instruction instruction, uint32_t address, const uint8_t* tx, uint8_t* rx, std::size_t length, uint8_t nrDummyCycles) { - RF_BEGIN(); - i = 0; instructionBuffer[i++] = static_cast(instruction); if constexpr (DeviceSize > ExtendedAddressThreshold) { @@ -266,18 +239,16 @@ modm::BdSpiFlash::spiOperation(Instruction instruction, uint instructionBuffer[i++] = 0x00; } - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); - RF_CALL(Spi::transfer(instructionBuffer, nullptr, i)); + Spi::transfer(instructionBuffer, nullptr, i); if(length > 0) { - RF_CALL(Spi::transfer(const_cast(tx), rx, length)); + Spi::transfer(const_cast(tx), rx, length); } if (this->releaseMaster()) { Cs::set(); } - - RF_END(); } diff --git a/src/modm/driver/storage/block_device_spistack_flash.hpp b/src/modm/driver/storage/block_device_spistack_flash.hpp index 4355110f87..5c3851d4fa 100644 --- a/src/modm/driver/storage/block_device_spistack_flash.hpp +++ b/src/modm/driver/storage/block_device_spistack_flash.hpp @@ -17,7 +17,7 @@ #include #include -#include +#include namespace modm { @@ -34,15 +34,15 @@ namespace modm * \author Rasmus Kleist Hørlyck Sørensen */ template -class BdSpiStackFlash : public modm::BlockDevice, protected NestedResumable<3> +class BdSpiStackFlash : public modm::BlockDevice { public: /// Initializes the storage hardware - modm::ResumableResult + bool initialize(); /// Deinitializes the storage hardware - modm::ResumableResult + bool deinitialize(); /** Read data from one or more blocks @@ -52,7 +52,7 @@ class BdSpiStackFlash : public modm::BlockDevice, protected NestedResumable<3> * @param size Size to read in bytes (multiple of read block size) * @return True on success */ - modm::ResumableResult + bool read(uint8_t* buffer, bd_address_t address, bd_size_t size); /** Program blocks with data @@ -64,7 +64,7 @@ class BdSpiStackFlash : public modm::BlockDevice, protected NestedResumable<3> * @param size Size to write in bytes (multiple of read block size) * @return True on success */ - modm::ResumableResult + bool program(const uint8_t* buffer, bd_address_t address, bd_size_t size); /** Erase blocks @@ -75,7 +75,7 @@ class BdSpiStackFlash : public modm::BlockDevice, protected NestedResumable<3> * @param size Size to erase in bytes (multiple of read block size) * @return True on success */ - modm::ResumableResult + bool erase(bd_address_t address, bd_size_t size); /** Writes data to one or more blocks after erasing them @@ -87,7 +87,7 @@ class BdSpiStackFlash : public modm::BlockDevice, protected NestedResumable<3> * @param size Size to write in bytes (multiple of read block size) * @return True on success */ - modm::ResumableResult + bool write(const uint8_t* buffer, bd_address_t address, bd_size_t size); public: @@ -95,13 +95,13 @@ class BdSpiStackFlash : public modm::BlockDevice, protected NestedResumable<3> * * @return True if any die in the stack is busy. */ - modm::ResumableResult + bool isBusy(); /** This function can be used in another resumable function * to wait until the flash operation is finished. */ - modm::ResumableResult + void waitWhileBusy(); public: diff --git a/src/modm/driver/storage/block_device_spistack_flash_impl.hpp b/src/modm/driver/storage/block_device_spistack_flash_impl.hpp index 8d33e73ce2..da0105d56a 100644 --- a/src/modm/driver/storage/block_device_spistack_flash_impl.hpp +++ b/src/modm/driver/storage/block_device_spistack_flash_impl.hpp @@ -17,164 +17,149 @@ // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdSpiStackFlash::initialize() { - RF_BEGIN(); - - if (RF_CALL(spiBlockDevice.initialize())) { - RF_CALL(spiBlockDevice.selectDie(currentDie = 0x00)); - RF_RETURN(true); + if (spiBlockDevice.initialize()) { + spiBlockDevice.selectDie(currentDie = 0x00); + return true; } - RF_END_RETURN(false); + return false; } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdSpiStackFlash::deinitialize() { - RF_BEGIN(); - RF_END_RETURN_CALL(spiBlockDevice.deinitialize()); + return spiBlockDevice.deinitialize(); } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdSpiStackFlash::read(uint8_t* buffer, bd_address_t address, bd_size_t size) { - RF_BEGIN(); - if((size == 0) || (size % BlockSizeRead != 0) || (address + size > DeviceSize)) { - RF_RETURN(false); + return false; } index = 0; while (index < size) { dv = std::ldiv(index + address, DieSize); // dv.quot = die #ID, dv.rem = die address if (currentDie != dv.quot) { - RF_CALL(spiBlockDevice.selectDie(currentDie = dv.quot)); + spiBlockDevice.selectDie(currentDie = dv.quot); } - if (RF_CALL(spiBlockDevice.read(&buffer[index], dv.rem, std::min(size - index, DieSize - dv.rem)))) { + if (spiBlockDevice.read(&buffer[index], dv.rem, std::min(size - index, DieSize - dv.rem))) { index += DieSize - dv.rem; // size - index <= DieSize - dv.rem only on last iteration! } else { - RF_RETURN(false); + return false; } } - RF_END_RETURN(true); + return true; } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdSpiStackFlash::program(const uint8_t* buffer, bd_address_t address, bd_size_t size) { - RF_BEGIN(); - if((size == 0) || (size % BlockSizeWrite != 0) || (address + size > DeviceSize)) { - RF_RETURN(false); + return false; } index = 0; while (index < size) { dv = std::ldiv(index + address, DieSize); // dv.quot = die #ID, dv.rem = die address if (currentDie != dv.quot) { - RF_CALL(spiBlockDevice.selectDie(currentDie = dv.quot)); + spiBlockDevice.selectDie(currentDie = dv.quot); } - if (RF_CALL(spiBlockDevice.program(&buffer[index], dv.rem, std::min(size - index, DieSize - dv.rem)))) { + if (spiBlockDevice.program(&buffer[index], dv.rem, std::min(size - index, DieSize - dv.rem))) { index += DieSize - dv.rem; // size - index <= DieSize - dv.rem only on last iteration! } else { - RF_RETURN(false); + return false; } } - RF_END_RETURN(true); + return true; } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdSpiStackFlash::erase(bd_address_t address, bd_size_t size) { - RF_BEGIN(); - if((size == 0) || (size % BlockSizeErase != 0) || (address + size > DeviceSize)) { - RF_RETURN(false); + return false; } index = 0; while (index < size) { dv = std::ldiv(index + address, DieSize); // dv.quot = die #ID, dv.rem = die address if (currentDie != dv.quot) { - RF_CALL(spiBlockDevice.selectDie(currentDie = dv.quot)); + spiBlockDevice.selectDie(currentDie = dv.quot); } - if (RF_CALL(spiBlockDevice.erase(dv.rem, std::min(size - index, DieSize - dv.rem)))) { + if (spiBlockDevice.erase(dv.rem, std::min(size - index, DieSize - dv.rem))) { index += DieSize - dv.rem; // size - index <= DieSize - dv.rem only on last iteration! } else { - RF_RETURN(false); + return false; } } - RF_END_RETURN(true); + return true; } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdSpiStackFlash::write(const uint8_t* buffer, bd_address_t address, bd_size_t size) { - RF_BEGIN(); - if((size == 0) || (size % BlockSizeErase != 0) || (size % BlockSizeWrite != 0) || (address + size > DeviceSize)) { - RF_RETURN(false); + return false; } - if(!RF_CALL(this->erase(address, size))) { - RF_RETURN(false); + if(!this->erase(address, size)) { + return false; } - if(!RF_CALL(this->program(buffer, address, size))) { - RF_RETURN(false); + if(!this->program(buffer, address, size)) { + return false; } - RF_END_RETURN(true); + return true; } // ---------------------------------------------------------------------------- template -modm::ResumableResult +bool modm::BdSpiStackFlash::isBusy() { - RF_BEGIN(); - currentDie = DieCount; while (currentDie > 0) { - RF_CALL(spiBlockDevice.selectDie(--currentDie)); - if (RF_CALL(spiBlockDevice.isBusy())) { - RF_RETURN(true); + spiBlockDevice.selectDie(--currentDie); + if (spiBlockDevice.isBusy()) { + return true; } } - RF_END_RETURN(false); + return false; } // ---------------------------------------------------------------------------- template -modm::ResumableResult +void modm::BdSpiStackFlash::waitWhileBusy() { - RF_BEGIN(); - while(RF_CALL(isBusy())) { - RF_YIELD(); + while(isBusy()) { + modm::this_fiber::yield(); } - RF_END(); } diff --git a/src/modm/driver/storage/cat24aa.hpp b/src/modm/driver/storage/cat24aa.hpp index 0352934b97..7376d1c1a9 100644 --- a/src/modm/driver/storage/cat24aa.hpp +++ b/src/modm/driver/storage/cat24aa.hpp @@ -12,7 +12,7 @@ #ifndef MODM_CAT24AA_HPP #define MODM_CAT24AA_HPP -#include +#include #include namespace modm @@ -72,7 +72,7 @@ class Cat24Aa : public modm::I2cDevice + inline bool writeByte(uint32_t address, uint8_t data) { return write(address, &data, 1); @@ -88,7 +88,7 @@ class Cat24Aa : public modm::I2cDevice + bool write(uint32_t address, const uint8_t *data, std::size_t length); /** @@ -100,21 +100,21 @@ class Cat24Aa : public modm::I2cDevice - inline modm::ResumableResult + inline bool write(uint32_t address, const T& data) { return write(address, reinterpret_cast(&data), sizeof(T)); } /// Read byte - inline modm::ResumableResult + inline bool readByte(uint32_t address, uint8_t &data) { return read(address, &data, 1); } /// Read block - modm::ResumableResult + bool read(uint32_t address, uint8_t *data, std::size_t length); /** @@ -126,7 +126,7 @@ class Cat24Aa : public modm::I2cDevice - inline modm::ResumableResult + inline bool read(uint16_t address, T& data) { return read(address, reinterpret_cast(&data), sizeof(T)); diff --git a/src/modm/driver/storage/cat24aa_impl.hpp b/src/modm/driver/storage/cat24aa_impl.hpp index 863b97b3d5..93274ef709 100644 --- a/src/modm/driver/storage/cat24aa_impl.hpp +++ b/src/modm/driver/storage/cat24aa_impl.hpp @@ -21,32 +21,28 @@ modm::Cat24Aa::Cat24Aa() : } template -modm::ResumableResult +bool modm::Cat24Aa::write(uint32_t address, const uint8_t *data, std::size_t length) { - RF_BEGIN(); - this->setAddress(this->transaction.getAddress() | ((address >> 8) & 0x07)); - RF_WAIT_UNTIL( this->transaction.configureWrite(address, data, length) and this->startTransaction() ); + modm::this_fiber::poll([&]{ return this->transaction.configureWrite(address, data, length) and this->startTransaction(); }); - RF_WAIT_WHILE( this->isTransactionRunning() ); + modm::this_fiber::poll([&]{ return not this->isTransactionRunning(); }); - RF_END_RETURN( this->wasTransactionSuccessful() ); + return this->wasTransactionSuccessful(); } template -modm::ResumableResult +bool modm::Cat24Aa::read(uint32_t address, uint8_t *data, std::size_t length) { - RF_BEGIN(); - this->setAddress(this->transaction.getAddress() | ((address >> 8) & 0x07)); - RF_WAIT_UNTIL( this->transaction.configureRead(address, data, length) and this->startTransaction() ); + modm::this_fiber::poll([&]{ return this->transaction.configureRead(address, data, length) and this->startTransaction(); }); - RF_WAIT_WHILE( this->isTransactionRunning() ); + modm::this_fiber::poll([&]{ return not this->isTransactionRunning(); }); - RF_END_RETURN( this->wasTransactionSuccessful() ); + return this->wasTransactionSuccessful(); } diff --git a/src/modm/driver/storage/i2c_eeprom.hpp b/src/modm/driver/storage/i2c_eeprom.hpp index a488ea9536..1db4a00530 100644 --- a/src/modm/driver/storage/i2c_eeprom.hpp +++ b/src/modm/driver/storage/i2c_eeprom.hpp @@ -16,7 +16,7 @@ #ifndef MODM_I2C_EEPROM_HPP #define MODM_I2C_EEPROM_HPP -#include +#include #include namespace modm @@ -81,7 +81,7 @@ class I2cEeprom : public I2cDevice + inline bool writeByte(uint32_t address, uint8_t data) { return write(address, &data, 1); @@ -97,7 +97,7 @@ class I2cEeprom : public I2cDevice + bool write(uint32_t address, const uint8_t *data, std::size_t length); /** @@ -109,21 +109,21 @@ class I2cEeprom : public I2cDevice - inline modm::ResumableResult + inline bool write(uint32_t address, const T& data) { return write(address, reinterpret_cast(&data), sizeof(T)); } /// Read byte - inline modm::ResumableResult + inline bool readByte(uint32_t address, uint8_t &data) { return read(address, &data, 1); } /// Read block - modm::ResumableResult + bool read(uint32_t address, uint8_t *data, std::size_t length); /** @@ -135,7 +135,7 @@ class I2cEeprom : public I2cDevice - inline modm::ResumableResult + inline bool read(uint16_t address, T& data) { return read(address, reinterpret_cast(&data), sizeof(T)); diff --git a/src/modm/driver/storage/i2c_eeprom_impl.hpp b/src/modm/driver/storage/i2c_eeprom_impl.hpp index 8ac0d77400..f1fd0da90e 100644 --- a/src/modm/driver/storage/i2c_eeprom_impl.hpp +++ b/src/modm/driver/storage/i2c_eeprom_impl.hpp @@ -27,11 +27,9 @@ modm::I2cEeprom::I2cEeprom(uint8_t address) : // MARK: - write operations template -modm::ResumableResult +bool modm::I2cEeprom::write(uint32_t address, const uint8_t *data, std::size_t length) { - RF_BEGIN(); - if constexpr (AddressBytes > 1) { if (address & 0x10000) this->setAddress(this->transaction.getAddress() | 0b100); else this->setAddress(this->transaction.getAddress() & ~0b100); @@ -39,20 +37,18 @@ modm::I2cEeprom::write(uint32_t address, const uint8_t this->setAddress(this->transaction.getAddress()); } - RF_WAIT_UNTIL( this->transaction.configureWrite(address, data, length) and this->startTransaction() ); + modm::this_fiber::poll([&]{ return this->transaction.configureWrite(address, data, length) and this->startTransaction(); }); - RF_WAIT_WHILE( this->isTransactionRunning() ); + modm::this_fiber::poll([&]{ return not this->isTransactionRunning(); }); - RF_END_RETURN( this->wasTransactionSuccessful() ); + return this->wasTransactionSuccessful(); } // MARK: - read operations template -modm::ResumableResult +bool modm::I2cEeprom::read(uint32_t address, uint8_t *data, std::size_t length) { - RF_BEGIN(); - if constexpr (AddressBytes > 1) { if (address & 0x10000) this->setAddress(this->transaction.getAddress() | 0b100); else this->setAddress(this->transaction.getAddress() & ~0b100); @@ -60,11 +56,11 @@ modm::I2cEeprom::read(uint32_t address, uint8_t *data, this->setAddress(this->transaction.getAddress()); } - RF_WAIT_UNTIL( this->transaction.configureRead(address, data, length) and this->startTransaction() ); + modm::this_fiber::poll([&]{ return this->transaction.configureRead(address, data, length) and this->startTransaction(); }); - RF_WAIT_WHILE( this->isTransactionRunning() ); + modm::this_fiber::poll([&]{ return not this->isTransactionRunning(); }); - RF_END_RETURN( this->wasTransactionSuccessful() ); + return this->wasTransactionSuccessful(); } namespace modm::i2c_eeprom::detail diff --git a/src/modm/driver/storage/spi_ram_impl.hpp b/src/modm/driver/storage/spi_ram_impl.hpp index 5a0ad48357..8d4c72b30d 100644 --- a/src/modm/driver/storage/spi_ram_impl.hpp +++ b/src/modm/driver/storage/spi_ram_impl.hpp @@ -15,7 +15,7 @@ #error "Don't include this file directly, use 'spi_ram.hpp' instead!" #endif -#include +#include template Spi modm::SpiRam::spi; @@ -39,14 +39,14 @@ modm::SpiRam::initialize() hold.set(); hold.setOutput(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); cs.reset(); spi.write(WRITE_STATUS_REGISTER); spi.write(SEQUENTIAL_MODE); cs.set(); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); // Check if the status register has the right content. This also used // as a general check that the device is available. diff --git a/src/modm/driver/temperature/ds1631.hpp b/src/modm/driver/temperature/ds1631.hpp index a4946cf08f..7dd98a1b4d 100644 --- a/src/modm/driver/temperature/ds1631.hpp +++ b/src/modm/driver/temperature/ds1631.hpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include "lm75.hpp" namespace modm @@ -109,8 +109,7 @@ struct ds1631 * @author Niklas Hauser */ template < typename I2cMaster > -class Ds1631 : public ds1631, public I2cDevice< I2cMaster, 2 >, - protected modm::pt::Protothread +class Ds1631 : public ds1631, public I2cDevice< I2cMaster > { public: /// @@ -119,47 +118,47 @@ class Ds1631 : public ds1631, public I2cDevice< I2cMaster, 2 >, bool update(); - modm::ResumableResult + bool initialize(); // @param rate Update rate in Hz: 1 to 33. - modm::ResumableResult + bool setUpdateRate(uint8_t rate); - modm::ResumableResult + bool setResolution(Resolution resolution); - modm::ResumableResult + bool setAlertPolarity(AlertPolarity polarity); - modm::ResumableResult + bool setConversionMode(ConversionMode mode); /// Writes the upper limit of the alarm. - modm::ResumableResult + bool setUpperLimit(float temperature) { return setLimitRegister(Command::TemperatureUpperLimit, temperature); } /// Writes the lower limit of the alarm. - modm::ResumableResult + bool setLowerLimit(float temperature) { return setLimitRegister(Command::TemperatureLowerLimit, temperature); } /// reads the Temperature registers and buffers the results - modm::ResumableResult + bool readTemperature(); - modm::ResumableResult + bool startConversion() { return writeCommand(Command::StartConvert); } - modm::ResumableResult + bool stopConversion() { return writeCommand(Command::StopConvert); } - modm::ResumableResult + bool reset() { return writeCommand(Command::SoftwareReset); } @@ -169,23 +168,21 @@ class Ds1631 : public ds1631, public I2cDevice< I2cMaster, 2 >, { return data; } private: - modm::ResumableResult + bool writeCommand(Command cmd); - modm::ResumableResult + bool writeConfiguration(); - modm::ResumableResult + bool setLimitRegister(Command cmd, float temperature); Data &data; uint8_t buffer[3]; - Config_t config; + Config_t config{0}; - modm::ShortDuration updateTime; - modm::ShortDuration conversionTime; - modm::ShortTimeout periodTimeout; - modm::ShortTimeout conversionTimeout; + modm::ShortDuration conversionTime{232ms}; + modm::ShortPeriodicTimer timer{250ms}; }; } // namespace modm diff --git a/src/modm/driver/temperature/ds1631.lb b/src/modm/driver/temperature/ds1631.lb index 9863bc2ee5..f6235b1331 100644 --- a/src/modm/driver/temperature/ds1631.lb +++ b/src/modm/driver/temperature/ds1631.lb @@ -20,7 +20,7 @@ def prepare(module, options): ":architecture:i2c.device", ":architecture:register", ":driver:lm75", - ":processing:protothread") + ":processing:timer") return True def build(env): diff --git a/src/modm/driver/temperature/ds1631_impl.hpp b/src/modm/driver/temperature/ds1631_impl.hpp index 3819fd065f..82251e2fef 100644 --- a/src/modm/driver/temperature/ds1631_impl.hpp +++ b/src/modm/driver/temperature/ds1631_impl.hpp @@ -19,153 +19,112 @@ // ---------------------------------------------------------------------------- template modm::Ds1631::Ds1631(Data &data, uint8_t address) : - I2cDevice(address), data(data), config(0), - updateTime(250), conversionTime(232), - periodTimeout(updateTime), conversionTimeout(conversionTime) + I2cDevice(address), data(data) { - this->stop(); } template < typename I2cMaster > -modm::ResumableResult +bool modm::Ds1631::initialize() { - RF_BEGIN(); - buffer[0] = uint8_t(Command::Configuration); - - this->transaction.configureWriteRead(buffer, 1, &config, 1); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::writeRead(buffer, 1, &config, 1); } template < typename I2cMaster > bool modm::Ds1631::update() { - PT_BEGIN(); - - while(true) + if (timer.execute()) { - PT_WAIT_UNTIL(periodTimeout.isExpired()); - periodTimeout.restart(updateTime); - if (config.none(Config::OneShot)) { - PT_CALL(startConversion()); - - conversionTimeout.restart(conversionTime); - PT_WAIT_UNTIL(conversionTimeout.isExpired()); + startConversion(); + modm::this_fiber::sleep_for(std::chrono::milliseconds(conversionTime.count())); } - PT_CALL(readTemperature()); + readTemperature(); + return true; } - - PT_END(); + return false; } template < typename I2cMaster > -modm::ResumableResult +bool modm::Ds1631::setUpdateRate(uint8_t rate) { - RF_BEGIN(); - // clamp conversion rate to max 33Hz (=~30ms) if (rate == 0) rate = 1; if (rate > 33) rate = 33; if (config.any(Config::OneShot)) { - if (not RF_CALL(startConversion())) - RF_RETURN(false); + if (not startConversion()) + return false; } - updateTime = modm::ShortDuration(1000/rate - 29); - periodTimeout.restart(updateTime); - this->restart(); - - RF_END_RETURN(true); + periodTimeout.restart(1000/rate - 29); + return true; } template < typename I2cMaster > -modm::ResumableResult +bool modm::Ds1631::setResolution(Resolution resolution) { - RF_BEGIN(); - Resolution_t::set(config, resolution); - RF_END_RETURN_CALL( writeConfiguration() ); + return writeConfiguration(); } template < typename I2cMaster > -modm::ResumableResult +bool modm::Ds1631::setAlertPolarity(AlertPolarity polarity) { - RF_BEGIN(); - config.update(Config::Polarity, bool(polarity)); - RF_END_RETURN_CALL( writeConfiguration() ); + return writeConfiguration(); } template < typename I2cMaster > -modm::ResumableResult +bool modm::Ds1631::setConversionMode(ConversionMode mode) { - RF_BEGIN(); - config.update(Config::OneShot, bool(mode)); - RF_END_RETURN_CALL( writeConfiguration() ); + return writeConfiguration(); } // MARK: read temperature template < typename I2cMaster > -modm::ResumableResult +bool modm::Ds1631::readTemperature() { - RF_BEGIN(); - buffer[0] = uint8_t(Command::Temperature); - this->transaction.configureWriteRead(buffer, 1, data.data, 2); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::writeRead(buffer, 1, data.data, 2); } // MARK: configuration template < typename I2cMaster > -modm::ResumableResult +bool modm::Ds1631::writeConfiguration() { - RF_BEGIN(); - buffer[0] = uint8_t(Command::Configuration); buffer[1] = config.value; - - this->transaction.configureWrite(buffer, 2); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(buffer, 2); } template < typename I2cMaster > -modm::ResumableResult +bool modm::Ds1631::writeCommand(Command cmd) { - RF_BEGIN(); - buffer[0] = uint8_t(cmd); - this->transaction.configureWrite(buffer, 1); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(buffer, 1); } template < typename I2cMaster > -modm::ResumableResult +bool modm::Ds1631::setLimitRegister(Command cmd, float temperature) { - RF_BEGIN(); - { uint8_t res = uint8_t(Resolution_t::get(config)); @@ -176,8 +135,5 @@ modm::Ds1631::setLimitRegister(Command cmd, float temperature) buffer[1] = (temp >> 8); buffer[2] = temp; } - - this->transaction.configureWrite(buffer, 3); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(buffer, 3); } diff --git a/src/modm/driver/temperature/lm75.hpp b/src/modm/driver/temperature/lm75.hpp index 0bb74bb64d..59b3f32fe4 100644 --- a/src/modm/driver/temperature/lm75.hpp +++ b/src/modm/driver/temperature/lm75.hpp @@ -122,7 +122,7 @@ struct lm75 * @author Niklas Hauser */ template < class I2cMaster > -class Lm75 : public lm75, public I2cDevice< I2cMaster, 2 > +class Lm75 : public lm75, public I2cDevice< I2cMaster > { template < class OtherI2cMaster > friend class Tmp102; @@ -135,21 +135,21 @@ class Lm75 : public lm75, public I2cDevice< I2cMaster, 2 > /// sets address to default of 0x48 (7 alternative addresses up to 0x4F possible). Lm75(Data &data, uint8_t address=0x48); - modm::ResumableResult + bool configureAlertMode(ThermostatMode mode, AlertPolarity polarity, FaultQueue faults); /// Writes the upper limit of the alarm. - modm::ResumableResult + bool setUpperLimit(float temperature) { return setLimitRegister(Register::TemperatureUpperLimit, temperature); } /// Writes the lower limit of the alarm. - modm::ResumableResult + bool setLowerLimit(float temperature) { return setLimitRegister(Register::TemperatureLowerLimit, temperature); } /// reads the Temperature registers and buffers the results - modm::ResumableResult + bool readTemperature(); inline Data& @@ -157,7 +157,7 @@ class Lm75 : public lm75, public I2cDevice< I2cMaster, 2 > { return data; } private: - modm::ResumableResult + bool setLimitRegister(Register reg, float temperature); Data &data; diff --git a/src/modm/driver/temperature/lm75_impl.hpp b/src/modm/driver/temperature/lm75_impl.hpp index ab40370229..5fe6aec635 100644 --- a/src/modm/driver/temperature/lm75_impl.hpp +++ b/src/modm/driver/temperature/lm75_impl.hpp @@ -18,7 +18,7 @@ // ---------------------------------------------------------------------------- template < typename I2cMaster > modm::Lm75::Lm75(Data &data, uint8_t address) : - I2cDevice(address), + I2cDevice(address), data(data), config_msb(0) { } @@ -27,43 +27,32 @@ modm::Lm75::Lm75(Data &data, uint8_t address) : // MARK: - tasks // MARK: Alert mode template < typename I2cMaster > -modm::ResumableResult +bool modm::Lm75::configureAlertMode(ThermostatMode mode, AlertPolarity polarity, FaultQueue faults) { - RF_BEGIN(); - config_msb.update(Config1::ThermostatMode, bool(mode)); config_msb.update(Config1::Polarity, bool(polarity)); FaultQueue_t::set(config_msb, faults); buffer[0] = uint8_t(Register::Configuration); buffer[1] = config_msb.value; - - this->transaction.configureWrite(buffer, 2); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(buffer, 2); } // MARK: read temperature template < typename I2cMaster > -modm::ResumableResult +bool modm::Lm75::readTemperature() { - RF_BEGIN(); - buffer[0] = uint8_t(Register::Temperature); - this->transaction.configureWriteRead(buffer, 1, data.data, 2); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::writeRead(buffer, 1, data.data, 2); } // MARK: configuration template < typename I2cMaster > -modm::ResumableResult +bool modm::Lm75::setLimitRegister(Register reg, float temperature) { - RF_BEGIN(); - { int16_t temp = temperature * 2.f; temp <<= 7; @@ -72,8 +61,5 @@ modm::Lm75::setLimitRegister(Register reg, float temperature) buffer[1] = (temp >> 8); buffer[2] = temp; } - - this->transaction.configureWrite(buffer, 3); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(buffer, 3); } diff --git a/src/modm/driver/temperature/ltc2984.hpp b/src/modm/driver/temperature/ltc2984.hpp index 70fabe531e..f4e9091835 100644 --- a/src/modm/driver/temperature/ltc2984.hpp +++ b/src/modm/driver/temperature/ltc2984.hpp @@ -16,7 +16,7 @@ #include #include #include -#include +#include namespace modm { @@ -340,7 +340,7 @@ inline ltc2984::CommandStatus operator |(ltc2984::CommandStatus r, ltc2984::Chan * \author Raphael Lehmann */ template < class SpiMaster, class Cs > -class Ltc2984 : public ltc2984, public modm::SpiDevice< SpiMaster >, protected modm::NestedResumable<3> +class Ltc2984 : public ltc2984, public modm::SpiDevice< SpiMaster > { public: /** @@ -358,7 +358,7 @@ class Ltc2984 : public ltc2984, public modm::SpiDevice< SpiMaster >, protected m * * \return config True if the device is reachable. */ - modm::ResumableResult + bool ping(); /** @@ -369,7 +369,7 @@ class Ltc2984 : public ltc2984, public modm::SpiDevice< SpiMaster >, protected m * \param temperatureUnit Temperature unit to use, °C or °F * \param muxDelay Mux configuration delay in 100 microseconds (maximum: 255 x 100us = 25.5ms) */ - modm::ResumableResult + void configure(ltc2984::Configuration::Rejection rejection, ltc2984::Configuration::TemperatureUnit temperatureUnit, uint8_t muxDelay = 0); @@ -380,7 +380,7 @@ class Ltc2984 : public ltc2984, public modm::SpiDevice< SpiMaster >, protected m * * \return True if device is busy. */ - modm::ResumableResult + bool isBusy(); /** @@ -389,7 +389,7 @@ class Ltc2984 : public ltc2984, public modm::SpiDevice< SpiMaster >, protected m * \param channel The channel to configure. * \param config The configuration for the channel. */ - modm::ResumableResult + void configureChannel(ltc2984::Channel channel, uint32_t config); /** @@ -397,13 +397,13 @@ class Ltc2984 : public ltc2984, public modm::SpiDevice< SpiMaster >, protected m * * \param channel The channel to measure. */ - modm::ResumableResult + void initiateSingleMeasurement(ltc2984::Channel channel); /** * \brief Initiate measurements for all actived channels */ - modm::ResumableResult + void initiateMeasurements(); /** @@ -429,13 +429,13 @@ class Ltc2984 : public ltc2984, public modm::SpiDevice< SpiMaster >, protected m /** * \brief Write actived channel configuartion to device */ - modm::ResumableResult + void setChannels(); /** * \brief Enter sleep mode */ - modm::ResumableResult + void enterSleepMode(); /** @@ -445,20 +445,20 @@ class Ltc2984 : public ltc2984, public modm::SpiDevice< SpiMaster >, protected m * \param value The temperature value as configured before. * \return True if result is valid. */ - modm::ResumableResult + void readChannel(ltc2984::Channel channel, ltc2984::Data& value); protected: /// Write 32-bit word to ltc2984 memory - modm::ResumableResult + void writeData(Register address, uint8_t* word, size_t length); - modm::ResumableResult + void writeData(Register address, CommandStatus command); /// Read 32-bit word to ltc2984 memory - modm::ResumableResult + void readFourBytes(Register address, uint8_t* data); - modm::ResumableResult + void readByte(Register address, uint8_t& byte); private: diff --git a/src/modm/driver/temperature/ltc2984.lb b/src/modm/driver/temperature/ltc2984.lb index da16cbe3e0..1edafcc75f 100644 --- a/src/modm/driver/temperature/ltc2984.lb +++ b/src/modm/driver/temperature/ltc2984.lb @@ -29,7 +29,7 @@ def prepare(module, options): ":architecture:gpio", ":architecture:register", ":architecture:spi.device", - ":processing:resumable") + ":architecture:fiber") return True def build(env): diff --git a/src/modm/driver/temperature/ltc2984_impl.hpp b/src/modm/driver/temperature/ltc2984_impl.hpp index 9b83a3f517..183a4fb826 100644 --- a/src/modm/driver/temperature/ltc2984_impl.hpp +++ b/src/modm/driver/temperature/ltc2984_impl.hpp @@ -23,58 +23,55 @@ modm::Ltc2984::Ltc2984() } template < class SpiMaster, class Cs > -modm::ResumableResult +bool modm::Ltc2984::ping() { - RF_BEGIN(); // Store first byte of Custom Sensor Table Data into buffer[4] - RF_CALL(readByte(Register::CustomDataTable, buffer[4])); + readByte(Register::CustomDataTable, buffer[4]); buffer[5] = 0x42; - RF_CALL(writeData(Register::CustomDataTable, &buffer[5], 1)); - RF_CALL(readByte(Register::CustomDataTable, buffer[6])); + writeData(Register::CustomDataTable, &buffer[5], 1); + readByte(Register::CustomDataTable, buffer[6]); // Restore first byte of Custom Sensor Table Data from buffer[4] - RF_CALL(writeData(Register::CustomDataTable, &buffer[4], 1)); - RF_END_RETURN(buffer[5] == buffer[6]); + writeData(Register::CustomDataTable, &buffer[4], 1); + return buffer[5] == buffer[6]; } template < class SpiMaster, class Cs > -modm::ResumableResult +void modm::Ltc2984::configure(ltc2984::Configuration::Rejection rejection, ltc2984::Configuration::TemperatureUnit temperatureUnit, uint8_t muxDelay) { - RF_BEGIN(); buffer[0] = static_cast(rejection) | static_cast(temperatureUnit); - RF_CALL(writeData(ltc2984::Register::GlobalConfiguration, buffer, 1)); - RF_END_RETURN_CALL(writeData(ltc2984::Register::MuxConfigDelay, muxDelay, 1)); + writeData(ltc2984::Register::GlobalConfiguration, buffer, 1); + return writeData(ltc2984::Register::MuxConfigDelay, muxDelay, 1); } template < class SpiMaster, class Cs > -modm::ResumableResult +bool modm::Ltc2984::isBusy() { - RF_BEGIN(); - RF_CALL(readByte(Register::CommandStatus, buffer[0])); - RF_END_RETURN((static_cast(buffer[0]) & CommandStatus::DoneAndStart) == CommandStatus::Start); + readByte(Register::CommandStatus, buffer[0]); + return (static_cast(buffer[0]) & CommandStatus::DoneAndStart) == CommandStatus::Start; } // ---------------------------------------------------------------------------- template < class SpiMaster, class Cs > -modm::ResumableResult +void modm::Ltc2984::configureChannel(ltc2984::Channel channel, uint32_t config) { return writeData(ltc2984::Register::ChannelConfig + channel, reinterpret_cast(&config), 4); } template < class SpiMaster, class Cs > -modm::ResumableResult +void modm::Ltc2984::initiateSingleMeasurement(ltc2984::Channel channel) { return writeData(ltc2984::Register::CommandStatus, ltc2984::CommandStatus::Start | channel); } template < class SpiMaster, class Cs > -modm::ResumableResult +void modm::Ltc2984::initiateMeasurements() { return writeData(ltc2984::Register::CommandStatus, CommandStatus::Start); @@ -95,14 +92,14 @@ modm::Ltc2984::disableChannel(ltc2984::Configuration::MuxChannel } template < class SpiMaster, class Cs > -modm::ResumableResult +void modm::Ltc2984::setChannels() { return writeData(ltc2984::Register::MuxChannels, reinterpret_cast(&enabledChannels), 4); } template < class SpiMaster, class Cs > -modm::ResumableResult +void modm::Ltc2984::enterSleepMode() { return writeData(ltc2984::Register::CommandStatus, CommandStatus::Sleep); @@ -110,7 +107,7 @@ modm::Ltc2984::enterSleepMode() // ---------------------------------------------------------------------------- template < class SpiMaster, class Cs > -modm::ResumableResult +void modm::Ltc2984::readChannel(ltc2984::Channel channel, ltc2984::Data& value) { return readFourBytes(ltc2984::Register::Results + channel, reinterpret_cast(&value.data)); @@ -118,16 +115,14 @@ modm::Ltc2984::readChannel(ltc2984::Channel channel, ltc2984::Dat // ---------------------------------------------------------------------------- template < class SpiMaster, class Cs > -modm::ResumableResult +void modm::Ltc2984::writeData(Register address, uint8_t* data, size_t length) { - RF_BEGIN(); - if (length > 4) { - RF_RETURN(); + return; } - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); buffer[0] = Write; @@ -140,29 +135,25 @@ modm::Ltc2984::writeData(Register address, uint8_t* data, size_t buffer[3+i] = data[length-1-i]; } - RF_CALL(SpiMaster::transfer(buffer, nullptr, length+3)); + SpiMaster::transfer(buffer, nullptr, length+3); if (this->releaseMaster()) { Cs::set(); } - - RF_END(); } template < class SpiMaster, class Cs > -modm::ResumableResult +void modm::Ltc2984::writeData(Register address, CommandStatus command) { return writeData(address, reinterpret_cast(&command), 1); } template < class SpiMaster, class Cs > -modm::ResumableResult +void modm::Ltc2984::readFourBytes(Register address, uint8_t* data) { - RF_BEGIN(); - - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); buffer[0] = Read; @@ -170,8 +161,8 @@ modm::Ltc2984::readFourBytes(Register address, uint8_t* data) buffer[1] = static_cast(address) >> 8; buffer[2] = static_cast(address); - RF_CALL(SpiMaster::transfer(buffer, nullptr, 3)); - RF_CALL(SpiMaster::transfer(nullptr, buffer, 4)); + SpiMaster::transfer(buffer, nullptr, 3); + SpiMaster::transfer(nullptr, buffer, 4); // swap byte order data[0] = buffer[3]; @@ -182,17 +173,13 @@ modm::Ltc2984::readFourBytes(Register address, uint8_t* data) if (this->releaseMaster()) { Cs::set(); } - - RF_END(); } template < class SpiMaster, class Cs > -modm::ResumableResult +void modm::Ltc2984::readByte(Register address, uint8_t& command) { - RF_BEGIN(); - - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); buffer[0] = Read; @@ -200,12 +187,10 @@ modm::Ltc2984::readByte(Register address, uint8_t& command) buffer[1] = static_cast(address) >> 8; buffer[2] = static_cast(address); - RF_CALL(SpiMaster::transfer(buffer, nullptr, 3)); - RF_CALL(SpiMaster::transfer(nullptr, &command, 1)); + SpiMaster::transfer(buffer, nullptr, 3); + SpiMaster::transfer(nullptr, &command, 1); if (this->releaseMaster()) { Cs::set(); } - - RF_END(); } diff --git a/src/modm/driver/temperature/max31855.hpp b/src/modm/driver/temperature/max31855.hpp index 8e911df458..18bb723672 100644 --- a/src/modm/driver/temperature/max31855.hpp +++ b/src/modm/driver/temperature/max31855.hpp @@ -14,7 +14,7 @@ #ifndef MODM_MAX31855_HPP #define MODM_MAX31855_HPP -#include +#include #include namespace modm @@ -71,7 +71,7 @@ struct max31855 * @ingroup modm_driver_max31855 */ template -class Max31855 : public max31855, public modm::SpiDevice, protected modm::NestedResumable<1> +class Max31855 : public max31855, public modm::SpiDevice { public: /** @@ -84,7 +84,7 @@ class Max31855 : public max31855, public modm::SpiDevice, protected m initialize(); /// Read the raw data from the sensor - modm::ResumableResult + void readout(); public: @@ -101,4 +101,4 @@ class Max31855 : public max31855, public modm::SpiDevice, protected m #include "max31855_impl.hpp" -#endif // MODM_MAX31855_HPP \ No newline at end of file +#endif // MODM_MAX31855_HPP diff --git a/src/modm/driver/temperature/max31855.lb b/src/modm/driver/temperature/max31855.lb index 9d4bd5e5ba..3be953ab8d 100644 --- a/src/modm/driver/temperature/max31855.lb +++ b/src/modm/driver/temperature/max31855.lb @@ -23,10 +23,10 @@ def prepare(module, options): module.depends( ":architecture:gpio", ":architecture:spi.device", - ":processing:resumable") + ":architecture:fiber") return True def build(env): env.outbasepath = "modm/src/modm/driver/temperature" env.copy("max31855.hpp") - env.copy("max31855_impl.hpp") \ No newline at end of file + env.copy("max31855_impl.hpp") diff --git a/src/modm/driver/temperature/max31855_impl.hpp b/src/modm/driver/temperature/max31855_impl.hpp index 40d9bca091..68653b0d1b 100644 --- a/src/modm/driver/temperature/max31855_impl.hpp +++ b/src/modm/driver/temperature/max31855_impl.hpp @@ -35,21 +35,18 @@ Max31855::initialize() // ----------------------------------------------------------------------------- template -modm::ResumableResult +void Max31855::readout() { - RF_BEGIN(); - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); - RF_CALL(SpiMaster::transfer(nullptr, data.data, 4)); + SpiMaster::transfer(nullptr, data.data, 4); if (this->releaseMaster()) { Cs::set(); } - - RF_END(); } -} // namespace modm \ No newline at end of file +} // namespace modm diff --git a/src/modm/driver/temperature/max31865.hpp b/src/modm/driver/temperature/max31865.hpp index 322d367d8e..a93d4cea77 100644 --- a/src/modm/driver/temperature/max31865.hpp +++ b/src/modm/driver/temperature/max31865.hpp @@ -17,8 +17,7 @@ #include #include #include -#include -#include +#include namespace modm { @@ -178,9 +177,7 @@ struct max31865 * @ingroup modm_driver_max31865 */ template -class Max31865 : public max31865, - public modm::SpiDevice, - protected modm::NestedResumable<3> +class Max31865 : public max31865, public modm::SpiDevice { public: using Data = max31865::Data; @@ -191,11 +188,11 @@ class Max31865 : public max31865, Max31865(Data &data); /// Call this function once before using the device - modm::ResumableResult + void initialize(); /// Read the raw data from the sensor - modm::ResumableResult + void readout(); /// Get the data object for this sensor @@ -211,15 +208,13 @@ class Max31865 : public max31865, Config_t config; uint16_t d; - modm::ShortTimeout timeout; - - modm::ResumableResult + void writeSingleRegister(Register address, uint8_t data); - modm::ResumableResult + uint8_t readSingleRegister(Register address); - modm::ResumableResult + uint16_t readTwoRegisters(Register address); }; diff --git a/src/modm/driver/temperature/max31865.lb b/src/modm/driver/temperature/max31865.lb index 7299d6e82b..880dbd81f7 100644 --- a/src/modm/driver/temperature/max31865.lb +++ b/src/modm/driver/temperature/max31865.lb @@ -23,11 +23,10 @@ def prepare(module, options): module.depends( ":architecture:gpio", ":architecture:spi.device", - ":processing:resumable", - ":processing:timer") + ":architecture:fiber") return True def build(env): env.outbasepath = "modm/src/modm/driver/temperature" env.copy("max31865.hpp") - env.copy("max31865_impl.hpp") \ No newline at end of file + env.copy("max31865_impl.hpp") diff --git a/src/modm/driver/temperature/max31865_impl.hpp b/src/modm/driver/temperature/max31865_impl.hpp index c44961f5cb..922528d185 100644 --- a/src/modm/driver/temperature/max31865_impl.hpp +++ b/src/modm/driver/temperature/max31865_impl.hpp @@ -26,92 +26,80 @@ Max31865::Max31865(Data &data) : data(data) } template -modm::ResumableResult +void Max31865::initialize() { - RF_BEGIN(); config = Config(); config.set(Config::FaultStatusClear); config.set(Rejection_t(Rejection::Rejection50Hz)); - RF_CALL(writeSingleRegister(Register::WriteConfiguration, config.value)); - config = Config_t(RF_CALL(readSingleRegister(Register::ReadConfiguration))); - RF_END(); + writeSingleRegister(Register::WriteConfiguration, config.value); + config = Config_t(readSingleRegister(Register::ReadConfiguration)); } template -modm::ResumableResult +void Max31865::readout() { - RF_BEGIN(); config.set(Config::VBias); - RF_CALL(writeSingleRegister(Register::WriteConfiguration, config.value)); - timeout.restart(10ms); - RF_WAIT_UNTIL(timeout.isExpired()); + writeSingleRegister(Register::WriteConfiguration, config.value); + modm::this_fiber::sleep_for(10ms); config.set(Config::OneShot); - RF_CALL(writeSingleRegister(Register::WriteConfiguration, config.value)); - timeout.restart(65ms); - RF_WAIT_UNTIL(timeout.isExpired()); + writeSingleRegister(Register::WriteConfiguration, config.value); + modm::this_fiber::sleep_for(65ms); - // data.data = RF_CALL(readTwoRegisters(Register::ReadRtdMsb)); - d = RF_CALL(readTwoRegisters(Register::ReadRtdMsb)); + // data.data = readTwoRegisters(Register::ReadRtdMsb); + d = readTwoRegisters(Register::ReadRtdMsb); data.data = d; config.reset(Config::VBias); - RF_CALL(writeSingleRegister(Register::WriteConfiguration, config.value)); - - RF_END(); + writeSingleRegister(Register::WriteConfiguration, config.value); } template -modm::ResumableResult +uint8_t Max31865::readSingleRegister(Register address) { - RF_BEGIN(); - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); buffer[0] = uint8_t(address); - RF_CALL(SpiMaster::transfer(buffer.data(), nullptr, 1)); - RF_CALL(SpiMaster::transfer(nullptr, buffer.data(), 1)); + SpiMaster::transfer(buffer.data(), nullptr, 1); + SpiMaster::transfer(nullptr, buffer.data(), 1); if (this->releaseMaster()) { Cs::set(); } - RF_END_RETURN(buffer[0]); + return buffer[0]; } template -modm::ResumableResult +uint16_t Max31865::readTwoRegisters(Register address) { - RF_BEGIN(); - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); buffer[0] = uint8_t(address); - RF_CALL(SpiMaster::transfer(buffer.data(), nullptr, 1)); - RF_CALL(SpiMaster::transfer(nullptr, buffer.data(), 2)); + SpiMaster::transfer(buffer.data(), nullptr, 1); + SpiMaster::transfer(nullptr, buffer.data(), 2); if (this->releaseMaster()) { Cs::set(); } - RF_END_RETURN(static_cast(buffer[0] << 8 | buffer[1])); + return static_cast(buffer[0] << 8 | buffer[1]); } template -modm::ResumableResult +void Max31865::writeSingleRegister(Register address, uint8_t data) { - RF_BEGIN(); - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); buffer[0] = uint8_t(address); buffer[1] = data; - RF_CALL(SpiMaster::transfer(buffer.data(), nullptr, 2)); + SpiMaster::transfer(buffer.data(), nullptr, 2); if (this->releaseMaster()) { Cs::set(); } - - RF_END(); } -} // namespace modm \ No newline at end of file +} // namespace modm diff --git a/src/modm/driver/temperature/mcp990x.hpp b/src/modm/driver/temperature/mcp990x.hpp index 6b04d71fa4..b56f605dcb 100644 --- a/src/modm/driver/temperature/mcp990x.hpp +++ b/src/modm/driver/temperature/mcp990x.hpp @@ -101,33 +101,33 @@ struct mcp990x * @author Christopher Durand */ template -class Mcp990x : public mcp990x, public I2cDevice +class Mcp990x : public mcp990x, public I2cDevice { public: /// \param address I2C address, MCP990xT-1: 0x4c, -2: 0x4d, -A: adjustable, see datasheet Mcp990x(Data& data, uint8_t address = 0x4d); /// Initialize sensor - modm::ResumableResult + bool initialize(); /// Detect sensor - modm::ResumableResult + bool ping(); /// Read internal temperature sensor /// \pre sensor is succesfully initialized - modm::ResumableResult + bool readInternalTemperature(); /// Read external diode temperature sensor /// \pre sensor is succesfully initialized - modm::ResumableResult + bool readExternalDiodeTemperature(ExternalDiode diode); /// Set external diode ideality factor /// See datasheet for values - modm::ResumableResult + bool setExternalDiodeIdealityFactor(ExternalDiode diode, uint8_t idealitySetting); inline Data& @@ -154,13 +154,13 @@ class Mcp990x : public mcp990x, public I2cDevice Register::Ext3Ideality }; - modm::ResumableResult + bool write(Register reg, uint8_t config); - modm::ResumableResult + bool read(Register reg, uint8_t& value); - modm::ResumableResult + bool readTemperature(Register highReg, Register lowReg); Data& data_; diff --git a/src/modm/driver/temperature/mcp990x_impl.hpp b/src/modm/driver/temperature/mcp990x_impl.hpp index d548ccecab..1e35cd82a6 100644 --- a/src/modm/driver/temperature/mcp990x_impl.hpp +++ b/src/modm/driver/temperature/mcp990x_impl.hpp @@ -18,69 +18,58 @@ namespace modm template < typename I2cMaster > Mcp990x::Mcp990x(Data &data, uint8_t address) : - I2cDevice{address}, data_{data} + I2cDevice{address}, data_{data} { } template -ResumableResult +bool Mcp990x::write(Register reg, uint8_t value) { - RF_BEGIN(); - buffer_[0] = static_cast(reg); buffer_[1] = value; - - this->transaction.configureWrite(&buffer_[0], 2); - - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::write(&buffer_[0], 2); } template -ResumableResult +bool Mcp990x::read(Register reg, uint8_t& value) { - RF_BEGIN(); - buffer_[0] = static_cast(reg); - this->transaction.configureWriteRead(&buffer_[0], 1, &value, 1); - - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::writeRead(&buffer_[0], 1, &value, 1); } template -ResumableResult +bool Mcp990x::initialize() { - RF_BEGIN(); - if (!RF_CALL(this->ping())) { - RF_RETURN(false); + if (!this->ping()) { + return false; } - RF_END_RETURN_CALL(write(Register::Config, uint8_t(Config::ExtendedRange))); + return write(Register::Config, uint8_t(Config::ExtendedRange)); } template -ResumableResult +bool Mcp990x::ping() { - RF_BEGIN(); // It's ok here to use buffer_[1] as temporary storage // since read() only uses buffer_[0] - if (!RF_CALL(read(Register::ProductId, buffer_[1]))) { - RF_RETURN(false); + if (!read(Register::ProductId, buffer_[1])) { + return false; } - RF_END_RETURN(buffer_[1] == DeviceIds[0] || buffer_[1] == DeviceIds[1] || buffer_[1] == DeviceIds[2]); + return buffer_[1] == DeviceIds[0] || buffer_[1] == DeviceIds[1] || buffer_[1] == DeviceIds[2]; } template -ResumableResult +bool Mcp990x::readInternalTemperature() { return readTemperature(Register::IntTempHigh, Register::IntTempLow); } template -ResumableResult +bool Mcp990x::readExternalDiodeTemperature(ExternalDiode diode) { const auto index = static_cast(diode); @@ -88,7 +77,7 @@ Mcp990x::readExternalDiodeTemperature(ExternalDiode diode) } template -ResumableResult +bool Mcp990x::setExternalDiodeIdealityFactor(ExternalDiode diode, uint8_t idealitySetting) { const auto index = static_cast(diode); @@ -96,14 +85,13 @@ Mcp990x::setExternalDiodeIdealityFactor(ExternalDiode diode, uint8_t } template -ResumableResult +bool Mcp990x::readTemperature(Register highReg, Register lowReg) { - RF_BEGIN(); - if (!RF_CALL(read(highReg, data_.data[1]))) { - RF_RETURN(false); + if (!read(highReg, data_.data[1])) { + return false; } - RF_END_RETURN_CALL(read(lowReg, data_.data[0])); + return read(lowReg, data_.data[0]); } } // namespace modm diff --git a/src/modm/driver/temperature/stts22h.hpp b/src/modm/driver/temperature/stts22h.hpp index dc8a9581e0..049a3235eb 100644 --- a/src/modm/driver/temperature/stts22h.hpp +++ b/src/modm/driver/temperature/stts22h.hpp @@ -102,23 +102,23 @@ struct stts22h * @author Christopher Durand */ template -class Stts22h : public stts22h, public I2cDevice +class Stts22h : public stts22h, public I2cDevice { public: /// \param address I2C address, selectable on device between 0x38 and 0x3f Stts22h(Data& data, uint8_t address = 0x3f); /// Initialize sensor - modm::ResumableResult + bool initialize(); /// Detect sensor - modm::ResumableResult + bool ping(); /// Read temperature from device /// \pre sensor is succesfully initialized - modm::ResumableResult + bool readTemperature(); inline Data& @@ -128,13 +128,13 @@ class Stts22h : public stts22h, public I2cDevice private: static constexpr inline uint8_t DeviceId{0xA0}; - modm::ResumableResult + bool write(Register reg, RegisterValue value); - modm::ResumableResult + bool read(Register reg, uint8_t& value); - modm::ResumableResult + bool read(Register reg, uint8_t* buffer, uint8_t length); Data& data_; diff --git a/src/modm/driver/temperature/stts22h_impl.hpp b/src/modm/driver/temperature/stts22h_impl.hpp index b8f6f07190..347a545c90 100644 --- a/src/modm/driver/temperature/stts22h_impl.hpp +++ b/src/modm/driver/temperature/stts22h_impl.hpp @@ -18,75 +18,64 @@ namespace modm template < typename I2cMaster > Stts22h::Stts22h(Data &data, uint8_t address) : - I2cDevice{address}, data_{data} + I2cDevice{address}, data_{data} { } template -ResumableResult +bool Stts22h::write(Register reg, RegisterValue value) { - RF_BEGIN(); - buffer_[0] = static_cast(reg); buffer_[1] = value.value; - - this->transaction.configureWrite(&buffer_[0], 2); - - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::write(&buffer_[0], 2); } template -ResumableResult +bool Stts22h::read(Register reg, uint8_t& value) { return read(reg, &value, 1); } template -ResumableResult +bool Stts22h::read(Register reg, uint8_t* data, uint8_t length) { - RF_BEGIN(); - buffer_[0] = static_cast(reg); - this->transaction.configureWriteRead(&buffer_[0], 1, data, length); - - RF_END_RETURN_CALL(this->runTransaction()); + return I2cDevice::writeRead(&buffer_[0], 1, data, length); } template -ResumableResult +bool Stts22h::initialize() { - RF_BEGIN(); - if (!RF_CALL(this->ping())) { - RF_RETURN(false); + if (!this->ping()) { + return false; } - if (!RF_CALL(write(Register::SoftwareReset, SoftwareReset::SwReset))) { - RF_RETURN(false); + if (!write(Register::SoftwareReset, SoftwareReset::SwReset)) { + return false; } - if (!RF_CALL(write(Register::SoftwareReset, SoftwareReset{}))) { - RF_RETURN(false); + if (!write(Register::SoftwareReset, SoftwareReset{})) { + return false; } - RF_END_RETURN_CALL(write(Register::Ctrl, Ctrl::FreeRun | Ctrl::IfAddInc)); + return write(Register::Ctrl, Ctrl::FreeRun | Ctrl::IfAddInc); } template -ResumableResult +bool Stts22h::ping() { - RF_BEGIN(); // It's ok here to use buffer_[1] as temporary storage // since read() only uses buffer_[0] - if (!RF_CALL(read(Register::WhoAmI, buffer_[1]))) { - RF_RETURN(false); + if (!read(Register::WhoAmI, buffer_[1])) { + return false; } - RF_END_RETURN(buffer_[1] == DeviceId); + return buffer_[1] == DeviceId; } template -ResumableResult +bool Stts22h::readTemperature() { return read(Register::TempLOut, data_.data, 2); diff --git a/src/modm/driver/temperature/tmp102.hpp b/src/modm/driver/temperature/tmp102.hpp index f39dec23af..ae59c74b9e 100644 --- a/src/modm/driver/temperature/tmp102.hpp +++ b/src/modm/driver/temperature/tmp102.hpp @@ -16,8 +16,8 @@ #include #include -#include #include +#include #include "lm75.hpp" @@ -110,8 +110,7 @@ struct tmp102 : public lm75 * @author Niklas Hauser */ template < class I2cMaster > -class Tmp102 : public tmp102, public Lm75< I2cMaster >, - protected modm::pt::Protothread +class Tmp102 : public tmp102, public Lm75< I2cMaster > { public: /// Constructor, requires a tmp102::Data object, @@ -123,45 +122,45 @@ class Tmp102 : public tmp102, public Lm75< I2cMaster >, // MARK: Configuration // @param rate Update rate in Hz: 0 to 33. (Use 0 to update at 0.25Hz). - modm::ResumableResult + bool setUpdateRate(uint8_t rate); /// Enables extended mode with 13 bit data format. - modm::ResumableResult + bool enableExtendedMode(bool enable = true); /// param[out] result contains comparator mode alert in the configured polarity - modm::ResumableResult + bool readComparatorMode(bool &result); /// Writes the upper limit of the alarm. - modm::ResumableResult + bool setUpperLimit(float temperature) { return setLimitRegister(Register::TemperatureUpperLimit, temperature); } /// Writes the lower limit of the alarm. - modm::ResumableResult + bool setLowerLimit(float temperature) { return setLimitRegister(Register::TemperatureLowerLimit, temperature); } /// starts a temperature conversion right now - modm::ResumableResult + bool startConversion(); inline Data& getData(); private: - modm::ResumableResult + bool writeConfiguration(uint8_t length=3); - modm::ResumableResult + bool setLimitRegister(Register reg, float temperature); - uint16_t updateTime; - modm::ShortTimeout timeout; + modm::ShortPeriodicTimer timer{250ms}; + bool withConversion{false}; - Config2_t config_lsb; + Config2_t config_lsb{ConversionRate_t(ConversionRate::Hz4)}; }; } // namespace modm diff --git a/src/modm/driver/temperature/tmp102.lb b/src/modm/driver/temperature/tmp102.lb index f583a110d9..536bc2b260 100644 --- a/src/modm/driver/temperature/tmp102.lb +++ b/src/modm/driver/temperature/tmp102.lb @@ -36,7 +36,7 @@ def prepare(module, options): ":architecture:register", ":driver:lm75", ":math:utils", - ":processing:protothread", + ":architecture:fiber", ":processing:timer") return True diff --git a/src/modm/driver/temperature/tmp102_impl.hpp b/src/modm/driver/temperature/tmp102_impl.hpp index 8b1a4d96a0..77c7bb2d87 100644 --- a/src/modm/driver/temperature/tmp102_impl.hpp +++ b/src/modm/driver/temperature/tmp102_impl.hpp @@ -18,9 +18,7 @@ // ---------------------------------------------------------------------------- template < typename I2cMaster > modm::Tmp102::Tmp102(Data &data, uint8_t address) -: Lm75(reinterpret_cast(data), address), - updateTime(250), timeout(modm::ShortDuration(updateTime)), - config_lsb(ConversionRate_t(ConversionRate::Hz4)) +: Lm75(reinterpret_cast(data), address) { } @@ -28,25 +26,17 @@ template < typename I2cMaster > bool modm::Tmp102::update() { - PT_BEGIN(); - - while(true) + if (timer.execute()) { - PT_WAIT_UNTIL(timeout.isExpired()); - - if (updateTime & (1 << 15)) + if (withConversion) { - PT_CALL(startConversion()); - timeout.restart(29ms); + startConversion(); + modm::this_fiber::sleep_for(29ms); } - - PT_WAIT_UNTIL(timeout.isExpired()); - timeout.restart(std::chrono::milliseconds(updateTime & ~(1 << 15))); - - PT_CALL(this->readTemperature()); + this->readTemperature(); + return true; } - - PT_END(); + return false; } template < typename I2cMaster > @@ -59,12 +49,9 @@ modm::Tmp102::getData() // ---------------------------------------------------------------------------- // MARK: - tasks template < typename I2cMaster > -modm::ResumableResult +bool modm::Tmp102::setUpdateRate(uint8_t rate) { - RF_BEGIN(); - - this->restart(); // clamp conversion rate to max 33Hz (=~30ms) if (rate > 33) rate = 33; @@ -81,99 +68,82 @@ modm::Tmp102::setUpdateRate(uint8_t rate) // 8 to 0b11 if (rate & 0b1001) ConversionRate_t::set(config_lsb, ConversionRate::Hz1); if (rate & 0b1100) ConversionRate_t::set(config_lsb, ConversionRate::Hz4); - if ( RF_CALL(writeConfiguration(3)) ) + if ( writeConfiguration(3) ) { - if (rate == 0) updateTime = 4000; - else updateTime = 1000/rate; - timeout.restart(std::chrono::milliseconds(updateTime & ~(1 << 15))); - RF_RETURN(true); + if (rate == 0) timer.restart(4s); + else timer.restart(std::chrono::milliseconds(1000/rate)); + withConversion = false; + return true; } } else { - updateTime = (1000/rate - 29) | (1 << 15); - timeout.restart(std::chrono::milliseconds(updateTime & ~(1 << 15))); - RF_RETURN(true); + timer.restart(std::chrono::milliseconds(1000/rate - 29)); + withConversion = true; + return true; } - this->stop(); - - RF_END_RETURN(false); + return false; } // MARK: Extended mode template < typename I2cMaster > -modm::ResumableResult +bool modm::Tmp102::enableExtendedMode(bool enable) { - RF_BEGIN(); - config_lsb.update(Config2::ExtendedMode, enable); - RF_END_RETURN_CALL(writeConfiguration(3)); + return writeConfiguration(3); } // MARK: conversion template < typename I2cMaster > -modm::ResumableResult +bool modm::Tmp102::startConversion() { - RF_BEGIN(); - reinterpret_cast(this->config_msb).set(Config1::OneShot); - if ( RF_CALL(writeConfiguration(2)) ) + if ( writeConfiguration(2) ) { reinterpret_cast(this->config_msb).reset(Config1::OneShot); - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } // MARK: read temperature template < typename I2cMaster > -modm::ResumableResult +bool modm::Tmp102::readComparatorMode(bool &result) { - RF_BEGIN(); - this->buffer[0] = uint8_t(Register::Configuration); - this->transaction.configureWriteRead(this->buffer, 1, this->buffer, 2); - - if (RF_CALL( this->runTransaction() )) + if (I2cDevice::writeRead(this->buffer, 1, this->buffer, 2)) { reinterpret_cast(this->config_msb) = Config1_t(this->buffer[0]) & ~Resolution_t::mask(); result = static_cast(Config2_t(this->buffer[1]) & Config2::Alert); config_lsb = Config2_t(this->buffer[1]) & ~Config2::Alert; - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } // MARK: configuration template < typename I2cMaster > -modm::ResumableResult +bool modm::Tmp102::writeConfiguration(uint8_t length) { - RF_BEGIN(); - this->buffer[0] = uint8_t(Register::Configuration); this->buffer[1] = reinterpret_cast(this->config_msb).value; this->buffer[2] = config_lsb.value; - - this->transaction.configureWrite(this->buffer, length); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(this->buffer, length); } template < typename I2cMaster > -modm::ResumableResult +bool modm::Tmp102::setLimitRegister(Register reg, float temperature) { - RF_BEGIN(); - { int16_t temp = temperature * 16.f; temp <<= (config_lsb & Config2::ExtendedMode) ? 3 : 4; @@ -182,8 +152,5 @@ modm::Tmp102::setLimitRegister(Register reg, float temperature) this->buffer[1] = (temp >> 8); this->buffer[2] = temp; } - - this->transaction.configureWrite(this->buffer, 3); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(this->buffer, 3); } diff --git a/src/modm/driver/temperature/tmp12x.hpp b/src/modm/driver/temperature/tmp12x.hpp index 0c62733c76..0ec63b505a 100644 --- a/src/modm/driver/temperature/tmp12x.hpp +++ b/src/modm/driver/temperature/tmp12x.hpp @@ -13,7 +13,7 @@ #define MODM_TMP12x_HPP #include -#include +#include #include #include @@ -100,7 +100,7 @@ using Tmp125Temperature = Tmp12xTemperature<10, 5, 1, 4>; // 10 bit, 1/4 °C res * @tparam TemperatureT Sensor temperature type */ template -class Tmp12x : public modm::SpiDevice, protected modm::NestedResumable<1> +class Tmp12x : public modm::SpiDevice { public: using Temperature = TemperatureT; @@ -120,20 +120,18 @@ class Tmp12x : public modm::SpiDevice, protected modm::NestedResumabl Cs::setOutput(true); } - modm::ResumableResult + Temperature read() { - RF_BEGIN(); - - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); - RF_CALL(SpiMaster::transfer(nullptr, buffer_.data(), 2)); + SpiMaster::transfer(nullptr, buffer_.data(), 2); if (this->releaseMaster()) { Cs::set(); } - RF_END_RETURN(Temperature(buffer_[1] | (buffer_[0] << 8))); + return Temperature(buffer_[1] | (buffer_[0] << 8)); } private: std::array buffer_{}; diff --git a/src/modm/driver/temperature/tmp12x.lb b/src/modm/driver/temperature/tmp12x.lb index d2fb2ef899..ebdac74c92 100644 --- a/src/modm/driver/temperature/tmp12x.lb +++ b/src/modm/driver/temperature/tmp12x.lb @@ -26,7 +26,7 @@ def prepare(module, options): ":architecture:gpio", ":architecture:register", ":architecture:spi.device", - ":processing:resumable") + ":architecture:fiber") return True def build(env): diff --git a/src/modm/driver/temperature/tmp175.hpp b/src/modm/driver/temperature/tmp175.hpp index 50d77fd1da..d3aa791c67 100644 --- a/src/modm/driver/temperature/tmp175.hpp +++ b/src/modm/driver/temperature/tmp175.hpp @@ -16,7 +16,7 @@ #include #include -#include +#include #include "lm75.hpp" namespace modm @@ -66,8 +66,7 @@ struct tmp175 : public lm75 * @tparam I2cMaster Asynchronous Interface */ template < typename I2cMaster > -class Tmp175 : public tmp175, public Lm75< I2cMaster >, - protected modm::pt::Protothread +class Tmp175 : public tmp175, public Lm75< I2cMaster > { public: /// Constructor, requires a tmp175::Data object, @@ -81,37 +80,35 @@ class Tmp175 : public tmp175, public Lm75< I2cMaster >, void setUpdateRate(uint8_t rate); - modm::ResumableResult + bool setResolution(Resolution resolution); /// Writes the upper limit of the alarm. - modm::ResumableResult + bool setUpperLimit(float temperature) { return setLimitRegister(Register::TemperatureUpperLimit, temperature); } /// Writes the lower limit of the alarm. - modm::ResumableResult + bool setLowerLimit(float temperature) { return setLimitRegister(Register::TemperatureLowerLimit, temperature); } /// starts a temperature conversion right now - modm::ResumableResult + bool startConversion(); inline Data& getData(); private: - modm::ResumableResult + bool writeConfiguration(); - modm::ResumableResult + bool setLimitRegister(Register reg, float temperature); - modm::ShortDuration updateTime; - modm::ShortDuration conversionTime; - modm::ShortTimeout periodTimeout; - modm::ShortTimeout conversionTimeout; + modm::ShortDuration conversionTime{232ms}; + modm::ShortPeriodicTimer timer{250ms}; }; } // namespace modm diff --git a/src/modm/driver/temperature/tmp175.lb b/src/modm/driver/temperature/tmp175.lb index 57f9f2a0ae..b532ba9e66 100644 --- a/src/modm/driver/temperature/tmp175.lb +++ b/src/modm/driver/temperature/tmp175.lb @@ -31,8 +31,8 @@ def prepare(module, options): module.depends( ":architecture:i2c.device", ":architecture:register", - ":driver:lm75", - ":processing:protothread") + ":processing:timer", + ":driver:lm75") return True def build(env): diff --git a/src/modm/driver/temperature/tmp175_impl.hpp b/src/modm/driver/temperature/tmp175_impl.hpp index 8d38059544..12165ce5de 100644 --- a/src/modm/driver/temperature/tmp175_impl.hpp +++ b/src/modm/driver/temperature/tmp175_impl.hpp @@ -18,33 +18,23 @@ // ---------------------------------------------------------------------------- template < typename I2cMaster > modm::Tmp175::Tmp175(Data &data, uint8_t address) -: Lm75(reinterpret_cast(data), address), - updateTime(250), conversionTime(232), - periodTimeout(updateTime), conversionTimeout(conversionTime) +: Lm75(reinterpret_cast(data), address) { - this->stop(); } template < typename I2cMaster > bool modm::Tmp175::update() { - PT_BEGIN(); - - while(true) + if (timer.execute()) { - PT_WAIT_UNTIL(periodTimeout.isExpired()); - periodTimeout.restart(updateTime); - - PT_CALL(startConversion()); - - conversionTimeout.restart(conversionTime); - PT_WAIT_UNTIL(conversionTimeout.isExpired()); + startConversion(); + modm::this_fiber::sleep_for(std::chrono::milliseconds(conversionTime.count())); - PT_CALL(this->readTemperature()); + readTemperature(); + return true; } - - PT_END(); + return false; } template < typename I2cMaster > @@ -63,64 +53,50 @@ modm::Tmp175::setUpdateRate(uint8_t rate) if (rate == 0) rate = 1; if (rate > 33) rate = 33; - updateTime = modm::ShortDuration(1000/rate - 29); - periodTimeout.restart(updateTime); - - this->restart(); + timer.restart(1000/rate - 29); } template < typename I2cMaster > -modm::ResumableResult +bool modm::Tmp175::setResolution(Resolution resolution) { - RF_BEGIN(); - Resolution_t::set(reinterpret_cast(this->config_msb), resolution); conversionTime = modm::ShortDuration((uint8_t(resolution) + 1) * 29); - RF_END_RETURN_CALL( writeConfiguration() ); + return writeConfiguration(); } // MARK: conversion template < typename I2cMaster > -modm::ResumableResult +bool modm::Tmp175::startConversion() { - RF_BEGIN(); - reinterpret_cast(this->config_msb).set(Config1::OneShot); - if ( RF_CALL(writeConfiguration()) ) + if ( writeConfiguration() ) { reinterpret_cast(this->config_msb).reset(Config1::OneShot); - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } // MARK: configuration template < typename I2cMaster > -modm::ResumableResult +bool modm::Tmp175::writeConfiguration() { - RF_BEGIN(); - this->buffer[0] = uint8_t(Register::Configuration); this->buffer[1] = reinterpret_cast(this->config_msb).value; - - this->transaction.configureWrite(this->buffer, 2); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(this->buffer, 2); } template < typename I2cMaster > -modm::ResumableResult +bool modm::Tmp175::setLimitRegister(Register reg, float temperature) { - RF_BEGIN(); - { uint8_t res = uint8_t(Resolution_t::get(reinterpret_cast(this->config_msb))); @@ -131,9 +107,6 @@ modm::Tmp175::setLimitRegister(Register reg, float temperature) this->buffer[1] = (temp >> 8); this->buffer[2] = temp; } - - this->transaction.configureWrite(this->buffer, 3); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(this->buffer, 3); } diff --git a/src/modm/driver/touch/ads7843.hpp b/src/modm/driver/touch/ads7843.hpp index bba284d609..85f01f4012 100644 --- a/src/modm/driver/touch/ads7843.hpp +++ b/src/modm/driver/touch/ads7843.hpp @@ -17,7 +17,7 @@ #include #include -#include +#include namespace modm { diff --git a/src/modm/driver/touch/ads7843.lb b/src/modm/driver/touch/ads7843.lb index 053e0f6394..ba1cbd2fe2 100644 --- a/src/modm/driver/touch/ads7843.lb +++ b/src/modm/driver/touch/ads7843.lb @@ -28,7 +28,7 @@ with the ADS7843 and have their own driver "modm:driver:touch2046". def prepare(module, options): module.depends( - ":architecture:delay", + ":architecture:fiber", ":ui:display") return True diff --git a/src/modm/driver/touch/ads7843_impl.hpp b/src/modm/driver/touch/ads7843_impl.hpp index a8f1ef47fa..9f7f2c6673 100644 --- a/src/modm/driver/touch/ads7843_impl.hpp +++ b/src/modm/driver/touch/ads7843_impl.hpp @@ -94,15 +94,15 @@ uint16_t modm::Ads7843::readData(uint8_t command) { Cs::reset(); - modm::delay_us(1); // modm::delay_ns(100); - Spi::transferBlocking(command); - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); // modm::delay_ns(100); + Spi::transfer(command); + modm::this_fiber::sleep_for(1us); - uint16_t temp = Spi::transferBlocking(0x00); + uint16_t temp = Spi::transfer(0x00); temp <<= 8; - modm::delay_us(1); + modm::this_fiber::sleep_for(1us); - temp |= Spi::transferBlocking(0x00); + temp |= Spi::transfer(0x00); temp >>= 3; Cs::set(); diff --git a/src/modm/driver/touch/ft6x06.hpp b/src/modm/driver/touch/ft6x06.hpp index dc7022d548..c71ab52a98 100644 --- a/src/modm/driver/touch/ft6x06.hpp +++ b/src/modm/driver/touch/ft6x06.hpp @@ -167,17 +167,17 @@ struct ft6x06 * @ingroup modm_driver_ft6x06 */ template < typename I2cMaster > -class Ft6x06 : public ft6x06, public modm::I2cDevice< I2cMaster, 3 > +class Ft6x06 : public ft6x06, public modm::I2cDevice< I2cMaster > { public: /// Constructor, requires an ft6x06::Data object, sets address to default of 0x2A Ft6x06(Data &data, uint8_t address=0x2A); - modm::ResumableResult + bool configure(InterruptMode mode, uint8_t activeRate = 60, uint8_t monitorRate = 25); /// Reads all touches and writes the result to the data object - modm::ResumableResult + bool readTouches(); public: @@ -189,11 +189,11 @@ class Ft6x06 : public ft6x06, public modm::I2cDevice< I2cMaster, 3 > protected: /// @cond /// write a 8bit value a register - modm::ResumableResult + bool write(Register reg, uint8_t value); /// read multiple 8bit values from a start register - modm::ResumableResult + bool read(Register reg, uint8_t *buffer, uint8_t length); /// @endcond diff --git a/src/modm/driver/touch/ft6x06_impl.hpp b/src/modm/driver/touch/ft6x06_impl.hpp index a882976ea2..f7ea6e8f05 100644 --- a/src/modm/driver/touch/ft6x06_impl.hpp +++ b/src/modm/driver/touch/ft6x06_impl.hpp @@ -16,67 +16,55 @@ // ---------------------------------------------------------------------------- template < typename I2cMaster > modm::Ft6x06::Ft6x06(Data &data, uint8_t address) -: I2cDevice(address), data(data) +: I2cDevice(address), data(data) { } // MARK: - Tasks template < typename I2cMaster > -modm::ResumableResult +bool modm::Ft6x06::configure(InterruptMode mode, uint8_t activeRate, uint8_t monitorRate) { - RF_BEGIN(); - - if (RF_CALL(write(Register::G_MODE, uint8_t(mode)))) + if (write(Register::G_MODE, uint8_t(mode))) { - if (RF_CALL(write(Register::PERIOD_ACTIVE, activeRate))) + if (write(Register::PERIOD_ACTIVE, activeRate)) { - RF_RETURN_CALL(write(Register::PERIOD_MONITOR, monitorRate)); + return write(Register::PERIOD_MONITOR, monitorRate); } } - RF_END_RETURN(false); + return false; } template < typename I2cMaster > -modm::ResumableResult +bool modm::Ft6x06::readTouches() { - RF_BEGIN(); - - if (RF_CALL(read(Register::GEST_ID, buffer, 14))) + if (read(Register::GEST_ID, buffer, 14)) { std::memcpy(data.data, buffer, 14); - RF_RETURN(true); + return true; } - RF_END_RETURN(false); + return false; } // ---------------------------------------------------------------------------- // MARK: write register template < class I2cMaster > -modm::ResumableResult +bool modm::Ft6x06::write(Register reg, uint8_t value) { - RF_BEGIN(); - buffer[0] = uint8_t(reg); buffer[1] = value; - this->transaction.configureWrite(buffer, 2); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::write(buffer, 2); } // MARK: read multilength register template < class I2cMaster > -modm::ResumableResult +bool modm::Ft6x06::read(Register reg, uint8_t *buffer, uint8_t length) { - RF_BEGIN(); - this->buffer[0] = uint8_t(reg); - this->transaction.configureWriteRead(this->buffer, 1, buffer, length); - - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::writeRead(this->buffer, 1, buffer, length); } diff --git a/src/modm/driver/touch/touch2046.hpp b/src/modm/driver/touch/touch2046.hpp index 97f75ce12b..bb54dab43d 100644 --- a/src/modm/driver/touch/touch2046.hpp +++ b/src/modm/driver/touch/touch2046.hpp @@ -15,7 +15,7 @@ #include #include -#include +#include #include #include @@ -68,7 +68,7 @@ struct touch2046 { * Datasheet TSC2046: https://www.ti.com/lit/ds/symlink/tsc2046.pdf */ template < class SpiMaster, class Cs> -class Touch2046 : public touch2046, public modm::SpiDevice< SpiMaster >, protected modm::NestedResumable<3> +class Touch2046 : public touch2046, public modm::SpiDevice< SpiMaster > { public: /** @@ -86,7 +86,7 @@ class Touch2046 : public touch2046, public modm::SpiDevice< SpiMaster >, protect * * \return Position and intensity of touch point. Full int16_t range. */ - modm::ResumableResult> + std::tuple getRawValues(); /** @@ -94,7 +94,7 @@ class Touch2046 : public touch2046, public modm::SpiDevice< SpiMaster >, protect * * \return bool true if screen is touched */ - modm::ResumableResult + bool isTouched(); /** @@ -102,7 +102,7 @@ class Touch2046 : public touch2046, public modm::SpiDevice< SpiMaster >, protect * * \return Position (X, Y) of touch point. */ - modm::ResumableResult> + std::tuple getTouchPosition(); private: diff --git a/src/modm/driver/touch/touch2046.lb b/src/modm/driver/touch/touch2046.lb index 56d09b40d6..c4d709d49b 100644 --- a/src/modm/driver/touch/touch2046.lb +++ b/src/modm/driver/touch/touch2046.lb @@ -29,7 +29,7 @@ All of these are compatible with the TSC2046. def prepare(module, options): module.depends( ":architecture:spi.device", - ":processing:resumable") + ":architecture:fiber") return True def build(env): diff --git a/src/modm/driver/touch/touch2046_impl.hpp b/src/modm/driver/touch/touch2046_impl.hpp index c5f9718363..0248fb3ade 100644 --- a/src/modm/driver/touch/touch2046_impl.hpp +++ b/src/modm/driver/touch/touch2046_impl.hpp @@ -16,18 +16,16 @@ template < class SpiMaster, class Cs > -modm::ResumableResult> +std::tuple modm::Touch2046::getRawValues() { - RF_BEGIN(); - - RF_WAIT_UNTIL(this->acquireMaster()); + modm::this_fiber::poll([&]{ return this->acquireMaster(); }); Cs::reset(); - RF_CALL(SpiMaster::transfer( + SpiMaster::transfer( bufferWrite.data(), reinterpret_cast(bufferRead.data()) + 1, - 17)); + 17); z = 4095 + (modm::fromBigEndian(bufferRead[1]) >> 3) - (modm::fromBigEndian(bufferRead[2]) >> 3); @@ -44,25 +42,22 @@ modm::Touch2046::getRawValues() Cs::set(); } - RF_END_RETURN(std::make_tuple(x, y, z)); + return std::make_tuple(x, y, z); } template < class SpiMaster, class Cs > -modm::ResumableResult +bool modm::Touch2046::isTouched() { - RF_BEGIN(); - std::tie(std::ignore, std::ignore, z) = RF_CALL(getRawValues()); - RF_END_RETURN(z > cal.ThresholdZ); + std::tie(std::ignore, std::ignore, z) = getRawValues(); + return z > cal.ThresholdZ; } template < class SpiMaster, class Cs > -modm::ResumableResult> +std::tuple modm::Touch2046::getTouchPosition() { - RF_BEGIN(); - - std::tie(x, y, std::ignore) = RF_CALL(getRawValues()); + std::tie(x, y, std::ignore) = getRawValues(); x = std::min( ((static_cast(x * cal.FactorX) / 1'000'000) @@ -75,5 +70,5 @@ modm::Touch2046::getTouchPosition() // todo: orientation processing - RF_END_RETURN(std::make_tuple(x, y)); + return std::make_tuple(x, y); } diff --git a/src/modm/driver/usb/stusb4500.hpp b/src/modm/driver/usb/stusb4500.hpp index a7a498ddec..1f8034d492 100644 --- a/src/modm/driver/usb/stusb4500.hpp +++ b/src/modm/driver/usb/stusb4500.hpp @@ -47,11 +47,11 @@ struct stusb4500 * @author Henrik Hose, Raphael Lehmann */ template -class Stusb4500 : public stusb4500, public modm::I2cDevice< I2cMaster, 4 > +class Stusb4500 : public stusb4500, public modm::I2cDevice< I2cMaster > { public: Stusb4500(uint8_t address=0x28): - modm::I2cDevice(address) + modm::I2cDevice(address) {}; /** @@ -64,15 +64,13 @@ class Stusb4500 : public stusb4500, public modm::I2cDevice< I2cMaster, 4 > * @param voltage in mV * @param current in mA */ - modm::ResumableResult + bool configurePdo(uint8_t pdoNumber, uint32_t voltage, uint32_t current) { - RF_BEGIN(); - if((pdoNumber < 1) || (pdoNumber > 3)) - RF_RETURN(false); + return false; - RF_CALL(readRegister(DpmSnkPdos[pdoNumber - 1], buffer2.data(), 4)); + readRegister(DpmSnkPdos[pdoNumber - 1], buffer2.data(), 4); current /= 10; voltage /= 50; @@ -81,7 +79,7 @@ class Stusb4500 : public stusb4500, public modm::I2cDevice< I2cMaster, 4 > buffer2[1] = ((current >> 8) & 0b11) | ((voltage & 0b11'1111 ) << 2); buffer2[2] = (buffer2[2] & 0b1111'0000) | ((voltage >> 6) & 0b1111); - RF_END_RETURN_CALL(updateRegister(DpmSnkPdos[pdoNumber - 1], buffer2.data(), 4)); + return updateRegister(DpmSnkPdos[pdoNumber - 1], buffer2.data(), 4); } /** @@ -89,15 +87,13 @@ class Stusb4500 : public stusb4500, public modm::I2cDevice< I2cMaster, 4 > * * @param pdoNumber Which PDO to set valid, range 1 to 3. */ - modm::ResumableResult + bool setValidPdo(uint8_t pdoNumber) { - RF_BEGIN(); - if((pdoNumber < 1) || (pdoNumber > 3)) - RF_RETURN(false); + return false; - RF_END_RETURN_CALL(updateRegister(Register::DpmPdoNumb, &pdoNumber, 1)); + return updateRegister(Register::DpmPdoNumb, &pdoNumber, 1); } /** @@ -106,12 +102,10 @@ class Stusb4500 : public stusb4500, public modm::I2cDevice< I2cMaster, 4 > * @return RdoRegStatusData object. Typically only * RdoRegStatusData::MaxCurrent [mA] is of interest. */ - modm::ResumableResult + RdoRegStatusData getRdoRegStatus() { - RF_BEGIN(); - - RF_CALL(readRegister(Register::RdoRegStatus, buffer2.data(), 4)); + readRegister(Register::RdoRegStatus, buffer2.data(), 4); // MaxCurrent: Bits 9..0; OperatingCurrent: Bits 19..10; ObjectPos: Bits 30..28 rdoRegStatusData.MaxCurrent = buffer2[0] | ((buffer2[1] & 0b11) << 8); @@ -120,29 +114,25 @@ class Stusb4500 : public stusb4500, public modm::I2cDevice< I2cMaster, 4 > rdoRegStatusData.OperatingCurrent *= 10; rdoRegStatusData.ObjectPos = (buffer2[3] & 0b0110'0000) >> 5; - RF_END_RETURN(rdoRegStatusData); + return rdoRegStatusData; } public: - modm::ResumableResult + bool readRegister(Register reg, uint8_t* output, size_t length) { - RF_BEGIN(); buffer[0] = uint8_t(reg); - this->transaction.configureWriteRead(buffer.data(), 1, output, length); - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::writeRead(buffer.data(), 1, output, length); } - modm::ResumableResult + bool updateRegister(Register reg, const uint8_t* data, size_t length) { - RF_BEGIN(); if(length > (std::tuple_size::value - 1)) - RF_RETURN(false); + return false; buffer[0] = uint8_t(reg); std::memcpy(&buffer[1], data, length); - this->transaction.configureWriteRead(buffer.data(), length + 1, nullptr, 0); - RF_END_RETURN_CALL( this->runTransaction() ); + return I2cDevice::writeRead(buffer.data(), length + 1, nullptr, 0); } private: diff --git a/src/modm/platform/clock/avr/clock.cpp.in b/src/modm/platform/clock/avr/clock.cpp.in index 4bab623fa8..01cf6f54a9 100644 --- a/src/modm/platform/clock/avr/clock.cpp.in +++ b/src/modm/platform/clock/avr/clock.cpp.in @@ -72,8 +72,17 @@ MODM_ISR(TIMER0_COMPA) interrupt = true; } +// ---------------------------------------------------------------------------- +// Allow the ability to override the clock now functions +#ifndef MODM_CHRONO_MILLI_CLOCK_NOW +# define MODM_CHRONO_MILLI_CLOCK_NOW modm::chrono::milli_clock::now +#endif +#ifndef MODM_CHRONO_MICRO_CLOCK_NOW +# define MODM_CHRONO_MICRO_CLOCK_NOW modm::chrono::micro_clock::now +#endif + modm::chrono::milli_clock::time_point modm_weak -modm::chrono::milli_clock::now() noexcept +MODM_CHRONO_MILLI_CLOCK_NOW() noexcept { uint32_t time; do @@ -82,11 +91,11 @@ modm::chrono::milli_clock::now() noexcept time = milli_time; } while(interrupt); - return time_point{duration{time}}; + return modm::chrono::milli_clock::time_point{modm::chrono::milli_clock::duration{time}}; } modm::chrono::micro_clock::time_point modm_weak -modm::chrono::micro_clock::now() noexcept +MODM_CHRONO_MICRO_CLOCK_NOW() noexcept { uint32_t time; uint8_t val; @@ -101,5 +110,5 @@ modm::chrono::micro_clock::now() noexcept time += uint16_t(val * uint16_t({{scaler}})) >> {{shift}}; - return time_point{duration{time}}; + return modm::chrono::micro_clock::time_point{modm::chrono::micro_clock::duration{time}}; } diff --git a/src/modm/platform/clock/systick/systick_timer.cpp.in b/src/modm/platform/clock/systick/systick_timer.cpp.in index eabc3a67c3..e4f3fc169b 100644 --- a/src/modm/platform/clock/systick/systick_timer.cpp.in +++ b/src/modm/platform/clock/systick/systick_timer.cpp.in @@ -79,11 +79,19 @@ modm::platform::SysTickTimer::disable() } // ---------------------------------------------------------------------------- +// Allow the ability to override the clock now functions +#ifndef MODM_CHRONO_MILLI_CLOCK_NOW +# define MODM_CHRONO_MILLI_CLOCK_NOW modm::chrono::milli_clock::now +#endif +#ifndef MODM_CHRONO_MICRO_CLOCK_NOW +# define MODM_CHRONO_MICRO_CLOCK_NOW modm::chrono::micro_clock::now +#endif + modm::chrono::milli_clock::time_point modm_weak -modm::chrono::milli_clock::now() noexcept +MODM_CHRONO_MILLI_CLOCK_NOW() noexcept { %% if systick_frequency == 1000 - return time_point{duration{milli_time}}; + return modm::chrono::milli_clock::time_point{modm::chrono::milli_clock::duration{milli_time}}; %% else uint32_t val, ms; %% if multicore @@ -98,16 +106,16 @@ modm::chrono::milli_clock::now() noexcept } while(interrupt); const auto diff = SysTick->LOAD - val; - const auto ms_per_Ncycles = platform::SysTickTimer::ms_per_Ncycles; - constexpr auto Ncycles = platform::SysTickTimer::Ncycles; + const auto ms_per_Ncycles = modm::platform::SysTickTimer::ms_per_Ncycles; + constexpr auto Ncycles = modm::platform::SysTickTimer::Ncycles; ms += (uint64_t(diff) * uint64_t(ms_per_Ncycles)) >> Ncycles; - return time_point{duration{ms}}; + return modm::chrono::milli_clock::time_point{modm::chrono::milli_clock::duration{ms}}; %% endif } modm::chrono::micro_clock::time_point modm_weak -modm::chrono::micro_clock::now() noexcept +MODM_CHRONO_MICRO_CLOCK_NOW() noexcept { uint32_t val, us; %% if multicore @@ -122,8 +130,8 @@ modm::chrono::micro_clock::now() noexcept } while(interrupt); const auto diff = SysTick->LOAD - val; - const auto us_per_Ncycles = platform::SysTickTimer::us_per_Ncycles; - constexpr auto Ncycles = platform::SysTickTimer::Ncycles; + const auto us_per_Ncycles = modm::platform::SysTickTimer::us_per_Ncycles; + constexpr auto Ncycles = modm::platform::SysTickTimer::Ncycles; %% if systick_frequency == 1000 // use a 32x32=32bit multiplication @@ -132,5 +140,6 @@ modm::chrono::micro_clock::now() noexcept // use a 32x32=64bit multiplication us += (uint64_t(diff) * uint64_t(us_per_Ncycles)) >> Ncycles; %% endif - return time_point{duration{us}}; + + return modm::chrono::micro_clock::time_point{modm::chrono::micro_clock::duration{us}}; } diff --git a/src/modm/platform/clock/systick/systick_timer.hpp.in b/src/modm/platform/clock/systick/systick_timer.hpp.in index c4fd39d7d3..d1619aaec5 100644 --- a/src/modm/platform/clock/systick/systick_timer.hpp.in +++ b/src/modm/platform/clock/systick/systick_timer.hpp.in @@ -82,6 +82,8 @@ private: static void enable(uint32_t reload, bool use_processor_clock); +public: + /// @cond %% if systick_frequency != 1000 // FCPU < 8MHz // 536e6/4 < 27-bit, 8e6/4 < 21-bit @@ -101,8 +103,7 @@ private: static constexpr uint8_t Ncycles{20}; %% endif static inline uint32_t us_per_Ncycles{0}; - friend class modm::chrono::milli_clock; - friend class modm::chrono::micro_clock; + /// @endcond }; } diff --git a/src/modm/platform/core/hosted/clock.cpp b/src/modm/platform/core/hosted/clock.cpp index 6c24c8a485..89586befbd 100644 --- a/src/modm/platform/core/hosted/clock.cpp +++ b/src/modm/platform/core/hosted/clock.cpp @@ -11,16 +11,27 @@ #include +// Allow the ability to override the clock now functions +#ifndef MODM_CHRONO_MILLI_CLOCK_NOW +# define MODM_CHRONO_MILLI_CLOCK_NOW modm::chrono::milli_clock::now +#else +#endif +#ifndef MODM_CHRONO_MICRO_CLOCK_NOW +# define MODM_CHRONO_MICRO_CLOCK_NOW modm::chrono::micro_clock::now +#endif + modm::chrono::milli_clock::time_point modm_weak -modm::chrono::milli_clock::now() noexcept +MODM_CHRONO_MILLI_CLOCK_NOW() noexcept { const auto time = std::chrono::steady_clock::now().time_since_epoch(); - return time_point{std::chrono::duration_cast(time)}; + return modm::chrono::milli_clock::time_point{ + std::chrono::duration_cast(time)}; } modm::chrono::micro_clock::time_point modm_weak -modm::chrono::micro_clock::now() noexcept +MODM_CHRONO_MICRO_CLOCK_NOW() noexcept { const auto time = std::chrono::high_resolution_clock::now().time_since_epoch(); - return time_point{std::chrono::duration_cast(time)}; + return modm::chrono::micro_clock::time_point{ + std::chrono::duration_cast(time)}; } diff --git a/src/modm/platform/spi/at90_tiny_mega/module.lb b/src/modm/platform/spi/at90_tiny_mega/module.lb index 4e3c87d805..26368efe55 100644 --- a/src/modm/platform/spi/at90_tiny_mega/module.lb +++ b/src/modm/platform/spi/at90_tiny_mega/module.lb @@ -15,19 +15,11 @@ def get_properties(env): device = env[":target"] driver = device.get_driver("spi:avr") properties = {} - properties["use_fiber"] = env.get(":processing:protothread:use_fiber", True) properties["target"] = device.identifier properties["partname"] = device.partname properties["driver"] = driver return properties -def load_options(module): - module.add_option( - BooleanOption( - name="busywait", - description="", - default=False)) - class Instance(Module): def __init__(self, instance): @@ -38,8 +30,6 @@ class Instance(Module): module.description = "Instance {}".format(self.instance) def prepare(self, module, options): - module.depends(":platform:spi") - load_options(module) return True def build(self, env): @@ -66,11 +56,10 @@ def prepare(module, options): if "instance" in driver: for instance in listify(driver["instance"]): module.add_submodule(Instance(int(instance))) - else: - load_options(module) module.depends( ":architecture:atomic", + ":architecture:fiber", ":architecture:spi", ":platform:gpio", ":math:algorithm", diff --git a/src/modm/platform/spi/at90_tiny_mega/spi_master.cpp.in b/src/modm/platform/spi/at90_tiny_mega/spi_master.cpp.in index 724ad1e3c3..3b27244ed0 100644 --- a/src/modm/platform/spi/at90_tiny_mega/spi_master.cpp.in +++ b/src/modm/platform/spi/at90_tiny_mega/spi_master.cpp.in @@ -26,22 +26,11 @@ modm::platform::SpiMaster{{ id }}::initialize(Prescaler prescaler) SPCR{{ id }} = (1 << SPE{{ id }}) | (1 << MSTR{{ id }}) | (static_cast(prescaler) & ~0x80); SPSR{{ id }} = (static_cast(prescaler) & 0x80) ? (1 << SPI2X{{ id }}) : 0; - state = 0; } -modm::ResumableResult +uint8_t modm::platform::SpiMaster{{ id }}::transfer(uint8_t data) { -%% if options["busywait"] - SPDR{{ id }} = data; - - // wait for transfer to finish - while (!(SPSR{{ id }} & (1 << SPIF{{ id }}))) - ; - - data = SPDR{{ id }}; - return {modm::rf::Stop, data}; -%% elif use_fiber // start transfer by copying data into register SPDR{{ id }} = data; @@ -49,44 +38,11 @@ modm::platform::SpiMaster{{ id }}::transfer(uint8_t data) while (!(SPSR{{ id }} & (1 << SPIF{{ id }}))); return SPDR{{ id }}; -%% else - // this is a manually implemented "fast resumable function" - // there is no context or nesting protection, since we don't need it. - // there are only two states encoded into 1 bit (LSB of state): - // 1. waiting to start, and - // 2. waiting to finish. - - // MSB != Bit7 ? - if ( !(state & Bit7) ) - { - // start transfer by copying data into register - SPDR{{ id }} = data; - - // set MSB = Bit7 - state |= Bit7; - } - - // wait for transfer to finish - if (!(SPSR{{ id }} & (1 << SPIF{{ id }}))) - return {modm::rf::Running}; - - data = SPDR{{ id }}; - state &= ~Bit7; - return {modm::rf::Stop, data}; -%% endif } -modm::ResumableResult +void modm::platform::SpiMaster{{ id }}::transfer(const uint8_t *tx, uint8_t *rx, std::size_t length) { -%% if options["busywait"] - for (std::size_t index = 0; index < length; index++) - { - modm::ResumableResult result = transfer(tx ? tx[index] : 0); - if (rx) rx[index] = result.getResult(); - } - return {modm::rf::Stop}; -%% elif use_fiber for (std::size_t index = 0; index < length; index++) { // if tx == 0, we use a dummy byte 0x00 else we copy it from the array @@ -94,45 +50,4 @@ modm::platform::SpiMaster{{ id }}::transfer(const uint8_t *tx, uint8_t *rx, std: // if rx != 0, we copy the result into the array if (rx) rx[index] = result; } -%% else - // this is a manually implemented "fast resumable function" - // there is no context or nesting protection, since we don't need it. - // there are only two states encoded into 1 bit (Bit6 of state): - // 1. initialize index, and - // 2. wait for 1-byte transfer to finish. - - // we need to globally remember which byte we are currently transferring - static std::size_t index = 0; - - // we are only interested in Bit6 - switch(state & Bit6) - { - case 0: - // we will only visit this state once - state |= Bit6; - - // initialize index and check range - index = 0; - while (index < length) - { - default: - { - // call the resumable function - modm::ResumableResult result = transfer(tx ? tx[index] : 0); - - // if the resumable function is still running, so are we - if (result.getState() > modm::rf::NestingError) - return {modm::rf::Running}; - - // if rx != 0, we copy the result into the array - if (rx) rx[index] = result.getResult(); - } - index++; - } - - // clear the state - state &= ~Bit6; - return {modm::rf::Stop}; - } -%% endif } diff --git a/src/modm/platform/spi/at90_tiny_mega/spi_master.hpp.in b/src/modm/platform/spi/at90_tiny_mega/spi_master.hpp.in index 469547490b..ecba16facd 100644 --- a/src/modm/platform/spi/at90_tiny_mega/spi_master.hpp.in +++ b/src/modm/platform/spi/at90_tiny_mega/spi_master.hpp.in @@ -20,6 +20,7 @@ #include #include #include +#include %% set sck = "Sck" %% if target["type"] in ["u2"] @@ -118,41 +119,30 @@ public: } } + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static uint8_t transferBlocking(uint8_t data) { -%% if options["busywait"] - return transfer(data).getResult(); -%% else - return RF_CALL_BLOCKING(transfer(data)); -%% endif + return transfer(data); } + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static void transferBlocking(const uint8_t *tx, uint8_t *rx, std::size_t length) { -%% if options["busywait"] transfer(tx, rx, length); -%% else - RF_CALL_BLOCKING(transfer(tx, rx, length)); -%% endif } - static modm::ResumableResult + static uint8_t transfer(uint8_t data); - static modm::ResumableResult + static void transfer(const uint8_t *tx, uint8_t *rx, std::size_t length); protected: static void initialize(Prescaler prescaler); - -private: - // bit 7 (0x80) is used for transfer 1 byte - // bit 6 (0x40) is used for transfer multiple byte - static inline uint8_t state{}; }; } // namespace platform diff --git a/src/modm/platform/spi/at90_tiny_mega_uart/module.lb b/src/modm/platform/spi/at90_tiny_mega_uart/module.lb index 24b0e88fcb..8d29b777e9 100644 --- a/src/modm/platform/spi/at90_tiny_mega_uart/module.lb +++ b/src/modm/platform/spi/at90_tiny_mega_uart/module.lb @@ -29,8 +29,6 @@ class Instance(Module): module.description = "Instance {}".format(self.instance) def prepare(self, module, options): - module.depends(":platform:spi_uart") - module.add_option( BooleanOption( name="buffered", diff --git a/src/modm/platform/spi/at90_tiny_mega_uart/uart_spi_master.cpp.in b/src/modm/platform/spi/at90_tiny_mega_uart/uart_spi_master.cpp.in index fe9677929b..9813aac694 100644 --- a/src/modm/platform/spi/at90_tiny_mega_uart/uart_spi_master.cpp.in +++ b/src/modm/platform/spi/at90_tiny_mega_uart/uart_spi_master.cpp.in @@ -53,13 +53,11 @@ modm::platform::UartSpiMaster{{id}}::initialize(uint16_t prescaler) %% if not extended dataOrder = DataOrder::MsbFirst; %% endif - state = 0; } -modm::ResumableResult +uint8_t modm::platform::UartSpiMaster{{id}}::transfer(uint8_t data) { -%% if use_fiber // wait for transmit register empty while (!((UCSR{{id}}A & (1 << UDRE{{id}})))) modm::this_fiber::yield(); @@ -82,52 +80,12 @@ modm::platform::UartSpiMaster{{id}}::transfer(uint8_t data) } %% endif return data; -%% else - // this is a manually implemented "fast resumable function" - // there is no context or nesting protection, since we don't need it. - // there are only two states encoded into 1 bit (LSB of state): - // 1. waiting to start, and - // 2. waiting to finish. - - // LSB != Bit7 ? - if ( !(state & Bit7) ) - { - // wait for transmit register empty - if (!((UCSR{{id}}A & (1 << UDRE{{id}})))) - return {modm::rf::Running}; - -%% if not extended - if(dataOrder == DataOrder::MsbFirst) { - data = ::modm::bitReverse(data); - } -%% endif - UDR{{id}} = data; - - // set LSB = Bit7 - state |= Bit7; - } - - // wait for receive register not empty - if (!((UCSR{{id}}A & (1 << RXC{{id}})))) - return {modm::rf::Running}; - - data = UDR{{id}}; -%% if not extended - if(dataOrder == DataOrder::MsbFirst) { - data = ::modm::bitReverse(data); - } -%% endif - // transfer finished - state &= ~Bit7; - return {modm::rf::Stop, data}; -%% endif } -modm::ResumableResult +void modm::platform::UartSpiMaster{{id}}::transfer( const uint8_t *tx, uint8_t *rx, std::size_t length) { -%% if use_fiber for (std::size_t index = 0; index < length; index++) { // if tx == 0, we use a dummy byte 0x00 else we copy it from the array @@ -135,45 +93,4 @@ modm::platform::UartSpiMaster{{id}}::transfer( // if rx != 0, we copy the result into the array if (rx) rx[index] = result; } -%% else - // this is a manually implemented "fast resumable function" - // there is no context or nesting protection, since we don't need it. - // there are only two states encoded into 1 bit (Bit6 of state): - // 1. initialize index, and - // 2. wait for 1-byte transfer to finish. - - // we need to globally remember which byte we are currently transferring - static std::size_t index = 0; - - // we are only interested in Bit6 - switch(state & Bit6) - { - case 0: - // we will only visit this state once - state |= Bit6; - - // initialize index and check range - index = 0; - while (index < length) - { - default: - { - // call the resumable function - modm::ResumableResult result = transfer(tx ? tx[index] : 0); - - // if the resumable function is still running, so are we - if (result.getState() > modm::rf::NestingError) - return {modm::rf::Running}; - - // if rx != 0, we copy the result into the array - if (rx) rx[index] = result.getResult(); - } - index++; - } - - // clear the state - state &= ~Bit6; - return {modm::rf::Stop}; - } -%% endif } diff --git a/src/modm/platform/spi/at90_tiny_mega_uart/uart_spi_master.hpp.in b/src/modm/platform/spi/at90_tiny_mega_uart/uart_spi_master.hpp.in index cbbfd22d15..acedaa2ba9 100644 --- a/src/modm/platform/spi/at90_tiny_mega_uart/uart_spi_master.hpp.in +++ b/src/modm/platform/spi/at90_tiny_mega_uart/uart_spi_master.hpp.in @@ -19,6 +19,7 @@ #include #include #include +#include namespace modm @@ -142,35 +143,33 @@ public: %% endif } + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static uint8_t transferBlocking(uint8_t data) { - return RF_CALL_BLOCKING(transfer(data)); + return transfer(data); } + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static void transferBlocking(const uint8_t *tx, uint8_t *rx, std::size_t length) { - RF_CALL_BLOCKING(transfer(tx, rx, length)); + transfer(tx, rx, length); } - static modm::ResumableResult + static uint8_t transfer(uint8_t data); - static modm::ResumableResult + static void transfer(const uint8_t *tx, uint8_t *rx, std::size_t length); protected: static void initialize(uint16_t prescaler); - -private: - // bit 7 (0x80) is used for transfer 1 byte - // bit 6 (0x40) is used for transfer multiple byte - static inline uint8_t state{}; - %% if not extended +%# +private: static inline DataOrder dataOrder = DataOrder::MsbFirst; %% endif }; diff --git a/src/modm/platform/spi/bitbang/bitbang_spi_master.hpp b/src/modm/platform/spi/bitbang/bitbang_spi_master.hpp index 6de402fd96..960c6e4ad0 100644 --- a/src/modm/platform/spi/bitbang/bitbang_spi_master.hpp +++ b/src/modm/platform/spi/bitbang/bitbang_spi_master.hpp @@ -21,6 +21,7 @@ #include #include #include +#include namespace modm { @@ -61,28 +62,26 @@ class BitBangSpiMaster : public ::modm::SpiMaster, static void setDataOrder(DataOrder order); + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static uint8_t transferBlocking(uint8_t data); + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static void transferBlocking(const uint8_t *tx, uint8_t *rx, std::size_t length); - static modm::ResumableResult + static uint8_t transfer(uint8_t data); - static modm::ResumableResult + static void transfer(const uint8_t *tx, uint8_t *rx, std::size_t length); private: static void delay(); - static uint16_t delayTime; - - static uint8_t operationMode; - static uint8_t count; - static void *context; - static ConfigurationHandler configuration; + static inline uint16_t delayTime{1}; + static inline uint8_t operationMode{0}; }; } // namespace platform diff --git a/src/modm/platform/spi/bitbang/bitbang_spi_master_impl.hpp b/src/modm/platform/spi/bitbang/bitbang_spi_master_impl.hpp index ea2f614f7d..be5db0363f 100644 --- a/src/modm/platform/spi/bitbang/bitbang_spi_master_impl.hpp +++ b/src/modm/platform/spi/bitbang/bitbang_spi_master_impl.hpp @@ -17,15 +17,6 @@ #ifndef MODM_SOFTWARE_BITBANG_SPI_MASTER_HPP # error "Don't include this file directly, use 'bitbang_spi_master.hpp' instead!" #endif -#include - -template -uint16_t -modm::platform::BitBangSpiMaster::delayTime(1); - -template -uint8_t -modm::platform::BitBangSpiMaster::operationMode(0); // ---------------------------------------------------------------------------- @@ -83,7 +74,7 @@ modm::platform::BitBangSpiMaster::setDataOrder(DataOrder order) template uint8_t -modm::platform::BitBangSpiMaster::transferBlocking(uint8_t data) +modm::platform::BitBangSpiMaster::transfer(uint8_t data) { modm::this_fiber::yield(); @@ -135,7 +126,7 @@ modm::platform::BitBangSpiMaster::transferBlocking(uint8_t data template void -modm::platform::BitBangSpiMaster::transferBlocking( +modm::platform::BitBangSpiMaster::transfer( const uint8_t *tx, uint8_t *rx, std::size_t length) { uint8_t tx_byte = 0xff; @@ -145,33 +136,25 @@ modm::platform::BitBangSpiMaster::transferBlocking( { if (tx) tx_byte = tx[i]; - rx_byte = transferBlocking(tx_byte); + rx_byte = transfer(tx_byte); if (rx) rx[i] = rx_byte; } } template -modm::ResumableResult -modm::platform::BitBangSpiMaster::transfer(uint8_t data) +uint8_t +modm::platform::BitBangSpiMaster::transferBlocking(uint8_t data) { - data = transferBlocking(data); -#ifdef MODM_RESUMABLE_IS_FIBER - return data; -#else - return {modm::rf::Stop, data}; -#endif + return transfer(data); } template -modm::ResumableResult -modm::platform::BitBangSpiMaster::transfer( +void +modm::platform::BitBangSpiMaster::transferBlocking( const uint8_t *tx, uint8_t *rx, std::size_t length) { - transferBlocking(tx, rx, length); -#ifndef MODM_RESUMABLE_IS_FIBER - return {modm::rf::Stop, 0}; -#endif + transfer(tx, rx, length); } // ---------------------------------------------------------------------------- diff --git a/src/modm/platform/spi/bitbang/module.lb b/src/modm/platform/spi/bitbang/module.lb index 10e9310831..012112e151 100644 --- a/src/modm/platform/spi/bitbang/module.lb +++ b/src/modm/platform/spi/bitbang/module.lb @@ -19,6 +19,7 @@ def prepare(module, options): return False module.depends( + ":architecture:fiber", ":architecture:delay", ":architecture:spi", ":platform:gpio") diff --git a/src/modm/platform/spi/rp/module.lb b/src/modm/platform/spi/rp/module.lb index cff1b1e8df..d33a48fa6b 100644 --- a/src/modm/platform/spi/rp/module.lb +++ b/src/modm/platform/spi/rp/module.lb @@ -23,10 +23,7 @@ class Instance(Module): return True def build(self, env): - env.substitutions = { - "id": self.instance, - "use_fiber": env.get(":processing:protothread:use_fiber", True) - } + env.substitutions = {"id": self.instance} env.outbasepath = "modm/src/modm/platform/spi" env.template("spi_master.hpp.in", "spi_master_{}.hpp".format(self.instance)) @@ -49,6 +46,7 @@ def prepare(module, options): module.depends( ":architecture:spi", ":architecture:assert", + ":architecture:fiber", ":cmsis:device", ":math:algorithm", ":platform:gpio", diff --git a/src/modm/platform/spi/rp/spi_master.cpp.in b/src/modm/platform/spi/rp/spi_master.cpp.in index ca1d026cb9..42d70b3b3d 100644 --- a/src/modm/platform/spi/rp/spi_master.cpp.in +++ b/src/modm/platform/spi/rp/spi_master.cpp.in @@ -25,10 +25,9 @@ void modm::platform::SpiMaster{{ id }}::unreset() Resets::unresetWait(RESETS_RESET_SPI{{ id }}_BITS); } -modm::ResumableResult +uint8_t modm::platform::SpiMaster{{ id }}::transfer(uint8_t data) { -%% if use_fiber // wait for previous transfer to finish while (txFifoFull()) modm::this_fiber::yield(); @@ -38,41 +37,12 @@ modm::platform::SpiMaster{{ id }}::transfer(uint8_t data) while (rxFifoEmpty()) modm::this_fiber::yield(); return read(); -%% else - // this is a manually implemented "fast resumable function" - // there is no context or nesting protection, since we don't need it. - // there are only two states encoded into 1 bit (LSB of state): - // 1. waiting to start, and - // 2. waiting to finish. - - // LSB != Bit0 ? - if ( !(state & Bit0) ) - { - // wait for previous transfer to finish - if (txFifoFull()) - return {modm::rf::Running}; - - // start transfer by copying data into register - write(data); - - // set LSB = Bit0 - state |= Bit0; - } - - if (rxFifoEmpty()) - return {modm::rf::Running}; - - // transfer finished - state &= ~Bit0; - return {modm::rf::Stop, read()}; -%% endif } -modm::ResumableResult +void modm::platform::SpiMaster{{ id }}::transfer( const uint8_t * tx, uint8_t * rx, std::size_t length) { -%% if use_fiber for (std::size_t index = 0; index < length; index++) { // if tx == 0, we use a dummy byte 0x00 else we copy it from the array @@ -80,62 +50,4 @@ modm::platform::SpiMaster{{ id }}::transfer( // if rx != 0, we copy the result into the array if (rx) rx[index] = result; } -%% else - // this is a manually implemented "fast resumable function" - // there is no context or nesting protection, since we don't need it. - // there are only two states encoded into 1 bit (Bit1 of state): - // 1. initialize index, and - // 2. wait for 1-byte transfer to finish. - - // we need to globally remember which byte we are currently transferring - static std::size_t index = 0; - - // we are only interested in Bit1 - switch(state & Bit1) - { - case 0: - // we will only visit this state once - state |= Bit1; - - // initialize index and check range - index = 0; - while (index < length) - { - default: - { - // if tx == 0, we use a dummy byte 0x00 - // else we copy it from the array - // call the resumable function - modm::ResumableResult result = transfer(tx ? tx[index] : 0); - - // if the resumable function is still running, so are we - if (result.getState() > modm::rf::NestingError) - return {modm::rf::Running}; - - // if rx != 0, we copy the result into the array - if (rx) rx[index] = result.getResult(); - } - index++; - } - - // clear the state - state &= ~Bit1; - return {modm::rf::Stop}; - } -%% endif -} - -void modm::platform::SpiMaster{{ id }}::transferBlocking( - const uint8_t *tx, std::size_t length) -{ - std::size_t index = 0; - while (index < length) { - // Wait for tx empty - while(txFifoFull()) __NOP(); - // Write next byte - write(tx ? tx[index] : 0); - index++; - } - - drainRx(); } diff --git a/src/modm/platform/spi/rp/spi_master.hpp.in b/src/modm/platform/spi/rp/spi_master.hpp.in index a95ec572d1..cc27553e0e 100644 --- a/src/modm/platform/spi/rp/spi_master.hpp.in +++ b/src/modm/platform/spi/rp/spi_master.hpp.in @@ -15,6 +15,7 @@ #include #include #include +#include #include @@ -165,41 +166,27 @@ public: modm_assert(order == DataOrder::MsbFirst, "SPI DataOrder", "SPI hardware does not support alternate transmit order"); } + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static uint8_t transferBlocking(uint8_t data) { - return RF_CALL_BLOCKING(transfer(data)); + return transfer(data); } + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static void transferBlocking(const uint8_t *tx, uint8_t *rx, std::size_t length) { - // If we do not need to receive data, use a more efficient - // transmit-only routine to increase throughput - if(rx) { - RF_CALL_BLOCKING(transfer(tx, rx, length)); - } else { - transferBlocking(tx, length); - } + transfer(tx, rx, length); } - static modm::ResumableResult + static uint8_t transfer(uint8_t data); - static modm::ResumableResult + static void transfer(const uint8_t *tx, uint8_t *rx, std::size_t length); protected: - /** Perform transmit-only transaction - * - * A faster version of blocking transfer when transmitting only. - * - * If no receive is needed, the next byte can be loaded while the - * current transfer is in progress. - */ - static void - transferBlocking(const uint8_t *tx, std::size_t length); - static inline void write(uint8_t data) { diff --git a/src/modm/platform/spi/rp/spi_master_dma.hpp.in b/src/modm/platform/spi/rp/spi_master_dma.hpp.in index 0b59e3f2f3..7aed8f2998 100644 --- a/src/modm/platform/spi/rp/spi_master_dma.hpp.in +++ b/src/modm/platform/spi/rp/spi_master_dma.hpp.in @@ -56,22 +56,24 @@ public: static void initialize(); + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static uint8_t transferBlocking(uint8_t data) { - return RF_CALL_BLOCKING(transfer(data)); + return transfer(data); } + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static void transferBlocking(const uint8_t *tx, uint8_t *rx, std::size_t length) { - RF_CALL_BLOCKING(transfer(tx, rx, length)); + transfer(tx, rx, length); } - static modm::ResumableResult + static uint8_t transfer(uint8_t data); - static modm::ResumableResult + static void transfer(const uint8_t *tx, uint8_t *rx, std::size_t length); static void @@ -81,7 +83,7 @@ public: static void waitCompleted(Wait wait); static void waitCompleted() { - waitCompleted([](){}); + waitCompleted(modm::this_fiber::yield); } private: diff --git a/src/modm/platform/spi/rp/spi_master_dma_impl.hpp.in b/src/modm/platform/spi/rp/spi_master_dma_impl.hpp.in index 96bfbd1c95..be68612aca 100644 --- a/src/modm/platform/spi/rp/spi_master_dma_impl.hpp.in +++ b/src/modm/platform/spi/rp/spi_master_dma_impl.hpp.in @@ -29,7 +29,7 @@ modm::platform::SpiMaster{{ id }}_Dma::initialize() } template -modm::ResumableResult +uint8_t modm::platform::SpiMaster{{ id }}_Dma::transfer(uint8_t data) { // this is a manually implemented "fast resumable function" @@ -112,11 +112,10 @@ modm::platform::SpiMaster{{ id }}_Dma::waitCompleted } template -modm::ResumableResult +void modm::platform::SpiMaster{{ id }}_Dma::transfer( const uint8_t *tx, uint8_t *rx, std::size_t length) { -%% if use_fiber if ((!rx and !tx) or length == 0) return; if constexpr (Dma::RxChannel::mask == 0) rx = nullptr; @@ -139,49 +138,4 @@ modm::platform::SpiMaster{{ id }}_Dma::transfer( } disableDreq(Dma::DreqTx | Dma::DreqRx); -%% else - if ( (!rx && !tx) || length == 0) { - return {modm::rf::Stop}; - } - if constexpr (Dma::RxChannel::mask == 0) rx = nullptr; - // this is a manually implemented "fast resumable function" - // there is no context or nesting protection, since we don't need it. - // there are only two states encoded into 1 bit (Bit1 of state): - // 1. initialize index, and - // 2. wait for 1-byte transfer to finish. - - // we are only interested in Bit1 - switch(state & Bit1) - { - case 0: - // we will only visit this state once - state |= Bit1; - startTransfer(tx,rx,length); - - [[fallthrough]]; - - default: - if (Dma::TxChannel::isBusy() or (rx && Dma::RxChannel::isBusy())) - return { modm::rf::Running }; - if (!txFifoEmpty() or (rx && !rxFifoEmpty()) or isBusy()) - return { modm::rf::Running }; - if (!rx) { - // Drain RX FIFO, then wait for shifting to finish (which may be *after* - // TX FIFO drains), then drain RX FIFO again - while (!rxFifoEmpty()) - (void)read(); - if (isBusy()) - return { modm::rf::Running }; - - // Don't leave overrun flag set - spi{{ id }}_hw->icr = SPI_SSPICR_RORIC_BITS; - } - - - disableDreq(Dma::DreqTx|Dma::DreqRx); - // clear the state - state &= ~Bit1; - return {modm::rf::Stop}; - } -%% endif } diff --git a/src/modm/platform/spi/sam/module.lb b/src/modm/platform/spi/sam/module.lb index 82bc97f3b6..c67f28a698 100644 --- a/src/modm/platform/spi/sam/module.lb +++ b/src/modm/platform/spi/sam/module.lb @@ -14,7 +14,6 @@ def get_properties(env): device = env[":target"] driver = device.get_driver("spi") properties = {} - properties["use_fiber"] = env.get(":processing:protothread:use_fiber", True) properties["target"] = device.identifier properties["features"] = driver["feature"] if "feature" in driver else [] return properties diff --git a/src/modm/platform/spi/sam/spi_master.cpp.in b/src/modm/platform/spi/sam/spi_master.cpp.in index b069d79172..cb2265ea9e 100644 --- a/src/modm/platform/spi/sam/spi_master.cpp.in +++ b/src/modm/platform/spi/sam/spi_master.cpp.in @@ -17,10 +17,9 @@ #include "spi_master_{{id}}.hpp" -modm::ResumableResult +uint8_t modm::platform::SpiMaster{{ id }}::transfer(uint8_t data) { -%% if use_fiber // Clear any stale rx data ready status read(); @@ -36,44 +35,12 @@ modm::platform::SpiMaster{{ id }}::transfer(uint8_t data) modm::this_fiber::yield(); return read(); -%% else - // this is a manually implemented "fast resumable function" - // there is no context or nesting protection, since we don't need it. - // there are only two states encoded into 1 bit (LSB of state): - // 1. waiting to start, and - // 2. waiting to finish. - - // LSB != Bit0 ? - if ( !(state & Bit0) ) - { - // Clear any stale rx data ready status - read(); - - // wait for previous transfer to finish - if (!isTransmitDataRegisterEmpty()) - return {modm::rf::Running}; - - // start transfer by copying data into register - write(data); - - // set LSB = Bit0 - state |= Bit0; - } - - if (!isReceiveDataRegisterFull()) - return {modm::rf::Running}; - - // transfer finished - state &= ~Bit0; - return {modm::rf::Stop, read()}; -%% endif } -modm::ResumableResult +void modm::platform::SpiMaster{{ id }}::transfer( const uint8_t * tx, uint8_t * rx, std::size_t length) { -%% if use_fiber for (std::size_t index = 0; index < length; index++) { // if tx == 0, we use a dummy byte 0x00 else we copy it from the array @@ -81,63 +48,4 @@ modm::platform::SpiMaster{{ id }}::transfer( // if rx != 0, we copy the result into the array if (rx) rx[index] = result; } -%% else - // this is a manually implemented "fast resumable function" - // there is no context or nesting protection, since we don't need it. - // there are only two states encoded into 1 bit (Bit1 of state): - // 1. initialize index, and - // 2. wait for 1-byte transfer to finish. - - // we need to globally remember which byte we are currently transferring - static std::size_t index = 0; - - // we are only interested in Bit1 - switch(state & Bit1) - { - case 0: - // we will only visit this state once - state |= Bit1; - - // initialize index and check range - index = 0; - while (index < length) - { - default: - { - // if tx == 0, we use a dummy byte 0x00 - // else we copy it from the array - // call the resumable function - modm::ResumableResult result = transfer(tx ? tx[index] : 0); - - // if the resumable function is still running, so are we - if (result.getState() > modm::rf::NestingError) - return {modm::rf::Running}; - - // if rx != 0, we copy the result into the array - if (rx) rx[index] = result.getResult(); - } - index++; - } - - // clear the state - state &= ~Bit1; - return {modm::rf::Stop}; - } -%% endif } - -void modm::platform::SpiMaster{{ id }}::transferBlocking( - const uint8_t *tx, std::size_t length) -{ - uint8_t index = 0; - while(index < length) { - // Wait for tx empty - while(!isTransmitDataRegisterEmpty()); - // Write next byte - write(tx ? tx[index] : 0); - index++; - } - // wait for the internal shift register to be empty, indicating the transmission of the final byte is complete - while(!isTxEmpty()); -} - diff --git a/src/modm/platform/spi/sam/spi_master.hpp.in b/src/modm/platform/spi/sam/spi_master.hpp.in index a6f9ef804d..59de086efe 100644 --- a/src/modm/platform/spi/sam/spi_master.hpp.in +++ b/src/modm/platform/spi/sam/spi_master.hpp.in @@ -15,6 +15,7 @@ #include #include #include +#include namespace modm::platform { @@ -114,26 +115,22 @@ public: modm_assert(order == DataOrder::MsbFirst, "SPI DataOrder", "SPI hardware does not support alternate transmit order"); } + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static uint8_t transferBlocking(uint8_t data) { - return RF_CALL_BLOCKING(transfer(data)); + return transfer(data); } + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static void transferBlocking(const uint8_t *tx, uint8_t *rx, std::size_t length) { - // If we do not need to receive data, use a more efficient - // transmit-only routine to increase throughput - if(rx) { - RF_CALL_BLOCKING(transfer(tx, rx, length)); - } else { - transferBlocking(tx, length); - } + transfer(tx, rx, length); } - static modm::ResumableResult + static uint8_t transfer(uint8_t data); - static modm::ResumableResult + static void transfer(const uint8_t *tx, uint8_t *rx, std::size_t length); /** Enable local loopback mode, to internally connect MOSI to MISO */ @@ -145,17 +142,6 @@ public: Regs()->SPI_MR &= ~SPI_MR_LLB; } } - -protected: - /** Perform transmit-only transaction - * - * A faster version of blocking transfer when transmitting only. - * - * If no receive is needed, the next byte can be loaded while the - * current transfer is in progress. - */ - static void - transferBlocking(const uint8_t *tx, std::size_t length); }; } // namespace modm::platform diff --git a/src/modm/platform/spi/sam_x7x/module.lb b/src/modm/platform/spi/sam_x7x/module.lb index f10473fe97..8f596a57b0 100644 --- a/src/modm/platform/spi/sam_x7x/module.lb +++ b/src/modm/platform/spi/sam_x7x/module.lb @@ -15,7 +15,6 @@ def get_properties(env): device = env[":target"] properties = {} - properties["use_fiber"] = env.get(":processing:protothread:use_fiber", True) properties["target"] = device.identifier return properties @@ -29,7 +28,6 @@ class Instance(Module): module.description = "Instance {}".format(self.instance) def prepare(self, module, options): - module.depends(":platform:spi") return True def build(self, env): @@ -61,6 +59,7 @@ def prepare(module, options): module.depends( ":architecture:register", ":architecture:spi", + ":architecture:fiber", ":cmsis:device", ":math:algorithm", ":platform:gpio") diff --git a/src/modm/platform/spi/sam_x7x/spi_master.cpp.in b/src/modm/platform/spi/sam_x7x/spi_master.cpp.in index f9a8cc0c58..a5312b7143 100644 --- a/src/modm/platform/spi/sam_x7x/spi_master.cpp.in +++ b/src/modm/platform/spi/sam_x7x/spi_master.cpp.in @@ -17,10 +17,9 @@ #include "spi_master_{{id}}.hpp" -modm::ResumableResult +uint8_t modm::platform::SpiMaster{{ id }}::transfer(uint8_t data) { -%% if use_fiber // wait for previous transfer to finish while(!(SpiHal{{ id }}::readStatusFlags() & StatusFlag::TxRegisterEmpty)) modm::this_fiber::yield(); @@ -37,46 +36,12 @@ modm::platform::SpiMaster{{ id }}::transfer(uint8_t data) SpiHal{{ id }}::read(value); return uint8_t(value); -%% else - // this is a manually implemented "fast resumable function" - // there is no context or nesting protection, since we don't need it. - // there are only two states encoded into 1 bit (LSB of state): - // 1. waiting to start, and - // 2. waiting to finish. - - const auto status = SpiHal{{ id }}::readStatusFlags(); - - // LSB != Bit0 ? - if (!(state & Bit0)) - { - // wait for previous transfer to finish - if (!(status & StatusFlag::TxRegisterEmpty)) - return {modm::rf::Running}; - - // start transfer by copying data into register - SpiHal{{ id }}::write(std::byte{data}); - - // set LSB = Bit0 - state |= Bit0; - } - - if (!(status & StatusFlag::RxRegisterFull)) - return {modm::rf::Running}; - - std::byte value; - SpiHal{{ id }}::read(value); - - // transfer finished - state &= ~Bit0; - return {modm::rf::Stop, uint8_t(value)}; -%% endif } -modm::ResumableResult +void modm::platform::SpiMaster{{ id }}::transfer( const uint8_t* tx, uint8_t* rx, std::size_t length) { -%% if use_fiber for (std::size_t index = 0; index < length; index++) { // if tx == 0, we use a dummy byte 0x00 else we copy it from the array @@ -84,47 +49,4 @@ modm::platform::SpiMaster{{ id }}::transfer( // if rx != 0, we copy the result into the array if (rx) rx[index] = result; } -%% else - // this is a manually implemented "fast resumable function" - // there is no context or nesting protection, since we don't need it. - // there are only two states encoded into 1 bit (Bit1 of state): - // 1. initialize index, and - // 2. wait for 1-byte transfer to finish. - - // we need to globally remember which byte we are currently transferring - static std::size_t index = 0; - - // we are only interested in Bit1 - switch(state & Bit1) - { - case 0: - // we will only visit this state once - state |= Bit1; - - // initialize index and check range - index = 0; - while (index < length) - { - default: - { - // if tx == 0, we use a dummy byte 0x00 - // else we copy it from the array - // call the resumable function - modm::ResumableResult result = transfer(tx ? tx[index] : 0); - - // if the resumable function is still running, so are we - if (result.getState() > modm::rf::NestingError) - return {modm::rf::Running}; - - // if rx != 0, we copy the result into the array - if (rx) rx[index] = result.getResult(); - } - index++; - } - - // clear the state - state &= ~Bit1; - return {modm::rf::Stop}; - } -%% endif } diff --git a/src/modm/platform/spi/sam_x7x/spi_master.hpp.in b/src/modm/platform/spi/sam_x7x/spi_master.hpp.in index 3cd2f1326a..e4285b711e 100644 --- a/src/modm/platform/spi/sam_x7x/spi_master.hpp.in +++ b/src/modm/platform/spi/sam_x7x/spi_master.hpp.in @@ -21,6 +21,7 @@ #include #include #include +#include #include #include "spi_hal_{{ id }}.hpp" @@ -36,10 +37,6 @@ namespace modm::platform */ class SpiMaster{{ id }} : public modm::SpiMaster, public SpiLock, public SpiBase { -private: - // Bit0: single transfer state - // Bit1: block transfer state - static inline uint8_t state{0}; public: using DataMode = SpiBase::DataMode; using Hal = SpiHal{{ id }}; @@ -78,7 +75,6 @@ public: assertBaudrateInTolerance(); SpiHal{{ id }}::initialize(result.prescaler); - state = 0; } static void @@ -93,22 +89,24 @@ public: SpiHal{{ id }}::setDataSize(size); } + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static uint8_t transferBlocking(uint8_t data) { - return RF_CALL_BLOCKING(transfer(data)); + return transfer(data); } + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static void transferBlocking(const uint8_t* tx, uint8_t* rx, std::size_t length) { - RF_CALL_BLOCKING(transfer(tx, rx, length)); + transfer(tx, rx, length); } - static modm::ResumableResult + static uint8_t transfer(uint8_t data); - static modm::ResumableResult + static void transfer(const uint8_t* tx, uint8_t* rx, std::size_t length); }; diff --git a/src/modm/platform/spi/stm32/module.lb b/src/modm/platform/spi/stm32/module.lb index a3a1278613..ff9670f876 100644 --- a/src/modm/platform/spi/stm32/module.lb +++ b/src/modm/platform/spi/stm32/module.lb @@ -16,7 +16,6 @@ def get_properties(env): device = env[":target"] driver = device.get_driver("spi") properties = {} - properties["use_fiber"] = env.get(":processing:protothread:use_fiber", True) properties["target"] = device.identifier properties["features"] = driver["feature"] if "feature" in driver else [] return properties @@ -69,6 +68,7 @@ def prepare(module, options): module.depends( ":architecture:register", ":architecture:spi", + ":architecture:fiber", ":cmsis:device", ":math:algorithm", ":platform:gpio", diff --git a/src/modm/platform/spi/stm32/spi_master.cpp.in b/src/modm/platform/spi/stm32/spi_master.cpp.in index 07baf4b5da..7b6d8fd12b 100644 --- a/src/modm/platform/spi/stm32/spi_master.cpp.in +++ b/src/modm/platform/spi/stm32/spi_master.cpp.in @@ -16,10 +16,9 @@ #include "spi_master_{{id}}.hpp" -modm::ResumableResult +uint8_t modm::platform::SpiMaster{{ id }}::transfer(uint8_t data) { -%% if use_fiber // wait for previous transfer to finish while(!SpiHal{{ id }}::isTransmitRegisterEmpty()) modm::this_fiber::yield(); @@ -35,43 +34,12 @@ modm::platform::SpiMaster{{ id }}::transfer(uint8_t data) SpiHal{{ id }}::read(data); return data; -%% else - // this is a manually implemented "fast resumable function" - // there is no context or nesting protection, since we don't need it. - // there are only two states encoded into 1 bit (LSB of state): - // 1. waiting to start, and - // 2. waiting to finish. - - // LSB != Bit0 ? - if ( !(state & Bit0) ) - { - // wait for previous transfer to finish - if (!SpiHal{{ id }}::isTransmitRegisterEmpty()) - return {modm::rf::Running}; - - // start transfer by copying data into register - SpiHal{{ id }}::write(data); - - // set LSB = Bit0 - state |= Bit0; - } - - if (!SpiHal{{ id }}::isReceiveRegisterNotEmpty()) - return {modm::rf::Running}; - - SpiHal{{ id }}::read(data); - - // transfer finished - state &= ~Bit0; - return {modm::rf::Stop, data}; -%% endif } -modm::ResumableResult +void modm::platform::SpiMaster{{ id }}::transfer( const uint8_t * tx, uint8_t * rx, std::size_t length) { -%% if use_fiber for (std::size_t index = 0; index < length; index++) { // if tx == 0, we use a dummy byte 0x00 else we copy it from the array @@ -79,47 +47,4 @@ modm::platform::SpiMaster{{ id }}::transfer( // if rx != 0, we copy the result into the array if (rx) rx[index] = result; } -%% else - // this is a manually implemented "fast resumable function" - // there is no context or nesting protection, since we don't need it. - // there are only two states encoded into 1 bit (Bit1 of state): - // 1. initialize index, and - // 2. wait for 1-byte transfer to finish. - - // we need to globally remember which byte we are currently transferring - static std::size_t index = 0; - - // we are only interested in Bit1 - switch(state & Bit1) - { - case 0: - // we will only visit this state once - state |= Bit1; - - // initialize index and check range - index = 0; - while (index < length) - { - default: - { - // if tx == 0, we use a dummy byte 0x00 - // else we copy it from the array - // call the resumable function - modm::ResumableResult result = transfer(tx ? tx[index] : 0); - - // if the resumable function is still running, so are we - if (result.getState() > modm::rf::NestingError) - return {modm::rf::Running}; - - // if rx != 0, we copy the result into the array - if (rx) rx[index] = result.getResult(); - } - index++; - } - - // clear the state - state &= ~Bit1; - return {modm::rf::Stop}; - } -%% endif } diff --git a/src/modm/platform/spi/stm32/spi_master.hpp.in b/src/modm/platform/spi/stm32/spi_master.hpp.in index bb94cee464..4fd60f63a7 100644 --- a/src/modm/platform/spi/stm32/spi_master.hpp.in +++ b/src/modm/platform/spi/stm32/spi_master.hpp.in @@ -22,6 +22,7 @@ #include #include #include +#include #include "spi_hal_{{ id }}.hpp" namespace modm @@ -40,11 +41,6 @@ namespace platform */ class SpiMaster{{ id }} : public modm::SpiMaster, public SpiLock { -protected: - // `state` must be protected to allow access from SpiMasterDma subclass - // Bit0: single transfer state - // Bit1: block transfer state - static inline uint8_t state{0}; public: using Hal = SpiHal{{ id }}; @@ -98,7 +94,6 @@ public: // initialize the Spi SpiHal{{ id }}::initialize(prescaler); - state = 0; } static void @@ -124,23 +119,25 @@ public: SpiHal{{ id }}::enableTransfer(); } + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static uint8_t transferBlocking(uint8_t data) { - return RF_CALL_BLOCKING(transfer(data)); + return transfer(data); } + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static void transferBlocking(const uint8_t *tx, uint8_t *rx, std::size_t length) { - RF_CALL_BLOCKING(transfer(tx, rx, length)); + transfer(tx, rx, length); } - static modm::ResumableResult + static uint8_t transfer(uint8_t data); - static modm::ResumableResult + static void transfer(const uint8_t *tx, uint8_t *rx, std::size_t length); }; diff --git a/src/modm/platform/spi/stm32/spi_master_dma.hpp.in b/src/modm/platform/spi/stm32/spi_master_dma.hpp.in index 1f53b2eb1c..7649a24dd6 100644 --- a/src/modm/platform/spi/stm32/spi_master_dma.hpp.in +++ b/src/modm/platform/spi/stm32/spi_master_dma.hpp.in @@ -13,6 +13,7 @@ #define MODM_STM32_SPI_MASTER{{ id }}_DMA_HPP #include +#include #include "spi_master_{{ id }}.hpp" namespace modm @@ -56,22 +57,24 @@ public: static void initialize(); + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static uint8_t transferBlocking(uint8_t data) { - return RF_CALL_BLOCKING(transfer(data)); + return transfer(data); } + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static void transferBlocking(const uint8_t *tx, uint8_t *rx, std::size_t length) { - RF_CALL_BLOCKING(transfer(tx, rx, length)); + transfer(tx, rx, length); } - static modm::ResumableResult + static uint8_t transfer(uint8_t data); - static modm::ResumableResult + static void transfer(const uint8_t *tx, uint8_t *rx, std::size_t length); private: diff --git a/src/modm/platform/spi/stm32/spi_master_dma_impl.hpp.in b/src/modm/platform/spi/stm32/spi_master_dma_impl.hpp.in index 5d8102a8f9..e3138c2bee 100644 --- a/src/modm/platform/spi/stm32/spi_master_dma_impl.hpp.in +++ b/src/modm/platform/spi/stm32/spi_master_dma_impl.hpp.in @@ -51,10 +51,9 @@ modm::platform::SpiMaster{{ id }}_Dma::initialize() } template -modm::ResumableResult +uint8_t modm::platform::SpiMaster{{ id }}_Dma::transfer(uint8_t data) { -%% if use_fiber // disable DMA for single byte transfer SpiHal{{ id }}::disableInterrupt(SpiBase::Interrupt::TxDmaEnable | SpiBase::Interrupt::RxDmaEnable); @@ -74,47 +73,13 @@ modm::platform::SpiMaster{{ id }}_Dma::transfer(uint SpiHal{{ id }}::read(data); return data; -%% else - // this is a manually implemented "fast resumable function" - // there is no context or nesting protection, since we don't need it. - // there are only two states encoded into 1 bit (LSB of state): - // 1. waiting to start, and - // 2. waiting to finish. - // LSB != Bit0 ? - if ( !(state & Bit0) ) - { - // disable DMA for single byte transfer - SpiHal{{ id }}::disableInterrupt(SpiBase::Interrupt::TxDmaEnable | - SpiBase::Interrupt::RxDmaEnable); - - // wait for previous transfer to finish - if (!SpiHal{{ id }}::isTransmitRegisterEmpty()) - return {modm::rf::Running}; - - // start transfer by copying data into register - SpiHal{{ id }}::write(data); - - // set LSB = Bit0 - state |= Bit0; - } - - if (!SpiHal{{ id }}::isReceiveRegisterNotEmpty()) - return {modm::rf::Running}; - - SpiHal{{ id }}::read(data); - - // transfer finished - state &= ~Bit0; - return {modm::rf::Stop, data}; -%% endif } template -modm::ResumableResult +void modm::platform::SpiMaster{{ id }}_Dma::transfer( const uint8_t *tx, uint8_t *rx, std::size_t length) { -%% if use_fiber dmaError = false; SpiHal{{ id }}::enableInterrupt( SpiBase::Interrupt::TxDmaEnable | SpiBase::Interrupt::RxDmaEnable); @@ -159,70 +124,6 @@ modm::platform::SpiMaster{{ id }}_Dma::transfer( SpiHal{{ id }}::disableInterrupt( SpiBase::Interrupt::TxDmaEnable | SpiBase::Interrupt::RxDmaEnable); - -%% else - // this is a manually implemented "fast resumable function" - // there is no context or nesting protection, since we don't need it. - // there are only two states encoded into 1 bit (Bit1 of state): - // 1. initialize index, and - // 2. wait for 1-byte transfer to finish. - - // we are only interested in Bit1 - switch(state & Bit1) - { - case 0: - // we will only visit this state once - state |= Bit1; - dmaError = false; - SpiHal{{ id }}::enableInterrupt(SpiBase::Interrupt::TxDmaEnable | - SpiBase::Interrupt::RxDmaEnable); - - if (tx) { - Dma::TxChannel::setMemoryAddress(uint32_t(tx)); - Dma::TxChannel::setMemoryIncrementMode(true); - } else { - Dma::TxChannel::setMemoryAddress(uint32_t(&dmaDummy)); - Dma::TxChannel::setMemoryIncrementMode(false); - } - if (rx) { - Dma::RxChannel::setMemoryAddress(uint32_t(rx)); - Dma::RxChannel::setMemoryIncrementMode(true); - } else { - Dma::RxChannel::setMemoryAddress(uint32_t(&dmaDummy)); - Dma::RxChannel::setMemoryIncrementMode(false); - } - - Dma::RxChannel::setDataLength(length); - dmaReceiveComplete = false; - Dma::RxChannel::start(); - - Dma::TxChannel::setDataLength(length); - dmaTransmitComplete = false; - Dma::TxChannel::start(); - - [[fallthrough]]; - - default: - if (not dmaError) - { - if (not dmaTransmitComplete and not dmaReceiveComplete) - return { modm::rf::Running }; - if (SpiHal{{ id }}::getInterruptFlags() & SpiBase::InterruptFlag::Busy) - return { modm::rf::Running }; -%% if "fifo" in features - if (SpiHal{{ id }}::getInterruptFlags() & (SpiBase::InterruptFlag::FifoTxLevel | - SpiBase::InterruptFlag::FifoRxLevel)) - return { modm::rf::Running }; -%% endif - } - - SpiHal{{ id }}::disableInterrupt(SpiBase::Interrupt::TxDmaEnable | - SpiBase::Interrupt::RxDmaEnable); - // clear the state - state &= ~Bit1; - return {modm::rf::Stop}; - } -%% endif } template diff --git a/src/modm/platform/spi/stm32_uart/module.lb b/src/modm/platform/spi/stm32_uart/module.lb index 67fe3ba014..590f811884 100644 --- a/src/modm/platform/spi/stm32_uart/module.lb +++ b/src/modm/platform/spi/stm32_uart/module.lb @@ -17,7 +17,6 @@ def get_properties(env): "target": device.identifier, "driver": driver, "over8_sampling": ("feature" in driver) and ("over8" in driver["feature"]), - "use_fiber": env.get(":processing:protothread:use_fiber", True) } return properties @@ -56,6 +55,7 @@ def prepare(module, options): module.depends( ":architecture:spi", + ":architecture:fiber", ":cmsis:device", ":platform:gpio", ":platform:uart") diff --git a/src/modm/platform/spi/stm32_uart/uart_spi_master.cpp.in b/src/modm/platform/spi/stm32_uart/uart_spi_master.cpp.in index f03238143c..23a28c22bf 100644 --- a/src/modm/platform/spi/stm32_uart/uart_spi_master.cpp.in +++ b/src/modm/platform/spi/stm32_uart/uart_spi_master.cpp.in @@ -14,27 +14,17 @@ #include "uart_spi_master_{{id}}.hpp" #include -modm::platform::UartSpiMaster{{ id }}::DataOrder - modm::platform::UartSpiMaster{{ id }}::dataOrder = - modm::platform::UartSpiMaster{{ id }}::DataOrder::MsbFirst; - -// Bit0: single transfer state -// Bit1: block transfer state -uint8_t -modm::platform::UartSpiMaster{{ id }}::state(0); - // ---------------------------------------------------------------------------- -modm::ResumableResult +uint8_t modm::platform::UartSpiMaster{{ id }}::transfer(uint8_t data) { -%% if use_fiber // wait for previous transfer to finish while (!UsartHal{{ id }}::isTransmitRegisterEmpty()) modm::this_fiber::yield(); // start transfer by copying data into register - if(dataOrder == DataOrder::MsbFirst) + if (dataOrder == DataOrder::MsbFirst) data = ::modm::bitReverse(data); UsartHal{{ id }}::write(data); @@ -44,54 +34,16 @@ modm::platform::UartSpiMaster{{ id }}::transfer(uint8_t data) UsartHal{{ id }}::read(data); - if(dataOrder == DataOrder::MsbFirst) + if (dataOrder == DataOrder::MsbFirst) data = ::modm::bitReverse(data); return data; -%% else - // this is a manually implemented "fast resumable function" - // there is no context or nesting protection, since we don't need it. - // there are only two states encoded into 1 bit (LSB of state): - // 1. waiting to start, and - // 2. waiting to finish. - - // LSB == Bit0 ? - if ( !(state & Bit0) ) - { - // wait for previous transfer to finish - if (!UsartHal{{ id }}::isTransmitRegisterEmpty()) - return {modm::rf::Running}; - - // start transfer by copying data into register - if(dataOrder == DataOrder::MsbFirst) { - data = ::modm::bitReverse(data); - } - UsartHal{{ id }}::write(data); - - // set LSB = 1 - state |= Bit0; - } - - if (!UsartHal{{ id }}::isReceiveRegisterNotEmpty()) - return {modm::rf::Running}; - - UsartHal{{ id }}::read(data); - - if(dataOrder == DataOrder::MsbFirst) { - data = ::modm::bitReverse(data); - } - - // transfer finished - state &= ~Bit0; - return {modm::rf::Stop, data}; -%% endif } -modm::ResumableResult +void modm::platform::UartSpiMaster{{ id }}::transfer( const uint8_t * tx, uint8_t * rx, std::size_t length) { -%% if use_fiber for (std::size_t index = 0; index < length; index++) { // if tx == 0, we use a dummy byte 0x00 else we copy it from the array @@ -99,45 +51,4 @@ modm::platform::UartSpiMaster{{ id }}::transfer( // if rx != 0, we copy the result into the array if (rx) rx[index] = result; } -%% else - // this is a manually implemented "fast resumable function" - // there is no context or nesting protection, since we don't need it. - // there are only two states encoded into 1 bit (0x02 of state): - // 1. initialize index, and - // 2. wait for 1-byte transfer to finish. - - // we need to globally remember which byte we are currently transferring - static std::size_t index = 0; - - // we are only interested in Bit1 - switch(state & Bit1) - { - case 0: - // we will only visit this state once - state |= Bit1; - - // initialize index and check range - index = 0; - while (index < length) - { - default: - { - // call the resumable function - modm::ResumableResult result = transfer(tx ? tx[index] : 0); - - // if the resumable function is still running, so are we - if (result.getState() > modm::rf::NestingError) - return {modm::rf::Running}; - - // if rx != 0, we copy the result into the array - if (rx) rx[index] = result.getResult(); - } - index++; - } - - // clear the state - state &= ~Bit1; - return {modm::rf::Stop}; - } -%% endif } diff --git a/src/modm/platform/spi/stm32_uart/uart_spi_master.hpp.in b/src/modm/platform/spi/stm32_uart/uart_spi_master.hpp.in index 9915fc19cb..f2911a1927 100644 --- a/src/modm/platform/spi/stm32_uart/uart_spi_master.hpp.in +++ b/src/modm/platform/spi/stm32_uart/uart_spi_master.hpp.in @@ -17,6 +17,7 @@ #include #include #include +#include #include "../uart/uart_hal_{{ id }}.hpp" namespace modm @@ -37,11 +38,7 @@ namespace platform class UartSpiMaster{{ id }} : public modm::SpiMaster, public UartBase, public SpiLock { - static DataOrder dataOrder; - static uint8_t state; - static uint8_t count; - static void *context; - static ConfigurationHandler configuration; + static inline DataOrder dataOrder{DataOrder::MsbFirst}; public: enum class DataMode : uint32_t @@ -82,7 +79,6 @@ public: UsartHal{{ id }}::setReceiverEnable(true); UsartHal{{ id }}::enableOperation(); dataOrder = DataOrder::MsbFirst; - state = 0; } @@ -105,23 +101,25 @@ public: dataOrder = order; } + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static uint8_t transferBlocking(uint8_t data) { - return RF_CALL_BLOCKING(transfer(data)); + return transfer(data); } + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static void transferBlocking(const uint8_t *tx, uint8_t *rx, std::size_t length) { - RF_CALL_BLOCKING(transfer(tx, rx, length)); + transfer(tx, rx, length); } - static modm::ResumableResult + static uint8_t transfer(uint8_t data); - static modm::ResumableResult + static void transfer(const uint8_t *tx, uint8_t *rx, std::size_t length); }; diff --git a/src/modm/platform/spi/stm32h7/module.lb b/src/modm/platform/spi/stm32h7/module.lb index 5ec9b91010..865718282a 100644 --- a/src/modm/platform/spi/stm32h7/module.lb +++ b/src/modm/platform/spi/stm32h7/module.lb @@ -12,15 +12,6 @@ # file, You can obtain one at http://mozilla.org/MPL/2.0/. # ----------------------------------------------------------------------------- -def get_properties(env): - device = env[":target"] - driver = device.get_driver("spi") - properties = {} - properties["use_fiber"] = env.get(":processing:protothread:use_fiber", True) - properties["target"] = device.identifier - - return properties - class Instance(Module): def __init__(self, driver, instance): self.instance = int(instance) @@ -35,8 +26,10 @@ class Instance(Module): return True def build(self, env): - properties = get_properties(env) - properties["id"] = self.instance + properties = { + "target": env[":target"].identifier, + "id": self.instance + } device = env[":target"] if device.identifier.family in ["h7"]: @@ -74,6 +67,7 @@ def prepare(module, options): module.depends( ":architecture:register", + ":architecture:fiber", ":architecture:spi", ":cmsis:device", ":math:algorithm", @@ -87,7 +81,5 @@ def prepare(module, options): return True def build(env): - env.substitutions = get_properties(env) env.outbasepath = "modm/src/modm/platform/spi" - - env.template("spi_base.hpp.in") + env.copy("spi_base.hpp") diff --git a/src/modm/platform/spi/stm32h7/spi_base.hpp.in b/src/modm/platform/spi/stm32h7/spi_base.hpp similarity index 100% rename from src/modm/platform/spi/stm32h7/spi_base.hpp.in rename to src/modm/platform/spi/stm32h7/spi_base.hpp diff --git a/src/modm/platform/spi/stm32h7/spi_master.cpp.in b/src/modm/platform/spi/stm32h7/spi_master.cpp.in index 78da242913..5f1be286b3 100644 --- a/src/modm/platform/spi/stm32h7/spi_master.cpp.in +++ b/src/modm/platform/spi/stm32h7/spi_master.cpp.in @@ -20,10 +20,9 @@ namespace modm::platform { -modm::ResumableResult +uint8_t SpiMaster{{ id }}::transfer(uint8_t data) { -%% if use_fiber while (Hal::isTxFifoFull()) modm::this_fiber::yield(); @@ -33,40 +32,12 @@ SpiMaster{{ id }}::transfer(uint8_t data) modm::this_fiber::yield(); return Hal::read(); -%% else - // this is a manually implemented "fast resumable function" - // there is no context or nesting protection, since we don't need it. - // there are only two states encoded into 1 bit (LSB of state): - // 1. waiting to start, and - // 2. waiting to finish. - - if (!(state & Bit0)) - { - // wait for previous transfer to finish - if (Hal::isTxFifoFull()) - return {modm::rf::Running}; - - Hal::write(data); - - // set LSB = Bit0 - state |= Bit0; - } - - if (!Hal::isRxDataAvailable()) - return {modm::rf::Running}; - - data = Hal::read(); - - state &= ~Bit0; - return {modm::rf::Stop, data}; -%% endif } -modm::ResumableResult +void SpiMaster{{ id }}::transfer( const uint8_t* tx, uint8_t* rx, std::size_t length) { -%% if use_fiber std::size_t rxIndex = 0; std::size_t txIndex = 0; @@ -86,60 +57,13 @@ SpiMaster{{ id }}::transfer( modm::this_fiber::yield(); } } -%% else - // this is a manually implemented "fast resumable function" - // there is no context or nesting protection, since we don't need it. - // there are only two states encoded into 1 bit (Bit1 of state): - // 1. initialize index, and - // 2. wait for transfer to finish. - - // we need to globally remember which byte we are currently transferring - static std::size_t rxIndex = 0; - static std::size_t txIndex = 0; - - // we are only interested in Bit1 - switch(state & Bit1) - { - case 0: - // we will only visit this state once - state |= Bit1; - - // initialize index and check range - rxIndex = 0; - txIndex = 0; - while (rxIndex < length) { - default: - { - while ((txIndex < length) and !Hal::isTxFifoFull() and (txIndex - rxIndex) < Hal::FifoSize) { - Hal::write(tx ? tx[txIndex] : 0); - ++txIndex; - } - while ((rxIndex < length) and Hal::isRxDataAvailable()) { - if (rx) { - rx[rxIndex] = Hal::read(); - } else { - Hal::read(); - } - ++rxIndex; - } - if (rxIndex < length) { - return {modm::rf::Running}; - } - } - } - - // clear the state - state &= ~Bit1; - return {modm::rf::Stop}; - } -%% endif } void SpiMaster{{ id }}::finishTransfer() { if (Hal::isTransferEnabled()) { - while (!(Hal::status() & Hal::StatusFlag::TxTransferComplete)); + modm::this_fiber::poll([&]{ return Hal::status() & Hal::StatusFlag::TxTransferComplete; }); Hal::disableTransfer(); } diff --git a/src/modm/platform/spi/stm32h7/spi_master.hpp.in b/src/modm/platform/spi/stm32h7/spi_master.hpp.in index f44a0883a9..75f1f17962 100644 --- a/src/modm/platform/spi/stm32h7/spi_master.hpp.in +++ b/src/modm/platform/spi/stm32h7/spi_master.hpp.in @@ -22,9 +22,7 @@ #include #include #include -%% if use_fiber #include -%% endif #include "spi_hal_{{ id }}.hpp" namespace modm::platform @@ -37,11 +35,6 @@ namespace modm::platform */ class SpiMaster{{ id }} : public modm::SpiMaster, public SpiLock { -%% if not use_fiber - // Bit0: single transfer state - // Bit1: block transfer state - static inline uint8_t state{0}; -%% endif public: using Hal = SpiHal{{ id }}; @@ -73,9 +66,6 @@ public: // translate the prescaler into the bitmapping constexpr Hal::Prescaler prescaler{result.index << SPI_CFG1_MBR_Pos}; -%% if not use_fiber - state = 0; -%% endif Hal::initialize(prescaler); Hal::enableTransfer(); @@ -109,23 +99,25 @@ public: Hal::startMasterTransfer(); } + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static uint8_t transferBlocking(uint8_t data) { - return RF_CALL_BLOCKING(transfer(data)); + return transfer(data); } + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static void transferBlocking(const uint8_t* tx, uint8_t* rx, std::size_t length) { - RF_CALL_BLOCKING(transfer(tx, rx, length)); + transfer(tx, rx, length); } - static modm::ResumableResult + static uint8_t transfer(uint8_t data); - static modm::ResumableResult + static void transfer(const uint8_t* tx, uint8_t* rx, std::size_t length); private: diff --git a/src/modm/platform/spi/stm32h7/spi_master_dma.hpp.in b/src/modm/platform/spi/stm32h7/spi_master_dma.hpp.in index 992a666a1e..ad727214ac 100644 --- a/src/modm/platform/spi/stm32h7/spi_master_dma.hpp.in +++ b/src/modm/platform/spi/stm32h7/spi_master_dma.hpp.in @@ -45,12 +45,6 @@ protected: Peripheral::Spi{{ id }}, DmaChannelTx::Signal::Tx>::Request; }; -%% if not use_fiber - // Bit0: single transfer state - // Bit1: block transfer state - // Bit2: block transfer rx dma transfer completed - static inline uint8_t state{0}; -%% endif public: using Hal = SpiHal{{ id }}; @@ -95,24 +89,26 @@ public: Hal::setDataSize(static_cast(size)); } + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static uint8_t transferBlocking(uint8_t data) { - return RF_CALL_BLOCKING(transfer(data)); + return transfer(data); } /// @pre At least one of tx or rx must not be nullptr + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static void transferBlocking(const uint8_t* tx, uint8_t* rx, std::size_t length) { - RF_CALL_BLOCKING(transfer(tx, rx, length)); + transfer(tx, rx, length); } - static modm::ResumableResult + static uint8_t transfer(uint8_t data); /// @pre At least one of tx or rx must not be nullptr - static modm::ResumableResult + static void transfer(const uint8_t* tx, uint8_t* rx, std::size_t length); protected: diff --git a/src/modm/platform/spi/stm32h7/spi_master_dma_impl.hpp.in b/src/modm/platform/spi/stm32h7/spi_master_dma_impl.hpp.in index 92039310b4..fa59b0dae5 100644 --- a/src/modm/platform/spi/stm32h7/spi_master_dma_impl.hpp.in +++ b/src/modm/platform/spi/stm32h7/spi_master_dma_impl.hpp.in @@ -41,10 +41,6 @@ SpiMaster{{ id }}_Dma::initialize() Tx::setPeripheralAddress(reinterpret_cast(Hal::transmitRegister())); Tx::template setPeripheralRequest(); -%% if not use_fiber - state = 0; -%% endif - constexpr auto result = modm::Prescaler::from_power(SystemClock::Spi{{ id }}, baudrate, 2, 256); assertBaudrateInTolerance< result.frequency, baudrate, tolerance >(); @@ -53,11 +49,10 @@ SpiMaster{{ id }}_Dma::initialize() } template -modm::ResumableResult +uint8_t SpiMaster{{ id }}_Dma::transfer(uint8_t data) { // DMA is not used for single byte transfers -%% if use_fiber Hal::setDuplexMode(Hal::DuplexMode::FullDuplex); Hal::setTransferSize(1); Hal::enableTransfer(); @@ -72,35 +67,6 @@ SpiMaster{{ id }}_Dma::transfer(uint8_t data) finishTransfer(); return data; -%% else - // this is a manually implemented "fast resumable function" - // there is no context or nesting protection, since we don't need it. - // there are only two states encoded into 1 bit (LSB of state): - // 1. waiting to start, and - // 2. waiting to finish. - // LSB != Bit0 ? - if ( !(state & Bit0) ) - { - Hal::setDuplexMode(Hal::DuplexMode::FullDuplex); - Hal::setTransferSize(1); - Hal::enableTransfer(); - Hal::write(data); - Hal::startMasterTransfer(); - - // set LSB = Bit0 - state |= Bit0; - } - - if (!Hal::isTransferCompleted()) - return {modm::rf::Running}; - - data = SpiHal{{ id }}::read(); - finishTransfer(); - - // transfer finished - state &= ~Bit0; - return {modm::rf::Stop, data}; -%% endif } template @@ -149,12 +115,11 @@ SpiMaster{{ id }}_Dma::startDmaTransfer( template -modm::ResumableResult +void SpiMaster{{ id }}_Dma::transfer( const uint8_t* tx, uint8_t* rx, std::size_t length) { using Flags = DmaChannelRx::InterruptFlags; -%% if use_fiber startDmaTransfer(tx, rx, length); bool dmaRxFinished = (rx == nullptr); @@ -177,58 +142,6 @@ SpiMaster{{ id }}_Dma::transfer( modm::this_fiber::yield(); } finishTransfer(); -%% else - // this is a manually implemented "fast resumable function" - // there is no context or nesting protection, since we don't need it. - // there are only two states encoded into 1 bit (Bit1 of state): - // 1. initialize index, and - // 2. wait for 1-byte transfer to finish. - - // we are only interested in Bit1 - switch(state & Bit1) - { - case 0: - // we will only visit this state once - state |= Bit1; - startDmaTransfer(tx, rx, length); - if (!rx) { - state |= Bit2; - } - [[fallthrough]]; - - default: - if (!Hal::isTransferCompleted() or !(state & Bit2)) { - if(rx) { - static typename DmaChannelRx::InterruptFlags_t flags; - flags = DmaChannelRx::getInterruptFlags(); - if (flags & Flags::Error) { - // abort on DMA error - finishTransfer(); - state &= ~(Bit2 | Bit1); - return {modm::rf::Stop}; - } - if (flags & Flags::TransferComplete) { - state |= Bit2; - } - } - if(tx) { - if (DmaChannelTx::getInterruptFlags() & Flags::Error) { - // abort on DMA error - finishTransfer(); - state &= ~(Bit2 | Bit1); - return {modm::rf::Stop}; - } - } - return { modm::rf::Running }; - } - - finishTransfer(); - - // clear the state - state &= ~(Bit2 | Bit1); - return {modm::rf::Stop}; - } -%% endif } template diff --git a/src/modm/ui/display/graphic_display.hpp b/src/modm/ui/display/graphic_display.hpp index 4faf23aadc..c2cc521e53 100644 --- a/src/modm/ui/display/graphic_display.hpp +++ b/src/modm/ui/display/graphic_display.hpp @@ -74,8 +74,7 @@ class GraphicDisplay : public IOStream virtual uint16_t getHeight() const = 0; - // TODO Requires all inherited drivers work with resumable functions - // virtual modm::ResumableResult + // virtual bool // setOrientation() = 0; /** @@ -142,8 +141,7 @@ class GraphicDisplay : public IOStream virtual void update() = 0; - // TODO Requires all inherited drivers work with resumable functions - // modm::ResumableResult + // bool // writeDisplay(); // TODO Set a clipping area diff --git a/test/config/lbuild.xml b/test/config/lbuild.xml index d1699f8ee5..45c64ae326 100644 --- a/test/config/lbuild.xml +++ b/test/config/lbuild.xml @@ -12,4 +12,8 @@ modm:build:scons + + MODM_CHRONO_MILLI_CLOCK_NOW=modm_platform_milli_now + MODM_CHRONO_MICRO_CLOCK_NOW=modm_platform_micro_now + diff --git a/test/modm/architecture/driver/clock_test.cpp b/test/modm/architecture/driver/clock_test.cpp index 8014553efa..f981f7ef0b 100644 --- a/test/modm/architecture/driver/clock_test.cpp +++ b/test/modm/architecture/driver/clock_test.cpp @@ -17,6 +17,18 @@ #include using test_clock = modm_test::chrono::milli_clock; +void +ClockTest::setUp() +{ + test_clock::enable(); +} + +void +ClockTest::tearDown() +{ + test_clock::disable(); +} + void ClockTest::testClock() { diff --git a/test/modm/architecture/driver/clock_test.hpp b/test/modm/architecture/driver/clock_test.hpp index 38711ef708..18fd906ba8 100644 --- a/test/modm/architecture/driver/clock_test.hpp +++ b/test/modm/architecture/driver/clock_test.hpp @@ -24,6 +24,12 @@ class ClockTest : public unittest::TestSuite { public: + void + setUp() override; + + void + tearDown() override; + void testClock(); }; diff --git a/test/modm/communication/amnb/interface_test.cpp b/test/modm/communication/amnb/interface_test.cpp index 3f291f1e96..71862eeda8 100644 --- a/test/modm/communication/amnb/interface_test.cpp +++ b/test/modm/communication/amnb/interface_test.cpp @@ -45,7 +45,7 @@ AmnbInterfaceTest::testSerialize() // Test inplace message tx AmnbTestMessage msg(7, 0x7D); msg.setValid(); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&msg)), InterfaceStatus::Ok); + TEST_ASSERT_EQUALS(interface.transmit(&msg), InterfaceStatus::Ok); // with byte stuffing! const uint8_t raw[] = {0x7E, 0x7E, 108, 7, 0x7D, 0x5D, 0}; TEST_ASSERT_EQUALS_ARRAY(SharedMedium::transmitted, raw, sizeof(raw)); @@ -57,7 +57,7 @@ AmnbInterfaceTest::testSerialize() msg.get()[0] = 0x03020100ul; msg.get()[1] = 0x07067E7Dul; msg.setValid(); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&msg)), InterfaceStatus::Ok); + TEST_ASSERT_EQUALS(interface.transmit(&msg), InterfaceStatus::Ok); // with byte stuffing! const uint8_t raw[] = {0x7E, 0x7E, 18, 200, 0x7D, 0x5E, 0x48, 0, 1, 2, 3, 0x7D, 0x5D, 0x7D, 0x5E, 6, 7}; TEST_ASSERT_EQUALS_ARRAY(SharedMedium::transmitted, raw, sizeof(raw)); @@ -69,7 +69,7 @@ AmnbInterfaceTest::testSerialize() for (size_t ii=0; ii < 32/sizeof(uint32_t); ii++) msg.get()[ii] = 0x0302017Dul+ii; msg.setValid(); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&msg)), InterfaceStatus::Ok); + TEST_ASSERT_EQUALS(interface.transmit(&msg), InterfaceStatus::Ok); // with byte stuffing! const uint8_t raw[] = {0x7E, 0x7E, 8, 10, 14, 0x9F, 32, 0, 205, 202, 0x7D, 0x5D, 1, 2, 3, 0x7D, 0x5E, 1, 2, 3, 0x7F, 1, 2, 3, 0x80, 1, 2, 3, @@ -82,7 +82,7 @@ AmnbInterfaceTest::testSerialize() // Test inplace message rx SharedMedium::add_rx({0x7E, 0x7E, 247, 0x7D, 0x5D, 100, 0}); AmnbTestMessage msg; - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.receiveHeader(&msg)), InterfaceStatus::Ok); + TEST_ASSERT_EQUALS(interface.receiveHeader(&msg), InterfaceStatus::Ok); TEST_ASSERT_TRUE(msg.isHeaderValid()); TEST_ASSERT_TRUE(msg.isDataValid()); @@ -97,7 +97,7 @@ AmnbInterfaceTest::testSerialize() // Test inplace message rx SharedMedium::add_rx({0x7E, 0x7E, 56, 5, 15, 0x44, 0x7D, 0x5E, 1, 2, 3}); AmnbTestMessage msg; - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.receiveHeader(&msg)), InterfaceStatus::Ok); + TEST_ASSERT_EQUALS(interface.receiveHeader(&msg), InterfaceStatus::Ok); TEST_ASSERT_TRUE(msg.isHeaderValid()); TEST_ASSERT_TRUE(msg.isDataValid()); @@ -114,7 +114,7 @@ AmnbInterfaceTest::testSerialize() 5, 2, 3, 4, 6, 2, 3, 4, 7, 2, 3, 4, 8, 2, 3, 4, 9, 2, 3, 4}); AmnbTestMessage msg; - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.receiveHeader(&msg)), InterfaceStatus::Ok); + TEST_ASSERT_EQUALS(interface.receiveHeader(&msg), InterfaceStatus::Ok); TEST_ASSERT_TRUE(msg.isHeaderValid()); TEST_ASSERT_EQUALS(msg.address(), 20); @@ -122,7 +122,7 @@ AmnbInterfaceTest::testSerialize() TEST_ASSERT_EQUALS(msg.length(), 36); TEST_ASSERT_EQUALS(msg.type(), Type::Request); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.receiveData(&msg)), InterfaceStatus::Ok); + TEST_ASSERT_EQUALS(interface.receiveData(&msg), InterfaceStatus::Ok); TEST_ASSERT_TRUE(msg.isDataValid()); const uint32_t raw[] = {0x04030201ul, 0x04030202ul, 0x04030203ul, 0x04030204ul, @@ -142,29 +142,29 @@ AmnbInterfaceTest::testFailures() { SharedMedium::fail_tx_index = 0; - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&msg)), InterfaceStatus::SyncWriteFailed); + TEST_ASSERT_EQUALS(interface.transmit(&msg), InterfaceStatus::SyncWriteFailed); const uint8_t raw[] = {0x7E}; TEST_ASSERT_EQUALS(SharedMedium::raw_transmitted.size(), sizeof(raw)); TEST_ASSERT_EQUALS_ARRAY(SharedMedium::raw_transmitted, raw, sizeof(raw)); }{ SharedMedium::reset(); SharedMedium::fail_tx_index = 1; - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&msg)), InterfaceStatus::SyncWriteFailed); + TEST_ASSERT_EQUALS(interface.transmit(&msg), InterfaceStatus::SyncWriteFailed); const uint8_t raw[] = {0x7E, 0x7E}; TEST_ASSERT_EQUALS(SharedMedium::raw_transmitted.size(), sizeof(raw)); TEST_ASSERT_EQUALS_ARRAY(SharedMedium::raw_transmitted, raw, sizeof(raw)); }{ SharedMedium::reset(); SharedMedium::fail_tx_index = 2; - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&msg)), InterfaceStatus::HeaderWriteFailed); + TEST_ASSERT_EQUALS(interface.transmit(&msg), InterfaceStatus::HeaderWriteFailed); const uint8_t raw[] = {0x7E, 0x7E, 110}; TEST_ASSERT_EQUALS(SharedMedium::raw_transmitted.size(), sizeof(raw)); TEST_ASSERT_EQUALS_ARRAY(SharedMedium::raw_transmitted, raw, sizeof(raw)); }{ SharedMedium::reset(); SharedMedium::fail_tx_index = 4; - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&msg)), InterfaceStatus::HeaderWriteFailed); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&msg)), InterfaceStatus::Ok); + TEST_ASSERT_EQUALS(interface.transmit(&msg), InterfaceStatus::HeaderWriteFailed); + TEST_ASSERT_EQUALS(interface.transmit(&msg), InterfaceStatus::Ok); const uint8_t raw[] = {0x7E, 0x7E, 110,0,0,0x7E, 0x7E, 110,0,0,0}; TEST_ASSERT_EQUALS(SharedMedium::raw_transmitted.size(), sizeof(raw)); TEST_ASSERT_EQUALS_ARRAY(SharedMedium::raw_transmitted, raw, sizeof(raw)); @@ -176,51 +176,51 @@ AmnbInterfaceTest::testFailures() // wrong synchronization SharedMedium::reset(); SharedMedium::add_rx({0x00}); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.receiveHeader(&msg)), InterfaceStatus::SyncReadFailed); + TEST_ASSERT_EQUALS(interface.receiveHeader(&msg), InterfaceStatus::SyncReadFailed); // second sync byte wrong SharedMedium::add_rx({0x7E, 0x00}); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.receiveHeader(&msg)), InterfaceStatus::SyncReadFailed); + TEST_ASSERT_EQUALS(interface.receiveHeader(&msg), InterfaceStatus::SyncReadFailed); // sync byte read error SharedMedium::fail_rx_index = 1; // sync read error SharedMedium::add_rx({0x7E, 0x7E}); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.receiveHeader(&msg)), InterfaceStatus::SyncReadFailed); + TEST_ASSERT_EQUALS(interface.receiveHeader(&msg), InterfaceStatus::SyncReadFailed); SharedMedium::reset(); SharedMedium::fail_rx_index = 2; // header read error SharedMedium::add_rx({0x7E, 0x7E, 0x00}); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.receiveHeader(&msg)), InterfaceStatus::HeaderReadFailed); + TEST_ASSERT_EQUALS(interface.receiveHeader(&msg), InterfaceStatus::HeaderReadFailed); // CRC is wrong for no data SharedMedium::add_rx({0x7E, 0x7E, 0x00, 0x00, 0x00, 0x00}); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.receiveHeader(&msg)), InterfaceStatus::HeaderInvalid); + TEST_ASSERT_EQUALS(interface.receiveHeader(&msg), InterfaceStatus::HeaderInvalid); SharedMedium::reset(); SharedMedium::fail_rx_index = 6; // short data read error SharedMedium::add_rx({0x7E, 0x7E, 0x00, 0x00, 0x00, 0x04, 0x01, 0x02, 0x03, 0x04}); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.receiveHeader(&msg)), InterfaceStatus::HeaderReadFailed); + TEST_ASSERT_EQUALS(interface.receiveHeader(&msg), InterfaceStatus::HeaderReadFailed); // wrong CRC for short data SharedMedium::add_rx({0x7E, 0x7E, 0x00, 0x00, 0x00, 0x04, 0x01, 0x02, 0x03, 0x04}); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.receiveHeader(&msg)), InterfaceStatus::HeaderInvalid); + TEST_ASSERT_EQUALS(interface.receiveHeader(&msg), InterfaceStatus::HeaderInvalid); SharedMedium::reset(); SharedMedium::fail_rx_index = 11; // long data with read error SharedMedium::add_rx({0x7E, 0x7E, 8, 10, 14, 0x9F, 32, 0, 205, 202, 1, 2}); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.receiveHeader(&msg)), InterfaceStatus::Ok); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.receiveData(&msg)), InterfaceStatus::DataReadFailed); + TEST_ASSERT_EQUALS(interface.receiveHeader(&msg), InterfaceStatus::Ok); + TEST_ASSERT_EQUALS(interface.receiveData(&msg), InterfaceStatus::DataReadFailed); // allocation failed, received message too large SharedMedium::add_rx({0x7E, 0x7E, 82, 20, 54, 0x5F, 36, 0, 143, 101, 1, 2, 3, 4, 2, 2, 3, 4, 3, 2, 3, 4, 4, 2, 3, 4, 5, 2, 3, 4, 6, 2, 3, 4, 7, 2, 3, 4, 8, 2, 3, 4, 9, 2, 3, 4}); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.receiveHeader(&msg)), InterfaceStatus::Ok); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.receiveData(&msg)), InterfaceStatus::AllocationFailed); + TEST_ASSERT_EQUALS(interface.receiveHeader(&msg), InterfaceStatus::Ok); + TEST_ASSERT_EQUALS(interface.receiveData(&msg), InterfaceStatus::AllocationFailed); // Data CRC is wrong SharedMedium::add_rx({0x7E, 0x7E, 8, 10, 14, 0x9F, 32, 0, 205, 202, 1, 2, 3, 4, 2, 2, 3, 4, 3, 2, 3, 4, 4, 2, 3, 4, 5, 2, 3, 4, 6, 2, 3, 4, 7, 2, 3, 4, 8, 2, 3, 4}); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.receiveHeader(&msg)), InterfaceStatus::Ok); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.receiveData(&msg)), InterfaceStatus::DataInvalid); + TEST_ASSERT_EQUALS(interface.receiveHeader(&msg), InterfaceStatus::Ok); + TEST_ASSERT_EQUALS(interface.receiveData(&msg), InterfaceStatus::DataInvalid); } } @@ -232,68 +232,68 @@ AmnbInterfaceTest::testInterlock() AmnbTestMessage tx_msg; AmnbTestMessage rx_msg; - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.receiveHeader(&rx_msg)), InterfaceStatus::MediumEmpty); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&tx_msg)), InterfaceStatus::Ok); + TEST_ASSERT_EQUALS(interface.receiveHeader(&rx_msg), InterfaceStatus::MediumEmpty); + TEST_ASSERT_EQUALS(interface.transmit(&tx_msg), InterfaceStatus::Ok); SharedMedium::reset(); SharedMedium::add_rx({0x7E}); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&tx_msg)), InterfaceStatus::MediumBusy); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&tx_msg)), InterfaceStatus::MediumBusy); + TEST_ASSERT_EQUALS(interface.transmit(&tx_msg), InterfaceStatus::MediumBusy); + TEST_ASSERT_EQUALS(interface.transmit(&tx_msg), InterfaceStatus::MediumBusy); interface.receiveHeader(&rx_msg); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&tx_msg)), InterfaceStatus::MediumBusy); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&tx_msg)), InterfaceStatus::MediumBusy); + TEST_ASSERT_EQUALS(interface.transmit(&tx_msg), InterfaceStatus::MediumBusy); + TEST_ASSERT_EQUALS(interface.transmit(&tx_msg), InterfaceStatus::MediumBusy); SharedMedium::add_rx({0x00}); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.receiveHeader(&rx_msg)), InterfaceStatus::SyncReadFailed); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&tx_msg)), InterfaceStatus::Ok); + TEST_ASSERT_EQUALS(interface.receiveHeader(&rx_msg), InterfaceStatus::SyncReadFailed); + TEST_ASSERT_EQUALS(interface.transmit(&tx_msg), InterfaceStatus::Ok); // same but with read error SharedMedium::reset(); SharedMedium::fail_rx_index = 1; SharedMedium::add_rx({0x7E}); interface.receiveHeader(&rx_msg); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&tx_msg)), InterfaceStatus::MediumBusy); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&tx_msg)), InterfaceStatus::MediumBusy); + TEST_ASSERT_EQUALS(interface.transmit(&tx_msg), InterfaceStatus::MediumBusy); + TEST_ASSERT_EQUALS(interface.transmit(&tx_msg), InterfaceStatus::MediumBusy); SharedMedium::add_rx({0x7E}); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.receiveHeader(&rx_msg)), InterfaceStatus::SyncReadFailed); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&tx_msg)), InterfaceStatus::Ok); + TEST_ASSERT_EQUALS(interface.receiveHeader(&rx_msg), InterfaceStatus::SyncReadFailed); + TEST_ASSERT_EQUALS(interface.transmit(&tx_msg), InterfaceStatus::Ok); // same but with read error later SharedMedium::add_rx({0x7E, 0x7E}); interface.receiveHeader(&rx_msg); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&tx_msg)), InterfaceStatus::MediumBusy); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&tx_msg)), InterfaceStatus::MediumBusy); + TEST_ASSERT_EQUALS(interface.transmit(&tx_msg), InterfaceStatus::MediumBusy); + TEST_ASSERT_EQUALS(interface.transmit(&tx_msg), InterfaceStatus::MediumBusy); SharedMedium::add_rx({207, 7, 4}); interface.receiveHeader(&rx_msg); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&tx_msg)), InterfaceStatus::MediumBusy); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&tx_msg)), InterfaceStatus::MediumBusy); + TEST_ASSERT_EQUALS(interface.transmit(&tx_msg), InterfaceStatus::MediumBusy); + TEST_ASSERT_EQUALS(interface.transmit(&tx_msg), InterfaceStatus::MediumBusy); SharedMedium::add_rx({0}); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.receiveHeader(&rx_msg)), InterfaceStatus::Ok); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&tx_msg)), InterfaceStatus::Ok); + TEST_ASSERT_EQUALS(interface.receiveHeader(&rx_msg), InterfaceStatus::Ok); + TEST_ASSERT_EQUALS(interface.transmit(&tx_msg), InterfaceStatus::Ok); // same but with read error later SharedMedium::reset(); SharedMedium::add_rx({0x7E, 0x7E}); SharedMedium::fail_rx_index = 3; interface.receiveHeader(&rx_msg); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&tx_msg)), InterfaceStatus::MediumBusy); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&tx_msg)), InterfaceStatus::MediumBusy); + TEST_ASSERT_EQUALS(interface.transmit(&tx_msg), InterfaceStatus::MediumBusy); + TEST_ASSERT_EQUALS(interface.transmit(&tx_msg), InterfaceStatus::MediumBusy); SharedMedium::add_rx({207, 7, 4}); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.receiveHeader(&rx_msg)), InterfaceStatus::HeaderReadFailed); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&tx_msg)), InterfaceStatus::Ok); + TEST_ASSERT_EQUALS(interface.receiveHeader(&rx_msg), InterfaceStatus::HeaderReadFailed); + TEST_ASSERT_EQUALS(interface.transmit(&tx_msg), InterfaceStatus::Ok); // header invalid SharedMedium::reset(); SharedMedium::add_rx({0x7E, 0x7E}); interface.receiveHeader(&rx_msg); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&tx_msg)), InterfaceStatus::MediumBusy); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&tx_msg)), InterfaceStatus::MediumBusy); + TEST_ASSERT_EQUALS(interface.transmit(&tx_msg), InterfaceStatus::MediumBusy); + TEST_ASSERT_EQUALS(interface.transmit(&tx_msg), InterfaceStatus::MediumBusy); SharedMedium::add_rx({207, 7, 5, 0}); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.receiveHeader(&rx_msg)), InterfaceStatus::HeaderInvalid); - TEST_ASSERT_EQUALS(RF_CALL_BLOCKING(interface.transmit(&tx_msg)), InterfaceStatus::Ok); + TEST_ASSERT_EQUALS(interface.receiveHeader(&rx_msg), InterfaceStatus::HeaderInvalid); + TEST_ASSERT_EQUALS(interface.transmit(&tx_msg), InterfaceStatus::Ok); } diff --git a/test/modm/communication/xpcc/dispatcher_test.cpp b/test/modm/communication/xpcc/dispatcher_test.cpp index de6122402b..3d7daeca6b 100644 --- a/test/modm/communication/xpcc/dispatcher_test.cpp +++ b/test/modm/communication/xpcc/dispatcher_test.cpp @@ -21,6 +21,7 @@ using test_clock = modm_test::chrono::milli_clock; void DispatcherTest::setUp() { + test_clock::enable(); backend = new FakeBackend(); postman = new FakePostman(); dispatcher = new xpcc::Dispatcher(backend, postman); @@ -36,6 +37,7 @@ DispatcherTest::setUp() void DispatcherTest::tearDown() { + test_clock::disable(); delete component2; delete component1; delete timeline; diff --git a/test/modm/driver/adc/ad7280a_test.cpp b/test/modm/driver/adc/ad7280a_test.cpp index 82a4b94bb6..c372cf152b 100644 --- a/test/modm/driver/adc/ad7280a_test.cpp +++ b/test/modm/driver/adc/ad7280a_test.cpp @@ -15,21 +15,23 @@ #include #include +#include #include "ad7280a_test.hpp" // ---------------------------------------------------------------------------- #define ENABLE_MACRO_EXPORT #include +#include #undef ENABLE_MACRO_EXPORT modm_test::SpiDevice device; -class DummySpi +class DummySpi : public modm_test::platform::SpiMaster { public: static uint8_t - write(uint8_t data) + transfer(uint8_t data) { return device.write(data); } @@ -55,19 +57,19 @@ struct Cs } }; -typedef modm::Ad7280a Ad7280a; +modm::Ad7280a ad7280a; // ---------------------------------------------------------------------------- void Ad7280aTest::testCrcByte() { - TEST_ASSERT_EQUALS(Ad7280a::updateCrc(0x00), 0); - TEST_ASSERT_EQUALS(Ad7280a::updateCrc(0x10), 174); - TEST_ASSERT_EQUALS(Ad7280a::updateCrc(0x20), 115); - TEST_ASSERT_EQUALS(Ad7280a::updateCrc(0x51), 103); - TEST_ASSERT_EQUALS(Ad7280a::updateCrc(0xAB), 182); - TEST_ASSERT_EQUALS(Ad7280a::updateCrc(0xEF), 236); - TEST_ASSERT_EQUALS(Ad7280a::updateCrc(0xFF), 66); + TEST_ASSERT_EQUALS(ad7280a.updateCrc(0x00), 0); + TEST_ASSERT_EQUALS(ad7280a.updateCrc(0x10), 174); + TEST_ASSERT_EQUALS(ad7280a.updateCrc(0x20), 115); + TEST_ASSERT_EQUALS(ad7280a.updateCrc(0x51), 103); + TEST_ASSERT_EQUALS(ad7280a.updateCrc(0xAB), 182); + TEST_ASSERT_EQUALS(ad7280a.updateCrc(0xEF), 236); + TEST_ASSERT_EQUALS(ad7280a.updateCrc(0xFF), 66); } // ---------------------------------------------------------------------------- @@ -75,16 +77,16 @@ void Ad7280aTest::testCrcMessage() { // Datasheet Example 1 - TEST_ASSERT_EQUALS(Ad7280a::calculateCrc(0x003430), 0x51); + TEST_ASSERT_EQUALS(ad7280a.calculateCrc(0x003430), 0x51); // Datasheet Example 2 - TEST_ASSERT_EQUALS(Ad7280a::calculateCrc(0x103430), 0x74); + TEST_ASSERT_EQUALS(ad7280a.calculateCrc(0x103430), 0x74); // Datasheet Example 3 - TEST_ASSERT_EQUALS(Ad7280a::calculateCrc(0x0070A1), 0x9A); + TEST_ASSERT_EQUALS(ad7280a.calculateCrc(0x0070A1), 0x9A); // Datasheet Example 4 - TEST_ASSERT_EQUALS(Ad7280a::calculateCrc(0x205335), 0x46); + TEST_ASSERT_EQUALS(ad7280a.calculateCrc(0x205335), 0x46); } // ---------------------------------------------------------------------------- @@ -112,7 +114,7 @@ Ad7280aTest::testChainSetup() device.start(transmissionsInitialize, ARRAY_SIZE(transmissionsInitialize), __LINE__); - TEST_ASSERT_TRUE(Ad7280a::chainSetup()); + TEST_ASSERT_TRUE(ad7280a.chainSetup()); device.finish(); } @@ -144,7 +146,7 @@ Ad7280aTest::testSelftest() device.start(transmissionsInitialize, ARRAY_SIZE(transmissionsInitialize), __LINE__); - TEST_ASSERT_TRUE(Ad7280a::performSelftest()); + TEST_ASSERT_TRUE(ad7280a.performSelftest()); device.finish(); } @@ -166,7 +168,7 @@ Ad7280aTest::testSoftwareReset() device.start(transmissionsInitialize, ARRAY_SIZE(transmissionsInitialize), __LINE__); - Ad7280a::softwareReset(); + ad7280a.softwareReset(); device.finish(); } @@ -198,7 +200,7 @@ Ad7280aTest::testChannelRead() device.start(transmissionsInitialize, ARRAY_SIZE(transmissionsInitialize), __LINE__); uint16_t value = 0; - TEST_ASSERT_TRUE(Ad7280a::readChannel(0, modm::ad7280a::CELL_VOLTAGE_4, &value)); + TEST_ASSERT_TRUE(ad7280a.readChannel(0, modm::ad7280a::CELL_VOLTAGE_4, &value)); TEST_ASSERT_EQUALS(value, 549u); @@ -248,7 +250,7 @@ Ad7280aTest::testAllChannelRead() device.start(transmissionsInitialize, ARRAY_SIZE(transmissionsInitialize), __LINE__); uint16_t values[6]; - TEST_ASSERT_TRUE(Ad7280a::readAllChannels(values)); + TEST_ASSERT_TRUE(ad7280a.readAllChannels(values)); TEST_ASSERT_EQUALS(values[0], 100u); TEST_ASSERT_EQUALS(values[1], 200u); @@ -273,7 +275,7 @@ Ad7280aTest::testBalancer() device.start(transmissionsInitialize, ARRAY_SIZE(transmissionsInitialize), __LINE__); - Ad7280a::enableBalancer(0, modm::ad7280a::CB1 | modm::ad7280a::CB2); + ad7280a.enableBalancer(0, modm::ad7280a::CB1 | modm::ad7280a::CB2); device.finish(); } diff --git a/test/modm/driver/motor/drv832x_spi_test.cpp b/test/modm/driver/motor/drv832x_spi_test.cpp index f795593306..3c1d508ac3 100644 --- a/test/modm/driver/motor/drv832x_spi_test.cpp +++ b/test/modm/driver/motor/drv832x_spi_test.cpp @@ -69,7 +69,7 @@ Drv832xSpiTest::testSpi() modm::Drv832xSpi gateDriver; // Real DRV832x device has default register values, but we expect all flags zero because SpiMasterMock reads zero per default - RF_CALL_BLOCKING(gateDriver.initialize()); + gateDriver.initialize(); // initialize reads 14 bytes, but we don't care SpiMaster::clearBuffers(); @@ -77,7 +77,7 @@ Drv832xSpiTest::testSpi() gateDriver.driverControl() |= modm::drv832xSpi::PwmMode_t(modm::drv832xSpi::PwmMode::PwmModeIndependent); // Spi transfer should not occure before commit() TEST_ASSERT_EQUALS(SpiMaster::getTxBufferLength(), 0u); - RF_CALL_BLOCKING(gateDriver.commit()); + gateDriver.commit(); TEST_ASSERT_EQUALS(SpiMaster::getTxBufferLength(), 2u); uint8_t txBuffer[2]; SpiMaster::popTxBuffer(txBuffer); @@ -86,14 +86,14 @@ Drv832xSpiTest::testSpi() // Test csa control register write gateDriver.csaControl() |= modm::drv832xSpi::CsaControl::DisableOvercurrentSense | modm::drv832xSpi::CsaControl::VrefDiv2; - RF_CALL_BLOCKING(gateDriver.commit()); + gateDriver.commit(); TEST_ASSERT_EQUALS(SpiMaster::getTxBufferLength(), 2u); SpiMaster::popTxBuffer(txBuffer); uint8_t txBufferCompare2[] = {(0x6 << 3 | 0b1 << 1), (0b1 << 5)}; TEST_ASSERT_EQUALS_ARRAY(txBuffer, txBufferCompare2, 2u); // Test fault status register read - RF_CALL_BLOCKING(gateDriver.readFaultStatus1()); + gateDriver.readFaultStatus1(); TEST_ASSERT_EQUALS(SpiMaster::getTxBufferLength(), 2u); SpiMaster::popTxBuffer(txBuffer); uint8_t txBufferCompare3[] = {(0b1 << 7 | 0x0 << 3), 0x00}; diff --git a/test/modm/driver/temperature/ltc2984_test.cpp b/test/modm/driver/temperature/ltc2984_test.cpp index d092e32056..bf10da08fa 100644 --- a/test/modm/driver/temperature/ltc2984_test.cpp +++ b/test/modm/driver/temperature/ltc2984_test.cpp @@ -112,7 +112,7 @@ Ltc2984Test::testSpi() uint32_t channelConfigurationTest = (0b11101ul << 27) | (2000ul*1024); uint32_t channelConfiguration = modm::ltc2984::Configuration::rsense(modm::ltc2984::Configuration::Rsense::Resistance_t(2000ul*1024)); TEST_ASSERT_EQUALS(channelConfiguration, channelConfigurationTest); - RF_CALL_BLOCKING(tempSensor.configureChannel(modm::ltc2984::Channel::Ch2, channelConfiguration)); + tempSensor.configureChannel(modm::ltc2984::Channel::Ch2, channelConfiguration); constexpr std::size_t txBufferLength = 7; uint8_t txBuffer[txBufferLength]; @@ -125,7 +125,7 @@ Ltc2984Test::testSpi() channelConfigurationTest = 0x00000000; channelConfiguration = modm::ltc2984::Configuration::disabled(); TEST_ASSERT_EQUALS(channelConfiguration, channelConfigurationTest); - RF_CALL_BLOCKING(tempSensor.configureChannel(modm::ltc2984::Channel::Ch2, channelConfiguration)); + tempSensor.configureChannel(modm::ltc2984::Channel::Ch2, channelConfiguration); uint8_t txBufferCompare2[] = {0x02, 0x02, 0x04, 0, 0, 0, 0}; TEST_ASSERT_EQUALS(SpiMaster::getTxBufferLength(), 7u); @@ -142,7 +142,7 @@ Ltc2984Test::testSpi() modm::ltc2984::Configuration::Rtd::RtdCurve::European ); TEST_ASSERT_EQUALS(channelConfiguration, channelConfigurationTest); - RF_CALL_BLOCKING(tempSensor.configureChannel(modm::ltc2984::Channel::Ch4, channelConfiguration)); + tempSensor.configureChannel(modm::ltc2984::Channel::Ch4, channelConfiguration); uint8_t txBufferCompare3[] = {0x02, 0x02, 0x0C, 0x60, 0xA9, 0xC0, 0x00}; TEST_ASSERT_EQUALS(SpiMaster::getTxBufferLength(), 7u); @@ -150,13 +150,13 @@ Ltc2984Test::testSpi() TEST_ASSERT_EQUALS_ARRAY(txBuffer, txBufferCompare3, 7u); tempSensor.enableChannel(modm::ltc2984::Configuration::MuxChannel::Ch4); - RF_CALL_BLOCKING(tempSensor.setChannels()); + tempSensor.setChannels(); uint8_t txBufferCompare4[] = {0x02, 0x00, 0xF4, 0x00, 0x00, 0x00, 0x08}; TEST_ASSERT_EQUALS(SpiMaster::getTxBufferLength(), 7u); SpiMaster::popTxBuffer(txBuffer); TEST_ASSERT_EQUALS_ARRAY(txBuffer, txBufferCompare4, 7u); - RF_CALL_BLOCKING(tempSensor.initiateSingleMeasurement(modm::ltc2984::Channel::Ch4)); + tempSensor.initiateSingleMeasurement(modm::ltc2984::Channel::Ch4); uint8_t txBufferCompare5[] = {0x02, 0x00, 0x00, 0x84}; TEST_ASSERT_EQUALS(SpiMaster::getTxBufferLength(), 4u); SpiMaster::popTxBuffer(txBuffer); @@ -165,7 +165,7 @@ Ltc2984Test::testSpi() modm::ltc2984::Data temperature; uint8_t rxBuffer[] = {0x00, 0x00, 0x00, 0x01, 0x00, 0x08, 0x00}; // valid temperature: 2.000 deg C SpiMaster::appendRxBuffer(rxBuffer, 7u); - RF_CALL_BLOCKING(tempSensor.readChannel(modm::ltc2984::Channel::Ch4, temperature)); + tempSensor.readChannel(modm::ltc2984::Channel::Ch4, temperature); uint8_t txBufferCompare6[] = {0x03, 0x00, 0x1C, 0x00, 0x00, 0x00, 0x00}; TEST_ASSERT_EQUALS(SpiMaster::getTxBufferLength(), 7u); SpiMaster::popTxBuffer(txBuffer); diff --git a/test/modm/mock/clock.cpp b/test/modm/mock/clock.cpp index c44dad1119..3dc8f08cbb 100644 --- a/test/modm/mock/clock.cpp +++ b/test/modm/mock/clock.cpp @@ -14,11 +14,16 @@ // ---------------------------------------------------------------------------- static uint32_t milli_time{0}; +static bool milli_enabled{false}; + +modm::chrono::milli_clock::time_point +modm_platform_milli_now() noexcept; modm::chrono::milli_clock::time_point modm::chrono::milli_clock::now() noexcept { - return time_point{duration{milli_time}}; + if (milli_enabled) return time_point{duration{milli_time}}; + return modm_platform_milli_now(); } void @@ -33,15 +38,31 @@ modm_test::chrono::milli_clock::increment(uint32_t milliseconds) milli_time += milliseconds; } +void +modm_test::chrono::milli_clock::enable() +{ + milli_enabled = true; +} + +void +modm_test::chrono::milli_clock::disable() +{ + milli_enabled = false; +} + // ---------------------------------------------------------------------------- static uint32_t micro_time{0}; +static bool micro_enabled{false}; + +modm::chrono::micro_clock::time_point +modm_platform_micro_now() noexcept; modm::chrono::micro_clock::time_point modm::chrono::micro_clock::now() noexcept { - return time_point{duration{micro_time}}; + if (micro_enabled) return time_point{duration{micro_time}}; + return modm_platform_micro_now(); } - void modm_test::chrono::micro_clock::setTime(uint32_t microseconds) { @@ -53,3 +74,15 @@ modm_test::chrono::micro_clock::increment(uint32_t microseconds) { micro_time += microseconds; } + +void +modm_test::chrono::micro_clock::enable() +{ + micro_enabled = true; +} + +void +modm_test::chrono::micro_clock::disable() +{ + micro_enabled = false; +} diff --git a/test/modm/mock/clock.hpp b/test/modm/mock/clock.hpp index 8732450fa0..f2632339a9 100644 --- a/test/modm/mock/clock.hpp +++ b/test/modm/mock/clock.hpp @@ -20,6 +20,9 @@ namespace modm_test::chrono class milli_clock : modm::chrono::milli_clock { public: + static void enable(); + static void disable(); + static inline void setTime(std::chrono::milliseconds time) { setTime(time.count()); } static void setTime(uint32_t milliseconds); @@ -33,6 +36,9 @@ class milli_clock : modm::chrono::milli_clock class micro_clock : modm::chrono::micro_clock { public: + static void enable(); + static void disable(); + static inline void setTime(std::chrono::microseconds time) { setTime(time.count()); } static void setTime(uint32_t microseconds); diff --git a/test/modm/mock/module.lb b/test/modm/mock/module.lb index b02310933e..0eb3634c3a 100644 --- a/test/modm/mock/module.lb +++ b/test/modm/mock/module.lb @@ -49,11 +49,8 @@ class SpiMaster(Module): def build(self, env): env.outbasepath = "modm-test/src/modm-test/mock" - env.substitutions = { - "use_fiber": env.get(":processing:protothread:use_fiber", True) - } env.copy("spi_master.hpp") - env.template("spi_master.cpp.in") + env.copy("spi_master.cpp") class CanDriver(Module): def init(self, module): diff --git a/test/modm/mock/spi_master.cpp.in b/test/modm/mock/spi_master.cpp similarity index 90% rename from test/modm/mock/spi_master.cpp.in rename to test/modm/mock/spi_master.cpp index da7f610f32..238645b5b0 100644 --- a/test/modm/mock/spi_master.cpp.in +++ b/test/modm/mock/spi_master.cpp @@ -44,7 +44,7 @@ modm_test::platform::SpiMaster::release(void *ctx) } // ---------------------------------------------------------------------------- -modm::ResumableResult +uint8_t modm_test::platform::SpiMaster::transfer(uint8_t data) { txBuffer.append(data); @@ -56,14 +56,10 @@ modm_test::platform::SpiMaster::transfer(uint8_t data) else { tmp = 0; } -%% if use_fiber return tmp; -%% else - return {modm::rf::Stop, tmp}; -%% endif } -modm::ResumableResult +void modm_test::platform::SpiMaster::transfer(uint8_t * tx, uint8_t * rx, std::size_t length) { for(std::size_t i = 0; i < length; ++i) { @@ -91,8 +87,4 @@ modm_test::platform::SpiMaster::transfer(uint8_t * tx, uint8_t * rx, std::size_t rxBuffer.removeFront(); } } - -%% if not use_fiber - return {modm::rf::Stop}; -%% endif } diff --git a/test/modm/mock/spi_master.hpp b/test/modm/mock/spi_master.hpp index 6d106f26d1..8a74839c54 100644 --- a/test/modm/mock/spi_master.hpp +++ b/test/modm/mock/spi_master.hpp @@ -73,23 +73,25 @@ class SpiMaster : public modm::SpiMaster release(void *ctx); + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static uint8_t transferBlocking(uint8_t data) { - return RF_CALL_BLOCKING(transfer(data)); + return transfer(data); } + [[deprecated("Use transfer() instead!")]] // DEPRECATED: 2026q3 static void transferBlocking(uint8_t *tx, uint8_t *rx, std::size_t length) { - RF_CALL_BLOCKING(transfer(tx, rx, length)); + transfer(tx, rx, length); } - static modm::ResumableResult + static uint8_t transfer(uint8_t data); - static modm::ResumableResult + static void transfer(uint8_t *tx, uint8_t *rx, std::size_t length); public: diff --git a/test/modm/platform/bitbang/spi_bitbang_test.cpp b/test/modm/platform/bitbang/spi_bitbang_test.cpp index 5d4bd8c288..0eaa6db741 100644 --- a/test/modm/platform/bitbang/spi_bitbang_test.cpp +++ b/test/modm/platform/bitbang/spi_bitbang_test.cpp @@ -33,7 +33,7 @@ SpiBitbangTest::testSpiMaster() { auto r = Start(50); - SpiMaster::transferBlocking(0); + SpiMaster::transfer(0); TEST_ASSERT_EQUALS(r[1].size(), 1u); TEST_ASSERT_EQUALS(r[0].size(), 17u); @@ -44,7 +44,7 @@ SpiBitbangTest::testSpiMaster() r[1].dump(); r.restart(); - SpiMaster::transferBlocking(0b1010'1010); + SpiMaster::transfer(0b1010'1010); TEST_ASSERT_EQUALS(r[1].size(), 9u); TEST_ASSERT_EQUALS(r[0].size(), 17u); @@ -54,7 +54,7 @@ SpiBitbangTest::testSpiMaster() r[1].dump(); r.restart(); - SpiMaster::transferBlocking(0b0001'1110); + SpiMaster::transfer(0b0001'1110); TEST_ASSERT_EQUALS(r[1].size(), 3u); TEST_ASSERT_EQUALS(r[0].size(), 17u); @@ -65,7 +65,7 @@ SpiBitbangTest::testSpiMaster() r.restart(); SpiMaster::setDataOrder(SpiMaster::DataOrder::LsbFirst); - SpiMaster::transferBlocking(0b0001'1110); + SpiMaster::transfer(0b0001'1110); TEST_ASSERT_EQUALS(r[1].size(), 3u); TEST_ASSERT_EQUALS(r[0].size(), 17u); diff --git a/test/modm/platform/i2c/samx7x/i2c_platform_test.cpp b/test/modm/platform/i2c/samx7x/i2c_platform_test.cpp index 57c397cb98..71a2cf0d27 100644 --- a/test/modm/platform/i2c/samx7x/i2c_platform_test.cpp +++ b/test/modm/platform/i2c/samx7x/i2c_platform_test.cpp @@ -36,14 +36,14 @@ I2cPlatformTest::testPing() // ping at wrong address for (uint8_t address = 0x50; address <= 0x56; ++address) { eeprom.setAddress(address); - const bool response = RF_CALL_BLOCKING(eeprom.ping()); + const bool response = eeprom.ping(); TEST_ASSERT_FALSE(response); } // set correct address 0x57 eeprom.setAddress(0x57); // ping at correct address for (int i = 0; i < 20; ++i) { - const bool response = RF_CALL_BLOCKING(eeprom.ping()); + const bool response = eeprom.ping(); TEST_ASSERT_TRUE(response); } } @@ -57,12 +57,12 @@ I2cPlatformTest::testDataRead() // read at wrong address eeprom.setAddress(0x55); - bool readSuccess = RF_CALL_BLOCKING(eeprom.readMac(buffer)); + bool readSuccess = eeprom.readMac(buffer); TEST_ASSERT_FALSE(readSuccess); // read at correct address eeprom.setAddress(0x57); - readSuccess = RF_CALL_BLOCKING(eeprom.readMac(buffer)); + readSuccess = eeprom.readMac(buffer); TEST_ASSERT_TRUE(readSuccess); TEST_ASSERT_EQUALS(buffer[0], 0xfc); diff --git a/test/modm/platform/spi/samx7x/spi_test.cpp b/test/modm/platform/spi/samx7x/spi_test.cpp index 3797a6efb0..2e22de6f53 100644 --- a/test/modm/platform/spi/samx7x/spi_test.cpp +++ b/test/modm/platform/spi/samx7x/spi_test.cpp @@ -32,19 +32,19 @@ SpiTest::testTransferLoopback() { SpiHal0::setLoopbackMode(SpiHal0::LocalLoopback::Enabled); - uint8_t value = SpiMaster0::transferBlocking(0xAB); + uint8_t value = SpiMaster0::transfer(0xAB); TEST_ASSERT_EQUALS(value, 0xAB); - value = RF_CALL_BLOCKING(SpiMaster0::transfer(0x12)); + value = SpiMaster0::transfer(0x12); TEST_ASSERT_EQUALS(value, 0x12); constexpr std::array tx{0xDE, 0xAD, 0xBE, 0xEF, 0x12, 0x34, 0x56}; std::array rx{}; - SpiMaster0::transferBlocking(tx.data(), rx.data(), 7); + SpiMaster0::transfer(tx.data(), rx.data(), 7); TEST_ASSERT_TRUE(rx == tx); rx.fill(0); - RF_CALL_BLOCKING(SpiMaster0::transfer(tx.data(), rx.data(), 7)); + SpiMaster0::transfer(tx.data(), rx.data(), 7); TEST_ASSERT_TRUE(rx == tx); SpiHal0::setLoopbackMode(SpiHal0::LocalLoopback::Disabled); @@ -55,12 +55,12 @@ SpiTest::testReceivePin() { Miso::configure(Miso::InputType::PullUp); modm::delay_us(1); - uint8_t value = SpiMaster0::transferBlocking(0xAB); + uint8_t value = SpiMaster0::transfer(0xAB); TEST_ASSERT_EQUALS(value, 0xFF); Miso::configure(Miso::InputType::PullDown); modm::delay_us(1); - value = SpiMaster0::transferBlocking(0xAB); + value = SpiMaster0::transfer(0xAB); TEST_ASSERT_EQUALS(value, 0x00); Miso::configure(Miso::InputType::Floating); diff --git a/test/modm/platform/spi/stm32h7/spi_bdma_test.cpp b/test/modm/platform/spi/stm32h7/spi_bdma_test.cpp index b177057401..84ac50526b 100644 --- a/test/modm/platform/spi/stm32h7/spi_bdma_test.cpp +++ b/test/modm/platform/spi/stm32h7/spi_bdma_test.cpp @@ -37,17 +37,17 @@ SpiBdmaTest::testReceive() Miso::configure(Miso::InputType::PullUp); modm::delay_ns(500); - Spi::transferBlocking(nullptr, buffer.data(), buffer.size()); + Spi::transfer(nullptr, buffer.data(), buffer.size()); TEST_ASSERT_TRUE(buffer == ones); Miso::configure(Miso::InputType::PullDown); modm::delay_ns(500); - Spi::transferBlocking(nullptr, buffer.data(), buffer.size()); + Spi::transfer(nullptr, buffer.data(), buffer.size()); TEST_ASSERT_TRUE(buffer == zeros); Miso::configure(Miso::InputType::PullUp); modm::delay_ns(500); - Spi::transferBlocking(nullptr, buffer.data(), buffer.size()); + Spi::transfer(nullptr, buffer.data(), buffer.size()); TEST_ASSERT_TRUE(buffer == ones); Miso::configure(Miso::InputType::Floating); diff --git a/test/modm/processing/fiber/fiber_test.cpp b/test/modm/processing/fiber/fiber_test.cpp index 6e1f4446a2..1cdccb2e5a 100644 --- a/test/modm/processing/fiber/fiber_test.cpp +++ b/test/modm/processing/fiber/fiber_test.cpp @@ -23,9 +23,18 @@ using test_clock_us = modm_test::chrono::micro_clock; void FiberTest::setUp() { + test_clock_ms::enable(); + test_clock_us::enable(); state = 0; } +void +FiberTest::tearDown() +{ + test_clock_ms::disable(); + test_clock_us::disable(); +} + // ================================== FIBER =================================== static void f1() diff --git a/test/modm/processing/fiber/fiber_test.hpp b/test/modm/processing/fiber/fiber_test.hpp index 4d25516995..b8b0fb699e 100644 --- a/test/modm/processing/fiber/fiber_test.hpp +++ b/test/modm/processing/fiber/fiber_test.hpp @@ -18,7 +18,10 @@ class FiberTest : public unittest::TestSuite { public: void - setUp(); + setUp() override; + + void + tearDown() override; void testOneFiber(); diff --git a/test/modm/processing/timer/periodic_timer_test.cpp b/test/modm/processing/timer/periodic_timer_test.cpp index 9d9b837903..9fc92f13e4 100644 --- a/test/modm/processing/timer/periodic_timer_test.cpp +++ b/test/modm/processing/timer/periodic_timer_test.cpp @@ -22,9 +22,16 @@ using test_clock = modm_test::chrono::milli_clock; void PeriodicTimerTest::setUp() { + test_clock::enable(); test_clock::setTime(0); } +void +PeriodicTimerTest::tearDown() +{ + test_clock::disable(); +} + void PeriodicTimerTest::testConstructor() { diff --git a/test/modm/processing/timer/periodic_timer_test.hpp b/test/modm/processing/timer/periodic_timer_test.hpp index dbdc3aa5b4..c283eae4b4 100644 --- a/test/modm/processing/timer/periodic_timer_test.hpp +++ b/test/modm/processing/timer/periodic_timer_test.hpp @@ -17,8 +17,11 @@ class PeriodicTimerTest : public unittest::TestSuite { public: - virtual void - setUp(); + void + setUp() override; + + void + tearDown() override; void diff --git a/test/modm/processing/timer/timeout_test.cpp b/test/modm/processing/timer/timeout_test.cpp index 3670c64bd0..b8fe02f5d4 100644 --- a/test/modm/processing/timer/timeout_test.cpp +++ b/test/modm/processing/timer/timeout_test.cpp @@ -25,9 +25,17 @@ using test_clock = modm_test::chrono::milli_clock; void TimeoutTest::setUp() { + test_clock::enable(); test_clock::setTime(0); } +void +TimeoutTest::tearDown() +{ + test_clock::disable(); +} + + void TimeoutTest::testDefaultConstructor() { diff --git a/test/modm/processing/timer/timeout_test.hpp b/test/modm/processing/timer/timeout_test.hpp index e37e5aaf79..5a67c6dc1c 100644 --- a/test/modm/processing/timer/timeout_test.hpp +++ b/test/modm/processing/timer/timeout_test.hpp @@ -17,8 +17,11 @@ class TimeoutTest : public unittest::TestSuite { public: - virtual void - setUp(); + void + setUp() override; + + void + tearDown() override; void testDefaultConstructor();