diff --git a/src/boards/Ciuti/Algorithm/UprightDetector.cpp b/src/boards/Ciuti/Algorithm/UprightDetector.cpp index bf62f453f3b379c6d496cbc7d8cb972cdb24157c..ab1649ea8f33e2e995bf1e50cf713d2f0279058f 100644 --- a/src/boards/Ciuti/Algorithm/UprightDetector.cpp +++ b/src/boards/Ciuti/Algorithm/UprightDetector.cpp @@ -31,6 +31,7 @@ /* % Matlab algorithm +% Author: Marco Marchesi testlog = ciutilog; testlog.timestamp = testlog.timestamp*1e-6; angle_trigger = deg2rad(75); @@ -59,27 +60,12 @@ for i = 1:length(z_filter) end */ -namespace Ciuti -{ - -void UprightDetector::start() -{ - BoardScheduler::getInstance().getScheduler().addTask( - [=]() { this->update(); }, UprightDetectorConfig::ALGO_PERIOD); -} +using namespace Boardcore; -void UprightDetector::update() +namespace Ciuti { - auto sample = Sensors::getInstance().getLIS331HHLastSample(); - - if (!triggered) - { - algoStep(sample.accelerationZ - - Ciuti::SensorsConfig::Z_AXIS_OFFSET_LIS331HH); - } -} -void UprightDetector::algoStep(float axis) +void UprightDetector::update(float axis) { filtered.push(axis); @@ -92,17 +78,36 @@ void UprightDetector::algoStep(float axis) count = 0; } - if (count > UprightDetectorConfig::DETECT_SAMPLES && !triggered) - { - triggered = true; - trigger(); + upright = count > UprightDetectorConfig::DETECT_SAMPLES; +} + +void UprightDetectorController::start() +{ + BoardScheduler::getInstance().getScheduler().addTask( + [=]() { this->update(); }, UprightDetectorConfig::ALGO_PERIOD); +} + +void UprightDetectorController::update() +{ + auto sample = Sensors::getInstance().getLIS331HHLastSample(); + + if(!fired) { + algo.update(sample.accelerationZ - + Ciuti::SensorsConfig::Z_AXIS_OFFSET_LIS331HH); + + if(algo.isUpright()) { + trigger(); + fired = true; + } } } -void UprightDetector::trigger() +void UprightDetectorController::trigger() { LOG_INFO(logger, "Upright triggered!"); - // TODO: + + Logger::getInstance().resetStats(); + Logger::getInstance().start(); } } // namespace Ciuti diff --git a/src/boards/Ciuti/Algorithm/UprightDetector.h b/src/boards/Ciuti/Algorithm/UprightDetector.h index af754211487539f18d22047b7d75837330f02a28..ba96e58363d330601b785b7f88579e60e97e68b4 100644 --- a/src/boards/Ciuti/Algorithm/UprightDetector.h +++ b/src/boards/Ciuti/Algorithm/UprightDetector.h @@ -32,25 +32,38 @@ namespace Ciuti { -class UprightDetector : public Boardcore::Singleton<UprightDetector> +class UprightDetector { - friend Boardcore::Singleton<UprightDetector>; - public: UprightDetector() {} + void update(float axis); + bool isUpright() { return upright; } + +private: + bool upright = false; + int count = 0; + Boardcore::MovingAverage<float, UprightDetectorConfig::MEAN_SAMPLES> + filtered; +}; + +class UprightDetectorController + : public Boardcore::Singleton<UprightDetectorController> +{ + friend class Boardcore::Singleton<UprightDetectorController>; + +public: void start(); private: - void update(); + UprightDetectorController() {} + void update(); void trigger(); - void algoStep(float axis); - bool triggered = false; - int count = 0; - Boardcore::MovingAverage<float, UprightDetectorConfig::MEAN_SAMPLES> - filtered; + bool fired = false; + + UprightDetector algo; Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("ciuti.uprightdetector"); diff --git a/src/boards/Ciuti/Configs/SensorsConfig.h b/src/boards/Ciuti/Configs/SensorsConfig.h index 0610aa0d5fb0f56387468bad7332f6ee0237a2fb..0699f8d43d7bc28dd709c8085484444787ecbaad 100644 --- a/src/boards/Ciuti/Configs/SensorsConfig.h +++ b/src/boards/Ciuti/Configs/SensorsConfig.h @@ -36,7 +36,7 @@ constexpr Boardcore::InternalADC::Channel INTERNAL_ADC_CH_0 = Boardcore::InternalADC::Channel::CH0; constexpr Boardcore::InternalADC::Channel INTERNAL_ADC_CH_1 = Boardcore::InternalADC::Channel::CH1; -constexpr unsigned int SAMPLE_PERIOD_INTERNAL_ADC = 1; +constexpr unsigned int SAMPLE_PERIOD_INTERNAL_ADC = 200; // LIS331HH constexpr unsigned int SAMPLE_PERIOD_LIS331HH = 1000 / 50; diff --git a/src/boards/Ciuti/Serial/SerialWatcher.cpp b/src/boards/Ciuti/Serial/SerialWatcher.cpp index cd1648b251b52c44aed2418189e7e48d53ac2efe..4b648d1446edd3b2fcb8ea6af67c056d8ff706ed 100644 --- a/src/boards/Ciuti/Serial/SerialWatcher.cpp +++ b/src/boards/Ciuti/Serial/SerialWatcher.cpp @@ -22,25 +22,57 @@ #include "SerialWatcher.h" -#include <utils/Debug.h> +#include <Ciuti/BoardScheduler.h> #include <Ciuti/Buses.h> +#include <drivers/timer/TimestampTimer.h> #include <miosix.h> +#include <utils/Debug.h> + +using namespace miosix; +using namespace Boardcore; + +// Simple xorshift RNG +uint32_t xorshift32() +{ + // https://xkcd.com/221/ + static uint32_t STATE = 0x08104444; + + uint32_t x = STATE; + x ^= x << 13; + x ^= x >> 17; + x ^= x << 5; + + // Do I really need to lock this? Nah, introduce extra randomness. + return STATE = x; +} namespace Ciuti { +SerialWatcher::SerialWatcher(USART &usart, unsigned int id) : usart(usart) +{ + stats.usart_id = id; +} + void SerialWatcher::run() { - while(isRunning()) { - uint8_t sent = 25; - uint8_t recv; + while (isRunning()) + { + long long now = getTick(); + + uint32_t sent = xorshift32(); + uint32_t recv; + // Perform a loopback read usart.clearQueue(); - usart.write(&sent, 1); + usart.write(&sent, 4); + usart.read(&recv, 4); - usart.read(&recv, 1); + if (sent != recv) + stats.error_count += 1; - miosix::Thread::sleep(1000); + stats.last_timestamp = TimestampTimer::getTimestamp(); + Thread::sleepUntil(now + SerialWatcher::PERIOD); } } @@ -48,24 +80,28 @@ void SerialWatcherController::start() { serial_watcher1->start(); serial_watcher2->start(); -} -void SerialWatcherController::stop() -{ - serial_watcher1->stop(); - serial_watcher2->stop(); + BoardScheduler::getInstance().getScheduler().addTask( + [=]() { Logger::getInstance().log(this->serial_watcher1->getStats()); }, + SerialWatcher::PERIOD); + + BoardScheduler::getInstance().getScheduler().addTask( + [=]() { Logger::getInstance().log(this->serial_watcher2->getStats()); }, + SerialWatcher::PERIOD); + + LOG_INFO(logger, "Serial watcher controller setup done!"); } -SerialWatcherController::SerialWatcherController() +SerialWatcherController::SerialWatcherController() { - serial_watcher1 = new SerialWatcher(Ciuti::Buses::getInstance().usart2); - serial_watcher2 = new SerialWatcher(Ciuti::Buses::getInstance().usart3); + serial_watcher1 = new SerialWatcher(Ciuti::Buses::getInstance().usart2, 2); + serial_watcher2 = new SerialWatcher(Ciuti::Buses::getInstance().usart3, 3); } -SerialWatcherController::~SerialWatcherController() +SerialWatcherController::~SerialWatcherController() { delete serial_watcher1; delete serial_watcher2; } -} \ No newline at end of file +} // namespace Ciuti \ No newline at end of file diff --git a/src/boards/Ciuti/Serial/SerialWatcher.h b/src/boards/Ciuti/Serial/SerialWatcher.h index 837cc4c3df897acbdd6572a1b02bb28cfb689cfe..dbb3d2961a5845c1a6f354f093891111e7a0363a 100644 --- a/src/boards/Ciuti/Serial/SerialWatcher.h +++ b/src/boards/Ciuti/Serial/SerialWatcher.h @@ -25,6 +25,9 @@ #include <ActiveObject.h> #include <Singleton.h> #include <drivers/usart/USART.h> +#include <diagnostic/PrintLogger.h> + +#include "SerialWatcherStats.h" namespace Ciuti { @@ -32,11 +35,17 @@ namespace Ciuti class SerialWatcher : public Boardcore::ActiveObject { public: - SerialWatcher(Boardcore::USART &usart) : usart(usart) {} + static constexpr long long PERIOD = 1000; + + SerialWatcher(Boardcore::USART &usart, unsigned int id); + + SerialWatcherStats getStats() { return stats; } private: void run() override; + SerialWatcherStats stats; + Boardcore::USART &usart; }; @@ -47,7 +56,6 @@ class SerialWatcherController public: void start(); - void stop(); private: SerialWatcherController(); @@ -55,6 +63,9 @@ private: SerialWatcher *serial_watcher1 = nullptr; SerialWatcher *serial_watcher2 = nullptr; + + Boardcore::PrintLogger logger = + Boardcore::Logging::getLogger("ciuti.serialwatcher"); }; } // namespace Ciuti \ No newline at end of file diff --git a/src/boards/Ciuti/Serial/SerialWatcherStats.h b/src/boards/Ciuti/Serial/SerialWatcherStats.h new file mode 100644 index 0000000000000000000000000000000000000000..9f083fd2d30eeb2ecc5c1b1f6c87e4bdde51e6e0 --- /dev/null +++ b/src/boards/Ciuti/Serial/SerialWatcherStats.h @@ -0,0 +1,37 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Davide Mor + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <cstdint> + +namespace Ciuti +{ + +struct SerialWatcherStats +{ + uint64_t last_timestamp = 0; + unsigned int error_count = 0; + unsigned int usart_id = 0; +}; + +} \ No newline at end of file diff --git a/src/entrypoints/Ciuti/ciuti-entry.cpp b/src/entrypoints/Ciuti/ciuti-entry.cpp index a6e2494025d084408f91167ca090920c0988dbe9..9b75822c5956614f64ee786d33bcdf099aa981c5 100644 --- a/src/entrypoints/Ciuti/ciuti-entry.cpp +++ b/src/entrypoints/Ciuti/ciuti-entry.cpp @@ -32,22 +32,42 @@ using namespace miosix; using namespace Boardcore; using namespace Ciuti; +void printCiutiBanner() +{ + printf( + R"( Booting ... )""\n" + R"( ________ ___ ___ ___ _________ ___ )""\n" + R"(|\ ____\|\ \|\ \|\ \|\___ ___\\ \ )""\n" + R"(\ \ \___|\ \ \ \ \\\ \|___ \ \_\ \ \ )""\n" + R"( \ \ \ \ \ \ \ \\\ \ \ \ \ \ \ \ )""\n" + R"( \ \ \____\ \ \ \ \\\ \ \ \ \ \ \ \ )""\n" + R"( \ \_______\ \__\ \_______\ \ \__\ \ \__\)""\n" + R"( \|_______|\|__|\|_______| \|__| \|__|)""\n" + R"( On Board Software )""\n" + ); +} + int main() { - // Logger::getInstance().start(); + printCiutiBanner(); + + Boardcore::PrintLogger logger = + Boardcore::Logging::getLogger("ciuti.main"); + Sensors::getInstance().start(); - UprightDetector::getInstance().start(); + UprightDetectorController::getInstance().start(); SerialWatcherController::getInstance().start(); BoardScheduler::getInstance().getScheduler().start(); + LOG_INFO(logger, "Initialization complete!"); + // Periodical statistics while (true) { Thread::sleep(1000); - // Logger::getInstance().log(CpuMeter::getCpuStats()); - // CpuMeter::resetCpuStats(); - // Logger::getInstance().logStats(); - // MemoryProfiling::print(); + Logger::getInstance().log(CpuMeter::getCpuStats()); + CpuMeter::resetCpuStats(); + Logger::getInstance().logStats(); } }