diff --git a/src/ConRIG/BoardScheduler.h b/src/ConRIG/BoardScheduler.h index f0c45b4a4221e89563381f739fd4c3a94c38c5b2..3736c89c1ef83790d652f590b52f9d31f407fadb 100644 --- a/src/ConRIG/BoardScheduler.h +++ b/src/ConRIG/BoardScheduler.h @@ -42,7 +42,6 @@ public: [[nodiscard]] bool start() { - if (!radio.start()) { LOG_ERR(logger, "Failed to start radio scheduler"); @@ -58,9 +57,9 @@ public: return true; } - Boardcore::TaskScheduler &getRadioScheduler() { return radio; } + Boardcore::TaskScheduler& getRadioScheduler() { return radio; } - Boardcore::TaskScheduler &getButtonsScheduler() { return buttons; } + Boardcore::TaskScheduler& getButtonsScheduler() { return buttons; } private: Boardcore::PrintLogger logger = diff --git a/src/ConRIG/Buses.h b/src/ConRIG/Buses.h index c46cf45340e3b3035b396df63204cda3464af33f..820e2db3b452b039cdd7fdeca52ee1c82d08f838 100644 --- a/src/ConRIG/Buses.h +++ b/src/ConRIG/Buses.h @@ -38,7 +38,7 @@ private: public: Buses() : spi1(SPI1) {} - Boardcore::SPIBus &getRadio() { return spi1; } + Boardcore::SPIBus& getRadio() { return spi1; } }; } // namespace ConRIG diff --git a/src/ConRIG/Buttons/Buttons.cpp b/src/ConRIG/Buttons/Buttons.cpp index f4214f4b624a45b6179f9ff3b5f024aa68c80b59..69593bf16c66da2654a62707601071379c650a8f 100644 --- a/src/ConRIG/Buttons/Buttons.cpp +++ b/src/ConRIG/Buttons/Buttons.cpp @@ -40,7 +40,7 @@ Buttons::Buttons() bool Buttons::start() { - TaskScheduler &scheduler = getModule<BoardScheduler>()->getRadioScheduler(); + TaskScheduler& scheduler = getModule<BoardScheduler>()->getRadioScheduler(); return scheduler.addTask([this]() { periodicStatusCheck(); }, Config::Buttons::BUTTON_SAMPLE_PERIOD) != 0; diff --git a/src/ConRIG/Buttons/Buttons.h b/src/ConRIG/Buttons/Buttons.h index efe691daafffbe226b3674f9b2e17d6b1d57fc74..ecd160f2fd7d43d43d0c08e8de4aba4aee57a0bf 100644 --- a/src/ConRIG/Buttons/Buttons.h +++ b/src/ConRIG/Buttons/Buttons.h @@ -35,7 +35,6 @@ class Radio; class Buttons : public Boardcore::InjectableWithDeps<BoardScheduler, Radio> { - public: Buttons(); diff --git a/src/ConRIG/Radio/Radio.cpp b/src/ConRIG/Radio/Radio.cpp index b433375176840500ab44a8dc0d59a0134195391e..04a5ab8d19c71297e2d6f0f7067de2e5515aa949 100644 --- a/src/ConRIG/Radio/Radio.cpp +++ b/src/ConRIG/Radio/Radio.cpp @@ -43,9 +43,7 @@ void handleDioIRQ() { SX1278Lora* instance = gRadio; if (instance) - { instance->handleDioIRQ(); - } } void setIRQRadio(SX1278Lora* radio) @@ -69,13 +67,9 @@ void Radio::handleMessage(const mavlink_message_t& msg) isArmed = armingState == 1; if (armingState == 1) - { getModule<Buttons>()->enableIgnition(); - } else - { getModule<Buttons>()->disableIgnition(); - } break; } @@ -220,10 +214,8 @@ bool Radio::start() // Initialize mavdriver mavDriver = std::make_unique<MavDriver>( - radio.get(), - [this](MavDriver*, const mavlink_message_t& msg) - { handleMessage(msg); }, - Config::Radio::MAV_SLEEP_AFTER_SEND, + radio.get(), [this](MavDriver*, const mavlink_message_t& msg) + { handleMessage(msg); }, Config::Radio::MAV_SLEEP_AFTER_SEND, Config::Radio::MAV_OUT_BUFFER_MAX_AGE); if (!mavDriver->start()) @@ -264,4 +256,4 @@ Radio::Radio() buttonState.tars_btn = false; buttonState.arm_switch = false; buttonState.ignition_btn = false; -} \ No newline at end of file +} diff --git a/src/ConRIG/Serial/Serial.cpp b/src/ConRIG/Serial/Serial.cpp index d529813193780789526043f4bc75641ecda15f14..e2e7773e506f3e1fdc61ce89c573d0a2dd4ce149 100644 --- a/src/ConRIG/Serial/Serial.cpp +++ b/src/ConRIG/Serial/Serial.cpp @@ -32,15 +32,11 @@ using namespace Boardcore; bool Serial::start() { mavDriver = std::make_unique<SerialMavDriver>( - this, - [this](SerialMavDriver* channel, const mavlink_message_t& msg) - { handleMessage(msg); }, - 0, 10); + this, [this](SerialMavDriver* channel, const mavlink_message_t& msg) + { handleMessage(msg); }, 0, 10); if (!mavDriver->start()) - { return false; - } return true; } @@ -48,9 +44,7 @@ bool Serial::start() void Serial::sendMessage(const mavlink_message_t& msg) { if (mavDriver) - { mavDriver->enqueueMsg(msg); - } } void Serial::sendNack(const mavlink_message_t& msg) @@ -74,9 +68,7 @@ void Serial::sendAck(const mavlink_message_t& msg) void Serial::handleMessage(const mavlink_message_t& msg) { if (!getModule<Radio>()->enqueueMessage(msg)) - { sendNack(msg); - } } ssize_t Serial::receive(uint8_t* pkt, size_t maxLen) @@ -89,4 +81,4 @@ bool Serial::send(uint8_t* pkt, size_t len) { auto serial = miosix::DefaultConsole::instance().get(); return serial->writeBlock(pkt, len, 0) != static_cast<ssize_t>(len); -} \ No newline at end of file +} diff --git a/src/ConRIG/Serial/Serial.h b/src/ConRIG/Serial/Serial.h index a7a52c3555cae7adf78895a9d9e871a7b1cc9666..5003455593370028962d675e52b908d70506381c 100644 --- a/src/ConRIG/Serial/Serial.h +++ b/src/ConRIG/Serial/Serial.h @@ -78,4 +78,4 @@ private: Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("serial"); }; -} // namespace ConRIG \ No newline at end of file +} // namespace ConRIG diff --git a/src/Groundstation/Automated/Actuators/Actuators.cpp b/src/Groundstation/Automated/Actuators/Actuators.cpp index b9463c541c4b3ca388be47f6b03bb418a3be8086..a3d780d9b2337f3099db49b68eb204a14075b1ee 100644 --- a/src/Groundstation/Automated/Actuators/Actuators.cpp +++ b/src/Groundstation/Automated/Actuators/Actuators.cpp @@ -86,8 +86,8 @@ void Actuators::disarm() ActuationStatus Actuators::setSpeed(StepperList axis, float speed) { - const auto *config = getStepperConfig(axis); - auto *stepper = getStepper(axis); + const auto* config = getStepperConfig(axis); + auto* stepper = getStepper(axis); auto actuationState = ActuationStatus::OK; auto multiplier = getStepperMultiplier(axis); @@ -130,14 +130,14 @@ int16_t Actuators::getCurrentPosition(StepperList axis) float Actuators::getCurrentDegPosition(StepperList axis) { auto multiplier = getStepperMultiplier(axis); - auto *stepper = getStepper(axis); + auto* stepper = getStepper(axis); return stepper->getCurrentDegPosition() / multiplier; } ActuationStatus Actuators::move(StepperList axis, int16_t steps) { - auto *stepper = getStepper(axis); + auto* stepper = getStepper(axis); ActuationStatus actuationState = ActuationStatus::OK; //< In case the move command is not limited @@ -145,7 +145,7 @@ ActuationStatus Actuators::move(StepperList axis, int16_t steps) if (!stepper->isEnabled()) return ActuationStatus::DISABLED; - const auto *config = getStepperConfig(axis); + const auto* config = getStepperConfig(axis); float position = getCurrentPosition(axis); int16_t maxSteps = @@ -172,12 +172,12 @@ ActuationStatus Actuators::move(StepperList axis, int16_t steps) ActuationStatus Actuators::moveDeg(StepperList axis, float degrees) { - auto *stepper = getStepper(axis); + auto* stepper = getStepper(axis); ActuationStatus actuationState = ActuationStatus::OK; //< In case the move command is not limited - const auto *config = getStepperConfig(axis); + const auto* config = getStepperConfig(axis); auto multiplier = getStepperMultiplier(axis); float positionDeg = getCurrentDegPosition(axis); diff --git a/src/Groundstation/Automated/Config/PinHandlerConfig.h b/src/Groundstation/Automated/Config/PinHandlerConfig.h index 10e956a7421503248b44554d797683b775eb28ab..f0c0486a138f1a8d78771cafe8065d65b7c5a260 100644 --- a/src/Groundstation/Automated/Config/PinHandlerConfig.h +++ b/src/Groundstation/Automated/Config/PinHandlerConfig.h @@ -32,4 +32,4 @@ constexpr unsigned int ARM_SWITCH_THRESHOLD = 1; constexpr unsigned int ACTIVE_SWITCH_THRESHOLD = 1; } // namespace PinHandlerConfig -} // namespace Antennas \ No newline at end of file +} // namespace Antennas diff --git a/src/Groundstation/Automated/Converter.h b/src/Groundstation/Automated/Converter.h index 5c7ddf01828574a3ec122a1ba70682914efc69c2..c5ba00c636fbbd567b4a2d815fbd37ecd827c1ad 100644 --- a/src/Groundstation/Automated/Converter.h +++ b/src/Groundstation/Automated/Converter.h @@ -43,4 +43,4 @@ AntennaAngles rocketPositionToAntennaAngles(const NEDCoords& ned) angles.theta1 = std::atan2(ned.n, ned.e); angles.theta2 = std::atan2(-ned.d, ned.n); return angles; -} \ No newline at end of file +} diff --git a/src/Groundstation/Automated/Hub.cpp b/src/Groundstation/Automated/Hub.cpp index bf6c80042a85b239f6e9ad0ec7ef97a1d40da867..adc96f2c6ac6e0f43b20e1b10e2cede944e2b877 100644 --- a/src/Groundstation/Automated/Hub.cpp +++ b/src/Groundstation/Automated/Hub.cpp @@ -50,18 +50,14 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg) if (status->isMainRadioPresent() && msg.sysid == MAV_SYSID_MAIN) { if (!radioMain->sendMsg(msg)) - { sendNack(msg, 306); - } } if (status->isPayloadRadioPresent() && msg.sysid == MAV_SYSID_PAYLOAD) { LyraGS::RadioPayload* radioPayload = getModule<LyraGS::RadioPayload>(); if (!radioPayload->sendMsg(msg)) - { sendNack(msg, 306); - } } if (msg.sysid == MAV_SYSID_ARP) @@ -88,20 +84,14 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg) mavlink_msg_arp_command_tc_get_command_id(&msg)); if (commandId == MAV_CMD_FORCE_REBOOT) - { reboot(); - } auto it = commandToEvent.find(commandId); if (it != commandToEvent.end()) - { EventBroker::getInstance().post(it->second, TOPIC_TMTC); - } else - { return sendNack(msg, 301); - } // Acknowledge the message sendAck(msg); diff --git a/src/Groundstation/Automated/PinHandler/PinData.h b/src/Groundstation/Automated/PinHandler/PinData.h index b22b80f756347c4b5bc6bef6ddee5451be694f87..25f13d77223ca2f0ed04be45981fbe6e9fb7e84a 100644 --- a/src/Groundstation/Automated/PinHandler/PinData.h +++ b/src/Groundstation/Automated/PinHandler/PinData.h @@ -51,4 +51,4 @@ struct PinChangeData } }; -} // namespace Antennas \ No newline at end of file +} // namespace Antennas diff --git a/src/Groundstation/Automated/PinHandler/PinHandler.cpp b/src/Groundstation/Automated/PinHandler/PinHandler.cpp index d27517762252c0f4c1ef547bee4775d4f938c47a..d3a8dd0ffdc2f9a0dfce23634fa0e81fcb4f7d10 100644 --- a/src/Groundstation/Automated/PinHandler/PinHandler.cpp +++ b/src/Groundstation/Automated/PinHandler/PinHandler.cpp @@ -129,4 +129,4 @@ PinData PinHandler::getPinData(PinList pin) } } } -} // namespace Antennas \ No newline at end of file +} // namespace Antennas diff --git a/src/Groundstation/Automated/PinHandler/PinHandler.h b/src/Groundstation/Automated/PinHandler/PinHandler.h index 07448cdd1b30d5b864eaacf2483de207f0a0f213..18bffa9c808c69fef8bf49210f56f3d47b75bb86 100644 --- a/src/Groundstation/Automated/PinHandler/PinHandler.h +++ b/src/Groundstation/Automated/PinHandler/PinHandler.h @@ -72,4 +72,4 @@ private: Boardcore::TaskScheduler scheduler; Boardcore::PinObserver pin_observer; }; -} // namespace Antennas \ No newline at end of file +} // namespace Antennas diff --git a/src/Groundstation/Automated/SMA/SMA.cpp b/src/Groundstation/Automated/SMA/SMA.cpp index 7ae4e1da99c375e38c16d9c56137f0664dd1405e..57b1707431eac53f15799160dfeb1d7feca1c915 100644 --- a/src/Groundstation/Automated/SMA/SMA.cpp +++ b/src/Groundstation/Automated/SMA/SMA.cpp @@ -181,9 +181,7 @@ void SMA::update() case SMAState::INSERT_INFO: { if (antennaCoordinatesSet) - { EventBroker::getInstance().post(ARP_INFO_INSERTED, TOPIC_ARP); - } break; } @@ -785,9 +783,7 @@ State SMA::state_fix_rocket(const Event& event) // init the follower before leaving the state // (compute initial arp-rocket distance and bearing) if (!follower.init()) - { LOG_ERR(logger, "Follower initialization failed"); - } leds->setOn(LedColor::YELLOW); return HANDLED; @@ -942,9 +938,7 @@ State SMA::state_fix_rocket_nf(const Event& event) // init the follower before leaving the state // (compute initial arp-rocket distance and bearing) if (!follower.init()) - { LOG_ERR(logger, "Follower initialization failed"); - } leds->setOn(LedColor::YELLOW); return HANDLED; diff --git a/src/Groundstation/Automated/Sensors/Sensors.cpp b/src/Groundstation/Automated/Sensors/Sensors.cpp index ec7be31c65c28fd1c3b3c8b2336d82356db7d90b..866231a74dfb27b41b52d496cde6decf28920177 100644 --- a/src/Groundstation/Automated/Sensors/Sensors.cpp +++ b/src/Groundstation/Automated/Sensors/Sensors.cpp @@ -35,9 +35,7 @@ Sensors::Sensors() {} bool Sensors::start() { if (!vn300Init()) - { return false; - } sm = new SensorManager(sensorsMap); if (!sm->start()) @@ -72,4 +70,4 @@ void Sensors::vn300Callback() VN300Data Sensors::getVN300LastSample() { return vn300->getLastSample(); } -} // namespace Antennas \ No newline at end of file +} // namespace Antennas diff --git a/src/Groundstation/Automated/Sensors/Sensors.h b/src/Groundstation/Automated/Sensors/Sensors.h index a293f0a2f60bf3a0f2e2f955973f7129b0006836..87ce9a237b67bfc8b1923e3bace41de0a20a61b4 100644 --- a/src/Groundstation/Automated/Sensors/Sensors.h +++ b/src/Groundstation/Automated/Sensors/Sensors.h @@ -48,10 +48,10 @@ private: bool vn300Init(); void vn300Callback(); - Boardcore::VN300 *vn300 = nullptr; + Boardcore::VN300* vn300 = nullptr; - Boardcore::SensorManager *sm = nullptr; + Boardcore::SensorManager* sm = nullptr; Boardcore::SensorManager::SensorMap_t sensorsMap; Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("sensors"); }; -} // namespace Antennas \ No newline at end of file +} // namespace Antennas diff --git a/src/Groundstation/Automated/automated-antennas-entry.cpp b/src/Groundstation/Automated/automated-antennas-entry.cpp index 15c5ed3ed2562f390fbfe632ba6eebf8b8110ef7..3a022fc8cb07f136522f48d9634cc54bc626b7a7 100644 --- a/src/Groundstation/Automated/automated-antennas-entry.cpp +++ b/src/Groundstation/Automated/automated-antennas-entry.cpp @@ -62,7 +62,7 @@ GpioPin button = GpioPin(GPIOG_BASE, 10); ///< Emergency stop button */ int main() { - ModuleManager &modules = ModuleManager::getInstance(); + ModuleManager& modules = ModuleManager::getInstance(); PrintLogger logger = Logging::getLogger("automated_antennas"); bool ok = true; @@ -82,18 +82,18 @@ int main() }); ButtonHandler::getInstance().start(); - TaskScheduler *scheduler_low = new TaskScheduler(0); - TaskScheduler *scheduler_high = new TaskScheduler(); - Leds *leds = new Leds(scheduler_low); - Hub *hub = new Hub(); - Buses *buses = new Buses(); - Serial *serial = new Serial(); - RadioMain *radio_main = new RadioMain(); - BoardStatus *board_status = new BoardStatus(); - Actuators *actuators = new Actuators(); - Sensors *sensors = new Sensors(); - SMA *sma = new SMA(scheduler_high); - Ethernet *ethernet = new Ethernet(); + TaskScheduler* scheduler_low = new TaskScheduler(0); + TaskScheduler* scheduler_high = new TaskScheduler(); + Leds* leds = new Leds(scheduler_low); + Hub* hub = new Hub(); + Buses* buses = new Buses(); + Serial* serial = new Serial(); + RadioMain* radio_main = new RadioMain(); + BoardStatus* board_status = new BoardStatus(); + Actuators* actuators = new Actuators(); + Sensors* sensors = new Sensors(); + SMA* sma = new SMA(scheduler_high); + Ethernet* ethernet = new Ethernet(); // Inserting Modules { // TODO remove this scope (improve readability) @@ -181,9 +181,7 @@ int main() LOG_INFO(logger, "Modules setup successful"); if (board_status->isMainRadioPresent()) - { LOG_DEBUG(logger, "Main radio is present\n"); - } // If init fatal and sma not started, blink red endlessly if (init_fatal) @@ -201,8 +199,6 @@ int main() } while (true) - { Thread::wait(); - } return 0; } diff --git a/src/Groundstation/Automated/test-actuators.cpp b/src/Groundstation/Automated/test-actuators.cpp index 064b76b6aa4e8f8f15619548cae18fe6b4224a74..a92629b1535cecff7f8819344f433e9e6247de02 100644 --- a/src/Groundstation/Automated/test-actuators.cpp +++ b/src/Groundstation/Automated/test-actuators.cpp @@ -79,7 +79,7 @@ void errorLoop() } } -void test1(Actuators *actuators) +void test1(Actuators* actuators) { PrintLogger logger = PrintLogger{Logging::getLogger("test-actuators")}; LOG_INFO(logger, "Executing Test 1"); @@ -109,7 +109,7 @@ void test1(Actuators *actuators) LOG_INFO(logger, "Test 1 completed\n"); } -void test2(Actuators *actuators) +void test2(Actuators* actuators) { PrintLogger logger = PrintLogger{Logging::getLogger("test-actuators")}; LOG_INFO(logger, "Executing Test 2"); @@ -138,7 +138,7 @@ void test2(Actuators *actuators) LOG_INFO(logger, "Test 2 completed\n"); } -void test3(Actuators *actuators) +void test3(Actuators* actuators) { actuators->setSpeed(StepperList::STEPPER_X, STEPPER_SPEED); actuators->setSpeed(StepperList::STEPPER_Y, STEPPER_SPEED); @@ -154,7 +154,7 @@ void test3(Actuators *actuators) ledWaitLoop(TEST_WAIT); } -void test6(Actuators *actuators) +void test6(Actuators* actuators) { PrintLogger logger = PrintLogger{Logging::getLogger("test-actuators")}; LOG_INFO(logger, "Executing Test 6"); @@ -193,11 +193,11 @@ int main() { ledOff(); - Hub *hub = new Hub(); - LyraGS::Buses *buses = new LyraGS::Buses(); - Serial *serial = new Serial(); - Actuators *actuators = new Actuators(); - Sensors *sensors = new Sensors(); + Hub* hub = new Hub(); + LyraGS::Buses* buses = new LyraGS::Buses(); + Serial* serial = new Serial(); + Actuators* actuators = new Actuators(); + Sensors* sensors = new Sensors(); DependencyManager manager; PrintLogger logger = PrintLogger{Logging::getLogger("test-actuators")}; @@ -264,8 +264,6 @@ int main() led1Off(); while (1) - { Thread::sleep(1000); - } return 0; -} \ No newline at end of file +} diff --git a/src/Groundstation/Automated/test-automated-radio.cpp b/src/Groundstation/Automated/test-automated-radio.cpp index 0924097e9701dd137db438926729d641ba5ce01f..c33b71cdde30e8d197e6e4ca81e12f2c697e5838 100644 --- a/src/Groundstation/Automated/test-automated-radio.cpp +++ b/src/Groundstation/Automated/test-automated-radio.cpp @@ -35,9 +35,7 @@ using namespace miosix; void spinLoop() { while (1) - { Thread::sleep(1000); - } } void errorLoop() @@ -55,13 +53,13 @@ int main() { ledOff(); - Hub *hub = new Hub(); - Buses *buses = new Buses(); - Serial *serial = new Serial(); - RadioMain *radio_main = new RadioMain(); - BoardStatus *board_status = new BoardStatus(); + Hub* hub = new Hub(); + Buses* buses = new Buses(); + Serial* serial = new Serial(); + RadioMain* radio_main = new RadioMain(); + BoardStatus* board_status = new BoardStatus(); - ModuleManager &modules = ModuleManager::getInstance(); + ModuleManager& modules = ModuleManager::getInstance(); bool ok = true; @@ -82,33 +80,23 @@ int main() ok &= serial->start(); if (!ok) - { printf("[error] Failed to start serial!\n"); - } ok &= radio_main->start(); if (!ok) - { printf("[error] Failed to start main radio!\n"); - } ok &= board_status->start(); if (!ok) - { printf("[error] Failed to start radio status!\n"); - } if (board_status->isMainRadioPresent()) - { led2On(); - } if (!ok) - { errorLoop(); - } led1On(); spinLoop(); return 0; -} \ No newline at end of file +} diff --git a/src/Groundstation/Automated/test-smcontroller.cpp b/src/Groundstation/Automated/test-smcontroller.cpp index 500e82291a8eacf0442306d00a7a35529dd755b1..a77712da36b4483cfa9ae33f823ea4fa89d41a0d 100644 --- a/src/Groundstation/Automated/test-smcontroller.cpp +++ b/src/Groundstation/Automated/test-smcontroller.cpp @@ -29,19 +29,19 @@ #include <memory> #include <unordered_set> -#define ARP_EVENTS \ - { \ - ARP_INIT_OK, ARP_INIT_ERROR, ARP_CAL_DONE, ARP_FIX_ANTENNAS, \ - ARP_FIX_ROCKET \ - } - -#define TMTC_EVENTS \ - { \ - TMTC_ARP_FORCE_INIT, TMTC_ARP_RESET_ALGORITHM, \ - TMTC_ARP_FORCE_NO_FEEDBACK, TMTC_ARP_ARM, TMTC_ARP_DISARM, \ - TMTC_ARP_CALIBRATE, TMTC_ARP_ENTER_TEST_MODE, \ - TMTC_ARP_EXIT_TEST_MODE \ - } +#define ARP_EVENTS \ + {ARP_INIT_OK, ARP_INIT_ERROR, ARP_CAL_DONE, ARP_FIX_ANTENNAS, \ + ARP_FIX_ROCKET} + +#define TMTC_EVENTS \ + {TMTC_ARP_FORCE_INIT, \ + TMTC_ARP_RESET_ALGORITHM, \ + TMTC_ARP_FORCE_NO_FEEDBACK, \ + TMTC_ARP_ARM, \ + TMTC_ARP_DISARM, \ + TMTC_ARP_CALIBRATE, \ + TMTC_ARP_ENTER_TEST_MODE, \ + TMTC_ARP_EXIT_TEST_MODE} #define TEST_STATE(INITIAL_STATE, EVENT, TOPIC_SM, FINAL_STATE) \ sm->forceState(&SMA::INITIAL_STATE); \ diff --git a/src/Groundstation/Common/Config/EthernetConfig.h b/src/Groundstation/Common/Config/EthernetConfig.h index e54c5658b9a6638ef19d54f78e2767e85f0eefde..863597f1db1119fc6ea41bf279890bfa1d90de9f 100644 --- a/src/Groundstation/Common/Config/EthernetConfig.h +++ b/src/Groundstation/Common/Config/EthernetConfig.h @@ -37,4 +37,4 @@ constexpr Boardcore::WizIp IP_BASE = {192, 168, 1, 0}; constexpr Boardcore::WizIp GATEWAY = {192, 168, 1, 1}; constexpr Boardcore::WizIp SUBNET = {255, 255, 255, 0}; -} // namespace Groundstation \ No newline at end of file +} // namespace Groundstation diff --git a/src/Groundstation/Common/Config/GeneralConfig.h b/src/Groundstation/Common/Config/GeneralConfig.h index ef304c86dd5bc6069f6bd6430691b44cb461e22a..0cbd6a431f167ce45d416728968fc5129ef2fd8f 100644 --- a/src/Groundstation/Common/Config/GeneralConfig.h +++ b/src/Groundstation/Common/Config/GeneralConfig.h @@ -32,4 +32,4 @@ constexpr uint8_t GS_COMPONENT_ID = 1; constexpr uint8_t ARP_SYSTEM_ID = SysIDs::MAV_SYSID_ARP; constexpr uint8_t ARP_COMPONENT_ID = 1; -} // namespace Groundstation \ No newline at end of file +} // namespace Groundstation diff --git a/src/Groundstation/Common/HubBase.cpp b/src/Groundstation/Common/HubBase.cpp index 8effcfb775c71bdfad2d9df313ee2a067caa5235..9c191b2a2d7284af250275fb1be81283f30f5902 100644 --- a/src/Groundstation/Common/HubBase.cpp +++ b/src/Groundstation/Common/HubBase.cpp @@ -33,4 +33,4 @@ void HubBase::sendNack(const mavlink_message_t& msg, const uint16_t errId) msg.msgid, msg.seq, 0); dispatchIncomingMsg(nack_msg); -} \ No newline at end of file +} diff --git a/src/Groundstation/Common/HubBase.h b/src/Groundstation/Common/HubBase.h index f121655d1b7415db23fa08dc3d6809ca157e74bb..8118bd296b38cffec333eac00e08dedd9551aab4 100644 --- a/src/Groundstation/Common/HubBase.h +++ b/src/Groundstation/Common/HubBase.h @@ -54,4 +54,4 @@ protected: void sendNack(const mavlink_message_t& msg, const uint16_t errId); }; -} // namespace Groundstation \ No newline at end of file +} // namespace Groundstation diff --git a/src/Groundstation/Common/Ports/EthernetBase.cpp b/src/Groundstation/Common/Ports/EthernetBase.cpp index 2a07d43310fd5c75e95ba7a715f0997def1e5e23..f183a9a64b728425db6bc3a3c2982159b5e80874 100644 --- a/src/Groundstation/Common/Ports/EthernetBase.cpp +++ b/src/Groundstation/Common/Ports/EthernetBase.cpp @@ -51,17 +51,13 @@ WizMac Groundstation::genNewRandomMac() void EthernetBase::handleINTn() { if (wiz5500) - { wiz5500->handleINTn(); - } } void EthernetBase::sendMsg(const mavlink_message_t& msg) { if (mav_driver && mav_driver->isStarted()) - { mav_driver->enqueueMsg(msg); - } } Boardcore::Wiz5500::PhyState EthernetBase::getState() @@ -110,9 +106,7 @@ bool EthernetBase::start(std::unique_ptr<Boardcore::Wiz5500> wiz5500) mav_driver = std::make_unique<EthernetMavDriver>(this, mav_handler, 0, 10); if (!mav_driver->start()) - { return false; - } return true; } @@ -133,4 +127,4 @@ ssize_t EthernetBase::receive(uint8_t* pkt, size_t max_len) bool EthernetBase::send(uint8_t* pkt, size_t len) { return wiz5500->send(0, pkt, len, 100); -} \ No newline at end of file +} diff --git a/src/Groundstation/Common/Ports/EthernetBase.h b/src/Groundstation/Common/Ports/EthernetBase.h index 1113560c822abf9b11e8bd81ec92101ae80bb5f5..5f997f6caed74ecbfaf0dc24cc7353dcd116cd52 100644 --- a/src/Groundstation/Common/Ports/EthernetBase.h +++ b/src/Groundstation/Common/Ports/EthernetBase.h @@ -44,7 +44,7 @@ class EthernetBase : public Boardcore::Transceiver, public Boardcore::InjectableWithDeps<HubBase> { public: - EthernetBase(){}; + EthernetBase() {}; EthernetBase(bool randomIp, uint8_t ipOffset) : randomIp{randomIp}, ipOffset{ipOffset} {}; @@ -74,4 +74,4 @@ private: uint8_t ipOffset = 0; }; -} // namespace Groundstation \ No newline at end of file +} // namespace Groundstation diff --git a/src/Groundstation/Common/Ports/Serial.cpp b/src/Groundstation/Common/Ports/Serial.cpp index a8716aaeba18190e5825aa57c8cd82f58f6c8b08..5c9cc2276b980eb726136e7b197be153d8b49d02 100644 --- a/src/Groundstation/Common/Ports/Serial.cpp +++ b/src/Groundstation/Common/Ports/Serial.cpp @@ -34,9 +34,7 @@ bool Serial::start() mav_driver = std::make_unique<SerialMavDriver>(this, mav_handler, 0, 10); if (!mav_driver->start()) - { return false; - } return true; } @@ -44,9 +42,7 @@ bool Serial::start() void Serial::sendMsg(const mavlink_message_t& msg) { if (mav_driver && mav_driver->isStarted()) - { mav_driver->enqueueMsg(msg); - } } void Serial::handleMsg(const mavlink_message_t& msg) @@ -65,4 +61,4 @@ bool Serial::send(uint8_t* pkt, size_t len) { auto serial = miosix::DefaultConsole::instance().get(); return serial->writeBlock(pkt, len, 0) != static_cast<ssize_t>(len); -} \ No newline at end of file +} diff --git a/src/Groundstation/Common/Ports/Serial.h b/src/Groundstation/Common/Ports/Serial.h index abfba264dbec44b73200440f594bffaa68f5af84..108625dcc35cd403ec35f0fca5317710261803f6 100644 --- a/src/Groundstation/Common/Ports/Serial.h +++ b/src/Groundstation/Common/Ports/Serial.h @@ -70,4 +70,4 @@ private: std::unique_ptr<SerialMavDriver> mav_driver; }; -} // namespace Groundstation \ No newline at end of file +} // namespace Groundstation diff --git a/src/Groundstation/Common/Radio/RadioBase.cpp b/src/Groundstation/Common/Radio/RadioBase.cpp index e6931d701754e2537ee12813bacd1baf4fea17ae..0026a0291bab1873e00b7eea95cf592bacc0354c 100644 --- a/src/Groundstation/Common/Radio/RadioBase.cpp +++ b/src/Groundstation/Common/Radio/RadioBase.cpp @@ -49,9 +49,7 @@ bool RadioBase::sendMsg(const mavlink_message_t& msg) void RadioBase::handleDioIRQ() { if (started) - { sx1278->handleDioIRQ(); - } } RadioStats RadioBase::getStats() @@ -87,14 +85,10 @@ bool RadioBase::start(std::unique_ptr<SX1278Fsk> sx1278) Groundstation::MAV_OUT_BUFFER_MAX_AGE); if (!mav_driver->start()) - { return false; - } if (!ActiveObject::start()) - { return false; - } started = true; return true; @@ -102,7 +96,6 @@ bool RadioBase::start(std::unique_ptr<SX1278Fsk> sx1278) void RadioBase::run() { - while (!shouldStop()) { miosix::Thread::sleep(AUTOMATIC_FLUSH_PERIOD); @@ -120,9 +113,7 @@ ssize_t RadioBase::receive(uint8_t* pkt, size_t max_len) { ssize_t ret = sx1278->receive(pkt, max_len); if (ret > 0) - { bits_rx_count += ret * 8; - } return ret; } @@ -131,9 +122,7 @@ bool RadioBase::send(uint8_t* pkt, size_t len) { bool ret = sx1278->send(pkt, len); if (ret) - { bits_tx_count += len * 8; - } return ret; } @@ -154,9 +143,7 @@ void RadioBase::flush() { Lock<FastMutex> l(pending_msgs_mutex); for (size_t i = 0; i < pending_msgs_count; i++) - { mav_driver->enqueueMsg(pending_msgs[i]); - } pending_msgs_count = 0; } @@ -165,4 +152,4 @@ bool RadioBase::isEndOfTransmissionPacket(const mavlink_message_t& msg) { return msg.msgid == MAVLINK_MSG_ID_ROCKET_FLIGHT_TM || msg.msgid == MAVLINK_MSG_ID_PAYLOAD_FLIGHT_TM; -} \ No newline at end of file +} diff --git a/src/Groundstation/Common/Radio/RadioBase.h b/src/Groundstation/Common/Radio/RadioBase.h index 6a6c454ab395f52ce38ad9c6b434a4b63e4f3321..12d44d748eff31951ba83878a0fc690cf8661bb5 100644 --- a/src/Groundstation/Common/Radio/RadioBase.h +++ b/src/Groundstation/Common/Radio/RadioBase.h @@ -128,4 +128,4 @@ private: std::unique_ptr<RadioMavDriver> mav_driver; }; -} // namespace Groundstation \ No newline at end of file +} // namespace Groundstation diff --git a/src/Groundstation/LyraGS/Base/Hub.cpp b/src/Groundstation/LyraGS/Base/Hub.cpp index e6ea141eadbc17520b4f0279c5b12ad6e94b7fe3..da98e603e3010a780a757a3261b8f400bf0ac51e 100644 --- a/src/Groundstation/LyraGS/Base/Hub.cpp +++ b/src/Groundstation/LyraGS/Base/Hub.cpp @@ -71,4 +71,4 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& msg) LyraGS::EthernetGS* ethernet = getModule<LyraGS::EthernetGS>(); ethernet->sendMsg(msg); } -} \ No newline at end of file +} diff --git a/src/Groundstation/LyraGS/Base/Hub.h b/src/Groundstation/LyraGS/Base/Hub.h index 628f3d6177ebb11283cc90907ed0333d03e9cea6..d1040b642864cd91025c263aad120775a25905b6 100644 --- a/src/Groundstation/LyraGS/Base/Hub.h +++ b/src/Groundstation/LyraGS/Base/Hub.h @@ -52,4 +52,4 @@ public: void dispatchIncomingMsg(const mavlink_message_t& msg) override; }; -} // namespace GroundstationBase \ No newline at end of file +} // namespace GroundstationBase diff --git a/src/Groundstation/LyraGS/BoardStatus.cpp b/src/Groundstation/LyraGS/BoardStatus.cpp index 4aaeeaaaea12237c82e3fd5bd5ef31b12421295a..0b7f6bb91c95ca19ae1e31f94340a0b79ffbfee5 100644 --- a/src/Groundstation/LyraGS/BoardStatus.cpp +++ b/src/Groundstation/LyraGS/BoardStatus.cpp @@ -33,9 +33,7 @@ bool BoardStatus::isEthernetPresent() { return ethernet_present; } bool BoardStatus::start() { if (!ActiveObject::start()) - { return false; - } return true; } @@ -63,8 +61,8 @@ void BoardStatus::arpRoutine() auto vn300 = getModule<Sensors>()->getVN300LastSample(); - Actuators *actuators = getModule<Actuators>(); - SMA *sm = getModule<SMA>(); + Actuators* actuators = getModule<Actuators>(); + SMA* sm = getModule<SMA>(); AntennaAngles targetAngles = sm->getTargetAngles(); @@ -190,10 +188,8 @@ void BoardStatus::GSRoutine() void BoardStatus::run() { while (!shouldStop()) - { if (isArp) arpRoutine(); else GSRoutine(); - } -} \ No newline at end of file +} diff --git a/src/Groundstation/LyraGS/BoardStatus.h b/src/Groundstation/LyraGS/BoardStatus.h index 185327fc8af97ed0a897857e658d3079fb6022fd..3eb286799a41a297744808a0244e435ecfa7f4c6 100644 --- a/src/Groundstation/LyraGS/BoardStatus.h +++ b/src/Groundstation/LyraGS/BoardStatus.h @@ -138,4 +138,4 @@ private: bool isArp = false; }; -} // namespace LyraGS \ No newline at end of file +} // namespace LyraGS diff --git a/src/Groundstation/LyraGS/Buses.h b/src/Groundstation/LyraGS/Buses.h index 9d678f612e13d9cc128d229d9e1956f9705aed71..092145958e1ed1f07938b630cb841a48a6a50045 100644 --- a/src/Groundstation/LyraGS/Buses.h +++ b/src/Groundstation/LyraGS/Buses.h @@ -33,7 +33,7 @@ namespace LyraGS class Buses : public Boardcore::Injectable { public: - Boardcore::SPIBus &getRadio() { return radio1_bus; } + Boardcore::SPIBus& getRadio() { return radio1_bus; } Boardcore::SPIBus radio1_bus{MIOSIX_RADIO1_SPI}; Boardcore::SPIBus radio2_bus{MIOSIX_RADIO2_SPI}; @@ -42,4 +42,4 @@ public: Boardcore::SPIBus ethernet_bus{MIOSIX_ETHERNET_SPI}; }; -} // namespace LyraGS \ No newline at end of file +} // namespace LyraGS diff --git a/src/Groundstation/LyraGS/Ports/Ethernet.cpp b/src/Groundstation/LyraGS/Ports/Ethernet.cpp index f4223d71deca2c96ffa71999ae01a712f8c430bd..6b3e3270fc12fa4186473c77f74e6cf6f1c58d41 100644 --- a/src/Groundstation/LyraGS/Ports/Ethernet.cpp +++ b/src/Groundstation/LyraGS/Ports/Ethernet.cpp @@ -53,14 +53,10 @@ bool EthernetGS::start() bool present = wiz5500->checkVersion(); if (!present) - { return false; - } if (!EthernetBase::start(std::move(wiz5500))) - { return false; - } ethernetGSGlobal = this; getModule<BoardStatus>()->setEthernetPresent(true); @@ -75,4 +71,4 @@ Boardcore::Wiz5500::PhyState EthernetGS::getState() { return Super::getState(); }; -} // namespace LyraGS \ No newline at end of file +} // namespace LyraGS diff --git a/src/Groundstation/LyraGS/Ports/Ethernet.h b/src/Groundstation/LyraGS/Ports/Ethernet.h index fe6913f22d14bbc265c124a9487aba57b2ba9332..a267a7abe1cef7bc863076659560f706ab3e3167 100644 --- a/src/Groundstation/LyraGS/Ports/Ethernet.h +++ b/src/Groundstation/LyraGS/Ports/Ethernet.h @@ -43,4 +43,4 @@ public: Boardcore::Wiz5500::PhyState getState(); }; -} // namespace LyraGS \ No newline at end of file +} // namespace LyraGS diff --git a/src/Groundstation/LyraGS/Ports/SerialLyraGS.cpp b/src/Groundstation/LyraGS/Ports/SerialLyraGS.cpp index 09f07474f57ca0b8a5c22805aa711413cf380f78..1b4cef60cab1305a1b177341981da22b1103d8da 100644 --- a/src/Groundstation/LyraGS/Ports/SerialLyraGS.cpp +++ b/src/Groundstation/LyraGS/Ports/SerialLyraGS.cpp @@ -33,9 +33,7 @@ bool SerialLyraGS::start() mav_driver = std::make_unique<SerialMavDriver>(this, mav_handler, 0, 10); if (!mav_driver->start()) - { return false; - } return true; } @@ -43,9 +41,7 @@ bool SerialLyraGS::start() void SerialLyraGS::sendMsg(const mavlink_message_t& msg) { if (mav_driver && mav_driver->isStarted()) - { mav_driver->enqueueMsg(msg); - } } void SerialLyraGS::handleMsg(const mavlink_message_t& msg) @@ -67,4 +63,4 @@ bool SerialLyraGS::send(uint8_t* pkt, size_t len) Boardcore::USART& serial = getModule<Buses>()->uart4; serial.write(pkt, len); return true; -} \ No newline at end of file +} diff --git a/src/Groundstation/LyraGS/Ports/SerialLyraGS.h b/src/Groundstation/LyraGS/Ports/SerialLyraGS.h index bc54eb152fc0a72f592572001ad32792eef5f896..5f926c036d927ac44e2521fc3fa60d489dcc6d95 100644 --- a/src/Groundstation/LyraGS/Ports/SerialLyraGS.h +++ b/src/Groundstation/LyraGS/Ports/SerialLyraGS.h @@ -75,4 +75,4 @@ private: std::unique_ptr<SerialMavDriver> mav_driver; }; -} // namespace LyraGS \ No newline at end of file +} // namespace LyraGS diff --git a/src/Groundstation/LyraGS/Radio/Radio.cpp b/src/Groundstation/LyraGS/Radio/Radio.cpp index b52bb12410232be1f63dc52c14c2374ecd5e1b54..1093b22e5b35a0e30224265ce648b93582efed05 100644 --- a/src/Groundstation/LyraGS/Radio/Radio.cpp +++ b/src/Groundstation/LyraGS/Radio/Radio.cpp @@ -143,9 +143,7 @@ bool RadioPayload::start() // Initialize if only if present if (!RadioBase::start(std::move(sx1278))) - { return false; - } } return true; @@ -169,4 +167,4 @@ bool RadioPayload::sendMsg(const mavlink_message_t& msg) if (txEnable) return RadioBase::sendMsg(msg); return false; -}; \ No newline at end of file +}; diff --git a/src/Groundstation/LyraGS/Radio/Radio.h b/src/Groundstation/LyraGS/Radio/Radio.h index 48d6d3009c6d9a5ca16579d88b811ea4bbfbc796..840d81283f72a993ff6561849fa2887783a135e8 100644 --- a/src/Groundstation/LyraGS/Radio/Radio.h +++ b/src/Groundstation/LyraGS/Radio/Radio.h @@ -89,4 +89,4 @@ private: bool txEnable = true; }; -} // namespace LyraGS \ No newline at end of file +} // namespace LyraGS diff --git a/src/Groundstation/LyraGS/lyra-gs-entry.cpp b/src/Groundstation/LyraGS/lyra-gs-entry.cpp index 67afc5177d80d27c2a74b8a0e32aea279107b775..869328463a5abc32c44df6001423c7d8c5337773 100644 --- a/src/Groundstation/LyraGS/lyra-gs-entry.cpp +++ b/src/Groundstation/LyraGS/lyra-gs-entry.cpp @@ -76,9 +76,7 @@ DipStatusLyraGS getDipStatus(uint8_t read) void idleLoop() { while (1) - { Thread::wait(); - } } /** @@ -135,26 +133,26 @@ int main() PrintLogger logger = Logging::getLogger("lyra_gs"); // TODO: Board scheduler for the schedulers - TaskScheduler *scheduler_low = new TaskScheduler(0); - TaskScheduler *scheduler_high = new TaskScheduler(); - Buses *buses = new Buses(); - SerialLyraGS *serial = new SerialLyraGS(); - LyraGS::RadioMain *radio_main = + TaskScheduler* scheduler_low = new TaskScheduler(0); + TaskScheduler* scheduler_high = new TaskScheduler(); + Buses* buses = new Buses(); + SerialLyraGS* serial = new SerialLyraGS(); + LyraGS::RadioMain* radio_main = new LyraGS::RadioMain(dipRead.mainHasBackup, dipRead.mainTXenable); - LyraGS::BoardStatus *board_status = new LyraGS::BoardStatus(dipRead.isARP); - LyraGS::EthernetGS *ethernet = + LyraGS::BoardStatus* board_status = new LyraGS::BoardStatus(dipRead.isARP); + LyraGS::EthernetGS* ethernet = new LyraGS::EthernetGS(false, dipRead.ipConfig); - LyraGS::RadioPayload *radio_payload = new LyraGS::RadioPayload( + LyraGS::RadioPayload* radio_payload = new LyraGS::RadioPayload( dipRead.payloadHasBackup, dipRead.payloadTXenable); - HubBase *hub = nullptr; + HubBase* hub = nullptr; // ARP-related things - Antennas::Actuators *actuators = nullptr; - Antennas::Leds *leds = nullptr; - Antennas::Sensors *sensors = nullptr; - Antennas::SMA *sma = nullptr; - Antennas::PinHandler *pinHandler = nullptr; + Antennas::Actuators* actuators = nullptr; + Antennas::Leds* leds = nullptr; + Antennas::Sensors* sensors = nullptr; + Antennas::SMA* sma = nullptr; + Antennas::PinHandler* pinHandler = nullptr; bool ok = true; diff --git a/src/Groundstation/Nokia/Buses.h b/src/Groundstation/Nokia/Buses.h index d167571c6ae835426613c9ec01f1e47c2382fd51..a05ac44c01212b2f5982b32059ed055b6901e805 100644 --- a/src/Groundstation/Nokia/Buses.h +++ b/src/Groundstation/Nokia/Buses.h @@ -38,4 +38,4 @@ public: Buses() : radio_bus(SPI4) {} }; -} // namespace GroundstationNokia \ No newline at end of file +} // namespace GroundstationNokia diff --git a/src/Groundstation/Nokia/Hub.cpp b/src/Groundstation/Nokia/Hub.cpp index 1767efca300eee3581fbc04622c3e9b3e168c427..b43665c31aaae0f0109af21ae09748f92c35dadb 100644 --- a/src/Groundstation/Nokia/Hub.cpp +++ b/src/Groundstation/Nokia/Hub.cpp @@ -35,13 +35,11 @@ void Hub::dispatchOutgoingMsg(const mavlink_message_t& msg) Radio* radio = getModule<Radio>(); if (!radio->sendMsg(msg)) - { sendNack(msg, 0); - } } void Hub::dispatchIncomingMsg(const mavlink_message_t& msg) { Serial* serial = getModule<Serial>(); serial->sendMsg(msg); -} \ No newline at end of file +} diff --git a/src/Groundstation/Nokia/Hub.h b/src/Groundstation/Nokia/Hub.h index ab90123f1d74359d6a51f1522ee2e2142eb99ce7..15c641ccb9e122a1b7f92d3e769477979f550d5e 100644 --- a/src/Groundstation/Nokia/Hub.h +++ b/src/Groundstation/Nokia/Hub.h @@ -54,4 +54,4 @@ public: void dispatchIncomingMsg(const mavlink_message_t& msg) override; }; -} // namespace GroundstationNokia \ No newline at end of file +} // namespace GroundstationNokia diff --git a/src/Groundstation/Nokia/Radio/Radio.cpp b/src/Groundstation/Nokia/Radio/Radio.cpp index 77a4c41bd8d98107597addfee0de8d87f416f777..1c0bea484f1f4e6959cbfa6e4a51ee054647af15 100644 --- a/src/Groundstation/Nokia/Radio/Radio.cpp +++ b/src/Groundstation/Nokia/Radio/Radio.cpp @@ -77,21 +77,15 @@ bool Radio::start() // First check if the device is even connected if (!sx1278->checkVersion()) - { return false; - } // Configure the radio if (sx1278->configure(Common::MAIN_RADIO_CONFIG) != SX1278Fsk::Error::NONE) - { return false; - } // Initialize if only if present if (!RadioBase::start(std::move(sx1278))) - { return false; - } return true; -} \ No newline at end of file +} diff --git a/src/Groundstation/Nokia/Radio/Radio.h b/src/Groundstation/Nokia/Radio/Radio.h index 746310fa71a61aefaa7055d4ed0a44e27ac7036a..7df336e6b5dd488bdc985159aabdc0c6251a4b5c 100644 --- a/src/Groundstation/Nokia/Radio/Radio.h +++ b/src/Groundstation/Nokia/Radio/Radio.h @@ -36,4 +36,4 @@ public: [[nodiscard]] bool start(); }; -} // namespace GroundstationNokia \ No newline at end of file +} // namespace GroundstationNokia diff --git a/src/Groundstation/Nokia/nokia-groundstation-entry.cpp b/src/Groundstation/Nokia/nokia-groundstation-entry.cpp index 93d0f1e22be1bc2d51c1e7c501fb52361b598cee..6823bb091537cdb83eebab88af6b1c243d5bc020 100644 --- a/src/Groundstation/Nokia/nokia-groundstation-entry.cpp +++ b/src/Groundstation/Nokia/nokia-groundstation-entry.cpp @@ -35,19 +35,17 @@ using namespace miosix; void idleLoop() { while (1) - { Thread::wait(); - } } int main() { ledOff(); - Hub *hub = new Hub(); - Buses *buses = new Buses(); - Radio *radio = new Radio(); - Serial *serial = new Serial(); + Hub* hub = new Hub(); + Buses* buses = new Buses(); + Radio* radio = new Radio(); + Serial* serial = new Serial(); DependencyManager manager; @@ -75,21 +73,15 @@ int main() ok &= serial->start(); if (!ok) - { printf("[error] Failed to start serial!\n"); - } ok &= radio->start(); if (!ok) - { printf("[error] Failed to start radio!\n"); - } if (ok) - { printf("Init complete!\n"); - } idleLoop(); return 0; -} \ No newline at end of file +} diff --git a/src/Groundstation/Rovie/Buses.h b/src/Groundstation/Rovie/Buses.h index 54d1ce0175a906953dc95ef8ec5f0dfaaf25f33d..d38d37821acc5af96c002731491a4ae8b63be711 100644 --- a/src/Groundstation/Rovie/Buses.h +++ b/src/Groundstation/Rovie/Buses.h @@ -39,4 +39,4 @@ public: Buses() : radio1(MIOSIX_RADIO1_SPI), ethernet(MIOSIX_ETHERNET_SPI) {} }; -} // namespace GroundstationRovie \ No newline at end of file +} // namespace GroundstationRovie diff --git a/src/Groundstation/Rovie/Hub.cpp b/src/Groundstation/Rovie/Hub.cpp index f91e023cdd12f7149e27d52a97423458530d183f..e5c9f4302469ff8e61e396858b9494bce8aa3ad5 100644 --- a/src/Groundstation/Rovie/Hub.cpp +++ b/src/Groundstation/Rovie/Hub.cpp @@ -40,4 +40,4 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& msg) { getModule<Serial>()->sendMsg(msg); getModule<Ethernet>()->sendMsg(msg); -} \ No newline at end of file +} diff --git a/src/Groundstation/Rovie/Hub.h b/src/Groundstation/Rovie/Hub.h index 02b1328267bf1946f9e4e1ea9141d3616d5b0d64..4aa25089ba3981008aa8fe3901948dce7326ed87 100644 --- a/src/Groundstation/Rovie/Hub.h +++ b/src/Groundstation/Rovie/Hub.h @@ -55,4 +55,4 @@ public: void dispatchIncomingMsg(const mavlink_message_t& msg) override; }; -} // namespace GroundstationRovie \ No newline at end of file +} // namespace GroundstationRovie diff --git a/src/Groundstation/Rovie/Ports/Ethernet.cpp b/src/Groundstation/Rovie/Ports/Ethernet.cpp index 3be88ebe667cc86baeae81945b32fd1a451cb62b..bf2cdc7ec9e76aa9c2fb1ae609b7d9fedebdf46e 100644 --- a/src/Groundstation/Rovie/Ports/Ethernet.cpp +++ b/src/Groundstation/Rovie/Ports/Ethernet.cpp @@ -36,9 +36,7 @@ void __attribute__((used)) MIOSIX_ETHERNET_IRQ() { Wiz5500* instance = gWiz5500; if (instance) - { instance->handleINTn(); - } } void setIRQWiz5500(Wiz5500* instance) @@ -55,9 +53,7 @@ bool Ethernet::start() setIRQWiz5500(wiz5500.get()); if (!EthernetBase::start(std::move(wiz5500))) - { return false; - } return true; -} \ No newline at end of file +} diff --git a/src/Groundstation/Rovie/Ports/Ethernet.h b/src/Groundstation/Rovie/Ports/Ethernet.h index 7184483e7ed4ca09893d30aa65e3afe3e7cc73d9..9e3ac3904b065655d7aec02f21ab62f4456308e3 100644 --- a/src/Groundstation/Rovie/Ports/Ethernet.h +++ b/src/Groundstation/Rovie/Ports/Ethernet.h @@ -37,4 +37,4 @@ public: [[nodiscard]] bool start(); }; -} // namespace GroundstationRovie \ No newline at end of file +} // namespace GroundstationRovie diff --git a/src/Groundstation/Rovie/Radio/Radio.cpp b/src/Groundstation/Rovie/Radio/Radio.cpp index 4a3bc05945f74c3c296b52dd4ccd9518153cc02d..069111b431c28a21335bbd75067d33f0a0ec859e 100644 --- a/src/Groundstation/Rovie/Radio/Radio.cpp +++ b/src/Groundstation/Rovie/Radio/Radio.cpp @@ -37,9 +37,7 @@ void handleDioIRQ() { SX1278Lora* instance = gRadio; if (instance) - { instance->handleDioIRQ(); - } } void setIRQRadio(SX1278Lora* radio) @@ -73,9 +71,7 @@ bool RadioRig::start() // Configure the radio if (sx1278->configure(Common::RIG_RADIO_CONFIG) != SX1278Lora::Error::NONE) - { return false; - } auto mavHandler = [this](RadioMavDriver* channel, const mavlink_message_t& msg) { handleMsg(msg); }; @@ -85,9 +81,7 @@ bool RadioRig::start() Groundstation::MAV_OUT_BUFFER_MAX_AGE); if (!mavDriver->start()) - { return false; - } started = true; return true; @@ -97,4 +91,4 @@ void RadioRig::handleMsg(const mavlink_message_t& msg) { // Dispatch the message through the hub. getModule<HubBase>()->dispatchIncomingMsg(msg); -} \ No newline at end of file +} diff --git a/src/Groundstation/Rovie/Radio/Radio.h b/src/Groundstation/Rovie/Radio/Radio.h index 05dfb37514801b499f3169c8bea5f91d668c1c1e..e056b7ebb4d286f23f52c6b19703ae6cde4af09c 100644 --- a/src/Groundstation/Rovie/Radio/Radio.h +++ b/src/Groundstation/Rovie/Radio/Radio.h @@ -59,4 +59,4 @@ private: std::unique_ptr<RadioMavDriver> mavDriver; }; -} // namespace GroundstationRovie \ No newline at end of file +} // namespace GroundstationRovie diff --git a/src/Groundstation/Rovie/rovie-groundstation-entry.cpp b/src/Groundstation/Rovie/rovie-groundstation-entry.cpp index b52aaa1cd3a8246ca14c1c0726761a5ca8cf3182..dad14ececf5b6a2478f7ff11d196b98a2d20e99a 100644 --- a/src/Groundstation/Rovie/rovie-groundstation-entry.cpp +++ b/src/Groundstation/Rovie/rovie-groundstation-entry.cpp @@ -36,9 +36,7 @@ using namespace miosix; void idleLoop() { while (1) - { Thread::wait(); - } } void errorLoop() @@ -56,11 +54,11 @@ int main() { ledOff(); - Hub *hub = new Hub(); - Buses *buses = new Buses(); - Serial *serial = new Serial(); - Ethernet *ethernet = new Ethernet(); - RadioRig *radioRig = new RadioRig(); + Hub* hub = new Hub(); + Buses* buses = new Buses(); + Serial* serial = new Serial(); + Ethernet* ethernet = new Ethernet(); + RadioRig* radioRig = new RadioRig(); DependencyManager manager; @@ -110,4 +108,4 @@ int main() } return 0; -} \ No newline at end of file +} diff --git a/src/Main/Actuators/Actuators.cpp b/src/Main/Actuators/Actuators.cpp index f08f994740d867c984138d7f6bdcf02ee0d2f015..3fd35dad07dd76b19cb1d35e4b8717e78792baba 100644 --- a/src/Main/Actuators/Actuators.cpp +++ b/src/Main/Actuators/Actuators.cpp @@ -53,7 +53,7 @@ bool Actuators::isStarted() { return started; } bool Actuators::start() { - TaskScheduler &scheduler = + TaskScheduler& scheduler = getModule<BoardScheduler>()->getLowPriorityActuatorsScheduler(); servoAbk->enable(); @@ -103,7 +103,7 @@ void Actuators::openExpulsion() void Actuators::wiggleServo(ServosList servo) { Lock<FastMutex> lock{servosMutex}; - Servo *info = getServo(servo); + Servo* info = getServo(servo); if (info != nullptr) { unsafeSetServoPosition(info, 1.0f); @@ -125,7 +125,7 @@ void Actuators::wiggleCanServo(ServosList servo) float Actuators::getServoPosition(ServosList servo) { Lock<FastMutex> lock{servosMutex}; - Servo *info = getServo(servo); + Servo* info = getServo(servo); return info ? info->getPosition() : 0.0f; } @@ -133,17 +133,11 @@ bool Actuators::isCanServoOpen(ServosList servo) { Lock<FastMutex> lock{canServosMutex}; if (servo == ServosList::MAIN_VALVE) - { return canMainOpen; - } else if (servo == ServosList::VENTING_VALVE) - { return canVentingOpen; - } else - { return false; - } } void Actuators::setStatusOff() { statusOverflow = 0; } @@ -174,13 +168,9 @@ void Actuators::setCanServoOpen(ServosList servo, bool open) { Lock<FastMutex> lock{canServosMutex}; if (servo == ServosList::MAIN_VALVE) - { canMainOpen = open; - } else if (servo == ServosList::VENTING_VALVE) - { canVentingOpen = open; - } } void Actuators::camOn() { gpios::camEnable::high(); } @@ -209,13 +199,13 @@ void Actuators::buzzerOff() buzzer->disableChannel(TimerUtils::Channel::MIOSIX_BUZZER_CHANNEL); } -void Actuators::unsafeSetServoPosition(Servo *servo, float position) +void Actuators::unsafeSetServoPosition(Servo* servo, float position) { servo->setPosition(position); sdLogger.log(servo->getState()); } -Servo *Actuators::getServo(ServosList servo) +Servo* Actuators::getServo(ServosList servo) { switch (servo) { @@ -234,19 +224,16 @@ void Actuators::updateBuzzer() { buzzerOff(); } + else if (buzzerCounter >= buzzerOverflow) + { + // Enable the channel for this period + buzzerOn(); + buzzerCounter = 0; + } else { - if (buzzerCounter >= buzzerOverflow) - { - // Enable the channel for this period - buzzerOn(); - buzzerCounter = 0; - } - else - { - buzzerOff(); - buzzerCounter += 1; - } + buzzerOff(); + buzzerCounter += 1; } } @@ -259,13 +246,9 @@ void Actuators::updateStatus() else { if (statusCounter >= statusOverflow) - { statusOn(); - } else - { statusOff(); - } if (statusCounter >= statusOverflow * 2) { diff --git a/src/Main/Actuators/Actuators.h b/src/Main/Actuators/Actuators.h index 6b86511795dcd3ebc699f01f7fddd540b718ca84..85aa5e531ecec707a8fe5a203ab5a54d98f84941 100644 --- a/src/Main/Actuators/Actuators.h +++ b/src/Main/Actuators/Actuators.h @@ -74,9 +74,9 @@ public: void setCanServoOpen(ServosList servo, bool open); private: - void unsafeSetServoPosition(Boardcore::Servo *servo, float position); + void unsafeSetServoPosition(Boardcore::Servo* servo, float position); - Boardcore::Servo *getServo(ServosList servo); + Boardcore::Servo* getServo(ServosList servo); void statusOn(); void statusOff(); @@ -88,7 +88,7 @@ private: void updateStatus(); Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("actuators"); - Boardcore::Logger &sdLogger = Boardcore::Logger::getInstance(); + Boardcore::Logger& sdLogger = Boardcore::Logger::getInstance(); std::atomic<bool> started{false}; @@ -108,4 +108,4 @@ private: bool canVentingOpen = false; }; -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/AlgoReference/AlgoReference.cpp b/src/Main/AlgoReference/AlgoReference.cpp index b132f62bbdb8dc844be6a5e10a1165395f0ec2c4..903d3e0eff197f00f6901eddfeb02e8f276f1541 100644 --- a/src/Main/AlgoReference/AlgoReference.cpp +++ b/src/Main/AlgoReference/AlgoReference.cpp @@ -77,4 +77,4 @@ ReferenceValues AlgoReference::getReferenceValues() { Lock<FastMutex> lock{referenceMutex}; return reference; -} \ No newline at end of file +} diff --git a/src/Main/AlgoReference/AlgoReference.h b/src/Main/AlgoReference/AlgoReference.h index 1942f9bd2c301f191b07954e9a505b5ea2e97eff..b53649c4744d907030ec3f1f2b06b6d094cc1150 100644 --- a/src/Main/AlgoReference/AlgoReference.h +++ b/src/Main/AlgoReference/AlgoReference.h @@ -47,4 +47,4 @@ private: Boardcore::ReferenceValues reference; }; -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/BoardScheduler.h b/src/Main/BoardScheduler.h index 1403f23d3a38c07a16e9488f7ddd798ff45569ca..524fd8f1ccefbec827f4c0e417cc6e57a105185b 100644 --- a/src/Main/BoardScheduler.h +++ b/src/Main/BoardScheduler.h @@ -72,27 +72,27 @@ public: bool isStarted() { return started; } - Boardcore::TaskScheduler &getNasScheduler() { return nas; } + Boardcore::TaskScheduler& getNasScheduler() { return nas; } - Boardcore::TaskScheduler &getMeaScheduler() { return nas; } + Boardcore::TaskScheduler& getMeaScheduler() { return nas; } - Boardcore::TaskScheduler &getAbkScheduler() { return nas; } + Boardcore::TaskScheduler& getAbkScheduler() { return nas; } - Boardcore::TaskScheduler &getAdaScheduler() { return ada; } + Boardcore::TaskScheduler& getAdaScheduler() { return ada; } - Boardcore::TaskScheduler &getSensorsScheduler() { return sensors; } + Boardcore::TaskScheduler& getSensorsScheduler() { return sensors; } - Boardcore::TaskScheduler &getPinObserverScheduler() + Boardcore::TaskScheduler& getPinObserverScheduler() { // TODO(davide.mor): Does this make sense? return sensors; } - Boardcore::TaskScheduler &getRadioScheduler() { return others; } + Boardcore::TaskScheduler& getRadioScheduler() { return others; } - Boardcore::TaskScheduler &getCanBusScheduler() { return others; } + Boardcore::TaskScheduler& getCanBusScheduler() { return others; } - Boardcore::TaskScheduler &getLowPriorityActuatorsScheduler() + Boardcore::TaskScheduler& getLowPriorityActuatorsScheduler() { return others; } @@ -109,4 +109,4 @@ private: Boardcore::TaskScheduler others; }; -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/Buses.h b/src/Main/Buses.h index bed733f3ed3e41f5bb69f351b6d0c96c57322093..76d1843b83d2625fce8e82ea9233ee3db7647990 100644 --- a/src/Main/Buses.h +++ b/src/Main/Buses.h @@ -36,18 +36,18 @@ class Buses : public Boardcore::Injectable public: Buses() {} - Boardcore::SPIBus &getH3LIS331DL() { return spi1; } - Boardcore::SPIBus &getLPS22DF() { return spi1; } - Boardcore::SPIBus &getLIS2MDL() { return spi3; } - Boardcore::SPIBus &getLSM6DSRX() { return spi3; } - Boardcore::SPIBus &getVN100() { return spi1; } - Boardcore::SPIBus &getUBXGps() { return spi3; } - Boardcore::SPIBus &getADS131M08() { return spi4; } - Boardcore::SPIBus &getRadio() { return spi6; } + Boardcore::SPIBus& getH3LIS331DL() { return spi1; } + Boardcore::SPIBus& getLPS22DF() { return spi1; } + Boardcore::SPIBus& getLIS2MDL() { return spi3; } + Boardcore::SPIBus& getLSM6DSRX() { return spi3; } + Boardcore::SPIBus& getVN100() { return spi1; } + Boardcore::SPIBus& getUBXGps() { return spi3; } + Boardcore::SPIBus& getADS131M08() { return spi4; } + Boardcore::SPIBus& getRadio() { return spi6; } - Boardcore::USART &getHILUart() { return usart4; } + Boardcore::USART& getHILUart() { return usart4; } - Boardcore::I2C &getLPS28DFW() { return i2c1; } + Boardcore::I2C& getLPS28DFW() { return i2c1; } private: Boardcore::SPIBus spi1{SPI1}; @@ -61,4 +61,4 @@ private: miosix::interfaces::i2c1::sda::getPin()}; }; -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/CanHandler/CanHandler.cpp b/src/Main/CanHandler/CanHandler.cpp index cb8546f71eba8ccc52bc55eab770e40f9d2703eb..dccecff947c75969bdef38f54279c015e53ef5cb 100644 --- a/src/Main/CanHandler/CanHandler.cpp +++ b/src/Main/CanHandler/CanHandler.cpp @@ -35,7 +35,7 @@ using namespace Common; CanHandler::CanHandler() : driver{CAN1, CanConfig::CONFIG, CanConfig::BIT_TIMING}, - protocol{&driver, [this](const CanMessage &msg) { handleMessage(msg); }, + protocol{&driver, [this](const CanMessage& msg) { handleMessage(msg); }, Config::Scheduler::OTHERS_PRIORITY} { protocol.addFilter(static_cast<uint8_t>(CanConfig::Board::PAYLOAD), @@ -52,7 +52,7 @@ bool CanHandler::start() { driver.init(); - TaskScheduler &scheduler = + TaskScheduler& scheduler = getModule<BoardScheduler>()->getCanBusScheduler(); uint8_t result = scheduler.addTask( @@ -132,7 +132,7 @@ CanHandler::CanStatus CanHandler::getCanStatus() return status; } -void CanHandler::handleMessage(const Canbus::CanMessage &msg) +void CanHandler::handleMessage(const Canbus::CanMessage& msg) { CanConfig::PrimaryType type = static_cast<CanConfig::PrimaryType>(msg.getPrimaryType()); @@ -171,7 +171,7 @@ void CanHandler::handleMessage(const Canbus::CanMessage &msg) } } -void CanHandler::handleEvent(const Canbus::CanMessage &msg) +void CanHandler::handleEvent(const Canbus::CanMessage& msg) { sdLogger.log(CanEvent{TimestampTimer::getTimestamp(), msg.getSource(), msg.getDestination(), msg.getSecondaryType()}); @@ -188,12 +188,12 @@ void CanHandler::handleEvent(const Canbus::CanMessage &msg) } } -void CanHandler::handleSensor(const Canbus::CanMessage &msg) +void CanHandler::handleSensor(const Canbus::CanMessage& msg) { CanConfig::SensorId sensor = static_cast<CanConfig::SensorId>(msg.getSecondaryType()); - Sensors *sensors = getModule<Sensors>(); + Sensors* sensors = getModule<Sensors>(); switch (sensor) { case CanConfig::SensorId::PITOT_DYNAMIC_PRESSURE: @@ -259,7 +259,7 @@ void CanHandler::handleSensor(const Canbus::CanMessage &msg) } } -void CanHandler::handleActuator(const Canbus::CanMessage &msg) +void CanHandler::handleActuator(const Canbus::CanMessage& msg) { ServosList servo = static_cast<ServosList>(msg.getSecondaryType()); CanServoFeedback data = servoFeedbackFromCanMessage(msg); @@ -268,7 +268,7 @@ void CanHandler::handleActuator(const Canbus::CanMessage &msg) getModule<Actuators>()->setCanServoOpen(servo, data.open); } -void CanHandler::handleStatus(const Canbus::CanMessage &msg) +void CanHandler::handleStatus(const Canbus::CanMessage& msg) { CanConfig::Board source = static_cast<CanConfig::Board>(msg.getSource()); CanDeviceStatus deviceStatus = deviceStatusFromCanMessage(msg); @@ -309,4 +309,4 @@ void CanHandler::handleStatus(const Canbus::CanMessage &msg) LOG_WARN(logger, "Received unsupported status: {}", source); } } -} \ No newline at end of file +} diff --git a/src/Main/CanHandler/CanHandler.h b/src/Main/CanHandler/CanHandler.h index b8ddac87dcec5b23b718224282e6e949fa2d20d2..1ea17ea7c31752d7ca620ff205cda0736bac3b63 100644 --- a/src/Main/CanHandler/CanHandler.h +++ b/src/Main/CanHandler/CanHandler.h @@ -103,14 +103,14 @@ public: CanStatus getCanStatus(); private: - void handleMessage(const Boardcore::Canbus::CanMessage &msg); - void handleEvent(const Boardcore::Canbus::CanMessage &msg); - void handleSensor(const Boardcore::Canbus::CanMessage &msg); - void handleActuator(const Boardcore::Canbus::CanMessage &msg); - void handleStatus(const Boardcore::Canbus::CanMessage &msg); + void handleMessage(const Boardcore::Canbus::CanMessage& msg); + void handleEvent(const Boardcore::Canbus::CanMessage& msg); + void handleSensor(const Boardcore::Canbus::CanMessage& msg); + void handleActuator(const Boardcore::Canbus::CanMessage& msg); + void handleStatus(const Boardcore::Canbus::CanMessage& msg); Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("canhandler"); - Boardcore::Logger &sdLogger = Boardcore::Logger::getInstance(); + Boardcore::Logger& sdLogger = Boardcore::Logger::getInstance(); std::atomic<bool> started{false}; @@ -121,4 +121,4 @@ private: CanStatus status; }; -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/Configs/ABKConfig.h b/src/Main/Configs/ABKConfig.h index a128619eb54b7bdb75ed297c5d2b06a24283dc44..e6f39ebdfec7708beb4ec471954e5c7ed5042e21 100644 --- a/src/Main/Configs/ABKConfig.h +++ b/src/Main/Configs/ABKConfig.h @@ -77,4 +77,4 @@ static const Boardcore::AirBrakesInterpConfig CONFIG = { } // namespace Config -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/Configs/ADAConfig.h b/src/Main/Configs/ADAConfig.h index 1f5b58aceb01be5316e7ffcaa1610063d2a6c884..121095565318ef5b95744726f4ac3aba68349f98 100644 --- a/src/Main/Configs/ADAConfig.h +++ b/src/Main/Configs/ADAConfig.h @@ -52,4 +52,4 @@ constexpr unsigned int DEPLOYMENT_N_SAMPLES = 5; } // namespace Config -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/Configs/ActuatorsConfig.h b/src/Main/Configs/ActuatorsConfig.h index f2754beea8d7751176c42ee7b476747ae475dba6..d33d99c228674a11bd929a68fda2af6928e5876d 100644 --- a/src/Main/Configs/ActuatorsConfig.h +++ b/src/Main/Configs/ActuatorsConfig.h @@ -55,4 +55,4 @@ constexpr uint32_t STATUS_ERR_RATE = 1; // 1 * 100ms = 100ms } // namespace Actuators } // namespace Config -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/Configs/FMMConfig.h b/src/Main/Configs/FMMConfig.h index 04ce8d7498190d5a1d8eb976a1fdd98241bd41e6..deebe435455ca0b797feec3adc9850cfc856362e 100644 --- a/src/Main/Configs/FMMConfig.h +++ b/src/Main/Configs/FMMConfig.h @@ -44,4 +44,4 @@ constexpr unsigned int CUT_DURATION = 500; // [ms] } // namespace Config -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/Configs/HILSimulationConfig.h b/src/Main/Configs/HILSimulationConfig.h index 8fa66fcf31088bdf8f6cd70763664d641f076172..0077eb1626b52241c1e52a9713b9a96b4adb430c 100644 --- a/src/Main/Configs/HILSimulationConfig.h +++ b/src/Main/Configs/HILSimulationConfig.h @@ -80,4 +80,4 @@ static_assert(N_DATA_PITOT * SIMULATION_RATE >= BARO_PITOT_RATE, } // namespace HIL } // namespace Config -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/Configs/MEAConfig.h b/src/Main/Configs/MEAConfig.h index d42c6edc63f8fe90c1a557ac0d4d855b76d34e3b..ec88c7a8e079aad412260cda05a433b633fe7fc5 100644 --- a/src/Main/Configs/MEAConfig.h +++ b/src/Main/Configs/MEAConfig.h @@ -75,4 +75,4 @@ constexpr Boardcore::Aeroutils::AerodynamicCoeff AERO_COEFF = { } // namespace Config -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/Configs/NASConfig.h b/src/Main/Configs/NASConfig.h index 82e5ab80f0a68a74fdbb3d59b28afc6494911aee..7ea2940bb41cd256dc07710957053df9c59845bb 100644 --- a/src/Main/Configs/NASConfig.h +++ b/src/Main/Configs/NASConfig.h @@ -86,4 +86,4 @@ constexpr float PITOT_SPEED_THRESHOLD = 70; // [m/s] } // namespace Config -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/Configs/PinHandlerConfig.h b/src/Main/Configs/PinHandlerConfig.h index 3e30bec16d65e5d9d8152db022adde5a7de67d0f..445e85055926fd96db918c55a9bc18f806450acb 100644 --- a/src/Main/Configs/PinHandlerConfig.h +++ b/src/Main/Configs/PinHandlerConfig.h @@ -49,4 +49,4 @@ constexpr uint32_t CUTTER_SENSE_PIN_THRESHOLD = 20; } // namespace Config -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/Configs/RadioConfig.h b/src/Main/Configs/RadioConfig.h index 34fe287ffeb0e7312f615336b5582eaeaed95824..ecc0f7632d15580f3fb6a55f446afd7887e81414 100644 --- a/src/Main/Configs/RadioConfig.h +++ b/src/Main/Configs/RadioConfig.h @@ -54,4 +54,4 @@ constexpr Hertz HIGH_RATE_TELEMETRY = 4_hz; } // namespace Config -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/Configs/ReferenceConfig.h b/src/Main/Configs/ReferenceConfig.h index b79656ed93f2067911ddebc4567fb92b6b6470ea..00e108a8014c63a62b4a7d1df93a643d900725d3 100644 --- a/src/Main/Configs/ReferenceConfig.h +++ b/src/Main/Configs/ReferenceConfig.h @@ -38,4 +38,4 @@ constexpr unsigned int CALIBRATION_SLEEP_TIME = 100; // [ms] } // namespace Config -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/Configs/SchedulerConfig.h b/src/Main/Configs/SchedulerConfig.h index 1d28559e1fcadbba7cf37ead818450c818cd48d0..8c46222465032d536f6f0c70e7d67195ac3d127a 100644 --- a/src/Main/Configs/SchedulerConfig.h +++ b/src/Main/Configs/SchedulerConfig.h @@ -56,4 +56,4 @@ static const miosix::Priority FMM_PRIORITY = miosix::PRIORITY_MAX - 1; } // namespace Config -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/Configs/SensorsConfig.h b/src/Main/Configs/SensorsConfig.h index 823d88167feabf06bb0f36d21c53c17bfc2ff723..aa5d84f390cea39b77764e25df399892448aa326 100644 --- a/src/Main/Configs/SensorsConfig.h +++ b/src/Main/Configs/SensorsConfig.h @@ -189,4 +189,4 @@ constexpr bool USE_PORT_2 = false; } // namespace Config -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/Data/ABKTrajectorySet.h b/src/Main/Data/ABKTrajectorySet.h index 760b7c3732293b242305b6ac46ffc367320852ae..d43aa100ddb46045c4fa0e1a9307bbe753e03de5 100644 --- a/src/Main/Data/ABKTrajectorySet.h +++ b/src/Main/Data/ABKTrajectorySet.h @@ -40,4 +40,4 @@ extern Boardcore::TrajectorySet OPEN_TRAJECTORY_SET; } // namespace Data -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/HIL/HIL.cpp b/src/Main/HIL/HIL.cpp index df612388640501e79055094cd3887cf154a81e6c..04e501dd53d7ebb5e6433208052a6f3768f9826b 100644 --- a/src/Main/HIL/HIL.cpp +++ b/src/Main/HIL/HIL.cpp @@ -301,4 +301,4 @@ ActuatorData MainHIL::updateActuatorData() actuatorsStateHIL}; } -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/HIL/HIL.h b/src/Main/HIL/HIL.h index bb13a25605f67aface55aa81e324c6ef96f029e9..22ccb5b33290264b5cc3ecf4919cca7083483ee8 100644 --- a/src/Main/HIL/HIL.h +++ b/src/Main/HIL/HIL.h @@ -80,4 +80,4 @@ private: ActuatorData updateActuatorData(); }; -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/HIL/HILData.h b/src/Main/HIL/HILData.h index 8aa67209ca8a102afaedeefabf787f3ffd623ac2..f4e0b785400b5feb079db539f8291c76344e4418 100644 --- a/src/Main/HIL/HILData.h +++ b/src/Main/HIL/HILData.h @@ -337,4 +337,4 @@ struct ActuatorData } }; -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/PersistentVars/PersistentVars.cpp b/src/Main/PersistentVars/PersistentVars.cpp index 2ac4c53d915bc97a6607ebcf3a375931836168ba..383721de31c16588faa61d09de61f7de93774d99 100644 --- a/src/Main/PersistentVars/PersistentVars.cpp +++ b/src/Main/PersistentVars/PersistentVars.cpp @@ -45,4 +45,4 @@ bool getHilMode() { return hilMode; } } // namespace PersistentVars -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/PersistentVars/PersistentVars.h b/src/Main/PersistentVars/PersistentVars.h index d533078bf4f74a3a4cd1331acb02cc4efcc97866..4975ee7f12b2fd07b079021bd18350592ede91fe 100644 --- a/src/Main/PersistentVars/PersistentVars.h +++ b/src/Main/PersistentVars/PersistentVars.h @@ -34,4 +34,4 @@ bool getHilMode(); } // namespace PersistentVars -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/PinHandler/PinData.h b/src/Main/PinHandler/PinData.h index 418febb15b45cf95a3123035b25a6eeb46a70845..eb3f0b1bfb4822ece63118a3c00ba2bca1a7a140 100644 --- a/src/Main/PinHandler/PinData.h +++ b/src/Main/PinHandler/PinData.h @@ -47,4 +47,4 @@ struct PinChangeData } }; -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/PinHandler/PinHandler.cpp b/src/Main/PinHandler/PinHandler.cpp index 93bb005cc0e0d6558ccfe849b6b9b93e018212cc..999d7785aa4a940a173d68107d55c867d37be1f7 100644 --- a/src/Main/PinHandler/PinHandler.cpp +++ b/src/Main/PinHandler/PinHandler.cpp @@ -38,7 +38,7 @@ bool PinHandler::isStarted() { return started; } bool PinHandler::start() { - TaskScheduler &scheduler = + TaskScheduler& scheduler = getModule<BoardScheduler>()->getPinObserverScheduler(); pinObserver = std::make_unique<PinObserver>(scheduler, 20); @@ -49,26 +49,22 @@ bool PinHandler::start() Config::PinHandler::RAMP_PIN_THRESHOLD); pinObserver->registerPinCallback( - sense::detachMain::getPin(), - [this](PinTransition transition) + sense::detachMain::getPin(), [this](PinTransition transition) { onDetachMainTransition(transition); }, Config::PinHandler::MAIN_DETACH_PIN_THRESHOLD); pinObserver->registerPinCallback( - sense::detachPayload::getPin(), - [this](PinTransition transition) + sense::detachPayload::getPin(), [this](PinTransition transition) { onDetachPayloadTransition(transition); }, Config::PinHandler::PAYLOAD_DETACH_PIN_THRESHOLD); pinObserver->registerPinCallback( - sense::expulsionSense::getPin(), - [this](PinTransition transition) + sense::expulsionSense::getPin(), [this](PinTransition transition) { onExpulsionSenseTransition(transition); }, Config::PinHandler::EXPULSION_SENSE_PIN_THRESHOLD); pinObserver->registerPinCallback( - sense::cutterSense::getPin(), - [this](PinTransition transition) + sense::cutterSense::getPin(), [this](PinTransition transition) { onCutterSenseTransition(transition); }, Config::PinHandler::CUTTER_SENSE_PIN_THRESHOLD); diff --git a/src/Main/PinHandler/PinHandler.h b/src/Main/PinHandler/PinHandler.h index 913884d8381a142a0a9d2ba3d29a9798e1808c4e..d74959958e0ed45c3c04d75b189a094fd59ce543 100644 --- a/src/Main/PinHandler/PinHandler.h +++ b/src/Main/PinHandler/PinHandler.h @@ -68,4 +68,4 @@ private: std::unique_ptr<Boardcore::PinObserver> pinObserver; }; -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/Radio/Radio.cpp b/src/Main/Radio/Radio.cpp index 7d58d8f5be15ded9bb52be822f0c5555bdb5f559..fe7cbd2d8d358300c136761944238bb2e2cddd00 100644 --- a/src/Main/Radio/Radio.cpp +++ b/src/Main/Radio/Radio.cpp @@ -43,9 +43,7 @@ void handleDioIRQ() { SX1278Fsk* instance = gRadio; if (instance) - { instance->handleDioIRQ(); - } } void setIRQRadio(SX1278Fsk* radio) @@ -86,10 +84,8 @@ bool Radio::start() // Initialize mavdriver mavDriver = std::make_unique<MavDriver>( - radio.get(), - [this](MavDriver*, const mavlink_message_t& msg) - { handleMessage(msg); }, - Config::Radio::MAV_SLEEP_AFTER_SEND, + radio.get(), [this](MavDriver*, const mavlink_message_t& msg) + { handleMessage(msg); }, Config::Radio::MAV_SLEEP_AFTER_SEND, Config::Radio::MAV_OUT_BUFFER_MAX_AGE); if (!mavDriver->start()) @@ -188,7 +184,6 @@ void Radio::handleMessage(const mavlink_message_t& msg) { switch (msg.msgid) { - case MAVLINK_MSG_ID_PING_TC: { enqueueAck(msg); @@ -205,13 +200,9 @@ void Radio::handleMessage(const mavlink_message_t& msg) { uint8_t tmId = mavlink_msg_system_tm_request_tc_get_tm_id(&msg); if (enqueueSystemTm(tmId)) - { enqueueAck(msg); - } else - { enqueueNack(msg, 0); - } break; } @@ -221,13 +212,9 @@ void Radio::handleMessage(const mavlink_message_t& msg) uint8_t tmId = mavlink_msg_sensor_tm_request_tc_get_sensor_name(&msg); if (enqueueSensorsTm(tmId)) - { enqueueAck(msg); - } else - { enqueueNack(msg, 0); - } break; } @@ -313,13 +300,9 @@ void Radio::handleMessage(const mavlink_message_t& msg) getModule<NASController>()->setOrientation(quat.normalized()); if (std::abs(qNorm - 1) > 0.001) - { enqueueWack(msg, 0); - } else - { enqueueAck(msg); - } } else { @@ -351,13 +334,9 @@ void Radio::handleMessage(const mavlink_message_t& msg) TMTC_SET_CALIBRATION_PRESSURE, TOPIC_TMTC); if (press < 50000) - { enqueueWack(msg, 0); - } else - { enqueueAck(msg); - } } } else @@ -406,13 +385,9 @@ void Radio::handleCommand(const mavlink_message_t& msg) FlightModeManagerState::TEST_MODE) { if (getModule<Sensors>()->saveMagCalibration()) - { enqueueAck(msg); - } else - { enqueueNack(msg, 0); - } } else { @@ -1352,4 +1327,4 @@ bool Radio::enqueueSensorsTm(uint8_t tmId) default: return false; } -} \ No newline at end of file +} diff --git a/src/Main/Radio/Radio.h b/src/Main/Radio/Radio.h index 33e89244e86cdb2e9180fac16cc014037ab3623b..60ac57b61aee45c64fc4b58be13a3d405c04fbbd 100644 --- a/src/Main/Radio/Radio.h +++ b/src/Main/Radio/Radio.h @@ -87,4 +87,4 @@ private: std::unique_ptr<MavDriver> mavDriver; }; -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/Sensors/HILSensors.h b/src/Main/Sensors/HILSensors.h index 09fa66912253303dd6a062c0ca261c728437c89e..87e74fec5caebf6bb0077e9b562acb76578685a7 100644 --- a/src/Main/Sensors/HILSensors.h +++ b/src/Main/Sensors/HILSensors.h @@ -313,4 +313,4 @@ private: bool enableHw; }; -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/Sensors/Sensors.cpp b/src/Main/Sensors/Sensors.cpp index 096eef5bb85100dcbcc5c32cd51fe613a3169afe..d8dd731aa2059794cd9e3a55f5c4c4aacae34eb5 100644 --- a/src/Main/Sensors/Sensors.cpp +++ b/src/Main/Sensors/Sensors.cpp @@ -39,39 +39,25 @@ bool Sensors::start() magCalibration.fromFile(Config::Sensors::MAG_CALIBRATION_FILENAME); if (Config::Sensors::LPS22DF::ENABLED) - { lps22dfInit(); - } if (Config::Sensors::LPS28DFW::ENABLED) - { lps28dfwInit(); - } if (Config::Sensors::H3LIS331DL::ENABLED) - { h3lis331dlInit(); - } if (Config::Sensors::LIS2MDL::ENABLED) - { lis2mdlInit(); - } if (Config::Sensors::UBXGPS::ENABLED) - { ubxgpsInit(); - } if (Config::Sensors::LSM6DSRX::ENABLED) - { lsm6dsrxInit(); - } if (Config::Sensors::VN100::ENABLED) - { vn100Init(); - } if (Config::Sensors::ADS131M08::ENABLED) { @@ -82,14 +68,10 @@ bool Sensors::start() } if (Config::Sensors::InternalADC::ENABLED) - { internalAdcInit(); - } if (Config::Sensors::IMU::ENABLED) - { rotatedImuInit(); - } if (!postSensorCreationHook()) { @@ -382,13 +364,9 @@ IMUData Sensors::getIMULastSample() PressureData Sensors::getAtmosPressureLastSample() { if (Config::Sensors::Atmos::USE_PORT_2) - { return getStaticPressure2LastSample(); - } else - { return getStaticPressure1LastSample(); - } } PressureData Sensors::getCanPitotDynamicPressLastSample() @@ -648,9 +626,7 @@ void Sensors::lsm6dsrxCallback() // For every instance inside the fifo log the sample for (uint16_t i = 0; i < lsm6dsrx->getLastFifoSize(); i++) - { sdLogger.log(lsm6dsrx->getLastFifo().at(i)); - } } void Sensors::vn100Init() diff --git a/src/Main/Sensors/Sensors.h b/src/Main/Sensors/Sensors.h index 9a1beeaea930c168dcdf74651d13131bde1a66b2..92850391f6e6a1ac800740e8b53358ada8aec6a0 100644 --- a/src/Main/Sensors/Sensors.h +++ b/src/Main/Sensors/Sensors.h @@ -209,4 +209,4 @@ private: std::atomic<bool> started{false}; }; -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/Sensors/SensorsData.h b/src/Main/Sensors/SensorsData.h index d44f20cae9c2a44b4f8ad4f36e920740d0ae6e33..628e83912f7fe0805212a05680630b27a0af787e 100644 --- a/src/Main/Sensors/SensorsData.h +++ b/src/Main/Sensors/SensorsData.h @@ -29,7 +29,7 @@ namespace Main struct StaticPressureData1 : Boardcore::PressureData { - explicit StaticPressureData1(const Boardcore::PressureData &data) + explicit StaticPressureData1(const Boardcore::PressureData& data) : Boardcore::PressureData(data) { } @@ -39,7 +39,7 @@ struct StaticPressureData1 : Boardcore::PressureData struct StaticPressureData2 : Boardcore::PressureData { - explicit StaticPressureData2(const Boardcore::PressureData &data) + explicit StaticPressureData2(const Boardcore::PressureData& data) : Boardcore::PressureData(data) { } @@ -49,7 +49,7 @@ struct StaticPressureData2 : Boardcore::PressureData struct DplBayPressureData : Boardcore::PressureData { - explicit DplBayPressureData(const Boardcore::PressureData &data) + explicit DplBayPressureData(const Boardcore::PressureData& data) : Boardcore::PressureData(data) { } @@ -84,7 +84,7 @@ struct CalibrationData "dplBayPressBias,dplBayPressScale\n"; } - void print(std::ostream &os) const + void print(std::ostream& os) const { os << timestamp << "," << gyroBiasX << "," << gyroBiasY << "," << gyroBiasZ << "," << magBiasX << "," << magBiasY << "," << magBiasZ @@ -95,4 +95,4 @@ struct CalibrationData } }; -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/StateMachines/ABKController/ABKController.cpp b/src/Main/StateMachines/ABKController/ABKController.cpp index a5103c943ac403416ece1f552fab2f24c122dab5..085527243c396d6eea6b8b848c22d69b17450996 100644 --- a/src/Main/StateMachines/ABKController/ABKController.cpp +++ b/src/Main/StateMachines/ABKController/ABKController.cpp @@ -42,8 +42,7 @@ ABKController::ABKController() getModule<NASController>()->getNASState()}; }, Data::ABK::OPEN_TRAJECTORY_SET, Data::ABK::CLOSED_TRAJECTORY_SET, - Config::ABK::CONFIG, - [this](float position) + Config::ABK::CONFIG, [this](float position) { getModule<Actuators>()->setAbkPosition(position); }} { @@ -229,4 +228,4 @@ void ABKController::updateAndLogStatus(ABKControllerState state) this->state = state; ABKControllerStatus data = {TimestampTimer::getTimestamp(), state}; sdLogger.log(data); -} \ No newline at end of file +} diff --git a/src/Main/StateMachines/ABKController/ABKController.h b/src/Main/StateMachines/ABKController/ABKController.h index 949244d0f42d91b3eb004d0026b8653e9798166c..e815a6944ae731456cc3a03916ba7139b0a9e6b7 100644 --- a/src/Main/StateMachines/ABKController/ABKController.h +++ b/src/Main/StateMachines/ABKController/ABKController.h @@ -69,4 +69,4 @@ private: Boardcore::AirBrakesInterp abk; }; -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/StateMachines/ABKController/ABKControllerData.h b/src/Main/StateMachines/ABKController/ABKControllerData.h index c4d17d0b21ddc16589f6e72085a5c107476f6c7b..e5d9d13d41152adbf82ff1259a4c8905fb005d09 100644 --- a/src/Main/StateMachines/ABKController/ABKControllerData.h +++ b/src/Main/StateMachines/ABKController/ABKControllerData.h @@ -52,4 +52,4 @@ struct ABKControllerStatus } }; -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/StateMachines/ADAController/ADAController.cpp b/src/Main/StateMachines/ADAController/ADAController.cpp index 107180354748dfadfb3bff864dbc706648a4819c..aa7bed2201e36584cdb55ad085c0a5eaef78d41b 100644 --- a/src/Main/StateMachines/ADAController/ADAController.cpp +++ b/src/Main/StateMachines/ADAController/ADAController.cpp @@ -187,13 +187,9 @@ void ADAController::update() if (curState == ADAControllerState::ACTIVE_DROGUE_DESCENT) { if (ada.getState().aglAltitude < getDeploymentAltitude()) - { detectedDeployments++; - } else - { detectedDeployments = 0; - } if (detectedDeployments > Config::ADA::DEPLOYMENT_N_SAMPLES) { @@ -461,4 +457,4 @@ void ADAController::updateAndLogStatus(ADAControllerState state) this->state = state; ADAControllerStatus data = {TimestampTimer::getTimestamp(), state}; sdLogger.log(data); -} \ No newline at end of file +} diff --git a/src/Main/StateMachines/ADAController/ADAController.h b/src/Main/StateMachines/ADAController/ADAController.h index b95fffaecbfd3cfa5bdffa94ebf3084b7f1f8a4e..004dbebc4b58dd5ad4094f36301f710d30aa46bd 100644 --- a/src/Main/StateMachines/ADAController/ADAController.h +++ b/src/Main/StateMachines/ADAController/ADAController.h @@ -81,4 +81,4 @@ private: unsigned int detectedDeployments = 0; }; -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/StateMachines/ADAController/ADAControllerData.h b/src/Main/StateMachines/ADAController/ADAControllerData.h index 94f1a64745333f107b14521bedbab94671da29fc..2d2483d9486deeb15747233a7ae5ca180305f0fc 100644 --- a/src/Main/StateMachines/ADAController/ADAControllerData.h +++ b/src/Main/StateMachines/ADAController/ADAControllerData.h @@ -74,4 +74,4 @@ struct ADAControllerSampleData } }; -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/Main/StateMachines/FlightModeManager/FlightModeManager.cpp index 19289554d7ebcc150a90ad991dd2b2be843ca0c0..285c16e63f3d01857bb8add942c0e432fb0317fd 100644 --- a/src/Main/StateMachines/FlightModeManager/FlightModeManager.cpp +++ b/src/Main/StateMachines/FlightModeManager/FlightModeManager.cpp @@ -297,25 +297,17 @@ State FlightModeManager::state_calibrate_algorithms(const Event& event) { nasReady = true; if (adaReady) - { return transition(&FlightModeManager::state_disarmed); - } else - { return HANDLED; - } } case ADA_READY: { adaReady = true; if (nasReady) - { return transition(&FlightModeManager::state_disarmed); - } else - { return HANDLED; - } } default: { @@ -797,4 +789,4 @@ void FlightModeManager::updateAndLogStatus(FlightModeManagerState state) FlightModeManagerStatus data = {TimestampTimer::getTimestamp(), state}; sdLogger.log(data); -} \ No newline at end of file +} diff --git a/src/Main/StateMachines/FlightModeManager/FlightModeManager.h b/src/Main/StateMachines/FlightModeManager/FlightModeManager.h index ef2ff49ee5556a9bb45e212475097ae91d56cf22..af8361e3648af16234ed5e14731b1436807eccd6 100644 --- a/src/Main/StateMachines/FlightModeManager/FlightModeManager.h +++ b/src/Main/StateMachines/FlightModeManager/FlightModeManager.h @@ -81,4 +81,4 @@ private: FlightModeManagerState::ON_GROUND}; }; -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/StateMachines/FlightModeManager/FlightModeManagerData.h b/src/Main/StateMachines/FlightModeManager/FlightModeManagerData.h index 89857161c99db862eddd9e874c294c068c64b9ab..d6218b63db547505905cd5dc4ba2ce587d51a78b 100644 --- a/src/Main/StateMachines/FlightModeManager/FlightModeManagerData.h +++ b/src/Main/StateMachines/FlightModeManager/FlightModeManagerData.h @@ -58,4 +58,4 @@ struct FlightModeManagerStatus } }; -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/StateMachines/MEAController/MEAController.cpp b/src/Main/StateMachines/MEAController/MEAController.cpp index 527e80cf1b2b2d7b9975b02ac99adbd7512e8124..cc463d7fbf5696f965df960c270f7237ed094e36 100644 --- a/src/Main/StateMachines/MEAController/MEAController.cpp +++ b/src/Main/StateMachines/MEAController/MEAController.cpp @@ -155,19 +155,13 @@ void MEAController::update() MEA::Step step{aperture}; if (baro.pressureTimestamp > lastBaroTimestamp) - { step.withCCPressure(baro); - } if (imu.accelerationTimestamp > lastAccTimestamp) - { step.withAcceleration(imu); - } if (nas.timestamp > lastNasTimestamp) - { step.withSpeedAndAlt(-nas.vd, mslAltitude); - } mea.update(step); @@ -390,4 +384,4 @@ void MEAController::updateAndLogStatus(MEAControllerState state) this->state = state; MEAControllerStatus data = {TimestampTimer::getTimestamp(), state}; sdLogger.log(data); -} \ No newline at end of file +} diff --git a/src/Main/StateMachines/MEAController/MEAController.h b/src/Main/StateMachines/MEAController/MEAController.h index 7e116b9a317131af42fe1d22d7bc894044d0a62f..f07cbbc519d9aaa50ade532f19080c28cbbec775 100644 --- a/src/Main/StateMachines/MEAController/MEAController.h +++ b/src/Main/StateMachines/MEAController/MEAController.h @@ -83,4 +83,4 @@ private: unsigned int detectedShutdowns = 0; }; -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/StateMachines/MEAController/MEAControllerData.h b/src/Main/StateMachines/MEAController/MEAControllerData.h index b60e8e1ffcd9b6fa0d1f9e3b53b72026e5e494ce..6a0811caf0425ef15d99129dcf6afe8c9da24e44 100644 --- a/src/Main/StateMachines/MEAController/MEAControllerData.h +++ b/src/Main/StateMachines/MEAController/MEAControllerData.h @@ -53,4 +53,4 @@ struct MEAControllerStatus } }; -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/StateMachines/NASController/NASController.cpp b/src/Main/StateMachines/NASController/NASController.cpp index 1cf296d05eb8cb101bd62486c85af8a3981e0a8f..476dc4b6fbdef45482313859ee0ed2f66fe231ca 100644 --- a/src/Main/StateMachines/NASController/NASController.cpp +++ b/src/Main/StateMachines/NASController/NASController.cpp @@ -154,9 +154,7 @@ void NASController::update() } if (lastBaroTimestamp < baro.pressureTimestamp) - { nas.correctBaro(baro.pressure); - } // Correct with pitot if one pressure sample is new // Disable pitot correction @@ -171,9 +169,7 @@ void NASController::update() // Correct with accelerometer if the acceleration is in specs if (lastAccTimestamp < imu.accelerationTimestamp && acc1g) - { nas.correctAcc(imu); - } // Check if the accelerometer is measuring 1g if (accLength < @@ -182,13 +178,9 @@ void NASController::update() (Constants::g - Config::NAS::ACCELERATION_1G_CONFIDENCE / 2)) { if (acc1gSamplesCount < Config::NAS::ACCELERATION_1G_SAMPLES) - { acc1gSamplesCount++; - } else - { acc1g = true; - } } else { @@ -352,4 +344,4 @@ void NASController::updateAndLogStatus(NASControllerState state) this->state = state; NASControllerStatus data = {TimestampTimer::getTimestamp(), state}; sdLogger.log(data); -} \ No newline at end of file +} diff --git a/src/Main/StateMachines/NASController/NASController.h b/src/Main/StateMachines/NASController/NASController.h index 2a3f42e323e2a463fc763d0edc869e80ad658b25..b559d1565a2bd7904ced7b8bdf9f5cd814934c1a 100644 --- a/src/Main/StateMachines/NASController/NASController.h +++ b/src/Main/StateMachines/NASController/NASController.h @@ -89,4 +89,4 @@ private: uint64_t dynamicPitotTimestamp = 0; }; -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/StateMachines/NASController/NASControllerData.h b/src/Main/StateMachines/NASController/NASControllerData.h index 32f6c97e981766830c9d819e56ec19a48ee2ebaa..b87aa50941884fcb2820690949af4f02a928548a 100644 --- a/src/Main/StateMachines/NASController/NASControllerData.h +++ b/src/Main/StateMachines/NASController/NASControllerData.h @@ -51,4 +51,4 @@ struct NASControllerStatus } }; -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/StatsRecorder/StatsRecorder.cpp b/src/Main/StatsRecorder/StatsRecorder.cpp index 2c3573d1d1432f37f0496439b31fcf0382fe2b22..3c661fe372022b9fa24035d56472817c7cc73b7e 100644 --- a/src/Main/StatsRecorder/StatsRecorder.cpp +++ b/src/Main/StatsRecorder/StatsRecorder.cpp @@ -73,7 +73,7 @@ void StatsRecorder::deploymentDetected(uint64_t ts, float alt) stats.dplAlt = alt; } -void StatsRecorder::updateAcc(const AccelerometerData &data) +void StatsRecorder::updateAcc(const AccelerometerData& data) { auto state = getModule<FlightModeManager>()->getState(); @@ -110,7 +110,7 @@ void StatsRecorder::updateAcc(const AccelerometerData &data) } } -void StatsRecorder::updateNas(const NASState &data) +void StatsRecorder::updateNas(const NASState& data) { auto state = getModule<FlightModeManager>()->getState(); @@ -144,7 +144,7 @@ void StatsRecorder::updateNas(const NASState &data) } } -void StatsRecorder::updateDplPressure(const PressureData &data) +void StatsRecorder::updateDplPressure(const PressureData& data) { auto state = getModule<FlightModeManager>()->getState(); @@ -159,4 +159,4 @@ void StatsRecorder::updateDplPressure(const PressureData &data) stats.maxDplPressureTs = data.pressureTimestamp; } } -} \ No newline at end of file +} diff --git a/src/Main/StatsRecorder/StatsRecorder.h b/src/Main/StatsRecorder/StatsRecorder.h index cf18a91009139a4fb2ebef8ed8a10e8d68f42973..dd9a04193c9939f468333cbaf0d560d04be9203c 100644 --- a/src/Main/StatsRecorder/StatsRecorder.h +++ b/src/Main/StatsRecorder/StatsRecorder.h @@ -92,13 +92,13 @@ public: void apogeeDetected(uint64_t ts, float lat, float lon, float alt); void deploymentDetected(uint64_t ts, float alt); - void updateAcc(const Boardcore::AccelerometerData &data); - void updateNas(const Boardcore::NASState &data); - void updateDplPressure(const Boardcore::PressureData &data); + void updateAcc(const Boardcore::AccelerometerData& data); + void updateNas(const Boardcore::NASState& data); + void updateDplPressure(const Boardcore::PressureData& data); private: miosix::FastMutex statsMutex; Stats stats; }; -} // namespace Main \ No newline at end of file +} // namespace Main diff --git a/src/Main/main-entry.cpp b/src/Main/main-entry.cpp index 606176673e0daa6cf329794d9dfeb3279a9b0c35..29dbf54788fd06ca5613b0c894e1835daa7a6343 100644 --- a/src/Main/main-entry.cpp +++ b/src/Main/main-entry.cpp @@ -63,22 +63,22 @@ int main() bool initResult = true; - Buses *buses = new Buses(); - BoardScheduler *scheduler = new BoardScheduler(); - - Sensors *sensors; - Actuators *actuators = new Actuators(); - Radio *radio = new Radio(); - CanHandler *canHandler = new CanHandler(); - PinHandler *pinHandler = new PinHandler(); - FlightModeManager *fmm = new FlightModeManager(); - AlgoReference *ref = new AlgoReference(); - ADAController *ada = new ADAController(); - NASController *nas = new NASController(); - MEAController *mea = new MEAController(); - ABKController *abk = new ABKController(); - StatsRecorder *recorder = new StatsRecorder(); - MainHIL *hil = nullptr; + Buses* buses = new Buses(); + BoardScheduler* scheduler = new BoardScheduler(); + + Sensors* sensors; + Actuators* actuators = new Actuators(); + Radio* radio = new Radio(); + CanHandler* canHandler = new CanHandler(); + PinHandler* pinHandler = new PinHandler(); + FlightModeManager* fmm = new FlightModeManager(); + AlgoReference* ref = new AlgoReference(); + ADAController* ada = new ADAController(); + NASController* nas = new NASController(); + MEAController* mea = new MEAController(); + ABKController* abk = new ABKController(); + StatsRecorder* recorder = new StatsRecorder(); + MainHIL* hil = nullptr; // HIL if (PersistentVars::getHilMode()) @@ -96,17 +96,17 @@ int main() sensors = new Sensors(); } - Logger &sdLogger = Logger::getInstance(); - EventBroker &broker = EventBroker::getInstance(); + Logger& sdLogger = Logger::getInstance(); + EventBroker& broker = EventBroker::getInstance(); // Setup event sniffer - EventSniffer sniffer( - broker, - [&](uint8_t event, uint8_t topic) - { - EventData data{TimestampTimer::getTimestamp(), event, topic}; - sdLogger.log(data); - }); + EventSniffer sniffer(broker, + [&](uint8_t event, uint8_t topic) + { + EventData data{TimestampTimer::getTimestamp(), + event, topic}; + sdLogger.log(data); + }); // Insert modules initResult = initResult && manager.insert<Buses>(buses) && @@ -258,15 +258,11 @@ int main() { std::cout << "LIFTOFF!" << std::endl; if (Config::HIL::IS_FULL_HIL) - { canHandler->sendServoOpenCommand( ServosList::MAIN_VALVE, 7000); - } else - { actuators->setCanServoOpen( ServosList::MAIN_VALVE, true); - } }); std::cout << "Waiting start simulation" << std::endl; @@ -317,4 +313,4 @@ int main() } return 0; -} \ No newline at end of file +} diff --git a/src/Motor/Actuators/Actuators.cpp b/src/Motor/Actuators/Actuators.cpp index f866834af04b41fe776646d76e50f49121849dd6..8582d035f283338c4c430993bcfff76c04cfc1a1 100644 --- a/src/Motor/Actuators/Actuators.cpp +++ b/src/Motor/Actuators/Actuators.cpp @@ -48,15 +48,11 @@ void Actuators::ServoInfo::unsafeSetServoPosition(float position) { // Check that the servo is actually there, just to be safe if (!servo) - { return; - } position *= limit; if (flipped) - { position = 1.0f - position; - } servo->setPosition(position); } @@ -65,15 +61,11 @@ float Actuators::ServoInfo::getServoPosition() { // Check that the servo is actually there, just to be safe if (!servo) - { return 0.0f; - } float position = servo->getPosition(); if (flipped) - { position = 1.0f - position; - } position /= limit; return position; @@ -90,7 +82,7 @@ Actuators::Actuators() Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE, Config::Servos::FREQUENCY); - ServoInfo *info; + ServoInfo* info; info = getServo(ServosList::MAIN_VALVE); info->limit = Config::Servos::MAIN_LIMIT; info->flipped = Config::Servos::MAIN_FLIPPED; @@ -104,7 +96,7 @@ Actuators::Actuators() bool Actuators::start() { - TaskScheduler &scheduler = + TaskScheduler& scheduler = getModule<BoardScheduler>()->getActuatorsScheduler(); infos[0].servo->enable(); @@ -128,11 +120,9 @@ bool Actuators::start() bool Actuators::openServoWithTime(ServosList servo, uint32_t time) { Lock<FastMutex> lock(infosMutex); - ServoInfo *info = getServo(servo); + ServoInfo* info = getServo(servo); if (info == nullptr) - { return false; - } lastActionTs = getTime(); info->openServoWithTime(time); @@ -142,11 +132,9 @@ bool Actuators::openServoWithTime(ServosList servo, uint32_t time) bool Actuators::closeServo(ServosList servo) { Lock<FastMutex> lock(infosMutex); - ServoInfo *info = getServo(servo); + ServoInfo* info = getServo(servo); if (info == nullptr) - { return false; - } lastActionTs = getTime(); info->closeServo(); @@ -156,11 +144,9 @@ bool Actuators::closeServo(ServosList servo) float Actuators::getServoPosition(ServosList servo) { Lock<FastMutex> lock(infosMutex); - ServoInfo *info = getServo(servo); + ServoInfo* info = getServo(servo); if (info == nullptr) - { return 0.0f; - } return info->getServoPosition(); } @@ -168,16 +154,14 @@ float Actuators::getServoPosition(ServosList servo) bool Actuators::isServoOpen(ServosList servo) { Lock<FastMutex> lock(infosMutex); - ServoInfo *info = getServo(servo); + ServoInfo* info = getServo(servo); if (info == nullptr) - { return false; - } return info->closeTs != 0; } -Actuators::ServoInfo *Actuators::getServo(ServosList servo) +Actuators::ServoInfo* Actuators::getServo(ServosList servo) { switch (servo) { @@ -270,4 +254,4 @@ void Actuators::updatePositionsTask() openServoWithTime(ServosList::VENTING_VALVE, Config::Servos::SERVO_ACTION_TIMEOUT + 1000); } -} \ No newline at end of file +} diff --git a/src/Motor/Actuators/Actuators.h b/src/Motor/Actuators/Actuators.h index 329d0c5ff70544075ab3a881d083133a64ac2aec..15133ade166bccf168662904efbb00b6062f760a 100644 --- a/src/Motor/Actuators/Actuators.h +++ b/src/Motor/Actuators/Actuators.h @@ -64,13 +64,13 @@ public: float getServoPosition(ServosList servo); private: - ServoInfo *getServo(ServosList servo); + ServoInfo* getServo(ServosList servo); void unsafeSetServoPosition(uint8_t idx, float position); void updatePositionsTask(); - Boardcore::Logger &sdLogger = Boardcore::Logger::getInstance(); + Boardcore::Logger& sdLogger = Boardcore::Logger::getInstance(); Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("actuators"); miosix::FastMutex infosMutex; @@ -79,4 +79,4 @@ private: ServoInfo infos[2] = {}; }; -} // namespace Motor \ No newline at end of file +} // namespace Motor diff --git a/src/Motor/Actuators/ActuatorsData.h b/src/Motor/Actuators/ActuatorsData.h index 64f181d90107a6c6a4e1cd9216d05d83c5778689..a623c9a32fdf4059cdf1a9690cb6c41dde219b0a 100644 --- a/src/Motor/Actuators/ActuatorsData.h +++ b/src/Motor/Actuators/ActuatorsData.h @@ -53,4 +53,4 @@ struct ActuatorsData } }; -} // namespace Motor \ No newline at end of file +} // namespace Motor diff --git a/src/Motor/BoardScheduler.h b/src/Motor/BoardScheduler.h index 9703540da3075a8283bfe6a8e739df7a3141186f..503908b53dd1f4e39725dcba5f9f2bf1cb6fefe6 100644 --- a/src/Motor/BoardScheduler.h +++ b/src/Motor/BoardScheduler.h @@ -55,11 +55,11 @@ public: return true; } - Boardcore::TaskScheduler &getSensorsScheduler() { return sensors; } + Boardcore::TaskScheduler& getSensorsScheduler() { return sensors; } - Boardcore::TaskScheduler &getActuatorsScheduler() { return actuators; } + Boardcore::TaskScheduler& getActuatorsScheduler() { return actuators; } - Boardcore::TaskScheduler &getCanBusScheduler() { return actuators; } + Boardcore::TaskScheduler& getCanBusScheduler() { return actuators; } private: Boardcore::PrintLogger logger = @@ -69,4 +69,4 @@ private: Boardcore::TaskScheduler actuators; }; -} // namespace Motor \ No newline at end of file +} // namespace Motor diff --git a/src/Motor/Buses.h b/src/Motor/Buses.h index 6c9ffb1bb9fd5496bfece7f4680f9b54f196dd89..62a25fde5d8e258c69e59a8467c9da5ea6bc708d 100644 --- a/src/Motor/Buses.h +++ b/src/Motor/Buses.h @@ -34,13 +34,13 @@ class Buses : public Boardcore::Injectable public: Buses() {} - Boardcore::SPIBus &getH3LIS331DL() { return spi1; } - Boardcore::SPIBus &getLPS22DF() { return spi1; } - Boardcore::SPIBus &getLIS2MDL() { return spi3; } - Boardcore::SPIBus &getLSM6DSRX() { return spi3; } - Boardcore::SPIBus &getADS131M08() { return spi4; } + Boardcore::SPIBus& getH3LIS331DL() { return spi1; } + Boardcore::SPIBus& getLPS22DF() { return spi1; } + Boardcore::SPIBus& getLIS2MDL() { return spi3; } + Boardcore::SPIBus& getLSM6DSRX() { return spi3; } + Boardcore::SPIBus& getADS131M08() { return spi4; } - Boardcore::USART &getHILUart() { return usart4; } + Boardcore::USART& getHILUart() { return usart4; } private: Boardcore::SPIBus spi1{SPI1}; @@ -50,4 +50,4 @@ private: Boardcore::USART usart4{UART4, 256000, 1024}; }; -} // namespace Motor \ No newline at end of file +} // namespace Motor diff --git a/src/Motor/CanHandler/CanHandler.cpp b/src/Motor/CanHandler/CanHandler.cpp index b045f0cabee22669f159fa7c1ed12d4937a24de5..a110d856d9fba927476b02dc9d4f289f422efc22 100644 --- a/src/Motor/CanHandler/CanHandler.cpp +++ b/src/Motor/CanHandler/CanHandler.cpp @@ -37,7 +37,7 @@ using namespace Common; CanHandler::CanHandler() : driver(CAN1, CanConfig::CONFIG, CanConfig::BIT_TIMING), protocol( - &driver, [this](const CanMessage &msg) { handleMessage(msg); }, + &driver, [this](const CanMessage& msg) { handleMessage(msg); }, Config::Scheduler::CAN_PRIORITY) { protocol.addFilter(static_cast<uint8_t>(CanConfig::Board::RIG), @@ -52,7 +52,7 @@ bool CanHandler::start() { driver.init(); - TaskScheduler &scheduler = + TaskScheduler& scheduler = getModule<BoardScheduler>()->getCanBusScheduler(); uint8_t result = scheduler.addTask( @@ -85,7 +85,7 @@ bool CanHandler::start() result = scheduler.addTask( [this]() { - Sensors *sensors = getModule<Sensors>(); + Sensors* sensors = getModule<Sensors>(); protocol.enqueueData( static_cast<uint8_t>(CanConfig::Priority::HIGH), @@ -124,7 +124,7 @@ bool CanHandler::start() result = scheduler.addTask( [this]() { - Sensors *sensors = getModule<Sensors>(); + Sensors* sensors = getModule<Sensors>(); protocol.enqueueData( static_cast<uint8_t>(CanConfig::Priority::MEDIUM), @@ -154,7 +154,7 @@ bool CanHandler::start() result = scheduler.addTask( [this]() { - Actuators *actuators = getModule<Actuators>(); + Actuators* actuators = getModule<Actuators>(); protocol.enqueueData( static_cast<uint8_t>(CanConfig::Priority::HIGH), @@ -197,7 +197,7 @@ bool CanHandler::start() void CanHandler::setInitStatus(InitStatus status) { initStatus = status; } -void CanHandler::handleMessage(const Canbus::CanMessage &msg) +void CanHandler::handleMessage(const Canbus::CanMessage& msg) { CanConfig::PrimaryType type = static_cast<CanConfig::PrimaryType>(msg.getPrimaryType()); @@ -224,7 +224,7 @@ void CanHandler::handleMessage(const Canbus::CanMessage &msg) } } -void CanHandler::handleEvent(const Canbus::CanMessage &msg) +void CanHandler::handleEvent(const Canbus::CanMessage& msg) { CanConfig::EventId event = static_cast<CanConfig::EventId>(msg.getSecondaryType()); @@ -248,18 +248,14 @@ void CanHandler::handleEvent(const Canbus::CanMessage &msg) msg.getDestination(), msg.getSecondaryType()}); } -void CanHandler::handleCommand(const Canbus::CanMessage &msg) +void CanHandler::handleCommand(const Canbus::CanMessage& msg) { ServosList servo = static_cast<ServosList>(msg.getSecondaryType()); CanServoCommand command = servoCommandFromCanMessage(msg); sdLogger.log(command); if (command.openingTime == 0) - { getModule<Actuators>()->closeServo(servo); - } else - { getModule<Actuators>()->openServoWithTime(servo, command.openingTime); - } -} \ No newline at end of file +} diff --git a/src/Motor/CanHandler/CanHandler.h b/src/Motor/CanHandler/CanHandler.h index b6177b012345b61754af4a16395283a50f0ada3f..c29da6fd311c3a726858d7aca0e03ab3b0b8cf0f 100644 --- a/src/Motor/CanHandler/CanHandler.h +++ b/src/Motor/CanHandler/CanHandler.h @@ -54,11 +54,11 @@ public: void setInitStatus(InitStatus status); private: - void handleMessage(const Boardcore::Canbus::CanMessage &msg); - void handleEvent(const Boardcore::Canbus::CanMessage &msg); - void handleCommand(const Boardcore::Canbus::CanMessage &msg); + void handleMessage(const Boardcore::Canbus::CanMessage& msg); + void handleEvent(const Boardcore::Canbus::CanMessage& msg); + void handleCommand(const Boardcore::Canbus::CanMessage& msg); - Boardcore::Logger &sdLogger = Boardcore::Logger::getInstance(); + Boardcore::Logger& sdLogger = Boardcore::Logger::getInstance(); Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("canhandler"); std::atomic<InitStatus> initStatus{InitStatus::UNKNOWN}; diff --git a/src/Motor/Configs/ActuatorsConfig.h b/src/Motor/Configs/ActuatorsConfig.h index 2fdc0fc3399ed1639ffc7bda287f1422b15018ba..36a876cd7fef7e0a3bfd45096f3f3342319a9b0e 100644 --- a/src/Motor/Configs/ActuatorsConfig.h +++ b/src/Motor/Configs/ActuatorsConfig.h @@ -58,4 +58,4 @@ constexpr bool MAIN_FLIPPED = true; } // namespace Servos } // namespace Config -} // namespace Motor \ No newline at end of file +} // namespace Motor diff --git a/src/Motor/Configs/CanHandlerConfig.h b/src/Motor/Configs/CanHandlerConfig.h index d73da97699e4629fee4329f552dfa882d2ed8e4e..dca3dbc199b9bffe0dccd1f4f6870a8b07baebad 100644 --- a/src/Motor/Configs/CanHandlerConfig.h +++ b/src/Motor/Configs/CanHandlerConfig.h @@ -50,4 +50,4 @@ constexpr Hertz TEMPERATURE_PERIOD = 10_hz; } // namespace Config -} // namespace Motor \ No newline at end of file +} // namespace Motor diff --git a/src/Motor/Configs/SchedulerConfig.h b/src/Motor/Configs/SchedulerConfig.h index d94fecba4534409f8f0bfa4675beea7e089bf220..ec35d17a496a042a344830cfd485c68f9bc63fbd 100644 --- a/src/Motor/Configs/SchedulerConfig.h +++ b/src/Motor/Configs/SchedulerConfig.h @@ -43,4 +43,4 @@ static const miosix::Priority CAN_PRIORITY = miosix::PRIORITY_MAX - 1; } // namespace Config -} // namespace Motor \ No newline at end of file +} // namespace Motor diff --git a/src/Motor/HIL/HIL.cpp b/src/Motor/HIL/HIL.cpp index a613463e75d9d2fdcabfa52133b292b7590040d3..4f5f6ded024e30901093f6d300c3f6bc811bac4e 100644 --- a/src/Motor/HIL/HIL.cpp +++ b/src/Motor/HIL/HIL.cpp @@ -140,4 +140,4 @@ ActuatorData MotorHIL::updateActuatorData() return ActuatorData{actuatorsStateHIL}; } -} // namespace Motor \ No newline at end of file +} // namespace Motor diff --git a/src/Motor/HIL/HIL.h b/src/Motor/HIL/HIL.h index 71aad12f79ef6cf8930189f237be5d101939d153..67569f4d6fd7e01034ebadc7100387650c959b1c 100644 --- a/src/Motor/HIL/HIL.h +++ b/src/Motor/HIL/HIL.h @@ -75,4 +75,4 @@ public: private: ActuatorData updateActuatorData(); }; -} // namespace Motor \ No newline at end of file +} // namespace Motor diff --git a/src/Motor/HIL/HILData.h b/src/Motor/HIL/HILData.h index 126270017458f8935bf5e42bd4a93a156f0cbf44..14051e4528417bad2beada7bcfcb63fd08ad21a9 100644 --- a/src/Motor/HIL/HILData.h +++ b/src/Motor/HIL/HILData.h @@ -99,4 +99,4 @@ struct ActuatorData void print() { actuatorsState.print(); } }; -} // namespace Motor \ No newline at end of file +} // namespace Motor diff --git a/src/Motor/PersistentVars/PersistentVars.cpp b/src/Motor/PersistentVars/PersistentVars.cpp index 91fb0f0abef7b6adb337c682b1e2f114efb9511d..4864e564dc232d94d199a555b393487e555d5ed6 100644 --- a/src/Motor/PersistentVars/PersistentVars.cpp +++ b/src/Motor/PersistentVars/PersistentVars.cpp @@ -45,4 +45,4 @@ bool getHilMode() { return hilMode; } } // namespace PersistentVars -} // namespace Motor \ No newline at end of file +} // namespace Motor diff --git a/src/Motor/PersistentVars/PersistentVars.h b/src/Motor/PersistentVars/PersistentVars.h index 195cfb2cb5391e646cb8cf3c1eb89e5408dfd3c2..ab9c6031845ce88437a7c9768e9aebc5ced305cf 100644 --- a/src/Motor/PersistentVars/PersistentVars.h +++ b/src/Motor/PersistentVars/PersistentVars.h @@ -34,4 +34,4 @@ bool getHilMode(); } // namespace PersistentVars -} // namespace Motor \ No newline at end of file +} // namespace Motor diff --git a/src/Motor/Sensors/HILSensors.h b/src/Motor/Sensors/HILSensors.h index ac16b64824ab94653fd6a40b881d467135ddd423..8e46a0ef16cdbf500d4cd6c8a37414ce622e0fc6 100644 --- a/src/Motor/Sensors/HILSensors.h +++ b/src/Motor/Sensors/HILSensors.h @@ -51,7 +51,7 @@ private: return true; }; - ~HILSensors(){}; + ~HILSensors() {}; int getSampleCounter(int nData) { diff --git a/src/Motor/Sensors/KuliteThermocouple.h b/src/Motor/Sensors/KuliteThermocouple.h index 1be99a5ec0249bf0b8419b5db79022287e557da6..99f241b0ccc7b5dd65ee8723bf49925f80b276d1 100644 --- a/src/Motor/Sensors/KuliteThermocouple.h +++ b/src/Motor/Sensors/KuliteThermocouple.h @@ -36,8 +36,8 @@ public: KuliteThermocouple(std::function<Boardcore::ADCData()> getVoltage, float p0Voltage, float p0Temp, float p1Voltage, float p1Temp) - : getVoltage{getVoltage}, scale{(p1Temp - p0Temp) / - (p1Voltage - p0Voltage)}, + : getVoltage{getVoltage}, + scale{(p1Temp - p0Temp) / (p1Voltage - p0Voltage)}, offset{p0Temp - scale * p0Voltage} { } @@ -61,4 +61,4 @@ private: const float offset; }; -} // namespace Motor \ No newline at end of file +} // namespace Motor diff --git a/src/Motor/Sensors/Sensors.cpp b/src/Motor/Sensors/Sensors.cpp index 3227a04e95cce8d8525e1ec866216d0eb46f1e1c..5d8ed74e473e1d8f42bc734e20e52b1f65541db7 100644 --- a/src/Motor/Sensors/Sensors.cpp +++ b/src/Motor/Sensors/Sensors.cpp @@ -34,24 +34,16 @@ using namespace miosix; bool Sensors::start() { if (Config::Sensors::LPS22DF::ENABLED) - { lps22dfInit(); - } if (Config::Sensors::H3LIS331DL::ENABLED) - { h3lis331dlInit(); - } if (Config::Sensors::LIS2MDL::ENABLED) - { lis2mdlInit(); - } if (Config::Sensors::LSM6DSRX::ENABLED) - { lsm6dsrxInit(); - } if (Config::Sensors::ADS131M08::ENABLED) { @@ -63,9 +55,7 @@ bool Sensors::start() } if (Config::Sensors::InternalADC::ENABLED) - { internalAdcInit(); - } if (!postSensorCreationHook()) { @@ -271,9 +261,7 @@ void Sensors::lsm6dsrxCallback() // For every instance inside the fifo log the sample for (uint16_t i = 0; i < lsm6dsrx->getLastFifoSize(); i++) - { sdLogger.log(lsm6dsrx->getLastFifo().at(i)); - } } void Sensors::ads131m08Init() @@ -499,4 +487,4 @@ bool Sensors::sensorManagerInit() manager = std::make_unique<SensorManager>(map, &getSensorsScheduler()); return manager->start(); -} \ No newline at end of file +} diff --git a/src/Motor/Sensors/Sensors.h b/src/Motor/Sensors/Sensors.h index 64202f5b8fc1fba1d26c9d0405dda3249c47d9f6..ce8b56fd4bd487e79ab297a08b26d6c06ee8a06c 100644 --- a/src/Motor/Sensors/Sensors.h +++ b/src/Motor/Sensors/Sensors.h @@ -122,4 +122,4 @@ private: Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("sensors"); }; -} // namespace Motor \ No newline at end of file +} // namespace Motor diff --git a/src/Motor/Sensors/SensorsData.h b/src/Motor/Sensors/SensorsData.h index c04d97742614928378583910318c9665575893e3..b205266fb42ecaabaff0f7a4ea0ccf4f78683822 100644 --- a/src/Motor/Sensors/SensorsData.h +++ b/src/Motor/Sensors/SensorsData.h @@ -28,7 +28,7 @@ namespace Motor struct TopTankPressureData : Boardcore::PressureData { - explicit TopTankPressureData(const Boardcore::PressureData &data) + explicit TopTankPressureData(const Boardcore::PressureData& data) : Boardcore::PressureData{data} { } @@ -38,7 +38,7 @@ struct TopTankPressureData : Boardcore::PressureData struct BottomTankPressureData : Boardcore::PressureData { - explicit BottomTankPressureData(const Boardcore::PressureData &data) + explicit BottomTankPressureData(const Boardcore::PressureData& data) : Boardcore::PressureData{data} { } @@ -48,7 +48,7 @@ struct BottomTankPressureData : Boardcore::PressureData struct CCPressureData : Boardcore::PressureData { - explicit CCPressureData(const Boardcore::PressureData &data) + explicit CCPressureData(const Boardcore::PressureData& data) : Boardcore::PressureData{data} { } @@ -56,4 +56,4 @@ struct CCPressureData : Boardcore::PressureData CCPressureData() {} }; -} // namespace Motor \ No newline at end of file +} // namespace Motor diff --git a/src/Motor/motor-entry.cpp b/src/Motor/motor-entry.cpp index fd0814bd87b2483bf436ea7953ae48cb8fbe7687..24888615fa0dfbfb684fe90cecd3916e47b63876 100644 --- a/src/Motor/motor-entry.cpp +++ b/src/Motor/motor-entry.cpp @@ -49,19 +49,19 @@ int main() DependencyManager manager; - Buses *buses = new Buses(); - BoardScheduler *scheduler = new BoardScheduler(); + Buses* buses = new Buses(); + BoardScheduler* scheduler = new BoardScheduler(); - Sensors *sensors = + Sensors* sensors = (PersistentVars::getHilMode() ? new HILSensors(Config::HIL::ENABLE_HW) : new Sensors()); - Actuators *actuators = new Actuators(); - CanHandler *canHandler = new CanHandler(); + Actuators* actuators = new Actuators(); + CanHandler* canHandler = new CanHandler(); - Logger &sdLogger = Logger::getInstance(); + Logger& sdLogger = Logger::getInstance(); // HIL - MotorHIL *hil = nullptr; + MotorHIL* hil = nullptr; if (PersistentVars::getHilMode()) { hil = new MotorHIL(); @@ -199,4 +199,4 @@ int main() } return 0; -} \ No newline at end of file +} diff --git a/src/Payload/Actuators/Actuators.cpp b/src/Payload/Actuators/Actuators.cpp index ab210108fa0bada312546a16adb4254fc2b2d1b3..669fe010b4d646117f3f75d76a9241d995bd0f5c 100644 --- a/src/Payload/Actuators/Actuators.cpp +++ b/src/Payload/Actuators/Actuators.cpp @@ -126,9 +126,7 @@ bool Actuators::setServoPosition(ServosList servoId, float position) { auto actuator = getServoActuator(servoId); if (!actuator) - { return false; - } miosix::Lock<miosix::FastMutex> lock(actuator->mutex); @@ -141,9 +139,7 @@ bool Actuators::setServoAngle(ServosList servoId, float angle) { auto actuator = getServoActuator(servoId); if (!actuator) - { return false; - } miosix::Lock<miosix::FastMutex> lock(actuator->mutex); @@ -157,9 +153,7 @@ float Actuators::getServoPosition(ServosList servoId) { auto actuator = getServoActuator(servoId); if (!actuator) - { return -1.f; - } miosix::Lock<miosix::FastMutex> lock(actuator->mutex); @@ -170,9 +164,7 @@ float Actuators::getServoAngle(ServosList servoId) { auto actuator = getServoActuator(servoId); if (!actuator) - { return -1.f; - } miosix::Lock<miosix::FastMutex> lock(actuator->mutex); @@ -183,9 +175,7 @@ bool Actuators::wiggleServo(ServosList servoId) { auto actuator = getServoActuator(servoId); if (!actuator) - { return false; - } miosix::Lock<miosix::FastMutex> lock(actuator->mutex); @@ -200,9 +190,7 @@ bool Actuators::disableServo(ServosList servoId) { auto actuator = getServoActuator(servoId); if (!actuator) - { return false; - } miosix::Lock<miosix::FastMutex> lock(actuator->mutex); @@ -305,13 +293,9 @@ void Actuators::updateStatusLed() } if (statusLedCounter >= statusLedThreshold) - { statusOn(); - } else - { statusOff(); - } if (statusLedCounter >= statusLedThreshold * 2) { diff --git a/src/Payload/AltitudeTrigger/AltitudeTrigger.cpp b/src/Payload/AltitudeTrigger/AltitudeTrigger.cpp index c0ecf485cebf660e4bf5bcfaaac429fb612a307d..d6abb19405a99937c3ac8005e4ede40e1f573c21 100644 --- a/src/Payload/AltitudeTrigger/AltitudeTrigger.cpp +++ b/src/Payload/AltitudeTrigger/AltitudeTrigger.cpp @@ -43,9 +43,7 @@ bool AltitudeTrigger::start() config::AltitudeTrigger::UPDATE_RATE); if (task == 0) - { return false; - } started = true; return true; @@ -56,9 +54,7 @@ bool AltitudeTrigger::isStarted() { return started; } void AltitudeTrigger::enable() { if (running) - { return; - } confidence = 0; running = true; @@ -76,22 +72,16 @@ void AltitudeTrigger::setDeploymentAltitude(float altitude) void AltitudeTrigger::update() { if (!running) - { return; - } // NED frame, flip the D sign to get above-ground-level altitude auto nasState = getModule<NASController>()->getNasState(); float altitude = -nasState.d; if (altitude < targetAltitude) - { confidence++; - } else - { confidence = 0; - } if (confidence >= config::AltitudeTrigger::CONFIDENCE) { diff --git a/src/Payload/CanHandler/CanHandler.cpp b/src/Payload/CanHandler/CanHandler.cpp index 2fad71b8e1fcb4398320bfaa7ed4fd828fd87e1c..ed916b236205c86b410608f4e73ff1fe109023f4 100644 --- a/src/Payload/CanHandler/CanHandler.cpp +++ b/src/Payload/CanHandler/CanHandler.cpp @@ -251,9 +251,7 @@ void CanHandler::handleEvent(const Boardcore::Canbus::CanMessage& msg) auto event = canEventToEvent(canEvent.event); if (event == LAST_EVENT) - { return; - } EventBroker::getInstance().post(event, TOPIC_CAN); } diff --git a/src/Payload/CanHandler/CanHandler.h b/src/Payload/CanHandler/CanHandler.h index fee6448ca5183bcbffad44bae80e89bb0595c3ee..6087184b68c4b0387bee60e8781b5d5e0756398b 100644 --- a/src/Payload/CanHandler/CanHandler.h +++ b/src/Payload/CanHandler/CanHandler.h @@ -77,9 +77,9 @@ public: CanStatus getCanStatus(); private: - void handleMessage(const Boardcore::Canbus::CanMessage &msg); - void handleEvent(const Boardcore::Canbus::CanMessage &msg); - void handleStatus(const Boardcore::Canbus::CanMessage &msg); + void handleMessage(const Boardcore::Canbus::CanMessage& msg); + void handleEvent(const Boardcore::Canbus::CanMessage& msg); + void handleStatus(const Boardcore::Canbus::CanMessage& msg); std::unique_ptr<Boardcore::Canbus::CanbusDriver> driver; std::unique_ptr<Boardcore::Canbus::CanProtocol> protocol; diff --git a/src/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp b/src/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp index 617c3424b44489dbfd8c909f5c3070cd3d37d6c0..496f5eaed74502f0fa23ee12cdcaf00932c076a8 100644 --- a/src/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp +++ b/src/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp @@ -148,13 +148,9 @@ void FlightStatsRecorder::updatePressure(const PressureData& data) { // Record this event only during flight if (stats.minPressure == 0) - { stats.minPressure = data.pressure; - } else if (data.pressure < stats.minPressure) - { stats.minPressure = data.pressure; - } } } diff --git a/src/Payload/HIL/HIL.cpp b/src/Payload/HIL/HIL.cpp index 9505d87623c57332154c7aac3c2548091e3a5052..304f82bd25caf309ef4580a9b69384e1dc36d469 100644 --- a/src/Payload/HIL/HIL.cpp +++ b/src/Payload/HIL/HIL.cpp @@ -281,4 +281,4 @@ ActuatorData PayloadHIL::updateActuatorData() nasStateHIL, actuatorsStateHIL, guidanceData, (fmm->testState(&Payload::FlightModeManager::FlyingAscending) ? 3 : 0)); }; -} // namespace Payload \ No newline at end of file +} // namespace Payload diff --git a/src/Payload/HIL/HIL.h b/src/Payload/HIL/HIL.h index 1275b2985922ce2aaf9611106c74e389ab79b081..ac166a58a5b841d3485a6400a8eea79a9ea965be 100644 --- a/src/Payload/HIL/HIL.h +++ b/src/Payload/HIL/HIL.h @@ -70,7 +70,6 @@ class PayloadHIL Payload::Buses, Payload::Actuators, Payload::FlightModeManager, Payload::WingController, Payload::NASController> { - public: PayloadHIL(); @@ -80,4 +79,4 @@ private: ActuatorData updateActuatorData(); }; -} // namespace Payload \ No newline at end of file +} // namespace Payload diff --git a/src/Payload/HIL/HILData.h b/src/Payload/HIL/HILData.h index fed7c3d1194cb1ec80caade5915eb77d0a2ca8f7..3dd0597783ad688da95fc0b9ed8a71ec564b59eb 100644 --- a/src/Payload/HIL/HILData.h +++ b/src/Payload/HIL/HILData.h @@ -235,4 +235,4 @@ struct ActuatorData guidanceData.print(); } }; -} // namespace Payload \ No newline at end of file +} // namespace Payload diff --git a/src/Payload/PersistentVars/PersistentVars.cpp b/src/Payload/PersistentVars/PersistentVars.cpp index 4d59bae1a81f83076633e0bbde3a211910466e9c..da39e9d35b3420f71eafd1a2a396b74881442883 100644 --- a/src/Payload/PersistentVars/PersistentVars.cpp +++ b/src/Payload/PersistentVars/PersistentVars.cpp @@ -45,4 +45,4 @@ bool getHilMode() { return hilMode; } } // namespace PersistentVars -} // namespace Payload \ No newline at end of file +} // namespace Payload diff --git a/src/Payload/PersistentVars/PersistentVars.h b/src/Payload/PersistentVars/PersistentVars.h index 9f2ed5db5b213fb50d4630b9f1a8765609f04443..c504501465af4f38f3d600c7e85d3e564fb35427 100644 --- a/src/Payload/PersistentVars/PersistentVars.h +++ b/src/Payload/PersistentVars/PersistentVars.h @@ -34,4 +34,4 @@ bool getHilMode(); } // namespace PersistentVars -} // namespace Payload \ No newline at end of file +} // namespace Payload diff --git a/src/Payload/PinHandler/PinData.h b/src/Payload/PinHandler/PinData.h index b4a64ea3d841a301b4db7255f1971cfa6081a651..0f531f19888d56af32cbaee3da231b2aa7c60d5f 100644 --- a/src/Payload/PinHandler/PinData.h +++ b/src/Payload/PinHandler/PinData.h @@ -47,4 +47,4 @@ struct PinChangeData } }; -} // namespace Payload \ No newline at end of file +} // namespace Payload diff --git a/src/Payload/PinHandler/PinHandler.cpp b/src/Payload/PinHandler/PinHandler.cpp index 783101d1251485b01138a3921a01303241e51daf..defeb9f6c5d0b05ca97d455afb802e8f594d49af 100644 --- a/src/Payload/PinHandler/PinHandler.cpp +++ b/src/Payload/PinHandler/PinHandler.cpp @@ -128,9 +128,7 @@ void PinHandler::onNoseconeDetachTransition(PinTransition transition) static_cast<int>(transition)); if (transition == config::NoseconeDetach::TRIGGERING_TRANSITION) - { EventBroker::getInstance().post(FLIGHT_NC_DETACHED, TOPIC_FLIGHT); - } } } // namespace Payload diff --git a/src/Payload/Radio/MessageHandler.cpp b/src/Payload/Radio/MessageHandler.cpp index 5d5e322f1cf1f5c08c1c0bcd1c525c06f5824556..42e9172d053267c0b80b2b2c7aa32f85d18e1b25 100644 --- a/src/Payload/Radio/MessageHandler.cpp +++ b/src/Payload/Radio/MessageHandler.cpp @@ -65,9 +65,7 @@ void Radio::MavlinkBackend::handleMessage(const mavlink_message_t& msg) mavlink_msg_system_tm_request_tc_get_tm_id(&msg)); if (!enqueueSystemTm(tmId)) - { return enqueueNack(msg); - } return enqueueAck(msg); } @@ -78,9 +76,7 @@ void Radio::MavlinkBackend::handleMessage(const mavlink_message_t& msg) mavlink_msg_sensor_tm_request_tc_get_sensor_name(&msg)); if (!enqueueSensorsTm(sensorId)) - { return enqueueNack(msg); - } return enqueueAck(msg); } @@ -93,9 +89,7 @@ void Radio::MavlinkBackend::handleMessage(const mavlink_message_t& msg) float position = parent.getModule<Actuators>()->getServoPosition(servo); if (position < 0) - { return enqueueNack(msg); - } mavlink_message_t tmMsg; mavlink_servo_tm_t tm; @@ -116,22 +110,16 @@ void Radio::MavlinkBackend::handleMessage(const mavlink_message_t& msg) parent.getModule<FlightModeManager>()->servoMovesAllowed(); // Allow arbitrary servo movements in allowed states only if (!allowed) - { return enqueueNack(msg); - } auto servo = static_cast<ServosList>( mavlink_msg_set_servo_angle_tc_get_servo_id(&msg)); float angle = mavlink_msg_set_servo_angle_tc_get_angle(&msg); if (parent.getModule<Actuators>()->setServoAngle(servo, angle)) - { return enqueueAck(msg); - } else - { return enqueueNack(msg); - } } case MAVLINK_MSG_ID_RESET_SERVO_TC: @@ -140,9 +128,7 @@ void Radio::MavlinkBackend::handleMessage(const mavlink_message_t& msg) parent.getModule<FlightModeManager>()->servoMovesAllowed(); // Reset servos in allowed states only if (!allowed) - { return enqueueNack(msg); - } auto servo = static_cast<ServosList>( mavlink_msg_reset_servo_tc_get_servo_id(&msg)); @@ -166,9 +152,7 @@ void Radio::MavlinkBackend::handleMessage(const mavlink_message_t& msg) parent.getModule<FlightModeManager>()->servoMovesAllowed(); // Perform the wiggle in allowed states only if (!allowed) - { return enqueueNack(msg); - } auto servo = static_cast<ServosList>( mavlink_msg_wiggle_servo_tc_get_servo_id(&msg)); @@ -206,13 +190,9 @@ void Radio::MavlinkBackend::handleMessage(const mavlink_message_t& msg) float qNorm = quat.norm(); if (std::abs(qNorm - 1) > 0.001) - { return enqueueWack(msg); - } else - { return enqueueAck(msg); - } } case MAVLINK_MSG_ID_SET_DEPLOYMENT_ALTITUDE_TC: @@ -224,13 +204,9 @@ void Radio::MavlinkBackend::handleMessage(const mavlink_message_t& msg) altitude); if (altitude < 200 || altitude > 450) - { return enqueueWack(msg); - } else - { return enqueueAck(msg); - } } case MAVLINK_MSG_ID_SET_TARGET_COORDINATES_TC: @@ -245,13 +221,9 @@ void Radio::MavlinkBackend::handleMessage(const mavlink_message_t& msg) latitude, longitude); if (targetSet) - { return enqueueAck(msg); - } else - { return enqueueNack(msg); - } } case MAVLINK_MSG_ID_SET_ALGORITHM_TC: @@ -263,13 +235,9 @@ void Radio::MavlinkBackend::handleMessage(const mavlink_message_t& msg) parent.getModule<WingController>()->selectAlgorithm(index); if (algorithmSet) - { return enqueueAck(msg); - } else - { return enqueueNack(msg); - } } case MAVLINK_MSG_ID_SET_CALIBRATION_PRESSURE_TC: @@ -297,13 +265,9 @@ void Radio::MavlinkBackend::handleMessage(const mavlink_message_t& msg) TOPIC_TMTC); if (press < 50000) - { return enqueueWack(msg); - } else - { return enqueueAck(msg); - } } } @@ -315,9 +279,7 @@ void Radio::MavlinkBackend::handleMessage(const mavlink_message_t& msg) bool testMode = parent.getModule<FlightModeManager>()->isTestMode(); // Raw events are allowed in test mode only if (!testMode) - { return enqueueNack(msg); - } EventBroker::getInstance().post(topicId, eventId); return enqueueAck(msg); @@ -341,9 +303,7 @@ void Radio::MavlinkBackend::handleCommand(const mavlink_message_t& msg) { bool started = Logger::getInstance().start(); if (!started) - { return enqueueNack(msg); - } Logger::getInstance().resetStats(); return enqueueAck(msg); @@ -360,19 +320,13 @@ void Radio::MavlinkBackend::handleCommand(const mavlink_message_t& msg) bool testMode = parent.getModule<FlightModeManager>()->isTestMode(); // Save calibration data in test mode only if (!testMode) - { return enqueueNack(msg); - } bool magResult = parent.getModule<Sensors>()->saveMagCalibration(); if (magResult) - { return enqueueAck(msg); - } else - { return enqueueNack(msg); - } } default: @@ -380,9 +334,7 @@ void Radio::MavlinkBackend::handleCommand(const mavlink_message_t& msg) // Map the command to an event and post it, if it exists auto event = mavCmdToEvent(command); if (event == LAST_EVENT) - { return enqueueNack(msg); - } EventBroker::getInstance().post(event, TOPIC_TMTC); return enqueueAck(msg); diff --git a/src/Payload/Radio/Radio.cpp b/src/Payload/Radio/Radio.cpp index f82d188700426991d636561e3364b06fe30f061d..46c12a4645ef050821b3ce999956ac0998e38ab9 100644 --- a/src/Payload/Radio/Radio.cpp +++ b/src/Payload/Radio/Radio.cpp @@ -41,9 +41,7 @@ inline void handleDioIRQ() { auto transceiver = staticTransceiver.load(); if (transceiver) - { transceiver->handleDioIRQ(); - } } } // namespace @@ -86,8 +84,7 @@ bool Radio::start() // Initialize the Mavlink driver radioMavlink.driver = std::make_unique<MavDriver>( - transceiver.get(), - [this](MavDriver*, const mavlink_message_t& msg) + transceiver.get(), [this](MavDriver*, const mavlink_message_t& msg) { handleRadioMessage(msg); }, milliseconds{config::MavlinkDriver::SLEEP_AFTER_SEND}.count(), milliseconds{config::MavlinkDriver::MAX_PKT_AGE}.count()); @@ -194,9 +191,7 @@ void Radio::MavlinkBackend::flushQueue() Lock<FastMutex> lock(mutex); for (uint32_t i = 0; i < index; i++) - { driver->enqueueMsg(queue[i]); - } // Reset the index index = 0; diff --git a/src/Payload/Sensors/HILSensors.h b/src/Payload/Sensors/HILSensors.h index b2d39e8c8b202bf3866c9c7dac3fa692fc9440a3..20a45a80403db770035742b4bc75720770d2e915 100644 --- a/src/Payload/Sensors/HILSensors.h +++ b/src/Payload/Sensors/HILSensors.h @@ -261,4 +261,4 @@ private: bool enableHw; }; -} // namespace Payload \ No newline at end of file +} // namespace Payload diff --git a/src/Payload/Sensors/Sensors.cpp b/src/Payload/Sensors/Sensors.cpp index 96e5694b9525708204447b72352a2c9945e5eb0b..c763eaddb25b73be5078c7454b9d0bf21facdf49 100644 --- a/src/Payload/Sensors/Sensors.cpp +++ b/src/Payload/Sensors/Sensors.cpp @@ -42,34 +42,22 @@ bool Sensors::start() magCalibration.fromFile(config::MagCalibration::CALIBRATION_PATH); if (Config::Sensors::LPS22DF::ENABLED) - { lps22dfInit(); - } if (Config::Sensors::LPS28DFW::ENABLED) - { lps28dfwInit(); - } if (Config::Sensors::H3LIS331DL::ENABLED) - { h3lis331dlInit(); - } if (Config::Sensors::LIS2MDL::ENABLED) - { lis2mdlInit(); - } if (Config::Sensors::UBXGPS::ENABLED) - { ubxgpsInit(); - } if (Config::Sensors::LSM6DSRX::ENABLED) - { lsm6dsrxInit(); - } if (Config::Sensors::ADS131M08::ENABLED) { @@ -79,14 +67,10 @@ bool Sensors::start() } if (Config::Sensors::InternalADC::ENABLED) - { internalAdcInit(); - } if (Config::Sensors::RotatedIMU::ENABLED) - { rotatedImuInit(); - } // Return immediately if the hook fails as we cannot know what the hook does if (!postSensorCreationHook()) @@ -394,7 +378,7 @@ std::vector<SensorInfo> Sensors::getSensorInfo() infos.push_back(manager->getSensorInfo(instance.get())); \ else \ infos.push_back( \ - SensorInfo{#name, config::name::SAMPLING_RATE, nullptr, false}) + SensorInfo { #name, config::name::SAMPLING_RATE, nullptr, false }) PUSH_SENSOR_INFO(lps22df, LPS22DF); PUSH_SENSOR_INFO(lps28dfw, LPS28DFW); @@ -563,9 +547,7 @@ void Sensors::lsm6dsrxCallback() // For every instance inside the fifo log the sample for (auto i = 0; i < fifoSize; i++) - { logger.log(fifo.at(i)); - } } void Sensors::ads131m08Init() @@ -582,9 +564,7 @@ void Sensors::ads131m08Init() // Disable all channels for (auto& channel : channels) - { channel.enabled = false; - } // Enable required channels channels[(int)config::StaticPressure::ADC_CH] = { .enabled = true, diff --git a/src/Payload/StateMachines/NASController/NASController.cpp b/src/Payload/StateMachines/NASController/NASController.cpp index 0a6f5cf0ac51de5246a4f0d14b9fd70c8df57c9f..959bff61491b73b9e2d6f85f91fc914b6b65038c 100644 --- a/src/Payload/StateMachines/NASController/NASController.cpp +++ b/src/Payload/StateMachines/NASController/NASController.cpp @@ -252,7 +252,7 @@ void NASController::calibrate() ReferenceValues reference = nas.getReferenceValues(); reference.refPressure = meanBaro; reference.refAltitude = Aeroutils::relAltitude( - reference.refPressure, reference.mslPressure, reference.mslTemperature); + reference.refPressure, reference.mslPressure, reference.mslTemperature); // Also update the reference with the GPS if we have fix UBXGPSData gps = sensors->getUBXGPSLastSample(); @@ -274,9 +274,7 @@ void NASController::update() { // Update the NAS state only if the FSM is active if (state != NASControllerState::ACTIVE) - { return; - } auto* sensors = getModule<Sensors>(); @@ -316,9 +314,7 @@ void NASController::update() } if (lastBaroTimestamp < baro.pressureTimestamp) - { nas.correctBaro(baro.pressure); - } // Correct with pitot if one pressure sample is new // Disable pitot correction @@ -333,9 +329,7 @@ void NASController::update() // Correct with accelerometer if the acceleration is in specs if (lastAccTimestamp < imu.accelerationTimestamp && acc1g) - { nas.correctAcc(imu); - } // Check if the accelerometer is measuring 1g if (accLength < @@ -344,13 +338,9 @@ void NASController::update() (Constants::g - Config::NAS::ACCELERATION_1G_CONFIDENCE / 2)) { if (acc1gSamplesCount < Config::NAS::ACCELERATION_1G_SAMPLES) - { acc1gSamplesCount++; - } else - { acc1g = true; - } } else { diff --git a/src/Payload/StateMachines/WingController/WingController.cpp b/src/Payload/StateMachines/WingController/WingController.cpp index 5530bac87666701d26ceabbf5284e548decd01d4..66fd9ebf3ba7463b76fd2fc1f8b7ea9afcc3e1fa 100644 --- a/src/Payload/StateMachines/WingController/WingController.cpp +++ b/src/Payload/StateMachines/WingController/WingController.cpp @@ -284,9 +284,7 @@ State WingController::OnGround(const Boardcore::Event& event) void WingController::inject(DependencyInjector& injector) { for (auto& algorithm : algorithms) - { algorithm->inject(injector); - } Super::inject(injector); } @@ -317,9 +315,7 @@ bool WingController::start() { // Do not update the active target if the wing is not flying if (!running) - { return; - } auto nasState = getModule<NASController>()->getNasState(); float altitude = -nasState.d; @@ -356,9 +352,7 @@ bool WingController::setTargetCoordinates(float latitude, float longitude) { // Allow changing the target position in the IDLE state only if (state != WingControllerState::IDLE) - { return false; - } targetPositionGEO = Coordinates{latitude, longitude}; @@ -391,9 +385,7 @@ bool WingController::selectAlgorithm(uint8_t index) { // Allow changing the algorithm in the IDLE state only if (state != WingControllerState::IDLE) - { return false; - } switch (index) { @@ -519,7 +511,7 @@ void WingController::updateEarlyManeuverPoints() Vector2f currentPositionNED = {nasState.n, nasState.e}; Vector2f targetNED = Aeroutils::geodetic2NED( - targetGEO, {nasRef.refLatitude, nasRef.refLongitude}); + targetGEO, {nasRef.refLatitude, nasRef.refLongitude}); Vector2f targetOffsetNED = targetNED - currentPositionNED; Vector2f normPoint = targetOffsetNED / targetOffsetNED.norm(); @@ -570,9 +562,7 @@ void WingController::updateEarlyManeuverPoints() void WingController::update() { if (running) - { getCurrentAlgorithm().step(); - } } void WingController::flareWing() diff --git a/src/Payload/Wing/AutomaticWingAlgorithm.cpp b/src/Payload/Wing/AutomaticWingAlgorithm.cpp index 544ba3f9cd0c9d31e403ea094c49d27e69587343..f26d010d3fb5397048f0109fb62052ee0daf9c04 100644 --- a/src/Payload/Wing/AutomaticWingAlgorithm.cpp +++ b/src/Payload/Wing/AutomaticWingAlgorithm.cpp @@ -155,9 +155,7 @@ float AutomaticWingAlgorithm::angleDiff(float a, float b) // diff = fmod(diff, 2 * Constants::PI); if (diff == 0 && positiveInput) - { diff = 2 * Constants::PI; - } diff -= Constants::PI; } diff --git a/src/Payload/Wing/FileWingAlgorithm.cpp b/src/Payload/Wing/FileWingAlgorithm.cpp index 7d4f8e7a18e9191b5fd55fa962ebb9febd8e31ff..9c412996fc19b3f0110df12d075f032492e53bc4 100644 --- a/src/Payload/Wing/FileWingAlgorithm.cpp +++ b/src/Payload/Wing/FileWingAlgorithm.cpp @@ -55,9 +55,7 @@ bool FileWingAlgorithm::init() // Communicate it via serial if (fileValid) - { LOG_INFO(logger, "File valid"); - } // Close the file parser.close(); diff --git a/src/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp b/src/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp index e8ac40537e6c7bfd28d50bc5cd6a684d962133b6..a30a15872a6c1548c76fbe2dfa55d8f191d59b9e 100644 --- a/src/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp +++ b/src/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp @@ -37,9 +37,9 @@ namespace Payload EarlyManeuversGuidanceAlgorithm::EarlyManeuversGuidanceAlgorithm() : activeTarget(Target::EMC), targetAltitudeConfidence(0), m2AltitudeConfidence(0), m1AltitudeConfidence(0), - emcAltitudeConfidence(0){}; + emcAltitudeConfidence(0) {}; -EarlyManeuversGuidanceAlgorithm::~EarlyManeuversGuidanceAlgorithm(){}; +EarlyManeuversGuidanceAlgorithm::~EarlyManeuversGuidanceAlgorithm() {}; float EarlyManeuversGuidanceAlgorithm::calculateTargetAngle( const Eigen::Vector3f& currentPositionNED, Eigen::Vector2f& heading) diff --git a/src/Payload/payload-entry.cpp b/src/Payload/payload-entry.cpp index efbf2d2529cdac6bf1281245b346d3b51911db4c..a2160a7abf826f046417bfad198ea0429229a741 100644 --- a/src/Payload/payload-entry.cpp +++ b/src/Payload/payload-entry.cpp @@ -202,13 +202,13 @@ int main() // Log all posted events std::cout << "Starting event sniffer" << std::endl; - EventSniffer sniffer( - EventBroker::getInstance(), TOPICS_LIST, - [](uint8_t event, uint8_t topic) - { - EventData ev{TimestampTimer::getTimestamp(), event, topic}; - Logger::getInstance().log(ev); - }); + EventSniffer sniffer(EventBroker::getInstance(), TOPICS_LIST, + [](uint8_t event, uint8_t topic) + { + EventData ev{TimestampTimer::getTimestamp(), event, + topic}; + Logger::getInstance().log(ev); + }); if (hil) { diff --git a/src/RIGv2/Actuators/Actuators.cpp b/src/RIGv2/Actuators/Actuators.cpp index eb744975e378d7ed46b92d443ef7e1b0ef9df178..26e29428228639febb8bb89cd8f357a0b0207b54 100644 --- a/src/RIGv2/Actuators/Actuators.cpp +++ b/src/RIGv2/Actuators/Actuators.cpp @@ -41,9 +41,7 @@ void Actuators::ServoInfo::openServoWithTime(uint32_t time) lastActionTs = currentTime; if (openingEvent != 0) - { EventBroker::getInstance().post(openingEvent, TOPIC_MOTOR); - } } void Actuators::ServoInfo::closeServo() @@ -52,24 +50,18 @@ void Actuators::ServoInfo::closeServo() lastActionTs = getTime(); if (closingEvent != 0) - { EventBroker::getInstance().post(closingEvent, TOPIC_MOTOR); - } } void Actuators::ServoInfo::unsafeSetServoPosition(float position) { // Check that the servo is actually there, just to be safe if (!servo) - { return; - } position *= limit; if (flipped) - { position = 1.0f - position; - } servo->setPosition(position); } @@ -78,15 +70,11 @@ float Actuators::ServoInfo::getServoPosition() { // Check that the servo is actually there, just to be safe if (!servo) - { return 0.0f; - } float position = servo->getPosition(); if (flipped) - { position = 1.0f - position; - } position /= limit; return position; @@ -170,7 +158,7 @@ Actuators::Actuators() Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE, Config::Servos::FREQUENCY); - ServoInfo *info; + ServoInfo* info; info = getServo(ServosList::FILLING_VALVE); info->defaultMaxAperture = Config::Servos::DEFAULT_FILLING_MAX_APERTURE; info->defaultOpeningTime = Config::Servos::DEFAULT_FILLING_OPENING_TIME; @@ -230,7 +218,7 @@ bool Actuators::isStarted() { return started; } bool Actuators::start() { - TaskScheduler &scheduler = + TaskScheduler& scheduler = getModule<BoardScheduler>()->getActuatorsScheduler(); infos[0].servo->enable(); @@ -273,11 +261,9 @@ bool Actuators::wiggleServo(ServosList servo) bool Actuators::toggleServo(ServosList servo) { Lock<FastMutex> lock(infosMutex); - ServoInfo *info = getServo(servo); + ServoInfo* info = getServo(servo); if (info == nullptr) - { return false; - } if (info->closeTs == 0) { @@ -300,11 +286,9 @@ bool Actuators::toggleServo(ServosList servo) bool Actuators::openServo(ServosList servo) { Lock<FastMutex> lock(infosMutex); - ServoInfo *info = getServo(servo); + ServoInfo* info = getServo(servo); if (info == nullptr) - { return false; - } uint32_t time = info->getOpeningTime(); getModule<CanHandler>()->sendServoOpenCommand(servo, time); @@ -316,11 +300,9 @@ bool Actuators::openServo(ServosList servo) bool Actuators::openServoWithTime(ServosList servo, uint32_t time) { Lock<FastMutex> lock(infosMutex); - ServoInfo *info = getServo(servo); + ServoInfo* info = getServo(servo); if (info == nullptr) - { return false; - } getModule<CanHandler>()->sendServoOpenCommand(servo, time); info->openServoWithTime(time); @@ -330,11 +312,9 @@ bool Actuators::openServoWithTime(ServosList servo, uint32_t time) bool Actuators::closeServo(ServosList servo) { Lock<FastMutex> lock(infosMutex); - ServoInfo *info = getServo(servo); + ServoInfo* info = getServo(servo); if (info == nullptr) - { return false; - } getModule<CanHandler>()->sendServoCloseCommand(servo); info->closeServo(); @@ -345,9 +325,7 @@ void Actuators::closeAllServos() { Lock<FastMutex> lock(infosMutex); for (uint8_t idx = 0; idx < 10; idx++) - { infos[idx].closeServo(); - } getModule<CanHandler>()->sendServoCloseCommand(ServosList::MAIN_VALVE); getModule<CanHandler>()->sendServoCloseCommand(ServosList::VENTING_VALVE); @@ -356,11 +334,9 @@ void Actuators::closeAllServos() bool Actuators::setMaxAperture(ServosList servo, float aperture) { Lock<FastMutex> lock(infosMutex); - ServoInfo *info = getServo(servo); + ServoInfo* info = getServo(servo); if (info == nullptr) - { return false; - } return info->setMaxAperture(aperture); } @@ -368,11 +344,9 @@ bool Actuators::setMaxAperture(ServosList servo, float aperture) bool Actuators::setOpeningTime(ServosList servo, uint32_t time) { Lock<FastMutex> lock(infosMutex); - ServoInfo *info = getServo(servo); + ServoInfo* info = getServo(servo); if (info == nullptr) - { return false; - } return info->setOpeningTime(time); } @@ -380,11 +354,9 @@ bool Actuators::setOpeningTime(ServosList servo, uint32_t time) bool Actuators::isServoOpen(ServosList servo) { Lock<FastMutex> lock(infosMutex); - ServoInfo *info = getServo(servo); + ServoInfo* info = getServo(servo); if (info == nullptr) - { return false; - } return info->closeTs != 0; } @@ -393,17 +365,11 @@ bool Actuators::isCanServoOpen(ServosList servo) { Lock<FastMutex> lock(infosMutex); if (servo == ServosList::MAIN_VALVE) - { return canMainOpen; - } else if (servo == ServosList::VENTING_VALVE) - { return canVentingOpen; - } else - { return false; - } } void Actuators::openNitrogen() @@ -435,11 +401,9 @@ bool Actuators::isNitrogenOpen() uint32_t Actuators::getServoOpeningTime(ServosList servo) { Lock<FastMutex> lock(infosMutex); - ServoInfo *info = getServo(servo); + ServoInfo* info = getServo(servo); if (info == nullptr) - { return 0; - } return info->getOpeningTime(); } @@ -447,11 +411,9 @@ uint32_t Actuators::getServoOpeningTime(ServosList servo) float Actuators::getServoMaxAperture(ServosList servo) { Lock<FastMutex> lock(infosMutex); - ServoInfo *info = getServo(servo); + ServoInfo* info = getServo(servo); if (info == nullptr) - { return 0; - } return info->getMaxAperture(); } @@ -460,25 +422,19 @@ void Actuators::setCanServoOpen(ServosList servo, bool open) { Lock<FastMutex> lock(infosMutex); if (servo == ServosList::MAIN_VALVE) - { canMainOpen = open; - } else if (servo == ServosList::VENTING_VALVE) - { canVentingOpen = open; - } } -void Actuators::inject(DependencyInjector &injector) +void Actuators::inject(DependencyInjector& injector) { Super::inject(injector); - for (ServoInfo &info : infos) - { + for (ServoInfo& info : infos) info.inject(injector); - } } -Actuators::ServoInfo *Actuators::getServo(ServosList servo) +Actuators::ServoInfo* Actuators::getServo(ServosList servo) { switch (servo) { @@ -580,4 +536,4 @@ void Actuators::updatePositionsTask() unsafeCloseNitrogen(); } -} \ No newline at end of file +} diff --git a/src/RIGv2/Actuators/Actuators.h b/src/RIGv2/Actuators/Actuators.h index 939cd5e257b5586772c2528a0e7d5a8199030752..58342d074134aca1e148f84f61740faa2c759adb 100644 --- a/src/RIGv2/Actuators/Actuators.h +++ b/src/RIGv2/Actuators/Actuators.h @@ -108,10 +108,10 @@ public: void setCanServoOpen(ServosList servo, bool open); - void inject(Boardcore::DependencyInjector &injector) override; + void inject(Boardcore::DependencyInjector& injector) override; private: - ServoInfo *getServo(ServosList servo); + ServoInfo* getServo(ServosList servo); void unsafeSetServoPosition(uint8_t idx, float position); void unsafeOpenNitrogen(); @@ -119,7 +119,7 @@ private: void updatePositionsTask(); - Boardcore::Logger &sdLogger = Boardcore::Logger::getInstance(); + Boardcore::Logger& sdLogger = Boardcore::Logger::getInstance(); Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("actuators"); std::atomic<bool> started{false}; @@ -135,4 +135,4 @@ private: bool canVentingOpen = false; }; -} // namespace RIGv2 \ No newline at end of file +} // namespace RIGv2 diff --git a/src/RIGv2/Actuators/ActuatorsData.h b/src/RIGv2/Actuators/ActuatorsData.h index 5d638d8b38c818e5b88f151b1298ced8b1a9ed3a..b43263fe1f8452256707723bbf2ce6f33f0018cb 100644 --- a/src/RIGv2/Actuators/ActuatorsData.h +++ b/src/RIGv2/Actuators/ActuatorsData.h @@ -53,4 +53,4 @@ struct ActuatorsData } }; -} // namespace RIGv2 \ No newline at end of file +} // namespace RIGv2 diff --git a/src/RIGv2/BoardScheduler.h b/src/RIGv2/BoardScheduler.h index bea27fb47284c64d222ea827363ef3b4f0637cbd..11556aa7f0bc041d1079f7e4883f70f2addcee78 100644 --- a/src/RIGv2/BoardScheduler.h +++ b/src/RIGv2/BoardScheduler.h @@ -58,13 +58,13 @@ public: bool isStarted() { return started; } - Boardcore::TaskScheduler &getTars1Scheduler() { return tars1; } + Boardcore::TaskScheduler& getTars1Scheduler() { return tars1; } - Boardcore::TaskScheduler &getSensorsScheduler() { return sensors; } + Boardcore::TaskScheduler& getSensorsScheduler() { return sensors; } - Boardcore::TaskScheduler &getActuatorsScheduler() { return sensors; } + Boardcore::TaskScheduler& getActuatorsScheduler() { return sensors; } - Boardcore::TaskScheduler &getCanBusScheduler() { return sensors; } + Boardcore::TaskScheduler& getCanBusScheduler() { return sensors; } private: Boardcore::PrintLogger logger = @@ -76,4 +76,4 @@ private: Boardcore::TaskScheduler sensors; }; -} // namespace RIGv2 \ No newline at end of file +} // namespace RIGv2 diff --git a/src/RIGv2/Buses.h b/src/RIGv2/Buses.h index 6ba76033d03bf9c547853360d2f0f05f1f1254db..a777b13e6aac9420ad4ca291b9fe294a4601a1bf 100644 --- a/src/RIGv2/Buses.h +++ b/src/RIGv2/Buses.h @@ -33,15 +33,15 @@ class Buses : public Boardcore::Injectable public: Buses() : spi1(SPI1), spi2(SPI2), spi3(SPI3), spi4(SPI4), spi6(SPI6) {} - Boardcore::SPIBus &getH3LIS331DL() { return spi4; } - Boardcore::SPIBus &getLPS22DF() { return spi4; } - Boardcore::SPIBus &getLSM6DSRX() { return spi4; } - Boardcore::SPIBus &getLIS2MDL() { return spi4; } - Boardcore::SPIBus &getADS131M08_1() { return spi1; } - Boardcore::SPIBus &getADS131M08_2() { return spi3; } - Boardcore::SPIBus &getMAX31856_1() { return spi3; } - Boardcore::SPIBus &getMAX31856_2() { return spi1; } - Boardcore::SPIBus &getRadio() { return spi6; } + Boardcore::SPIBus& getH3LIS331DL() { return spi4; } + Boardcore::SPIBus& getLPS22DF() { return spi4; } + Boardcore::SPIBus& getLSM6DSRX() { return spi4; } + Boardcore::SPIBus& getLIS2MDL() { return spi4; } + Boardcore::SPIBus& getADS131M08_1() { return spi1; } + Boardcore::SPIBus& getADS131M08_2() { return spi3; } + Boardcore::SPIBus& getMAX31856_1() { return spi3; } + Boardcore::SPIBus& getMAX31856_2() { return spi1; } + Boardcore::SPIBus& getRadio() { return spi6; } private: Boardcore::SPIBus spi1; @@ -51,4 +51,4 @@ private: Boardcore::SPIBus spi6; }; -} // namespace RIGv2 \ No newline at end of file +} // namespace RIGv2 diff --git a/src/RIGv2/CanHandler/CanHandler.cpp b/src/RIGv2/CanHandler/CanHandler.cpp index d5783864aa31179fca9df421eea375f44e504528..055ae6e57bae3043a192eaaa1f7e61c2606e7efc 100644 --- a/src/RIGv2/CanHandler/CanHandler.cpp +++ b/src/RIGv2/CanHandler/CanHandler.cpp @@ -39,7 +39,7 @@ using namespace Common; CanHandler::CanHandler() : driver(CAN1, CanConfig::CONFIG, CanConfig::BIT_TIMING), protocol( - &driver, [this](const CanMessage &msg) { handleMessage(msg); }, + &driver, [this](const CanMessage& msg) { handleMessage(msg); }, Config::Scheduler::CAN_PRIORITY) { protocol.addFilter(static_cast<uint8_t>(CanConfig::Board::MAIN), @@ -56,7 +56,7 @@ bool CanHandler::start() { driver.init(); - TaskScheduler &scheduler = + TaskScheduler& scheduler = getModule<BoardScheduler>()->getCanBusScheduler(); uint8_t result = scheduler.addTask( @@ -136,7 +136,7 @@ CanHandler::CanStatus CanHandler::getCanStatus() return status; } -void CanHandler::handleMessage(const Canbus::CanMessage &msg) +void CanHandler::handleMessage(const Canbus::CanMessage& msg) { CanConfig::PrimaryType type = static_cast<CanConfig::PrimaryType>(msg.getPrimaryType()); @@ -175,7 +175,7 @@ void CanHandler::handleMessage(const Canbus::CanMessage &msg) } } -void CanHandler::handleEvent(const Canbus::CanMessage &msg) +void CanHandler::handleEvent(const Canbus::CanMessage& msg) { sdLogger.log(CanEvent{TimestampTimer::getTimestamp(), msg.getSource(), msg.getDestination(), msg.getSecondaryType()}); @@ -193,12 +193,12 @@ void CanHandler::handleEvent(const Canbus::CanMessage &msg) } } -void CanHandler::handleSensor(const Canbus::CanMessage &msg) +void CanHandler::handleSensor(const Canbus::CanMessage& msg) { CanConfig::SensorId sensor = static_cast<CanConfig::SensorId>(msg.getSecondaryType()); - Sensors *sensors = getModule<Sensors>(); + Sensors* sensors = getModule<Sensors>(); switch (sensor) { case CanConfig::SensorId::CC_PRESSURE: @@ -248,7 +248,7 @@ void CanHandler::handleSensor(const Canbus::CanMessage &msg) } } -void CanHandler::handleActuator(const Canbus::CanMessage &msg) +void CanHandler::handleActuator(const Canbus::CanMessage& msg) { ServosList servo = static_cast<ServosList>(msg.getSecondaryType()); CanServoFeedback data = servoFeedbackFromCanMessage(msg); @@ -257,7 +257,7 @@ void CanHandler::handleActuator(const Canbus::CanMessage &msg) getModule<Actuators>()->setCanServoOpen(servo, data.open); } -void CanHandler::handleStatus(const Canbus::CanMessage &msg) +void CanHandler::handleStatus(const Canbus::CanMessage& msg) { CanConfig::Board source = static_cast<CanConfig::Board>(msg.getSource()); CanDeviceStatus deviceStatus = deviceStatusFromCanMessage(msg); @@ -301,4 +301,4 @@ void CanHandler::handleStatus(const Canbus::CanMessage &msg) LOG_WARN(logger, "Received unsupported status: {}", source); } } -} \ No newline at end of file +} diff --git a/src/RIGv2/CanHandler/CanHandler.h b/src/RIGv2/CanHandler/CanHandler.h index 66a242ac808e07669f4cca24035a310a7dc2f0ce..282d8eef931a444188179e981d0e906dd77257a3 100644 --- a/src/RIGv2/CanHandler/CanHandler.h +++ b/src/RIGv2/CanHandler/CanHandler.h @@ -103,14 +103,14 @@ public: CanStatus getCanStatus(); private: - void handleMessage(const Boardcore::Canbus::CanMessage &msg); - void handleEvent(const Boardcore::Canbus::CanMessage &msg); - void handleSensor(const Boardcore::Canbus::CanMessage &msg); - void handleActuator(const Boardcore::Canbus::CanMessage &msg); - void handleStatus(const Boardcore::Canbus::CanMessage &msg); + void handleMessage(const Boardcore::Canbus::CanMessage& msg); + void handleEvent(const Boardcore::Canbus::CanMessage& msg); + void handleSensor(const Boardcore::Canbus::CanMessage& msg); + void handleActuator(const Boardcore::Canbus::CanMessage& msg); + void handleStatus(const Boardcore::Canbus::CanMessage& msg); Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("canhandler"); - Boardcore::Logger &sdLogger = Boardcore::Logger::getInstance(); + Boardcore::Logger& sdLogger = Boardcore::Logger::getInstance(); std::atomic<bool> started{false}; @@ -121,4 +121,4 @@ private: CanStatus status; }; -} // namespace RIGv2 \ No newline at end of file +} // namespace RIGv2 diff --git a/src/RIGv2/Configs/ActuatorsConfig.h b/src/RIGv2/Configs/ActuatorsConfig.h index 9497690d7ab24da7695b7926d35e9d2fc5c145e5..bfa3ddf4397c4c2f6a7cc3fb2bf77fc4ffa98bfa 100644 --- a/src/RIGv2/Configs/ActuatorsConfig.h +++ b/src/RIGv2/Configs/ActuatorsConfig.h @@ -82,4 +82,4 @@ static constexpr uint32_t NITROGEN_OPENING_TIME = 5000; // 5s } } // namespace Config -} // namespace RIGv2 \ No newline at end of file +} // namespace RIGv2 diff --git a/src/RIGv2/Configs/GMMConfig.h b/src/RIGv2/Configs/GMMConfig.h index 7656ea25d57bd4dd689bc43cffd0bf6b54ac399a..cc02af3a0d76b53b6a49e82835511bf81287ea2e 100644 --- a/src/RIGv2/Configs/GMMConfig.h +++ b/src/RIGv2/Configs/GMMConfig.h @@ -38,4 +38,4 @@ constexpr uint32_t NITROGEN_TIME = 20000; // 20s } // namespace GroundModeManager } // namespace Config -} // namespace RIGv2 \ No newline at end of file +} // namespace RIGv2 diff --git a/src/RIGv2/Configs/RadioConfig.h b/src/RIGv2/Configs/RadioConfig.h index 9b3039b9ad5ca165566b40dc18eec0bd74255573..98830ac094ccfdcb2c929d808389de2448c4a60f 100644 --- a/src/RIGv2/Configs/RadioConfig.h +++ b/src/RIGv2/Configs/RadioConfig.h @@ -51,4 +51,4 @@ constexpr long long LAST_COMMAND_THRESHOLD = 300; } // namespace Config -} // namespace RIGv2 \ No newline at end of file +} // namespace RIGv2 diff --git a/src/RIGv2/Configs/SchedulerConfig.h b/src/RIGv2/Configs/SchedulerConfig.h index 3acf93071d03c23f9f2b1f600295b3b2fcb8ccf9..be37f3cf651b7c1e4e89c628ba9e6e5122bf6b7d 100644 --- a/src/RIGv2/Configs/SchedulerConfig.h +++ b/src/RIGv2/Configs/SchedulerConfig.h @@ -47,4 +47,4 @@ static const miosix::Priority CAN_PRIORITY = miosix::PRIORITY_MAX - 1; } // namespace Config -} // namespace RIGv2 \ No newline at end of file +} // namespace RIGv2 diff --git a/src/RIGv2/Configs/SensorsConfig.h b/src/RIGv2/Configs/SensorsConfig.h index 230c2cc8e84746a2f55c747452eb70b16dee2f68..6348afffa34dd69e65e1faf4080f5758a064f72b 100644 --- a/src/RIGv2/Configs/SensorsConfig.h +++ b/src/RIGv2/Configs/SensorsConfig.h @@ -155,4 +155,4 @@ constexpr bool ENABLED = true; } // namespace Config -} // namespace RIGv2 \ No newline at end of file +} // namespace RIGv2 diff --git a/src/RIGv2/Configs/TARS1Config.h b/src/RIGv2/Configs/TARS1Config.h index bc5c06a35763ec26cf1771260294e63aa384eb19..6cb04df6806168b5ca48551832f1b601e43e6743 100644 --- a/src/RIGv2/Configs/TARS1Config.h +++ b/src/RIGv2/Configs/TARS1Config.h @@ -52,4 +52,4 @@ constexpr float PRESSURE_TOLERANCE = 0.035; // [bar] constexpr bool STOP_ON_MASS_STABILIZATION = false; } // namespace TARS1 } // namespace Config -} // namespace RIGv2 \ No newline at end of file +} // namespace RIGv2 diff --git a/src/RIGv2/Radio/Radio.cpp b/src/RIGv2/Radio/Radio.cpp index 1eb650b0793c695629ddc5b00656dd8062f9f1a4..5e7bdc47aaf2ac5870f6952b5fe21d35e833ab20 100644 --- a/src/RIGv2/Radio/Radio.cpp +++ b/src/RIGv2/Radio/Radio.cpp @@ -43,9 +43,7 @@ void handleDioIRQ() { SX1278Lora* instance = gRadio; if (instance) - { instance->handleDioIRQ(); - } } void setIRQRadio(SX1278Lora* radio) @@ -86,10 +84,8 @@ bool Radio::start() // Initialize mavdriver mavDriver = std::make_unique<MavDriver>( - radio.get(), - [this](MavDriver*, const mavlink_message_t& msg) - { handleMessage(msg); }, - Config::Radio::MAV_SLEEP_AFTER_SEND, + radio.get(), [this](MavDriver*, const mavlink_message_t& msg) + { handleMessage(msg); }, Config::Radio::MAV_SLEEP_AFTER_SEND, Config::Radio::MAV_OUT_BUFFER_MAX_AGE); if (!mavDriver->start()) @@ -155,13 +151,9 @@ void Radio::enqueueWack(const mavlink_message_t& msg, uint8_t errorId) MavlinkStatus Radio::getMavStatus() { if (mavDriver) - { return mavDriver->getStatus(); - } else - { return {}; - } } void Radio::handleMessage(const mavlink_message_t& msg) @@ -190,13 +182,9 @@ void Radio::handleMessage(const mavlink_message_t& msg) { uint8_t tmId = mavlink_msg_system_tm_request_tc_get_tm_id(&msg); if (enqueueSystemTm(tmId)) - { enqueueAck(msg); - } else - { enqueueNack(msg, 0); - } break; } @@ -206,13 +194,9 @@ void Radio::handleMessage(const mavlink_message_t& msg) uint8_t tmId = mavlink_msg_sensor_tm_request_tc_get_sensor_name(&msg); if (enqueueSensorTm(tmId)) - { enqueueAck(msg); - } else - { enqueueNack(msg, 0); - } break; } @@ -226,13 +210,9 @@ void Radio::handleMessage(const mavlink_message_t& msg) GroundModeManagerState::DISARMED) { if (getModule<Actuators>()->wiggleServo(servo)) - { enqueueAck(msg); - } else - { enqueueNack(msg, 0); - } } else { @@ -249,13 +229,9 @@ void Radio::handleMessage(const mavlink_message_t& msg) mavlink_msg_set_atomic_valve_timing_tc_get_servo_id(&msg)); if (getModule<Actuators>()->setOpeningTime(servo, time)) - { enqueueAck(msg); - } else - { enqueueNack(msg, 0); - } break; } @@ -268,13 +244,9 @@ void Radio::handleMessage(const mavlink_message_t& msg) mavlink_msg_set_valve_maximum_aperture_tc_get_servo_id(&msg)); if (getModule<Actuators>()->setMaxAperture(servo, aperture)) - { enqueueAck(msg); - } else - { enqueueNack(msg, 0); - } break; } @@ -325,26 +297,18 @@ void Radio::handleCommand(const mavlink_message_t& msg) case MAV_CMD_REGISTRY_LOAD: { if (getModule<Registry>()->load() == RegistryError::OK) - { enqueueAck(msg); - } else - { enqueueNack(msg, 0); - } break; } case MAV_CMD_REGISTRY_SAVE: { if (getModule<Registry>()->save() == RegistryError::OK) - { enqueueAck(msg); - } else - { enqueueNack(msg, 0); - } break; } diff --git a/src/RIGv2/Radio/Radio.h b/src/RIGv2/Radio/Radio.h index e55d15bdef990d2577777c5f794f752c8e33062c..26a32da17469e7c7b3a031d438fd882120b6bd0d 100644 --- a/src/RIGv2/Radio/Radio.h +++ b/src/RIGv2/Radio/Radio.h @@ -91,4 +91,4 @@ private: mavlink_conrig_state_tc_t oldConrigState; }; -} // namespace RIGv2 \ No newline at end of file +} // namespace RIGv2 diff --git a/src/RIGv2/Registry/Registry.cpp b/src/RIGv2/Registry/Registry.cpp index 65aab5be218f21df7e1d6dd47242455d083d2a8d..847764c6014b729205183d8d8869229616515b8e 100644 --- a/src/RIGv2/Registry/Registry.cpp +++ b/src/RIGv2/Registry/Registry.cpp @@ -72,4 +72,4 @@ Registry::Registry() bool Registry::start() { return RegistryFrontend::start() == RegistryError::OK; -} \ No newline at end of file +} diff --git a/src/RIGv2/Registry/Registry.h b/src/RIGv2/Registry/Registry.h index 03d71c0c8dae32021f70af5b3e6e17fb9ae69ff2..81164f3179ff2f48c30ab108fb59ccaff21aad1e 100644 --- a/src/RIGv2/Registry/Registry.h +++ b/src/RIGv2/Registry/Registry.h @@ -56,4 +56,4 @@ public: [[nodiscard]] bool start(); }; -} // namespace RIGv2 \ No newline at end of file +} // namespace RIGv2 diff --git a/src/RIGv2/Sensors/Sensors.cpp b/src/RIGv2/Sensors/Sensors.cpp index 478c550c521f07f9fd9f790029ea1d11f60e24d5..cf83ecd54f1cdb5b89c415926e450b47b1970b13 100644 --- a/src/RIGv2/Sensors/Sensors.cpp +++ b/src/RIGv2/Sensors/Sensors.cpp @@ -35,9 +35,7 @@ bool Sensors::isStarted() { return started; } bool Sensors::start() { if (Config::Sensors::InternalADC::ENABLED) - { internalAdcInit(); - } if (Config::Sensors::ADS131M08::ENABLED) { @@ -51,9 +49,7 @@ bool Sensors::start() } if (Config::Sensors::MAX31856::ENABLED) - { tc1Init(); - } if (!sensorManagerInit()) { @@ -119,49 +115,33 @@ PressureData Sensors::getBottomTankPressLastSample() PressureData Sensors::getCCPressLastSample() { if (useCanData) - { return getCanCCPressLastSample(); - } else - { return PressureData{}; - } } TemperatureData Sensors::getTankTempLastSample() { if (useCanData) - { return getCanTankTempLastSample(); - } else - { return getTc1LastSample(); - } } LoadCellData Sensors::getVesselWeightLastSample() { if (vesselWeight) - { return vesselWeight->getLastSample(); - } else - { return {}; - } } LoadCellData Sensors::getTankWeightLastSample() { if (tankWeight) - { return tankWeight->getLastSample(); - } else - { return {}; - } } CurrentData Sensors::getUmbilicalCurrentLastSample() @@ -197,13 +177,9 @@ VoltageData Sensors::getBatteryVoltageLastSample() VoltageData Sensors::getMotorBatteryVoltageLastSample() { if (useCanData) - { return getCanMotorBatteryVoltageLastSample(); - } else - { return VoltageData{}; - } } PressureData Sensors::getCanTopTankPressLastSample() @@ -551,7 +527,7 @@ void Sensors::tankWeightCallback() bool Sensors::sensorManagerInit() { - TaskScheduler &scheduler = + TaskScheduler& scheduler = getModule<BoardScheduler>()->getSensorsScheduler(); SensorManager::SensorMap_t map; @@ -622,4 +598,4 @@ bool Sensors::sensorManagerInit() manager = std::make_unique<SensorManager>(map, &scheduler); return manager->start(); -} \ No newline at end of file +} diff --git a/src/RIGv2/Sensors/Sensors.h b/src/RIGv2/Sensors/Sensors.h index 3ca145caad6b2480cbffa851c96c63e1cd16e8c6..c349b48b95fc2b8e668de38f629501d4cd6d3866 100644 --- a/src/RIGv2/Sensors/Sensors.h +++ b/src/RIGv2/Sensors/Sensors.h @@ -116,7 +116,7 @@ private: bool sensorManagerInit(); - Boardcore::Logger &sdLogger = Boardcore::Logger::getInstance(); + Boardcore::Logger& sdLogger = Boardcore::Logger::getInstance(); Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("sensors"); std::atomic<bool> started{false}; @@ -144,4 +144,4 @@ private: std::unique_ptr<Boardcore::SensorManager> manager; }; -} // namespace RIGv2 \ No newline at end of file +} // namespace RIGv2 diff --git a/src/RIGv2/Sensors/SensorsData.h b/src/RIGv2/Sensors/SensorsData.h index 25b6e2399ff9c1b52ae89dfc42b7d09f0123a0d8..5f418da1ef30262444e114f47359ae3954a11428 100644 --- a/src/RIGv2/Sensors/SensorsData.h +++ b/src/RIGv2/Sensors/SensorsData.h @@ -31,7 +31,7 @@ namespace RIGv2 struct ADC1Data : Boardcore::ADS131M08Data { - explicit ADC1Data(const Boardcore::ADS131M08Data &data) + explicit ADC1Data(const Boardcore::ADS131M08Data& data) : Boardcore::ADS131M08Data(data) { } @@ -41,7 +41,7 @@ struct ADC1Data : Boardcore::ADS131M08Data struct TC1Data : Boardcore::MAX31856Data { - explicit TC1Data(const Boardcore::MAX31856Data &data) + explicit TC1Data(const Boardcore::MAX31856Data& data) : Boardcore::MAX31856Data(data) { } @@ -51,7 +51,7 @@ struct TC1Data : Boardcore::MAX31856Data struct VesselWeightData : Boardcore::LoadCellData { - explicit VesselWeightData(const Boardcore::LoadCellData &data) + explicit VesselWeightData(const Boardcore::LoadCellData& data) : Boardcore::LoadCellData(data) { } @@ -61,7 +61,7 @@ struct VesselWeightData : Boardcore::LoadCellData struct TankWeightData : Boardcore::LoadCellData { - explicit TankWeightData(const Boardcore::LoadCellData &data) + explicit TankWeightData(const Boardcore::LoadCellData& data) : Boardcore::LoadCellData(data) { } @@ -71,7 +71,7 @@ struct TankWeightData : Boardcore::LoadCellData struct VesselPressureData : Boardcore::PressureData { - explicit VesselPressureData(const Boardcore::PressureData &data) + explicit VesselPressureData(const Boardcore::PressureData& data) : Boardcore::PressureData(data) { } @@ -81,7 +81,7 @@ struct VesselPressureData : Boardcore::PressureData struct FillingPressureData : Boardcore::PressureData { - explicit FillingPressureData(const Boardcore::PressureData &data) + explicit FillingPressureData(const Boardcore::PressureData& data) : Boardcore::PressureData(data) { } @@ -91,7 +91,7 @@ struct FillingPressureData : Boardcore::PressureData struct TopTankPressureData : Boardcore::PressureData { - explicit TopTankPressureData(const Boardcore::PressureData &data) + explicit TopTankPressureData(const Boardcore::PressureData& data) : Boardcore::PressureData(data) { } @@ -101,7 +101,7 @@ struct TopTankPressureData : Boardcore::PressureData struct BottomTankPressureData : Boardcore::PressureData { - explicit BottomTankPressureData(const Boardcore::PressureData &data) + explicit BottomTankPressureData(const Boardcore::PressureData& data) : Boardcore::PressureData(data) { } @@ -109,4 +109,4 @@ struct BottomTankPressureData : Boardcore::PressureData BottomTankPressureData() {} }; -} // namespace RIGv2 \ No newline at end of file +} // namespace RIGv2 diff --git a/src/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp b/src/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp index b4024646faeb72a017f1c1c0bc206f96a899d148..2b372417e9c13014f71c6c9e89cbca9058f85936 100644 --- a/src/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp +++ b/src/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp @@ -47,7 +47,7 @@ void GroundModeManager::setIgnitionTime(uint32_t time) getModule<Registry>()->setUnsafe(CONFIG_ID_IGNITION_TIME, time); } -State GroundModeManager::state_idle(const Event &event) +State GroundModeManager::state_idle(const Event& event) { switch (event) { @@ -85,7 +85,7 @@ State GroundModeManager::state_idle(const Event &event) } } -State GroundModeManager::state_init(const Event &event) +State GroundModeManager::state_init(const Event& event) { switch (event) { @@ -127,7 +127,7 @@ State GroundModeManager::state_init(const Event &event) } } -State GroundModeManager::state_init_error(const Event &event) +State GroundModeManager::state_init_error(const Event& event) { switch (event) { @@ -164,7 +164,7 @@ State GroundModeManager::state_init_error(const Event &event) } } -State GroundModeManager::state_disarmed(const Event &event) +State GroundModeManager::state_disarmed(const Event& event) { switch (event) { @@ -216,7 +216,7 @@ State GroundModeManager::state_disarmed(const Event &event) } } -State GroundModeManager::state_armed(const Event &event) +State GroundModeManager::state_armed(const Event& event) { switch (event) { @@ -271,7 +271,7 @@ State GroundModeManager::state_armed(const Event &event) } } -State GroundModeManager::state_firing(const Event &event) +State GroundModeManager::state_firing(const Event& event) { switch (event) { @@ -328,7 +328,7 @@ State GroundModeManager::state_firing(const Event &event) } } -State GroundModeManager::state_igniting(const Event &event) +State GroundModeManager::state_igniting(const Event& event) { switch (event) { @@ -379,7 +379,7 @@ State GroundModeManager::state_igniting(const Event &event) } } -State GroundModeManager::state_oxidizer(const Event &event) +State GroundModeManager::state_oxidizer(const Event& event) { switch (event) { @@ -419,7 +419,7 @@ State GroundModeManager::state_oxidizer(const Event &event) } } -State GroundModeManager::state_cooling(const Event &event) +State GroundModeManager::state_cooling(const Event& event) { switch (event) { diff --git a/src/RIGv2/StateMachines/GroundModeManager/GroundModeManager.h b/src/RIGv2/StateMachines/GroundModeManager/GroundModeManager.h index 1825140098b63349c350d09d42a3ef27bd5dbb1e..a4939c0cbbbc7720bc3f11c37f67bc31edbe1963 100644 --- a/src/RIGv2/StateMachines/GroundModeManager/GroundModeManager.h +++ b/src/RIGv2/StateMachines/GroundModeManager/GroundModeManager.h @@ -52,19 +52,19 @@ public: void setIgnitionTime(uint32_t time); private: - Boardcore::State state_idle(const Boardcore::Event &event); - Boardcore::State state_init(const Boardcore::Event &event); - Boardcore::State state_init_error(const Boardcore::Event &event); - Boardcore::State state_disarmed(const Boardcore::Event &event); - Boardcore::State state_armed(const Boardcore::Event &event); - Boardcore::State state_firing(const Boardcore::Event &event); - Boardcore::State state_igniting(const Boardcore::Event &event); - Boardcore::State state_oxidizer(const Boardcore::Event &event); - Boardcore::State state_cooling(const Boardcore::Event &event); + Boardcore::State state_idle(const Boardcore::Event& event); + Boardcore::State state_init(const Boardcore::Event& event); + Boardcore::State state_init_error(const Boardcore::Event& event); + Boardcore::State state_disarmed(const Boardcore::Event& event); + Boardcore::State state_armed(const Boardcore::Event& event); + Boardcore::State state_firing(const Boardcore::Event& event); + Boardcore::State state_igniting(const Boardcore::Event& event); + Boardcore::State state_oxidizer(const Boardcore::Event& event); + Boardcore::State state_cooling(const Boardcore::Event& event); void updateAndLogStatus(GroundModeManagerState state); - Boardcore::Logger &sdLogger = Boardcore::Logger::getInstance(); + Boardcore::Logger& sdLogger = Boardcore::Logger::getInstance(); Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("gmm"); std::atomic<GroundModeManagerState> state{GroundModeManagerState::IDLE}; @@ -73,4 +73,4 @@ private: uint16_t coolingDelayEventId = -1; }; -} // namespace RIGv2 \ No newline at end of file +} // namespace RIGv2 diff --git a/src/RIGv2/StateMachines/GroundModeManager/GroundModeManagerData.h b/src/RIGv2/StateMachines/GroundModeManager/GroundModeManagerData.h index 864f5c5832e137bd5fbfa55f9f5b6b489952afd5..bc08a83a3744cfc3d26ad8ff57ccb872e07311ec 100644 --- a/src/RIGv2/StateMachines/GroundModeManager/GroundModeManagerData.h +++ b/src/RIGv2/StateMachines/GroundModeManager/GroundModeManagerData.h @@ -65,4 +65,4 @@ struct GroundModeManagerData } }; -} // namespace RIGv2 \ No newline at end of file +} // namespace RIGv2 diff --git a/src/RIGv2/StateMachines/TARS1/MedianFilter.h b/src/RIGv2/StateMachines/TARS1/MedianFilter.h index cb125b5a4108f3875575509298e3d1e12fc654ac..0d14f3f1b12f4cd83a2ecb0a3d01fbc5623fe371 100644 --- a/src/RIGv2/StateMachines/TARS1/MedianFilter.h +++ b/src/RIGv2/StateMachines/TARS1/MedianFilter.h @@ -54,4 +54,4 @@ private: std::array<T, Max> values = {0}; }; -} // namespace RIGv2 \ No newline at end of file +} // namespace RIGv2 diff --git a/src/RIGv2/StateMachines/TARS1/TARS1.cpp b/src/RIGv2/StateMachines/TARS1/TARS1.cpp index b48d54609fa77c55eee6ba4db38dc9ea518dc0c3..5b0f879d6ee4252d57bf60c0893bbbbf5318ee5f 100644 --- a/src/RIGv2/StateMachines/TARS1/TARS1.cpp +++ b/src/RIGv2/StateMachines/TARS1/TARS1.cpp @@ -287,4 +287,4 @@ void TARS1::logSample(float pressure, float mass) { TarsSampleData data = {TimestampTimer::getTimestamp(), pressure, mass}; sdLogger.log(data); -} \ No newline at end of file +} diff --git a/src/RIGv2/StateMachines/TARS1/TARS1.h b/src/RIGv2/StateMachines/TARS1/TARS1.h index e68cf5310b0caaba64076a8163de8cb21d56375f..eba7653f2d985ada2ef80897f3b798946e4254a9 100644 --- a/src/RIGv2/StateMachines/TARS1/TARS1.h +++ b/src/RIGv2/StateMachines/TARS1/TARS1.h @@ -50,13 +50,13 @@ public: private: void sample(); - void state_ready(const Boardcore::Event &event); - void state_refueling(const Boardcore::Event &event); + void state_ready(const Boardcore::Event& event); + void state_refueling(const Boardcore::Event& event); void logAction(TarsActionType action); void logSample(float pressure, float mass); - Boardcore::Logger &sdLogger = Boardcore::Logger::getInstance(); + Boardcore::Logger& sdLogger = Boardcore::Logger::getInstance(); Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("tars1"); float previousMass = 0; @@ -77,4 +77,4 @@ private: uint16_t nextDelayedEventId = 0; }; -} // namespace RIGv2 \ No newline at end of file +} // namespace RIGv2 diff --git a/src/RIGv2/StateMachines/TARS1/TARS1Data.h b/src/RIGv2/StateMachines/TARS1/TARS1Data.h index f3d9a2005a54591142c6f25fd8c2e8a86c600179..bf1fc75e8c92389dfef10f7a79c76de8a81c59cb 100644 --- a/src/RIGv2/StateMachines/TARS1/TARS1Data.h +++ b/src/RIGv2/StateMachines/TARS1/TARS1Data.h @@ -55,7 +55,7 @@ struct TarsActionData static std::string header() { return "timestamp,action\n"; } - void print(std::ostream &os) const + void print(std::ostream& os) const { os << timestamp << "," << (int)action << "\n"; } @@ -76,10 +76,10 @@ struct TarsSampleData static std::string header() { return "timestamp,pressure,mass\n"; } - void print(std::ostream &os) const + void print(std::ostream& os) const { os << timestamp << "," << pressure << "," << mass << "\n"; } }; -} // namespace RIGv2 \ No newline at end of file +} // namespace RIGv2 diff --git a/src/RIGv2/rig-v2-entry.cpp b/src/RIGv2/rig-v2-entry.cpp index d4042b48f2915adeef5a6db8e03182816f538b33..ebc6174462b608020ed3757a216a2adaf8dbbdb6 100644 --- a/src/RIGv2/rig-v2-entry.cpp +++ b/src/RIGv2/rig-v2-entry.cpp @@ -49,28 +49,28 @@ int main() { DependencyManager manager; - Buses *buses = new Buses(); - BoardScheduler *scheduler = new BoardScheduler(); + Buses* buses = new Buses(); + BoardScheduler* scheduler = new BoardScheduler(); - Sensors *sensors = new Sensors(); - Actuators *actuators = new Actuators(); - Registry *registry = new Registry(); - CanHandler *canHandler = new CanHandler(); - GroundModeManager *gmm = new GroundModeManager(); - TARS1 *tars1 = new TARS1(); - Radio *radio = new Radio(); + Sensors* sensors = new Sensors(); + Actuators* actuators = new Actuators(); + Registry* registry = new Registry(); + CanHandler* canHandler = new CanHandler(); + GroundModeManager* gmm = new GroundModeManager(); + TARS1* tars1 = new TARS1(); + Radio* radio = new Radio(); - Logger &sdLogger = Logger::getInstance(); - EventBroker &broker = EventBroker::getInstance(); + Logger& sdLogger = Logger::getInstance(); + EventBroker& broker = EventBroker::getInstance(); // Setup event sniffer - EventSniffer sniffer( - broker, - [&](uint8_t event, uint8_t topic) - { - EventData data{TimestampTimer::getTimestamp(), event, topic}; - sdLogger.log(data); - }); + EventSniffer sniffer(broker, + [&](uint8_t event, uint8_t topic) + { + EventData data{TimestampTimer::getTimestamp(), + event, topic}; + sdLogger.log(data); + }); // Insert modules bool initResult = manager.insert<Buses>(buses) && @@ -115,9 +115,7 @@ int main() // Perform an initial registry load std::cout << "Loading backed registry" << std::endl; if (registry->load() != RegistryError::OK) - { std::cout << "*** Failed to load backed registry ***" << std::endl; - } std::cout << "Starting Actuators" << std::endl; if (!actuators->start()) @@ -223,4 +221,4 @@ int main() } return 0; -} \ No newline at end of file +} diff --git a/src/common/MavlinkLyra.h b/src/common/MavlinkLyra.h index e6cd6b5c05506e41994a0388c112b59c7c4f1ad4..234fa932a62531731a3de9348497a4d05d80e34a 100644 --- a/src/common/MavlinkLyra.h +++ b/src/common/MavlinkLyra.h @@ -84,4 +84,4 @@ inline Events mavCmdToEvent(uint8_t id) } } -} // namespace Common \ No newline at end of file +} // namespace Common diff --git a/src/common/MavlinkOrion.h b/src/common/MavlinkOrion.h index bd6f7ffaab1fa8cd812d6ce99ce0cb023a6274b1..24f7f4dd50a57188d266eb67f6e20d0f5f787b76 100644 --- a/src/common/MavlinkOrion.h +++ b/src/common/MavlinkOrion.h @@ -82,4 +82,4 @@ inline Events mavCmdToEvent(uint8_t id) } } -} // namespace Common \ No newline at end of file +} // namespace Common diff --git a/src/common/Radio.h b/src/common/Radio.h index 3a246312fbd8a76063aabfd618267f88a1d039d1..c202d14cdc7493d5608a20be4b17362129c3b11d 100644 --- a/src/common/Radio.h +++ b/src/common/Radio.h @@ -62,4 +62,4 @@ static constexpr Boardcore::SX1278Lora::Config RIG_RADIO_CONFIG = { .power = 2, .enable_crc = false}; -} // namespace Common \ No newline at end of file +} // namespace Common