From 793bc7810f14b4b6a5e8fc95d1f6c7922c756a1a Mon Sep 17 00:00:00 2001 From: Niklas Hauser Date: Sun, 24 Nov 2024 20:54:59 +0100 Subject: [PATCH] [examples] WIP Replace Protothreads with Fibers --- examples/arduino_nano/color/main.cpp | 100 ++--- examples/arduino_nano/color/project.xml | 3 +- examples/avr/fiber/main.cpp | 101 ++--- examples/avr/fiber/project.xml | 10 +- examples/avr/fiber_benchmark/main.cpp | 94 ++++ examples/avr/fiber_benchmark/project.xml | 12 + examples/avr/protothread/main.cpp | 112 ----- examples/avr/protothread/project.xml | 15 - examples/blue_pill_f103/adns_9800/main.cpp | 120 ++---- examples/blue_pill_f103/adns_9800/project.xml | 2 +- examples/blue_pill_f103/environment/main.cpp | 10 +- .../blue_pill_f103/environment/project.xml | 2 +- .../environment/thread_blink.cpp | 26 +- .../environment/thread_blink.hpp | 32 -- .../environment/thread_bme280.cpp | 53 +-- .../environment/thread_bme280.hpp | 9 +- .../environment/thread_bmp180.cpp | 47 +- .../environment/thread_bmp180.hpp | 9 +- .../blue_pill_f103/environment/threads.hpp | 3 +- examples/blue_pill_f103/logger/main.cpp | 51 +-- examples/blue_pill_f103/logger/project.xml | 2 - .../weight_scale_hx711/main.cpp | 64 +-- .../weight_scale_hx711/project.xml | 3 +- examples/generic/i2c_multiplex/main.cpp | 73 +--- examples/generic/i2c_multiplex/project.xml | 8 +- examples/generic/resumable/main.cpp | 87 ---- examples/generic/resumable/project.xml | 13 - examples/generic/ros/environment/main.cpp | 110 ++--- examples/generic/ros/environment/project.xml | 3 +- .../generic/ros/environment/thread_bme280.cpp | 85 ++-- .../generic/ros/environment/thread_bme280.hpp | 9 +- .../ros/environment/thread_display.cpp | 14 +- .../ros/environment/thread_display.hpp | 8 +- .../nucleo_f401re/distance_vl53l0/main.cpp | 152 +++---- .../nucleo_f401re/distance_vl53l0/project.xml | 2 +- .../dw3110-communication/main.cpp | 115 +++-- .../dw3110-communication/project.xml | 2 +- examples/nucleo_f411re/imu_bno055/main.cpp | 84 ++-- examples/nucleo_f411re/imu_bno055/project.xml | 2 +- examples/nucleo_f429zi/pat9125el/main.cpp | 80 +--- examples/nucleo_f429zi/pat9125el/project.xml | 2 - examples/nucleo_f429zi/rtc_mcp7941x/main.cpp | 135 +++--- .../nucleo_f429zi/rtc_mcp7941x/project.xml | 3 +- .../nucleo_f429zi/spi_flash_fatfs/main.cpp | 2 - .../nucleo_f429zi/spi_flash_fatfs/project.xml | 2 - examples/nucleo_f439zi/spi_dma/main.cpp | 72 ++-- examples/nucleo_f439zi/spi_dma/project.xml | 2 - examples/nucleo_f446re/color/main.cpp | 94 ++-- examples/nucleo_f446re/color/project.xml | 3 +- examples/nucleo_f767zi/spi_dma/main.cpp | 72 ++-- examples/nucleo_f767zi/spi_dma/project.xml | 2 - examples/nucleo_g071rb/amnb/main.cpp | 76 ++-- examples/nucleo_g071rb/amnb/project.xml | 4 +- examples/nucleo_g071rb/amnb_fiber/main.cpp | 166 ------- examples/nucleo_g071rb/amnb_fiber/project.xml | 18 - examples/nucleo_g474re/ads101x/main.cpp | 251 +++++------ examples/nucleo_g474re/ads101x/project.xml | 4 +- examples/nucleo_g474re/ads7828/main.cpp | 302 ++++++------- examples/nucleo_g474re/ads7828/project.xml | 5 +- examples/nucleo_g474re/as5047/main.cpp | 52 +-- examples/nucleo_g474re/as5047/project.xml | 4 +- examples/nucleo_g474re/ixm42xxx/main.cpp | 100 ++--- examples/nucleo_g474re/ixm42xxx/project.xml | 5 +- examples/nucleo_g474re/ixm42xxx_fifo/main.cpp | 77 ++-- .../nucleo_g474re/ixm42xxx_fifo/project.xml | 5 +- examples/nucleo_g474re/max31855/main.cpp | 100 ++--- examples/nucleo_g474re/max31855/project.xml | 5 +- examples/nucleo_g474re/ms5611/main.cpp | 127 ++---- examples/nucleo_g474re/ms5611/project.xml | 5 +- examples/nucleo_g474re/sx128x_lora/main.cpp | 408 +++++++++--------- .../nucleo_g474re/sx128x_lora/project.xml | 6 +- examples/nucleo_h723zg/bmi088/i2c/project.xml | 2 - examples/nucleo_h723zg/bmi088/spi/project.xml | 2 - examples/nucleo_l432kc/gyroscope/main.cpp | 87 ++-- examples/nucleo_l432kc/gyroscope/project.xml | 2 - examples/nucleo_l476rg/i2c_test/main.cpp | 95 ++-- examples/nucleo_l476rg/i2c_test/project.xml | 4 +- examples/rp_pico/logger/main.cpp | 49 +-- examples/rp_pico/logger/project.xml | 3 - examples/rp_pico/mclogger/main.cpp | 61 +-- examples/rp_pico/mclogger/project.xml | 3 +- examples/rp_pico/rtc_mcp7941x/main.cpp | 21 +- .../stm32f072_discovery/rotation/main.cpp | 11 +- examples/stm32f072_discovery/tmp102/main.cpp | 22 +- .../stm32f3_discovery/accelerometer/main.cpp | 11 +- .../accelerometer/main.cpp | 11 +- .../stm32f401_discovery/gyroscope/main.cpp | 11 +- .../stm32f469_discovery/max31865/main.cpp | 13 +- .../stm32f469_discovery/touchscreen/main.cpp | 10 +- .../stm32f4_discovery/accelerometer/main.cpp | 15 +- .../barometer_bmp085_bmp180/main.cpp | 16 +- .../stm32f4_discovery/colour_tcs3414/main.cpp | 22 +- .../display/hd44780/main.cpp | 12 +- .../display/nokia_5110/main.cpp | 7 +- .../distance_vl6180/main.cpp | 22 +- .../pressure_ams5915/main.cpp | 17 +- .../stm32f4_discovery/protothreads/main.cpp | 22 +- .../temperature_ltc2984/main.cpp | 22 +- .../stm32f746g_discovery/adc_ad7928/main.cpp | 25 +- examples/stm32f746g_discovery/tmp102/main.cpp | 22 +- 100 files changed, 1653 insertions(+), 2803 deletions(-) create mode 100644 examples/avr/fiber_benchmark/main.cpp create mode 100644 examples/avr/fiber_benchmark/project.xml delete mode 100644 examples/avr/protothread/main.cpp delete mode 100644 examples/avr/protothread/project.xml delete mode 100644 examples/blue_pill_f103/environment/thread_blink.hpp delete mode 100644 examples/generic/resumable/main.cpp delete mode 100644 examples/generic/resumable/project.xml delete mode 100644 examples/nucleo_g071rb/amnb_fiber/main.cpp delete mode 100644 examples/nucleo_g071rb/amnb_fiber/project.xml diff --git a/examples/arduino_nano/color/main.cpp b/examples/arduino_nano/color/main.cpp index 80d7cd9d52..84cda3924d 100644 --- a/examples/arduino_nano/color/main.cpp +++ b/examples/arduino_nano/color/main.cpp @@ -15,65 +15,59 @@ using namespace modm::platform; -class Sensorthread : public modm::pt::Protothread +modm::Fiber fiber_blink([] { -private: - modm::ShortTimeout timeout; - - modm::tcs3472::Data data; - modm::Tcs3472 sensor{data}; - using TCS3472_INT = Board::D2; - -public: - bool - update() + LedD13::setOutput(); + while (true) { - PT_BEGIN(); + LedD13::toggle(); + modm::this_fiber::sleep_for(0.5s); + } +}); - TCS3472_INT::setInput(Gpio::InputType::PullUp); +modm::tcs3472::Data data; +modm::Tcs3472 sensor{data}; +using TCS3472_INT = Board::D2; +modm::Fiber fiber_sensor([] +{ + TCS3472_INT::setInput(Gpio::InputType::PullUp); - MODM_LOG_INFO << "Ping TCS34725" << modm::endl; - // ping the device until it responds - while (true) - { - // we wait until the task started - if (PT_CALL(sensor.ping())) { break; } - // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); - } + MODM_LOG_INFO << "Ping TCS34725" << modm::endl; + // ping the device until it responds + while (true) + { + // we wait until the task started + if (sensor.ping()) break; + // otherwise, try again in 100ms + modm::this_fiber::sleep_for(100ms); + } - MODM_LOG_INFO << "TCS34725 responded" << modm::endl; + MODM_LOG_INFO << "TCS34725 responded" << modm::endl; - PT_CALL(sensor.initialize(sensor.Enable_InterruptMode_Waittime)); - PT_CALL(sensor.configure(modm::tcs3472::Gain::X16, modm::tcs3472::IntegrationTime::MSEC_2_4)); - PT_CALL(sensor.setInterruptPersistenceFilter(modm::tcs3472::InterruptPersistence::CNT_20)); - // Setup WaitTime to further slow down samplerate - PT_CALL(sensor.setWaitTime(modm::tcs3472::WaitTime::MSEC_2_4)); + sensor.initialize(sensor.Enable_InterruptMode_Waittime); + sensor.configure(modm::tcs3472::Gain::X16, modm::tcs3472::IntegrationTime::MSEC_2_4); + sensor.setInterruptPersistenceFilter(modm::tcs3472::InterruptPersistence::CNT_20); + // Setup WaitTime to further slow down samplerate + sensor.setWaitTime(modm::tcs3472::WaitTime::MSEC_2_4); - // Dummy read required - PT_CALL(sensor.readColor()); - // Fetch one sample ... - PT_CALL(sensor.readColor()); - // ...and set the high threshold 20% above current clear - PT_CALL(sensor.setInterruptHighThreshold(data.getClear() * 1.2)); + // Dummy read required + sensor.readColor(); + // Fetch one sample ... + sensor.readColor(); + // ...and set the high threshold 20% above current clear + sensor.setInterruptHighThreshold(data.getClear() * 1.2); - while (true) + while (true) + { + sensor.reloadInterrupt(); + modm::this_fiber::poll([]{ return not TCS3472_INT::read(); }); + if (sensor.readColor()) { - PT_CALL(sensor.reloadInterrupt()); - PT_WAIT_UNTIL(TCS3472_INT::read() == false); - if (PT_CALL(sensor.readColor())) - { - const auto rgb = data.getColor(); - MODM_LOG_INFO << "RGB: " << rgb << "\tHSV: " << modm::color::Hsv(rgb) << modm::endl; - } + const auto rgb = data.getColor(); + MODM_LOG_INFO << "RGB: " << rgb << "\tHSV: " << modm::color::Hsv(rgb) << modm::endl; } - - PT_END(); } -}; - -Sensorthread sensorthread; +}); int main() @@ -81,12 +75,6 @@ main() Board::initialize(); I2cMaster::initialize(); - LedD13::setOutput(); - modm::ShortPeriodicTimer heartbeat(500ms); - - while (true) - { - sensorthread.update(); - if (heartbeat.execute()) Board::LedD13::toggle(); - } + modm::fiber::Scheduler::run(); + return 0; } diff --git a/examples/arduino_nano/color/project.xml b/examples/arduino_nano/color/project.xml index 3a249e8797..80e6ae2182 100644 --- a/examples/arduino_nano/color/project.xml +++ b/examples/arduino_nano/color/project.xml @@ -5,8 +5,7 @@ modm:build:scons - modm:processing:protothread - modm:processing:timer + modm:processing:fiber modm:platform:i2c modm:driver:tcs3472 diff --git a/examples/avr/fiber/main.cpp b/examples/avr/fiber/main.cpp index b7571a17c9..841dd11863 100644 --- a/examples/avr/fiber/main.cpp +++ b/examples/avr/fiber/main.cpp @@ -1,5 +1,7 @@ /* - * Copyright (c) 2020, Erik Henriksson + * Copyright (c) 2010-2011, Fabian Greif + * Copyright (c) 2012-2013, 2015-2017, Niklas Hauser + * Copyright (c) 2014, Sascha Schade * * This file is part of the modm project. * @@ -9,86 +11,57 @@ */ // ---------------------------------------------------------------------------- -#include -#include +#include +#include #include -using namespace Board; +using namespace modm::platform; using namespace std::chrono_literals; -constexpr uint32_t cycles = 100'000; -volatile uint32_t f1counter = 0, f2counter = 0; -uint32_t total_counter=0; +using LedGreen = GpioOutputB0; +using LedRed = GpioOutputB1; -void -fiber_function1() +modm::Fiber fiber_green([] { - MODM_LOG_INFO << MODM_FILE_INFO << modm::endl; - while (++f1counter < cycles) { modm::this_fiber::yield(); total_counter++; } -} - -void -fiber_function2(uint32_t cyc) -{ - MODM_LOG_INFO << MODM_FILE_INFO << modm::endl; - while (++f2counter < cyc) { modm::this_fiber::yield(); total_counter++; } -} + LedGreen::setOutput(); + LedGreen::set(); -struct Test -{ - void - fiber_function3() + while (true) { - MODM_LOG_INFO << MODM_FILE_INFO << modm::endl; - while (++f3counter < cycles) { modm::this_fiber::yield(); total_counter++; } + LedGreen::set(); + modm::this_fiber::sleep_for(100ms); + + LedGreen::reset(); + modm::this_fiber::sleep_for(600ms); } +}); + +modm::Fiber fiber_red([] +{ + LedRed::setOutput(); + LedRed::set(); - void - fiber_function4(uint32_t cyc) + while (true) { - MODM_LOG_INFO << MODM_FILE_INFO << modm::endl; - while (++f4counter < cyc) { modm::this_fiber::yield(); total_counter++; } - } + LedRed::set(); + modm::this_fiber::sleep_for(200ms); + + LedRed::reset(); + modm::this_fiber::sleep_for(300ms); - volatile uint32_t f3counter{0}; - volatile uint32_t f4counter{0}; -} test; + LedRed::set(); + modm::this_fiber::sleep_for(200ms); -modm::Fiber<> fiber1(fiber_function1); -modm::Fiber<> fiber2(+[](){ fiber_function2(cycles); }); -modm::Fiber<> fiber3(+[](){ test.fiber_function3(); }); -modm::Fiber<> fiber4([cyc=uint32_t(cycles)]() mutable { cyc++; test.fiber_function4(cyc); }); + LedRed::reset(); + modm::this_fiber::sleep_for(1s); + } +}); -// ATmega2560@16MHz: 239996 yields in 2492668us, 96280 yields per second, 10386ns per yield int main() { - Board::initialize(); - Board::LedD13::setOutput(); - MODM_LOG_INFO << "Starting fiber modm::yield benchmark..." << modm::endl; - MODM_LOG_INFO.flush(); - - fiber1.stack_watermark(); - fiber2.stack_watermark(); - fiber3.stack_watermark(); - fiber4.stack_watermark(); + SystemClock::enable(); + enableInterrupts(); - const modm::PreciseTimestamp start = modm::PreciseClock::now(); modm::fiber::Scheduler::run(); - const auto diff = (modm::PreciseClock::now() - start); - - MODM_LOG_INFO << "Benchmark done!" << modm::endl; - MODM_LOG_INFO << "Executed " << total_counter << " yields in " << diff << modm::endl; - MODM_LOG_INFO << uint32_t((total_counter * 1'000'000ull) / std::chrono::microseconds(diff).count()); - MODM_LOG_INFO << " yields per second, "; - MODM_LOG_INFO << uint32_t(std::chrono::nanoseconds(diff).count() / total_counter); - MODM_LOG_INFO << "ns per yield" << modm::endl; - - MODM_LOG_INFO << "Stack usage 1 = " << fiber1.stack_usage() << modm::endl; - MODM_LOG_INFO << "Stack usage 2 = " << fiber2.stack_usage() << modm::endl; - MODM_LOG_INFO << "Stack usage 3 = " << fiber3.stack_usage() << modm::endl; - MODM_LOG_INFO << "Stack usage 4 = " << fiber4.stack_usage() << modm::endl; - - while(1) ; - return 0; } diff --git a/examples/avr/fiber/project.xml b/examples/avr/fiber/project.xml index d06e96568a..b277d24da6 100644 --- a/examples/avr/fiber/project.xml +++ b/examples/avr/fiber/project.xml @@ -1,12 +1,14 @@ - modm:mega-2560-pro - + + - modm:build:scons - modm:processing:timer + modm:platform:core + modm:platform:clock + modm:platform:gpio modm:processing:fiber + modm:build:scons diff --git a/examples/avr/fiber_benchmark/main.cpp b/examples/avr/fiber_benchmark/main.cpp new file mode 100644 index 0000000000..b7571a17c9 --- /dev/null +++ b/examples/avr/fiber_benchmark/main.cpp @@ -0,0 +1,94 @@ +/* + * Copyright (c) 2020, Erik Henriksson + * + * 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/. + */ +// ---------------------------------------------------------------------------- + +#include +#include +#include + +using namespace Board; +using namespace std::chrono_literals; + +constexpr uint32_t cycles = 100'000; +volatile uint32_t f1counter = 0, f2counter = 0; +uint32_t total_counter=0; + +void +fiber_function1() +{ + MODM_LOG_INFO << MODM_FILE_INFO << modm::endl; + while (++f1counter < cycles) { modm::this_fiber::yield(); total_counter++; } +} + +void +fiber_function2(uint32_t cyc) +{ + MODM_LOG_INFO << MODM_FILE_INFO << modm::endl; + while (++f2counter < cyc) { modm::this_fiber::yield(); total_counter++; } +} + +struct Test +{ + void + fiber_function3() + { + MODM_LOG_INFO << MODM_FILE_INFO << modm::endl; + while (++f3counter < cycles) { modm::this_fiber::yield(); total_counter++; } + } + + void + fiber_function4(uint32_t cyc) + { + MODM_LOG_INFO << MODM_FILE_INFO << modm::endl; + while (++f4counter < cyc) { modm::this_fiber::yield(); total_counter++; } + } + + volatile uint32_t f3counter{0}; + volatile uint32_t f4counter{0}; +} test; + +modm::Fiber<> fiber1(fiber_function1); +modm::Fiber<> fiber2(+[](){ fiber_function2(cycles); }); +modm::Fiber<> fiber3(+[](){ test.fiber_function3(); }); +modm::Fiber<> fiber4([cyc=uint32_t(cycles)]() mutable { cyc++; test.fiber_function4(cyc); }); + +// ATmega2560@16MHz: 239996 yields in 2492668us, 96280 yields per second, 10386ns per yield +int +main() +{ + Board::initialize(); + Board::LedD13::setOutput(); + MODM_LOG_INFO << "Starting fiber modm::yield benchmark..." << modm::endl; + MODM_LOG_INFO.flush(); + + fiber1.stack_watermark(); + fiber2.stack_watermark(); + fiber3.stack_watermark(); + fiber4.stack_watermark(); + + const modm::PreciseTimestamp start = modm::PreciseClock::now(); + modm::fiber::Scheduler::run(); + const auto diff = (modm::PreciseClock::now() - start); + + MODM_LOG_INFO << "Benchmark done!" << modm::endl; + MODM_LOG_INFO << "Executed " << total_counter << " yields in " << diff << modm::endl; + MODM_LOG_INFO << uint32_t((total_counter * 1'000'000ull) / std::chrono::microseconds(diff).count()); + MODM_LOG_INFO << " yields per second, "; + MODM_LOG_INFO << uint32_t(std::chrono::nanoseconds(diff).count() / total_counter); + MODM_LOG_INFO << "ns per yield" << modm::endl; + + MODM_LOG_INFO << "Stack usage 1 = " << fiber1.stack_usage() << modm::endl; + MODM_LOG_INFO << "Stack usage 2 = " << fiber2.stack_usage() << modm::endl; + MODM_LOG_INFO << "Stack usage 3 = " << fiber3.stack_usage() << modm::endl; + MODM_LOG_INFO << "Stack usage 4 = " << fiber4.stack_usage() << modm::endl; + + while(1) ; + return 0; +} diff --git a/examples/avr/fiber_benchmark/project.xml b/examples/avr/fiber_benchmark/project.xml new file mode 100644 index 0000000000..d60a677b8a --- /dev/null +++ b/examples/avr/fiber_benchmark/project.xml @@ -0,0 +1,12 @@ + + modm:mega-2560-pro + + + + + + modm:build:scons + modm:processing:timer + modm:processing:fiber + + diff --git a/examples/avr/protothread/main.cpp b/examples/avr/protothread/main.cpp deleted file mode 100644 index cff2977046..0000000000 --- a/examples/avr/protothread/main.cpp +++ /dev/null @@ -1,112 +0,0 @@ -/* - * Copyright (c) 2010-2011, Fabian Greif - * Copyright (c) 2012-2013, 2015-2017, Niklas Hauser - * Copyright (c) 2014, Sascha Schade - * - * 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/. - */ -// ---------------------------------------------------------------------------- - -#include -#include -#include -#include - -using namespace modm::platform; -using namespace std::chrono_literals; - -typedef GpioOutputB0 LedGreen; -typedef GpioOutputB1 LedRed; - -class BlinkingLightGreen : public modm::pt::Protothread -{ -public: - bool - run() - { - PT_BEGIN(); - - // set everything up - LedGreen::setOutput(); - LedGreen::set(); - - while (true) - { - LedGreen::set(); - - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); - - LedGreen::reset(); - - timeout.restart(600ms); - PT_WAIT_UNTIL(timeout.isExpired()); - } - - PT_END(); - } - -private: - modm::ShortTimeout timeout; -}; - -class BlinkingLightRed : public modm::pt::Protothread -{ -public: - bool - run() - { - PT_BEGIN(); - - // set everything up - LedRed::setOutput(); - LedRed::set(); - - while (true) - { - LedRed::set(); - - timeout.restart(200ms); - PT_WAIT_UNTIL(timeout.isExpired()); - - LedRed::reset(); - - timeout.restart(300ms); - PT_WAIT_UNTIL(timeout.isExpired()); - - LedRed::set(); - - timeout.restart(200ms); - PT_WAIT_UNTIL(timeout.isExpired()); - - LedRed::reset(); - - timeout.restart(1s); - PT_WAIT_UNTIL(timeout.isExpired()); - } - - PT_END(); - } - -private: - modm::ShortTimeout timeout; -}; - -int -main() -{ - SystemClock::enable(); - enableInterrupts(); - - BlinkingLightGreen greenLight; - BlinkingLightRed redLight; - while (true) - { - greenLight.run(); - redLight.run(); - } -} diff --git a/examples/avr/protothread/project.xml b/examples/avr/protothread/project.xml deleted file mode 100644 index 1dd52d5caf..0000000000 --- a/examples/avr/protothread/project.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - modm:platform:core - modm:platform:clock - modm:platform:gpio - modm:processing:protothread - modm:processing:timer - modm:build:scons - - diff --git a/examples/blue_pill_f103/adns_9800/main.cpp b/examples/blue_pill_f103/adns_9800/main.cpp index 4ba52d34fe..8464e666af 100644 --- a/examples/blue_pill_f103/adns_9800/main.cpp +++ b/examples/blue_pill_f103/adns_9800/main.cpp @@ -13,9 +13,8 @@ // ---------------------------------------------------------------------------- #include -#include -#include -#include +#include +#include #include @@ -36,95 +35,65 @@ modm::log::Logger modm::log::info(loggerDevice); modm::log::Logger modm::log::warning(loggerDevice); modm::log::Logger modm::log::error(loggerDevice); -class BlinkThread : public modm::pt::Protothread + +modm::Fiber fiber_blink([] { -public: - BlinkThread() - { - timeout.restart(100ms); - } + modm::ShortTimeout timeout(100ms); + uint32_t uptime{}; - bool - update() + while (true) { - PT_BEGIN(); - - while (true) - { - Board::LedGreen::reset(); + Board::LedGreen::reset(); - PT_WAIT_UNTIL(timeout.isExpired()); - timeout.restart(100ms); - - Board::LedGreen::set(); + timeout.wait(); + timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()) ; - timeout.restart(4.9s); + Board::LedGreen::set(); - MODM_LOG_INFO << "Seconds since reboot: " << uptime << modm::endl; + timeout.wait(); + timeout.restart(4.9s); - uptime += 5; - } + MODM_LOG_INFO << "Seconds since reboot: " << uptime << modm::endl; - PT_END(); + uptime += 5; } +}); + -private: - modm::ShortTimeout timeout; - uint32_t uptime; -}; +using Cs = GpioOutputA4; +using Adns9800 = modm::Adns9800< + /* Spi = */ SpiMaster1, + /* Ncs = */ Cs >; -class Adns9800Thread : public modm::pt::Protothread +modm::Fiber fiber_adns9800([] { -public: - Adns9800Thread() : timer(10ms), x(0), y(0) - { - } + modm::ShortPeriodicTimer timer(10ms); + int32_t x{}, y{}; - bool - update() - { - PT_BEGIN(); + Cs::setOutput(modm::Gpio::High); - Cs::setOutput(modm::Gpio::High); + SpiMaster1::connect(); + SpiMaster1::initialize(); + SpiMaster1::setDataMode(SpiMaster1::DataMode::Mode3); - SpiMaster1::connect(); - SpiMaster1::initialize(); - SpiMaster1::setDataMode(SpiMaster1::DataMode::Mode3); + Adns9800::initialise(); - adns9800::initialise(); + while (true) + { + timer.wait(); - while (true) { - PT_WAIT_UNTIL(timer.execute()); - - { - int16_t delta_x, delta_y; - adns9800::getDeltaXY(delta_x, delta_y); - MODM_LOG_INFO.printf("dx = %5" PRId16 ", dy = %5" PRId16"; x = %9" PRId32", y=%9" PRId32 "\n", delta_x, delta_y, x, y); - - x += delta_x; - y += delta_y; - } + int16_t delta_x, delta_y; + Adns9800::getDeltaXY(delta_x, delta_y); + MODM_LOG_INFO.printf( + "dx = %5"PRId16", dy = %5"PRId16"; x = %9"PRId32", y=%9"PRId32"\n", + delta_x, delta_y, x, y); + + x += delta_x; + y += delta_y; } - - PT_END(); } - -private: - modm::ShortPeriodicTimer timer; - int32_t x, y; - - using Cs = GpioOutputA4; - - using adns9800 = modm::Adns9800< - /* Spi = */ SpiMaster1, - /* Ncs = */ Cs >; -}; - - -BlinkThread blinkThread; -Adns9800Thread adns9800Thread; +}); // ---------------------------------------------------------------------------- @@ -146,11 +115,6 @@ main() MODM_LOG_INFO << "Welcome to ADNS 9800 demo." << modm::endl; - while (true) - { - blinkThread.update(); - adns9800Thread.update(); - } - + modm::fiber::Scheduler::run(); return 0; } diff --git a/examples/blue_pill_f103/adns_9800/project.xml b/examples/blue_pill_f103/adns_9800/project.xml index 4f54e9196d..a49056e800 100644 --- a/examples/blue_pill_f103/adns_9800/project.xml +++ b/examples/blue_pill_f103/adns_9800/project.xml @@ -10,7 +10,7 @@ modm:platform:gpio modm:platform:spi:1 modm:platform:uart:2 - modm:processing:protothread + modm:processing:fiber modm:processing:timer modm:build:scons diff --git a/examples/blue_pill_f103/environment/main.cpp b/examples/blue_pill_f103/environment/main.cpp index 8050ba91e0..fff74a2031 100644 --- a/examples/blue_pill_f103/environment/main.cpp +++ b/examples/blue_pill_f103/environment/main.cpp @@ -21,7 +21,6 @@ // ---------------------------------------------------------------------------- #include "threads.hpp" -BlinkThread blinkThread; Bmp180Thread bmp180Thread; Bme280Thread bme280Thread; @@ -65,14 +64,7 @@ main() MODM_LOG_DEBUG << "Welcome to Environment Sensor Test" << modm::endl; - LedGreen::set(); - - while (true) - { - blinkThread.update(); - bmp180Thread.update(); - bme280Thread.update(); - } + modm::fiber::Scheduler::run(); return 0; } diff --git a/examples/blue_pill_f103/environment/project.xml b/examples/blue_pill_f103/environment/project.xml index a679f45f7c..b930caf338 100644 --- a/examples/blue_pill_f103/environment/project.xml +++ b/examples/blue_pill_f103/environment/project.xml @@ -13,7 +13,7 @@ modm:platform:i2c:2 modm:platform:uart:2 modm:processing:timer - modm:processing:protothread + modm:processing:fiber modm:build:scons diff --git a/examples/blue_pill_f103/environment/thread_blink.cpp b/examples/blue_pill_f103/environment/thread_blink.cpp index 4607ac0f37..54855ca64e 100644 --- a/examples/blue_pill_f103/environment/thread_blink.cpp +++ b/examples/blue_pill_f103/environment/thread_blink.cpp @@ -21,32 +21,24 @@ #undef MODM_LOG_LEVEL #define MODM_LOG_LEVEL modm::log::INFO -// ---------------------------------------------------------------------------- -BlinkThread::BlinkThread() : - timer(5s), - uptime(0) -{ -} +modm::ShortPeriodicTimer timer(5s); +uint32_t uptime; -bool -BlinkThread::update() +// ---------------------------------------------------------------------------- +modm::Fiber fiber_blink([] { - PT_BEGIN(); - while (true) { - PT_WAIT_UNTIL(timer.execute()); - timeout.restart(100ms); + timer.wait(); Board::LedGreen::set(); bmp180Thread.startMeasurement(); bme280Thread.startMeasurement(); - - PT_WAIT_UNTIL(timeout.isExpired()) ; + modm::this_fiber::sleep_for(100ms); Board::LedGreen::reset(); - PT_WAIT_UNTIL(bmp180Thread.isNewDataAvailable() and bme280Thread.isNewDataAvailable()); + modm::this_fiber::poll([&]{ return bmp180Thread.isNewDataAvailable() and bme280Thread.isNewDataAvailable(); }); int16_t bmp180_a_temp = bmp180Thread.getTemperatureA(); int32_t bmp180_a_press = bmp180Thread.getPressureA(); @@ -73,6 +65,4 @@ BlinkThread::update() ++uptime; } - - PT_END(); -} +}); diff --git a/examples/blue_pill_f103/environment/thread_blink.hpp b/examples/blue_pill_f103/environment/thread_blink.hpp deleted file mode 100644 index b26753381b..0000000000 --- a/examples/blue_pill_f103/environment/thread_blink.hpp +++ /dev/null @@ -1,32 +0,0 @@ -/* - * Copyright (c) 2016, Sascha Schade - * - * 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/. - */ -// ---------------------------------------------------------------------------- - -#ifndef THREAD_BLINK -#define THREAD_BLINK - -#include -#include - -class BlinkThread: public modm::pt::Protothread -{ -public: - BlinkThread(); - - bool - update(); - -private: - modm::ShortTimeout timeout; - modm::ShortPeriodicTimer timer; - uint32_t uptime; -}; - -#endif // THREAD_BLINK diff --git a/examples/blue_pill_f103/environment/thread_bme280.cpp b/examples/blue_pill_f103/environment/thread_bme280.cpp index a9d46443fd..6e92cadb66 100644 --- a/examples/blue_pill_f103/environment/thread_bme280.cpp +++ b/examples/blue_pill_f103/environment/thread_bme280.cpp @@ -23,6 +23,7 @@ // ---------------------------------------------------------------------------- Bme280Thread::Bme280Thread() : + Fiber([this] { this->update(); }), barometerA(dataA, 0x76), barometerB(dataB, 0x76), start_measurement(false), @@ -38,11 +39,9 @@ Bme280Thread::startMeasurement() return true; } -bool +void Bme280Thread::update() { - PT_BEGIN() - MODM_LOG_DEBUG << MODM_FILE_INFO; MODM_LOG_DEBUG << "Ping the BME280" << modm::endl; @@ -50,21 +49,19 @@ Bme280Thread::update() while(true) { // we wait until the task started - if (PT_CALL(barometerA.ping())) + if (barometerA.ping()) break; // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(100ms); } while(true) { // we wait until the task started - if (PT_CALL(barometerB.ping())) + if (barometerB.ping()) break; // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(100ms); } // stream << "Barometer BME280 responded" << modm::endl; @@ -73,21 +70,19 @@ Bme280Thread::update() while(true) { // we wait until the task started - if (PT_CALL(barometerA.initialize())) + if (barometerA.initialize()) break; // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(100ms); } while(true) { // we wait until the task started - if (PT_CALL(barometerB.initialize())) + if (barometerB.initialize()) break; // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(100ms); } MODM_LOG_DEBUG << MODM_FILE_INFO; @@ -145,28 +140,24 @@ Bme280Thread::update() while (true) { - PT_WAIT_UNTIL(start_measurement); + modm::this_fiber::poll([&]{ return start_measurement; }); // Returns when new data was read from the sensor - PT_CALL(barometerA.readout()); - PT_CALL(barometerB.readout()); + barometerA.readout(); + barometerB.readout(); new_data = true; - { - int32_t temp = dataA.getTemperature(); - int32_t press = dataA.getPressure(); - int32_t hum = dataA.getHumidity(); - MODM_LOG_DEBUG << MODM_FILE_INFO; - MODM_LOG_DEBUG.printf("BME280: Calibrated temperature in 0.01 degree Celsius is: %" PRId32 "\n", temp ); - MODM_LOG_DEBUG << MODM_FILE_INFO; - MODM_LOG_DEBUG.printf("BME280: Calibrated pressure in mPa is : %" PRId32 "\n", press ); - MODM_LOG_DEBUG << MODM_FILE_INFO; - MODM_LOG_DEBUG.printf("BME280: Calibrated humidity in 0.001 %% is : %" PRId32 "\n", hum ); - } + int32_t temp = dataA.getTemperature(); + int32_t press = dataA.getPressure(); + int32_t hum = dataA.getHumidity(); + MODM_LOG_DEBUG << MODM_FILE_INFO; + MODM_LOG_DEBUG.printf("BME280: Calibrated temperature in 0.01 degree Celsius is: %" PRId32 "\n", temp ); + MODM_LOG_DEBUG << MODM_FILE_INFO; + MODM_LOG_DEBUG.printf("BME280: Calibrated pressure in mPa is : %" PRId32 "\n", press ); + MODM_LOG_DEBUG << MODM_FILE_INFO; + MODM_LOG_DEBUG.printf("BME280: Calibrated humidity in 0.001 %% is : %" PRId32 "\n", hum ); start_measurement = false; } - - PT_END(); } diff --git a/examples/blue_pill_f103/environment/thread_bme280.hpp b/examples/blue_pill_f103/environment/thread_bme280.hpp index 54315ede74..38890d49db 100644 --- a/examples/blue_pill_f103/environment/thread_bme280.hpp +++ b/examples/blue_pill_f103/environment/thread_bme280.hpp @@ -12,19 +12,18 @@ #ifndef THREAD_BME280 #define THREAD_BME280 -#include -#include +#include #include #include "hardware.hpp" -class Bme280Thread: public modm::pt::Protothread +class Bme280Thread: public modm::Fiber<> { public: Bme280Thread(); - bool + void update(); bool @@ -66,8 +65,6 @@ class Bme280Thread: public modm::pt::Protothread } private: - modm::ShortTimeout timeout; - modm::bme280::Data dataA; modm::Bme280 barometerA; diff --git a/examples/blue_pill_f103/environment/thread_bmp180.cpp b/examples/blue_pill_f103/environment/thread_bmp180.cpp index e9c30fb50b..81dc9d6937 100644 --- a/examples/blue_pill_f103/environment/thread_bmp180.cpp +++ b/examples/blue_pill_f103/environment/thread_bmp180.cpp @@ -23,6 +23,7 @@ // ---------------------------------------------------------------------------- Bmp180Thread::Bmp180Thread() : + Fiber([this] { this->update(); }), barometerA(dataA, 0x77), barometerB(dataB, 0x77), start_measurement(false), @@ -38,11 +39,9 @@ Bmp180Thread::startMeasurement() return true; } -bool +void Bmp180Thread::update() { - PT_BEGIN(); - MODM_LOG_DEBUG << MODM_FILE_INFO; MODM_LOG_DEBUG << "Ping the Barometer BMP180" << modm::endl; @@ -50,21 +49,19 @@ Bmp180Thread::update() while(true) { // we wait until the task started - if (PT_CALL(barometerA.ping())) + if (barometerA.ping()) break; // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(100ms); } while(true) { // we wait until the task started - if (PT_CALL(barometerB.ping())) + if (barometerB.ping()) break; // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(100ms); } MODM_LOG_DEBUG << MODM_FILE_INFO; @@ -74,21 +71,19 @@ Bmp180Thread::update() while(true) { // we wait until the task started - if (PT_CALL(barometerA.initialize())) + if (barometerA.initialize()) break; // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(100ms); } while(true) { // we wait until the task started - if (PT_CALL(barometerB.initialize())) + if (barometerB.initialize()) break; // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(100ms); } MODM_LOG_DEBUG << MODM_FILE_INFO; @@ -111,25 +106,23 @@ Bmp180Thread::update() while (true) { - PT_WAIT_UNTIL(start_measurement); + modm::this_fiber::poll([&]{ return start_measurement; }); // Returns when new data was read from the sensor - PT_CALL(barometerA.readout()); - PT_CALL(barometerB.readout()); + barometerA.readout(); + barometerB.readout(); new_data = true; - { - auto temp = dataA.getTemperature(); - auto press = dataA.getPressure(); + auto temp = dataA.getTemperature(); + auto press = dataA.getPressure(); - MODM_LOG_DEBUG << MODM_FILE_INFO; - MODM_LOG_DEBUG.printf("BMP180: Calibrated temperature in 0.1 degree Celsius is : %d\n", temp ); - MODM_LOG_DEBUG << MODM_FILE_INFO; - MODM_LOG_DEBUG.printf("BMP180: Calibrated pressure in Pa is : %" PRId32 "\n", press ); - } + MODM_LOG_DEBUG << MODM_FILE_INFO; + MODM_LOG_DEBUG.printf("BMP180: Calibrated temperature in 0.1 degree Celsius is : %d\n", temp ); + MODM_LOG_DEBUG << MODM_FILE_INFO; + MODM_LOG_DEBUG.printf("BMP180: Calibrated pressure in Pa is : %" PRId32 "\n", press ); start_measurement = false; } - PT_END(); + } diff --git a/examples/blue_pill_f103/environment/thread_bmp180.hpp b/examples/blue_pill_f103/environment/thread_bmp180.hpp index 0bddcb82fd..7c9bdd781c 100644 --- a/examples/blue_pill_f103/environment/thread_bmp180.hpp +++ b/examples/blue_pill_f103/environment/thread_bmp180.hpp @@ -12,19 +12,18 @@ #ifndef THREAD_BMP180 #define THREAD_BMP180 -#include -#include +#include #include #include "hardware.hpp" -class Bmp180Thread: public modm::pt::Protothread +class Bmp180Thread: public modm::Fiber<> { public: Bmp180Thread(); - bool + void update(); bool @@ -56,8 +55,6 @@ class Bmp180Thread: public modm::pt::Protothread } private: - modm::ShortTimeout timeout; - modm::bmp085::Data dataA; modm::Bmp085 barometerA; diff --git a/examples/blue_pill_f103/environment/threads.hpp b/examples/blue_pill_f103/environment/threads.hpp index c6a38440a2..fd52075f01 100644 --- a/examples/blue_pill_f103/environment/threads.hpp +++ b/examples/blue_pill_f103/environment/threads.hpp @@ -12,12 +12,11 @@ #ifndef THREADS #define THREADS -#include "thread_blink.hpp" #include "thread_bmp180.hpp" #include "thread_bme280.hpp" -extern BlinkThread blinkThread; extern Bmp180Thread bmp180Thread; extern Bme280Thread bme280Thread; #endif // THREADS + diff --git a/examples/blue_pill_f103/logger/main.cpp b/examples/blue_pill_f103/logger/main.cpp index 6fd55305b8..b3b3fc8e0b 100644 --- a/examples/blue_pill_f103/logger/main.cpp +++ b/examples/blue_pill_f103/logger/main.cpp @@ -14,8 +14,6 @@ #include #include -#include -#include // ---------------------------------------------------------------------------- // Set the log level @@ -32,46 +30,6 @@ modm::log::Logger modm::log::info(loggerDevice); modm::log::Logger modm::log::warning(loggerDevice); modm::log::Logger modm::log::error(loggerDevice); - -class BlinkThread : public modm::pt::Protothread -{ -public: - BlinkThread() - { - timeout.restart(100ms); - } - - bool - update() - { - PT_BEGIN(); - - while (true) - { - Board::LedGreen::reset(); - - PT_WAIT_UNTIL(timeout.isExpired()); - timeout.restart(100ms); - - Board::LedGreen::set(); - - PT_WAIT_UNTIL(timeout.isExpired()) ; - timeout.restart(900ms); - - MODM_LOG_INFO << "Seconds since reboot: " << ++uptime << modm::endl; - } - - PT_END(); - } - -private: - modm::ShortTimeout timeout; - uint32_t uptime; -}; - -BlinkThread blinkThread; - - // ---------------------------------------------------------------------------- int main() @@ -90,9 +48,16 @@ main() MODM_LOG_ERROR << "error" << modm::endl; + uint32_t uptime{}; while (true) { - blinkThread.update(); + Board::LedGreen::reset(); + modm::delay(100ms); + + Board::LedGreen::set(); + modm::delay(900ms); + + MODM_LOG_INFO << "Seconds since reboot: " << ++uptime << modm::endl; } return 0; diff --git a/examples/blue_pill_f103/logger/project.xml b/examples/blue_pill_f103/logger/project.xml index 04550dbc87..11c8de6645 100644 --- a/examples/blue_pill_f103/logger/project.xml +++ b/examples/blue_pill_f103/logger/project.xml @@ -8,8 +8,6 @@ modm:debug modm:platform:gpio modm:platform:uart:2 - modm:processing:timer - modm:processing:protothread modm:build:scons diff --git a/examples/blue_pill_f103/weight_scale_hx711/main.cpp b/examples/blue_pill_f103/weight_scale_hx711/main.cpp index 51edafd9db..b1622dd02f 100644 --- a/examples/blue_pill_f103/weight_scale_hx711/main.cpp +++ b/examples/blue_pill_f103/weight_scale_hx711/main.cpp @@ -11,8 +11,7 @@ #include #include -#include -#include +#include #include using namespace Board; @@ -40,54 +39,25 @@ struct hx711_config : public modm::hx711::Config // static const modm::hx711::InputChannelAndGain mode = modm::hx711::InputChannelAndGain::ChA_64; }; using Hx711 = modm::Hx711< hx711_config >; +Hx711 hx711; -class Hx711Thread : public modm::pt::Protothread +modm::Fiber fiber_hx711([] { -public: - bool - run() + while (true) { - PT_BEGIN(); - - while (true) - { - result = PT_CALL(hx711.singleConversion()); - MODM_LOG_DEBUG.printf("%" PRIi32 "\n", result); - } - - PT_END(); + const int32_t result = hx711.singleConversion(); + MODM_LOG_DEBUG.printf("%" PRIi32 "\n", result); } +}); -protected: - Hx711 hx711; - int32_t result; -}; - -Hx711Thread hx711_thread; - - -class BlinkThread : public modm::pt::Protothread +modm::Fiber fiber_blink([] { -public: - bool - run() + while (true) { - PT_BEGIN(); - - while (true) { - PT_WAIT_UNTIL(timer.execute()); - LedGreen::toggle(); - } - - PT_END(); + modm::this_fiber::sleep_for(1s); + LedGreen::toggle(); } - -protected: - modm::ShortPeriodicTimer timer{1s}; -}; - -BlinkThread blink_thread; - +}); /* * Blinks the green user LED with 1 Hz while measuring. @@ -106,18 +76,12 @@ main() Usart1::initialize(); // Use the logging streams to print some messages. - MODM_LOG_DEBUG << "HX711 demo" << modm::endl; + MODM_LOG_DEBUG << "HX711 demo" << modm::endl; hx711_config::Sck::setOutput(); hx711_config::Data::setInput(); - LedGreen::set(); - - while (true) - { - blink_thread.run(); - hx711_thread.run(); - } + modm::fiber::Scheduler::run(); return 0; } diff --git a/examples/blue_pill_f103/weight_scale_hx711/project.xml b/examples/blue_pill_f103/weight_scale_hx711/project.xml index 7858b9c500..6f3d9e0927 100644 --- a/examples/blue_pill_f103/weight_scale_hx711/project.xml +++ b/examples/blue_pill_f103/weight_scale_hx711/project.xml @@ -8,8 +8,7 @@ modm:debug modm:platform:gpio modm:platform:uart:1 - modm:processing:timer - modm:processing:protothread + modm:processing:fiber modm:build:scons modm:driver:hx711 diff --git a/examples/generic/i2c_multiplex/main.cpp b/examples/generic/i2c_multiplex/main.cpp index af93619dc3..804d05d2fe 100644 --- a/examples/generic/i2c_multiplex/main.cpp +++ b/examples/generic/i2c_multiplex/main.cpp @@ -8,8 +8,7 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ #include -#include -#include +#include #include #include #include @@ -54,32 +53,13 @@ namespace multiplexer using Ch7 = I2cMultiplexer::Ch7< i2cMultiplexer >; } +modm::I2cDevice dev0(0x29); +modm::I2cDevice dev1(0x29); +modm::I2cDevice dev2(0x29); +modm::I2cDevice dev3(0x29); -class DeviceThread: public modm::pt::Protothread +modm::Fiber fiber_ping([] { -public: - DeviceThread() : dev0(0x29), dev1(0x29), dev2(0x29), dev3(0x29) - {} - - bool - update(); - -private: - modm::ShortTimeout timeout; - - // Simple devices which are just pingable. - // Independent of real device. Any I2C device should be pingable at its address. - modm::I2cDevice dev0; - modm::I2cDevice dev1; - modm::I2cDevice dev2; - modm::I2cDevice dev3; -}; - -bool -DeviceThread::update() -{ - PT_BEGIN(); - MODM_LOG_DEBUG << MODM_FILE_INFO; MODM_LOG_DEBUG << "Ping the Devices" << modm::endl; @@ -87,20 +67,28 @@ DeviceThread::update() while(true) { MODM_LOG_DEBUG.printf("[dev ] ping0\n"); - MODM_LOG_DEBUG.printf("[dev ] ping0 res: %d\n", PT_CALL(dev0.ping())); + MODM_LOG_DEBUG.printf("[dev ] ping0 res: %d\n", dev0.ping()); MODM_LOG_DEBUG.printf("[dev ] ping1\n"); - MODM_LOG_DEBUG.printf("[dev ] ping1 res: %d\n", PT_CALL(dev1.ping())); + MODM_LOG_DEBUG.printf("[dev ] ping1 res: %d\n", dev1.ping()); MODM_LOG_DEBUG.printf("[dev ] ping2\n"); - MODM_LOG_DEBUG.printf("[dev ] ping2 res: %d\n", PT_CALL(dev2.ping())); + MODM_LOG_DEBUG.printf("[dev ] ping2 res: %d\n", dev2.ping()); MODM_LOG_DEBUG.printf("[dev ] ping3\n"); - MODM_LOG_DEBUG.printf("[dev ] ping3 res: %d\n", PT_CALL(dev3.ping())); + MODM_LOG_DEBUG.printf("[dev ] ping3 res: %d\n", dev3.ping()); // Do again in 1s - timeout.restart(1s); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(1s); } +}); - PT_END(); -} +modm::Fiber fiber_blink([] +{ + uint32_t counter{}; + while (true) + { + Board::Leds::toggle(); + MODM_LOG_INFO << "Loop counter: " << (counter++) << modm::endl; + modm::this_fiber::sleep_for(1s); + } +}); int main() @@ -115,22 +103,7 @@ main() modm::platform::I2cMaster1::connect(); modm::platform::I2cMaster1::initialize(); - constexpr std::chrono::milliseconds interval{1000}; - modm::ShortPeriodicTimer heartbeat{interval}; - - // Main loop - DeviceThread deviceThread; - - uint32_t counter(0); - while (true) - { - deviceThread.update(); - - if (heartbeat.execute()) { - Board::Leds::toggle(); - MODM_LOG_INFO << "Loop counter: " << (counter++) << modm::endl; - } - } + modm::fiber::Scheduler::run(); return 0; } diff --git a/examples/generic/i2c_multiplex/project.xml b/examples/generic/i2c_multiplex/project.xml index 46757ef9db..5df4462f19 100644 --- a/examples/generic/i2c_multiplex/project.xml +++ b/examples/generic/i2c_multiplex/project.xml @@ -1,9 +1,7 @@ - ../../../src/modm/board/nucleo_f303k8/board.xml + modm:nucleo-f303k8 - - @@ -13,9 +11,7 @@ :driver:pca9548a :platform:i2c:1 :platform:uart:2 - :processing:protothread - :processing:resumable - :processing:timer + :processing:fiber :build:scons diff --git a/examples/generic/resumable/main.cpp b/examples/generic/resumable/main.cpp deleted file mode 100644 index a8cfec5291..0000000000 --- a/examples/generic/resumable/main.cpp +++ /dev/null @@ -1,87 +0,0 @@ -/* - * Copyright (c) 2017, Sascha Schade - * - * 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/. - */ - -#include -#include -#include - -using Led = Board::LedGreen; - -using namespace std::chrono_literals; - -class BlinkingLight : public modm::pt::Protothread, private modm::NestedResumable<2> -{ -public: - bool - run() - { - PT_BEGIN(); - - // set everything up - Led::setOutput(); - Led::set(); - - while (true) - { - Led::set(); - PT_CALL(waitForTimer()); - - Led::reset(); - PT_CALL(setTimer(200ms)); - - PT_WAIT_UNTIL(timeout.isExpired()); - } - - PT_END(); - } - - modm::ResumableResult - waitForTimer() - { - RF_BEGIN(); - - // nested calling is allowed - if (RF_CALL(setTimer(100ms))) - { - RF_WAIT_UNTIL(timeout.isExpired()); - RF_RETURN(true); - } - - RF_END_RETURN(false); - } - - modm::ResumableResult - setTimer(std::chrono::milliseconds new_timeout) - { - RF_BEGIN(); - - timeout.restart(new_timeout); - - if(timeout.isArmed()) { - RF_RETURN(true); - } - - // clean up code goes here - - RF_END_RETURN(false); - } - -private: - modm::ShortTimeout timeout; -}; - -BlinkingLight light; - -int main(void) -{ - while (true) { - light.run(); - } -} diff --git a/examples/generic/resumable/project.xml b/examples/generic/resumable/project.xml deleted file mode 100644 index c04c67c333..0000000000 --- a/examples/generic/resumable/project.xml +++ /dev/null @@ -1,13 +0,0 @@ - - modm:nucleo-l476rg - - - - - modm:architecture:delay - modm:processing:protothread - modm:processing:resumable - modm:processing:timer - modm:build:scons - - diff --git a/examples/generic/ros/environment/main.cpp b/examples/generic/ros/environment/main.cpp index e27aea7187..bba80c7ea8 100644 --- a/examples/generic/ros/environment/main.cpp +++ b/examples/generic/ros/environment/main.cpp @@ -45,6 +45,14 @@ namespace ros } ros::ModmNodeHandle nh; +modm::Fiber fiber_ros([] +{ + while (true) + { + nh.spinOnce(); + modm::this_fiber::yield(); + } +}); sensor_msgs::Temperature temperature_msg; sensor_msgs::FluidPressure pressure_msg; @@ -54,6 +62,50 @@ ros::Publisher pub_temperature("/environment/temperature", &temperature_ms ros::Publisher pub_pressure ("/environment/pressure", &pressure_msg); ros::Publisher pub_humidity ("/environment/relative_humidity", &humidity_msg); +Bme280Thread bme280thread; +DisplayThread display_thread; + +modm::Fiber fiber_display([] +{ + modm::ShortPeriodicTimer timer(1s); + while(true) + { + timer.wait(); + bme280thread.startMeasurement(); + modm::this_fiber::poll([&]{ return bme280thread.isNewDataAvailable(); }); + + Board::LedGreen::toggle(); + int32_t temp = bme280thread.getTemperature(); + int32_t pres = bme280thread.getPressure(); + int32_t humi = bme280thread.getHumidity(); + + // Convert to standard ROS units. + temperature_msg.temperature = temp / 100.0; + temperature_msg.variance = 1.0; + temperature_msg.header.stamp = nh.now(); + + pressure_msg.fluid_pressure = pres / 1000.0; + pressure_msg.variance = 100; + pressure_msg.header.stamp = nh.now(); + + humidity_msg.relative_humidity = humi / 100000.0; + humidity_msg.variance = 0.03; + humidity_msg.header.stamp = nh.now(); + + pub_temperature.publish(&temperature_msg); + pub_pressure.publish(&pressure_msg); + pub_humidity.publish(&humidity_msg); + + display_thread.setSeq(temperature_msg.header.stamp.sec); + display_thread.setTemp(temp); + display_thread.setPres(pres); + display_thread.setHumi(humi); + + // Do not enable when STlink UART is used for rosserial + MODM_LOG_DEBUG.printf("Temp = %6.2f\n", temp / double(100.0)); + } +}); + // ---------------------------------------------------------------------------- int main() @@ -64,8 +116,8 @@ main() // Do not use it for logging because this will destroy ROS serial messages. Board::stlink::Uart::initialize(); - MyI2cMaster::connect(); - MyI2cMaster::initialize(); + MyI2cMaster::connect(); + MyI2cMaster::initialize(); Board::LedGreen::set(); @@ -75,59 +127,7 @@ main() nh.advertise(pub_pressure); nh.advertise(pub_humidity); - Bme280Thread bme280thread; - DisplayThread display_thread; - - modm::ShortPeriodicTimer timer(1s); - bool bme_sampling(false); - - while (true) - { - bme280thread.update(); - display_thread.update(); - - if (timer.execute()) - { - bme280thread.startMeasurement(); - bme_sampling = true; - } - - if (bme_sampling and bme280thread.isNewDataAvailable()) - { - bme_sampling = false; - Board::LedGreen::toggle(); - int32_t temp = bme280thread.getTemperature(); - int32_t pres = bme280thread.getPressure(); - int32_t humi = bme280thread.getHumidity(); - - // Convert to standard ROS units. - temperature_msg.temperature = temp / 100.0; - temperature_msg.variance = 1.0; - temperature_msg.header.stamp = nh.now(); - - pressure_msg.fluid_pressure = pres / 1000.0; - pressure_msg.variance = 100; - pressure_msg.header.stamp = nh.now(); - - humidity_msg.relative_humidity = humi / 100000.0; - humidity_msg.variance = 0.03; - humidity_msg.header.stamp = nh.now(); - - pub_temperature.publish(&temperature_msg); - pub_pressure.publish(&pressure_msg); - pub_humidity.publish(&humidity_msg); - - display_thread.setSeq(temperature_msg.header.stamp.sec); - display_thread.setTemp(temp); - display_thread.setPres(pres); - display_thread.setHumi(humi); - - // Do not enable when STlink UART is used for rosserial - MODM_LOG_DEBUG.printf("Temp = %6.2f\n", temp / double(100.0)); - } - - nh.spinOnce(); - } + modm::fiber::Scheduler::run(); return 0; } diff --git a/examples/generic/ros/environment/project.xml b/examples/generic/ros/environment/project.xml index db5f8c94a8..a7ce1829de 100644 --- a/examples/generic/ros/environment/project.xml +++ b/examples/generic/ros/environment/project.xml @@ -8,8 +8,7 @@ modm:driver:ssd1306 modm:ros modm:communication:ros - modm:processing:timer - modm:processing:protothread + modm:processing:fiber modm:platform:i2c:1 modm:build:scons modm:platform:heap diff --git a/examples/generic/ros/environment/thread_bme280.cpp b/examples/generic/ros/environment/thread_bme280.cpp index ea26af3086..274b2a197c 100644 --- a/examples/generic/ros/environment/thread_bme280.cpp +++ b/examples/generic/ros/environment/thread_bme280.cpp @@ -20,6 +20,7 @@ // ---------------------------------------------------------------------------- Bme280Thread::Bme280Thread() : + Fiber([this]{ this->update(); }), barometer(data, 0x76), start_measurement(false), new_data(false) @@ -34,11 +35,9 @@ Bme280Thread::startMeasurement() return true; } -bool +void Bme280Thread::update() { - PT_BEGIN(); - MODM_LOG_DEBUG << MODM_FILE_INFO; MODM_LOG_DEBUG << "Ping the BME280" << modm::endl; @@ -46,74 +45,66 @@ Bme280Thread::update() while(true) { // we wait until the task started - if (PT_CALL(barometer.ping())) + if (barometer.ping()) break; // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(100ms); } // Configure the device until it responds while(true) { // we wait until the task started - if (PT_CALL(barometer.initialize())) + if (barometer.initialize()) break; // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(100ms); } MODM_LOG_DEBUG << MODM_FILE_INFO; MODM_LOG_DEBUG << "BME280 configured" << modm::endl; - { - static modm::bme280::Calibration &cal = data.getCalibration(); + const modm::bme280::Calibration &cal = data.getCalibration(); - MODM_LOG_DEBUG << MODM_FILE_INFO; - MODM_LOG_DEBUG << "BME280 Calibration data is: " << modm::endl; - MODM_LOG_DEBUG.printf(" T1 %d\n", cal.T1); - MODM_LOG_DEBUG.printf(" T2 %d\n", cal.T2); - MODM_LOG_DEBUG.printf(" T3 %d\n", cal.T3); - MODM_LOG_DEBUG.printf(" P1 %d\n", cal.P1); - MODM_LOG_DEBUG.printf(" P2 %d\n", cal.P2); - MODM_LOG_DEBUG.printf(" P3 %d\n", cal.P3); - MODM_LOG_DEBUG.printf(" P4 %d\n", cal.P4); - MODM_LOG_DEBUG.printf(" P5 %d\n", cal.P5); - MODM_LOG_DEBUG.printf(" P6 %d\n", cal.P6); - MODM_LOG_DEBUG.printf(" P7 %d\n", cal.P7); - MODM_LOG_DEBUG.printf(" P8 %d\n", cal.P8); - MODM_LOG_DEBUG.printf(" P9 %d\n", cal.P9); - MODM_LOG_DEBUG.printf(" H1 %d\n", cal.H1); - MODM_LOG_DEBUG.printf(" H2 %d\n", cal.H2); - MODM_LOG_DEBUG.printf(" H3 %d\n", cal.H3); - MODM_LOG_DEBUG.printf(" H4 %d\n", cal.H4); - MODM_LOG_DEBUG.printf(" H5 %d\n", cal.H5); - MODM_LOG_DEBUG.printf(" H6 %d\n", cal.H6); - } + MODM_LOG_DEBUG << MODM_FILE_INFO; + MODM_LOG_DEBUG << "BME280 Calibration data is: " << modm::endl; + MODM_LOG_DEBUG.printf(" T1 %d\n", cal.T1); + MODM_LOG_DEBUG.printf(" T2 %d\n", cal.T2); + MODM_LOG_DEBUG.printf(" T3 %d\n", cal.T3); + MODM_LOG_DEBUG.printf(" P1 %d\n", cal.P1); + MODM_LOG_DEBUG.printf(" P2 %d\n", cal.P2); + MODM_LOG_DEBUG.printf(" P3 %d\n", cal.P3); + MODM_LOG_DEBUG.printf(" P4 %d\n", cal.P4); + MODM_LOG_DEBUG.printf(" P5 %d\n", cal.P5); + MODM_LOG_DEBUG.printf(" P6 %d\n", cal.P6); + MODM_LOG_DEBUG.printf(" P7 %d\n", cal.P7); + MODM_LOG_DEBUG.printf(" P8 %d\n", cal.P8); + MODM_LOG_DEBUG.printf(" P9 %d\n", cal.P9); + MODM_LOG_DEBUG.printf(" H1 %d\n", cal.H1); + MODM_LOG_DEBUG.printf(" H2 %d\n", cal.H2); + MODM_LOG_DEBUG.printf(" H3 %d\n", cal.H3); + MODM_LOG_DEBUG.printf(" H4 %d\n", cal.H4); + MODM_LOG_DEBUG.printf(" H5 %d\n", cal.H5); + MODM_LOG_DEBUG.printf(" H6 %d\n", cal.H6); while (true) { - PT_WAIT_UNTIL(start_measurement); + modm::this_fiber::poll([&]{ return start_measurement; }); // Returns when new data was read from the sensor - PT_CALL(barometer.readout()); + barometer.readout(); new_data = true; - { - int32_t temp = data.getTemperature(); - int32_t press = data.getPressure(); - int32_t hum = data.getHumidity(); - MODM_LOG_DEBUG << MODM_FILE_INFO; - MODM_LOG_DEBUG.printf("BME280: Calibrated temperature in 0.01 degree Celsius is: %" PRId32 "\n", temp ); - MODM_LOG_DEBUG << MODM_FILE_INFO; - MODM_LOG_DEBUG.printf("BME280: Calibrated pressure in mPa is : %" PRId32 "\n", press ); - MODM_LOG_DEBUG << MODM_FILE_INFO; - MODM_LOG_DEBUG.printf("BME280: Calibrated humidity in 0.001 %% is : %" PRId32 "\n", hum ); - } + int32_t temp = data.getTemperature(); + int32_t press = data.getPressure(); + int32_t hum = data.getHumidity(); + MODM_LOG_DEBUG << MODM_FILE_INFO; + MODM_LOG_DEBUG.printf("BME280: Calibrated temperature in 0.01 degree Celsius is: %" PRId32 "\n", temp ); + MODM_LOG_DEBUG << MODM_FILE_INFO; + MODM_LOG_DEBUG.printf("BME280: Calibrated pressure in mPa is : %" PRId32 "\n", press ); + MODM_LOG_DEBUG << MODM_FILE_INFO; + MODM_LOG_DEBUG.printf("BME280: Calibrated humidity in 0.001 %% is : %" PRId32 "\n", hum ); start_measurement = false; } - - PT_END(); } diff --git a/examples/generic/ros/environment/thread_bme280.hpp b/examples/generic/ros/environment/thread_bme280.hpp index 922c595ca6..1c2da163e0 100644 --- a/examples/generic/ros/environment/thread_bme280.hpp +++ b/examples/generic/ros/environment/thread_bme280.hpp @@ -11,19 +11,18 @@ #ifndef THREAD_BME280 #define THREAD_BME280 -#include -#include +#include #include #include "hardware.hpp" -class Bme280Thread: public modm::pt::Protothread +class Bme280Thread: public modm::Fiber<> { public: Bme280Thread(); - bool + void update(); bool @@ -50,8 +49,6 @@ class Bme280Thread: public modm::pt::Protothread } private: - modm::ShortTimeout timeout; - modm::bme280::Data data; modm::Bme280 barometer; diff --git a/examples/generic/ros/environment/thread_display.cpp b/examples/generic/ros/environment/thread_display.cpp index 1ebd624621..d96d565f9f 100644 --- a/examples/generic/ros/environment/thread_display.cpp +++ b/examples/generic/ros/environment/thread_display.cpp @@ -21,18 +21,16 @@ // ---------------------------------------------------------------------------- DisplayThread::DisplayThread() : + Fiber([this]{ this->update(); }), _dirty(true), _seq(0), _temp(0), _pres(0), _humi(0) { } -bool +void DisplayThread::update() { - PT_BEGIN(); - // Wait for 100 msec unitl display powered up. - boot_timeout.restart(100ms); - PT_WAIT_UNTIL(boot_timeout.isExpired()); + modm::this_fiber::sleep_for(100ms); display.initializeBlocking(); display.setFont(modm::font::Assertion); @@ -42,7 +40,7 @@ DisplayThread::update() while(true) { - PT_WAIT_UNTIL(_dirty); + modm::this_fiber::poll([&]{ return _dirty; }); display.clear(); @@ -54,6 +52,4 @@ DisplayThread::update() _dirty = false; } - - PT_END(); -} \ No newline at end of file +} diff --git a/examples/generic/ros/environment/thread_display.hpp b/examples/generic/ros/environment/thread_display.hpp index 15440cfa12..95de48f3f2 100644 --- a/examples/generic/ros/environment/thread_display.hpp +++ b/examples/generic/ros/environment/thread_display.hpp @@ -11,19 +11,18 @@ #ifndef THREAD_DISPLAY #define THREAD_DISPLAY -#include -#include +#include #include #include "hardware.hpp" -class DisplayThread: public modm::pt::Protothread +class DisplayThread: public modm::Fiber<> { public: DisplayThread(); - bool + void update(); bool @@ -56,7 +55,6 @@ class DisplayThread: public modm::pt::Protothread protected: modm::Ssd1306 display; - modm::ShortTimeout boot_timeout; bool _dirty; int32_t _seq; int32_t _temp; diff --git a/examples/nucleo_f401re/distance_vl53l0/main.cpp b/examples/nucleo_f401re/distance_vl53l0/main.cpp index 17505791d7..bf196a057d 100644 --- a/examples/nucleo_f401re/distance_vl53l0/main.cpp +++ b/examples/nucleo_f401re/distance_vl53l0/main.cpp @@ -14,7 +14,7 @@ #include #include -#include +#include #include #include @@ -45,112 +45,85 @@ modm::Vl53l0 distance(data); constexpr uint32_t MeasurementTimeFast = 30000; constexpr uint32_t MeasurementTimePrecision = 200000; -class ThreadOne : public modm::pt::Protothread +modm::Fiber fiber_name([] { -public: - bool - update() - { - PT_BEGIN(); + using namespace modm::this_fiber; + MODM_LOG_DEBUG << "Ping the device from ThreadOne" << modm::endl; - MODM_LOG_DEBUG << "Ping the device from ThreadOne" << modm::endl; + // ping the device until it responds - // ping the device until it responds - while (true) - { - // we wait until the device started - if (PT_CALL(distance.ping())) { - break; - } - - // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); - } + while(not distance.ping()) sleep_for(100ms); + MODM_LOG_DEBUG << "Device responded" << modm::endl; - MODM_LOG_DEBUG << "Device responded" << modm::endl; + while(not distance.initialize()) sleep_for(100ms); + MODM_LOG_DEBUG << "Device initialized" << modm::endl; - while (true) - { - if (PT_CALL(distance.initialize())) { - break; - } + // set measurement time to 200ms (high accuracy mode) + bool highAccuracyMode = distance.setMaxMeasurementTime(MeasurementTimePrecision); + if(not highAccuracyMode) { + MODM_LOG_DEBUG << "Setting measurement time failed" << modm::endl; + } - // otherwise, try again in 200ms - timeout.restart(200ms); - PT_WAIT_UNTIL(timeout.isExpired()); - } + modm::ShortPeriodicTimer timer(1s); - MODM_LOG_DEBUG << "Device initialized" << modm::endl; + while (true) + { + modm::Timestamp stamp = modm::Clock::now(); - // set measurement time to 200ms (high accuracy mode) - if(not PT_CALL(distance.setMaxMeasurementTime(MeasurementTimePrecision))) { - MODM_LOG_DEBUG << "Setting measurement time failed" << modm::endl; - } else { - this->highAccuracyMode = true; + if (distance.readDistance()) + { + modm::vl53l0::RangeErrorCode error = distance.getRangeError(); + if (distance.getData().isValid()) + { + uint16_t mm = distance.getData().getDistance(); + MODM_LOG_DEBUG << "mm: " << mm; + } + else + { + MODM_LOG_DEBUG << "Error: " << static_cast(error); + } } + MODM_LOG_DEBUG << ", t = " << (modm::Clock::now() - stamp) << "ms" << modm::endl; - timeout.restart(1s); - - while (true) + // query button state every 1s + if(timer.execute()) { - stamp = modm::Clock::now(); - - if (PT_CALL(distance.readDistance())) + // toggle between fast and high accuracy mode when button is pressed + if(Button::read()) { - modm::vl53l0::RangeErrorCode error = distance.getRangeError(); - if (distance.getData().isValid()) + if(highAccuracyMode) { - uint16_t mm = distance.getData().getDistance(); - MODM_LOG_DEBUG << "mm: " << mm; + if(distance.setMaxMeasurementTime(MeasurementTimeFast)) { + MODM_LOG_DEBUG << "Enable fast mode" << modm::endl; + highAccuracyMode = false; + } else { + MODM_LOG_DEBUG << "Setting measurement time failed" << modm::endl; + } } else { - MODM_LOG_DEBUG << "Error: " << static_cast(error); - } - } - MODM_LOG_DEBUG << ", t = " << (modm::Clock::now() - stamp) << "ms" << modm::endl; - - // query button state every 1s - if(timeout.isExpired()) - { - // toggle between fast and high accuracy mode when button is pressed - if(Button::read()) - { - if(this->highAccuracyMode) - { - if(PT_CALL(distance.setMaxMeasurementTime(MeasurementTimeFast))) { - MODM_LOG_DEBUG << "Enable fast mode" << modm::endl; - this->highAccuracyMode = false; - } else { - MODM_LOG_DEBUG << "Setting measurement time failed" << modm::endl; - } - } - else - { - if(PT_CALL(distance.setMaxMeasurementTime(MeasurementTimePrecision))) { - MODM_LOG_DEBUG << "Enable high accuracy mode" << modm::endl; - this->highAccuracyMode = true; - } else { - MODM_LOG_DEBUG << "Setting measurement time failed" << modm::endl; - } + if(distance.setMaxMeasurementTime(MeasurementTimePrecision)) { + MODM_LOG_DEBUG << "Enable high accuracy mode" << modm::endl; + highAccuracyMode = true; + } else { + MODM_LOG_DEBUG << "Setting measurement time failed" << modm::endl; } } - - timeout.restart(1s); } } - PT_END(); + sleep_for(100ms); } +}); -private: - modm::ShortTimeout timeout; - modm::Timestamp stamp; - bool highAccuracyMode = true; -}; - -ThreadOne one; +modm::Fiber fiber_blink([] +{ + while(true) + { + LedD13::toggle(); + modm::this_fiber::sleep_for(0.5s); + } +}); // ---------------------------------------------------------------------------- int @@ -164,16 +137,7 @@ main() MODM_LOG_INFO << "\n\nWelcome to VL53L0X demo!\n\n"; - modm::ShortPeriodicTimer tmr(500ms); - - while (true) - { - one.update(); - if(tmr.execute()) { - LedD13::toggle(); - } - - } + modm::fiber::Scheduler::run(); return 0; } diff --git a/examples/nucleo_f401re/distance_vl53l0/project.xml b/examples/nucleo_f401re/distance_vl53l0/project.xml index e06a51f19f..76b76481cc 100644 --- a/examples/nucleo_f401re/distance_vl53l0/project.xml +++ b/examples/nucleo_f401re/distance_vl53l0/project.xml @@ -8,7 +8,7 @@ modm:driver:vl53l0 modm:platform:gpio modm:platform:i2c:1 - modm:processing:protothread + modm:processing:fiber modm:build:scons diff --git a/examples/nucleo_f401re/dw3110-communication/main.cpp b/examples/nucleo_f401re/dw3110-communication/main.cpp index 7667be9aab..3b11b5c929 100644 --- a/examples/nucleo_f401re/dw3110-communication/main.cpp +++ b/examples/nucleo_f401re/dw3110-communication/main.cpp @@ -13,8 +13,7 @@ #include #include #include -#include -#include +#include using namespace Board; using namespace std::chrono_literals; @@ -23,101 +22,107 @@ using MySpiMaster = modm::platform::SpiMaster1; using MyDw3110_a = modm::Dw3110Phy; using MyDw3110_b = modm::Dw3110Phy; -class TXThread : public modm::pt::Protothread +class TXThread : public modm::Fiber<> { public: + TXThread() : Fiber([this]{ run(); }) {} + bool init() { - auto ret = RF_CALL_BLOCKING(radio.initialize( + auto ret = radio.initialize( modm::Dw3110::Channel::Channel9, modm::Dw3110::PreambleCode::Code_64Mhz_9, modm::Dw3110::PreambleLength::Preamble_128, - modm::Dw3110::StartFrameDelimiter::Decawave_8)); - RF_CALL_BLOCKING(radio.setEnableLongFrames(true)); + modm::Dw3110::StartFrameDelimiter::Decawave_8); + radio.setEnableLongFrames(true); return ret; } + size_t + getCount() + { + return sentCount; + } + +private: bool run() { - PT_BEGIN(); while (true) { txdata[txdata.size() - 1]++; - timeout.restart(Button::read() ? 500ms : 10ms); - PT_WAIT_UNTIL(timeout.execute()); - if (PT_CALL(radio.transmit(txdata, true)) == MyDw3110_b::Error::None) - { + modm::this_fiber::sleep_for(Button::read() ? 500ms : 10ms); + + if (radio.transmit(txdata, true) == MyDw3110_b::Error::None) { sentCount++; - } else - { + } else { MODM_LOG_DEBUG << "[TX] Failed to trasmit!" << modm::endl; } } - PT_END(); } - size_t - getCount() - { - return sentCount; - } - -private: MyDw3110_b radio{}; std::array txdata = {0xBA, 0xDE, 0xAF, 0xFE, 0x00}; modm::Timeout timeout{10ms}; size_t sentCount{0}; -}; +} tx; -class RXThread : public modm::pt::Protothread +class RXThread : public modm::Fiber<> { public: + RXThread() : Fiber([this]{ run(); }) {} + bool init() { - auto ret = RF_CALL_BLOCKING(radio.initialize( + auto ret = radio.initialize( modm::Dw3110::Channel::Channel9, modm::Dw3110::PreambleCode::Code_64Mhz_9, modm::Dw3110::PreambleLength::Preamble_128, - modm::Dw3110::StartFrameDelimiter::Decawave_8)); - RF_CALL_BLOCKING(radio.setEnableLongFrames(true)); + modm::Dw3110::StartFrameDelimiter::Decawave_8); + radio.setEnableLongFrames(true); return ret; } + size_t + getCount() + { + return recvCount; + } + +private: bool run() { - PT_BEGIN(); while (true) { - while (!PT_CALL(radio.packetReady())) + while (not radio.packetReady()) { - if (!PT_CALL(radio.isReceiving())) - { - // KEEP ON SEPERATE LINE - PT_CALL(radio.startReceive()); - } - PT_YIELD(); + if (not radio.isReceiving()) + radio.startReceive(); + modm::this_fiber::yield(); } - if (PT_CALL(radio.fetchPacket(rxdata, rxlen))) { recvCount++; } + if (radio.fetchPacket(rxdata, rxlen)) recvCount++; + modm::this_fiber::yield(); } - PT_END(); } - size_t - getCount() - { - return recvCount; - } - -private: constexpr static size_t RxBufferSize = 1021; // Maximum supported packet size MyDw3110_a radio{}; size_t rxlen{0}, recvCount{0}; std::array rxdata = {}; -}; +} rx; + +modm::Fiber fiber_report([] +{ + while(true) + { + modm::this_fiber::sleep_for(1s); + MODM_LOG_DEBUG << "Sent " << tx.getCount() << ", received " << rx.getCount() + << ". Diff:" << tx.getCount() - rx.getCount() << modm::endl; + } +}); int main() @@ -137,34 +142,20 @@ main() MODM_LOG_INFO << "Initializing Devices..." << modm::endl; bool success = true; - TXThread tx; - if (!tx.init()) + if (not tx.init()) { MODM_LOG_ERROR << "Failed to initialize TX Device!" << modm::endl; success = false; } - - RXThread rx; - if (!rx.init()) + if (not rx.init()) { MODM_LOG_ERROR << "Failed to initialize TR Device!" << modm::endl; success = false; } - if (!success) - while (true) { __NOP(); } + modm_assert(success, "user", "Failed to initialize devices!"); - modm::PeriodicTimer timer{1000ms}; MODM_LOG_INFO << "Starting ping pong..." << modm::endl; - while (true) - { - rx.run(); - tx.run(); - if (timer.execute()) - { - MODM_LOG_DEBUG << "Sent " << tx.getCount() << ", received " << rx.getCount() - << ". Diff:" << tx.getCount() - rx.getCount() << modm::endl; - } - } + modm::fiber::Scheduler::run(); return 0; } diff --git a/examples/nucleo_f401re/dw3110-communication/project.xml b/examples/nucleo_f401re/dw3110-communication/project.xml index c4d4d61ed7..be4d795176 100644 --- a/examples/nucleo_f401re/dw3110-communication/project.xml +++ b/examples/nucleo_f401re/dw3110-communication/project.xml @@ -7,6 +7,6 @@ modm:build:scons modm:driver:dw3110 modm:platform:spi:1 - modm:processing:protothread + modm:processing:fiber diff --git a/examples/nucleo_f411re/imu_bno055/main.cpp b/examples/nucleo_f411re/imu_bno055/main.cpp index 57a74788e9..212f647bec 100644 --- a/examples/nucleo_f411re/imu_bno055/main.cpp +++ b/examples/nucleo_f411re/imu_bno055/main.cpp @@ -39,81 +39,49 @@ using MyI2cMaster = I2cMaster1; modm::bno055::Data data; modm::Bno055 imu(data); -class ThreadOne : public modm::pt::Protothread +modm::Fiber fiber_bno055([] { -public: - bool - update() - { - PT_BEGIN(); - - MODM_LOG_DEBUG << "Ping the device from ThreadOne" << modm::endl; - - // ping the device until it responds - while (true) - { - // we wait until the device started - if (PT_CALL(imu.ping())) { - break; - } - PT_WAIT_UNTIL(timer.execute()); - } - - MODM_LOG_DEBUG << "Device responded" << modm::endl; - - while (true) - { - if (PT_CALL(imu.configure())) { - break; - } - - PT_WAIT_UNTIL(timer.execute()); - } + using namespace modm::this_fiber; + MODM_LOG_DEBUG << "Ping the device from ThreadOne" << modm::endl; - MODM_LOG_DEBUG << "Device configured" << modm::endl; + // ping the device until it responds + while(not imu.ping()) sleep_for(100ms); + MODM_LOG_DEBUG << "Device responded" << modm::endl; - while (true) - { - PT_WAIT_UNTIL(timer.execute()); - PT_CALL(imu.readData()); - MODM_LOG_INFO << (int)imu.getData().heading() << modm::endl; - } + while(not imu.configure()) sleep_for(100ms); + MODM_LOG_DEBUG << "Device configured" << modm::endl; - PT_END(); + while (true) + { + sleep_for(100ms); + imu.readData(); + MODM_LOG_INFO << (int)imu.getData().heading() << modm::endl; } +}); -private: - modm::ShortPeriodicTimer timer{100ms}; -}; - -ThreadOne one; +modm::Fiber fiber_blink([] +{ + LedD13::setOutput(); + while(true) + { + LedD13::toggle(); + modm::this_fiber::sleep_for(0.5s); + } +}); // ---------------------------------------------------------------------------- int main() { Board::initialize(); - LedD13::setOutput(); - // Board::D13::setOutput(modm::Gpio::Low); MyI2cMaster::connect(); MyI2cMaster::initialize(); MODM_LOG_INFO << "\n\nWelcome to BNO055 demo!\n\n" << modm::endl; - modm::ShortPeriodicTimer tmr(500ms); - - // Board::D15::setOutput(); - - while (true) - { - one.update(); - if(tmr.execute()) { - LedD13::toggle(); - // Board::D15::toggle(); - } - - } - + modm::fiber::Scheduler::run(); return 0; } + + diff --git a/examples/nucleo_f411re/imu_bno055/project.xml b/examples/nucleo_f411re/imu_bno055/project.xml index 146a01add6..7020753175 100644 --- a/examples/nucleo_f411re/imu_bno055/project.xml +++ b/examples/nucleo_f411re/imu_bno055/project.xml @@ -9,7 +9,7 @@ modm:platform:gpio modm:platform:i2c:1 modm:platform:i2c.bitbang - modm:processing:protothread + modm:processing:fiber modm:build:scons diff --git a/examples/nucleo_f429zi/pat9125el/main.cpp b/examples/nucleo_f429zi/pat9125el/main.cpp index 7a4ea3ea36..999a0e1897 100644 --- a/examples/nucleo_f429zi/pat9125el/main.cpp +++ b/examples/nucleo_f429zi/pat9125el/main.cpp @@ -10,9 +10,7 @@ // ---------------------------------------------------------------------------- #include - -#include -#include +#include #include using I2c = I2cMaster1; @@ -22,58 +20,8 @@ using Sda = GpioB9; // int pin is optional, set to void for polling mode using Int = GpioInputA5; -class Thread : public modm::pt::Protothread -{ -public: - Thread() : sensor{0x75} - { - } - - bool - update() - { - PT_BEGIN(); - - MODM_LOG_INFO << "Ping device" << modm::endl; - // ping the device until it responds - while(true) - { - if (PT_CALL(sensor.ping())) { - break; - } - // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); - } - MODM_LOG_INFO << "Ping successful" << modm::endl; - - // set x and y resolution - PT_CALL(sensor.configure(0x14, 0x14)); - - while (true) - { - PT_CALL(sensor.readData()); - if(sensor.hasMoved()) { - position += sensor.getData(); - - Board::Leds::write(0b111); - MODM_LOG_INFO << "Position: " << position.x << ", " << position.y << modm::endl; - sensor.resetMoved(); - } else { - Board::Leds::write(0b000); - } - } - - PT_END(); - } - -private: - modm::ShortTimeout timeout; - modm::pat9125el::Motion2D position; - modm::Pat9125el, Int> sensor; -}; - -Thread thread; +modm::pat9125el::Motion2D position; +modm::Pat9125el, Int> sensor(0x75); // ---------------------------------------------------------------------------- int @@ -87,8 +35,26 @@ main() I2c::connect(); I2c::initialize(); - while (true) { - thread.update(); + MODM_LOG_INFO << "Ping device" << modm::endl; + while(not sensor.ping()) modm::delay(100ms); + MODM_LOG_INFO << "Ping successful" << modm::endl; + + // set x and y resolution + sensor.configure(0x14, 0x14); + + while (true) + { + sensor.readData(); + if(sensor.hasMoved()) + { + position += sensor.getData(); + + Board::Leds::write(0b111); + MODM_LOG_INFO << "Position: " << position.x << ", " << position.y << modm::endl; + sensor.resetMoved(); + } + else Board::Leds::write(0b000); + modm::delay(10ms); } return 0; diff --git a/examples/nucleo_f429zi/pat9125el/project.xml b/examples/nucleo_f429zi/pat9125el/project.xml index e0cc9fc9f0..0c5b0ec9fc 100644 --- a/examples/nucleo_f429zi/pat9125el/project.xml +++ b/examples/nucleo_f429zi/pat9125el/project.xml @@ -7,8 +7,6 @@ modm:driver:pat9125el modm:io modm:platform:i2c:1 - modm:processing:protothread - modm:processing:timer modm:build:scons diff --git a/examples/nucleo_f429zi/rtc_mcp7941x/main.cpp b/examples/nucleo_f429zi/rtc_mcp7941x/main.cpp index adedeeb98d..58081e734d 100644 --- a/examples/nucleo_f429zi/rtc_mcp7941x/main.cpp +++ b/examples/nucleo_f429zi/rtc_mcp7941x/main.cpp @@ -11,116 +11,97 @@ #include #include -#include -#include -#include +#include using MyI2cMaster = modm::platform::I2cMaster1; using I2cScl = modm::platform::GpioB8; using I2cSda = modm::platform::GpioB9; +modm::Mcp7941x rtc{}; -class RtcThread : public modm::pt::Protothread +modm::Fiber fiber_name([] { -public: - bool - update() - { - PT_BEGIN(); + if(rtc.oscillatorRunning()) { + MODM_LOG_ERROR << "RTC oscillator is running." << modm::endl; + } + else { + MODM_LOG_ERROR << "RTC oscillator is NOT running." << modm::endl; + } - if(PT_CALL(rtc.oscillatorRunning())) { - MODM_LOG_ERROR << "RTC oscillator is running." << modm::endl; - } - else { - MODM_LOG_ERROR << "RTC oscillator is NOT running." << modm::endl; - } + MODM_LOG_INFO << "Setting date/time to 01.01.2020 00:00.00h" << modm::endl; + modm::mcp7941x::DateTime dateTime{}; + dateTime.days = 1; + dateTime.months = 1; + dateTime.years = 20; + dateTime.hours = 0; + dateTime.minutes = 0; + dateTime.seconds = 0; + while(not rtc.setDateTime(dateTime)) { + MODM_LOG_ERROR << "Unable to set date/time." << modm::endl; + modm::this_fiber::sleep_for(500ms); + } - MODM_LOG_INFO << "Setting date/time to 01.01.2020 00:00.00h" << modm::endl; - dateTime.days = 1; - dateTime.months = 1; - dateTime.years = 20; - dateTime.hours = 0; - dateTime.minutes = 0; - dateTime.seconds = 0; - while(not PT_CALL(rtc.setDateTime(dateTime))) { - MODM_LOG_ERROR << "Unable to set date/time." << modm::endl; - timeout.restart(500ms); - PT_WAIT_UNTIL(timeout.isExpired()); - } + modm::this_fiber::sleep_for(500ms); - timeout.restart(500ms); - PT_WAIT_UNTIL(timeout.isExpired()); + if(rtc.oscillatorRunning()) { + MODM_LOG_ERROR << "RTC oscillator is running." << modm::endl; + } + else { + MODM_LOG_ERROR << "RTC oscillator is NOT running." << modm::endl; + } - if(PT_CALL(rtc.oscillatorRunning())) { - MODM_LOG_ERROR << "RTC oscillator is running." << modm::endl; + while (true) + { + auto dateTime2 = rtc.getDateTime(); + if(dateTime2.has_value()) { + MODM_LOG_INFO.printf("%02u.%02u.%02u ", dateTime2->days, dateTime2->months, dateTime2->years); + MODM_LOG_INFO.printf("%02u:%02u.%02uh\n", dateTime2->hours, dateTime2->minutes, dateTime2->seconds); } else { - MODM_LOG_ERROR << "RTC oscillator is NOT running." << modm::endl; + MODM_LOG_ERROR << "Unable to read from RTC." << modm::endl; } - while (true) - { - dateTime2 = PT_CALL(rtc.getDateTime()); - if(dateTime2.has_value()) { - MODM_LOG_INFO.printf("%02u.%02u.%02u ", dateTime2->days, dateTime2->months, dateTime2->years); - MODM_LOG_INFO.printf("%02u:%02u.%02uh\n", dateTime2->hours, dateTime2->minutes, dateTime2->seconds); - } - else { - MODM_LOG_ERROR << "Unable to read from RTC." << modm::endl; - } - - timeout.restart(2500ms); - PT_WAIT_UNTIL(timeout.isExpired()); - } - - PT_END(); + modm::this_fiber::sleep_for(2.5s); } +}); -private: - modm::Mcp7941x rtc{}; - modm::mcp7941x::DateTime dateTime{}; - std::optional dateTime2{}; - modm::ShortTimeout timeout; -}; - - -using namespace Board; +modm::Fiber fiber_blink([] +{ + Board::LedGreen::setOutput(); + while(true) + { + Board::LedGreen::toggle(); + modm::this_fiber::sleep_for(0.5s); + } +}); int main() { Board::initialize(); - Leds::setOutput(); MyI2cMaster::connect(); - MyI2cMaster::initialize(); + MyI2cMaster::initialize(); MODM_LOG_INFO << "RTC MCP7941x Example on Nucleo-F429ZI" << modm::endl; modm::Mcp7941xEeprom eeprom{}; - if (auto data = RF_CALL_BLOCKING(eeprom.getUniqueId())) { - MODM_LOG_INFO << "Unique ID (EUI-48/64): "; - MODM_LOG_INFO << modm::hex << (*data)[0] << modm::ascii << ":"; - MODM_LOG_INFO << modm::hex << (*data)[1] << modm::ascii << ":"; - MODM_LOG_INFO << modm::hex << (*data)[2] << modm::ascii << ":"; - MODM_LOG_INFO << modm::hex << (*data)[3] << modm::ascii << ":"; - MODM_LOG_INFO << modm::hex << (*data)[4] << modm::ascii << ":"; - MODM_LOG_INFO << modm::hex << (*data)[5] << modm::ascii << ":"; - MODM_LOG_INFO << modm::hex << (*data)[6] << modm::ascii << ":"; - MODM_LOG_INFO << modm::hex << (*data)[7] << modm::ascii << modm::endl; + if (auto data = eeprom.getUniqueId()) { + MODM_LOG_INFO << "Unique ID (EUI-48/64): " << modm::hex; + MODM_LOG_INFO << (*data)[0] << ":"; + MODM_LOG_INFO << (*data)[1] << ":"; + MODM_LOG_INFO << (*data)[2] << ":"; + MODM_LOG_INFO << (*data)[3] << ":"; + MODM_LOG_INFO << (*data)[4] << ":"; + MODM_LOG_INFO << (*data)[5] << ":"; + MODM_LOG_INFO << (*data)[6] << ":"; + MODM_LOG_INFO << (*data)[7] << modm::endl; } else { MODM_LOG_ERROR << "Unable to read unique ID from RTC." << modm::endl; } modm::delay(500ms); - - RtcThread rtcThread; - - while (true) - { - LedGreen::toggle(); - rtcThread.update(); - } + modm::fiber::Scheduler::run(); return 0; } diff --git a/examples/nucleo_f429zi/rtc_mcp7941x/project.xml b/examples/nucleo_f429zi/rtc_mcp7941x/project.xml index 7369a82fa4..163da9620d 100644 --- a/examples/nucleo_f429zi/rtc_mcp7941x/project.xml +++ b/examples/nucleo_f429zi/rtc_mcp7941x/project.xml @@ -7,8 +7,7 @@ modm:driver:mcp7941x modm:io modm:platform:i2c:1 - modm:processing:protothread - modm:processing:timer + modm:processing:fiber modm:build:scons diff --git a/examples/nucleo_f429zi/spi_flash_fatfs/main.cpp b/examples/nucleo_f429zi/spi_flash_fatfs/main.cpp index 46dbe0e108..67466a5862 100644 --- a/examples/nucleo_f429zi/spi_flash_fatfs/main.cpp +++ b/examples/nucleo_f429zi/spi_flash_fatfs/main.cpp @@ -334,7 +334,5 @@ main() /* Check function/compatibility of the physical drive #0 */ modm::fiber::Scheduler::run(); - while (true); - return 0; } diff --git a/examples/nucleo_f429zi/spi_flash_fatfs/project.xml b/examples/nucleo_f429zi/spi_flash_fatfs/project.xml index ba04e1a17d..09f0b93edb 100644 --- a/examples/nucleo_f429zi/spi_flash_fatfs/project.xml +++ b/examples/nucleo_f429zi/spi_flash_fatfs/project.xml @@ -3,7 +3,6 @@ - modm:build:scons @@ -12,6 +11,5 @@ modm:platform:spi:1 modm:platform:gpio modm:processing:fiber - modm:processing:protothread diff --git a/examples/nucleo_f439zi/spi_dma/main.cpp b/examples/nucleo_f439zi/spi_dma/main.cpp index d548a15c2d..467cd1c6eb 100644 --- a/examples/nucleo_f439zi/spi_dma/main.cpp +++ b/examples/nucleo_f439zi/spi_dma/main.cpp @@ -10,7 +10,6 @@ // ---------------------------------------------------------------------------- #include -#include using Mosi = GpioOutputB5; using Miso = GpioInputB4; @@ -19,64 +18,41 @@ using DmaRx = Dma2::Channel0; using DmaTx = Dma2::Channel3; using Spi = SpiMaster1_Dma; -class SpiThread : public modm::pt::Protothread -{ -public: - - bool - update() - { - PT_BEGIN(); - - // Enable DMA controller - Dma2::enable(); - - // Enable and initialize SPI - Spi::connect(); - Spi::initialize(); +const uint8_t sendBuffer[13] {"data to send"}; +uint8_t receiveBuffer[13]; - while (true) - { - MODM_LOG_INFO << "sendBuffer adress: " << modm::hex << sendBuffer << modm::endl; - MODM_LOG_INFO << "receiveBuffer adress: " << modm::hex << receiveBuffer << modm::endl; - MODM_LOG_INFO << "Info: 0x20000000 is start of SRAM1" << modm::endl; - - MODM_LOG_INFO << "Before first transfer" << modm::endl; - - // send out 12 bytes, don't care about response - PT_CALL(Spi::transfer(sendBuffer, nullptr, 12)); +int main() +{ + Board::initialize(); - MODM_LOG_INFO << "After first transfer" << modm::endl; + MODM_LOG_INFO << "Hello from SPI-DMA example on Nucleo-F439ZI!" << modm::endl; - // send out 12 bytes, read in 12 bytes - PT_CALL(Spi::transfer(sendBuffer, receiveBuffer, 12)); + // Enable DMA controller + Dma2::enable(); - MODM_LOG_INFO << "After second transfer" << modm::endl << modm::endl; + // Enable and initialize SPI + Spi::connect(); + Spi::initialize(); - timeout.restart(500ms); - PT_WAIT_UNTIL(timeout.isExpired()); - } + while (true) + { + MODM_LOG_INFO << "sendBuffer adress: " << modm::hex << sendBuffer << modm::endl; + MODM_LOG_INFO << "receiveBuffer adress: " << modm::hex << receiveBuffer << modm::endl; + MODM_LOG_INFO << "Info: 0x20000000 is start of SRAM1" << modm::endl; - PT_END(); - } + MODM_LOG_INFO << "Before first transfer" << modm::endl; -private: - uint8_t sendBuffer[13] { "data to send" }; - uint8_t receiveBuffer[13]; - modm::ShortTimeout timeout; -}; + // send out 12 bytes, don't care about response + Spi::transfer(sendBuffer, nullptr, 12); -SpiThread spiThread; + MODM_LOG_INFO << "After first transfer" << modm::endl; -int main() -{ - Board::initialize(); + // send out 12 bytes, read in 12 bytes + Spi::transfer(sendBuffer, receiveBuffer, 12); - MODM_LOG_INFO << "Hello from SPI-DMA example on Nucleo-F439ZI!" << modm::endl; + MODM_LOG_INFO << "After second transfer" << modm::endl << modm::endl; - while (true) - { - spiThread.update(); + modm::delay(500ms); } return 0; diff --git a/examples/nucleo_f439zi/spi_dma/project.xml b/examples/nucleo_f439zi/spi_dma/project.xml index b8ca489c20..0bc180a4b3 100644 --- a/examples/nucleo_f439zi/spi_dma/project.xml +++ b/examples/nucleo_f439zi/spi_dma/project.xml @@ -7,8 +7,6 @@ modm:platform:gpio modm:platform:dma modm:platform:spi:1 - modm:processing:protothread - modm:processing:timer modm:build:scons diff --git a/examples/nucleo_f446re/color/main.cpp b/examples/nucleo_f446re/color/main.cpp index ac80a406ce..50aad8fca5 100644 --- a/examples/nucleo_f446re/color/main.cpp +++ b/examples/nucleo_f446re/color/main.cpp @@ -14,90 +14,56 @@ #include #include -class ThreadOne : public modm::pt::Protothread -{ -public: - bool - update() - { - PT_BEGIN(); - - MODM_LOG_INFO << "Ping the device from ThreadOne" << modm::endl; - - // ping the device until it responds - while (true) - { - // we wait until the task started - if (PT_CALL(sensor.ping())) { break; } - // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); - } +modm::tcs3472::Data data; +modm::Tcs3472 sensor{data}; - MODM_LOG_INFO << "Device responded" << modm::endl; - - while (true) - { - if (PT_CALL(sensor.initialize())) { break; } - // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); - } +modm::Fiber fiber_sensor([] +{ + MODM_LOG_INFO << "Ping the device from ThreadOne" << modm::endl; - MODM_LOG_INFO << "Device initialized" << modm::endl; + // ping the device until it responds + while (not sensor.ping()) modm::this_fiber::sleep_for(100ms); + MODM_LOG_INFO << "Device responded" << modm::endl; - while (true) - { - if (PT_CALL(sensor.configure(sensor.Gain::X4, sensor.IntegrationTime::MSEC_101))) - { - break; - } - // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); - } + while (not sensor.initialize()) modm::this_fiber::sleep_for(100ms); + MODM_LOG_INFO << "Device initialized" << modm::endl; - MODM_LOG_INFO << "Device configured" << modm::endl; + while (not sensor.configure(sensor.Gain::X4, sensor.IntegrationTime::MSEC_101)) + modm::this_fiber::sleep_for(100ms); + MODM_LOG_INFO << "Device configured" << modm::endl; - while (true) + while (true) + { + if (sensor.readColor()) { - if (PT_CALL(sensor.readColor())) - { - const auto rgb = data.getColor(); - MODM_LOG_INFO << "RGB: " << rgb << "\tHSV: " << modm::color::Hsv(rgb) << modm::endl; - } - timeout.restart(500ms); - PT_WAIT_UNTIL(timeout.isExpired()); + const auto rgb = data.getColor(); + MODM_LOG_INFO << "RGB: " << rgb << "\tHSV: " << modm::color::Hsv(rgb) << modm::endl; } - - PT_END(); + modm::this_fiber::sleep_for(500ms); } +}); -private: - modm::ShortTimeout timeout; - modm::tcs3472::Data data; - modm::Tcs3472 sensor{data}; -}; -ThreadOne one; +modm::Fiber fiber_blink([] +{ + Board::LedD13::setOutput(); + while(true) + { + Board::LedD13::toggle(); + modm::this_fiber::sleep_for(0.5s); + } +}); // ---------------------------------------------------------------------------- int main() { Board::initialize(); - Board::LedD13::setOutput(); I2cMaster1::connect(); I2cMaster1::initialize(); MODM_LOG_INFO << "\n\nWelcome to TCS3472 demo!\n\n"; - modm::ShortPeriodicTimer tmr(500ms); - while (true) - { - one.update(); - if (tmr.execute()) Board::LedD13::toggle(); - } - + modm::fiber::Scheduler::run(); return 0; } diff --git a/examples/nucleo_f446re/color/project.xml b/examples/nucleo_f446re/color/project.xml index b70f66949f..64c3bac354 100644 --- a/examples/nucleo_f446re/color/project.xml +++ b/examples/nucleo_f446re/color/project.xml @@ -6,8 +6,7 @@ modm:driver:tcs3472 modm:platform:i2c:1 - modm:processing:protothread - modm:processing:timer + modm:processing:fiber modm:build:scons diff --git a/examples/nucleo_f767zi/spi_dma/main.cpp b/examples/nucleo_f767zi/spi_dma/main.cpp index fb85e05bd7..1d88334157 100644 --- a/examples/nucleo_f767zi/spi_dma/main.cpp +++ b/examples/nucleo_f767zi/spi_dma/main.cpp @@ -10,7 +10,6 @@ // ---------------------------------------------------------------------------- #include -#include using Mosi = GpioOutputB5; using Miso = GpioInputB4; @@ -19,64 +18,41 @@ using DmaRx = Dma2::Channel0; using DmaTx = Dma2::Channel3; using Spi = SpiMaster1_Dma; -class SpiThread : public modm::pt::Protothread -{ -public: - - bool - update() - { - PT_BEGIN(); - - // Enable DMA controller - Dma2::enable(); - - // Enable and initialize SPI - Spi::connect(); - Spi::initialize(); +const uint8_t sendBuffer[13] {"data to send"}; +uint8_t receiveBuffer[13]; - while (true) - { - MODM_LOG_INFO << "sendBuffer adress: " << modm::hex << sendBuffer << modm::endl; - MODM_LOG_INFO << "receiveBuffer adress: " << modm::hex << receiveBuffer << modm::endl; - MODM_LOG_INFO << "Info: 0x20000000 is start of SRAM1" << modm::endl; - - MODM_LOG_INFO << "Before first transfer" << modm::endl; - - // send out 12 bytes, don't care about response - PT_CALL(Spi::transfer(sendBuffer, nullptr, 12)); +int main() +{ + Board::initialize(); - MODM_LOG_INFO << "After first transfer" << modm::endl; + MODM_LOG_INFO << "Hello from SPI-DMA example on Nucleo-F767ZI!" << modm::endl; - // send out 12 bytes, read in 12 bytes - PT_CALL(Spi::transfer(sendBuffer, receiveBuffer, 12)); + // Enable DMA controller + Dma2::enable(); - MODM_LOG_INFO << "After second transfer" << modm::endl << modm::endl; + // Enable and initialize SPI + Spi::connect(); + Spi::initialize(); - timeout.restart(500ms); - PT_WAIT_UNTIL(timeout.isExpired()); - } + while (true) + { + MODM_LOG_INFO << "sendBuffer adress: " << modm::hex << sendBuffer << modm::endl; + MODM_LOG_INFO << "receiveBuffer adress: " << modm::hex << receiveBuffer << modm::endl; + MODM_LOG_INFO << "Info: 0x20000000 is start of SRAM1" << modm::endl; - PT_END(); - } + MODM_LOG_INFO << "Before first transfer" << modm::endl; -private: - uint8_t sendBuffer[13] { "data to send" }; - uint8_t receiveBuffer[13]; - modm::ShortTimeout timeout; -}; + // send out 12 bytes, don't care about response + Spi::transfer(sendBuffer, nullptr, 12); -SpiThread spiThread; + MODM_LOG_INFO << "After first transfer" << modm::endl; -int main() -{ - Board::initialize(); + // send out 12 bytes, read in 12 bytes + Spi::transfer(sendBuffer, receiveBuffer, 12); - MODM_LOG_INFO << "Hello from SPI-DMA example on Nucleo-F767ZI!" << modm::endl; + MODM_LOG_INFO << "After second transfer" << modm::endl << modm::endl; - while (true) - { - spiThread.update(); + modm::delay(500ms); } return 0; diff --git a/examples/nucleo_f767zi/spi_dma/project.xml b/examples/nucleo_f767zi/spi_dma/project.xml index 0f59fd175c..3ac9129878 100644 --- a/examples/nucleo_f767zi/spi_dma/project.xml +++ b/examples/nucleo_f767zi/spi_dma/project.xml @@ -7,8 +7,6 @@ modm:platform:gpio modm:platform:dma modm:platform:spi:1 - modm:processing:protothread - modm:processing:timer modm:build:scons diff --git a/examples/nucleo_g071rb/amnb/main.cpp b/examples/nucleo_g071rb/amnb/main.cpp index 588db066e5..37feb5eadf 100644 --- a/examples/nucleo_g071rb/amnb/main.cpp +++ b/examples/nucleo_g071rb/amnb/main.cpp @@ -39,14 +39,14 @@ Action actions[] = {1, []() -> Response { static uint8_t counter{0}; - MODM_LOG_INFO << "Node1 and Node3 received Action 1" << modm::endl; + MODM_LOG_INFO << "Node1 or Node3 received Action 1" << modm::endl; return counter++; } }, {2, [](const uint32_t& data) -> Response { static uint8_t counter{0}; - MODM_LOG_INFO << "Node1 and Node3 received Action 2 with argument: " << data << modm::endl; + MODM_LOG_INFO << "Node1 or Node3 received Action 2 with argument: " << data << modm::endl; return ErrorResponse(counter++); } }, @@ -60,46 +60,48 @@ Node node1(device1, 1, actions); Node node2(device2, 2, listeners); Node node3(device3, 3, actions, listeners); +modm::Fiber<512> fiberNode1t([]{ node1.update_transmit(); }); +modm::Fiber<512> fiberNode1r([]{ node1.update_receive(); }); +modm::Fiber<512> fiberNode2t([]{ node2.update_transmit(); }); +modm::Fiber<512> fiberNode2r([]{ node2.update_receive(); }); +modm::Fiber<512> fiberNode3t([]{ node3.update_transmit(); }); +modm::Fiber<512> fiberNode3r([]{ node3.update_receive(); }); + // You need to connect D1 with D15 and with A0 using PinNode1 = GpioC4; // D1 using PinNode2 = GpioB8; // D15 using PinNode3 = GpioA0; // A0 -class Thread : public modm::pt::Protothread +modm::Fiber fiber_demo([] { - modm::ShortPeriodicTimer tmr{1s}; uint32_t counter{0}; - Result res1; - Result res2; - -public: - bool inline - update() + while(true) { - PT_BEGIN(); - - while(true) - { - PT_WAIT_UNTIL(tmr.execute()); + modm::this_fiber::sleep_for(1s); - node1.broadcast(1, counter++); - node3.broadcast(2); + node1.broadcast(1, counter++); + node3.broadcast(2); - res1 = PT_CALL(node2.request(1, 1)); - MODM_LOG_INFO << "Node1 responded with: " << res1.error(); - if (res1) { MODM_LOG_INFO << " " << *res1 << modm::endl; } + auto res1 = node2.request(1, 1); + MODM_LOG_INFO << "Node1 responded with: " << res1.error(); + if (res1) { MODM_LOG_INFO << " " << *res1 << modm::endl; } - res2 = PT_CALL(node1.request(3, 2, counter)); - MODM_LOG_INFO << "Node3 responded with: " << res2.error(); - if (res2.hasUserError()) { - MODM_LOG_INFO << " " << *res2.userError() << modm::endl; - } + auto res2 = node1.request(3, 2, counter); + MODM_LOG_INFO << "Node3 responded with: " << res2.error(); + if (res2.hasUserError()) { + MODM_LOG_INFO << " " << *res2.userError() << modm::endl; } - PT_END(); + if (counter % 10 == 0) + { + MODM_LOG_INFO << "Node1t stack=" << fiberNode1t.stack_usage() << "\nNode1r stack=" << fiberNode1r.stack_usage() << modm::endl; + MODM_LOG_INFO << "Node2t stack=" << fiberNode2t.stack_usage() << "\nNode2r stack=" << fiberNode2r.stack_usage() << modm::endl; + MODM_LOG_INFO << "Node3t stack=" << fiberNode3t.stack_usage() << "\nNode3r stack=" << fiberNode3r.stack_usage() << modm::endl; + } } -} -thread; +}); + + // ---------------------------------------------------------------------------- int @@ -135,16 +137,14 @@ main() USART4->CR3 = USART_CR3_HDSEL; USART4->CR1 |= USART_CR1_UE; - while (true) - { - node1.update_transmit(); - node1.update_receive(); - node2.update_transmit(); - node2.update_receive(); - node3.update_transmit(); - node3.update_receive(); - thread.update(); - } + fiberNode1t.stack_watermark(); + fiberNode1r.stack_watermark(); + fiberNode2t.stack_watermark(); + fiberNode2r.stack_watermark(); + fiberNode3t.stack_watermark(); + fiberNode3r.stack_watermark(); + + modm::fiber::Scheduler::run(); return 0; } diff --git a/examples/nucleo_g071rb/amnb/project.xml b/examples/nucleo_g071rb/amnb/project.xml index 158808e8a1..a65468a3cc 100644 --- a/examples/nucleo_g071rb/amnb/project.xml +++ b/examples/nucleo_g071rb/amnb/project.xml @@ -2,7 +2,7 @@ modm:nucleo-g071rb - + modm:platform:gpio @@ -11,6 +11,6 @@ modm:platform:uart:3 modm:platform:uart:4 modm:build:scons - modm:processing:protothread + modm:processing:fiber diff --git a/examples/nucleo_g071rb/amnb_fiber/main.cpp b/examples/nucleo_g071rb/amnb_fiber/main.cpp deleted file mode 100644 index 54f598900c..0000000000 --- a/examples/nucleo_g071rb/amnb_fiber/main.cpp +++ /dev/null @@ -1,166 +0,0 @@ -/* - * Copyright (c) 2019, Niklas Hauser - * - * 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/. - */ - -#include -#include -#include - -using namespace Board; -using namespace std::chrono_literals; -using namespace modm::amnb; -using Usart1 = BufferedUart>; -using Usart3 = BufferedUart>; -using Usart4 = BufferedUart; -// ---------------------------------------------------------------------------- - -Listener listeners[] = -{ - {1, [](uint8_t sender, const uint32_t& data) - { - MODM_LOG_INFO << "Node2 and Node3 received Broadcast 1 from '" << sender; - MODM_LOG_INFO << "': " << data << modm::endl; - } - }, - {2, [](uint8_t sender) - { - MODM_LOG_INFO << "Node2 and Node3 received Broadcast 2 from '" << sender << "'" << modm::endl; - } - }, -}; -Action actions[] = -{ - {1, []() -> 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 - { - static uint8_t counter{0}; - MODM_LOG_INFO << "Node1 or Node3 received Action 2 with argument: " << data << modm::endl; - return ErrorResponse(counter++); - } - }, -}; - -// Two nodes on the same device on different UARTs of course! -DeviceWrapper device1; -DeviceWrapper device2; -DeviceWrapper device3; -Node node1(device1, 1, actions); -Node node2(device2, 2, listeners); -Node node3(device3, 3, actions, listeners); - -modm::Fiber fiberNode1t([]{ node1.update_transmit(); }); -modm::Fiber fiberNode1r([]{ node1.update_receive(); }); -modm::Fiber fiberNode2t([]{ node2.update_transmit(); }); -modm::Fiber fiberNode2r([]{ node2.update_receive(); }); -modm::Fiber fiberNode3t([]{ node3.update_transmit(); }); -modm::Fiber fiberNode3r([]{ node3.update_receive(); }); - -// You need to connect D1 with D15 and with A0 -using PinNode1 = GpioC4; // D1 -using PinNode2 = GpioB8; // D15 -using PinNode3 = GpioA0; // A0 - -class Thread : public modm::pt::Protothread -{ - modm::ShortPeriodicTimer tmr{1s}; - uint32_t counter{0}; - Result res1; - Result res2; - -public: - bool inline - update() - { - PT_BEGIN(); - - while(true) - { - PT_WAIT_UNTIL(tmr.execute()); - - node1.broadcast(1, counter++); - node3.broadcast(2); - - res1 = PT_CALL(node2.request(1, 1)); - MODM_LOG_INFO << "Node1 responded with: " << res1.error(); - if (res1) { MODM_LOG_INFO << " " << *res1 << modm::endl; } - - res2 = PT_CALL(node1.request(3, 2, counter)); - MODM_LOG_INFO << "Node3 responded with: " << res2.error(); - if (res2.hasUserError()) { - MODM_LOG_INFO << " " << *res2.userError() << modm::endl; - } - - if (counter % 10 == 0) - { - MODM_LOG_INFO << "Node1t stack=" << fiberNode1t.stack_usage() << "\nNode1r stack=" << fiberNode1r.stack_usage() << modm::endl; - MODM_LOG_INFO << "Node2t stack=" << fiberNode2t.stack_usage() << "\nNode2r stack=" << fiberNode2r.stack_usage() << modm::endl; - MODM_LOG_INFO << "Node3t stack=" << fiberNode3t.stack_usage() << "\nNode3r stack=" << fiberNode3r.stack_usage() << modm::endl; - MODM_LOG_INFO << "Thread stack=" << this->stack_usage() << modm::endl; - } - } - - PT_END(); - } -} -thread; - - - -// ---------------------------------------------------------------------------- -int -main() -{ - Board::initialize(); - LedD13::setOutput(); - - Usart1::connect(); - Usart1::initialize(Usart1::Parity::Even, Usart1::WordLength::Bit9); - // Use Single-Wire Half-Duplex Mode - PinNode1::configure(Gpio::OutputType::OpenDrain); - PinNode1::configure(Gpio::InputType::PullUp); - USART1->CR1 &= ~USART_CR1_UE; - USART1->CR3 = USART_CR3_HDSEL; - USART1->CR1 |= USART_CR1_UE; - - Usart3::connect(); - Usart3::initialize(Usart1::Parity::Even, Usart1::WordLength::Bit9); - // Use Single-Wire Half-Duplex Mode - PinNode2::configure(Gpio::OutputType::OpenDrain); - PinNode2::configure(Gpio::InputType::PullUp); - USART3->CR1 &= ~USART_CR1_UE; - USART3->CR3 = USART_CR3_HDSEL; - USART3->CR1 |= USART_CR1_UE; - - Usart4::connect(); - Usart4::initialize(Usart1::Parity::Even, Usart1::WordLength::Bit9); - // Use Single-Wire Half-Duplex Mode - PinNode3::configure(Gpio::OutputType::OpenDrain); - PinNode3::configure(Gpio::InputType::PullUp); - USART4->CR1 &= ~USART_CR1_UE; - USART4->CR3 = USART_CR3_HDSEL; - USART4->CR1 |= USART_CR1_UE; - - fiberNode1t.stack_watermark(); - fiberNode1r.stack_watermark(); - fiberNode2t.stack_watermark(); - fiberNode2r.stack_watermark(); - fiberNode3t.stack_watermark(); - fiberNode3r.stack_watermark(); - thread.stack_watermark(); - - modm::fiber::Scheduler::run(); - - return 0; -} diff --git a/examples/nucleo_g071rb/amnb_fiber/project.xml b/examples/nucleo_g071rb/amnb_fiber/project.xml deleted file mode 100644 index 0c759874cf..0000000000 --- a/examples/nucleo_g071rb/amnb_fiber/project.xml +++ /dev/null @@ -1,18 +0,0 @@ - - modm:nucleo-g071rb - - - - - - - modm:platform:gpio - modm:communication:amnb - modm:platform:uart:1 - modm:platform:uart:3 - modm:platform:uart:4 - modm:build:scons - modm:processing:protothread - modm:processing:fiber - - diff --git a/examples/nucleo_g474re/ads101x/main.cpp b/examples/nucleo_g474re/ads101x/main.cpp index 74c6111e26..b2619def1f 100644 --- a/examples/nucleo_g474re/ads101x/main.cpp +++ b/examples/nucleo_g474re/ads101x/main.cpp @@ -18,162 +18,115 @@ using Scl = modm::platform::GpioC6; using Sda = modm::platform::GpioC7; using I2cMaster = modm::platform::I2cMaster4; -using namespace Board; +modm::ads101x::Data data; +modm::Ads101x adc{data}; -class AdcThread : public modm::pt::Protothread +modm::Fiber fiber_sensor([] { - -public: - AdcThread() : adc(data) - { - } - - inline bool - run() - { - PT_BEGIN(); - - // test of communication and initialization - while (not PT_CALL(adc.ping())) - { - MODM_LOG_ERROR << "Pinging Ads101x failed" << modm::endl; - - shortTimeout.restart(std::chrono::milliseconds(1000)); - PT_WAIT_UNTIL(shortTimeout.isExpired()); - } - - while (not PT_CALL(adc.initialize())) - { - MODM_LOG_ERROR << "Initialising Ads101x failed" << modm::endl; - - shortTimeout.restart(std::chrono::milliseconds(1000)); - PT_WAIT_UNTIL(shortTimeout.isExpired()); - } - - MODM_LOG_INFO << "Ads101x successfully initialised" << modm::endl; - - while (true) - { - // Single shot and full scale range test - MODM_LOG_INFO << "Started single conversion test" << modm::endl; - timeout.restart(std::chrono::seconds(10)); - while (not timeout.isExpired()) - { - // Test single shoot mode with full scale range 0.256 - PT_CALL(adc.setFullScaleRange(modm::ads101x::FullScaleRange::V0_256)); - PT_CALL(adc.startSingleShotConversion(modm::ads101x::InputMultiplexer::Input4)); - while (PT_CALL(adc.isBusy())) - { - PT_YIELD(); - } - - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO << "Single Conversion result in range 0.256V: " << data.getVoltage() << modm::endl; - - // Test single shoot mode with full scale range 0.512 - PT_CALL(adc.setFullScaleRange(modm::ads101x::FullScaleRange::V0_512)); - PT_CALL(adc.startSingleShotConversion(modm::ads101x::InputMultiplexer::Input4)); - while (PT_CALL(adc.isBusy())) - { - PT_YIELD(); - } - - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO << "Single Conversion result in range 0.512V: " << data.getVoltage() << modm::endl; - - // Test single shoot mode with full scale range 1.024 - PT_CALL(adc.setFullScaleRange(modm::ads101x::FullScaleRange::V1_024)); - PT_CALL(adc.startSingleShotConversion(modm::ads101x::InputMultiplexer::Input4)); - while (PT_CALL(adc.isBusy())) - { - PT_YIELD(); - } - - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO << "Single Conversion result in range 1.024V: " << data.getVoltage() << modm::endl; - - // Test single shoot mode with full scale range 2.048 - PT_CALL(adc.setFullScaleRange(modm::ads101x::FullScaleRange::V2_048)); - PT_CALL(adc.startSingleShotConversion(modm::ads101x::InputMultiplexer::Input4)); - while (PT_CALL(adc.isBusy())) - { - PT_YIELD(); - } - - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO << "Single Conversion result in range 2.048V: " << data.getVoltage() << modm::endl; - - // Test single shoot mode with full scale range 4.096 - PT_CALL(adc.setFullScaleRange(modm::ads101x::FullScaleRange::V4_096)); - PT_CALL(adc.startSingleShotConversion(modm::ads101x::InputMultiplexer::Input4)); - while (PT_CALL(adc.isBusy())) - { - PT_YIELD(); - } - - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO << "Single Conversion result in range 4.096V: " << data.getVoltage() << modm::endl; - - // Test single shoot mode with full scale range 6.144 - PT_CALL(adc.setFullScaleRange(modm::ads101x::FullScaleRange::V6_144)); - PT_CALL(adc.startSingleShotConversion(modm::ads101x::InputMultiplexer::Input4)); - while (PT_CALL(adc.isBusy())) - { - PT_YIELD(); - } - - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO << "Single Conversion result in range 6.144V: " << data.getVoltage() << modm::endl; - - shortTimeout.restart(std::chrono::milliseconds(1000)); - PT_WAIT_UNTIL(shortTimeout.isExpired()); - } - - // Continuous mode test - MODM_LOG_INFO << "Started continuous conversion test" << modm::endl; - PT_CALL(adc.setFullScaleRange(modm::ads101x::FullScaleRange::V6_144)); - PT_CALL(adc.startContinuousConversion(modm::ads101x::InputMultiplexer::Input4)); - - timeout.restart(std::chrono::seconds(10)); - while (not timeout.isExpired()) - { - shortTimeout.restart(std::chrono::milliseconds(1000)); - PT_WAIT_UNTIL(shortTimeout.isExpired()); - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO << "Continuous Conversion result in range 6.144V: " << data.getVoltage() << modm::endl; - } - } - PT_END(); - } - -private: - modm::ads101x::Data data; - modm::Ads101x adc; - - modm::Timeout timeout; - modm::ShortTimeout shortTimeout; - -} adcThread; + // test of communication and initialization + while (not adc.ping()) + { + MODM_LOG_ERROR << "Pinging Ads101x failed" << modm::endl; + modm::this_fiber::sleep_for(1s); + } + + while (not adc.initialize()) + { + MODM_LOG_ERROR << "Initialising Ads101x failed" << modm::endl; + modm::this_fiber::sleep_for(1s); + } + + MODM_LOG_INFO << "Ads101x successfully initialised" << modm::endl; + + while (true) + { + // Single shot and full scale range test + MODM_LOG_INFO << "Started single conversion test" << modm::endl; + + modm::ShortTimeout timeout(10s); + while(not timeout.isExpired()) + { + // Test single shoot mode with full scale range 0.256 + adc.setFullScaleRange(modm::ads101x::FullScaleRange::V0_256); + adc.startSingleShotConversion(modm::ads101x::InputMultiplexer::Input4); + while (adc.isBusy()) modm::this_fiber::yield(); + adc.readConversionResult(); + MODM_LOG_INFO << "Single Conversion result in range 0.256V: " << data.getVoltage() << modm::endl; + + // Test single shoot mode with full scale range 0.512 + adc.setFullScaleRange(modm::ads101x::FullScaleRange::V0_512); + adc.startSingleShotConversion(modm::ads101x::InputMultiplexer::Input4); + while (adc.isBusy()) modm::this_fiber::yield(); + adc.readConversionResult(); + MODM_LOG_INFO << "Single Conversion result in range 0.512V: " << data.getVoltage() << modm::endl; + + // Test single shoot mode with full scale range 1.024 + adc.setFullScaleRange(modm::ads101x::FullScaleRange::V1_024); + adc.startSingleShotConversion(modm::ads101x::InputMultiplexer::Input4); + while (adc.isBusy()) modm::this_fiber::yield(); + adc.readConversionResult(); + MODM_LOG_INFO << "Single Conversion result in range 1.024V: " << data.getVoltage() << modm::endl; + + // Test single shoot mode with full scale range 2.048 + adc.setFullScaleRange(modm::ads101x::FullScaleRange::V2_048); + adc.startSingleShotConversion(modm::ads101x::InputMultiplexer::Input4); + while (adc.isBusy()) modm::this_fiber::yield(); + adc.readConversionResult(); + MODM_LOG_INFO << "Single Conversion result in range 2.048V: " << data.getVoltage() << modm::endl; + + // Test single shoot mode with full scale range 4.096 + adc.setFullScaleRange(modm::ads101x::FullScaleRange::V4_096); + adc.startSingleShotConversion(modm::ads101x::InputMultiplexer::Input4); + while (adc.isBusy()) modm::this_fiber::yield(); + adc.readConversionResult(); + MODM_LOG_INFO << "Single Conversion result in range 4.096V: " << data.getVoltage() << modm::endl; + + // Test single shoot mode with full scale range 6.144 + adc.setFullScaleRange(modm::ads101x::FullScaleRange::V6_144); + adc.startSingleShotConversion(modm::ads101x::InputMultiplexer::Input4); + while (adc.isBusy()) modm::this_fiber::yield(); + adc.readConversionResult(); + MODM_LOG_INFO << "Single Conversion result in range 6.144V: " << data.getVoltage() << modm::endl; + + modm::this_fiber::sleep_for(1s); + } + + // Continuous mode test + MODM_LOG_INFO << "Started continuous conversion test" << modm::endl; + adc.setFullScaleRange(modm::ads101x::FullScaleRange::V6_144); + adc.startContinuousConversion(modm::ads101x::InputMultiplexer::Input4); + + timeout.restart(10s); + while (not timeout.isExpired()) + { + modm::this_fiber::sleep_for(1s); + adc.readConversionResult(); + MODM_LOG_INFO << "Continuous Conversion result in range 6.144V: " << data.getVoltage() << modm::endl; + } + } +}); + +modm::Fiber fiber_blink([] +{ + Board::LedD13::setOutput(); + while(true) + { + Board::LedD13::toggle(); + modm::this_fiber::sleep_for(0.5s); + } +}); int main() { - Board::initialize(); + Board::initialize(); - I2cMaster::connect(I2cMaster::PullUps::Internal); - I2cMaster::initialize(); + I2cMaster::connect(I2cMaster::PullUps::Internal); + I2cMaster::initialize(); - MODM_LOG_INFO << "==========ADS101x Test==========" << modm::endl; - MODM_LOG_DEBUG << "Debug logging here" << modm::endl; - MODM_LOG_INFO << "Info logging here" << modm::endl; - MODM_LOG_WARNING << "Warning logging here" << modm::endl; - MODM_LOG_ERROR << "Error logging here" << modm::endl; - MODM_LOG_INFO << "===============================" << modm::endl; + MODM_LOG_INFO << "==========ADS101x Test==========" << modm::endl; - while (true) - { - adcThread.run(); - } + modm::fiber::Scheduler::run(); - return 0; -} \ No newline at end of file + return 0; +} diff --git a/examples/nucleo_g474re/ads101x/project.xml b/examples/nucleo_g474re/ads101x/project.xml index 790f897315..74ad5e3114 100644 --- a/examples/nucleo_g474re/ads101x/project.xml +++ b/examples/nucleo_g474re/ads101x/project.xml @@ -7,8 +7,8 @@ modm:driver:ads101x modm:platform:gpio modm:platform:i2c:4 - modm:processing:protothread + modm:processing:fiber modm:processing:timer modm:build:scons - \ No newline at end of file + diff --git a/examples/nucleo_g474re/ads7828/main.cpp b/examples/nucleo_g474re/ads7828/main.cpp index 6cdccd7f6a..cc74240e9a 100644 --- a/examples/nucleo_g474re/ads7828/main.cpp +++ b/examples/nucleo_g474re/ads7828/main.cpp @@ -19,174 +19,154 @@ using Scl = modm::platform::GpioC6; using Sda = modm::platform::GpioC7; using I2cMaster = modm::platform::I2cMaster4; -using namespace Board; +modm::ads7828::Data data; +modm::Ads7828 adc{data, 0x48}; -class AdcThread : public modm::pt::Protothread +modm::Fiber fiber_sensor([] { - -public: - AdcThread() : adc(data, 0x48) - { - } - - inline bool - run() - { - PT_BEGIN(); - - while (PT_CALL(adc.ping()) == false) - { - MODM_LOG_ERROR << "Could not ping Ads7828" << modm::endl; - - timeout.restart(std::chrono::milliseconds(1000)); - PT_WAIT_UNTIL(timeout.isExpired()); - } - - while (true) - { - MODM_LOG_INFO << "-------------------------------" << modm::endl << modm::endl; - PT_CALL(adc.startMeasurement(modm::ads7828::InputChannel::Ch0)); - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO.printf("Ch0 measuremnt is \t %.4f", (double)data.getVoltage(3.3f)); - MODM_LOG_INFO << modm::endl; - - PT_CALL(adc.startMeasurement(modm::ads7828::InputChannel::Ch1)); - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO.printf("Ch1 measuremnt is \t %.4f", (double)data.getVoltage(3.3f)); - MODM_LOG_INFO << modm::endl; - - PT_CALL(adc.startMeasurement(modm::ads7828::InputChannel::Ch2)); - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO.printf("Ch2 measuremnt is \t %.4f", (double)data.getVoltage(3.3f)); - MODM_LOG_INFO << modm::endl; - - PT_CALL(adc.startMeasurement(modm::ads7828::InputChannel::Ch3)); - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO.printf("Ch3 measuremnt is \t %.4f", (double)data.getVoltage(3.3f)); - MODM_LOG_INFO << modm::endl; - - PT_CALL(adc.startMeasurement(modm::ads7828::InputChannel::Ch4)); - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO.printf("Ch4 measuremnt is \t %.4f", (double)data.getVoltage(3.3f)); - MODM_LOG_INFO << modm::endl; - - PT_CALL(adc.startMeasurement(modm::ads7828::InputChannel::Ch5)); - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO.printf("Ch5 measuremnt is \t %.4f", (double)data.getVoltage(3.3f)); - MODM_LOG_INFO << modm::endl; - - PT_CALL(adc.startMeasurement(modm::ads7828::InputChannel::Ch6)); - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO.printf("Ch6 measuremnt is \t %.4f", (double)data.getVoltage(3.3f)); - MODM_LOG_INFO << modm::endl; - - PT_CALL(adc.startMeasurement(modm::ads7828::InputChannel::Ch7)); - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO.printf("Ch7 measuremnt is \t %.4f", (double)data.getVoltage(3.3f)); - MODM_LOG_INFO << modm::endl; - - MODM_LOG_INFO << "----Diff Inputs-------------" << modm::endl; - - PT_CALL(adc.startMeasurement(modm::ads7828::InputChannel::Ch0Ch1)); - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO.printf("Ch0 - Ch1 is \t %.4f", (double)data.getVoltage(3.3f)); - MODM_LOG_INFO << modm::endl; - - PT_CALL(adc.startMeasurement(modm::ads7828::InputChannel::Ch2Ch3)); - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO.printf("Ch2 - Ch3 is\t %.4f", (double)data.getVoltage(3.3f)); - MODM_LOG_INFO << modm::endl; - - PT_CALL(adc.startMeasurement(modm::ads7828::InputChannel::Ch4Ch5)); - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO.printf("Ch4 - Ch5 is \t %.4f", (double)data.getVoltage(3.3f)); - MODM_LOG_INFO << modm::endl; - - PT_CALL(adc.startMeasurement(modm::ads7828::InputChannel::Ch6Ch7)); - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO.printf("Ch6 - Ch7 is \t %.4f", (double)data.getVoltage(3.3f)); - MODM_LOG_INFO << modm::endl; - - PT_CALL(adc.startMeasurement(modm::ads7828::InputChannel::Ch1Ch0)); - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO.printf("Ch1 - Ch0 is \t %.4f", (double)data.getVoltage(3.3f)); - MODM_LOG_INFO << modm::endl; - - PT_CALL(adc.startMeasurement(modm::ads7828::InputChannel::Ch3Ch2)); - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO.printf("Ch3 - Ch2 is \t %.4f", (double)data.getVoltage(3.3f)); - MODM_LOG_INFO << modm::endl; - - PT_CALL(adc.startMeasurement(modm::ads7828::InputChannel::Ch5Ch4)); - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO.printf("Ch5 - Ch4 is \t %.4f", (double)data.getVoltage(3.3f)); - MODM_LOG_INFO << modm::endl; - - PT_CALL(adc.startMeasurement(modm::ads7828::InputChannel::Ch7Ch6)); - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO.printf("Ch7 - Ch6 is \t %.4f", (double)data.getVoltage(3.3f)); - MODM_LOG_INFO << modm::endl; - - MODM_LOG_INFO << "---Toggling Power Down and Internal Ref----" << modm::endl; - - PT_CALL(adc.setPowerDownSelection(modm::ads7828::PowerDown::InternalReferenceOffAdcConverterOff)); - PT_CALL(adc.startMeasurement(modm::ads7828::InputChannel::Ch0)); - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO.printf("Default: \t\t\t\t %.4f", (double)data.getVoltage(3.3f)); - MODM_LOG_INFO << modm::endl; - - PT_CALL(adc.setPowerDownSelection(modm::ads7828::PowerDown::InternalReferenceOnAdcConverterOff)); - PT_CALL(adc.startMeasurement(modm::ads7828::InputChannel::Ch0)); - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO.printf("Internal ref on: \t\t\t %.4f", (double)data.getVoltage(2.5f)); - MODM_LOG_INFO << modm::endl; - - PT_CALL(adc.setPowerDownSelection(modm::ads7828::PowerDown::InternalReferenceOffAdcConverterOn)); - PT_CALL(adc.startMeasurement(modm::ads7828::InputChannel::Ch0)); - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO.printf("No power down \t\t\t\t %.4f", (double)data.getVoltage(3.3f)); - MODM_LOG_INFO << modm::endl; - - PT_CALL(adc.setPowerDownSelection(modm::ads7828::PowerDown::InternalReferenceOnAdcConverterOn)); - PT_CALL(adc.startMeasurement(modm::ads7828::InputChannel::Ch0)); - PT_CALL(adc.readConversionResult()); - MODM_LOG_INFO.printf("Internal ref on, no power down: \t %.4f", (double)data.getVoltage(2.5f)); - MODM_LOG_INFO << modm::endl; - - MODM_LOG_INFO << "-------------------------------" << modm::endl << modm::endl; - - timeout.restart(std::chrono::milliseconds(1000)); - PT_WAIT_UNTIL(timeout.isExpired()); - } - PT_END(); - } - -private: - modm::ads7828::Data data; - modm::Ads7828 adc; - - modm::ShortTimeout timeout; -} adcThread; + while (not adc.ping()) + { + MODM_LOG_ERROR << "Could not ping Ads7828" << modm::endl; + modm::this_fiber::sleep_for(1s); + } + + while (true) + { + MODM_LOG_INFO << "-------------------------------" << modm::endl << modm::endl; + adc.startMeasurement(modm::ads7828::InputChannel::Ch0); + adc.readConversionResult(); + MODM_LOG_INFO.printf("Ch0 measuremnt is \t %.4f", (double)data.getVoltage(3.3f)); + MODM_LOG_INFO << modm::endl; + + adc.startMeasurement(modm::ads7828::InputChannel::Ch1); + adc.readConversionResult(); + MODM_LOG_INFO.printf("Ch1 measuremnt is \t %.4f", (double)data.getVoltage(3.3f)); + MODM_LOG_INFO << modm::endl; + + adc.startMeasurement(modm::ads7828::InputChannel::Ch2); + adc.readConversionResult(); + MODM_LOG_INFO.printf("Ch2 measuremnt is \t %.4f", (double)data.getVoltage(3.3f)); + MODM_LOG_INFO << modm::endl; + + adc.startMeasurement(modm::ads7828::InputChannel::Ch3); + adc.readConversionResult(); + MODM_LOG_INFO.printf("Ch3 measuremnt is \t %.4f", (double)data.getVoltage(3.3f)); + MODM_LOG_INFO << modm::endl; + + adc.startMeasurement(modm::ads7828::InputChannel::Ch4); + adc.readConversionResult(); + MODM_LOG_INFO.printf("Ch4 measuremnt is \t %.4f", (double)data.getVoltage(3.3f)); + MODM_LOG_INFO << modm::endl; + + adc.startMeasurement(modm::ads7828::InputChannel::Ch5); + adc.readConversionResult(); + MODM_LOG_INFO.printf("Ch5 measuremnt is \t %.4f", (double)data.getVoltage(3.3f)); + MODM_LOG_INFO << modm::endl; + + adc.startMeasurement(modm::ads7828::InputChannel::Ch6); + adc.readConversionResult(); + MODM_LOG_INFO.printf("Ch6 measuremnt is \t %.4f", (double)data.getVoltage(3.3f)); + MODM_LOG_INFO << modm::endl; + + adc.startMeasurement(modm::ads7828::InputChannel::Ch7); + adc.readConversionResult(); + MODM_LOG_INFO.printf("Ch7 measuremnt is \t %.4f", (double)data.getVoltage(3.3f)); + MODM_LOG_INFO << modm::endl; + + MODM_LOG_INFO << "----Diff Inputs-------------" << modm::endl; + + adc.startMeasurement(modm::ads7828::InputChannel::Ch0Ch1); + adc.readConversionResult(); + MODM_LOG_INFO.printf("Ch0 - Ch1 is \t %.4f", (double)data.getVoltage(3.3f)); + MODM_LOG_INFO << modm::endl; + + adc.startMeasurement(modm::ads7828::InputChannel::Ch2Ch3); + adc.readConversionResult(); + MODM_LOG_INFO.printf("Ch2 - Ch3 is\t %.4f", (double)data.getVoltage(3.3f)); + MODM_LOG_INFO << modm::endl; + + adc.startMeasurement(modm::ads7828::InputChannel::Ch4Ch5); + adc.readConversionResult(); + MODM_LOG_INFO.printf("Ch4 - Ch5 is \t %.4f", (double)data.getVoltage(3.3f)); + MODM_LOG_INFO << modm::endl; + + adc.startMeasurement(modm::ads7828::InputChannel::Ch6Ch7); + adc.readConversionResult(); + MODM_LOG_INFO.printf("Ch6 - Ch7 is \t %.4f", (double)data.getVoltage(3.3f)); + MODM_LOG_INFO << modm::endl; + + adc.startMeasurement(modm::ads7828::InputChannel::Ch1Ch0); + adc.readConversionResult(); + MODM_LOG_INFO.printf("Ch1 - Ch0 is \t %.4f", (double)data.getVoltage(3.3f)); + MODM_LOG_INFO << modm::endl; + + adc.startMeasurement(modm::ads7828::InputChannel::Ch3Ch2); + adc.readConversionResult(); + MODM_LOG_INFO.printf("Ch3 - Ch2 is \t %.4f", (double)data.getVoltage(3.3f)); + MODM_LOG_INFO << modm::endl; + + adc.startMeasurement(modm::ads7828::InputChannel::Ch5Ch4); + adc.readConversionResult(); + MODM_LOG_INFO.printf("Ch5 - Ch4 is \t %.4f", (double)data.getVoltage(3.3f)); + MODM_LOG_INFO << modm::endl; + + adc.startMeasurement(modm::ads7828::InputChannel::Ch7Ch6); + adc.readConversionResult(); + MODM_LOG_INFO.printf("Ch7 - Ch6 is \t %.4f", (double)data.getVoltage(3.3f)); + MODM_LOG_INFO << modm::endl; + + MODM_LOG_INFO << "---Toggling Power Down and Internal Ref----" << modm::endl; + + adc.setPowerDownSelection(modm::ads7828::PowerDown::InternalReferenceOffAdcConverterOff); + adc.startMeasurement(modm::ads7828::InputChannel::Ch0); + adc.readConversionResult(); + MODM_LOG_INFO.printf("Default: \t\t\t\t %.4f", (double)data.getVoltage(3.3f)); + MODM_LOG_INFO << modm::endl; + + adc.setPowerDownSelection(modm::ads7828::PowerDown::InternalReferenceOnAdcConverterOff); + adc.startMeasurement(modm::ads7828::InputChannel::Ch0); + adc.readConversionResult(); + MODM_LOG_INFO.printf("Internal ref on: \t\t\t %.4f", (double)data.getVoltage(2.5f)); + MODM_LOG_INFO << modm::endl; + + adc.setPowerDownSelection(modm::ads7828::PowerDown::InternalReferenceOffAdcConverterOn); + adc.startMeasurement(modm::ads7828::InputChannel::Ch0); + adc.readConversionResult(); + MODM_LOG_INFO.printf("No power down \t\t\t\t %.4f", (double)data.getVoltage(3.3f)); + MODM_LOG_INFO << modm::endl; + + adc.setPowerDownSelection(modm::ads7828::PowerDown::InternalReferenceOnAdcConverterOn); + adc.startMeasurement(modm::ads7828::InputChannel::Ch0); + adc.readConversionResult(); + MODM_LOG_INFO.printf("Internal ref on, no power down: \t %.4f", (double)data.getVoltage(2.5f)); + MODM_LOG_INFO << modm::endl; + + MODM_LOG_INFO << "-------------------------------" << modm::endl << modm::endl; + + modm::this_fiber::sleep_for(1s); + } +}); + +modm::Fiber fiber_blink([] +{ + Board::LedD13::setOutput(); + while(true) + { + Board::LedD13::toggle(); + modm::this_fiber::sleep_for(0.5s); + } +}); int main() { - Board::initialize(); - - I2cMaster::connect(I2cMaster::PullUps::Internal); - I2cMaster::initialize(); + Board::initialize(); + I2cMaster::connect(I2cMaster::PullUps::Internal); + I2cMaster::initialize(); - MODM_LOG_INFO << "==========Ads7828 Test==========" << modm::endl; - MODM_LOG_DEBUG << "Debug logging here" << modm::endl; - MODM_LOG_INFO << "Info logging here" << modm::endl; - MODM_LOG_WARNING << "Warning logging here" << modm::endl; - MODM_LOG_ERROR << "Error logging here" << modm::endl; - MODM_LOG_INFO << "===============================" << modm::endl; + MODM_LOG_INFO << "==========Ads7828 Test==========" << modm::endl; - while (true) - { - adcThread.run(); - } - return 0; + modm::fiber::Scheduler::run(); + return 0; } diff --git a/examples/nucleo_g474re/ads7828/project.xml b/examples/nucleo_g474re/ads7828/project.xml index d35e31bff8..297a2fdc31 100644 --- a/examples/nucleo_g474re/ads7828/project.xml +++ b/examples/nucleo_g474re/ads7828/project.xml @@ -7,8 +7,7 @@ modm:driver:ads7828 modm:platform:gpio modm:platform:i2c:4 - modm:processing:protothread - modm:processing:timer + modm:processing:fiber modm:build:scons - \ No newline at end of file + diff --git a/examples/nucleo_g474re/as5047/main.cpp b/examples/nucleo_g474re/as5047/main.cpp index e56c038f0e..4ef7ab39b2 100644 --- a/examples/nucleo_g474re/as5047/main.cpp +++ b/examples/nucleo_g474re/as5047/main.cpp @@ -11,7 +11,6 @@ #include #include -#include using SpiMaster = SpiMaster1; @@ -20,41 +19,8 @@ using Mosi = modm::platform::GpioB5; using Miso = modm::platform::GpioB4; using Sck = modm::platform::GpioB3; -using namespace Board; -using namespace modm::literals; - -class EncoderThread : public modm::pt::Protothread -{ -public: - EncoderThread() : encoder(data) {} - - bool - run() - { - PT_BEGIN(); - - while (true) - { - PT_CALL(encoder.read()); - - MODM_LOG_INFO << "\nNew readout:" << modm::endl; - MODM_LOG_INFO << " angle degree: " << data.toDegree() << " degrees" << modm::endl; - MODM_LOG_INFO << " angle rad: " << data.toRadian() << " radians" << modm::endl; - MODM_LOG_INFO << " angle raw: " << data.data << modm::endl; - - timeout.restart(500ms); - PT_WAIT_UNTIL(timeout.isExpired()); - } - - PT_END(); - } - -private: - modm::as5047::Data data{0}; - modm::As5047 encoder; - - modm::ShortTimeout timeout; -} encoderThread; +modm::as5047::Data data{0}; +modm::As5047 encoder{data}; int main() @@ -73,7 +39,17 @@ main() MODM_LOG_ERROR << "Error logging here" << modm::endl; MODM_LOG_INFO << "===============================" << modm::endl; - while (true) { encoderThread.run(); } + while (true) + { + encoder.read(); + + MODM_LOG_INFO << "\nNew readout:" << modm::endl; + MODM_LOG_INFO << " angle degree: " << data.toDegree() << " degrees" << modm::endl; + MODM_LOG_INFO << " angle rad: " << data.toRadian() << " radians" << modm::endl; + MODM_LOG_INFO << " angle raw: " << data.data << modm::endl; + + modm::delay(500ms); + } return 0; -} \ No newline at end of file +} diff --git a/examples/nucleo_g474re/as5047/project.xml b/examples/nucleo_g474re/as5047/project.xml index 4b86fa2b25..75689e5fc0 100644 --- a/examples/nucleo_g474re/as5047/project.xml +++ b/examples/nucleo_g474re/as5047/project.xml @@ -7,8 +7,6 @@ modm:driver:as5047 modm:platform:gpio modm:platform:spi:1 - modm:processing:protothread - modm:processing:timer modm:build:scons - \ No newline at end of file + diff --git a/examples/nucleo_g474re/ixm42xxx/main.cpp b/examples/nucleo_g474re/ixm42xxx/main.cpp index 823410c239..a3163c9632 100644 --- a/examples/nucleo_g474re/ixm42xxx/main.cpp +++ b/examples/nucleo_g474re/ixm42xxx/main.cpp @@ -21,59 +21,57 @@ using Mosi = GpioA7; using Miso = GpioA6; using Sck = GpioA5; using Cs = GpioC5; +using Transport = modm::Ixm42xxxTransportSpi< SpiMaster, Cs >; -class ImuThread : public modm::pt::Protothread, public modm::ixm42xxx -{ - using Transport = modm::Ixm42xxxTransportSpi< SpiMaster, Cs >; - -public: - ImuThread() : imu(data), timer(std::chrono::milliseconds(500)) {} +modm::ixm42xxxdata::Data data; +modm::Ixm42xxx< Transport > imu{data}; - bool - run() +modm::Fiber fiber_sensor([] +{ + /// Initialize the IMU and verify that it is connected + imu.initialize(); + while (not imu.ping()) { - PT_BEGIN(); - - /// Initialize the IMU and verify that it is connected - PT_CALL(imu.initialize()); - while (not PT_CALL(imu.ping())) - { - MODM_LOG_ERROR << "Cannot ping IXM42xxx" << modm::endl; - PT_WAIT_UNTIL(timer.execute()); - } - - /// Configure data sensors - PT_CALL(imu.updateRegister(Register::GYRO_CONFIG0, GyroFs_t(GyroFs::dps2000) | GyroOdr_t(GyroOdr::kHz1))); - PT_CALL(imu.updateRegister(Register::ACCEL_CONFIG0, AccelFs_t(AccelFs::g16) | AccelOdr_t(AccelOdr::kHz1))); - PT_CALL(imu.updateRegister(Register::PWR_MGMT0, GyroMode_t(GyroMode::LowNoise) | AccelMode_t(AccelMode::LowNoise))); - - while (true) - { - PT_WAIT_UNTIL(timer.execute()); - PT_CALL(imu.readSensorData()); - - data.getTemp(&temp); - data.getAccel(&accel); - data.getGyro(&gyro); - - MODM_LOG_INFO.printf("Temp: %.3f\n", temp); - MODM_LOG_INFO.printf("Accel: (%.3f, %.3f, %.3f)\n", accel.x, accel.y, accel.z); - MODM_LOG_INFO.printf("Gyro: (%.3f, %.3f, %.3f)\n", gyro.x, gyro.y, gyro.z); - } - PT_END(); + MODM_LOG_ERROR << "Cannot ping IXM42xxx" << modm::endl; + modm::this_fiber::sleep_for(100ms); } + using ixm = modm::ixm42xxx; -private: - float temp; - modm::Vector3f accel; - modm::Vector3f gyro; - - modm::ixm42xxxdata::Data data; - modm::Ixm42xxx< Transport > imu; + /// Configure data sensors + imu.updateRegister(ixm::Register::GYRO_CONFIG0, + ixm::GyroFs_t(ixm::GyroFs::dps2000) | ixm::GyroOdr_t(ixm::GyroOdr::kHz1)); + imu.updateRegister(ixm::Register::ACCEL_CONFIG0, + ixm::AccelFs_t(ixm::AccelFs::g16) | ixm::AccelOdr_t(ixm::AccelOdr::kHz1)); + imu.updateRegister(ixm::Register::PWR_MGMT0, + ixm::GyroMode_t(ixm::GyroMode::LowNoise) | ixm::AccelMode_t(ixm::AccelMode::LowNoise)); - modm::PeriodicTimer timer; + while (true) + { + modm::this_fiber::sleep_for(500ms); + imu.readSensorData(); + + float temp; + modm::Vector3f accel; + modm::Vector3f gyro; + data.getTemp(&temp); + data.getAccel(&accel); + data.getGyro(&gyro); + + MODM_LOG_INFO.printf("Temp: %.3f\n", temp); + MODM_LOG_INFO.printf("Accel: (%.3f, %.3f, %.3f)\n", accel.x, accel.y, accel.z); + MODM_LOG_INFO.printf("Gyro: (%.3f, %.3f, %.3f)\n", gyro.x, gyro.y, gyro.z); + } +}); -} imuThread; +modm::Fiber fiber_blink([] +{ + Board::LedD13::setOutput(); + while(true) + { + Board::LedD13::toggle(); + modm::this_fiber::sleep_for(0.5s); + } +}); int main() @@ -85,16 +83,8 @@ main() SpiMaster::initialize(); MODM_LOG_INFO << "==========IXM-42xxx Test==========" << modm::endl; - MODM_LOG_DEBUG << "Debug logging here" << modm::endl; - MODM_LOG_INFO << "Info logging here" << modm::endl; - MODM_LOG_WARNING << "Warning logging here" << modm::endl; - MODM_LOG_ERROR << "Error logging here" << modm::endl; - MODM_LOG_INFO << "==================================" << modm::endl; - while (true) - { - imuThread.run(); - } + modm::fiber::Scheduler::run(); return 0; } diff --git a/examples/nucleo_g474re/ixm42xxx/project.xml b/examples/nucleo_g474re/ixm42xxx/project.xml index 5262c37935..757a5a2efc 100644 --- a/examples/nucleo_g474re/ixm42xxx/project.xml +++ b/examples/nucleo_g474re/ixm42xxx/project.xml @@ -8,8 +8,7 @@ modm:math:geometry modm:platform:gpio modm:platform:spi:1 - modm:processing:protothread - modm:processing:timer + modm:processing:fiber modm:build:scons - \ No newline at end of file + diff --git a/examples/nucleo_g474re/ixm42xxx_fifo/main.cpp b/examples/nucleo_g474re/ixm42xxx_fifo/main.cpp index 2f9b46bb8d..2fafb7316e 100644 --- a/examples/nucleo_g474re/ixm42xxx_fifo/main.cpp +++ b/examples/nucleo_g474re/ixm42xxx_fifo/main.cpp @@ -12,13 +12,10 @@ #include #include -#include #include +#include -namespace -{ - volatile bool interrupt = false; -} +std::atomic_bool interrupt{}; using SpiMaster = modm::platform::SpiMaster1; using Mosi = GpioA7; @@ -28,61 +25,57 @@ using Cs = GpioC5; using Int1 = GpioC3; -class ImuThread : public modm::pt::Protothread, public modm::ixm42xxx +class ImuThread : public modm::Fiber<>, public modm::ixm42xxx { using Transport = modm::Ixm42xxxTransportSpi< SpiMaster, Cs >; public: - ImuThread() : imu(fifoData), timer(std::chrono::milliseconds(500)) {} + ImuThread() : Fiber([this]{ run(); }) {} - bool +private: + void run() { - PT_BEGIN(); - Int1::setInput(modm::platform::Gpio::InputType::PullDown); Exti::connect(Exti::Trigger::RisingEdge, [](uint8_t) { interrupt = true; }); - /// Initialize the IMU and verify that it is connected - PT_CALL(imu.initialize()); - while (not PT_CALL(imu.ping())) + while (not imu.ping()) { MODM_LOG_ERROR << "Cannot ping IXM-42xxx" << modm::endl; - PT_WAIT_UNTIL(timer.execute()); + modm::this_fiber::sleep_for(0.5s); } + /// Initialize the IMU and verify that it is connected + imu.initialize(); MODM_LOG_INFO << "IXM-42xxx Initialized" << modm::endl; MODM_LOG_INFO << "Fifo Buffer Size: " << fifoData.getFifoBufferSize() << modm::endl; /// Configure FIFO - PT_CALL(imu.updateRegister(Register::FIFO_CONFIG, FifoMode_t(FifoMode::StopOnFull))); - PT_CALL(imu.updateRegister(Register::FIFO_CONFIG1, FifoConfig1::FIFO_HIRES_EN | FifoConfig1::FIFO_TEMP_EN | FifoConfig1::FIFO_GYRO_EN | FifoConfig1::FIFO_ACCEL_EN)); - PT_CALL(imu.writeFifoWatermark(1024)); + imu.updateRegister(Register::FIFO_CONFIG, FifoMode_t(FifoMode::StopOnFull)); + imu.updateRegister(Register::FIFO_CONFIG1, FifoConfig1::FIFO_HIRES_EN | FifoConfig1::FIFO_TEMP_EN | FifoConfig1::FIFO_GYRO_EN | FifoConfig1::FIFO_ACCEL_EN); + imu.writeFifoWatermark(1024); /// Configure interrupt - PT_CALL(imu.updateRegister(Register::INT_CONFIG, IntConfig::INT1_MODE | IntConfig::INT1_DRIVE_CIRCUIT | IntConfig::INT1_POLARITY)); - PT_CALL(imu.updateRegister(Register::INT_CONFIG1, IntConfig1::INT_ASYNC_RESET)); - PT_CALL(imu.updateRegister(Register::INT_SOURCE0, IntSource0::FIFO_THS_INT1_EN | IntSource0::FIFO_FULL_INT1_EN, IntSource0::UI_DRDY_INT1_EN)); + imu.updateRegister(Register::INT_CONFIG, IntConfig::INT1_MODE | IntConfig::INT1_DRIVE_CIRCUIT | IntConfig::INT1_POLARITY); + imu.updateRegister(Register::INT_CONFIG1, IntConfig1::INT_ASYNC_RESET); + imu.updateRegister(Register::INT_SOURCE0, IntSource0::FIFO_THS_INT1_EN | IntSource0::FIFO_FULL_INT1_EN, IntSource0::UI_DRDY_INT1_EN); /// Configure data sensors - PT_CALL(imu.updateRegister(Register::GYRO_CONFIG0, GyroFs_t(GyroFs::dps2000) | GyroOdr_t(GyroOdr::kHz1))); - PT_CALL(imu.updateRegister(Register::ACCEL_CONFIG0, AccelFs_t(AccelFs::g16) | AccelOdr_t(AccelOdr::kHz1))); - PT_CALL(imu.updateRegister(Register::PWR_MGMT0, GyroMode_t(GyroMode::LowNoise) | AccelMode_t(AccelMode::LowNoise))); + imu.updateRegister(Register::GYRO_CONFIG0, GyroFs_t(GyroFs::dps2000) | GyroOdr_t(GyroOdr::kHz1)); + imu.updateRegister(Register::ACCEL_CONFIG0, AccelFs_t(AccelFs::g16) | AccelOdr_t(AccelOdr::kHz1)); + imu.updateRegister(Register::PWR_MGMT0, GyroMode_t(GyroMode::LowNoise) | AccelMode_t(AccelMode::LowNoise)); while (true) { - if (interrupt) - { - PT_CALL(imu.readRegister(Register::INT_STATUS, &intStatus.value)); - interrupt = false; - } + if (interrupt.exchange(false)) + imu.readRegister(Register::INT_STATUS, &intStatus.value); if (intStatus.any(IntStatus::FIFO_FULL_INT | IntStatus::FIFO_THS_INT)) { - if (PT_CALL(imu.readFifoData())) + if (imu.readFifoData()) { // Count packets in FIFO buffer and print contents of last packet uint16_t count = 0; @@ -108,21 +101,27 @@ class ImuThread : public modm::pt::Protothread, public modm::ixm42xxx } intStatus.reset(IntStatus::FIFO_FULL_INT | IntStatus::FIFO_THS_INT); } + modm::this_fiber::yield(); } - PT_END(); - } -private: + } /// Due to the non-deterministic nature of system operation, driver memory allocation should always be the largest size of 2080 bytes. modm::ixm42xxxdata::FifoData<2080> fifoData; modm::ixm42xxx::IntStatus_t intStatus; - modm::Ixm42xxx< Transport > imu; - - modm::PeriodicTimer timer; - + modm::Ixm42xxx< Transport > imu{fifoData}; } imuThread; +modm::Fiber fiber_blink([] +{ + Board::LedD13::setOutput(); + while(true) + { + Board::LedD13::toggle(); + modm::this_fiber::sleep_for(0.5s); + } +}); + int main() { @@ -139,10 +138,6 @@ main() MODM_LOG_ERROR << "Error logging here" << modm::endl; MODM_LOG_INFO << "==================================" << modm::endl; - while (true) - { - imuThread.run(); - } - + modm::fiber::Scheduler::run(); return 0; } diff --git a/examples/nucleo_g474re/ixm42xxx_fifo/project.xml b/examples/nucleo_g474re/ixm42xxx_fifo/project.xml index fcf707cd29..fd45570bc6 100644 --- a/examples/nucleo_g474re/ixm42xxx_fifo/project.xml +++ b/examples/nucleo_g474re/ixm42xxx_fifo/project.xml @@ -9,8 +9,7 @@ modm:platform:exti modm:platform:gpio modm:platform:spi:1 - modm:processing:protothread - modm:processing:timer + modm:processing:fiber modm:build:scons - \ No newline at end of file + diff --git a/examples/nucleo_g474re/max31855/main.cpp b/examples/nucleo_g474re/max31855/main.cpp index f365e03b71..0f8568bd55 100644 --- a/examples/nucleo_g474re/max31855/main.cpp +++ b/examples/nucleo_g474re/max31855/main.cpp @@ -11,7 +11,6 @@ */ #include -#include #include using SpiMaster = modm::platform::SpiMaster2; @@ -20,80 +19,49 @@ using Cs = modm::platform::GpioA10; using Miso = modm::platform::GpioB14; using Sck = modm::platform::GpioB13; -using namespace Board; +modm::max31855::Data data; +modm::Max31855 thermocouple{data}; -class ThermocoupleThread : public modm::pt::Protothread +int +main() { -public: - ThermocoupleThread() : thermocouple(data) {} - - bool - run() - { - PT_BEGIN(); - - thermocouple.initialize(); - - MODM_LOG_INFO << "Max38155 initialized succeded" << modm::endl; - - while (true) - { - PT_CALL(thermocouple.readout()); + Board::initialize(); + Cs::setOutput(modm::Gpio::High); - switch (data.getFault()) - { - case modm::max31855::Fault::ShortCircuitVcc: - MODM_LOG_ERROR << "Thermocouple error short circuit vcc" << modm::endl; - break; + SpiMaster::connect(); + SpiMaster::initialize(); - case modm::max31855::Fault::ShortCircuitGnd: - MODM_LOG_ERROR << "Thermocouple error short circuit gnd" << modm::endl; - break; + MODM_LOG_INFO << "==========MAX 31855 Test==========" << modm::endl; - case modm::max31855::Fault::OpenCircuit: - MODM_LOG_ERROR << "Thermocouple error open circuit" << modm::endl; - break; + thermocouple.initialize(); + MODM_LOG_INFO << "Max38155 initialized succeded" << modm::endl; - default: - MODM_LOG_INFO << "Thermocouple Temperature: " << data.getThermocoupleTemperature() << " degrees Centigrade" << modm::endl; - MODM_LOG_INFO << "Internal Temperature: " << data.getReferenceJunctionTemperature() << " degrees Centigrade" << modm::endl; - break; - } + while (true) + { + thermocouple.readout(); - timeout.restart(std::chrono::milliseconds(1000)); - PT_WAIT_UNTIL(timeout.isExpired()); - } + switch (data.getFault()) + { + case modm::max31855::Fault::ShortCircuitVcc: + MODM_LOG_ERROR << "Thermocouple error short circuit vcc" << modm::endl; + break; - PT_END(); - } - -private: - modm::max31855::Data data; - modm::Max31855 thermocouple; - - modm::ShortTimeout timeout; -} thermocoupleThread; - -int -main() -{ - Board::initialize(); - Cs::setOutput(modm::Gpio::High); + case modm::max31855::Fault::ShortCircuitGnd: + MODM_LOG_ERROR << "Thermocouple error short circuit gnd" << modm::endl; + break; - SpiMaster::connect(); - SpiMaster::initialize(); + case modm::max31855::Fault::OpenCircuit: + MODM_LOG_ERROR << "Thermocouple error open circuit" << modm::endl; + break; - MODM_LOG_INFO << "==========MAX 31855 Test==========" << modm::endl; - MODM_LOG_DEBUG << "Debug logging here" << modm::endl; - MODM_LOG_INFO << "Info logging here" << modm::endl; - MODM_LOG_WARNING << "Warning logging here" << modm::endl; - MODM_LOG_ERROR << "Error logging here" << modm::endl; - MODM_LOG_INFO << "===============================" << modm::endl; + default: + MODM_LOG_INFO << "Thermocouple Temperature: " << data.getThermocoupleTemperature() << " degrees Centigrade" << modm::endl; + MODM_LOG_INFO << "Internal Temperature: " << data.getReferenceJunctionTemperature() << " degrees Centigrade" << modm::endl; + break; + } - while (true) - { - thermocoupleThread.run(); - } + modm::delay(1s); + } - return 0; -} \ No newline at end of file + return 0; +} diff --git a/examples/nucleo_g474re/max31855/project.xml b/examples/nucleo_g474re/max31855/project.xml index 506a31d432..42b215af3a 100644 --- a/examples/nucleo_g474re/max31855/project.xml +++ b/examples/nucleo_g474re/max31855/project.xml @@ -5,10 +5,7 @@ modm:driver:max31855 - modm:platform:gpio modm:platform:spi:2 - modm:processing:protothread - modm:processing:timer modm:build:scons - \ No newline at end of file + diff --git a/examples/nucleo_g474re/ms5611/main.cpp b/examples/nucleo_g474re/ms5611/main.cpp index ff0b33d321..d708522ebe 100644 --- a/examples/nucleo_g474re/ms5611/main.cpp +++ b/examples/nucleo_g474re/ms5611/main.cpp @@ -10,11 +10,8 @@ */ #include -#include #include -using namespace Board; - using Cs = modm::platform::GpioC13; using Mosi = modm::platform::GpioA7; using Miso = modm::platform::GpioA6; @@ -22,84 +19,54 @@ using Sck = modm::platform::GpioA5; using SpiMaster = modm::platform::SpiMaster1; -class BarometerThread : public modm::pt::Protothread -{ -public: - BarometerThread() : barometer(data) {} - - bool - run() - { - PT_BEGIN(); - - while (PT_CALL(barometer.initialize()) == false) - { - MODM_LOG_ERROR << "Ms5611 PROM CRC failed" << modm::endl; - timeout.restart(std::chrono::milliseconds(1000)); - PT_WAIT_UNTIL(timeout.isExpired()); - } - - MODM_LOG_INFO << "Ms5611 initialized and PROM CRC succeded" << modm::endl; - - prom = data.getProm(); - MODM_LOG_INFO << "MS5611 PROM Contents" << modm::endl; - MODM_LOG_INFO << "C0: " << prom.data[0] << modm::endl; - MODM_LOG_INFO << "C1: " << prom.data[1] << modm::endl; - MODM_LOG_INFO << "C2: " << prom.data[2] << modm::endl; - MODM_LOG_INFO << "C3: " << prom.data[3] << modm::endl; - MODM_LOG_INFO << "C4: " << prom.data[4] << modm::endl; - MODM_LOG_INFO << "C5: " << prom.data[5] << modm::endl; - MODM_LOG_INFO << "C6: " << prom.data[6] << modm::endl; - MODM_LOG_INFO << "C7: " << prom.data[7] << modm::endl; - - while (true) - { - PT_CALL(barometer.readout()); - - data.getPressure(pressure); - MODM_LOG_INFO << "Pressure: " << pressure << " mbar" << modm::endl; - - data.getTemperature(temperature); - MODM_LOG_INFO << "Temperature: " << temperature << " degrees Centigrade" << modm::endl; - - timeout.restart(std::chrono::milliseconds(1000)); - PT_WAIT_UNTIL(timeout.isExpired()); - } - - PT_END(); - } - -private: - modm::ms5611data::Data data; - modm::ms5611data::Prom prom; - modm::Ms5611 barometer; - - float pressure; - float temperature; - - modm::ShortTimeout timeout; -} barometerThread; +modm::ms5611data::Data data; +modm::ms5611data::Prom prom; +modm::Ms5611 barometer{data}; int main() { - Board::initialize(); - Cs::setOutput(modm::Gpio::High); - - SpiMaster::connect(); - SpiMaster::initialize(); - - MODM_LOG_INFO << "==========MS5611 Test==========" << modm::endl; - MODM_LOG_DEBUG << "Debug logging here" << modm::endl; - MODM_LOG_INFO << "Info logging here" << modm::endl; - MODM_LOG_WARNING << "Warning logging here" << modm::endl; - MODM_LOG_ERROR << "Error logging here" << modm::endl; - MODM_LOG_INFO << "===============================" << modm::endl; - - while (true) - { - barometerThread.run(); - } - - return 0; -} \ No newline at end of file + Board::initialize(); + Cs::setOutput(modm::Gpio::High); + + SpiMaster::connect(); + SpiMaster::initialize(); + + MODM_LOG_INFO << "==========MS5611 Test==========" << modm::endl; + + while (not barometer.initialize()) + { + MODM_LOG_ERROR << "Ms5611 PROM CRC failed" << modm::endl; + modm::delay(1s); + } + + MODM_LOG_INFO << "Ms5611 initialized and PROM CRC succeded" << modm::endl; + + prom = data.getProm(); + MODM_LOG_INFO << "MS5611 PROM Contents" << modm::endl; + MODM_LOG_INFO << "C0: " << prom.data[0] << modm::endl; + MODM_LOG_INFO << "C1: " << prom.data[1] << modm::endl; + MODM_LOG_INFO << "C2: " << prom.data[2] << modm::endl; + MODM_LOG_INFO << "C3: " << prom.data[3] << modm::endl; + MODM_LOG_INFO << "C4: " << prom.data[4] << modm::endl; + MODM_LOG_INFO << "C5: " << prom.data[5] << modm::endl; + MODM_LOG_INFO << "C6: " << prom.data[6] << modm::endl; + MODM_LOG_INFO << "C7: " << prom.data[7] << modm::endl; + + while (true) + { + barometer.readout(); + + float pressure; + data.getPressure(pressure); + MODM_LOG_INFO << "Pressure: " << pressure << " mbar" << modm::endl; + + float temperature; + data.getTemperature(temperature); + MODM_LOG_INFO << "Temperature: " << temperature << " degrees Centigrade" << modm::endl; + + modm::delay(1s); + } + + return 0; +} diff --git a/examples/nucleo_g474re/ms5611/project.xml b/examples/nucleo_g474re/ms5611/project.xml index d2af13d728..d95a395819 100644 --- a/examples/nucleo_g474re/ms5611/project.xml +++ b/examples/nucleo_g474re/ms5611/project.xml @@ -5,10 +5,7 @@ modm:driver:ms5611 - modm:platform:gpio modm:platform:spi:1 - modm:processing:protothread - modm:processing:timer modm:build:scons - \ No newline at end of file + diff --git a/examples/nucleo_g474re/sx128x_lora/main.cpp b/examples/nucleo_g474re/sx128x_lora/main.cpp index 54b0282ed2..2d11f3d662 100644 --- a/examples/nucleo_g474re/sx128x_lora/main.cpp +++ b/examples/nucleo_g474re/sx128x_lora/main.cpp @@ -16,7 +16,6 @@ #include #include #include -#include #include using Sck = GpioA5; @@ -29,250 +28,231 @@ namespace namespace rx { - std::atomic_bool dio1 = false; - std::atomic_bool dio2 = false; - std::atomic_bool dio3 = false; + std::atomic_bool dio1 = false; + std::atomic_bool dio2 = false; + std::atomic_bool dio3 = false; } namespace tx { - std::atomic_bool dio1 = false; - std::atomic_bool dio2 = false; - std::atomic_bool dio3 = false; + std::atomic_bool dio1 = false; + std::atomic_bool dio2 = false; + std::atomic_bool dio3 = false; } static constexpr modm::sx128x::LoRa::ModulationParams modulationParams = { - .spreadingFactor = modm::sx128x::LoRa::ModulationParams::SpreadingFactor::Sf9, - .bandwidth = modm::sx128x::LoRa::ModulationParams::Bandwidth::Bw400, - .codingRate = modm::sx128x::LoRa::ModulationParams::CodingRate::Cr_Li_4_7 + .spreadingFactor = modm::sx128x::LoRa::ModulationParams::SpreadingFactor::Sf9, + .bandwidth = modm::sx128x::LoRa::ModulationParams::Bandwidth::Bw400, + .codingRate = modm::sx128x::LoRa::ModulationParams::CodingRate::Cr_Li_4_7 }; static constexpr modm::sx128x::LoRa::PacketParams packetParams = { - .preambleLength = 12, - .headerType = modm::sx128x::LoRa::PacketParams::HeaderType::Explicit, - .payloadLength = 4, - .crc = modm::sx128x::LoRa::PacketParams::Crc::Enable, - .invertIq = modm::sx128x::LoRa::PacketParams::InvertIq::Standard + .preambleLength = 12, + .headerType = modm::sx128x::LoRa::PacketParams::HeaderType::Explicit, + .payloadLength = 4, + .crc = modm::sx128x::LoRa::PacketParams::Crc::Enable, + .invertIq = modm::sx128x::LoRa::PacketParams::InvertIq::Standard }; } -class RxThread : public modm::sx128x, public modm::pt::Protothread +class RxThread : public modm::sx128x, public modm::Fiber<> { - using Reset = GpioB3; - using Busy = GpioB4; - using Dio1 = GpioB5; - using Dio2 = GpioB6; - using Dio3 = GpioB7; + using Reset = GpioB3; + using Busy = GpioB4; + using Dio1 = GpioB5; + using Dio2 = GpioB6; + using Dio3 = GpioB7; - using Nss = GpioD2; - using Transport = modm::Sx128xTransportSpi; + using Nss = GpioD2; + using Transport = modm::Sx128xTransportSpi; public: - RxThread() {} - - inline bool - run() - { - PT_BEGIN(); - - Nss::setOutput(modm::Gpio::High); - Reset::setOutput(modm::Gpio::Low); - Busy::setInput(modm::platform::Gpio::InputType::PullDown); - - Dio1::setInput(modm::platform::Gpio::InputType::PullDown); - Exti::connect(Exti::Trigger::RisingEdge, [](uint8_t) { rx::dio1 = true; }); - - Dio2::setInput(modm::platform::Gpio::InputType::PullDown); - Exti::connect(Exti::Trigger::RisingEdge, [](uint8_t) { rx::dio2 = true; }); - - Dio3::setInput(modm::platform::Gpio::InputType::PullDown); - Exti::connect(Exti::Trigger::RisingEdge, [](uint8_t) { rx::dio3 = true; }); - - PT_CALL(radio.reset()); - PT_CALL(radio.setStandby()); - - // Initialize the sx128x - PT_CALL(radio.setPacketType(PacketType::LoRa)); - PT_CALL(radio.setRfFrequency(2457_MHz / radio.frequencyLsb)); - PT_CALL(radio.setRegulatorMode(RegulatorMode::Ldo)); - PT_CALL(radio.setBufferBaseAddress(0, 0)); - PT_CALL(radio.setModulationParams(modulationParams)); - PT_CALL(radio.writeRegister(Register::SfAdditionalConfiguration, 0x32)); - PT_CALL(radio.writeRegister(Register::FrequencyErrorCorrection, 0x01)); - PT_CALL(radio.setPacketParams(packetParams)); - PT_CALL(radio.setDioIrqParams(Irq::RxDone | Irq::RxTxTimeout, Irq::RxDone, Irq::RxTxTimeout)); - PT_CALL(radio.setRx(sx128x::PeriodBase::ms1, 1000)); - - MODM_LOG_DEBUG << "Sx128x initialization complete!" << modm::endl; - - while (true) - { - if (rx::dio1.exchange(false)) - { - PT_CALL(radio.getIrqStatus(&irqStatus)); - if (irqStatus.any(Irq::RxDone)) - { - PT_CALL(radio.clearIrqStatus(Irq::RxDone | Irq::RxTxTimeout)); - PT_CALL(radio.setRx(sx128x::PeriodBase::ms1, 1000)); - - PT_CALL(radio.getRxBufferStatus(&rxBufferStatus)); - PT_CALL(radio.getPacketStatus(&packetStatus)); - PT_CALL(radio.readBuffer(rxBufferStatus.rxStartBufferPointer, std::span{buffer, rxBufferStatus.rxPayloadLength})); - - if (rxBufferStatus.rxPayloadLength > 0) - { - uint32_t counter; - std::memcpy((uint8_t *) &counter, buffer, sizeof(counter)); - MODM_LOG_DEBUG << "Received Message" << modm::endl; - MODM_LOG_DEBUG << "Counter: " << counter << modm::endl; - } - } - } - - if (rx::dio2.exchange(false)) - { - PT_CALL(radio.getIrqStatus(&irqStatus)); - if (irqStatus.any(Irq::RxTxTimeout)) - { - // RxTxTimeout Interrupt received! - // Clear irq and set to rx again. - PT_CALL(radio.clearIrqStatus(Irq::RxTxTimeout)); - PT_CALL(radio.setRx(sx128x::PeriodBase::ms1, 1000)); - MODM_LOG_DEBUG << "RxTxTimeout Interrupt!" << modm::endl; - } - } - PT_YIELD(); - } - PT_END(); - } + RxThread() : Fiber([this]{ run(); }) {} private: - Irq_t irqStatus; - PacketStatus packetStatus; - RxBufferStatus rxBufferStatus; - - modm::Sx128x< Transport, Reset, Busy > radio; - -private: - static constexpr size_t bufferSize = 256; - uint8_t buffer[bufferSize]; -} rxThread; - -class TxThread : public modm::sx128x, public modm::pt::Protothread + bool + run() + { + Nss::setOutput(modm::Gpio::High); + Reset::setOutput(modm::Gpio::Low); + Busy::setInput(modm::platform::Gpio::InputType::PullDown); + + Dio1::setInput(modm::platform::Gpio::InputType::PullDown); + Exti::connect(Exti::Trigger::RisingEdge, [](uint8_t) { rx::dio1 = true; }); + + Dio2::setInput(modm::platform::Gpio::InputType::PullDown); + Exti::connect(Exti::Trigger::RisingEdge, [](uint8_t) { rx::dio2 = true; }); + + Dio3::setInput(modm::platform::Gpio::InputType::PullDown); + Exti::connect(Exti::Trigger::RisingEdge, [](uint8_t) { rx::dio3 = true; }); + + radio.reset(); + radio.setStandby(); + + // Initialize the sx128x + radio.setPacketType(PacketType::LoRa); + radio.setRfFrequency(2457_MHz / radio.frequencyLsb); + radio.setRegulatorMode(RegulatorMode::Ldo); + radio.setBufferBaseAddress(0, 0); + radio.setModulationParams(modulationParams); + radio.writeRegister(Register::SfAdditionalConfiguration, 0x32); + radio.writeRegister(Register::FrequencyErrorCorrection, 0x01); + radio.setPacketParams(packetParams); + radio.setDioIrqParams(Irq::RxDone | Irq::RxTxTimeout, Irq::RxDone, Irq::RxTxTimeout); + radio.setRx(sx128x::PeriodBase::ms1, 1000); + + MODM_LOG_DEBUG << "Sx128x initialization complete!" << modm::endl; + + while (true) + { + if (rx::dio1.exchange(false)) + { + radio.getIrqStatus(&irqStatus); + if (irqStatus.any(Irq::RxDone)) + { + radio.clearIrqStatus(Irq::RxDone | Irq::RxTxTimeout); + radio.setRx(sx128x::PeriodBase::ms1, 1000); + + PacketStatus packetStatus; + RxBufferStatus rxBufferStatus; + radio.getRxBufferStatus(&rxBufferStatus); + radio.getPacketStatus(&packetStatus); + radio.readBuffer(rxBufferStatus.rxStartBufferPointer, std::span{buffer, rxBufferStatus.rxPayloadLength}); + + if (rxBufferStatus.rxPayloadLength > 0) + { + uint32_t counter; + std::memcpy((uint8_t *) &counter, buffer, sizeof(counter)); + MODM_LOG_DEBUG << "Received Message" << modm::endl; + MODM_LOG_DEBUG << "Counter: " << counter << modm::endl; + } + } + } + + if (rx::dio2.exchange(false)) + { + radio.getIrqStatus(&irqStatus); + if (irqStatus.any(Irq::RxTxTimeout)) + { + // RxTxTimeout Interrupt received! + // Clear irq and set to rx again. + radio.clearIrqStatus(Irq::RxTxTimeout); + radio.setRx(sx128x::PeriodBase::ms1, 1000); + MODM_LOG_DEBUG << "RxTxTimeout Interrupt!" << modm::endl; + } + } + modm::this_fiber::yield(); + } + } + + modm::Sx128x< Transport, Reset, Busy > radio; + static constexpr size_t bufferSize = 256; + uint8_t buffer[bufferSize]; + Irq_t irqStatus; +} txFiber; + +class TxThread : public modm::sx128x, public modm::Fiber<> { - using Reset = modm::platform::GpioC2; - using Busy = modm::platform::GpioC3; - using Dio1 = modm::platform::GpioA0; - using Dio2 = modm::platform::GpioA1; - using Dio3 = modm::platform::GpioA2; + using Reset = modm::platform::GpioC2; + using Busy = modm::platform::GpioC3; + using Dio1 = modm::platform::GpioA0; + using Dio2 = modm::platform::GpioA1; + using Dio3 = modm::platform::GpioA2; - using Nss = modm::platform::GpioC1; - using Transport = modm::Sx128xTransportSpi; + using Nss = modm::platform::GpioC1; + using Transport = modm::Sx128xTransportSpi; public: - TxThread() : timer(std::chrono::milliseconds(500)) {} - - inline bool - run() - { - PT_BEGIN(); - - Nss::setOutput(modm::Gpio::High); - Reset::setOutput(modm::Gpio::Low); - Busy::setInput(modm::platform::Gpio::InputType::PullDown); - - Dio1::setInput(modm::platform::Gpio::InputType::PullDown); - Exti::connect(Exti::Trigger::RisingEdge, [](uint8_t) { tx::dio1 = true; }); - - Dio2::setInput(modm::platform::Gpio::InputType::PullDown); - Exti::connect(Exti::Trigger::RisingEdge, [](uint8_t) { tx::dio2 = true; }); - - Dio3::setInput(modm::platform::Gpio::InputType::PullDown); - Exti::connect(Exti::Trigger::RisingEdge, [](uint8_t) { tx::dio3 = true; }); - - - PT_CALL(radio.reset()); - PT_CALL(radio.setStandby()); - - // Initialize the sx128x - PT_CALL(radio.setPacketType(PacketType::LoRa)); - PT_CALL(radio.setRfFrequency(2457_MHz / radio.frequencyLsb)); - PT_CALL(radio.setRegulatorMode(RegulatorMode::Ldo)); - PT_CALL(radio.setBufferBaseAddress(0, 0)); - PT_CALL(radio.setModulationParams(modulationParams)); - PT_CALL(radio.writeRegister(Register::SfAdditionalConfiguration, 0x32)); - PT_CALL(radio.writeRegister(Register::FrequencyErrorCorrection, 0x01)); - PT_CALL(radio.setPacketParams(packetParams)); - PT_CALL(radio.setDioIrqParams(Irq::TxDone | Irq::RxTxTimeout, Irq::TxDone, Irq::RxTxTimeout)); - - MODM_LOG_DEBUG << "Sx128x initialization complete!" << modm::endl; - - while (true) - { - if (tx::dio1.exchange(false)) - { - PT_CALL(radio.getIrqStatus(&irqStatus)); - if (irqStatus.any(Irq::TxDone)) - { - PT_CALL(radio.clearIrqStatus(Irq::TxDone)); - irqStatus.reset(Irq::TxDone); - - MODM_LOG_DEBUG << "Message sent" << modm::endl; - MODM_LOG_DEBUG << "Counter: " << counter << modm::endl; - counter++; - } - } - - if (tx::dio2.exchange(false)) - { - PT_CALL(radio.getIrqStatus(&irqStatus)); - if (irqStatus.any(Irq::RxTxTimeout)) - { - PT_CALL(radio.clearIrqStatus(Irq::RxTxTimeout)); - irqStatus.reset(Irq::RxTxTimeout); - MODM_LOG_DEBUG << "Received a timeout" << modm::endl; - } - } - - if (timer.execute()) - { - PT_CALL(radio.writeBuffer(0, std::span{(uint8_t *) &counter, sizeof(counter)})); - PT_CALL(radio.setTx(PeriodBase::ms1, 100)); - } - PT_YIELD(); - } - - PT_END(); - } + TxThread() : Fiber([this]{ run(); }) {} private: - Irq_t irqStatus; - modm::Sx128x< Transport, Reset, Busy > radio; + bool + run() + { + Nss::setOutput(modm::Gpio::High); + Reset::setOutput(modm::Gpio::Low); + Busy::setInput(modm::platform::Gpio::InputType::PullDown); + + Dio1::setInput(modm::platform::Gpio::InputType::PullDown); + Exti::connect(Exti::Trigger::RisingEdge, [](uint8_t) { tx::dio1 = true; }); + + Dio2::setInput(modm::platform::Gpio::InputType::PullDown); + Exti::connect(Exti::Trigger::RisingEdge, [](uint8_t) { tx::dio2 = true; }); + + Dio3::setInput(modm::platform::Gpio::InputType::PullDown); + Exti::connect(Exti::Trigger::RisingEdge, [](uint8_t) { tx::dio3 = true; }); + + + radio.reset(); + radio.setStandby(); + + // Initialize the sx128x + radio.setPacketType(PacketType::LoRa); + radio.setRfFrequency(2457_MHz / radio.frequencyLsb); + radio.setRegulatorMode(RegulatorMode::Ldo); + radio.setBufferBaseAddress(0, 0); + radio.setModulationParams(modulationParams); + radio.writeRegister(Register::SfAdditionalConfiguration, 0x32); + radio.writeRegister(Register::FrequencyErrorCorrection, 0x01); + radio.setPacketParams(packetParams); + radio.setDioIrqParams(Irq::TxDone | Irq::RxTxTimeout, Irq::TxDone, Irq::RxTxTimeout); + + MODM_LOG_DEBUG << "Sx128x initialization complete!" << modm::endl; + + modm::PeriodicTimer timer{500ms}; + uint32_t counter{}; + while (true) + { + if (tx::dio1.exchange(false)) + { + radio.getIrqStatus(&irqStatus); + if (irqStatus.any(Irq::TxDone)) + { + radio.clearIrqStatus(Irq::TxDone); + irqStatus.reset(Irq::TxDone); + + MODM_LOG_DEBUG << "Message sent" << modm::endl; + MODM_LOG_DEBUG << "Counter: " << counter << modm::endl; + counter++; + } + } + + if (tx::dio2.exchange(false)) + { + radio.getIrqStatus(&irqStatus); + if (irqStatus.any(Irq::RxTxTimeout)) + { + radio.clearIrqStatus(Irq::RxTxTimeout); + irqStatus.reset(Irq::RxTxTimeout); + MODM_LOG_DEBUG << "Received a timeout" << modm::endl; + } + } + + if (timer.execute()) + { + radio.writeBuffer(0, std::span{(uint8_t *) &counter, sizeof(counter)}); + radio.setTx(PeriodBase::ms1, 100); + } + modm::this_fiber::yield(); + } + } - uint32_t counter = 0; - modm::PeriodicTimer timer; -}; +private: + Irq_t irqStatus; + modm::Sx128x< Transport, Reset, Busy > radio; +} txFiber; int main() { - Board::initialize(); + Board::initialize(); - SpiMaster::connect(); - SpiMaster::initialize(); + SpiMaster::connect(); + SpiMaster::initialize(); - MODM_LOG_INFO << "==========SX128x Test==========" << modm::endl; - MODM_LOG_DEBUG << "Debug logging here" << modm::endl; - MODM_LOG_INFO << "Info logging here" << modm::endl; - MODM_LOG_WARNING << "Warning logging here" << modm::endl; - MODM_LOG_ERROR << "Error logging here" << modm::endl; - MODM_LOG_INFO << "===============================" << modm::endl; + MODM_LOG_INFO << "==========SX128x Test==========" << modm::endl; - while (true) - { - rxThread.run(); - } - - return 0; -} \ No newline at end of file + modm::fiber::Scheduler::run(); + return 0; +} diff --git a/examples/nucleo_g474re/sx128x_lora/project.xml b/examples/nucleo_g474re/sx128x_lora/project.xml index 544f5d2171..1fe7ec0a43 100644 --- a/examples/nucleo_g474re/sx128x_lora/project.xml +++ b/examples/nucleo_g474re/sx128x_lora/project.xml @@ -6,10 +6,8 @@ modm:driver:sx128x modm:platform:exti - modm:platform:gpio modm:platform:spi:1 - modm:processing:protothread - modm:processing:timer + modm:processing:fiber modm:build:scons - \ No newline at end of file + diff --git a/examples/nucleo_h723zg/bmi088/i2c/project.xml b/examples/nucleo_h723zg/bmi088/i2c/project.xml index 797c04a836..2050231053 100644 --- a/examples/nucleo_h723zg/bmi088/i2c/project.xml +++ b/examples/nucleo_h723zg/bmi088/i2c/project.xml @@ -2,11 +2,9 @@ modm:nucleo-h723zg - modm:build:scons - modm:processing:fiber modm:platform:exti modm:platform:i2c:1 modm:driver:bmi088 diff --git a/examples/nucleo_h723zg/bmi088/spi/project.xml b/examples/nucleo_h723zg/bmi088/spi/project.xml index 0ee7b8a80b..4c941b592e 100644 --- a/examples/nucleo_h723zg/bmi088/spi/project.xml +++ b/examples/nucleo_h723zg/bmi088/spi/project.xml @@ -2,11 +2,9 @@ modm:nucleo-h723zg - modm:build:scons - modm:processing:fiber modm:platform:dma modm:platform:exti modm:platform:spi:2 diff --git a/examples/nucleo_l432kc/gyroscope/main.cpp b/examples/nucleo_l432kc/gyroscope/main.cpp index f9fb23cb13..dfbd803a3a 100644 --- a/examples/nucleo_l432kc/gyroscope/main.cpp +++ b/examples/nucleo_l432kc/gyroscope/main.cpp @@ -14,7 +14,6 @@ #include #include -#include #include // Example for L3gd20 gyroscope connected to SPI USART interface @@ -31,63 +30,13 @@ struct UartSpi using Transport = modm::Lis3TransportSpi; -namespace -{ - modm::l3gd20::Data data; - modm::L3gd20 gyro{data}; -} - -class ReaderThread : public modm::pt::Protothread -{ -public: - bool - update() - { - PT_BEGIN(); - - // ping the device until it responds - while(true) - { - // we wait until the task started - if (PT_CALL(gyro.ping())) - break; - // otherwise, try again in 100ms - timeout.restart(100ms); - Board::LedD13::set(); - PT_WAIT_UNTIL(timeout.isExpired()); - Board::LedD13::reset(); - } - - PT_CALL(gyro.configure(gyro.Scale::Dps250, gyro.MeasurementRate::Hz380)); - - while (true) - { - PT_CALL(gyro.readRotation()); - - averageX.update(gyro.getData().getX()); - averageY.update(gyro.getData().getY()); - averageZ.update(gyro.getData().getZ()); - - MODM_LOG_INFO.printf("x: %.2f, y: %.2f, z: %.2f \n", - double(averageX.getValue()), - double(averageY.getValue()), - double(averageZ.getValue())); - - timeout.restart(50ms); - PT_WAIT_UNTIL(timeout.isExpired()); - } - - PT_END(); - } +modm::l3gd20::Data data; +modm::L3gd20 gyro{data}; -private: - modm::ShortTimeout timeout; - modm::filter::MovingAverage averageX; - modm::filter::MovingAverage averageY; - modm::filter::MovingAverage averageZ; -}; -ReaderThread reader; +modm::filter::MovingAverage averageX; +modm::filter::MovingAverage averageY; +modm::filter::MovingAverage averageZ; int main() @@ -97,8 +46,30 @@ main() UartSpi::Master::connect(); UartSpi::Master::initialize(); - while (true) { - reader.update(); + // ping the device until it responds + while(not gyro.ping()) + { + Board::LedD13::set(); + modm::delay(100ms); + Board::LedD13::reset(); + } + + gyro.configure(gyro.Scale::Dps250, gyro.MeasurementRate::Hz380); + + while (true) + { + gyro.readRotation(); + + averageX.update(gyro.getData().getX()); + averageY.update(gyro.getData().getY()); + averageZ.update(gyro.getData().getZ()); + + MODM_LOG_INFO.printf("x: %.2f, y: %.2f, z: %.2f \n", + double(averageX.getValue()), + double(averageY.getValue()), + double(averageZ.getValue())); + + modm::delay(50ms); } return 0; diff --git a/examples/nucleo_l432kc/gyroscope/project.xml b/examples/nucleo_l432kc/gyroscope/project.xml index 8fd87a3f22..917406267d 100644 --- a/examples/nucleo_l432kc/gyroscope/project.xml +++ b/examples/nucleo_l432kc/gyroscope/project.xml @@ -7,8 +7,6 @@ modm:driver:l3gd20 modm:math:filter modm:platform:uart.spi:1 - modm:processing:timer - modm:processing:protothread modm:build:scons diff --git a/examples/nucleo_l476rg/i2c_test/main.cpp b/examples/nucleo_l476rg/i2c_test/main.cpp index 916217be29..5fb8b16400 100644 --- a/examples/nucleo_l476rg/i2c_test/main.cpp +++ b/examples/nucleo_l476rg/i2c_test/main.cpp @@ -9,10 +9,7 @@ */ #include -#include - -#include -#include +#include #include /* @@ -37,14 +34,14 @@ class I2cTestDevice : public modm::I2cDevice public: I2cTestDevice(uint8_t address = 0x3C); - modm::ResumableResult - write(uint16_t len); + bool + write(size_t len); - modm::ResumableResult - read(uint16_t len); + bool + read(size_t len); - modm::ResumableResult - writeRead(uint16_t write_len, uint16_t read_len); + bool + writeRead(size_t write_len, size_t read_len); private: uint8_t buffer[1024]; @@ -57,72 +54,45 @@ I2cTestDevice::I2cTestDevice(uint8_t address) : } template < typename I2cMaster > -modm::ResumableResult -I2cTestDevice::write(uint16_t len) +bool +I2cTestDevice::write(size_t len) { - RF_BEGIN(); - - buffer[0] = 0xaa; - buffer[1] = 0x55; - buffer[2] = 0x82; - buffer[3] = 0x11; - buffer[4] = 0x22; - - for (uint16_t ii = 0; ii < 1024; ++ii) { + for (size_t ii = 0; ii < 1024; ++ii) { buffer[ii] = ii + 1; } buffer[255] = 0x82; - this->transaction.configureWrite(buffer, len); - - if (not RF_CALL( this->runTransaction() )) { - RF_RETURN(false); - } - - RF_END_RETURN(true); + return modm::I2cDevice::write(buffer, len); } template < typename I2cMaster > -modm::ResumableResult -I2cTestDevice::read(uint16_t len) +bool +I2cTestDevice::read(size_t len) { - RF_BEGIN(); + if (not modm::I2cDevice::read(buffer, len)) return false; - this->transaction.configureRead(buffer, len); - - if (not RF_CALL( this->runTransaction() )) { - RF_RETURN(false); - } - for (uint16_t ii = 0; ii < len; ++ii) { + for (size_t ii = 0; ii < len; ++ii) { MODM_LOG_DEBUG.printf("%02x ", buffer[ii]); } MODM_LOG_DEBUG << modm::endl; - - RF_END_RETURN(true); + return true; } template < typename I2cMaster > -modm::ResumableResult -I2cTestDevice::writeRead(uint16_t write_len, uint16_t read_len) +bool +I2cTestDevice::writeRead(size_t write_len, size_t read_len) { - RF_BEGIN(); - - for (uint16_t ii = 0; ii < 1024; ++ii) { + for (size_t ii = 0; ii < 1024; ++ii) { buffer[ii] = ii + 1; } buffer[255] = 0x82; + if (not modm::I2cDevice::writeRead(buffer, write_len, buffer, read_len)) + return false; - this->transaction.configureWriteRead(buffer, write_len, buffer, read_len); - - if (not RF_CALL( this->runTransaction() )) { - RF_RETURN(false); - } - - for (uint16_t ii = 0; ii < read_len; ++ii) { + for (size_t ii = 0; ii < read_len; ++ii) { MODM_LOG_DEBUG.printf("%02x ", buffer[ii]); } MODM_LOG_DEBUG << modm::endl; - - RF_END_RETURN(true); + return true; } I2cTestDevice< MyI2cMaster > i2c; @@ -138,32 +108,33 @@ main() LedGreen::set(); - RF_CALL_BLOCKING(i2c.ping()); + i2c.ping(); modm::delay(25us); - RF_CALL_BLOCKING(i2c.write(0)); + i2c.write(0); modm::delay(25us); - RF_CALL_BLOCKING(i2c.write(1)); + i2c.write(1); modm::delay(25us); - RF_CALL_BLOCKING(i2c.write(2)); + i2c.write(2); modm::delay(25us); - RF_CALL_BLOCKING(i2c.writeRead(0, 5)); + i2c.writeRead(0, 5); modm::delay(25us); - RF_CALL_BLOCKING(i2c.writeRead(1, 5)); + i2c.writeRead(1, 5); modm::delay(25us); - RF_CALL_BLOCKING(i2c.writeRead(2, 5)); + i2c.writeRead(2, 5); modm::delay(25us); // Blink if run without hanging. - while(true) { + while(true) + { LedGreen::toggle(); modm::delay(500ms); - }; + } return 0; } diff --git a/examples/nucleo_l476rg/i2c_test/project.xml b/examples/nucleo_l476rg/i2c_test/project.xml index aa169b452c..ed2434fbf1 100644 --- a/examples/nucleo_l476rg/i2c_test/project.xml +++ b/examples/nucleo_l476rg/i2c_test/project.xml @@ -6,9 +6,7 @@ modm:architecture:i2c.device modm:platform:i2c:1 - modm:processing:protothread - modm:processing:resumable - modm:processing:timer + modm:processing:fiber modm:build:scons diff --git a/examples/rp_pico/logger/main.cpp b/examples/rp_pico/logger/main.cpp index 21f840fab6..33ec8b8e3d 100644 --- a/examples/rp_pico/logger/main.cpp +++ b/examples/rp_pico/logger/main.cpp @@ -15,8 +15,6 @@ #include #include -#include -#include // ---------------------------------------------------------------------------- // Set the log level @@ -32,41 +30,6 @@ modm::log::Logger modm::log::info(loggerDevice); modm::log::Logger modm::log::warning(loggerDevice); modm::log::Logger modm::log::error(loggerDevice); -class BlinkThread : public modm::pt::Protothread -{ -public: - BlinkThread() { timeout.restart(100ms); } - - bool - update() - { - PT_BEGIN(); - - while (true) - { - Board::LedGreen::reset(); - - PT_WAIT_UNTIL(timeout.isExpired()); - timeout.restart(100ms); - - Board::LedGreen::set(); - - PT_WAIT_UNTIL(timeout.isExpired()); - timeout.restart(900ms); - - MODM_LOG_INFO << "Seconds since reboot: " << ++uptime << modm::endl; - } - - PT_END(); - } - -private: - modm::ShortTimeout timeout; - uint32_t uptime; -}; - -BlinkThread blinkThread; - // ---------------------------------------------------------------------------- int main() @@ -84,7 +47,17 @@ main() MODM_LOG_WARNING << "warning" << modm::endl; MODM_LOG_ERROR << "error" << modm::endl; - while (true) { blinkThread.update(); } + uint32_t uptime{}; + while (true) + { + Board::LedGreen::reset(); + modm::delay(100ms); + + Board::LedGreen::set(); + modm::delay(900ms); + + MODM_LOG_INFO << "Seconds since reboot: " << ++uptime << modm::endl; + } return 0; } diff --git a/examples/rp_pico/logger/project.xml b/examples/rp_pico/logger/project.xml index 82492276ab..4acdae5497 100644 --- a/examples/rp_pico/logger/project.xml +++ b/examples/rp_pico/logger/project.xml @@ -5,10 +5,7 @@ modm:debug - modm:platform:gpio modm:platform:uart:0 - modm:processing:timer - modm:processing:protothread modm:build:scons diff --git a/examples/rp_pico/mclogger/main.cpp b/examples/rp_pico/mclogger/main.cpp index 17fec27ffa..8d5b34fcd7 100644 --- a/examples/rp_pico/mclogger/main.cpp +++ b/examples/rp_pico/mclogger/main.cpp @@ -11,8 +11,7 @@ #include #include -#include -#include +#include #include // ---------------------------------------------------------------------------- @@ -45,21 +44,20 @@ static multicore::Mutex log_mutex; #endif template -class Thread : public modm::pt::Protothread +class Thread : public modm::Fiber<> { static constexpr auto delay = 10ms + 1ms * Instance; public: - Thread() { timeout.restart(delay); } + Thread() : Fiber([this]{ run(); }, Core == 0 ? modm::fiber::Start::Now : modm::fiber::Start::Later) {} - bool - update() + void + run() { - PT_BEGIN(); + uint32_t uptime{}; while (true) { - PT_WAIT_UNTIL(timeout.isExpired()); - timeout.restart(delay); + modm::this_fiber::sleep_for(delay); { // try without this line for intermixed output LOG_GUARD(); @@ -67,42 +65,27 @@ class Thread : public modm::pt::Protothread << " thread: " << Instance << " uptime: " << ++uptime << modm::endl; } } - - PT_END(); } - -private: - modm::ShortTimeout timeout; - uint32_t uptime; }; -template -class Threads -{ -private: - Thread t0; - Thread t1; - Thread t2; - Thread t3; - -public: - void - update() - { - t0.update(); - t1.update(); - t2.update(); - t3.update(); - } -}; +Thread<0, 0> t00; +Thread<0, 1> t01; +Thread<0, 2> t02; +Thread<0, 3> t03; -Threads<0> core0; -Threads<1> core1; +Thread<1, 0> t10; +Thread<1, 1> t11; +Thread<1, 2> t12; +Thread<1, 3> t13; void core1_main() { - while (true) { core1.update(); } + t10.start(); + t11.start(); + t12.start(); + t13.start(); + modm::fiber::Scheduler::run(); } // ---------------------------------------------------------------------------- @@ -125,8 +108,6 @@ main() INIT_GUARD(); multicore::Core1::run(core1_main); - - while (true) { core0.update(); } - + modm::fiber::Scheduler::run(); return 0; } diff --git a/examples/rp_pico/mclogger/project.xml b/examples/rp_pico/mclogger/project.xml index 8f05615fd8..eaab88f459 100644 --- a/examples/rp_pico/mclogger/project.xml +++ b/examples/rp_pico/mclogger/project.xml @@ -8,8 +8,7 @@ modm:platform:gpio modm:platform:uart:0 modm:platform:multicore - modm:processing:timer - modm:processing:protothread + modm:processing:fiber modm:build:scons diff --git a/examples/rp_pico/rtc_mcp7941x/main.cpp b/examples/rp_pico/rtc_mcp7941x/main.cpp index 5a2a280f48..5cf833bd14 100644 --- a/examples/rp_pico/rtc_mcp7941x/main.cpp +++ b/examples/rp_pico/rtc_mcp7941x/main.cpp @@ -42,9 +42,9 @@ class RtcThread : public modm::pt::Protothread bool update() { - PT_BEGIN(); - if(PT_CALL(rtc.oscillatorRunning())) { + + if(rtc.oscillatorRunning()) { MODM_LOG_ERROR << "RTC oscillator is running." << modm::endl; } else { @@ -58,16 +58,14 @@ class RtcThread : public modm::pt::Protothread dateTime.hours = 0; dateTime.minutes = 0; dateTime.seconds = 0; - while(not PT_CALL(rtc.setDateTime(dateTime))) { + while(not rtc.setDateTime(dateTime)) { MODM_LOG_ERROR << "Unable to set date/time." << modm::endl; - timeout.restart(500ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(500ms); } - timeout.restart(500ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(500ms); - if(PT_CALL(rtc.oscillatorRunning())) { + if(rtc.oscillatorRunning()) { MODM_LOG_ERROR << "RTC oscillator is running." << modm::endl; } else { @@ -76,7 +74,7 @@ class RtcThread : public modm::pt::Protothread while (true) { - dateTime2 = PT_CALL(rtc.getDateTime()); + dateTime2 = rtc.getDateTime(); if(dateTime2.has_value()) { MODM_LOG_INFO.printf("%02u.%02u.%02u ", dateTime2->days, dateTime2->months, dateTime2->years); MODM_LOG_INFO.printf("%02u:%02u.%02uh\n", dateTime2->hours, dateTime2->minutes, dateTime2->seconds); @@ -85,11 +83,10 @@ class RtcThread : public modm::pt::Protothread MODM_LOG_ERROR << "Unable to read from RTC." << modm::endl; } - timeout.restart(2500ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(2500ms); } - PT_END(); + } private: diff --git a/examples/stm32f072_discovery/rotation/main.cpp b/examples/stm32f072_discovery/rotation/main.cpp index a7b7a6905a..5c3476e35d 100644 --- a/examples/stm32f072_discovery/rotation/main.cpp +++ b/examples/stm32f072_discovery/rotation/main.cpp @@ -37,15 +37,15 @@ class ReaderThread : public modm::pt::Protothread bool update() { - PT_BEGIN(); + // initialize with limited range of 250 degrees per second - PT_CALL(gyro.configure(gyro.Scale::Dps250)); + gyro.configure(gyro.Scale::Dps250); while (true) { // read out the sensor - PT_CALL(gyro.readRotation()); + gyro.readRotation(); // update the moving average averageZ.update(gyro.getData().getZ()); @@ -60,11 +60,10 @@ class ReaderThread : public modm::pt::Protothread } // repeat every 5 ms - timeout.restart(5ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(5ms); } - PT_END(); + } private: diff --git a/examples/stm32f072_discovery/tmp102/main.cpp b/examples/stm32f072_discovery/tmp102/main.cpp index 8007858c66..949c5f7f25 100644 --- a/examples/stm32f072_discovery/tmp102/main.cpp +++ b/examples/stm32f072_discovery/tmp102/main.cpp @@ -38,34 +38,33 @@ class ThreadOne : public modm::pt::Protothread { temp.update(); - PT_BEGIN(); + // ping the device until it responds while(true) { // we wait until the task started - if (PT_CALL(temp.ping())) + if (temp.ping()) break; // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(100ms); } - PT_CALL(temp.setUpdateRate(200)); - PT_CALL(temp.enableExtendedMode()); + temp.setUpdateRate(200); + temp.enableExtendedMode(); PT_CALL(temp.configureAlertMode( modm::tmp102::ThermostatMode::Comparator, modm::tmp102::AlertPolarity::ActiveLow, modm::tmp102::FaultQueue::Faults6)); - PT_CALL(temp.setLowerLimit(28.f)); - PT_CALL(temp.setUpperLimit(30.f)); + temp.setLowerLimit(28.f); + temp.setUpperLimit(30.f); while (true) { { - PT_CALL(temp.readComparatorMode(result)); + temp.readComparatorMode(result); float temperature = temperatureData.getTemperature(); uint8_t tI = (int) temperature; uint16_t tP = (temperature - tI) * 10000; @@ -85,12 +84,11 @@ class ThreadOne : public modm::pt::Protothread stream << modm::endl; if (result) stream << "Heat me up!" << modm::endl; } - timeout.restart(200ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(200ms); Board::LedDown::toggle(); } - PT_END(); + } private: diff --git a/examples/stm32f3_discovery/accelerometer/main.cpp b/examples/stm32f3_discovery/accelerometer/main.cpp index 21658e0dd5..b262295473 100644 --- a/examples/stm32f3_discovery/accelerometer/main.cpp +++ b/examples/stm32f3_discovery/accelerometer/main.cpp @@ -28,15 +28,15 @@ class ReaderThread : public modm::pt::Protothread bool update() { - PT_BEGIN(); + // initialize with limited range of ±2g - PT_CALL(accelerometer.configure(accelerometer.Scale::G2)); + accelerometer.configure(accelerometer.Scale::G2); while (true) { // read out the sensor - PT_CALL(accelerometer.readAcceleration()); + accelerometer.readAcceleration(); averageX.update(accelerometer.getData().getX()); averageY.update(accelerometer.getData().getY()); @@ -61,11 +61,10 @@ class ReaderThread : public modm::pt::Protothread } // repeat every 5 ms - timeout.restart(5ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(5ms); } - PT_END(); + } private: diff --git a/examples/stm32f401_discovery/accelerometer/main.cpp b/examples/stm32f401_discovery/accelerometer/main.cpp index 7260dc6124..7912805b9f 100644 --- a/examples/stm32f401_discovery/accelerometer/main.cpp +++ b/examples/stm32f401_discovery/accelerometer/main.cpp @@ -29,15 +29,15 @@ class ReaderThread : public modm::pt::Protothread bool update() { - PT_BEGIN(); + // initialize with limited range of ±2g - PT_CALL(accelerometer.configure(accelerometer.Scale::G2)); + accelerometer.configure(accelerometer.Scale::G2); while (true) { // read out the sensor - PT_CALL(accelerometer.readAcceleration()); + accelerometer.readAcceleration(); averageX.update(accelerometer.getData().getX()); averageY.update(accelerometer.getData().getY()); @@ -57,11 +57,10 @@ class ReaderThread : public modm::pt::Protothread } // repeat every 5 ms - timeout.restart(5ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(5ms); } - PT_END(); + } private: diff --git a/examples/stm32f401_discovery/gyroscope/main.cpp b/examples/stm32f401_discovery/gyroscope/main.cpp index 233aaa42df..cd9194b1a9 100644 --- a/examples/stm32f401_discovery/gyroscope/main.cpp +++ b/examples/stm32f401_discovery/gyroscope/main.cpp @@ -38,15 +38,15 @@ class ReaderThread : public modm::pt::Protothread bool update() { - PT_BEGIN(); + // initialize with limited range of 250 degrees per second - PT_CALL(gyro.configure(gyro.Scale::Dps250)); + gyro.configure(gyro.Scale::Dps250); while (true) { // read out the sensor - PT_CALL(gyro.readRotation()); + gyro.readRotation(); // update the moving average averageZ.update(gyro.getData().getZ()); @@ -61,11 +61,10 @@ class ReaderThread : public modm::pt::Protothread } // repeat every 5 ms - timeout.restart(5ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(5ms); } - PT_END(); + } private: diff --git a/examples/stm32f469_discovery/max31865/main.cpp b/examples/stm32f469_discovery/max31865/main.cpp index af655aeeea..11c66a0962 100644 --- a/examples/stm32f469_discovery/max31865/main.cpp +++ b/examples/stm32f469_discovery/max31865/main.cpp @@ -30,13 +30,13 @@ class ThermocoupleThread : public modm::pt::Protothread bool run() { - PT_BEGIN(); - PT_CALL(pt100.initialize()); + + pt100.initialize(); while (true) { MODM_LOG_INFO << "\nNew readout:" << modm::endl; - PT_CALL(pt100.readout()); + pt100.readout(); MODM_LOG_INFO << " resistance : " << data.getResistance() << " Ohm" << modm::endl; @@ -45,11 +45,10 @@ class ThermocoupleThread : public modm::pt::Protothread MODM_LOG_INFO << " temperature precise: " << data.getTemperaturePrecise() << " degrees" << modm::endl; - timeout.restart(std::chrono::milliseconds(1000)); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(std::chrono::milliseconds(1000)); } - PT_END(); + } private: @@ -85,4 +84,4 @@ main() } return 0; -} \ No newline at end of file +} diff --git a/examples/stm32f469_discovery/touchscreen/main.cpp b/examples/stm32f469_discovery/touchscreen/main.cpp index ad23a7f1a5..1d33ea2e8d 100644 --- a/examples/stm32f469_discovery/touchscreen/main.cpp +++ b/examples/stm32f469_discovery/touchscreen/main.cpp @@ -29,22 +29,22 @@ class LineDrawer : public modm::pt::Protothread bool update() { - PT_BEGIN(); + // Configure the touchscreen to sample with 60Hz in active and monitor mode. - PT_CALL(touch.configure(Touch::InterruptMode::Trigger, 60, 60)); + touch.configure(Touch::InterruptMode::Trigger, 60, 60); while (true) { do { // Wait for either touchscreen interrupt or clear screen button - PT_WAIT_UNTIL(Int::read() or Button::read()); + modm::this_fiber::poll([&]{ return Int::read() or Button::read(); }); if (Button::read()) display.clear(); } while (not Int::read()); LedRed::set(); - PT_CALL(touch.readTouches()); + touch.readTouches(); for (int ii=0; ii < 2; ii++) { @@ -77,7 +77,7 @@ class LineDrawer : public modm::pt::Protothread LedRed::reset(); } - PT_END(); + } private: diff --git a/examples/stm32f4_discovery/accelerometer/main.cpp b/examples/stm32f4_discovery/accelerometer/main.cpp index 098b8fbab9..c5cad1413c 100644 --- a/examples/stm32f4_discovery/accelerometer/main.cpp +++ b/examples/stm32f4_discovery/accelerometer/main.cpp @@ -57,27 +57,27 @@ class ReaderThread : public modm::pt::Protothread bool update() { - PT_BEGIN(); + // ping the device until it responds while(true) { // we wait until the task started - if (PT_CALL(accel.ping())) + if (accel.ping()) break; // otherwise, try again in 100ms timeout.restart(100ms); Board::LedOrange::set(); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::poll([&]{ return timeout.isExpired(); }); Board::LedOrange::reset(); } // initialize with limited range of ~2.3G - PT_CALL(accel.configure(accel.Scale::G2, accel.MeasurementRate::Hz400)); + accel.configure(accel.Scale::G2, accel.MeasurementRate::Hz400); while (true) { - PT_CALL(accel.readAcceleration()); + accel.readAcceleration(); #if REVISION_C averageX.update(-accel.getData().getY()); @@ -92,11 +92,10 @@ class ReaderThread : public modm::pt::Protothread Board::LedGreen::set(averageY.getValue() < -0.2f); Board::LedRed::set(averageY.getValue() > 0.2f); - timeout.restart(5ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(5ms); } - PT_END(); + } private: diff --git a/examples/stm32f4_discovery/barometer_bmp085_bmp180/main.cpp b/examples/stm32f4_discovery/barometer_bmp085_bmp180/main.cpp index 3243925b3d..da10b7054f 100644 --- a/examples/stm32f4_discovery/barometer_bmp085_bmp180/main.cpp +++ b/examples/stm32f4_discovery/barometer_bmp085_bmp180/main.cpp @@ -53,11 +53,10 @@ class ThreadOne : public modm::pt::Protothread while(true) { // we wait until the task started - if (PT_CALL(barometer.ping())) + if (barometer.ping()) break; // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(100ms); } stream << "Device responded" << modm::endl; @@ -66,11 +65,10 @@ class ThreadOne : public modm::pt::Protothread while(true) { // we wait until the task started - if (PT_CALL(barometer.initialize())) + if (barometer.initialize()) break; // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(100ms); } stream << "Device configured" << modm::endl; @@ -94,10 +92,10 @@ class ThreadOne : public modm::pt::Protothread { static modm::ShortPeriodicTimer timer(250ms); - PT_WAIT_UNTIL(timer.execute()); + modm::this_fiber::poll([&]{ return timer.execute(); }); // Returns when new data was read from the sensor - PT_CALL(barometer.readout()); + barometer.readout(); { int16_t temp = data.getTemperature(); @@ -108,7 +106,7 @@ class ThreadOne : public modm::pt::Protothread } } - PT_END(); + } private: diff --git a/examples/stm32f4_discovery/colour_tcs3414/main.cpp b/examples/stm32f4_discovery/colour_tcs3414/main.cpp index 00c804a98c..96b2278d2d 100644 --- a/examples/stm32f4_discovery/colour_tcs3414/main.cpp +++ b/examples/stm32f4_discovery/colour_tcs3414/main.cpp @@ -45,7 +45,7 @@ class ThreadOne : public modm::pt::Protothread bool update() { - PT_BEGIN(); + stream << "Ping the device from ThreadOne" << modm::endl; @@ -53,24 +53,22 @@ class ThreadOne : public modm::pt::Protothread while (true) { // we wait until the task started - if (PT_CALL(sensor.ping())) { + if (sensor.ping()) { break; } // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(100ms); } stream << "Device responded" << modm::endl; while (true) { - if (PT_CALL(sensor.initialize())) { + if (sensor.initialize()) { break; } // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(100ms); } stream << "Device initialized" << modm::endl; @@ -90,23 +88,21 @@ class ThreadOne : public modm::pt::Protothread break; } // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(100ms); } stream << "Device configured" << modm::endl; while (true) { - if (PT_CALL(sensor.readColor())) { + if (sensor.readColor()) { const auto rgb = data.getColor(); stream << "RGB: " << rgb << "\tHSV: " << modm::color::Hsv(rgb) << modm::endl; } - timeout.restart(500ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(500ms); } - PT_END(); + } private: diff --git a/examples/stm32f4_discovery/display/hd44780/main.cpp b/examples/stm32f4_discovery/display/hd44780/main.cpp index 332ba2a369..d70aaf41a7 100644 --- a/examples/stm32f4_discovery/display/hd44780/main.cpp +++ b/examples/stm32f4_discovery/display/hd44780/main.cpp @@ -110,21 +110,20 @@ class ThreadOne : public modm::pt::Protothread bool update() { - PT_BEGIN(); + MODM_LOG_DEBUG << "Ping the device from ThreadOne" << modm::endl; // ping the device until it responds while (true) { // we wait until the task started - if (PT_CALL(gpioExpander.ping())) { + if (gpioExpander.ping()) { break; } MODM_LOG_DEBUG << "Device did not respond" << modm::endl; // otherwise, try again in 100ms - timeout.restart(1s); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(1s); } MODM_LOG_DEBUG << "Device responded" << modm::endl; @@ -168,11 +167,10 @@ class ThreadOne : public modm::pt::Protothread counter++; - timeout.restart(1s); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(1s); } - PT_END(); + } private: diff --git a/examples/stm32f4_discovery/display/nokia_5110/main.cpp b/examples/stm32f4_discovery/display/nokia_5110/main.cpp index 3024268520..c2c9950a35 100644 --- a/examples/stm32f4_discovery/display/nokia_5110/main.cpp +++ b/examples/stm32f4_discovery/display/nokia_5110/main.cpp @@ -70,7 +70,7 @@ class ThreadOne : public modm::pt::Protothread bool update() { - PT_BEGIN(); + lcd::Reset::setOutput(modm::Gpio::Low); lcd::Ce::setOutput(modm::Gpio::High); @@ -95,11 +95,10 @@ class ThreadOne : public modm::pt::Protothread counter++; - timeout.restart(1s); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(1s); } - PT_END(); + } private: diff --git a/examples/stm32f4_discovery/distance_vl6180/main.cpp b/examples/stm32f4_discovery/distance_vl6180/main.cpp index c525add543..08ec2bfb17 100644 --- a/examples/stm32f4_discovery/distance_vl6180/main.cpp +++ b/examples/stm32f4_discovery/distance_vl6180/main.cpp @@ -53,7 +53,7 @@ class ThreadOne : public modm::pt::Protothread bool update() { - PT_BEGIN(); + MODM_LOG_DEBUG << "Ping the device from ThreadOne" << modm::endl; @@ -61,35 +61,33 @@ class ThreadOne : public modm::pt::Protothread while (true) { // we wait until the task started - if (PT_CALL(distance.ping())) { + if (distance.ping()) { break; } // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(100ms); } MODM_LOG_DEBUG << "Device responded" << modm::endl; while (true) { - if (PT_CALL(distance.initialize())) + if (distance.initialize()) break; // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(100ms); } MODM_LOG_DEBUG << "Device initialized" << modm::endl; timeout.restart(1ms); - PT_CALL(distance.setIntegrationTime(10)); + distance.setIntegrationTime(10); while (true) { stamp = modm::Clock::now(); - if (PT_CALL(distance.readDistance())) + if (distance.readDistance()) { modm::vl6180::RangeErrorCode error = distance.getRangeError(); if (error == distance.RangeErrorCode::NoError) @@ -111,7 +109,7 @@ class ThreadOne : public modm::pt::Protothread MODM_LOG_DEBUG << "\tt=" << (modm::Clock::now() - stamp); stamp = modm::Clock::now(); - if (PT_CALL(distance.readAmbientLight())) + if (distance.readAmbientLight()) { modm::vl6180::ALS_ErrorCode error = distance.getALS_Error(); if (error == distance.ALS_ErrorCode::NoError) @@ -126,11 +124,11 @@ class ThreadOne : public modm::pt::Protothread MODM_LOG_DEBUG << " \tt=" << (modm::Clock::now() - stamp) << modm::endl; - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::poll([&]{ return timeout.isExpired(); }); timeout.restart(40ms); } - PT_END(); + } private: diff --git a/examples/stm32f4_discovery/pressure_ams5915/main.cpp b/examples/stm32f4_discovery/pressure_ams5915/main.cpp index f341ba5e6b..cce8bdfea2 100644 --- a/examples/stm32f4_discovery/pressure_ams5915/main.cpp +++ b/examples/stm32f4_discovery/pressure_ams5915/main.cpp @@ -53,7 +53,7 @@ class ThreadOne : public modm::pt::Protothread bool update() { - PT_BEGIN(); + MODM_LOG_DEBUG << "Ping the device from ThreadOne" << modm::endl; @@ -61,32 +61,31 @@ class ThreadOne : public modm::pt::Protothread while (true) { // we wait until the task started - if (PT_CALL(pressureSensor.ping())) { + if (pressureSensor.ping()) { break; } // otherwise, try again in 10ms - timeout.restart(10ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(10ms); } MODM_LOG_DEBUG << "Device responded" << modm::endl; while (true) { - while (! PT_CALL(pressureSensor.readPressure())) + while (! pressureSensor.readPressure()) { - PT_YIELD(); + modm::this_fiber::yield(); + } MODM_LOG_INFO << "Pressure [0..1]: " << data.getPressure() << modm::endl; MODM_LOG_INFO << "Temperature [degree centigrade]: " << data.getTemperature() << modm::endl; // read next pressure measurement in 1ms - timeout.restart(1ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(1ms); } - PT_END(); + } private: modm::ShortTimeout timeout; diff --git a/examples/stm32f4_discovery/protothreads/main.cpp b/examples/stm32f4_discovery/protothreads/main.cpp index 876c1362ae..b9af50f35d 100644 --- a/examples/stm32f4_discovery/protothreads/main.cpp +++ b/examples/stm32f4_discovery/protothreads/main.cpp @@ -38,34 +38,33 @@ class ThreadOne : public modm::pt::Protothread { temp.update(); - PT_BEGIN(); + // ping the device until it responds while(true) { // we wait until the task started - if (PT_CALL(temp.ping())) + if (temp.ping()) break; // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(100ms); } - PT_CALL(temp.setUpdateRate(200)); - PT_CALL(temp.enableExtendedMode()); + temp.setUpdateRate(200); + temp.enableExtendedMode(); PT_CALL(temp.configureAlertMode( modm::tmp102::ThermostatMode::Comparator, modm::tmp102::AlertPolarity::ActiveLow, modm::tmp102::FaultQueue::Faults6)); - PT_CALL(temp.setLowerLimit(28.f)); - PT_CALL(temp.setUpperLimit(30.f)); + temp.setLowerLimit(28.f); + temp.setUpperLimit(30.f); while (true) { { - PT_CALL(temp.readComparatorMode(result)); + temp.readComparatorMode(result); float temperature = temperatureData.getTemperature(); uint8_t tI = (int) temperature; uint16_t tP = (temperature - tI) * 10000; @@ -85,12 +84,11 @@ class ThreadOne : public modm::pt::Protothread stream << modm::endl; if (result) stream << "Heat me up!" << modm::endl; } - timeout.restart(200ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(200ms); Board::LedRed::toggle(); } - PT_END(); + } private: diff --git a/examples/stm32f4_discovery/temperature_ltc2984/main.cpp b/examples/stm32f4_discovery/temperature_ltc2984/main.cpp index 25579682fb..6cdbad33bb 100644 --- a/examples/stm32f4_discovery/temperature_ltc2984/main.cpp +++ b/examples/stm32f4_discovery/temperature_ltc2984/main.cpp @@ -78,12 +78,11 @@ class ThreadOne : public modm::pt::Protothread bool update() { - PT_BEGIN(); - while(!PT_CALL(tempSensor.ping())) { + + while(!tempSensor.ping()) { logger << "Device not reachable" << modm::endl; - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(100ms); } @@ -100,33 +99,32 @@ class ThreadOne : public modm::pt::Protothread modm::ltc2984::Configuration::Rtd::RtdCurve::European ))); tempSensor.enableChannel(modm::ltc2984::Configuration::MuxChannel::Ch4); - PT_CALL(tempSensor.setChannels()); + tempSensor.setChannels(); logger << "Device configured" << modm::endl; while (true) { - //PT_CALL(tempSensor.initiateMeasurements()); - PT_CALL(tempSensor.initiateSingleMeasurement(modm::ltc2984::Channel::Ch4)); + //tempSensor.initiateMeasurements(); + tempSensor.initiateSingleMeasurement(modm::ltc2984::Channel::Ch4); stamp = modm::Clock::now(); // we wait until the conversations are done - while (PT_CALL(tempSensor.isBusy())) + while (tempSensor.isBusy()) { } logger << "Temperature measurement finished." << modm::endl; - PT_CALL(tempSensor.readChannel(modm::ltc2984::Channel::Ch4, temp)); + tempSensor.readChannel(modm::ltc2984::Channel::Ch4, temp); logger << "Temperature: " << temp << modm::endl; logger << "Time: " << (modm::Clock::now() - stamp) << modm::endl; - timeout.restart(1s); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(1s); } - PT_END(); + } private: diff --git a/examples/stm32f746g_discovery/adc_ad7928/main.cpp b/examples/stm32f746g_discovery/adc_ad7928/main.cpp index 85c1a754db..bcfbeb7035 100644 --- a/examples/stm32f746g_discovery/adc_ad7928/main.cpp +++ b/examples/stm32f746g_discovery/adc_ad7928/main.cpp @@ -54,44 +54,43 @@ class ThreadOne : public modm::pt::Protothread bool update() { - PT_BEGIN(); + MODM_LOG_INFO << "Initialize device" << modm::endl; - PT_CALL(adc.initialize()); + adc.initialize(); MODM_LOG_INFO << "Test single conversions (Ch 0-2):" << modm::endl; // Initiate first conversion, result will be output during the next conversion - PT_CALL(adc.singleConversion(ad7928::InputChannel::Ch0)); + adc.singleConversion(ad7928::InputChannel::Ch0); - MODM_LOG_INFO << PT_CALL(adc.singleConversion(ad7928::InputChannel::Ch1)) << modm::endl; - MODM_LOG_INFO << PT_CALL(adc.singleConversion(ad7928::InputChannel::Ch2)) << modm::endl; + MODM_LOG_INFO << adc.singleConversion(ad7928::InputChannel::Ch1) << modm::endl; + MODM_LOG_INFO << adc.singleConversion(ad7928::InputChannel::Ch2) << modm::endl; // Enable auto-shutdown adc.setAutoShutdownEnabled(true); - MODM_LOG_INFO << PT_CALL(adc.singleConversion(ad7928::InputChannel::Ch2)) << modm::endl; + MODM_LOG_INFO << adc.singleConversion(ad7928::InputChannel::Ch2) << modm::endl; MODM_LOG_INFO << "Test single conversion with auto-shutdown (Ch 5):" << modm::endl; - PT_CALL(adc.singleConversion(ad7928::InputChannel::Ch5)); - MODM_LOG_INFO << PT_CALL(adc.singleConversion(ad7928::InputChannel::Ch0)) << modm::endl; + adc.singleConversion(ad7928::InputChannel::Ch5); + MODM_LOG_INFO << adc.singleConversion(ad7928::InputChannel::Ch0) << modm::endl; adc.setAutoShutdownEnabled(false); MODM_LOG_INFO << "Test sequence mode" << modm::endl; MODM_LOG_INFO << "Program sequence Ch0,Ch1,Ch5, Ch0,Ch4,Ch5" << modm::endl; - PT_CALL(adc.startSequence(sequence1, sequence2)); + adc.startSequence(sequence1, sequence2); // Run forever while (true) { - MODM_LOG_INFO << PT_CALL(adc.nextSequenceConversion()) << modm::endl; + MODM_LOG_INFO << adc.nextSequenceConversion() << modm::endl; - timeout.restart(500ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(500ms); } - PT_END(); + } private: diff --git a/examples/stm32f746g_discovery/tmp102/main.cpp b/examples/stm32f746g_discovery/tmp102/main.cpp index 7921edb050..5d19d79572 100644 --- a/examples/stm32f746g_discovery/tmp102/main.cpp +++ b/examples/stm32f746g_discovery/tmp102/main.cpp @@ -32,34 +32,33 @@ class ThreadOne : public modm::pt::Protothread { temp.update(); - PT_BEGIN(); + // ping the device until it responds while(true) { // we wait until the task started - if (PT_CALL(temp.ping())) + if (temp.ping()) break; // otherwise, try again in 100ms - timeout.restart(100ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(100ms); } - PT_CALL(temp.setUpdateRate(200)); - PT_CALL(temp.enableExtendedMode()); + temp.setUpdateRate(200); + temp.enableExtendedMode(); PT_CALL(temp.configureAlertMode( modm::tmp102::ThermostatMode::Comparator, modm::tmp102::AlertPolarity::ActiveLow, modm::tmp102::FaultQueue::Faults6)); - PT_CALL(temp.setLowerLimit(28.f)); - PT_CALL(temp.setUpperLimit(30.f)); + temp.setLowerLimit(28.f); + temp.setUpperLimit(30.f); while (true) { { - PT_CALL(temp.readComparatorMode(result)); + temp.readComparatorMode(result); float temperature = temperatureData.getTemperature(); uint8_t tI = (int) temperature; uint16_t tP = (temperature - tI) * 10000; @@ -79,12 +78,11 @@ class ThreadOne : public modm::pt::Protothread if (result) { MODM_LOG_INFO << " Heat me up!"; } MODM_LOG_INFO << modm::endl; } - timeout.restart(200ms); - PT_WAIT_UNTIL(timeout.isExpired()); + modm::this_fiber::sleep_for(200ms); Board::LedD13::toggle(); } - PT_END(); + } private: