diff --git a/CMakeLists.txt b/CMakeLists.txt index 536525d4..0a747dc2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -89,6 +89,7 @@ function(add_app_subdirectory APP_SUBDIRECTORY) endfunction() ## Include Rover Apps +add_subdirectory(${ROVER_APPS_DIR}/common) add_app_subdirectory(${ROVER_APPS_DIR}/arm_2021) add_app_subdirectory(${ROVER_APPS_DIR}/gamepad_2021) add_app_subdirectory(${ROVER_APPS_DIR}/gimbal_2021) diff --git a/libs/can/CMakeLists.txt b/libs/can/CMakeLists.txt index 472dca76..c77449f0 100644 --- a/libs/can/CMakeLists.txt +++ b/libs/can/CMakeLists.txt @@ -33,7 +33,6 @@ target_link_libraries(CANInterface PUBLIC mbed-os mbed-events - PRIVATE uwrt-mars-rover-hw-bridge CANBus CANMsg diff --git a/libs/can/include/CANInterface.h b/libs/can/include/CANInterface.h index 0c0bf4f4..ebb3202b 100644 --- a/libs/can/include/CANInterface.h +++ b/libs/can/include/CANInterface.h @@ -36,7 +36,7 @@ class CANInterface { bool getRXSignalValue(HWBRIDGE::CANID msgID, HWBRIDGE::CANSIGNAL signalName, HWBRIDGE::CANSignalValue_t &signalValue); // Switch CAN bus - bool switchCANBus(HWBRIDGE::CANBUSID canBusID); + bool switchCANBus(HWBRIDGE::CANBUSID canBusID); // TODO: Unnecessary // Set CAN bus hw filter bool setFilter(HWBRIDGE::CANFILTER filter, CANFormat format = CANStandard, @@ -51,6 +51,9 @@ class CANInterface { uint16_t getNumCANRXFaults(void); uint16_t getNumCANTXFaults(void); + void rxClientPeriodic(void); + void txProcessorPeriodic(void); + private: static constexpr osPriority RX_POSTMAN_THREAD_PRIORITY = osPriorityRealtime; static constexpr osPriority RX_CLIENT_THREAD_PRIORITY = osPriorityAboveNormal; @@ -60,16 +63,12 @@ class CANInterface { void rxISR(void); void rxPostman(void); - void rxClient(void); - void txProcessor(void); CANBus m_CANBus1; CANBus m_CANBus2; CANBus *m_activeCANBus; Thread m_rxPostmanThread; - Thread m_rxClientThread; - Thread m_txProcessorThread; Mutex m_rxMutex; Mutex m_txMutex; diff --git a/libs/can/src/CANInterface.cpp b/libs/can/src/CANInterface.cpp index 68541338..e6b949b6 100644 --- a/libs/can/src/CANInterface.cpp +++ b/libs/can/src/CANInterface.cpp @@ -5,8 +5,6 @@ CANInterface::CANInterface(const Config &config) m_CANBus2(config.can2_RX, config.can2_TX, config.frequency_hz), m_activeCANBus(&m_CANBus1), m_rxPostmanThread(RX_POSTMAN_THREAD_PRIORITY), - m_rxClientThread(RX_CLIENT_THREAD_PRIORITY), - m_txProcessorThread(TX_PROCESSOR_THREAD_PRIORITY), m_rxMsgMap(config.rxMsgMap), m_txMsgMap(config.txMsgMap), m_rxOneShotMsgHandler(config.rxOneShotMsgHandler), @@ -22,8 +20,6 @@ CANInterface::CANInterface(const Config &config) // Processing threads m_rxPostmanThread.start(callback(&m_rxEventQueue, &EventQueue::dispatch_forever)); - m_rxClientThread.start(callback(this, &CANInterface::rxClient)); - m_txProcessorThread.start(callback(this, &CANInterface::txProcessor)); // RX ISR m_CANBus1.attach(callback(this, &CANInterface::rxISR), CAN::RxIrq); @@ -52,18 +48,12 @@ void CANInterface::rxPostman(void) { can_irq_set(m_activeCANBus->getHandle(), IRQ_RX, true); } -void CANInterface::rxClient(void) { - while (true) { - CANMsg *mail = nullptr; - - // Wait for a message to arrive - do { - mail = m_rxMailbox.try_get(); // using try_get() because try_get_for() was crashing - ThisThread::sleep_for(1ms); - } while (mail == nullptr); - - MBED_ASSERT(mail != nullptr); +void CANInterface::rxClientPeriodic(void) { + CANMsg *mail = nullptr; + // Check if a message has arrived: + mail = m_rxMailbox.try_get(); + if (mail != nullptr) { // Extract message CANMsg msg = *mail; MBED_ASSERT(m_rxMailbox.free(mail) == osOK); @@ -111,52 +101,45 @@ void CANInterface::rxClient(void) { } } -void CANInterface::txProcessor(void) { - while (true) { - auto startTime = Kernel::Clock::now(); +void CANInterface::txProcessorPeriodic(void) { - CANMsg *mail = nullptr; + CANMsg *mail = nullptr; - // Send all one-shot messages that were queued - while ((mail = m_txMailboxOneShot.try_get()) != nullptr) { - if (!m_activeCANBus->write(*mail)) { - MBED_WARNING(MBED_MAKE_ERROR(MBED_MODULE_PLATFORM, MBED_ERROR_CODE_WRITE_FAILED), "CAN TX write failed"); - m_numCANTXFaults++; - } - MBED_ASSERT(m_txMailboxOneShot.free(mail) == osOK); - ThisThread::sleep_for(TX_INTERDELAY); + // Send all one-shot messages that were queued + while ((mail = m_txMailboxOneShot.try_get()) != nullptr) { + if (!m_activeCANBus->write(*mail)) { + MBED_WARNING(MBED_MAKE_ERROR(MBED_MODULE_PLATFORM, MBED_ERROR_CODE_WRITE_FAILED), "CAN TX write failed"); + m_numCANTXFaults++; } + MBED_ASSERT(m_txMailboxOneShot.free(mail) == osOK); + } - // Send all streamed messages - if (m_txMsgMap != nullptr) { - for (auto it = m_txMsgMap->begin(); it != m_txMsgMap->end(); it++) { - HWBRIDGE::CANID msgID = it->first; - HWBRIDGE::CANMsgData_t msgData = {0}; - size_t len = 0; - - m_txMutex.lock(); - bool msgPacked = HWBRIDGE::packCANMsg(msgData.raw, msgID, m_txMsgMap, len); - m_txMutex.unlock(); - - if (msgPacked) { - // Send message - CANMsg msg; - msg.setID(msgID); - msg.setPayload(msgData, len); - m_activeCANBus->write(msg); - - m_numStreamedMsgsSent++; - - ThisThread::sleep_for(TX_INTERDELAY); - } else { - MBED_WARNING(MBED_MAKE_ERROR(MBED_MODULE_PLATFORM, MBED_ERROR_CODE_INVALID_DATA_DETECTED), - "CAN TX message packing failed"); - m_numCANTXFaults++; - } + // Send all streamed messages + if (m_txMsgMap != nullptr) { + for (auto it = m_txMsgMap->begin(); it != m_txMsgMap->end(); it++) { + HWBRIDGE::CANID msgID = it->first; + HWBRIDGE::CANMsgData_t msgData = {0}; + size_t len = 0; + + m_txMutex.lock(); + bool msgPacked = HWBRIDGE::packCANMsg(msgData.raw, msgID, m_txMsgMap, len); + m_txMutex.unlock(); + + if (msgPacked) { + // Send message + CANMsg msg; + msg.setID(msgID); + msg.setPayload(msgData, len); + m_activeCANBus->write(msg); + + m_numStreamedMsgsSent++; + + } else { + MBED_WARNING(MBED_MAKE_ERROR(MBED_MODULE_PLATFORM, MBED_ERROR_CODE_INVALID_DATA_DETECTED), + "CAN TX message packing failed"); + m_numCANTXFaults++; } } - - ThisThread::sleep_until(startTime + TX_PERIOD); } } diff --git a/libs/utility/include/WatchdogWrapper.h b/libs/utility/include/WatchdogWrapper.h index f9034813..723786ca 100644 --- a/libs/utility/include/WatchdogWrapper.h +++ b/libs/utility/include/WatchdogWrapper.h @@ -5,11 +5,8 @@ namespace Utility { class WatchdogWrapper { public: - static void startWatchdog(std::chrono::milliseconds countdown_ms = 5000ms, std::chrono::milliseconds pet_ms = 1000ms); - static void logResetReason(); - - private: - static void petWatchdog(std::chrono::milliseconds *pet_ms); - static Thread pet_thread; + static void startWatchdog(std::chrono::milliseconds countdown_ms); + static void logResetReason(void); + static void petWatchdog(void); }; } // namespace Utility \ No newline at end of file diff --git a/libs/utility/src/WatchdogWrapper.cpp b/libs/utility/src/WatchdogWrapper.cpp index b3328074..5e4b5b7c 100644 --- a/libs/utility/src/WatchdogWrapper.cpp +++ b/libs/utility/src/WatchdogWrapper.cpp @@ -6,17 +6,13 @@ namespace Utility { -Thread WatchdogWrapper::pet_thread; - -void WatchdogWrapper::startWatchdog(std::chrono::milliseconds countdown_ms /*= 5000ms*/, - std::chrono::milliseconds pet_ms /*= 1000ms*/) { +void WatchdogWrapper::startWatchdog(std::chrono::milliseconds countdown_ms) { uint32_t countdown_uint32 = countdown_ms.count(); Watchdog &watchdog = Watchdog::get_instance(); watchdog.start(countdown_uint32); - pet_thread.start(callback(WatchdogWrapper::petWatchdog, &pet_ms)); } -void WatchdogWrapper::logResetReason() { +void WatchdogWrapper::logResetReason(void) { const reset_reason_t reason = ResetReason::get(); if (reason == RESET_REASON_WATCHDOG) { time_t seconds = time(NULL); @@ -26,10 +22,7 @@ void WatchdogWrapper::logResetReason() { } } -void WatchdogWrapper::petWatchdog(std::chrono::milliseconds *pet_ms) { - while (1) { - Watchdog::get_instance().kick(); - ThisThread::sleep_for(*pet_ms); - } +void WatchdogWrapper::petWatchdog(void) { + Watchdog::get_instance().kick(); } -} // namespace Utility \ No newline at end of file +} // namespace Utility diff --git a/rover-apps/arm_2021/CMakeLists.txt b/rover-apps/arm_2021/CMakeLists.txt index 376fa47f..f969f8ad 100644 --- a/rover-apps/arm_2021/CMakeLists.txt +++ b/rover-apps/arm_2021/CMakeLists.txt @@ -1,6 +1,6 @@ add_executable(arm_2021) -target_sources(arm_2021 PRIVATE src/main.cpp) -target_include_directories(arm_2021 PUBLIC include) +target_sources(arm_2021 PRIVATE ../common/src/main.cpp) +target_include_directories(arm_2021 PUBLIC include ../common/include) target_link_libraries(arm_2021 PRIVATE #Control @@ -21,6 +21,9 @@ target_link_libraries(arm_2021 CANMsg #Sensor CurrentSensor + #common-modules + WatchdogModule + CANDriverModule #Other uwrt-mars-rover-hw-bridge Logger diff --git a/rover-apps/arm_2021/include/AppConfig.h b/rover-apps/arm_2021/include/AppConfig.h new file mode 100644 index 00000000..954df303 --- /dev/null +++ b/rover-apps/arm_2021/include/AppConfig.h @@ -0,0 +1,17 @@ +#pragma once + +#include + +#include "Module.h" +#include "WatchdogModule.h" +#include "CANDriverModule.h" +#include "CANConfig.h" +#include "hw_bridge.h" +#include "CANInterface.h" + +WatchdogModule arm_watchdog; + +std::vector gModules = { + // put modules here + &arm_watchdog, +}; diff --git a/rover-apps/common/CMakeLists.txt b/rover-apps/common/CMakeLists.txt new file mode 100644 index 00000000..02a84ee4 --- /dev/null +++ b/rover-apps/common/CMakeLists.txt @@ -0,0 +1,19 @@ +add_library(WatchdogModule STATIC) +target_sources(WatchdogModule PRIVATE src/WatchdogModule.cpp) +target_include_directories(WatchdogModule PUBLIC include) +target_link_libraries(WatchdogModule + PRIVATE + WatchdogWrapper + mbed-os + ) + +add_library(CANDriverModule STATIC) +target_sources(CANDriverModule PRIVATE src/CANDriverModule.cpp) +target_include_directories(CANDriverModule PUBLIC include) +target_link_libraries(CANDriverModule + PRIVATE + CANInterface + CANMsg + mbed-os + ) + diff --git a/rover-apps/common/include/CANDriverModule.h b/rover-apps/common/include/CANDriverModule.h new file mode 100644 index 00000000..1ef3b2e7 --- /dev/null +++ b/rover-apps/common/include/CANDriverModule.h @@ -0,0 +1,44 @@ +#pragma once + +#include "CANInterface.h" +#include "Module.h" +#include "mbed.h" +#include "CANBus.h" +#include "CANMsg.h" +#include "hw_bridge.h" +#include "mbed.h" + +class CANDriverModule final : public Module { + public: + CANDriverModule(const CANInterface::Config &config); + void periodic_1s(void) override; + + void periodic_10s(void) override; + void periodic_100ms(void) override; + void periodic_10ms(void) override; + void periodic_1ms(void) override; + void rxPostman(void); + bool sendOneShotMessage(CANMsg &msg, Kernel::Clock::duration_u32 timeout); + + // Update a TX CAN signal + bool setTXSignalValue(HWBRIDGE::CANID msgID, HWBRIDGE::CANSIGNAL signalName, HWBRIDGE::CANSignalValue_t signalValue); + + // Read a RX CAN signal + bool getRXSignalValue(HWBRIDGE::CANID msgID, HWBRIDGE::CANSIGNAL signalName, HWBRIDGE::CANSignalValue_t &signalValue); + + // Set CAN bus hw filter + bool setFilter(HWBRIDGE::CANFILTER filter, CANFormat format = CANStandard, + uint16_t mask = HWBRIDGE::ROVER_CANID_FILTER_MASK, int handle = 0); + + // For diagnostic purposes + uint32_t getNumStreamedMsgsReceived(void); + uint32_t getNumOneShotMsgsReceived(void); + uint32_t getNumStreamedMsgsSent(void); + uint32_t getNumOneShotMsgsSent(void); + + uint16_t getNumCANRXFaults(void); + uint16_t getNumCANTXFaults(void); + + private: + CANInterface interface; +}; diff --git a/rover-apps/common/include/Module.h b/rover-apps/common/include/Module.h new file mode 100644 index 00000000..8df2b8cf --- /dev/null +++ b/rover-apps/common/include/Module.h @@ -0,0 +1,10 @@ +#pragma once + +class Module { + public: + virtual void periodic_10s(void) = 0; + virtual void periodic_1s(void) = 0; + virtual void periodic_100ms(void) = 0; + virtual void periodic_10ms(void) = 0; + virtual void periodic_1ms(void) = 0; +}; diff --git a/rover-apps/common/include/WatchdogModule.h b/rover-apps/common/include/WatchdogModule.h new file mode 100644 index 00000000..3dbdfb42 --- /dev/null +++ b/rover-apps/common/include/WatchdogModule.h @@ -0,0 +1,24 @@ +#pragma once + +#include "Module.h" +#include "mbed.h" + +class WatchdogModule final : public Module { + public: + /* Initiates the watchdog with a countdown + * @param countdown - max timeout of the watchdog + * */ + WatchdogModule(); + + /* Periodic function to kick the watchdog and restart its timer every 1s + * */ + void periodic_1s(void) override; + + void periodic_10s(void) override {} + void periodic_100ms(void) override {} + void periodic_10ms(void) override {} + void periodic_1ms(void) override {} + + private: + static const std::chrono::milliseconds WATCHDOG_DEFAULT_COUNTDOWN; +}; diff --git a/rover-apps/common/src/CANDriverModule.cpp b/rover-apps/common/src/CANDriverModule.cpp new file mode 100644 index 00000000..d31424f5 --- /dev/null +++ b/rover-apps/common/src/CANDriverModule.cpp @@ -0,0 +1,69 @@ +#include "CANDriverModule.h" +#include "CANInterface.h" +#include "Module.h" +#include "mbed.h" + +// Using initializer lists to construct CANInterface object will attach ISRs and other required tasks +CANDriverModule::CANDriverModule(const CANInterface::Config &config) : interface(config) { + // Nothing should be required here +} + +void CANDriverModule::periodic_1ms(void) { + + // These functions need to be called at 1 kHz, or every 1ms + interface.rxClientPeriodic(); +} + +void CANDriverModule::periodic_10ms(void) { + + // These functions need to be called every 100 Hz (10ms) + interface.txProcessorPeriodic(); +} + +void CANDriverModule::rxPostman(void) { + interface.rxPostman(); +} + +bool CANDriverModule::sendOneShotMessage(CANMsg &msg, Kernel::Clock::duration_u32 timeout) { + return interface.sendOneShotMessage(msg, timeout); +} + +bool CANDriverModule::setTXSignalValue(HWBRIDGE::CANID msgID, HWBRIDGE::CANSIGNAL signalName, + HWBRIDGE::CANSignalValue_t signalValue) { + return interface.setTXSignalValue(msgID, signalName, signalValue); +} + +bool CANDriverModule::getRXSignalValue(HWBRIDGE::CANID msgID, HWBRIDGE::CANSIGNAL signalName, HWBRIDGE::CANSignalValue_t &signalValue) { + return interface.getRXSignalValue(msgID, signalName, signalValue); +} + +bool CANDriverModule::setFilter(HWBRIDGE::CANFILTER filter, CANFormat format = CANStandard, + uint16_t mask = HWBRIDGE::ROVER_CANID_FILTER_MASK, int handle = 0) { + return interface.setFilter(filter, format, mask, handle); +} + +uint32_t CANDriverModule::getNumStreamedMsgsReceived(void) { + return interface.getNumStreamedMsgsReceived(); +} + +uint32_t CANDriverModule::getNumOneShotMsgsReceived(void) { + return interface.getNumOneShotMsgsReceived(); +} + +uint32_t CANDriverModule::getNumStreamedMsgsSent(void) { + return interface.getNumStreamedMsgsSent(); +} + +uint32_t CANDriverModule::getNumOneShotMsgsSent(void) { + return interface.getNumOneShotMsgsSent(); +} + +uint16_t CANDriverModule::getNumCANRXFaults(void) { + return interface.getNumCANRXFaults(); +} + +uint16_t CANDriverModule::getNumCANTXFaults(void) { + return interface.getNumCANTXFaults(); +} + + diff --git a/rover-apps/common/src/WatchdogModule.cpp b/rover-apps/common/src/WatchdogModule.cpp new file mode 100644 index 00000000..14cac732 --- /dev/null +++ b/rover-apps/common/src/WatchdogModule.cpp @@ -0,0 +1,16 @@ +#include "WatchdogModule.h" + +#include "Module.h" +#include "WatchdogWrapper.h" +#include "mbed.h" + +const std::chrono::milliseconds WatchdogModule::WATCHDOG_DEFAULT_COUNTDOWN = 5000ms; + +WatchdogModule::WatchdogModule() { + Utility::WatchdogWrapper::logResetReason(); + Utility::WatchdogWrapper::startWatchdog(WATCHDOG_DEFAULT_COUNTDOWN); +} + +void WatchdogModule::periodic_1s(void) { + Utility::WatchdogWrapper::petWatchdog(); +} diff --git a/rover-apps/common/src/main.cpp b/rover-apps/common/src/main.cpp new file mode 100644 index 00000000..c60173cf --- /dev/null +++ b/rover-apps/common/src/main.cpp @@ -0,0 +1,59 @@ +#include "AppConfig.h" +#include "mbed.h" + +Thread periodic_10s_thread(osPriorityNormal1); +Thread periodic_1s_thread(osPriorityNormal2); +Thread periodic_100ms_thread(osPriorityNormal3); +Thread periodic_10ms_thread(osPriorityNormal4); +Thread periodic_1ms_thread(osPriorityNormal5); + +void periodic_10s(void) { + auto startTime = Kernel::Clock::now(); + for (Module* module : gModules) { + module->periodic_10s(); + } + ThisThread::sleep_until(startTime + 10s); +} + +void periodic_1s(void) { + auto startTime = Kernel::Clock::now(); + for (Module* module : gModules) { + module->periodic_1s(); + } + ThisThread::sleep_until(startTime + 1s); +} + +void periodic_100ms(void) { + auto startTime = Kernel::Clock::now(); + for (Module* module : gModules) { + module->periodic_100ms(); + } + ThisThread::sleep_until(startTime + 100ms); +} + +void periodic_10ms(void) { + auto startTime = Kernel::Clock::now(); + for (Module* module : gModules) { + module->periodic_10ms(); + } + ThisThread::sleep_until(startTime + 10ms); +} + +void periodic_1ms(void) { + auto startTime = Kernel::Clock::now(); + for (Module* module : gModules) { + module->periodic_1ms(); + } + ThisThread::sleep_until(startTime + 1ms); +} + +int main() { + periodic_1ms_thread.start(periodic_1ms); + periodic_10ms_thread.start(periodic_10ms); + periodic_100ms_thread.start(periodic_100ms); + periodic_1s_thread.start(periodic_1s); + periodic_10s_thread.start(periodic_10s); + + while (true) { + } +} diff --git a/rover-apps/gimbal_2021/CMakeLists.txt b/rover-apps/gimbal_2021/CMakeLists.txt index 98296ac5..f03ee922 100644 --- a/rover-apps/gimbal_2021/CMakeLists.txt +++ b/rover-apps/gimbal_2021/CMakeLists.txt @@ -1,6 +1,6 @@ add_executable(gimbal_2021) -target_sources(gimbal_2021 PRIVATE src/main.cpp) -target_include_directories(gimbal_2021 PUBLIC include) +target_sources(gimbal_2021 PRIVATE ../common/src/main.cpp) +target_include_directories(gimbal_2021 PUBLIC include ../common/include) target_link_libraries(gimbal_2021 PRIVATE #Control @@ -19,6 +19,8 @@ target_link_libraries(gimbal_2021 CANMsg #Sensor CurrentSensor + #common-modules + WatchdogModule #Other Logger uwrt-mars-rover-hw-bridge diff --git a/rover-apps/gimbal_2021/include/AppConfig.h b/rover-apps/gimbal_2021/include/AppConfig.h new file mode 100644 index 00000000..35955ba8 --- /dev/null +++ b/rover-apps/gimbal_2021/include/AppConfig.h @@ -0,0 +1,13 @@ +#pragma once + +#include + +#include "Module.h" +#include "WatchdogModule.h" + +WatchdogModule gimbal_watchdog; + +std::vector gModules = { + // put modules here + &gimbal_watchdog, +}; diff --git a/rover-apps/pdb_2021/CMakeLists.txt b/rover-apps/pdb_2021/CMakeLists.txt index 27d4cca7..241157b6 100644 --- a/rover-apps/pdb_2021/CMakeLists.txt +++ b/rover-apps/pdb_2021/CMakeLists.txt @@ -1,11 +1,25 @@ +add_library(PDB_Monitoring STATIC) +target_sources(PDB_Monitoring PRIVATE src/PDBMonitoring.cpp) +target_include_directories(PDB_Monitoring PUBLIC + include + ../common/include + ) +target_link_libraries(PDB_Monitoring + PRIVATE + Logger + mbed-os + ) + add_executable(pdb_2021) -target_sources(pdb_2021 PRIVATE src/main.cpp) -target_include_directories(pdb_2021 PUBLIC include) +target_sources(pdb_2021 PRIVATE ../common/src/main.cpp) +target_include_directories(pdb_2021 PUBLIC include ../common/include) target_link_libraries(pdb_2021 PRIVATE CANBus CANMsg Logger uwrt-mars-rover-hw-bridge + #Modules + PDB_Monitoring ) mbed_set_post_build(pdb_2021) diff --git a/rover-apps/pdb_2021/include/AppConfig.h b/rover-apps/pdb_2021/include/AppConfig.h new file mode 100644 index 00000000..b2e56f21 --- /dev/null +++ b/rover-apps/pdb_2021/include/AppConfig.h @@ -0,0 +1,13 @@ +#pragma once + +#include + +#include "Module.h" +#include "PDBMonitoring.h" + +PDBMonitoring PDB_monitoring; + +std::vector gModules = { + // put modules here + &PDB_monitoring, +}; diff --git a/rover-apps/pdb_2021/include/PDBMonitoring.h b/rover-apps/pdb_2021/include/PDBMonitoring.h new file mode 100644 index 00000000..fc97ce0b --- /dev/null +++ b/rover-apps/pdb_2021/include/PDBMonitoring.h @@ -0,0 +1,78 @@ +#pragma once + +#include "Module.h" +#include "mbed.h" + +/*This PDB module is for load, rail and temperature monitoring.*/ + +class PDBMonitoring final : public Module { + public: + /* Sets the Load DIAG_EN pins */ + PDBMonitoring(); + + void periodic_1s(void) override; + void periodic_10s(void) override {} + void periodic_100ms(void) override {} + void periodic_10ms(void) override {} + void periodic_1ms(void) override; + + private: + void load_monitoring(); + void rail_monitoring(); + void temperature_monitoring(); + + static const float PDB_VBAT_RAIL_NOMINAL_VOLTAGE; + static const float PDB_VBAT_RAIL_MIN_THRESHOLD; + static const float PDB_VBAT_RAIL_MAX_THRESHOLD; + + static const float PDB_24V_RAIL_NOMINAL_VOLTAGE; + static const float PDB_24V_RAIL_MIN_THRESHOLD; + static const float PDB_24V_RAIL_MAX_THRESHOLD; + + static const float PDB_17V_RAIL_NOMINAL_VOLTAGE; + static const float PDB_17V_RAIL_MIN_THRESHOLD; + static const float PDB_17V_RAIL_MAX_THRESHOLD; + + static const float PDB_5V_RAIL_NOMINAL_VOLTAGE; + static const float PDB_5V_RAIL_MIN_THRESHOLD; + static const float PDB_5V_RAIL_MAX_THRESHOLD; + + static const float PDB_TEMPERATURE_MIN_THRESHOLD; + static const float PDB_TEMPERATURE_MAX_THRESHOLD; + + static const bool PDB_5V_LOAD1_DIAG_EN; + static const bool PDB_5V_LOAD2_DIAG_EN; + static const bool PDB_5V_LOAD3_DIAG_EN; + static const bool PDB_5V_LOAD4_DIAG_EN; + static const bool PDB_5V_LOAD5_DIAG_EN; + static const bool PDB_17V_LOAD_DIAG_EN; + + /* Pins configuration for Load Monitoring */ + DigitalOut load1_5V_diag_en; + DigitalIn load1_5V_fault_n; + + DigitalOut load2_5V_diag_en; + DigitalIn load2_5V_fault_n; + + DigitalOut load3_5V_diag_en; + DigitalIn load3_5V_fault_n; + + DigitalOut load4_5V_diag_en; + DigitalIn load4_5V_fault_n; + + DigitalOut load5_5V_diag_en; + DigitalIn load5_5V_fault_n; + + DigitalOut load_17V_diag_en; + DigitalIn load_17V_fault_n; + + /* Pins configuration for Rail Monitoring */ + AnalogIn railBattery; + AnalogIn rail5V; + AnalogIn rail17V; + AnalogIn rail24V; + DigitalIn rail24V_pgood_n; + + /* Pins configuration for Temperature Monitoring */ + AnalogIn temperatureADC; +}; \ No newline at end of file diff --git a/rover-apps/pdb_2021/src/PDBMonitoring.cpp b/rover-apps/pdb_2021/src/PDBMonitoring.cpp new file mode 100644 index 00000000..21fc5a82 --- /dev/null +++ b/rover-apps/pdb_2021/src/PDBMonitoring.cpp @@ -0,0 +1,132 @@ +#include "PDBMonitoring.h" + +#include + +#include "Logger.h" + +const float PDBMonitoring::PDB_VBAT_RAIL_NOMINAL_VOLTAGE = 24.0; +const float PDBMonitoring::PDB_VBAT_RAIL_MIN_THRESHOLD = 18.0; +const float PDBMonitoring::PDB_VBAT_RAIL_MAX_THRESHOLD = 25.2; + +const float PDBMonitoring::PDB_24V_RAIL_NOMINAL_VOLTAGE = 24.0; +const float PDBMonitoring::PDB_24V_RAIL_MIN_THRESHOLD = 22.0; +const float PDBMonitoring::PDB_24V_RAIL_MAX_THRESHOLD = 26.0; + +const float PDBMonitoring::PDB_17V_RAIL_NOMINAL_VOLTAGE = 17.0; +const float PDBMonitoring::PDB_17V_RAIL_MIN_THRESHOLD = 16.0; +const float PDBMonitoring::PDB_17V_RAIL_MAX_THRESHOLD = 18.0; + +const float PDBMonitoring::PDB_5V_RAIL_NOMINAL_VOLTAGE = 5.0; +const float PDBMonitoring::PDB_5V_RAIL_MIN_THRESHOLD = 4.8; +const float PDBMonitoring::PDB_5V_RAIL_MAX_THRESHOLD = 6.0; + +const float PDBMonitoring::PDB_TEMPERATURE_MIN_THRESHOLD = 10.0; +const float PDBMonitoring::PDB_TEMPERATURE_MAX_THRESHOLD = 50.0; + +const bool PDBMonitoring::PDB_5V_LOAD1_DIAG_EN = 1; +const bool PDBMonitoring::PDB_5V_LOAD2_DIAG_EN = 0; +const bool PDBMonitoring::PDB_5V_LOAD3_DIAG_EN = 0; +const bool PDBMonitoring::PDB_5V_LOAD4_DIAG_EN = 0; +const bool PDBMonitoring::PDB_5V_LOAD5_DIAG_EN = 0; +const bool PDBMonitoring::PDB_17V_LOAD_DIAG_EN = 1; + +PDBMonitoring::PDBMonitoring() + : load1_5V_diag_en(LOAD1_5V_DIAG_EN), + load1_5V_fault_n(LOAD1_5V_FAULT), + load2_5V_diag_en(LOAD2_5V_DIAG_EN), + load2_5V_fault_n(LOAD2_5V_FAULT), + load3_5V_diag_en(LOAD3_5V_DIAG_EN), + load3_5V_fault_n(LOAD3_5V_FAULT), + load4_5V_diag_en(LOAD4_5V_DIAG_EN), + load4_5V_fault_n(LOAD4_5V_FAULT), + load5_5V_diag_en(LOAD5_5V_DIAG_EN), + load5_5V_fault_n(LOAD5_5V_FAULT), + load_17V_diag_en(LOAD_17V_DIAG_EN), + load_17V_fault_n(LOAD_17V_FAULT), + railBattery(RAIL_BATTERY_ANLG_IN), + rail5V(RAIL_5V_ANLG_IN), + rail17V(RAIL_17V_ANLG_IN), + rail24V(RAIL_24V_ANLG_IN), + rail24V_pgood_n(RAIL_24V_PGOOD_N), + temperatureADC(TEMPERATURE_ADC_IN) { + load1_5V_diag_en = PDB_5V_LOAD1_DIAG_EN; + load2_5V_diag_en = PDB_5V_LOAD2_DIAG_EN; + load3_5V_diag_en = PDB_5V_LOAD3_DIAG_EN; + load4_5V_diag_en = PDB_5V_LOAD4_DIAG_EN; + load5_5V_diag_en = PDB_5V_LOAD5_DIAG_EN; + load_17V_diag_en = PDB_17V_LOAD_DIAG_EN; +} + +/*TODO:Replace the logger statements with CAN logs + * after CAN Module is ready */ + +void PDBMonitoring::load_monitoring() { + if (load1_5V_diag_en && !load1_5V_fault_n) { + Utility::logger << "Fault on 5V load 1\n"; + } + if (load2_5V_diag_en && !load2_5V_fault_n) { + Utility::logger << "Fault on 5V load 2\n"; + } + if (load3_5V_diag_en && !load3_5V_fault_n) { + Utility::logger << "Fault on 5V load 3\n"; + } + if (load4_5V_diag_en && !load4_5V_fault_n) { + Utility::logger << "Fault on 5V load 4\n"; + } + if (load5_5V_diag_en && !load5_5V_fault_n) { + Utility::logger << "Fault on 5V load 5\n"; + } + if (load_17V_diag_en && !load_17V_fault_n) { + Utility::logger << "Fault on 17V load\n"; + } +} + +void PDBMonitoring::rail_monitoring() { + float rail_vbat_voltage = railBattery.read_voltage() / 3 * PDB_VBAT_RAIL_NOMINAL_VOLTAGE; + float rail_24V_voltage = rail24V.read_voltage() / 3 * PDB_24V_RAIL_NOMINAL_VOLTAGE; + float rail_17V_voltage = rail17V.read_voltage() / 3 * PDB_17V_RAIL_NOMINAL_VOLTAGE; + float rail_5V_voltage = rail5V.read_voltage() / 3 * PDB_5V_RAIL_NOMINAL_VOLTAGE; + + if (rail_vbat_voltage < PDB_VBAT_RAIL_MIN_THRESHOLD || rail_vbat_voltage > PDB_VBAT_RAIL_MAX_THRESHOLD) { + Utility::logger << "!!! VBAT RAIL VOLTAGE: %.3fV !!!\n", rail_vbat_voltage; + } + if (rail_24V_voltage < PDB_24V_RAIL_MIN_THRESHOLD || rail_24V_voltage > PDB_24V_RAIL_MAX_THRESHOLD) { + Utility::logger << "!!! 24V RAIL VOLTAGE: %.3fV !!!\n", rail_24V_voltage; + } + if (rail_17V_voltage < PDB_17V_RAIL_MIN_THRESHOLD || rail_17V_voltage > PDB_17V_RAIL_MAX_THRESHOLD) { + Utility::logger << "!!! 17V RAIL VOLTAGE: %.3fV !!!\n", rail_17V_voltage; + } + if (rail_5V_voltage < PDB_5V_RAIL_MIN_THRESHOLD || rail_5V_voltage > PDB_5V_RAIL_MAX_THRESHOLD) { + Utility::logger << "!!! 5V RAIL VOLTAGE: %.3fV !!!\n", rail_5V_voltage; + } +} + +/*https://www.ti.com/lit/ds/symlink/lmt87-q1.pdf?ts=1627158177761&ref_url=https%253A%252F%252Fwww.google.com%252F*/ + +void PDBMonitoring::temperature_monitoring() { + float temperature_celsius = + (13.582 - sqrt(pow(-13.582, 2) + 4 * 0.00433 * (2230.8 - temperatureADC.read_voltage() * 1000))) / + (2 * (-0.00433)) + + 30; + + if (temperature_celsius < PDB_TEMPERATURE_MIN_THRESHOLD || temperature_celsius > PDB_TEMPERATURE_MAX_THRESHOLD) { + Utility::logger << "!!! TEMPERATURE FAULT:" << temperature_celsius << "degrees Celsius\n"; + } +} + +void PDBMonitoring::periodic_1s() { + temperature_monitoring(); + load_monitoring(); +} + +void PDBMonitoring::periodic_1ms() { + /*Monitoring Period = 500ms*/ + int rail_counter = 0; + + if (rail_counter % 5 == 0) { + rail_monitoring(); + rail_counter = 0; + } + + rail_counter++; +} diff --git a/rover-apps/science_2021/CMakeLists.txt b/rover-apps/science_2021/CMakeLists.txt index 80b5d19b..91fcf710 100644 --- a/rover-apps/science_2021/CMakeLists.txt +++ b/rover-apps/science_2021/CMakeLists.txt @@ -1,6 +1,6 @@ add_executable(science_2021) -target_sources(science_2021 PRIVATE src/main.cpp) -target_include_directories(science_2021 PUBLIC include) +target_sources(science_2021 PRIVATE ../common/src/main.cpp) +target_include_directories(science_2021 PUBLIC include ../common/include) target_link_libraries(science_2021 PRIVATE #Control @@ -22,6 +22,9 @@ target_link_libraries(science_2021 #Sensor CurrentSensor AdafruitSTEMMA + #common-modules + WatchdogModule + CANDriverModule #Other uwrt-mars-rover-hw-bridge Logger diff --git a/rover-apps/science_2021/include/AppConfig.h b/rover-apps/science_2021/include/AppConfig.h new file mode 100644 index 00000000..b8f12354 --- /dev/null +++ b/rover-apps/science_2021/include/AppConfig.h @@ -0,0 +1,14 @@ +#pragma once + +#include + +#include "Module.h" +#include "WatchdogModule.h" +#include "CANDriverModule.h" + +WatchdogModule science_watchdog; + +std::vector gModules = { + // put modules here + &science_watchdog, +}; diff --git a/targets/TARGET_PDB_REV2_2021/include/PinNames.h b/targets/TARGET_PDB_REV2_2021/include/PinNames.h index ab8f8232..94e25d90 100644 --- a/targets/TARGET_PDB_REV2_2021/include/PinNames.h +++ b/targets/TARGET_PDB_REV2_2021/include/PinNames.h @@ -171,6 +171,29 @@ typedef enum { RAIL_17V_ANLG_IN = PA_5, RAIL_24V_ANLG_IN = PB_0, RAIL_BATTERY_ANLG_IN = PB_1, + RAIL_24V_PGOOD_N = PC_6, + + /**** Temperature Sensor ****/ + TEMPERATURE_ADC_IN = PA_3, + + /**** Load Monitor ****/ + LOAD1_5V_DIAG_EN = PA_6, + LOAD1_5V_FAULT = PA_7, + + LOAD2_5V_DIAG_EN = PC_2, + LOAD2_5V_FAULT = PC_3, + + LOAD3_5V_DIAG_EN = PC_0, + LOAD3_5V_FAULT = PC_1, + + LOAD4_5V_DIAG_EN = PC_13, + LOAD4_5V_FAULT = PC_14, + + LOAD5_5V_DIAG_EN = PB_8, + LOAD5_5V_FAULT = PB_9, + + LOAD_17V_DIAG_EN = PC_4, + LOAD_17V_FAULT = PC_5, /**** LED Matrix ****/ LED_MATRIX_R_CHANNEL = PB_4, diff --git a/test-apps/test-watchdog/src/main.cpp b/test-apps/test-watchdog/src/main.cpp index d6815ddb..bcd5b38d 100644 --- a/test-apps/test-watchdog/src/main.cpp +++ b/test-apps/test-watchdog/src/main.cpp @@ -1,12 +1,21 @@ #include "WatchdogWrapper.h" #include "mbed.h" +Thread pet_thread; std::chrono::milliseconds countdown_ms = 1000ms; std::chrono::milliseconds pet_ms = 200ms; +void pet_dog_task(std::chrono::milliseconds *pet_ms) { + while (1) { + Utility::WatchdogWrapper::petWatchdog(); + ThisThread::sleep_for(*pet_ms); + } +} + int main() { Utility::WatchdogWrapper::logResetReason(); - Utility::WatchdogWrapper::startWatchdog(countdown_ms, pet_ms); + Utility::WatchdogWrapper::startWatchdog(countdown_ms); + pet_thread.start(callback(pet_dog_task, &pet_ms)); ThisThread::sleep_for(2000ms); MBED_ASSERT(false); -} \ No newline at end of file +}