From 0725ed83862f3710aded3bd27399e9da789bdf5d Mon Sep 17 00:00:00 2001 From: "luca.conterio" <luca.conterio@skywarder.eu> Date: Sat, 27 Nov 2021 18:23:01 +0100 Subject: [PATCH 1/6] [Payload] Setup folders --- CMakeLists.txt | 12 +- cmake/dependencies.cmake | 15 +- .../DeathStack/AirBrakes/AirBrakesServo.h | 2 +- .../DeathStack/Deployment/DeploymentServo.h | 2 +- .../TelemetriesTelecommands/TmRepository.h | 2 +- src/boards/Payload/Main/Actuators.h | 36 ++ src/boards/Payload/Main/Bus.h | 39 ++ src/boards/Payload/Main/Radio.cpp | 180 +++++++ src/boards/Payload/Main/Radio.h | 54 ++ src/boards/Payload/Main/Sensors.cpp | 502 ++++++++++++++++++ src/boards/Payload/Main/Sensors.h | 129 +++++ src/boards/Payload/Main/SensorsData.h | 70 +++ src/boards/Payload/PayloadBoard.h | 236 ++++++++ src/boards/Payload/PayloadStatus.h | 75 +++ src/boards/Payload/WingControl/WingServo.cpp | 86 +++ src/boards/Payload/WingControl/WingServo.h | 68 +++ src/boards/Payload/configs/SensorsConfig.h | 135 +++++ src/boards/Payload/configs/WingConfig.h | 57 ++ src/{boards/DeathStack => common}/Algorithm.h | 5 - src/{boards => common}/CanInterfaces.h | 0 .../DeathStack => common}/ServoInterface.h | 5 - .../DeathStack/System => common}/SystemData.h | 0 .../events/EventStrings.cpp | 0 .../DeathStack => common}/events/Events.h | 0 .../DeathStack => common}/events/Topics.h | 0 src/entrypoints/payload-entry.cpp | 75 +++ 26 files changed, 1769 insertions(+), 16 deletions(-) create mode 100644 src/boards/Payload/Main/Actuators.h create mode 100644 src/boards/Payload/Main/Bus.h create mode 100644 src/boards/Payload/Main/Radio.cpp create mode 100644 src/boards/Payload/Main/Radio.h create mode 100644 src/boards/Payload/Main/Sensors.cpp create mode 100644 src/boards/Payload/Main/Sensors.h create mode 100644 src/boards/Payload/Main/SensorsData.h create mode 100644 src/boards/Payload/PayloadBoard.h create mode 100644 src/boards/Payload/PayloadStatus.h create mode 100644 src/boards/Payload/WingControl/WingServo.cpp create mode 100644 src/boards/Payload/WingControl/WingServo.h create mode 100644 src/boards/Payload/configs/SensorsConfig.h create mode 100644 src/boards/Payload/configs/WingConfig.h rename src/{boards/DeathStack => common}/Algorithm.h (97%) rename src/{boards => common}/CanInterfaces.h (100%) rename src/{boards/DeathStack => common}/ServoInterface.h (98%) rename src/{boards/DeathStack/System => common}/SystemData.h (100%) rename src/{boards/DeathStack => common}/events/EventStrings.cpp (100%) rename src/{boards/DeathStack => common}/events/Events.h (100%) rename src/{boards/DeathStack => common}/events/Topics.h (100%) create mode 100644 src/entrypoints/payload-entry.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index ffb1eac6b..52974fd93 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,10 +41,20 @@ target_include_directories(death-stack-x-entry PRIVATE ${OBSW_INCLUDE_DIRS}) target_compile_definitions(death-stack-x-entry PRIVATE FLIGHT EUROC) sbs_target(death-stack-x-entry stm32f429zi_skyward_death_stack_x) -add_executable(death-stack-x-testsuite src/entrypoints/death-stack-x-testsuite.cpp ${DEATHSTACK_NEW_SOURCES}) +add_executable(death-stack-x-testsuite + src/entrypoints/death-stack-x-testsuite.cpp + ${DEATHSTACK_NEW_SOURCES} +) target_include_directories(death-stack-x-testsuite PRIVATE ${OBSW_INCLUDE_DIRS}) sbs_target(death-stack-x-testsuite stm32f429zi_skyward_death_stack_x) +add_executable(payload-entry + src/entrypoints/payload-entry.cpp + ${PAYLOAD_SOURCES} +) +target_include_directories(payload-entry PRIVATE ${OBSW_INCLUDE_DIRS}) +sbs_target(payload-entry stm32f429zi_skyward_death_stack_x) + #add_executable(death-stack-entry src/entrypoints/death-stack-entry.cpp ${DEATHSTACK_SOURCES}) #target_include_directories(death-stack-entry PRIVATE ${OBSW_INCLUDE_DIRS}) #target_compile_definitions(death-stack-entry PRIVATE DEATH_STACK_1) diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake index fe291af39..9b75f9864 100644 --- a/cmake/dependencies.cmake +++ b/cmake/dependencies.cmake @@ -19,10 +19,15 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. -set(OBSW_INCLUDE_DIRS src src/boards/DeathStack) +set(OBSW_INCLUDE_DIRS + src + src/common + src/boards + src/boards/DeathStack +) set(DEATHSTACK_NEW_SOURCES - src/boards/DeathStack/events/EventStrings.cpp + src/common/events/EventStrings.cpp src/boards/DeathStack/PinHandler/PinHandler.cpp src/boards/DeathStack/TelemetriesTelecommands/TMTCController.cpp src/boards/DeathStack/TelemetriesTelecommands/TCHandler.cpp @@ -54,6 +59,12 @@ set(DEATHSTACK_NEW_SOURCES # src/boards/DeathStack/TMTCManager/XbeeInterrupt.cpp # src/boards/DeathStack/AirBrakes/AirBrakesServo.cpp #) +set(PAYLOAD_SOURCES + src/common/events/EventStrings.cpp + src/boards/Payload/WingControl/WingServo.cpp + src/boards/Payload/Main/Sensors.cpp + src/boards/Payload/Main/Radio.cpp +) set(ADA_SOURCES src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.cpp src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.cpp diff --git a/src/boards/DeathStack/AirBrakes/AirBrakesServo.h b/src/boards/DeathStack/AirBrakes/AirBrakesServo.h index 9e2a91c0d..7332d38d3 100644 --- a/src/boards/DeathStack/AirBrakes/AirBrakesServo.h +++ b/src/boards/DeathStack/AirBrakes/AirBrakesServo.h @@ -24,7 +24,7 @@ #include <AirBrakes/AirBrakesData.h> #include <LoggerService/LoggerService.h> -#include <ServoInterface.h> +#include <common/ServoInterface.h> #include <configs/AirBrakesConfig.h> #include <drivers/servo/servo.h> #include <miosix.h> diff --git a/src/boards/DeathStack/Deployment/DeploymentServo.h b/src/boards/DeathStack/Deployment/DeploymentServo.h index 7f686a369..473b82ced 100644 --- a/src/boards/DeathStack/Deployment/DeploymentServo.h +++ b/src/boards/DeathStack/Deployment/DeploymentServo.h @@ -22,7 +22,7 @@ #pragma once -#include <ServoInterface.h> +#include <common/ServoInterface.h> #include <configs/DeploymentConfig.h> #include <drivers/servo/servo.h> #include <miosix.h> diff --git a/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.h b/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.h index b33ac926b..fa1077442 100644 --- a/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.h +++ b/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.h @@ -32,7 +32,7 @@ #include <NavigationAttitudeSystem/NASData.h> #include <PinHandler/PinHandlerData.h> #include <Singleton.h> -#include <System/SystemData.h> +#include <SystemData.h> #include <Main/SensorsData.h> #include <TelemetriesTelecommands/Mavlink.h> #include <diagnostic/PrintLogger.h> diff --git a/src/boards/Payload/Main/Actuators.h b/src/boards/Payload/Main/Actuators.h new file mode 100644 index 000000000..9e69ce622 --- /dev/null +++ b/src/boards/Payload/Main/Actuators.h @@ -0,0 +1,36 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <Payload/WingControl/WingServo.h> + +namespace PayloadBoard +{ + +struct Actuators +{ + WingServo* left_servo; + WingServo* right_servo; +}; + +} // namespace PayloadBoard \ No newline at end of file diff --git a/src/boards/Payload/Main/Bus.h b/src/boards/Payload/Main/Bus.h new file mode 100644 index 000000000..dc9bfdbe9 --- /dev/null +++ b/src/boards/Payload/Main/Bus.h @@ -0,0 +1,39 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <drivers/spi/SPIBus.h> +#include <miosix.h> + +using namespace Boardcore; + +namespace PayloadBoard +{ + +struct Bus +{ + SPIBusInterface* spi1 = new SPIBus(SPI1); + SPIBusInterface* spi2 = new SPIBus(SPI2); +}; + +} // namespace PayloadBoard \ No newline at end of file diff --git a/src/boards/Payload/Main/Radio.cpp b/src/boards/Payload/Main/Radio.cpp new file mode 100644 index 000000000..39944320e --- /dev/null +++ b/src/boards/Payload/Main/Radio.cpp @@ -0,0 +1,180 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +//#include <LoggerService/LoggerService.h> +#include <Payload/Main/Radio.h> +//#include <TelemetriesTelecommands/TCHandler.h> +//#include <TelemetriesTelecommands/TMTCController.h> +//#include <TelemetriesTelecommands/TmRepository.h> +#include <drivers/Xbee/APIFramesLog.h> +#include <drivers/Xbee/ATCommands.h> +#include <drivers/interrupt/external_interrupts.h> +#include <interfaces-impl/hwmapping.h> + +#include <functional> + +// using std::function; +using std::bind; +using namespace std::placeholders; +using namespace Boardcore; + +// Xbee ATTN interrupt +void __attribute__((used)) EXTI10_IRQHandlerImpl() +{ + using namespace PayloadBoard; + + /*if (DeathStack::getInstance()->radio->xbee != nullptr) + { + DeathStack::getInstance()->radio->xbee->handleATTNInterrupt(); + }*/ +} + +namespace PayloadBoard +{ + +Radio::Radio(SPIBusInterface& xbee_bus) : xbee_bus(xbee_bus) +{ + SPIBusConfig xbee_cfg{}; + + xbee_cfg.clock_div = SPIClockDivider::DIV16; + + xbee = new Xbee::Xbee(xbee_bus, xbee_cfg, miosix::xbee::cs::getPin(), + miosix::xbee::attn::getPin(), + miosix::xbee::reset::getPin()); + xbee->setOnFrameReceivedListener( + bind(&Radio::onXbeeFrameReceived, this, _1)); + + // Xbee::setDataRate(*xbee, XBEE_80KBPS_DATA_RATE, XBEE_TIMEOUT); + + // mav_driver = new MavDriver(xbee, handleMavlinkMessage, SLEEP_AFTER_SEND, + // MAV_OUT_BUFFER_MAX_AGE); + + // tmtc_manager = new TMTCController(); + + // tm_repo = TmRepository::getInstance(); + + enableExternalInterrupt(GPIOF_BASE, 10, InterruptTrigger::FALLING_EDGE); +} + +Radio::~Radio() +{ + // tmtc_manager->stop(); + // mav_driver->stop(); + + // delete tmtc_manager; + // delete mav_driver; + delete xbee; +} + +bool Radio::start() +{ + // return mav_driver->start() && tmtc_manager->start(); + return true; +} + +void Radio::onXbeeFrameReceived(Xbee::APIFrame& frame) +{ + //LoggerService& logger = *LoggerService::getInstance(); + + using namespace Xbee; + bool logged = false; + switch (frame.frame_type) + { + case FTYPE_AT_COMMAND: + { + ATCommandFrameLog dest; + logged = ATCommandFrameLog::toFrameType(frame, &dest); + if (logged) + { + //logger.log(dest); + } + break; + } + case FTYPE_AT_COMMAND_RESPONSE: + { + ATCommandResponseFrameLog dest; + logged = ATCommandResponseFrameLog::toFrameType(frame, &dest); + if (logged) + { + //logger.log(dest); + } + break; + } + case FTYPE_MODEM_STATUS: + { + ModemStatusFrameLog dest; + logged = ModemStatusFrameLog::toFrameType(frame, &dest); + if (logged) + { + //logger.log(dest); + } + break; + } + case FTYPE_TX_REQUEST: + { + TXRequestFrameLog dest; + logged = TXRequestFrameLog::toFrameType(frame, &dest); + if (logged) + { + //logger.log(dest); + } + break; + } + case FTYPE_TX_STATUS: + { + TXStatusFrameLog dest; + logged = TXStatusFrameLog::toFrameType(frame, &dest); + if (logged) + { + //logger.log(dest); + } + break; + } + case FTYPE_RX_PACKET_FRAME: + { + RXPacketFrameLog dest; + logged = RXPacketFrameLog::toFrameType(frame, &dest); + if (logged) + { + //logger.log(dest); + } + break; + } + } + + if (!logged) + { + APIFrameLog api; + APIFrameLog::fromAPIFrame(frame, &api); + //logger.log(api); + } +} + +void Radio::logStatus() +{ + //MavlinkStatus status = mav_driver->getStatus(); + //status.timestamp = TimestampTimer::getTimestamp(); + //LoggerService::getInstance()->log(status); + //LoggerService::getInstance()->log(xbee->getStatus()); +} + +} // namespace PayloadBoard \ No newline at end of file diff --git a/src/boards/Payload/Main/Radio.h b/src/boards/Payload/Main/Radio.h new file mode 100644 index 000000000..db349d628 --- /dev/null +++ b/src/boards/Payload/Main/Radio.h @@ -0,0 +1,54 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +//#include <TelemetriesTelecommands/Mavlink.h> +#include <drivers/Xbee/Xbee.h> + +using namespace Boardcore; + +namespace PayloadBoard +{ + +class Radio +{ +public: + //TMTCController* tmtc_manager; + //TmRepository* tm_repo; + Xbee::Xbee* xbee; + //MavDriver* mav_driver; + + Radio(SPIBusInterface& xbee_bus_); + ~Radio(); + + bool start(); + + void logStatus(); + +private: + void onXbeeFrameReceived(Xbee::APIFrame& frame); + + SPIBusInterface& xbee_bus; +}; + +} // namespace PayloadBoard \ No newline at end of file diff --git a/src/boards/Payload/Main/Sensors.cpp b/src/boards/Payload/Main/Sensors.cpp new file mode 100644 index 000000000..48d38f6ed --- /dev/null +++ b/src/boards/Payload/Main/Sensors.cpp @@ -0,0 +1,502 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +//#include <ApogeeDetectionAlgorithm/ADAController.h> +#include <Payload/PayloadBoard.h> +#include <Debug.h> +//#include <LoggerService/LoggerService.h> +#include <TimestampTimer.h> +#include <Payload/configs/SensorsConfig.h> +#include <drivers/interrupt/external_interrupts.h> +#include <interfaces-impl/hwmapping.h> +#include <sensors/Sensor.h> +#include <utils/aero/AeroUtils.h> + +#include <functional> +#include <utility> + +using std::bind; +using std::function; + +using namespace Boardcore; + +// BMX160 Watermark interrupt +void __attribute__((used)) EXTI5_IRQHandlerImpl() +{ + using namespace PayloadBoard; + + /*if (Payload::getInstance()->sensors->imu_bmx160 != nullptr) + { + Payload::getInstance()->sensors->imu_bmx160->IRQupdateTimestamp( + TimestampTimer::getTimestamp()); + }*/ +} + +namespace PayloadBoard +{ + +using namespace SensorConfigs; + +Sensors::Sensors(SPIBusInterface& spi1_bus, TaskScheduler* scheduler) + : spi1_bus(spi1_bus) +{ + // sensors are added to the map ordered by increasing period + ADS1118Init(); + magLISinit(); + imuBMXInit(); + imuBMXWithCorrectionInit(); + pressDigiInit(); + pressPitotInit(); + pressDPLVaneInit(); + pressStaticInit(); + gpsUbloxInit(); + internalAdcInit(); + batteryVoltageInit(); + + sensor_manager = new SensorManager(scheduler, sensors_map); +} + +Sensors::~Sensors() +{ + delete imu_bmx160; + delete press_digital; + delete gps_ublox; + delete internal_adc; + delete battery_voltage; + delete adc_ads1118; + delete press_pitot; + delete press_dpl_vane; + delete press_static_port; + delete mag_lis3mdl; + + sensor_manager->stop(); + delete sensor_manager; +} + +bool Sensors::start() +{ + GpioPin int_pin = miosix::sensors::bmx160::intr::getPin(); + enableExternalInterrupt(int_pin.getPort(), int_pin.getNumber(), + InterruptTrigger::FALLING_EDGE); + + gps_ublox->start(); + + bool sm_start_result = sensor_manager->start(); + + // if not init ok, set failing sensors in sensors status + if (!sm_start_result) + { + updateSensorsStatus(); + } + + //LoggerService::getInstance()->log(status); + + return sm_start_result; +} + +void Sensors::calibrate() +{ + imu_bmx160_with_correction->calibrate(); + //LoggerService::getInstance()->log( + // imu_bmx160_with_correction->getGyroscopeBiases()); + + press_pitot->calibrate(); + + // use the digital barometer as a reference to compute the offset of the + // analog one (static ports sensor) + Stats press_digi_stats; + for (unsigned int i = 0; i < PRESS_STATIC_CALIB_SAMPLES_NUM / 10; i++) + { + Thread::sleep(SAMPLE_PERIOD_PRESS_DIGITAL); + press_digi_stats.add(press_digital->getLastSample().press); + } + press_static_port->setReferencePressure(press_digi_stats.getStats().mean); + press_static_port->calibrate(); + + // wait differential and static barometers calibration end + while (press_pitot->isCalibrating() && press_static_port->isCalibrating()) + { + Thread::sleep(10); + } +} + +void Sensors::internalAdcInit() +{ + internal_adc = new InternalADC(*ADC3, INTERNAL_ADC_VREF); + + internal_adc->enableChannel(ADC_BATTERY_VOLTAGE); + + SensorInfo info("InternalADC", SAMPLE_PERIOD_INTERNAL_ADC, + bind(&Sensors::internalAdcCallback, this), false, true); + sensors_map.emplace(std::make_pair(internal_adc, info)); + + LOG_INFO(log, "InternalADC setup done!"); +} + +void Sensors::batteryVoltageInit() +{ + function<ADCData()> voltage_fun( + bind(&InternalADC::getVoltage, internal_adc, ADC_BATTERY_VOLTAGE)); + battery_voltage = + new BatteryVoltageSensor(voltage_fun, BATTERY_VOLTAGE_COEFF); + + SensorInfo info("BatterySensor", SAMPLE_PERIOD_INTERNAL_ADC, + bind(&Sensors::batteryVoltageCallback, this), false, true); + + sensors_map.emplace(std::make_pair(battery_voltage, info)); + + LOG_INFO(log, "Battery voltage sensor setup done!"); +} + +void Sensors::pressDigiInit() +{ + SPIBusConfig spi_cfg{}; + spi_cfg.clock_div = SPIClockDivider::DIV16; + + press_digital = new MS5803(spi1_bus, miosix::sensors::ms5803::cs::getPin(), + spi_cfg, TEMP_DIVIDER_PRESS_DIGITAL); + + SensorInfo info("DigitalBarometer", SAMPLE_PERIOD_PRESS_DIGITAL, + bind(&Sensors::pressDigiCallback, this), false, true); + + sensors_map.emplace(std::make_pair(press_digital, info)); + + LOG_INFO(log, "MS5803 pressure sensor setup done!"); +} + +void Sensors::ADS1118Init() +{ + SPIBusConfig spi_cfg = ADS1118::getDefaultSPIConfig(); + spi_cfg.clock_div = SPIClockDivider::DIV64; + + ADS1118::ADS1118Config ads1118Config = ADS1118::ADS1118_DEFAULT_CONFIG; + ads1118Config.bits.mode = ADS1118::ADS1118Mode::CONTIN_CONV_MODE; + + adc_ads1118 = new ADS1118(spi1_bus, miosix::sensors::ads1118::cs::getPin(), + ads1118Config, spi_cfg); + + adc_ads1118->enableInput(ADC_CH_STATIC_PORT, ADC_DR_STATIC_PORT, + ADC_PGA_STATIC_PORT); + + adc_ads1118->enableInput(ADC_CH_PITOT_PORT, ADC_DR_PITOT_PORT, + ADC_PGA_PITOT_PORT); + adc_ads1118->enableInput(ADC_CH_DPL_PORT, ADC_DR_DPL_PORT, + ADC_PGA_DPL_PORT); + + adc_ads1118->enableInput(ADC_CH_VREF, ADC_DR_VREF, ADC_PGA_VREF); + + SensorInfo info("ADS1118", SAMPLE_PERIOD_ADC_ADS1118, + bind(&Sensors::ADS1118Callback, this), false, true); + sensors_map.emplace(std::make_pair(adc_ads1118, info)); + + LOG_INFO(log, "ADS1118 setup done!"); +} + +void Sensors::pressPitotInit() +{ + function<ADCData()> voltage_fun( + bind(&ADS1118::getVoltage, adc_ads1118, ADC_CH_PITOT_PORT)); + press_pitot = new SSCDRRN015PDA(voltage_fun, REFERENCE_VOLTAGE, + PRESS_PITOT_CALIB_SAMPLES_NUM); + + SensorInfo info("DiffBarometer", SAMPLE_PERIOD_PRESS_PITOT, + bind(&Sensors::pressPitotCallback, this), false, true); + + sensors_map.emplace(std::make_pair(press_pitot, info)); + + LOG_INFO(log, "Diff pressure sensor setup done!"); +} + +void Sensors::pressDPLVaneInit() +{ + function<ADCData()> voltage_fun( + bind(&ADS1118::getVoltage, adc_ads1118, ADC_CH_DPL_PORT)); + press_dpl_vane = new SSCDANN030PAA(voltage_fun, REFERENCE_VOLTAGE); + + SensorInfo info("DeploymentBarometer", SAMPLE_PERIOD_PRESS_DPL, + bind(&Sensors::pressDPLVaneCallback, this), false, true); + + sensors_map.emplace(std::make_pair(press_dpl_vane, info)); + + LOG_INFO(log, "DPL pressure sensor setup done!"); +} + +void Sensors::pressStaticInit() +{ + function<ADCData()> voltage_fun( + bind(&ADS1118::getVoltage, adc_ads1118, ADC_CH_STATIC_PORT)); + press_static_port = new MPXHZ6130A(voltage_fun, REFERENCE_VOLTAGE, + PRESS_STATIC_CALIB_SAMPLES_NUM, + PRESS_STATIC_MOVING_AVG_COEFF); + + SensorInfo info("StaticPortsBarometer", SAMPLE_PERIOD_PRESS_STATIC, + bind(&Sensors::pressStaticCallback, this), false, true); + + sensors_map.emplace(std::make_pair(press_static_port, info)); + + LOG_INFO(log, "Static pressure sensor setup done!"); +} + +void Sensors::imuBMXInit() +{ + SPIBusConfig spi_cfg; + spi_cfg.clock_div = SPIClockDivider::DIV8; + + BMX160Config bmx_config; + bmx_config.fifo_mode = BMX160Config::FifoMode::HEADER; + bmx_config.fifo_watermark = IMU_BMX_FIFO_WATERMARK; + bmx_config.fifo_int = BMX160Config::FifoInterruptPin::PIN_INT1; + + bmx_config.temp_divider = IMU_BMX_TEMP_DIVIDER; + + bmx_config.acc_range = IMU_BMX_ACC_FULLSCALE_ENUM; + bmx_config.gyr_range = IMU_BMX_GYRO_FULLSCALE_ENUM; + + bmx_config.acc_odr = IMU_BMX_ACC_GYRO_ODR_ENUM; + bmx_config.gyr_odr = IMU_BMX_ACC_GYRO_ODR_ENUM; + bmx_config.mag_odr = IMU_BMX_MAG_ODR_ENUM; + + bmx_config.gyr_unit = BMX160Config::GyroscopeMeasureUnit::RAD; + + imu_bmx160 = new BMX160(spi1_bus, miosix::sensors::bmx160::cs::getPin(), + bmx_config, spi_cfg); + + SensorInfo info("BMX160", SAMPLE_PERIOD_IMU_BMX, + bind(&Sensors::imuBMXCallback, this), false, true); + + sensors_map.emplace(std::make_pair(imu_bmx160, info)); + + LOG_INFO(log, "BMX160 Setup done!"); +} + +void Sensors::imuBMXWithCorrectionInit() +{ + // Read the correction parameters + BMX160CorrectionParameters correctionParameters = + BMX160WithCorrection::readCorrectionParametersFromFile( + BMX160_CORRECTION_PARAMETERS_FILE); + + // Print the calibration parameters + TRACE("[Sensors] Current accelerometer bias vector\n"); + TRACE("[Sensors] b = [ % 2.5f % 2.5f % 2.5f ]\n", + correctionParameters.accelParams(0, 1), + correctionParameters.accelParams(1, 1), + correctionParameters.accelParams(2, 1)); + TRACE("[Sensors] Matrix to be multiplied to the input vector\n"); + TRACE("[Sensors] | % 2.5f % 2.5f % 2.5f |\n", + correctionParameters.accelParams(0, 0), 0.f, 0.f); + TRACE("[Sensors] M = | % 2.5f % 2.5f % 2.5f |\n", 0.f, + correctionParameters.accelParams(1, 0), 0.f); + TRACE("[Sensors] | % 2.5f % 2.5f % 2.5f |\n\n", 0.f, 0.f, + correctionParameters.accelParams(2, 0)); + TRACE("[Sensors] Current magnetometer bias vector\n"); + TRACE("[Sensors] b = [ % 2.5f % 2.5f % 2.5f ]\n", + correctionParameters.magnetoParams(0, 1), + correctionParameters.magnetoParams(1, 1), + correctionParameters.magnetoParams(2, 1)); + TRACE("[Sensors] Matrix to be multiplied to the input vector\n"); + TRACE("[Sensors] | % 2.5f % 2.5f % 2.5f |\n", + correctionParameters.magnetoParams(0, 0), 0.f, 0.f); + TRACE("[Sensors] M = | % 2.5f % 2.5f % 2.5f |\n", 0.f, + correctionParameters.magnetoParams(1, 0), 0.f); + TRACE("[Sensors] | % 2.5f % 2.5f % 2.5f |\n\n", 0.f, 0.f, + correctionParameters.magnetoParams(2, 0)); + TRACE( + "[Sensors] The current minimun number of gyroscope samples for " + "calibration is %d\n", + correctionParameters.minGyroSamplesForCalibration); + + imu_bmx160_with_correction = new BMX160WithCorrection( + imu_bmx160, correctionParameters, BMX160_AXIS_ROTATION); + + SensorInfo info("BMX160WithCorrection", SAMPLE_PERIOD_IMU_BMX, + bind(&Sensors::imuBMXWithCorrectionCallback, this), false, + true); + + sensors_map.emplace(std::make_pair(imu_bmx160_with_correction, info)); + + LOG_INFO(log, "BMX160WithCorrection Setup done!"); +} + +void Sensors::magLISinit() +{ + SPIBusConfig busConfig; + busConfig.clock_div = SPIClockDivider::DIV32; + + LIS3MDL::Config config; + config.odr = MAG_LIS_ODR_ENUM; + config.scale = MAG_LIS_FULLSCALE; + config.temperatureDivider = 1; + + mag_lis3mdl = new LIS3MDL(spi1_bus, miosix::sensors::lis3mdl::cs::getPin(), + busConfig, config); + + SensorInfo info("LIS3MDL", SAMPLE_PERIOD_MAG_LIS, + bind(&Sensors::magLISCallback, this), false, true); + + sensors_map.emplace(std::make_pair(mag_lis3mdl, info)); + + LOG_INFO(log, "LIS3MDL Setup done!"); +} + +void Sensors::gpsUbloxInit() +{ + gps_ublox = new UbloxGPS(GPS_BAUD_RATE, GPS_SAMPLE_RATE); + + SensorInfo info("UbloxGPS", GPS_SAMPLE_PERIOD, + bind(&Sensors::gpsUbloxCallback, this), false, true); + + sensors_map.emplace(std::make_pair(gps_ublox, info)); + + LOG_INFO(log, "Ublox GPS Setup done!"); +} + +void Sensors::internalAdcCallback() +{ + //LoggerService::getInstance()->log(internal_adc->getLastSample()); +} + +void Sensors::batteryVoltageCallback() +{ + static float v = battery_voltage->getLastSample().bat_voltage; + if (v < BATTERY_MIN_SAFE_VOLTAGE) + { + battery_critical_counter++; + // every 30 seconds to avoid filling the log (if debug disabled) + if (battery_critical_counter % 30 == 0) + { + LOG_CRIT(log, "*** LOW BATTERY *** ---> Voltage = {:02f}", v); + battery_critical_counter = 0; + } + } + + //LoggerService::getInstance()->log(battery_voltage->getLastSample()); +} + +void Sensors::pressDigiCallback() +{ + //LoggerService::getInstance()->log(press_digital->getLastSample()); +} + +void Sensors::ADS1118Callback() +{ + //LoggerService::getInstance()->log(adc_ads1118->getLastSample()); +} + +void Sensors::pressPitotCallback() +{ + SSCDRRN015PDAData d = press_pitot->getLastSample(); + //LoggerService::getInstance()->log(d); + + /*ADAReferenceValues rv = + DeathStack::getInstance() + ->state_machines->ada_controller->getReferenceValues(); + + float rel_density = aeroutils::relDensity( + press_digital->getLastSample().press, rv.ref_pressure, rv.ref_altitude, + rv.ref_temperature); + if (rel_density != 0.0f) + { + float airspeed = sqrtf(2 * fabs(d.press) / rel_density); + + AirSpeedPitot aspeed_data{TimestampTimer::getTimestamp(), airspeed}; + //LoggerService::getInstance()->log(aspeed_data); + }*/ +} + +void Sensors::pressDPLVaneCallback() +{ + //LoggerService::getInstance()->log(press_dpl_vane->getLastSample()); +} + +void Sensors::pressStaticCallback() +{ + //LoggerService::getInstance()->log(press_static_port->getLastSample()); +} + +void Sensors::imuBMXCallback() +{ + uint8_t fifo_size = imu_bmx160->getLastFifoSize(); + auto& fifo = imu_bmx160->getLastFifo(); + + //LoggerService::getInstance()->log(imu_bmx160->getTemperature()); + + for (uint8_t i = 0; i < fifo_size; ++i) + { + //LoggerService::getInstance()->log(fifo.at(i)); + } + + //LoggerService::getInstance()->log(imu_bmx160->getFifoStats()); +} + +void Sensors::imuBMXWithCorrectionCallback() +{ + //LoggerService::getInstance()->log( + // imu_bmx160_with_correction->getLastSample()); +} + +void Sensors::magLISCallback() +{ + //LoggerService::getInstance()->log(mag_lis3mdl->getLastSample()); +} + +void Sensors::gpsUbloxCallback() +{ + //LoggerService::getInstance()->log(gps_ublox->getLastSample()); +} + +void Sensors::updateSensorsStatus() +{ + SensorInfo info; + + info = sensor_manager->getSensorInfo(imu_bmx160); + if (!info.is_initialized) + { + status.bmx160 = SensorDriverStatus::DRIVER_ERROR; + } + + info = sensor_manager->getSensorInfo(mag_lis3mdl); + if (!info.is_initialized) + { + status.lis3mdl = SensorDriverStatus::DRIVER_ERROR; + } + + info = sensor_manager->getSensorInfo(gps_ublox); + if (!info.is_initialized) + { + status.gps = SensorDriverStatus::DRIVER_ERROR; + } + + info = sensor_manager->getSensorInfo(internal_adc); + if (!info.is_initialized) + { + status.internal_adc = SensorDriverStatus::DRIVER_ERROR; + } + + info = sensor_manager->getSensorInfo(adc_ads1118); + if (!info.is_initialized) + { + status.ads1118 = SensorDriverStatus::DRIVER_ERROR; + } +} + +} // namespace PayloadBoard \ No newline at end of file diff --git a/src/boards/Payload/Main/Sensors.h b/src/boards/Payload/Main/Sensors.h new file mode 100644 index 000000000..4811c2c2b --- /dev/null +++ b/src/boards/Payload/Main/Sensors.h @@ -0,0 +1,129 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <Payload/Main/SensorsData.h> +#include <diagnostic/PrintLogger.h> +#include <drivers/adc/ADS1118/ADS1118.h> +#include <drivers/adc/InternalADC/InternalADC.h> +#include <drivers/gps/ublox/UbloxGPS.h> +#include <drivers/spi/SPIBusInterface.h> +#include <sensors/BMX160/BMX160.h> +#include <sensors/BMX160/BMX160WithCorrection.h> +#include <sensors/LIS3MDL/LIS3MDL.h> +#include <sensors/MS5803/MS5803.h> +#include <sensors/SensorData.h> +#include <sensors/SensorManager.h> +#include <sensors/analog/battery/BatteryVoltageSensor.h> +#include <sensors/analog/current/CurrentSensor.h> +#include <sensors/analog/pressure/MPXHZ6130A/MPXHZ6130A.h> +#include <sensors/analog/pressure/honeywell/SSCDANN030PAA.h> +#include <sensors/analog/pressure/honeywell/SSCDRRN015PDA.h> + +#include <map> + +using namespace Boardcore; + +namespace PayloadBoard +{ + +/** + * @brief Initializes all the sensors on the death stack + * + */ +class Sensors +{ +public: + SensorManager* sensor_manager = nullptr; + + InternalADC* internal_adc = nullptr; + BatteryVoltageSensor* battery_voltage = nullptr; + + MS5803* press_digital = nullptr; + + ADS1118* adc_ads1118 = nullptr; + SSCDANN030PAA* press_dpl_vane = nullptr; + MPXHZ6130A* press_static_port = nullptr; + SSCDRRN015PDA* press_pitot = nullptr; + + BMX160* imu_bmx160 = nullptr; + BMX160WithCorrection* imu_bmx160_with_correction = nullptr; + LIS3MDL* mag_lis3mdl = nullptr; + UbloxGPS* gps_ublox = nullptr; + + Sensors(SPIBusInterface& spi1_bus_, TaskScheduler* scheduler); + + ~Sensors(); + + bool start(); + + void calibrate(); + +private: + void internalAdcInit(); + void internalAdcCallback(); + + void batteryVoltageInit(); + void batteryVoltageCallback(); + + void pressDigiInit(); + void pressDigiCallback(); + + void ADS1118Init(); + void ADS1118Callback(); + + void pressPitotInit(); + void pressPitotCallback(); + + void pressDPLVaneInit(); + void pressDPLVaneCallback(); + + void pressStaticInit(); + void pressStaticCallback(); + + void imuBMXInit(); + void imuBMXCallback(); + + void imuBMXWithCorrectionInit(); + void imuBMXWithCorrectionCallback(); + + void magLISinit(); + void magLISCallback(); + + void gpsUbloxInit(); + void gpsUbloxCallback(); + + void updateSensorsStatus(); + + SPIBusInterface& spi1_bus; + + SensorManager::SensorMap_t sensors_map; + + SensorsStatus status; + + PrintLogger log = Logging::getLogger("sensors"); + + unsigned int battery_critical_counter = 0; +}; + +} // namespace PayloadBoard \ No newline at end of file diff --git a/src/boards/Payload/Main/SensorsData.h b/src/boards/Payload/Main/SensorsData.h new file mode 100644 index 000000000..c9751d386 --- /dev/null +++ b/src/boards/Payload/Main/SensorsData.h @@ -0,0 +1,70 @@ +/** + * Copyright (c) 2021 Skyward Experimental Rocketry + * Authors: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +namespace PayloadBoard +{ + +struct AirSpeedPitot +{ + uint64_t timestamp; + float airspeed; + + static std::string header() { return "timestamp,airspeed\n"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << airspeed << "\n"; + } +}; + +enum SensorDriverStatus +{ + DRIVER_ERROR = 0, + DRIVER_OK = 1 +}; + +struct SensorsStatus +{ + uint8_t bmx160 = DRIVER_OK; + uint8_t ms5803 = DRIVER_OK; + uint8_t lis3mdl = DRIVER_OK; + uint8_t gps = DRIVER_OK; + uint8_t internal_adc = DRIVER_OK; + uint8_t ads1118 = DRIVER_OK; + + static std::string header() + { + return "bmx160,ms5803,lis3mdl,gps,internal_adc,ads1118\n"; + } + + void print(std::ostream& os) const + { + os << (int)bmx160 << "," << (int)ms5803 << "," << (int)lis3mdl << "," + << (int)gps << "," << (int)internal_adc << "," << (int)ads1118 + << "\n"; + } +}; + +} // namespace PayloadBoard \ No newline at end of file diff --git a/src/boards/Payload/PayloadBoard.h b/src/boards/Payload/PayloadBoard.h new file mode 100644 index 000000000..7b32479c7 --- /dev/null +++ b/src/boards/Payload/PayloadBoard.h @@ -0,0 +1,236 @@ +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Alvise de'Faveri Tron + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <Common.h> +#include <Payload/PayloadStatus.h> +#include <Debug.h> +//#include <LoggerService/LoggerService.h> +#include <Payload/Main/Actuators.h> +#include <Payload/Main/Bus.h> +#include <Payload/Main/Radio.h> +#include <Payload/Main/Sensors.h> +//#include <Main/StateMachines.h> +//#include <PinHandler/PinHandler.h> +#include <System/StackLogger.h> +#include <System/TaskID.h> +#include <events/EventBroker.h> +#include <events/EventData.h> +#include <events/utils/EventInjector.h> +#include <events/Events.h> +#include <events/Topics.h> +#include <events/utils/EventSniffer.h> + +#include <functional> +#include <stdexcept> +#include <vector> + +using std::bind; +using namespace Boardcore; + +namespace PayloadBoard +{ + +// Add heres the events that you don't want to be TRACEd in +// Payload.logEvent() +static const std::vector<uint8_t> TRACE_EVENT_BLACKLIST{ + EV_SEND_HR_TM, EV_SEND_LR_TM, EV_SEND_TEST_TM, EV_SEND_SENS_TM}; +/** + * This file provides a simplified way to initialize and monitor all + * the components of the Payload. + */ +class Payload : public Singleton<Payload> +{ + friend class Singleton<Payload>; + +public: + // Shared Components + EventBroker* broker; + //LoggerService* logger; + + EventSniffer* sniffer; + + //StateMachines* state_machines; + Bus* bus; + Sensors* sensors; + Radio* radio; + Actuators* actuators; + + //PinHandler* pin_handler; + + TaskScheduler* scheduler; + + void start() + { + if (!broker->start()) + { + LOG_ERR(log, "Error starting EventBroker"); + status.setError(&PayloadStatus::ev_broker); + } + + /*if (!radio->start()) + { + LOG_ERR(log, "Error starting radio module"); + status.setError(&PayloadStatus::radio); + }*/ + + if (!sensors->start()) + { + LOG_ERR(log, "Error starting sensors"); + status.setError(&PayloadStatus::sensors); + } + + /*if (!state_machines->start()) + { + LOG_ERR(log, "Error starting state machines"); + status.setError(&PayloadStatus::state_machines); + } + + if (!pin_handler->start()) + { + LOG_ERR(log, "Error starting PinObserver"); + status.setError(&PayloadStatus::pin_obs); + }*/ + +#ifdef DEBUG + injector->start(); +#endif + + //logger->log(status); + + // If there was an error, signal it to the FMM and light a LED. + if (status.payload_board != COMP_OK) + { + LOG_ERR(log, "Initalization failed\n"); + sEventBroker->post(Event{EV_INIT_ERROR}, TOPIC_FLIGHT_EVENTS); + } + else + { + LOG_INFO(log, "Initalization ok"); + sEventBroker->post(Event{EV_INIT_OK}, TOPIC_FLIGHT_EVENTS); + } + } + + /*void startLogger() + { + try + { + logger->start(); + LOG_INFO(log, "Logger started"); + } + catch (const std::runtime_error& re) + { + LOG_ERR(log, "SD Logger init error"); + status.setError(&PayloadStatus::logger); + } + + logger->log(logger->getLogger().getLogStats()); + }*/ + +private: + /** + * @brief Initialize Everything. + */ + Payload() + { + /* Shared components */ + //logger = Singleton<LoggerService>::getInstance(); + //startLogger(); + + TimestampTimer::enableTimestampTimer(); + + broker = sEventBroker; + + // Bind the logEvent function to the event sniffer in order to log every + // event + /*{ + using namespace std::placeholders; + sniffer = new EventSniffer( + *broker, TOPIC_LIST, bind(&Payload::logEvent, this, _1, _2)); + }*/ + + scheduler = new TaskScheduler(); + + bus = new Bus(); + radio = new Radio(*bus->spi2); + sensors = new Sensors(*bus->spi1, scheduler); + actuators = new Actuators(); + +#ifdef DEBUG + injector = new EventInjector(); +#endif + + LOG_INFO(log, "Init finished"); + } + + /** + * @brief Helpers for debugging purposes. + */ + /*void logEvent(uint8_t event, uint8_t topic) + { + EventData ev{(long long)TimestampTimer::getTimestamp(), event, topic}; + logger->log(ev); + +#ifdef DEBUG + // Don't TRACE if event is in the blacklist to avoid cluttering the + // serial terminal + for (uint8_t bl_ev : TRACE_EVENT_BLACKLIST) + { + if (bl_ev == event) + { + return; + } + } + LOG_DEBUG(log, "{:s} on {:s}", getEventString(event), + getTopicString(topic)); +#endif + }*/ + + inline void postEvent(Event ev, uint8_t topic) { broker->post(ev, topic); } + + /*void addSchedulerStatsTask() + { + // add lambda to log scheduler tasks statistics + scheduler->add( + [&]() { + std::vector<TaskStatResult> scheduler_stats = + scheduler->getTaskStats(); + + for (TaskStatResult stat : scheduler_stats) + { + logger->log(stat); + } + + StackLogger::getInstance()->updateStack(THID_TASK_SCHEDULER); + }, + 1000, // 1 hz + TASK_SCHEDULER_STATS_ID, miosix::getTick()); + }*/ + + EventInjector* injector; + PayloadStatus status{}; + + PrintLogger log = Logging::getLogger("Payload"); +}; + +} // namespace PayloadBoard diff --git a/src/boards/Payload/PayloadStatus.h b/src/boards/Payload/PayloadStatus.h new file mode 100644 index 000000000..057b8ddee --- /dev/null +++ b/src/boards/Payload/PayloadStatus.h @@ -0,0 +1,75 @@ +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <cstdint> +#include <ostream> +#include <string> + +namespace PayloadBoard +{ + +enum PayloadComponentStatus +{ + COMP_ERROR = 0, + COMP_OK = 1 +}; + +struct PayloadStatus +{ + // Logic OR of all components errors. + uint8_t payload_board = COMP_OK; + + uint8_t logger = COMP_OK; + uint8_t ev_broker = COMP_OK; + uint8_t pin_obs = COMP_OK; + uint8_t sensors = COMP_OK; + uint8_t radio = COMP_OK; + uint8_t state_machines = COMP_OK; + + /** + * @brief Helper method to signal an error in the PayloadStatus struct. + * + * @param component_status Pointer to a member of PayloadStatus + * Eg: setError(&PayloadStatus::dpl) + */ + void setError(uint8_t PayloadStatus::*component_status) + { + this->*component_status = COMP_ERROR; + payload_board = COMP_ERROR; + } + + static std::string header() + { + return "logger,ev_broker,pin_obs,sensors,radio,state_machines\n"; + } + + void print(std::ostream& os) + { + os << (int)logger << "," << (int)ev_broker << "," << (int)pin_obs << "," + << (int)sensors << "," << (int)radio << "," << (int)state_machines + << "\n"; + } +}; + +} // namespace PayloadBoard \ No newline at end of file diff --git a/src/boards/Payload/WingControl/WingServo.cpp b/src/boards/Payload/WingControl/WingServo.cpp new file mode 100644 index 000000000..6587d27db --- /dev/null +++ b/src/boards/Payload/WingControl/WingServo.cpp @@ -0,0 +1,86 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <Payload/WingControl/WingServo.h> + +namespace PayloadBoard +{ + +using namespace WingConfigs; + +WingServo::WingServo(PWM::Timer servo_timer, PWMChannel servo_ch) + : ServoInterface(WING_SERVO_MIN_POS, WING_SERVO_MAX_POS), + servo(servo_timer), servo_channel(servo_ch) +{ +} + +WingServo::WingServo(PWM::Timer servo_timer, PWMChannel servo_ch, + float minPosition, float maxPosition) + : ServoInterface(minPosition, maxPosition), servo(servo_timer), + servo_channel(servo_ch) +{ +} + +WingServo::~WingServo() {} + +void WingServo::enable() +{ + servo.setMaxPulseWidth(2500); + servo.setMinPulseWidth(500); + servo.enable(servo_channel); + servo.start(); +} + +void WingServo::disable() +{ + servo.stop(); + servo.disable(servo_channel); +} + +void WingServo::selfTest() +{ + float base = (MAX_POS + RESET_POS) / 2; + float maxpos = base + WING_SERVO_WIGGLE_AMPLITUDE / 2; + float minpos = base - WING_SERVO_WIGGLE_AMPLITUDE / 2; + + set(base, true); + + for (int i = 0; i < 3; i++) + { + miosix::Thread::sleep(500); + set(maxpos, true); + miosix::Thread::sleep(500); + set(minpos, true); + } + + miosix::Thread::sleep(500); + reset(); +} + +void WingServo::setPosition(float angle) +{ + this->currentPosition = angle; + // map position to [0;1] interval for the servo driver + servo.setPosition(servo_channel, angle / 180.0f); +} + +} // namespace PayloadBoard \ No newline at end of file diff --git a/src/boards/Payload/WingControl/WingServo.h b/src/boards/Payload/WingControl/WingServo.h new file mode 100644 index 000000000..2c9f1893b --- /dev/null +++ b/src/boards/Payload/WingControl/WingServo.h @@ -0,0 +1,68 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <common/ServoInterface.h> +#include <Payload/configs/WingConfig.h> +#include <drivers/servo/servo.h> +#include <miosix.h> + +using namespace Boardcore; + +namespace PayloadBoard +{ + +class WingServo : public ServoInterface +{ + +public: + WingServo(PWM::Timer servo_timer, PWMChannel servo_ch); + + WingServo(PWM::Timer servo_timer, PWMChannel servo_ch, float minPosition, + float maxPosition); + + virtual ~WingServo(); + + void enable() override; + + void disable() override; + + /** + * @brief Perform wiggle around the middle point. + */ + void selfTest() override; + +private: + Servo servo; + PWMChannel servo_channel; + +protected: + /** + * @brief Set servo position. + * + * @param angle servo position (in degrees) + */ + void setPosition(float angle) override; +}; + +} // namespace PayloadBoard \ No newline at end of file diff --git a/src/boards/Payload/configs/SensorsConfig.h b/src/boards/Payload/configs/SensorsConfig.h new file mode 100644 index 000000000..e84959be0 --- /dev/null +++ b/src/boards/Payload/configs/SensorsConfig.h @@ -0,0 +1,135 @@ +/* Copyright (c) 2015-2021 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <drivers/adc/ADS1118/ADS1118.h> +#include <drivers/adc/InternalADC/InternalADC.h> +#include <interfaces-impl/hwmapping.h> +#include <sensors/BMX160/BMX160Config.h> +#include <sensors/LIS3MDL/LIS3MDL.h> +#include <sensors/calibration/Calibration.h> + +using miosix::Gpio; + +namespace PayloadBoard +{ + +namespace SensorConfigs +{ +static constexpr float INTERNAL_ADC_VREF = 3.3; +static constexpr InternalADC::Channel ADC_BATTERY_VOLTAGE = + InternalADC::Channel::CH5; + +static constexpr float BATTERY_VOLTAGE_COEFF = 5.98; +static constexpr float BATTERY_MIN_SAFE_VOLTAGE = 10.5; // [V] + +static constexpr ADS1118::ADS1118Mux ADC_CH_STATIC_PORT = ADS1118::MUX_AIN0_GND; +static constexpr ADS1118::ADS1118Mux ADC_CH_PITOT_PORT = ADS1118::MUX_AIN1_GND; +static constexpr ADS1118::ADS1118Mux ADC_CH_DPL_PORT = ADS1118::MUX_AIN2_GND; +static constexpr ADS1118::ADS1118Mux ADC_CH_VREF = ADS1118::MUX_AIN3_GND; + +static constexpr ADS1118::ADS1118DataRate ADC_DR_STATIC_PORT = ADS1118::DR_860; +static constexpr ADS1118::ADS1118DataRate ADC_DR_PITOT_PORT = ADS1118::DR_860; +static constexpr ADS1118::ADS1118DataRate ADC_DR_DPL_PORT = ADS1118::DR_860; +static constexpr ADS1118::ADS1118DataRate ADC_DR_VREF = ADS1118::DR_860; + +static constexpr ADS1118::ADS1118Pga ADC_PGA_STATIC_PORT = ADS1118::FSR_6_144; +static constexpr ADS1118::ADS1118Pga ADC_PGA_PITOT_PORT = ADS1118::FSR_6_144; +static constexpr ADS1118::ADS1118Pga ADC_PGA_DPL_PORT = ADS1118::FSR_6_144; +static constexpr ADS1118::ADS1118Pga ADC_PGA_VREF = ADS1118::FSR_6_144; + +// Sampling periods in milliseconds +static constexpr unsigned int SAMPLE_PERIOD_INTERNAL_ADC = + 1000; // only for battery voltage +static constexpr unsigned int SAMPLE_PERIOD_ADC_ADS1118 = 6; + +static constexpr unsigned int SAMPLE_PERIOD_PRESS_DIGITAL = 15; +static constexpr unsigned int TEMP_DIVIDER_PRESS_DIGITAL = 5; + +static constexpr unsigned int SAMPLE_PERIOD_PRESS_PITOT = + SAMPLE_PERIOD_ADC_ADS1118 * 4; +static constexpr unsigned int SAMPLE_PERIOD_PRESS_DPL = + SAMPLE_PERIOD_ADC_ADS1118 * 4; +static constexpr unsigned int SAMPLE_PERIOD_PRESS_STATIC = + SAMPLE_PERIOD_ADC_ADS1118 * 4; + +static constexpr unsigned int PRESS_PITOT_CALIB_SAMPLES_NUM = 500; +static constexpr unsigned int PRESS_STATIC_CALIB_SAMPLES_NUM = 500; +static constexpr float PRESS_STATIC_MOVING_AVG_COEFF = 0.95; + +static constexpr BMX160Config::AccelerometerRange IMU_BMX_ACC_FULLSCALE_ENUM = + BMX160Config::AccelerometerRange::G_16; +static constexpr BMX160Config::GyroscopeRange IMU_BMX_GYRO_FULLSCALE_ENUM = + BMX160Config::GyroscopeRange::DEG_1000; + +static constexpr unsigned int IMU_BMX_ACC_GYRO_ODR = 1600; +static constexpr BMX160Config::OutputDataRate IMU_BMX_ACC_GYRO_ODR_ENUM = + BMX160Config::OutputDataRate::HZ_1600; +static constexpr unsigned int IMU_BMX_MAG_ODR = 100; +static constexpr BMX160Config::OutputDataRate IMU_BMX_MAG_ODR_ENUM = + BMX160Config::OutputDataRate::HZ_100; + +static constexpr unsigned int IMU_BMX_FIFO_HEADER_SIZE = 1; +static constexpr unsigned int IMU_BMX_ACC_DATA_SIZE = 6; +static constexpr unsigned int IMU_BMX_GYRO_DATA_SIZE = 6; +static constexpr unsigned int IMU_BMX_MAG_DATA_SIZE = 8; + +static constexpr unsigned int IMU_BMX_FIFO_WATERMARK = 80; + +// How many bytes go into the fifo each second +static constexpr unsigned int IMU_BMX_FIFO_FILL_RATE = + IMU_BMX_ACC_GYRO_ODR * (IMU_BMX_FIFO_HEADER_SIZE + IMU_BMX_ACC_DATA_SIZE + + IMU_BMX_GYRO_DATA_SIZE) + + IMU_BMX_MAG_ODR * (IMU_BMX_MAG_DATA_SIZE + IMU_BMX_FIFO_HEADER_SIZE); + +// How long does it take for the bmx fifo to fill up +static constexpr unsigned int IMU_BMX_FIFO_FILL_TIME = + 1024 * 1000 / IMU_BMX_FIFO_FILL_RATE; + +// Sample before the fifo is full, but slightly after the watermark level +// (watermark + 30) ---> high slack due to scheduler imprecision, +// avoid clearing the fifo before the interrupt +static constexpr unsigned int SAMPLE_PERIOD_IMU_BMX = + IMU_BMX_FIFO_FILL_TIME * (IMU_BMX_FIFO_WATERMARK + 30) * 4 / 1024; + +static constexpr unsigned int IMU_BMX_TEMP_DIVIDER = 1; + +// IMU axis rotation +static const AxisOrthoOrientation BMX160_AXIS_ROTATION = { + Direction::NEGATIVE_Z, Direction::POSITIVE_Y}; + +static constexpr char BMX160_CORRECTION_PARAMETERS_FILE[30] = + "/sd/bmx160_params.csv"; + +static constexpr unsigned int SAMPLE_PERIOD_MAG_LIS = 15; +static constexpr LIS3MDL::ODR MAG_LIS_ODR_ENUM = LIS3MDL::ODR_80_HZ; +static constexpr LIS3MDL::FullScale MAG_LIS_FULLSCALE = LIS3MDL::FS_4_GAUSS; + +static constexpr unsigned int GPS_SAMPLE_RATE = 25; +static constexpr unsigned int GPS_SAMPLE_PERIOD = 1000 / GPS_SAMPLE_RATE; +static constexpr unsigned int GPS_BAUD_RATE = 460800; + +static constexpr float REFERENCE_VOLTAGE = 5.0; +} // namespace SensorConfigs + +} // namespace PayloadBoard diff --git a/src/boards/Payload/configs/WingConfig.h b/src/boards/Payload/configs/WingConfig.h new file mode 100644 index 000000000..4eb08d4b2 --- /dev/null +++ b/src/boards/Payload/configs/WingConfig.h @@ -0,0 +1,57 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <Constants.h> +#include <drivers/HardwareTimer.h> +#include <drivers/pwm/pwm.h> + +using namespace Boardcore; + +namespace PayloadBoard +{ + +namespace WingConfigs +{ + +static const PWM::Timer WING_SERVO_1_TIMER{ + TIM8, &(RCC->APB2ENR), RCC_APB2ENR_TIM8EN, + TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB2)}; + +static constexpr PWMChannel WING_SERVO_1_PWM_CH = PWMChannel::CH2; + +static const PWM::Timer WING_SERVO_2_TIMER{ + TIM8, &(RCC->APB2ENR), RCC_APB2ENR_TIM8EN, + TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB2)}; + +static constexpr PWMChannel WING_SERVO_2_PWM_CH = PWMChannel::CH2; + +// Wing servo configs +static constexpr float WING_SERVO_MAX_POS = 180.0; // deg +static constexpr float WING_SERVO_MIN_POS = 0.0; // deg + +static constexpr float WING_SERVO_WIGGLE_AMPLITUDE = 20.0; // deg + +} // namespace WingConfigs + +} // namespace PayloadBoard \ No newline at end of file diff --git a/src/boards/DeathStack/Algorithm.h b/src/common/Algorithm.h similarity index 97% rename from src/boards/DeathStack/Algorithm.h rename to src/common/Algorithm.h index b8347916f..7f8ace5e3 100644 --- a/src/boards/DeathStack/Algorithm.h +++ b/src/common/Algorithm.h @@ -22,9 +22,6 @@ #pragma once -namespace DeathStackBoard -{ - class Algorithm { public: @@ -66,5 +63,3 @@ protected: bool running = false; }; - -} // namespace DeathStackBoard diff --git a/src/boards/CanInterfaces.h b/src/common/CanInterfaces.h similarity index 100% rename from src/boards/CanInterfaces.h rename to src/common/CanInterfaces.h diff --git a/src/boards/DeathStack/ServoInterface.h b/src/common/ServoInterface.h similarity index 98% rename from src/boards/DeathStack/ServoInterface.h rename to src/common/ServoInterface.h index 4cb714017..e549b35b1 100644 --- a/src/boards/DeathStack/ServoInterface.h +++ b/src/common/ServoInterface.h @@ -22,9 +22,6 @@ #pragma once -namespace DeathStackBoard -{ - /** * @brief Class for interfacing with 180° servo motors, works in degrees. * @@ -155,5 +152,3 @@ protected: */ float currentPosition = 0; }; - -} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/System/SystemData.h b/src/common/SystemData.h similarity index 100% rename from src/boards/DeathStack/System/SystemData.h rename to src/common/SystemData.h diff --git a/src/boards/DeathStack/events/EventStrings.cpp b/src/common/events/EventStrings.cpp similarity index 100% rename from src/boards/DeathStack/events/EventStrings.cpp rename to src/common/events/EventStrings.cpp diff --git a/src/boards/DeathStack/events/Events.h b/src/common/events/Events.h similarity index 100% rename from src/boards/DeathStack/events/Events.h rename to src/common/events/Events.h diff --git a/src/boards/DeathStack/events/Topics.h b/src/common/events/Topics.h similarity index 100% rename from src/boards/DeathStack/events/Topics.h rename to src/common/events/Topics.h diff --git a/src/entrypoints/payload-entry.cpp b/src/entrypoints/payload-entry.cpp new file mode 100644 index 000000000..ecf8b8c1e --- /dev/null +++ b/src/entrypoints/payload-entry.cpp @@ -0,0 +1,75 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Erbetta + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include <Debug.h> +#include <Payload/PayloadBoard.h> +#include <SystemData.h> +#include <diagnostic/CpuMeter.h> +#include <math/Stats.h> +#include <miosix.h> + +using namespace miosix; +using namespace PayloadBoard; + +int main() +{ + PrintLogger log = Logging::getLogger("main"); +#ifndef DEBUG // if not debugging, output to file only INFO level or higher + unique_ptr<LogSink> log_sink = std::make_unique<FileLogSinkBuffered>(); + log_sink->setLevel(LOGL_INFO); + Logging::addLogSink(log_sink); +#endif + + Stats cpu_stat; + StatsResult cpu_stat_res; + SystemData system_data; + + // Instantiate the payload + Payload::getInstance()->start(); + LOG_INFO(log, "Payload started"); + + // LoggerService* logger_service = LoggerService::getInstance(); + + for (;;) + { + Thread::sleep(1000); + // logger_service->log(logger_service->getLogger().getLogStats()); + + // StackLogger::getInstance()->updateStack(THID_ENTRYPOINT); + + system_data.timestamp = miosix::getTick(); + system_data.cpu_usage = averageCpuUtilization(); + cpu_stat.add(system_data.cpu_usage); + + cpu_stat_res = cpu_stat.getStats(); + system_data.cpu_usage_min = cpu_stat_res.minValue; + system_data.cpu_usage_max = cpu_stat_res.maxValue; + system_data.cpu_usage_mean = cpu_stat_res.mean; + + system_data.min_free_heap = MemoryProfiling::getAbsoluteFreeHeap(); + system_data.free_heap = MemoryProfiling::getCurrentFreeHeap(); + + // logger_service->log(system_data); + + // StackLogger::getInstance()->log(); + } +} \ No newline at end of file -- GitLab From 2e2d1420b46d4702e8d6516a956cbe6944d4f7fe Mon Sep 17 00:00:00 2001 From: "luca.conterio" <luca.conterio@skywarder.eu> Date: Sat, 27 Nov 2021 18:33:47 +0100 Subject: [PATCH 2/6] [Payload] Added PinHandler for nosecone detach pin --- cmake/dependencies.cmake | 1 + src/boards/Payload/PayloadBoard.h | 12 +-- src/boards/Payload/PinHandler/PinHandler.cpp | 83 ++++++++++++++++++ src/boards/Payload/PinHandler/PinHandler.h | 84 +++++++++++++++++++ .../Payload/PinHandler/PinHandlerData.h | 66 +++++++++++++++ src/boards/Payload/configs/PinHandlerConfig.h | 60 +++++++++++++ 6 files changed, 301 insertions(+), 5 deletions(-) create mode 100644 src/boards/Payload/PinHandler/PinHandler.cpp create mode 100644 src/boards/Payload/PinHandler/PinHandler.h create mode 100644 src/boards/Payload/PinHandler/PinHandlerData.h create mode 100644 src/boards/Payload/configs/PinHandlerConfig.h diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake index 9b75f9864..d067614da 100644 --- a/cmake/dependencies.cmake +++ b/cmake/dependencies.cmake @@ -64,6 +64,7 @@ set(PAYLOAD_SOURCES src/boards/Payload/WingControl/WingServo.cpp src/boards/Payload/Main/Sensors.cpp src/boards/Payload/Main/Radio.cpp + src/boards/Payload/PinHandler/PinHandler.cpp ) set(ADA_SOURCES src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.cpp diff --git a/src/boards/Payload/PayloadBoard.h b/src/boards/Payload/PayloadBoard.h index 7b32479c7..c01728f44 100644 --- a/src/boards/Payload/PayloadBoard.h +++ b/src/boards/Payload/PayloadBoard.h @@ -30,8 +30,8 @@ #include <Payload/Main/Bus.h> #include <Payload/Main/Radio.h> #include <Payload/Main/Sensors.h> -//#include <Main/StateMachines.h> -//#include <PinHandler/PinHandler.h> +//#include <Payload/Main/StateMachines.h> +#include <Payload/PinHandler/PinHandler.h> #include <System/StackLogger.h> #include <System/TaskID.h> #include <events/EventBroker.h> @@ -76,7 +76,7 @@ public: Radio* radio; Actuators* actuators; - //PinHandler* pin_handler; + PinHandler* pin_handler; TaskScheduler* scheduler; @@ -104,13 +104,13 @@ public: { LOG_ERR(log, "Error starting state machines"); status.setError(&PayloadStatus::state_machines); - } + }*/ if (!pin_handler->start()) { LOG_ERR(log, "Error starting PinObserver"); status.setError(&PayloadStatus::pin_obs); - }*/ + } #ifdef DEBUG injector->start(); @@ -176,6 +176,8 @@ private: sensors = new Sensors(*bus->spi1, scheduler); actuators = new Actuators(); + pin_handler = new PinHandler(); + #ifdef DEBUG injector = new EventInjector(); #endif diff --git a/src/boards/Payload/PinHandler/PinHandler.cpp b/src/boards/Payload/PinHandler/PinHandler.cpp new file mode 100644 index 000000000..165b0a87a --- /dev/null +++ b/src/boards/Payload/PinHandler/PinHandler.cpp @@ -0,0 +1,83 @@ +/* Copyright (c) 2019-2021 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +//#include <LoggerService/LoggerService.h> +#include <Payload/PinHandler/PinHandler.h> +#include <diagnostic/PrintLogger.h> +#include <events/EventBroker.h> +#include <events/Events.h> +//#include <Payload/PayloadBoard.h> + +#include <functional> + +using std::bind; +using namespace Boardcore; + +namespace PayloadBoard +{ + +PinHandler::PinHandler() + : pin_obs(PIN_POLL_INTERVAL) //, logger(LoggerService::getInstance()) +{ + // Used for _1, _2. See std::bind cpp reference + using namespace std::placeholders; + + // Noseconse pin callbacks registration + PinObserver::OnTransitionCallback nc_transition_cb = + bind(&PinHandler::onNCPinTransition, this, _1, _2); + + PinObserver::OnStateChangeCallback nc_statechange_cb = + bind(&PinHandler::onNCPinStateChange, this, _1, _2, _3); + + pin_obs.observePin(nosecone_pin.getPort(), nosecone_pin.getNumber(), + TRIGGER_NC_DETACH_PIN, nc_transition_cb, + THRESHOLD_NC_DETACH_PIN, nc_statechange_cb); +} + +void PinHandler::onNCPinTransition(unsigned int p, unsigned char n) +{ + UNUSED(p); + UNUSED(n); + sEventBroker->post(Event{EV_NC_DETACHED}, TOPIC_FLIGHT_EVENTS); + + LOG_INFO(log, "Nosecone detached!"); + + status_pin_nosecone.last_detection_time = TimestampTimer::getTimestamp(); + //logger->log(status_pin_nosecone); +} + +void PinHandler::onNCPinStateChange(unsigned int p, unsigned char n, int state) +{ + UNUSED(p); + UNUSED(n); + + status_pin_nosecone.state = (uint8_t)state; + status_pin_nosecone.last_state_change = TimestampTimer::getTimestamp(); + status_pin_nosecone.num_state_changes += 1; + + LOG_INFO(log, "Nosecone pin state change at time {}: new state = {}", + status_pin_nosecone.last_state_change, status_pin_nosecone.state); + + //logger->log(status_pin_nosecone); +} + +} // namespace PayloadBoard \ No newline at end of file diff --git a/src/boards/Payload/PinHandler/PinHandler.h b/src/boards/Payload/PinHandler/PinHandler.h new file mode 100644 index 000000000..fdc081e0b --- /dev/null +++ b/src/boards/Payload/PinHandler/PinHandler.h @@ -0,0 +1,84 @@ +/* Copyright (c) 2019-2021 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <Payload/PinHandler/PinHandlerData.h> +#include <Payload/configs/PinHandlerConfig.h> +#include <diagnostic/PrintLogger.h> +#include <utils/PinObserver.h> + +using namespace Boardcore; + +namespace PayloadBoard +{ + +/** + * @brief Forward dec. + */ +//class LoggerService; + +/** + * @brief This class contains the handlers for both the launch pin (umbilical) + * and the nosecone detachment pin. + * + * It uses boardcore's PinObserver to bind these functions to the GPIO pins. + * The handlers post an event on the EventBroker. + */ +class PinHandler +{ +public: + PinHandler(); + + /** + * @brief Starts the pin observer. + * + */ + bool start() { return pin_obs.start(); } + + /** + * @brief Stops the pin observer. + * + */ + void stop() { pin_obs.stop(); } + + /** + * @brief Function called by the pinobserver when a nosecone pin detachment + * is detected. + * + * @param p + * @param n + */ + void onNCPinTransition(unsigned int p, unsigned char n); + + void onNCPinStateChange(unsigned int p, unsigned char n, int state); + +private: + PinStatus status_pin_nosecone{ObservedPin::NOSECONE}; + + PinObserver pin_obs; + + //LoggerService* logger; + PrintLogger log = Logging::getLogger("deathstack.pinhandler"); +}; + +} // namespace PayloadBoard \ No newline at end of file diff --git a/src/boards/Payload/PinHandler/PinHandlerData.h b/src/boards/Payload/PinHandler/PinHandlerData.h new file mode 100644 index 000000000..e3c0328df --- /dev/null +++ b/src/boards/Payload/PinHandler/PinHandlerData.h @@ -0,0 +1,66 @@ +/* Copyright (c) 2019-2021 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <cstdint> +#include <ostream> + +namespace PayloadBoard +{ + +enum class ObservedPin : uint8_t +{ + NOSECONE = 0 +}; + +/** + * @brief Struct represeting the status of an observed pin. + * + */ +struct PinStatus +{ + ObservedPin pin; + + uint64_t last_state_change = 0; // Last time the pin changed state + uint8_t state = 0; // Current state of the pin + unsigned int num_state_changes = 0; + + uint64_t last_detection_time = 0; // When a transition is detected + + PinStatus(){}; + PinStatus(ObservedPin pin) : pin(pin) {} + + static std::string header() + { + return "pin,last_state_change,state,num_state_changes,last_detection_" + "time\n"; + } + + void print(std::ostream& os) const + { + os << (int)pin << "," << last_state_change << "," << (int)state << "," + << num_state_changes << "," << last_detection_time << "\n"; + } +}; + +} // namespace PayloadBoard diff --git a/src/boards/Payload/configs/PinHandlerConfig.h b/src/boards/Payload/configs/PinHandlerConfig.h new file mode 100644 index 000000000..76588cbbb --- /dev/null +++ b/src/boards/Payload/configs/PinHandlerConfig.h @@ -0,0 +1,60 @@ +/* Copyright (c) 2019-2021 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#pragma once + +#include <interfaces-impl/hwmapping.h> +#include <miosix.h> +#include <utils/PinObserver.h> + +using namespace Boardcore; + +namespace PayloadBoard +{ + +static const unsigned int PIN_POLL_INTERVAL = 10; // ms + +// // Launch pin config +// static const GpioPin launch_pin(miosix::inputs::lp_dtch::getPin()); +// static const PinObserver::Transition TRIGGER_LAUNCH_PIN = +// PinObserver::Transition::FALLING_EDGE; +// // How many consecutive times the launch pin should be detected as detached +// // before triggering a launch event. +// static const unsigned int THRESHOLD_LAUNCH_PIN = 10; + +// Nosecone detach pin config +static const GpioPin nosecone_pin(miosix::inputs::nc_dtch::getPin()); +static const PinObserver::Transition TRIGGER_NC_DETACH_PIN = + PinObserver::Transition::FALLING_EDGE; +// How many consecutive times the nosecone pin should be detected as detached +// before triggering a nosecone detach event. +static const unsigned int THRESHOLD_NC_DETACH_PIN = 10; + +// // Dpl servo actuation pin config +// static const GpioPin dpl_servo_pin(miosix::inputs::expulsion_in::getPin()); +// static const PinObserver::Transition TRIGGER_DPL_SERVO_PIN = +// PinObserver::Transition::RISING_EDGE; +// // How many consecutive times the deployment servo pin should be detected as +// // detached before triggering a nosecone detach event. +// static const unsigned int THRESHOLD_DPL_SERVO_PIN = 10; + +} // namespace PayloadBoard \ No newline at end of file -- GitLab From 539b302ce134acad8f5d5723b8a34b169743ede5 Mon Sep 17 00:00:00 2001 From: Alberto Nidasio <alberto.nidasio@skywarder.eu> Date: Tue, 1 Feb 2022 00:33:04 +0100 Subject: [PATCH 3/6] Updated boardcore submodule and everything along with it --- .vscode/c_cpp_properties.json | 12 +- skyward-boardcore | 2 +- .../DeathStack/AirBrakes/AirBrakesAlgorithm.h | 14 +- .../AirBrakes/AirBrakesController.h | 32 +-- .../DeathStack/AirBrakes/AirBrakesServo.cpp | 17 +- .../DeathStack/AirBrakes/AirBrakesServo.h | 5 +- .../ApogeeDetectionAlgorithm/ADAAlgorithm.cpp | 7 +- .../ApogeeDetectionAlgorithm/ADACalibrator.h | 2 - .../ApogeeDetectionAlgorithm/ADAController.h | 73 +++--- .../ApogeeDetectionAlgorithm/ADAData.h | 3 +- src/boards/DeathStack/DeathStack.h | 44 ++-- .../Deployment/DeploymentController.cpp | 47 ++-- .../Deployment/DeploymentController.h | 2 +- .../DeathStack/Deployment/DeploymentData.h | 1 - .../DeathStack/Deployment/DeploymentServo.h | 20 +- .../FlightModeManager/FMMController.cpp | 136 +++++------ .../FlightStatsRecorder/FSRController.cpp | 96 ++++---- .../FlightStatsRecorder/FSRController.h | 11 +- .../DeathStack/LoggerService/LoggerService.h | 4 +- src/boards/DeathStack/Main/Radio.cpp | 18 +- src/boards/DeathStack/Main/Sensors.cpp | 159 +++++++------ src/boards/DeathStack/Main/Sensors.h | 7 +- src/boards/DeathStack/Main/StateMachines.cpp | 18 +- src/boards/DeathStack/Main/StateMachines.h | 4 +- .../NavigationAttitudeSystem/InitStates.h | 1 - .../DeathStack/NavigationAttitudeSystem/NAS.h | 46 ++-- .../NavigationAttitudeSystem/NASCalibrator.h | 6 +- .../NavigationAttitudeSystem/NASController.h | 54 ++--- .../DeathStack/PinHandler/PinHandler.cpp | 36 +-- .../TelemetriesTelecommands/TCHandler.cpp | 28 ++- .../TelemetriesTelecommands/TCHandler.h | 2 +- .../TMTCController.cpp | 72 +++--- .../TelemetriesTelecommands/TMTCController.h | 2 +- .../TelemetriesTelecommands/TmRepository.cpp | 211 +++++++++--------- .../TelemetriesTelecommands/TmRepository.h | 16 +- src/boards/DeathStack/configs/ADAConfig.h | 12 +- .../DeathStack/configs/AirBrakesConfig.h | 17 +- src/boards/DeathStack/configs/CutterConfig.h | 3 +- .../DeathStack/configs/DeploymentConfig.h | 12 +- src/boards/DeathStack/configs/NASConfig.h | 3 +- src/boards/DeathStack/configs/SensorsConfig.h | 4 +- src/boards/Payload/Main/Radio.cpp | 32 +-- src/boards/Payload/Main/Sensors.cpp | 126 +++++------ src/boards/Payload/Main/Sensors.h | 6 +- src/boards/Payload/PayloadBoard.h | 34 ++- src/boards/Payload/PinHandler/PinHandler.cpp | 17 +- src/boards/Payload/WingControl/WingServo.cpp | 28 +-- src/boards/Payload/WingControl/WingServo.h | 12 +- src/boards/Payload/configs/SensorsConfig.h | 5 +- src/boards/Payload/configs/WingConfig.h | 22 +- .../death-stack-x-decoder/LogTypes.h | 18 +- src/entrypoints/death-stack-x-entry.cpp | 15 +- src/entrypoints/death-stack-x-testsuite.cpp | 11 +- .../hardware_in_the_loop/hil-entry.cpp | 19 +- src/entrypoints/payload-entry.cpp | 9 +- src/entrypoints/windtunnel-entry.cpp | 6 +- .../windtunnel-test-decoder/LogTypes.h | 4 +- .../HILFlightPhasesManager.h | 19 +- .../HIL_sensors/HILBarometer.h | 2 +- .../HIL_sensors/HILSensor.h | 4 +- .../simulator_communication/SerialInterface.h | 1 - src/mocksensors/MockGPS.h | 36 +-- src/mocksensors/MockIMU.h | 36 +-- src/mocksensors/MockPressureSensor.h | 14 +- src/mocksensors/MockSensorsData.h | 22 +- src/mocksensors/MockSpeedSensor.h | 4 +- .../lynx_flight_data/lynx_airspeed_data.h | 2 - .../lynx_flight_data/lynx_gps_data.h | 2 +- .../mock_data/test-mock-sensors.cpp | 5 +- .../airbrakes/test-airbrakes-interactive.cpp | 10 +- .../ada/ada_kalman_p/test-ada-simulation.cpp | 25 +-- .../catch/ada/kalman_acc/test-kalman-acc.cpp | 3 +- src/tests/catch/ada/test-rogallo-dts.cpp | 2 +- src/tests/catch/catch-tests-entry.cpp | 4 +- src/tests/catch/fsm/test-ada.cpp | 9 +- src/tests/catch/fsm/test-airbrakes.cpp | 6 +- src/tests/catch/fsm/test-deployment.cpp | 8 +- .../catch/fsm/test-flightstatsrecorder.cpp | 9 +- src/tests/catch/fsm/test-fmm.cpp | 9 +- src/tests/catch/fsm/test-ignition.cpp | 14 +- src/tests/catch/fsm/test-nas.cpp | 7 +- src/tests/catch/fsm/test-tmtc.cpp | 9 +- src/tests/catch/nas/test-nas-simulation.cpp | 33 +-- .../deathstack-boards/test-analog-board.cpp | 35 ++- .../deathstack-boards/test-power-board.cpp | 15 +- src/tests/deathstack-boards/test-rf-board.cpp | 28 +-- .../deathstack-boards/test-stm-board.cpp | 4 - .../test-deployment-interactive.cpp | 12 +- src/tests/drivers/test-cutter.cpp | 44 ++-- src/tests/drivers/test-imus.cpp | 1 - src/tests/drivers/test-mavlink.cpp | 1 - src/tests/drivers/test-servo.cpp | 15 +- src/tests/eigen-test.cpp | 1 - .../test-HIL+ADA+Aerobrake.cpp | 7 +- .../test-HIL+ADA+AerobrakeController+nas.cpp | 7 +- .../test-HIL+ADA/test-HIL+ADA.cpp | 9 +- .../test-HIL+Aerobrake/test-HIL+Aerobrake.cpp | 3 - .../test-HIL/test-HIL.cpp | 3 - .../test-SerialInterface.cpp | 2 - src/tests/mock_sensors/test-mock-sensors.cpp | 5 +- src/tests/ram_test/ramtest.cpp | 94 ++++---- src/tests/test-ada-dpl-simulation.cpp | 17 +- src/tests/test-fmm-interactive.cpp | 8 +- src/tests/test-kalman-hitl.cpp | 13 +- src/tests/test-logproxy.cpp | 1 - src/tests/test-pinhandler.cpp | 3 - src/tests/test-sensormanager.cpp | 7 +- src/tests/test-sm+tmtc.cpp | 11 +- src/tests/test-tmtc.cpp | 6 +- 109 files changed, 1083 insertions(+), 1187 deletions(-) diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 3e76f3ac8..dbed30c87 100755 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -21,14 +21,16 @@ "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common", "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/arch/common", "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix", - "${workspaceFolder}/skyward-boardcore/libs/simple-template-matrix", + "${workspaceFolder}/skyward-boardcore/libs/mavlink_skyward_lib", "${workspaceFolder}/skyward-boardcore/libs/fmt/include", "${workspaceFolder}/skyward-boardcore/libs/eigen", "${workspaceFolder}/skyward-boardcore/libs", "${workspaceFolder}/skyward-boardcore/src/shared", "${workspaceFolder}/skyward-boardcore/src/tests", "${workspaceFolder}/src/boards/DeathStack", - "${workspaceFolder}/src/boards" + "${workspaceFolder}/src/boards", + "${workspaceFolder}/src/common", + "${workspaceFolder}/src" ], "browse": { "path": [ @@ -43,7 +45,6 @@ "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/util", "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/e20", "${workspaceFolder}/skyward-boardcore/libs/miosix-kernel/miosix/*", - "${workspaceFolder}/skyward-boardcore/libs/simple-template-matrix", "${workspaceFolder}/skyward-boardcore/libs/mavlink_skyward_lib", "${workspaceFolder}/skyward-boardcore/libs/eigen", "${workspaceFolder}/skyward-boardcore/libs/tscpp", @@ -51,8 +52,11 @@ "${workspaceFolder}/skyward-boardcore/libs/fmt", "${workspaceFolder}/skyward-boardcore/src/shared", "${workspaceFolder}/skyward-boardcore/src/tests", + "${workspaceFolder}/src/boards/DeathStack", "${workspaceFolder}/src/boards", - "${workspaceFolder}/src/tests" + "${workspaceFolder}/src/tests", + "${workspaceFolder}/src/common", + "${workspaceFolder}/src" ], "limitSymbolsToIncludedHeaders": true }, diff --git a/skyward-boardcore b/skyward-boardcore index 3a951420e..5fc8d0141 160000 --- a/skyward-boardcore +++ b/skyward-boardcore @@ -1 +1 @@ -Subproject commit 3a951420eb4de92f6c5b45cf65abe12869ad585a +Subproject commit 5fc8d0141d1881cf7dd690a5308c336166c3118c diff --git a/src/boards/DeathStack/AirBrakes/AirBrakesAlgorithm.h b/src/boards/DeathStack/AirBrakes/AirBrakesAlgorithm.h index 853292ae1..8f85fdfaa 100644 --- a/src/boards/DeathStack/AirBrakes/AirBrakesAlgorithm.h +++ b/src/boards/DeathStack/AirBrakes/AirBrakesAlgorithm.h @@ -28,9 +28,9 @@ #include <Algorithm.h> #include <LoggerService/LoggerService.h> #include <ServoInterface.h> -#include <TimestampTimer.h> #include <configs/AirBrakesConfig.h> #include <diagnostic/PrintLogger.h> +#include <drivers/timer/TimestampTimer.h> #include <sensors/Sensor.h> #include <algorithm> @@ -89,7 +89,7 @@ public: #ifdef HARDWARE_IN_THE_LOOP else { - HIL::getInstance()->send(0.0); + HIL::getInstance().send(0.0); } #endif } @@ -212,8 +212,8 @@ public: void logAirbrakesData(uint64_t t); private: - int indexMinVal = 0; - float alpha = 0; + int indexMinVal = 0; + float alpha = 0; uint64_t last_input_ts = 0; uint64_t begin_ts = 0; @@ -239,7 +239,7 @@ template <class T> AirBrakesControlAlgorithm<T>::AirBrakesControlAlgorithm( Sensor<T>& sensor, ServoInterface* actuator) : actuator(actuator), sensor(sensor), pid(Kp, Ki), - logger(*(LoggerService::getInstance())) + logger(LoggerService::getInstance()) { } @@ -253,7 +253,7 @@ void AirBrakesControlAlgorithm<T>::begin() running = true; - begin_ts = TimestampTimer::getTimestamp(); + begin_ts = TimestampTimer::getInstance().getTimestamp(); last_input_ts = (sensor.getLastSample()).timestamp; @@ -275,7 +275,7 @@ void AirBrakesControlAlgorithm<T>::step() alpha = computeAlpha(input, false); } - uint64_t curr_ts = TimestampTimer::getTimestamp(); + uint64_t curr_ts = TimestampTimer::getInstance().getTimestamp(); #ifdef EUROC if (curr_ts - begin_ts < AIRBRAKES_ACTIVATION_AFTER_SHADOW_MODE * 1000) diff --git a/src/boards/DeathStack/AirBrakes/AirBrakesController.h b/src/boards/DeathStack/AirBrakes/AirBrakesController.h index 281e1644e..3ddb7aa73 100644 --- a/src/boards/DeathStack/AirBrakes/AirBrakesController.h +++ b/src/boards/DeathStack/AirBrakes/AirBrakesController.h @@ -26,9 +26,9 @@ #include <AirBrakes/AirBrakesData.h> #include <AirBrakes/AirBrakesServo.h> #include <System/StackLogger.h> -#include <TimestampTimer.h> #include <configs/AirBrakesConfig.h> #include <diagnostic/PrintLogger.h> +#include <drivers/timer/TimestampTimer.h> #include <events/EventBroker.h> #include <events/Events.h> #include <events/FSM.h> @@ -95,14 +95,14 @@ AirBrakesController<T>::AirBrakesController(Sensor<T>& sensor, servo(servo), algorithm(sensor, servo) { memset(&status, 0, sizeof(AirBrakesControllerStatus)); - sEventBroker->subscribe(this, TOPIC_FLIGHT_EVENTS); - sEventBroker->subscribe(this, TOPIC_ABK); + sEventBroker.subscribe(this, TOPIC_FLIGHT_EVENTS); + sEventBroker.subscribe(this, TOPIC_ABK); } template <class T> AirBrakesController<T>::~AirBrakesController() { - sEventBroker->unsubscribe(this); + sEventBroker.unsubscribe(this); } template <class T> @@ -114,7 +114,7 @@ void AirBrakesController<T>::update() template <class T> void AirBrakesController<T>::state_initialization(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -141,7 +141,7 @@ void AirBrakesController<T>::state_initialization(const Event& ev) template <class T> void AirBrakesController<T>::state_idle(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -185,12 +185,12 @@ void AirBrakesController<T>::state_idle(const Event& ev) template <class T> void AirBrakesController<T>::state_shadowMode(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { ev_shadow_mode_timeout_id = - sEventBroker->postDelayed<SHADOW_MODE_DURATION>( + sEventBroker.postDelayed<SHADOW_MODE_DURATION>( Event{EV_SHADOW_MODE_TIMEOUT}, TOPIC_ABK); logStatus(AirBrakesControllerState::SHADOW_MODE); @@ -223,7 +223,7 @@ void AirBrakesController<T>::state_shadowMode(const Event& ev) template <class T> void AirBrakesController<T>::state_enabled(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -259,7 +259,7 @@ void AirBrakesController<T>::state_enabled(const Event& ev) template <class T> void AirBrakesController<T>::state_end(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -287,7 +287,7 @@ void AirBrakesController<T>::state_end(const Event& ev) template <class T> void AirBrakesController<T>::state_disabled(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -315,7 +315,7 @@ void AirBrakesController<T>::state_disabled(const Event& ev) template <class T> void AirBrakesController<T>::state_testAirbrakes(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -329,7 +329,7 @@ void AirBrakesController<T>::state_testAirbrakes(const Event& ev) logStatus(AirBrakesControllerState::TEST_AEROBRAKES); - sEventBroker->post(Event{EV_TEST_TIMEOUT}, TOPIC_ABK); + sEventBroker.post(Event{EV_TEST_TIMEOUT}, TOPIC_ABK); break; } case EV_EXIT: @@ -392,12 +392,12 @@ void AirBrakesController<T>::incrementallyClose() template <class T> void AirBrakesController<T>::logStatus(AirBrakesControllerState state) { - status.timestamp = TimestampTimer::getTimestamp(); + status.timestamp = TimestampTimer::getInstance().getTimestamp(); status.state = state; - LoggerService::getInstance()->log(status); + LoggerService::getInstance().log(status); - StackLogger::getInstance()->updateStack(THID_ABK_FSM); + StackLogger::getInstance().updateStack(THID_ABK_FSM); } template <class T> diff --git a/src/boards/DeathStack/AirBrakes/AirBrakesServo.cpp b/src/boards/DeathStack/AirBrakes/AirBrakesServo.cpp index cfd5cb472..b6a2c35cc 100644 --- a/src/boards/DeathStack/AirBrakes/AirBrakesServo.cpp +++ b/src/boards/DeathStack/AirBrakes/AirBrakesServo.cpp @@ -43,19 +43,9 @@ AirBrakesServo::AirBrakesServo(float minPosition, float maxPosition, AirBrakesServo::~AirBrakesServo() {} -void AirBrakesServo::enable() -{ - servo.setMaxPulseWidth(2500); - servo.setMinPulseWidth(500); - servo.enable(AB_SERVO_PWM_CH); - servo.start(); -} +void AirBrakesServo::enable() { servo.enable(); } -void AirBrakesServo::disable() -{ - servo.stop(); - servo.disable(AB_SERVO_PWM_CH); -} +void AirBrakesServo::disable() { servo.disable(); } void AirBrakesServo::selfTest() { @@ -80,8 +70,7 @@ void AirBrakesServo::selfTest() void AirBrakesServo::setPosition(float angle) { currentPosition = angle; - // map position to [0;1] interval for the servo driver - servo.setPosition(AirBrakesConfigs::AB_SERVO_PWM_CH, angle / 180.0f); + servo.setPosition180Deg(angle); #ifdef HARDWARE_IN_THE_LOOP simulator->send(angle); diff --git a/src/boards/DeathStack/AirBrakes/AirBrakesServo.h b/src/boards/DeathStack/AirBrakes/AirBrakesServo.h index 7332d38d3..8551324c3 100644 --- a/src/boards/DeathStack/AirBrakes/AirBrakesServo.h +++ b/src/boards/DeathStack/AirBrakes/AirBrakesServo.h @@ -26,7 +26,7 @@ #include <LoggerService/LoggerService.h> #include <common/ServoInterface.h> #include <configs/AirBrakesConfig.h> -#include <drivers/servo/servo.h> +#include <drivers/servo/Servo.h> #include <miosix.h> #ifdef HARDWARE_IN_THE_LOOP @@ -59,7 +59,8 @@ public: void selfTest() override; private: - Servo servo{AirBrakesConfigs::AB_SERVO_TIMER}; + Servo servo{AirBrakesConfigs::AB_SERVO_TIMER, AB_SERVO_PWM_CH, 50, 500, + 2500}; #ifdef HARDWARE_IN_THE_LOOP HIL *simulator = HIL::getInstance(); diff --git a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.cpp b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.cpp index 0edebf458..126834ac6 100644 --- a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.cpp +++ b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.cpp @@ -21,10 +21,9 @@ */ #include <ApogeeDetectionAlgorithm/ADAAlgorithm.h> -#include <Debug.h> -#include <TimestampTimer.h> #include <configs/ADAConfig.h> #include <diagnostic/CpuMeter.h> +#include <drivers/timer/TimestampTimer.h> #include <events/EventBroker.h> #include <events/Events.h> #include <utils/aero/AeroUtils.h> @@ -52,7 +51,7 @@ void ADAAlgorithm::updateBaro(float pressure) updatePressureKalman(pressure); // Convert filter data to altitudes & speeds - ada_data.timestamp = TimestampTimer::getTimestamp(); + ada_data.timestamp = TimestampTimer::getInstance().getTimestamp(); ada_data.msl_altitude = pressureToAltitude(filter.getState()(0, 0)); ada_data.agl_altitude = altitudeMSLtoAGL(ada_data.msl_altitude); ada_data.vert_speed = aeroutils::verticalSpeed( @@ -103,7 +102,7 @@ float ADAAlgorithm::altitudeMSLtoAGL(float altitude_msl) const ADAKalmanState ADAAlgorithm::getKalmanState() { ADAKalmanState state; - state.timestamp = TimestampTimer::getTimestamp(); + state.timestamp = TimestampTimer::getInstance().getTimestamp(); state.x0 = filter.getState()(0, 0); state.x1 = filter.getState()(1, 0); diff --git a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.h b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.h index 2f03b0a4c..a963aaeaa 100644 --- a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.h +++ b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.h @@ -23,8 +23,6 @@ #pragma once #include <ApogeeDetectionAlgorithm/ADAData.h> -#include <Common.h> -#include <Debug.h> #include <diagnostic/PrintLogger.h> #include <math/Stats.h> #include <miosix.h> diff --git a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAController.h b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAController.h index d70a3b4a6..d1f7aa655 100644 --- a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAController.h +++ b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAController.h @@ -29,6 +29,7 @@ #include <System/StackLogger.h> #include <configs/ADAConfig.h> #include <diagnostic/PrintLogger.h> +#include <drivers/timer/TimestampTimer.h> #include <events/EventBroker.h> #include <events/Events.h> #include <events/FSM.h> @@ -221,7 +222,7 @@ private: unsigned int n_samples_abk_disable_detected = 0; // Number of consecutive samples for abk disable - LoggerService& logger = *(LoggerService::getInstance()); // Logger + LoggerService& logger = LoggerService::getInstance(); // Logger PrintLogger log = Logging::getLogger("deathstack.fms.ada"); }; @@ -232,8 +233,8 @@ ADAController<Press, GPS>::ADAController(Sensor<Press>& barometer, ada(ADAReferenceValues{}), barometer(barometer), gps(gps) { // Subscribe to topics - sEventBroker->subscribe(this, TOPIC_FLIGHT_EVENTS); - sEventBroker->subscribe(this, TOPIC_ADA); + sEventBroker.subscribe(this, TOPIC_FLIGHT_EVENTS); + sEventBroker.subscribe(this, TOPIC_ADA); status.state = ADAState::IDLE; } @@ -248,9 +249,9 @@ void ADAController<Press, GPS>::update() { // if new gps data available, update GPS, regardless of the current state GPS gps_data = gps.getLastSample(); - if (gps_data.gps_timestamp != last_gps_timestamp) + if (gps_data.gpsTimestamp != last_gps_timestamp) { - last_gps_timestamp = gps_data.gps_timestamp; + last_gps_timestamp = gps_data.gpsTimestamp; ada.updateGPS(gps_data.latitude, gps_data.longitude, gps_data.fix); } @@ -258,11 +259,11 @@ void ADAController<Press, GPS>::update() // if new pressure data available, update baro, according to current state Press press_data = barometer.getLastSample(); - if (press_data.press_timestamp != last_press_timestamp) + if (press_data.pressureTimestamp != last_press_timestamp) { - last_press_timestamp = press_data.press_timestamp; + last_press_timestamp = press_data.pressureTimestamp; - updateBaroAccordingToState(press_data.press); + updateBaroAccordingToState(press_data.pressure); } } @@ -303,7 +304,7 @@ void ADAController<Press, GPS>::updateBaroAccordingToState(float pressure) // Log the altitude & vertical speed but don't use kalman pressure // while we are on the ramp ADAData d; - d.timestamp = TimestampTimer::getTimestamp(); + d.timestamp = TimestampTimer::getInstance().getTimestamp(); d.msl_altitude = ada.pressureToAltitude(pressure); d.agl_altitude = ada.altitudeMSLtoAGL(d.msl_altitude); d.vert_speed = 0; @@ -321,8 +322,8 @@ void ADAController<Press, GPS>::updateBaroAccordingToState(float pressure) if (ada.getVerticalSpeed() < APOGEE_VERTICAL_SPEED_TARGET) { // Log - ApogeeDetected apogee{TimestampTimer::getTimestamp(), - status.state}; + ApogeeDetected apogee{ + TimestampTimer::getInstance().getTimestamp(), status.state}; logger.log(apogee); } @@ -341,13 +342,13 @@ void ADAController<Press, GPS>::updateBaroAccordingToState(float pressure) if (++n_samples_apogee_detected >= APOGEE_N_SAMPLES) { // Active state send notifications for apogee - sEventBroker->post({EV_ADA_APOGEE_DETECTED}, TOPIC_ADA); + sEventBroker.post({EV_ADA_APOGEE_DETECTED}, TOPIC_ADA); status.apogee_reached = true; } // Log - ApogeeDetected apogee{TimestampTimer::getTimestamp(), - status.state}; + ApogeeDetected apogee{ + TimestampTimer::getInstance().getTimestamp(), status.state}; logger.log(apogee); } else if (n_samples_apogee_detected != 0) @@ -361,8 +362,8 @@ void ADAController<Press, GPS>::updateBaroAccordingToState(float pressure) if (++n_samples_abk_disable_detected >= ABK_DISABLE_N_SAMPLES) { // Active state send notifications for disabling airbrakes - sEventBroker->post({EV_ADA_DISABLE_ABK}, - TOPIC_FLIGHT_EVENTS); + sEventBroker.post({EV_ADA_DISABLE_ABK}, + TOPIC_FLIGHT_EVENTS); status.disable_airbrakes = true; } } @@ -385,8 +386,8 @@ void ADAController<Press, GPS>::updateBaroAccordingToState(float pressure) { if (++n_samples_deployment_detected >= DEPLOYMENT_N_SAMPLES) { - logger.log( - DplAltitudeReached{TimestampTimer::getTimestamp()}); + logger.log(DplAltitudeReached{ + TimestampTimer::getInstance().getTimestamp()}); } } else if (n_samples_deployment_detected != 0) @@ -407,10 +408,10 @@ void ADAController<Press, GPS>::updateBaroAccordingToState(float pressure) { if (++n_samples_deployment_detected >= DEPLOYMENT_N_SAMPLES) { - logger.log( - DplAltitudeReached{TimestampTimer::getTimestamp()}); + logger.log(DplAltitudeReached{ + TimestampTimer::getInstance().getTimestamp()}); - sEventBroker->post({EV_ADA_DPL_ALT_DETECTED}, TOPIC_ADA); + sEventBroker.post({EV_ADA_DPL_ALT_DETECTED}, TOPIC_ADA); } } else if (n_samples_deployment_detected != 0) @@ -503,7 +504,7 @@ void ADAController<Press, GPS>::finalizeCalibration() LOG_INFO(log, "Finalized calibration"); // ADA READY! - sEventBroker->post({EV_ADA_READY}, TOPIC_ADA); + sEventBroker.post({EV_ADA_READY}, TOPIC_ADA); logger.log(calibrator.getReferenceValues()); logger.log(ada.getKalmanState()); @@ -513,7 +514,7 @@ void ADAController<Press, GPS>::finalizeCalibration() template <typename Press, typename GPS> void ADAController<Press, GPS>::state_idle(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -541,7 +542,7 @@ void ADAController<Press, GPS>::state_idle(const Event& ev) template <typename Press, typename GPS> void ADAController<Press, GPS>::state_calibrating(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -579,7 +580,7 @@ void ADAController<Press, GPS>::state_calibrating(const Event& ev) template <typename Press, typename GPS> void ADAController<Press, GPS>::state_ready(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -612,12 +613,12 @@ void ADAController<Press, GPS>::state_ready(const Event& ev) template <typename Press, typename GPS> void ADAController<Press, GPS>::state_shadowMode(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { shadow_delayed_event_id = - sEventBroker->postDelayed<TIMEOUT_ADA_SHADOW_MODE>( + sEventBroker.postDelayed<TIMEOUT_ADA_SHADOW_MODE>( {EV_SHADOW_MODE_TIMEOUT}, TOPIC_ADA); logStatus(ADAState::SHADOW_MODE); LOG_DEBUG(log, "Entering state shadowMode"); @@ -625,7 +626,7 @@ void ADAController<Press, GPS>::state_shadowMode(const Event& ev) } case EV_EXIT: { - sEventBroker->removeDelayed(shadow_delayed_event_id); + sEventBroker.removeDelayed(shadow_delayed_event_id); LOG_DEBUG(log, "Exiting state shadowMode"); break; } @@ -644,7 +645,7 @@ void ADAController<Press, GPS>::state_shadowMode(const Event& ev) template <typename Press, typename GPS> void ADAController<Press, GPS>::state_active(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -672,12 +673,12 @@ void ADAController<Press, GPS>::state_active(const Event& ev) template <typename Press, typename GPS> void ADAController<Press, GPS>::state_pressureStabilization(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { pressure_delayed_event_id = - sEventBroker->postDelayed<TIMEOUT_ADA_P_STABILIZATION>( + sEventBroker.postDelayed<TIMEOUT_ADA_P_STABILIZATION>( {EV_TIMEOUT_PRESS_STABILIZATION}, TOPIC_ADA); logStatus(ADAState::PRESSURE_STABILIZATION); LOG_DEBUG(log, "Entering state pressureStabilization"); @@ -685,7 +686,7 @@ void ADAController<Press, GPS>::state_pressureStabilization(const Event& ev) } case EV_EXIT: { - sEventBroker->removeDelayed(pressure_delayed_event_id); + sEventBroker.removeDelayed(pressure_delayed_event_id); LOG_DEBUG(log, "Exiting state pressureStabilization"); break; } @@ -704,7 +705,7 @@ void ADAController<Press, GPS>::state_pressureStabilization(const Event& ev) template <typename Press, typename GPS> void ADAController<Press, GPS>::state_drogueDescent(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -736,7 +737,7 @@ void ADAController<Press, GPS>::state_drogueDescent(const Event& ev) template <typename Press, typename GPS> void ADAController<Press, GPS>::state_end(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -766,10 +767,10 @@ void ADAController<Press, GPS>::logStatus(ADAState state) template <typename Press, typename GPS> void ADAController<Press, GPS>::logStatus() { - status.timestamp = TimestampTimer::getTimestamp(); + status.timestamp = TimestampTimer::getInstance().getTimestamp(); logger.log(status); - StackLogger::getInstance()->updateStack(THID_ADA_FSM); + StackLogger::getInstance().updateStack(THID_ADA_FSM); } template <typename Press, typename GPS> diff --git a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAData.h b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAData.h index f9441bb31..7889a867c 100644 --- a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAData.h +++ b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAData.h @@ -25,7 +25,8 @@ #include <configs/ADAConfig.h> #include <configs/config.h> #include <math/Stats.h> -#include <Constants.h> +#include <utils/Constants.h> + #include <ostream> using namespace Boardcore; diff --git a/src/boards/DeathStack/DeathStack.h b/src/boards/DeathStack/DeathStack.h index bff9d078a..202703941 100644 --- a/src/boards/DeathStack/DeathStack.h +++ b/src/boards/DeathStack/DeathStack.h @@ -22,9 +22,7 @@ #pragma once -#include <Common.h> #include <DeathStackStatus.h> -#include <Debug.h> #include <LoggerService/LoggerService.h> #include <Main/Actuators.h> #include <Main/Bus.h> @@ -36,9 +34,9 @@ #include <System/TaskID.h> #include <events/EventBroker.h> #include <events/EventData.h> -#include <events/utils/EventInjector.h> #include <events/Events.h> #include <events/Topics.h> +#include <events/utils/EventInjector.h> #include <events/utils/EventSniffer.h> #include <functional> @@ -69,7 +67,6 @@ class DeathStack : public Singleton<DeathStack> public: // Shared Components - EventBroker* broker; LoggerService* logger; EventSniffer* sniffer; @@ -90,7 +87,7 @@ public: void start() { - if (!broker->start()) + if (!sEventBroker.start()) { LOG_ERR(log, "Error starting EventBroker"); status.setError(&DeathStackStatus::ev_broker); @@ -134,12 +131,12 @@ public: if (status.death_stack != COMP_OK) { LOG_ERR(log, "Initalization failed\n"); - sEventBroker->post(Event{EV_INIT_ERROR}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post(Event{EV_INIT_ERROR}, TOPIC_FLIGHT_EVENTS); } else { LOG_INFO(log, "Initalization ok"); - sEventBroker->post(Event{EV_INIT_OK}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post(Event{EV_INIT_OK}, TOPIC_FLIGHT_EVENTS); } } @@ -156,7 +153,7 @@ public: status.setError(&DeathStackStatus::logger); } - logger->log(logger->getLogger().getLogStats()); + logger->log(logger->getLogger().getLoggerStats()); } private: @@ -166,19 +163,16 @@ private: DeathStack() { /* Shared components */ - logger = Singleton<LoggerService>::getInstance(); + logger = &LoggerService::getInstance(); startLogger(); - TimestampTimer::enableTimestampTimer(); - - broker = sEventBroker; - // Bind the logEvent function to the event sniffer in order to log every // event { using namespace std::placeholders; - sniffer = new EventSniffer( - *broker, TOPIC_LIST, bind(&DeathStack::logEvent, this, _1, _2)); + sniffer = + new EventSniffer(sEventBroker, TOPIC_LIST, + bind(&DeathStack::logEvent, this, _1, _2)); } #ifdef HARDWARE_IN_THE_LOOP @@ -221,7 +215,8 @@ private: */ void logEvent(uint8_t event, uint8_t topic) { - EventData ev{(long long)TimestampTimer::getTimestamp(), event, topic}; + EventData ev{(long long)TimestampTimer::getInstance().getTimestamp(), + event, topic}; logger->log(ev); #ifdef DEBUG @@ -239,25 +234,26 @@ private: #endif } - inline void postEvent(Event ev, uint8_t topic) { broker->post(ev, topic); } + inline void postEvent(Event ev, uint8_t topic) + { + sEventBroker.post(ev, topic); + } void addSchedulerStatsTask() { // add lambda to log scheduler tasks statistics - scheduler->add( - [&]() { + scheduler->addTask( + [&]() + { std::vector<TaskStatResult> scheduler_stats = scheduler->getTaskStats(); for (TaskStatResult stat : scheduler_stats) - { logger->log(stat); - } - StackLogger::getInstance()->updateStack(THID_TASK_SCHEDULER); + StackLogger::getInstance().updateStack(THID_TASK_SCHEDULER); }, - 1000, // 1 hz - TASK_SCHEDULER_STATS_ID, miosix::getTick()); + 1000, TASK_SCHEDULER_STATS_ID); } EventInjector* injector; diff --git a/src/boards/DeathStack/Deployment/DeploymentController.cpp b/src/boards/DeathStack/Deployment/DeploymentController.cpp index 008d9b86f..b3cfc11f2 100644 --- a/src/boards/DeathStack/Deployment/DeploymentController.cpp +++ b/src/boards/DeathStack/Deployment/DeploymentController.cpp @@ -23,15 +23,8 @@ #include <Deployment/DeploymentController.h> #include <LoggerService/LoggerService.h> #include <System/StackLogger.h> -#include <TimestampTimer.h> -#include <configs/DeploymentConfig.h> -#include <events/EventBroker.h> -#include <events/Events.h> - -#include <LoggerService/LoggerService.h> -#include <System/StackLogger.h> -#include <TimestampTimer.h> #include <configs/DeploymentConfig.h> +#include <drivers/timer/TimestampTimer.h> #include <events/EventBroker.h> #include <events/Events.h> @@ -41,38 +34,38 @@ namespace DeathStackBoard { DeploymentController::DeploymentController(ServoInterface* ejection_servo) - : FSM(&DeploymentController::state_initialization), - ejection_servo{ejection_servo} + : FSM(&DeploymentController::state_initialization), ejection_servo{ + ejection_servo} { - sEventBroker->subscribe(this, TOPIC_DPL); + sEventBroker.subscribe(this, TOPIC_DPL); } DeploymentController::~DeploymentController() { - sEventBroker->unsubscribe(this); + sEventBroker.unsubscribe(this); } void DeploymentController::logStatus(DeploymentControllerState current_state) { - status.timestamp = TimestampTimer::getTimestamp(); - status.state = current_state; - status.servo_position = ejection_servo->getCurrentPosition(); + status.timestamp = TimestampTimer::getInstance().getTimestamp(); + status.state = current_state; + status.servo_position = ejection_servo->getCurrentPosition(); if (current_state == DeploymentControllerState::CUTTING) { status.cutters_enabled = true; } - else + else { status.cutters_enabled = false; } - LoggerService::getInstance()->log(status); - StackLogger::getInstance()->updateStack(THID_DPL_FSM); + LoggerService::getInstance().log(status); + StackLogger::getInstance().updateStack(THID_DPL_FSM); } void DeploymentController::state_initialization(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -80,7 +73,7 @@ void DeploymentController::state_initialization(const Event& ev) ejection_servo->enable(); ejection_servo->reset(); - + logStatus(DeploymentControllerState::INITIALIZATION); transition(&DeploymentController::state_idle); @@ -101,7 +94,7 @@ void DeploymentController::state_initialization(const Event& ev) void DeploymentController::state_idle(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -148,7 +141,7 @@ void DeploymentController::state_idle(const Event& ev) void DeploymentController::state_noseconeEjection(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -156,7 +149,7 @@ void DeploymentController::state_noseconeEjection(const Event& ev) ejectNosecone(); - ev_nc_open_timeout_id = sEventBroker->postDelayed<NC_OPEN_TIMEOUT>( + ev_nc_open_timeout_id = sEventBroker.postDelayed<NC_OPEN_TIMEOUT>( Event{EV_NC_OPEN_TIMEOUT}, TOPIC_DPL); logStatus(DeploymentControllerState::NOSECONE_EJECTION); @@ -170,7 +163,7 @@ void DeploymentController::state_noseconeEjection(const Event& ev) case EV_NC_DETACHED: { LOG_DEBUG(log, "Nosecone detached event"); - sEventBroker->removeDelayed(ev_nc_open_timeout_id); + sEventBroker.removeDelayed(ev_nc_open_timeout_id); transition(&DeploymentController::state_idle); break; } @@ -189,7 +182,7 @@ void DeploymentController::state_noseconeEjection(const Event& ev) void DeploymentController::state_cutting(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -197,7 +190,7 @@ void DeploymentController::state_cutting(const Event& ev) startCutting(); - ev_nc_cutting_timeout_id = sEventBroker->postDelayed<CUT_DURATION>( + ev_nc_cutting_timeout_id = sEventBroker.postDelayed<CUT_DURATION>( Event{EV_CUTTING_TIMEOUT}, TOPIC_DPL); logStatus(DeploymentControllerState::CUTTING); @@ -207,7 +200,7 @@ void DeploymentController::state_cutting(const Event& ev) case EV_EXIT: { stopCutting(); - + LOG_DEBUG(log, "Exiting state cutting"); break; diff --git a/src/boards/DeathStack/Deployment/DeploymentController.h b/src/boards/DeathStack/Deployment/DeploymentController.h index 8523cc4ae..c1f3363ae 100644 --- a/src/boards/DeathStack/Deployment/DeploymentController.h +++ b/src/boards/DeathStack/Deployment/DeploymentController.h @@ -27,7 +27,7 @@ #include <configs/CutterConfig.h> #include <diagnostic/PrintLogger.h> #include <drivers/hbridge/HBridge.h> -#include <drivers/servo/servo.h> +#include <drivers/servo/Servo.h> #include <events/Events.h> #include <events/FSM.h> diff --git a/src/boards/DeathStack/Deployment/DeploymentData.h b/src/boards/DeathStack/Deployment/DeploymentData.h index 48f504a58..bb5c5fda8 100644 --- a/src/boards/DeathStack/Deployment/DeploymentData.h +++ b/src/boards/DeathStack/Deployment/DeploymentData.h @@ -22,7 +22,6 @@ #pragma once -#include <drivers/hbridge/HBridgeData.h> #include <stdint.h> #include <iostream> diff --git a/src/boards/DeathStack/Deployment/DeploymentServo.h b/src/boards/DeathStack/Deployment/DeploymentServo.h index 473b82ced..29632f1b8 100644 --- a/src/boards/DeathStack/Deployment/DeploymentServo.h +++ b/src/boards/DeathStack/Deployment/DeploymentServo.h @@ -24,7 +24,7 @@ #include <common/ServoInterface.h> #include <configs/DeploymentConfig.h> -#include <drivers/servo/servo.h> +#include <drivers/servo/Servo.h> #include <miosix.h> using namespace Boardcore; @@ -37,7 +37,7 @@ using namespace DeathStackBoard::DeploymentConfigs; class DeploymentServo : public ServoInterface { public: - Servo servo{DPL_SERVO_TIMER}; + Servo servo{DPL_SERVO_TIMER, DPL_SERVO_PWM_CH, 50, 500, 2500}; DeploymentServo() : ServoInterface(DPL_SERVO_MIN_POS, DPL_SERVO_MAX_POS, @@ -50,19 +50,9 @@ public: { } - void enable() override - { - servo.setMinPulseWidth(500); - servo.setMaxPulseWidth(2500); - servo.enable(DPL_SERVO_PWM_CH); - servo.start(); - } + void enable() override { servo.enable(); } - void disable() override - { - servo.stop(); - servo.disable(DPL_SERVO_PWM_CH); - } + void disable() override { servo.disable(); } /** * @brief Perform wiggle around the reset position. @@ -82,7 +72,7 @@ protected: void setPosition(float angle) override { currentPosition = angle; - servo.setPosition(DPL_SERVO_PWM_CH, currentPosition / 180.0f); + servo.setPosition180Deg(currentPosition); } private: diff --git a/src/boards/DeathStack/FlightModeManager/FMMController.cpp b/src/boards/DeathStack/FlightModeManager/FMMController.cpp index 35940b9e1..4b2dac76f 100644 --- a/src/boards/DeathStack/FlightModeManager/FMMController.cpp +++ b/src/boards/DeathStack/FlightModeManager/FMMController.cpp @@ -34,28 +34,28 @@ namespace DeathStackBoard FMMController::FMMController() : HSM(&FMMController::state_initialization, STACK_MIN_FOR_SKYWARD, FMM_PRIORITY), - logger(*(LoggerService::getInstance())) + logger(LoggerService::getInstance()) { - sEventBroker->subscribe(this, TOPIC_ADA); - sEventBroker->subscribe(this, TOPIC_NAS); - sEventBroker->subscribe(this, TOPIC_TMTC); - sEventBroker->subscribe(this, TOPIC_FMM); - sEventBroker->subscribe(this, TOPIC_FLIGHT_EVENTS); + sEventBroker.subscribe(this, TOPIC_ADA); + sEventBroker.subscribe(this, TOPIC_NAS); + sEventBroker.subscribe(this, TOPIC_TMTC); + sEventBroker.subscribe(this, TOPIC_FMM); + sEventBroker.subscribe(this, TOPIC_FLIGHT_EVENTS); } FMMController::~FMMController() { // Unsubscribe from all topics - sEventBroker->unsubscribe(this); + sEventBroker.unsubscribe(this); } void FMMController::logState(FMMState current_state) { - status.timestamp = TimestampTimer::getTimestamp(); + status.timestamp = TimestampTimer::getInstance().getTimestamp(); status.state = current_state; logger.log(status); - StackLogger::getInstance()->updateStack(THID_FMM_FSM); + StackLogger::getInstance().updateStack(THID_FMM_FSM); } State FMMController::state_initialization(const Event& ev) @@ -69,7 +69,7 @@ State FMMController::state_initialization(const Event& ev) State FMMController::state_onGround(const Event& ev) { State retState = HANDLED; - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: /* Executed everytime state is entered */ { @@ -106,7 +106,7 @@ State FMMController::state_onGround(const Event& ev) default: /* If an event is not handled here, try with super-state */ { // Since this is an outer super-state, the parent is HSM_top - retState = tran_super(&FMMController::Hsm_top); + retState = tranSuper(&FMMController::Hsm_top); break; } } @@ -116,7 +116,7 @@ State FMMController::state_onGround(const Event& ev) State FMMController::state_init(const Event& ev) { State retState = HANDLED; - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: /* Executed everytime state is entered */ { @@ -146,7 +146,7 @@ State FMMController::state_init(const Event& ev) } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FMMController::state_onGround); + retState = tranSuper(&FMMController::state_onGround); break; } } @@ -156,7 +156,7 @@ State FMMController::state_init(const Event& ev) State FMMController::state_initError(const Event& ev) { State retState = HANDLED; - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: /* Executed everytime state is entered */ { @@ -181,7 +181,7 @@ State FMMController::state_initError(const Event& ev) } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FMMController::state_onGround); + retState = tranSuper(&FMMController::state_onGround); break; } } @@ -191,7 +191,7 @@ State FMMController::state_initError(const Event& ev) State FMMController::state_initDone(const Event& ev) { State retState = HANDLED; - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: /* Executed everytime state is entered */ { @@ -221,7 +221,7 @@ State FMMController::state_initDone(const Event& ev) } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FMMController::state_onGround); + retState = tranSuper(&FMMController::state_onGround); break; } } @@ -231,7 +231,7 @@ State FMMController::state_initDone(const Event& ev) State FMMController::state_sensorsCalibration(const Event& ev) { State retState = HANDLED; - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: /* Executed everytime state is entered */ { @@ -239,8 +239,8 @@ State FMMController::state_sensorsCalibration(const Event& ev) LOG_DEBUG(log, "Entering sensors_calibration"); - DeathStack::getInstance()->sensors->calibrate(); - sEventBroker->post({EV_SENSORS_READY}, TOPIC_FLIGHT_EVENTS); + DeathStack::getInstance().sensors->calibrate(); + sEventBroker.post({EV_SENSORS_READY}, TOPIC_FLIGHT_EVENTS); break; } @@ -266,7 +266,7 @@ State FMMController::state_sensorsCalibration(const Event& ev) } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FMMController::state_onGround); + retState = tranSuper(&FMMController::state_onGround); break; } } @@ -276,14 +276,14 @@ State FMMController::state_sensorsCalibration(const Event& ev) State FMMController::state_algosCalibration(const Event& ev) { State retState = HANDLED; - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: /* Executed everytime state is entered */ { logState(FMMState::ALGOS_CALIBRATION); - sEventBroker->post({EV_CALIBRATE_ADA}, TOPIC_ADA); - sEventBroker->post({EV_CALIBRATE_NAS}, TOPIC_NAS); + sEventBroker.post({EV_CALIBRATE_ADA}, TOPIC_ADA); + sEventBroker.post({EV_CALIBRATE_NAS}, TOPIC_NAS); LOG_DEBUG(log, "Entering algos_calibration"); break; @@ -308,8 +308,8 @@ State FMMController::state_algosCalibration(const Event& ev) ada_ready = true; if (nas_ready) { - sEventBroker->post(Event{EV_CALIBRATION_OK}, - TOPIC_FLIGHT_EVENTS); + sEventBroker.post(Event{EV_CALIBRATION_OK}, + TOPIC_FLIGHT_EVENTS); } break; } @@ -318,8 +318,8 @@ State FMMController::state_algosCalibration(const Event& ev) nas_ready = true; if (ada_ready) { - sEventBroker->post(Event{EV_CALIBRATION_OK}, - TOPIC_FLIGHT_EVENTS); + sEventBroker.post(Event{EV_CALIBRATION_OK}, + TOPIC_FLIGHT_EVENTS); } break; } @@ -330,7 +330,7 @@ State FMMController::state_algosCalibration(const Event& ev) } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FMMController::state_onGround); + retState = tranSuper(&FMMController::state_onGround); break; } } @@ -340,11 +340,11 @@ State FMMController::state_algosCalibration(const Event& ev) State FMMController::state_disarmed(const Event& ev) { State retState = HANDLED; - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: /* Executed everytime state is entered */ { - sEventBroker->post({EV_DISARMED}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post({EV_DISARMED}, TOPIC_FLIGHT_EVENTS); logState(FMMState::DISARMED); LOG_DEBUG(log, "Entering disarmed"); @@ -380,7 +380,7 @@ State FMMController::state_disarmed(const Event& ev) } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FMMController::state_onGround); + retState = tranSuper(&FMMController::state_onGround); break; } } @@ -390,11 +390,11 @@ State FMMController::state_disarmed(const Event& ev) State FMMController::state_armed(const Event& ev) { State retState = HANDLED; - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: /* Executed everytime state is entered */ { - sEventBroker->post({EV_ARMED}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post({EV_ARMED}, TOPIC_FLIGHT_EVENTS); logState(FMMState::ARMED); LOG_DEBUG(log, "Entering armed"); @@ -423,7 +423,7 @@ State FMMController::state_armed(const Event& ev) } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FMMController::Hsm_top); + retState = tranSuper(&FMMController::Hsm_top); break; } } @@ -433,7 +433,7 @@ State FMMController::state_armed(const Event& ev) State FMMController::state_testMode(const Event& ev) { State retState = HANDLED; - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: /* Executed everytime state is entered */ { @@ -455,37 +455,37 @@ State FMMController::state_testMode(const Event& ev) } case EV_TC_NC_OPEN: { - sEventBroker->post(Event{EV_NC_OPEN}, TOPIC_DPL); + sEventBroker.post(Event{EV_NC_OPEN}, TOPIC_DPL); break; } case EV_TC_DPL_WIGGLE_SERVO: { - sEventBroker->post(Event{EV_WIGGLE_SERVO}, TOPIC_DPL); + sEventBroker.post(Event{EV_WIGGLE_SERVO}, TOPIC_DPL); break; } case EV_TC_DPL_RESET_SERVO: { - sEventBroker->post(Event{EV_RESET_SERVO}, TOPIC_DPL); + sEventBroker.post(Event{EV_RESET_SERVO}, TOPIC_DPL); break; } case EV_TC_CUT_DROGUE: { - sEventBroker->post(Event{EV_CUT_DROGUE}, TOPIC_DPL); + sEventBroker.post(Event{EV_CUT_DROGUE}, TOPIC_DPL); break; } case EV_TC_ABK_WIGGLE_SERVO: { - sEventBroker->post(Event{EV_WIGGLE_SERVO}, TOPIC_ABK); + sEventBroker.post(Event{EV_WIGGLE_SERVO}, TOPIC_ABK); break; } case EV_TC_ABK_RESET_SERVO: { - sEventBroker->post(Event{EV_RESET_SERVO}, TOPIC_ABK); + sEventBroker.post(Event{EV_RESET_SERVO}, TOPIC_ABK); break; } case EV_TC_TEST_ABK: { - sEventBroker->post(Event{EV_TEST_ABK}, TOPIC_ABK); + sEventBroker.post(Event{EV_TEST_ABK}, TOPIC_ABK); break; } case EV_TC_CLOSE_LOG: @@ -495,12 +495,12 @@ State FMMController::state_testMode(const Event& ev) } case EV_TC_START_LOG: { - DeathStackBoard::DeathStack::getInstance()->startLogger(); + DeathStackBoard::DeathStack::getInstance().startLogger(); break; } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FMMController::state_onGround); + retState = tranSuper(&FMMController::state_onGround); break; } } @@ -510,20 +510,20 @@ State FMMController::state_testMode(const Event& ev) State FMMController::state_flying(const Event& ev) { State retState = HANDLED; - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: /* Executed everytime state is entered */ { logState(FMMState::FLYING); - sEventBroker->post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS); // Start timeout for closing file descriptors end_mission_d_event_id = - sEventBroker->postDelayed<TIMEOUT_END_MISSION>( + sEventBroker.postDelayed<TIMEOUT_END_MISSION>( {EV_TIMEOUT_END_MISSION}, TOPIC_FMM); #ifdef USE_MOCK_SENSORS - DeathStack::getInstance()->sensors->signalLiftoff(); + DeathStack::getInstance().ensors->signalLiftoff(); #endif LOG_DEBUG(log, "Entering flying"); @@ -540,21 +540,21 @@ State FMMController::state_flying(const Event& ev) { LOG_DEBUG(log, "Exiting flying"); - sEventBroker->removeDelayed(end_mission_d_event_id); + sEventBroker.removeDelayed(end_mission_d_event_id); break; } case EV_TC_NC_OPEN: { // Open nosecone command sent by GS in case of problems // during flight - sEventBroker->post(Event{EV_NC_OPEN}, TOPIC_DPL); + sEventBroker.post(Event{EV_NC_OPEN}, TOPIC_DPL); break; } case EV_TC_ABK_DISABLE: { // Disable airbrakes command sent by GS in case of problems // during flight - sEventBroker->post(Event{EV_DISABLE_ABK}, TOPIC_ABK); + sEventBroker.post(Event{EV_DISABLE_ABK}, TOPIC_ABK); break; } case EV_TC_END_MISSION: @@ -565,7 +565,7 @@ State FMMController::state_flying(const Event& ev) } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FMMController::Hsm_top); + retState = tranSuper(&FMMController::Hsm_top); break; } } @@ -575,7 +575,7 @@ State FMMController::state_flying(const Event& ev) State FMMController::state_ascending(const Event& ev) { State retState = HANDLED; - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: /* Executed everytime state is entered */ { @@ -597,7 +597,7 @@ State FMMController::state_ascending(const Event& ev) case EV_ADA_APOGEE_DETECTED: { // Notify of apogee all components - sEventBroker->post(Event{EV_APOGEE}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post(Event{EV_APOGEE}, TOPIC_FLIGHT_EVENTS); retState = transition(&FMMController::state_drogueDescent); break; @@ -605,14 +605,14 @@ State FMMController::state_ascending(const Event& ev) case EV_ADA_DISABLE_ABK: { // Send disable airbrakes - sEventBroker->post(Event{EV_DISABLE_ABK}, TOPIC_ABK); + sEventBroker.post(Event{EV_DISABLE_ABK}, TOPIC_ABK); retState = transition(&FMMController::state_ascending); break; } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FMMController::state_flying); + retState = tranSuper(&FMMController::state_flying); break; } } @@ -622,12 +622,12 @@ State FMMController::state_ascending(const Event& ev) State FMMController::state_drogueDescent(const Event& ev) { State retState = HANDLED; - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: /* Executed everytime state is entered */ { // Open nosecone - sEventBroker->post(Event{EV_NC_OPEN}, TOPIC_DPL); + sEventBroker.post(Event{EV_NC_OPEN}, TOPIC_DPL); logState(FMMState::DROGUE_DESCENT); LOG_DEBUG(log, "Entering drogueDescent"); @@ -651,7 +651,7 @@ State FMMController::state_drogueDescent(const Event& ev) } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FMMController::state_flying); + retState = tranSuper(&FMMController::state_flying); break; } } @@ -661,13 +661,13 @@ State FMMController::state_drogueDescent(const Event& ev) State FMMController::state_terminalDescent(const Event& ev) { State retState = HANDLED; - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: /* Executed everytime state is entered */ { - sEventBroker->post({EV_DPL_ALTITUDE}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post({EV_DPL_ALTITUDE}, TOPIC_FLIGHT_EVENTS); - sEventBroker->post(Event{EV_CUT_DROGUE}, TOPIC_DPL); + sEventBroker.post(Event{EV_CUT_DROGUE}, TOPIC_DPL); logState(FMMState::TERMINAL_DESCENT); @@ -684,12 +684,12 @@ State FMMController::state_terminalDescent(const Event& ev) } case EV_TC_CUT_DROGUE: // if you want to repeat cutting { - sEventBroker->post(Event{EV_CUT_DROGUE}, TOPIC_DPL); + sEventBroker.post(Event{EV_CUT_DROGUE}, TOPIC_DPL); break; } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FMMController::state_flying); + retState = tranSuper(&FMMController::state_flying); break; } } @@ -699,14 +699,14 @@ State FMMController::state_terminalDescent(const Event& ev) State FMMController::state_landed(const Event& ev) { State retState = HANDLED; - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: /* Executed everytime state is entered */ { logState(FMMState::LANDED); // Announce landing to all components - sEventBroker->post(Event{EV_LANDED}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post(Event{EV_LANDED}, TOPIC_FLIGHT_EVENTS); logger.stop(); LOG_DEBUG(log, "Entering landed"); @@ -722,7 +722,7 @@ State FMMController::state_landed(const Event& ev) } default: /* If an event is not handled here, try with super-state */ { - retState = tran_super(&FMMController::Hsm_top); + retState = tranSuper(&FMMController::Hsm_top); break; } } diff --git a/src/boards/DeathStack/FlightStatsRecorder/FSRController.cpp b/src/boards/DeathStack/FlightStatsRecorder/FSRController.cpp index c077390bb..1205db39d 100644 --- a/src/boards/DeathStack/FlightStatsRecorder/FSRController.cpp +++ b/src/boards/DeathStack/FlightStatsRecorder/FSRController.cpp @@ -35,12 +35,12 @@ namespace DeathStackBoard FlightStatsRecorder::FlightStatsRecorder() : FSM(&FlightStatsRecorder::state_idle) { - sEventBroker->subscribe(this, TOPIC_FLIGHT_EVENTS); - sEventBroker->subscribe(this, TOPIC_DPL); - sEventBroker->subscribe(this, TOPIC_STATS); + sEventBroker.subscribe(this, TOPIC_FLIGHT_EVENTS); + sEventBroker.subscribe(this, TOPIC_DPL); + sEventBroker.subscribe(this, TOPIC_STATS); } -FlightStatsRecorder::~FlightStatsRecorder() { sEventBroker->unsubscribe(this); } +FlightStatsRecorder::~FlightStatsRecorder() { sEventBroker.unsubscribe(this); } void FlightStatsRecorder::update(const ADAKalmanState& t) { @@ -125,9 +125,9 @@ void FlightStatsRecorder::update(const MS5803Data& t) { case FSRState::ASCENDING: { - if (t.press < apogee_stats.digital_min_pressure) + if (t.pressure < apogee_stats.digital_min_pressure) { - apogee_stats.digital_min_pressure = t.press; + apogee_stats.digital_min_pressure = t.pressure; } break; } @@ -142,9 +142,9 @@ void FlightStatsRecorder::update(const MPXHZ6130AData& t) { case FSRState::ASCENDING: { - if (t.press < apogee_stats.static_min_pressure) + if (t.pressure < apogee_stats.static_min_pressure) { - apogee_stats.static_min_pressure = t.press; + apogee_stats.static_min_pressure = t.pressure; } break; } @@ -159,9 +159,9 @@ void FlightStatsRecorder::update(const SSCDANN030PAAData& t) { case FSRState::ASCENDING: { - if (t.press > drogue_dpl_stats.max_dpl_vane_pressure) + if (t.pressure > drogue_dpl_stats.max_dpl_vane_pressure) { - drogue_dpl_stats.max_dpl_vane_pressure = t.press; + drogue_dpl_stats.max_dpl_vane_pressure = t.pressure; } break; } @@ -177,19 +177,19 @@ void FlightStatsRecorder::update(const BMX160WithCorrectionData& t) { case FSRState::LIFTOFF: { - if (fabs(t.accel_x) > liftoff_stats.acc_max) + if (fabs(t.accelerationX) > liftoff_stats.acc_max) { liftoff_stats.T_max_acc = static_cast<uint32_t>(miosix::getTick()); - liftoff_stats.acc_max = fabs(t.accel_x); + liftoff_stats.acc_max = fabs(t.accelerationX); } break; } case FSRState::DROGUE_DPL: { - if (fabs(t.accel_x) > fabs(drogue_dpl_stats.max_dpl_acc)) + if (fabs(t.accelerationX) > fabs(drogue_dpl_stats.max_dpl_acc)) { - drogue_dpl_stats.max_dpl_acc = t.accel_x; + drogue_dpl_stats.max_dpl_acc = t.accelerationX; drogue_dpl_stats.T_dpl = static_cast<uint32_t>(miosix::getTick()); } @@ -197,9 +197,9 @@ void FlightStatsRecorder::update(const BMX160WithCorrectionData& t) } case FSRState::MAIN_DPL: { - if (fabs(t.accel_x) > fabs(main_dpl_stats.max_dpl_acc)) + if (fabs(t.accelerationX) > fabs(main_dpl_stats.max_dpl_acc)) { - main_dpl_stats.max_dpl_acc = t.accel_x; + main_dpl_stats.max_dpl_acc = t.accelerationX; } break; } @@ -208,7 +208,7 @@ void FlightStatsRecorder::update(const BMX160WithCorrectionData& t) } } -void FlightStatsRecorder::update(const UbloxGPSData& t) +void FlightStatsRecorder::update(const GPSData& t) { switch (state) { @@ -232,7 +232,7 @@ void FlightStatsRecorder::update(const HILImuData& t) { BMX160Data d; d.accel_timestamp = t.accel_timestamp; - d.accel_x = t.accel_x; + d.accelerationX = t.accelerationX; d.accel_y = t.accel_y; d.accel_z = t.accel_z; d.gyro_timestamp = t.gyro_timestamp; @@ -250,13 +250,13 @@ void FlightStatsRecorder::update(const HILBaroData& t) { MS5803Data d; d.press_timestamp = t.press_timestamp; - d.press = t.press; + d.pressure = t.press; this->update(d); } void FlightStatsRecorder::update(const HILGpsData& t) { - UbloxGPSData d; + GPSData d; d.latitude = t.latitude; d.longitude = t.longitude; d.height = t.height; @@ -273,15 +273,15 @@ void FlightStatsRecorder::update(const HILGpsData& t) void FlightStatsRecorder::state_idle(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { LOG_DEBUG(log, "Entering IDLE state"); - + state = FSRState::IDLE; - StackLogger::getInstance()->updateStack(THID_STATS_FSM); + StackLogger::getInstance().updateStack(THID_STATS_FSM); break; } case EV_EXIT: @@ -309,7 +309,7 @@ void FlightStatsRecorder::state_idle(const Event& ev) void FlightStatsRecorder::state_liftOff(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -320,22 +320,22 @@ void FlightStatsRecorder::state_liftOff(const Event& ev) // Collect liftoff stats until this event is received ev_timeout_id = sEventBroker - ->postDelayed<FlightStatsConfig::TIMEOUT_LIFTOFF_STATS>( + .postDelayed<FlightStatsConfig::TIMEOUT_LIFTOFF_STATS>( {EV_STATS_TIMEOUT}, TOPIC_STATS); // Save liftoff time liftoff_stats.T_liftoff = static_cast<uint32_t>(miosix::getTick()); - StackLogger::getInstance()->updateStack(THID_STATS_FSM); + StackLogger::getInstance().updateStack(THID_STATS_FSM); break; } case EV_EXIT: { LOG_DEBUG(log, "Exiting LIFTOFF state"); - LoggerService::getInstance()->log(liftoff_stats); + LoggerService::getInstance().log(liftoff_stats); - sEventBroker->removeDelayed(ev_timeout_id); + sEventBroker.removeDelayed(ev_timeout_id); break; } case EV_STATS_TIMEOUT: @@ -352,7 +352,7 @@ void FlightStatsRecorder::state_liftOff(const Event& ev) void FlightStatsRecorder::state_ascending(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -360,16 +360,16 @@ void FlightStatsRecorder::state_ascending(const Event& ev) state = FSRState::ASCENDING; - StackLogger::getInstance()->updateStack(THID_STATS_FSM); + StackLogger::getInstance().updateStack(THID_STATS_FSM); break; } case EV_EXIT: { LOG_DEBUG(log, "Exiting ASCENDING state"); - LoggerService::getInstance()->log(apogee_stats); + LoggerService::getInstance().log(apogee_stats); - sEventBroker->removeDelayed(ev_timeout_id); + sEventBroker.removeDelayed(ev_timeout_id); break; } case EV_APOGEE: @@ -381,7 +381,7 @@ void FlightStatsRecorder::state_ascending(const Event& ev) // seconds in order to record the maximum altitude. ev_timeout_id = sEventBroker - ->postDelayed<FlightStatsConfig::TIMEOUT_APOGEE_STATS>( + .postDelayed<FlightStatsConfig::TIMEOUT_APOGEE_STATS>( {EV_STATS_TIMEOUT}, TOPIC_STATS); break; } @@ -400,7 +400,7 @@ void FlightStatsRecorder::state_ascending(const Event& ev) void FlightStatsRecorder::state_drogueDeployment(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -411,19 +411,19 @@ void FlightStatsRecorder::state_drogueDeployment(const Event& ev) // Collect stats until this event is received ev_timeout_id = sEventBroker - ->postDelayed<FlightStatsConfig::TIMEOUT_DROGUE_DPL_STATS>( + .postDelayed<FlightStatsConfig::TIMEOUT_DROGUE_DPL_STATS>( {EV_STATS_TIMEOUT}, TOPIC_STATS); - StackLogger::getInstance()->updateStack(THID_STATS_FSM); + StackLogger::getInstance().updateStack(THID_STATS_FSM); break; } case EV_EXIT: { LOG_DEBUG(log, "Exiting DROGUE_DPL state"); - LoggerService::getInstance()->log(drogue_dpl_stats); + LoggerService::getInstance().log(drogue_dpl_stats); - sEventBroker->removeDelayed(ev_timeout_id); + sEventBroker.removeDelayed(ev_timeout_id); break; } case EV_STATS_TIMEOUT: @@ -440,7 +440,7 @@ void FlightStatsRecorder::state_drogueDeployment(const Event& ev) void FlightStatsRecorder::state_mainDeployment(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -454,19 +454,19 @@ void FlightStatsRecorder::state_mainDeployment(const Event& ev) // Record stats until this event occurs ev_timeout_id = sEventBroker - ->postDelayed<FlightStatsConfig::TIMEOUT_MAIN_DPL_STATS>( + .postDelayed<FlightStatsConfig::TIMEOUT_MAIN_DPL_STATS>( {EV_STATS_TIMEOUT}, TOPIC_STATS); - StackLogger::getInstance()->updateStack(THID_STATS_FSM); + StackLogger::getInstance().updateStack(THID_STATS_FSM); break; } case EV_EXIT: { LOG_DEBUG(log, "Exiting MAIN_DPL state"); - LoggerService::getInstance()->log(main_dpl_stats); + LoggerService::getInstance().log(main_dpl_stats); - sEventBroker->removeDelayed(ev_timeout_id); + sEventBroker.removeDelayed(ev_timeout_id); break; } case EV_STATS_TIMEOUT: @@ -484,7 +484,7 @@ void FlightStatsRecorder::state_mainDeployment(const Event& ev) /* void FlightStatsRecorder::state_testingCutters(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -494,10 +494,10 @@ void FlightStatsRecorder::state_testingCutters(const Event& ev) const int timeout = CutterConfig::CUT_TEST_DURATION; - ev_timeout_id = sEventBroker->postDelayed<timeout>( + ev_timeout_id = sEventBroker.postDelayed<timeout>( {EV_STATS_TIMEOUT}, TOPIC_STATS); - StackLogger::getInstance()->updateStack(THID_STATS_FSM); + StackLogger::getInstance().updateStack(THID_STATS_FSM); LOG_DEBUG(log, "Entering CUTTER_TEST state"); break; } @@ -508,8 +508,8 @@ void FlightStatsRecorder::state_testingCutters(const Event& ev) cutters_stats.cutter_2_avg = cutters_stats.cutter_2_avg / cutters_stats.n_samples_2; - LoggerService::getInstance()->log(cutters_stats); - sEventBroker->removeDelayed(ev_timeout_id); + LoggerService::getInstance().log(cutters_stats); + sEventBroker.removeDelayed(ev_timeout_id); LOG_DEBUG(log, "Exiting CUTTER_TEST state"); break; diff --git a/src/boards/DeathStack/FlightStatsRecorder/FSRController.h b/src/boards/DeathStack/FlightStatsRecorder/FSRController.h index 018de20a1..f00899138 100644 --- a/src/boards/DeathStack/FlightStatsRecorder/FSRController.h +++ b/src/boards/DeathStack/FlightStatsRecorder/FSRController.h @@ -27,7 +27,6 @@ #include <Main/SensorsData.h> #include <configs/FlightStatsConfig.h> #include <diagnostic/PrintLogger.h> -#include <drivers/gps/ublox/UbloxGPSData.h> #include <events/FSM.h> #include <sensors/BMX160/BMX160WithCorrectionData.h> #include <sensors/MS5803/MS5803Data.h> @@ -60,11 +59,11 @@ public: void update(const ADAKalmanState& t); void update(const ADAData& t); - void update(const UbloxGPSData& t); + void update(const GPSData& t); void update(const BMX160WithCorrectionData& t); - //void update(const CurrentSensorData& t); - void update(const MS5803Data& t); // digitl baro - void update(const MPXHZ6130AData& t); // static ports baro + // void update(const CurrentSensorData& t); + void update(const MS5803Data& t); // digitl baro + void update(const MPXHZ6130AData& t); // static ports baro void update(const SSCDANN030PAAData& t); // DPL vane baro void update(const AirSpeedPitot& t); @@ -104,7 +103,7 @@ private: ApogeeStats apogee_stats{}; DrogueDPLStats drogue_dpl_stats{}; MainDPLStats main_dpl_stats{}; - //CutterTestStats cutters_stats{}; + // CutterTestStats cutters_stats{}; FSRState state = FSRState::IDLE; long long T_liftoff = 0; diff --git a/src/boards/DeathStack/LoggerService/LoggerService.h b/src/boards/DeathStack/LoggerService/LoggerService.h index 8f6584bd2..c4593aa70 100644 --- a/src/boards/DeathStack/LoggerService/LoggerService.h +++ b/src/boards/DeathStack/LoggerService/LoggerService.h @@ -46,7 +46,7 @@ public: * @brief Generic log function, to be implemented for each loggable struct. */ template <typename T> - inline LogResult log(const T& t) + inline LoggerResult log(const T& t) { { miosix::PauseKernelLock kLock; @@ -86,7 +86,7 @@ private: * @brief Private constructor to enforce the singleton */ LoggerService() - : logger(Logger::instance()), tmRepo(*(TmRepository::getInstance())) + : logger(Logger::getInstance()), tmRepo(TmRepository::getInstance()) { } diff --git a/src/boards/DeathStack/Main/Radio.cpp b/src/boards/DeathStack/Main/Radio.cpp index 1f6248121..aa50827f5 100644 --- a/src/boards/DeathStack/Main/Radio.cpp +++ b/src/boards/DeathStack/Main/Radio.cpp @@ -42,9 +42,9 @@ void __attribute__((used)) EXTI10_IRQHandlerImpl() { using namespace DeathStackBoard; - if (DeathStack::getInstance()->radio->xbee != nullptr) + if (DeathStack::getInstance().radio->xbee != nullptr) { - DeathStack::getInstance()->radio->xbee->handleATTNInterrupt(); + DeathStack::getInstance().radio->xbee->handleATTNInterrupt(); } } @@ -55,7 +55,7 @@ Radio::Radio(SPIBusInterface& xbee_bus) : xbee_bus(xbee_bus) { SPIBusConfig xbee_cfg{}; - xbee_cfg.clock_div = SPIClockDivider::DIV16; + xbee_cfg.clockDivider = SPI::ClockDivider::DIV_16; xbee = new Xbee::Xbee(xbee_bus, xbee_cfg, miosix::xbee::cs::getPin(), miosix::xbee::attn::getPin(), @@ -70,7 +70,7 @@ Radio::Radio(SPIBusInterface& xbee_bus) : xbee_bus(xbee_bus) tmtc_manager = new TMTCController(); - tm_repo = TmRepository::getInstance(); + tm_repo = &(TmRepository::getInstance()); enableExternalInterrupt(GPIOF_BASE, 10, InterruptTrigger::FALLING_EDGE); } @@ -89,11 +89,11 @@ bool Radio::start() { return mav_driver->start() && tmtc_manager->start(); } void Radio::onXbeeFrameReceived(Xbee::APIFrame& frame) { - LoggerService& logger = *LoggerService::getInstance(); + LoggerService& logger = LoggerService::getInstance(); using namespace Xbee; bool logged = false; - switch (frame.frame_type) + switch (frame.frameType) { case FTYPE_AT_COMMAND: { @@ -168,9 +168,9 @@ void Radio::onXbeeFrameReceived(Xbee::APIFrame& frame) void Radio::logStatus() { MavlinkStatus status = mav_driver->getStatus(); - status.timestamp = TimestampTimer::getTimestamp(); - LoggerService::getInstance()->log(status); - LoggerService::getInstance()->log(xbee->getStatus()); + status.timestamp = TimestampTimer::getInstance().getTimestamp(); + LoggerService::getInstance().log(status); + LoggerService::getInstance().log(xbee->getStatus()); } } // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/Main/Sensors.cpp b/src/boards/DeathStack/Main/Sensors.cpp index 3242f76a9..78721d919 100644 --- a/src/boards/DeathStack/Main/Sensors.cpp +++ b/src/boards/DeathStack/Main/Sensors.cpp @@ -22,11 +22,10 @@ #include <ApogeeDetectionAlgorithm/ADAController.h> #include <DeathStack.h> -#include <Debug.h> #include <LoggerService/LoggerService.h> -#include <TimestampTimer.h> #include <configs/SensorsConfig.h> #include <drivers/interrupt/external_interrupts.h> +#include <drivers/timer/TimestampTimer.h> #include <interfaces-impl/hwmapping.h> #include <sensors/Sensor.h> #include <utils/aero/AeroUtils.h> @@ -37,17 +36,15 @@ using std::bind; using std::function; -using namespace Boardcore; - // BMX160 Watermark interrupt void __attribute__((used)) EXTI5_IRQHandlerImpl() { using namespace DeathStackBoard; - if (DeathStack::getInstance()->sensors->imu_bmx160 != nullptr) + if (DeathStack::getInstance().sensors->imu_bmx160 != nullptr) { - DeathStack::getInstance()->sensors->imu_bmx160->IRQupdateTimestamp( - TimestampTimer::getTimestamp()); + DeathStack::getInstance().sensors->imu_bmx160->IRQupdateTimestamp( + TimestampTimer::getInstance().getTimestamp()); } } @@ -78,7 +75,7 @@ Sensors::Sensors(SPIBusInterface& spi1_bus, TaskScheduler* scheduler) mockSensorsInit(); #endif - sensor_manager = new SensorManager(scheduler, sensors_map); + sensor_manager = new SensorManager(sensors_map, scheduler); } Sensors::~Sensors() @@ -123,7 +120,7 @@ bool Sensors::start() updateSensorsStatus(); } - LoggerService::getInstance()->log(status); + LoggerService::getInstance().log(status); return sm_start_result; } @@ -131,7 +128,7 @@ bool Sensors::start() void Sensors::calibrate() { imu_bmx160_with_correction->calibrate(); - LoggerService::getInstance()->log( + LoggerService::getInstance().log( imu_bmx160_with_correction->getGyroscopeBiases()); press_pitot->calibrate(); @@ -142,7 +139,7 @@ void Sensors::calibrate() for (unsigned int i = 0; i < PRESS_STATIC_CALIB_SAMPLES_NUM / 10; i++) { Thread::sleep(SAMPLE_PERIOD_PRESS_DIGITAL); - press_digi_stats.add(press_digital->getLastSample().press); + press_digi_stats.add(press_digital->getLastSample().pressure); } press_static_port->setReferencePressure(press_digi_stats.getStats().mean); press_static_port->calibrate(); @@ -165,12 +162,12 @@ void Sensors::signalLiftoff() void Sensors::internalAdcInit() { - internal_adc = new InternalADC(*ADC3, INTERNAL_ADC_VREF); + internal_adc = new InternalADC(ADC3, INTERNAL_ADC_VREF); internal_adc->enableChannel(ADC_BATTERY_VOLTAGE); SensorInfo info("InternalADC", SAMPLE_PERIOD_INTERNAL_ADC, - bind(&Sensors::internalAdcCallback, this), false, true); + bind(&Sensors::internalAdcCallback, this)); sensors_map.emplace(std::make_pair(internal_adc, info)); LOG_INFO(log, "InternalADC setup done!"); @@ -184,7 +181,7 @@ void Sensors::batteryVoltageInit() new BatteryVoltageSensor(voltage_fun, BATTERY_VOLTAGE_COEFF); SensorInfo info("BatterySensor", SAMPLE_PERIOD_INTERNAL_ADC, - bind(&Sensors::batteryVoltageCallback, this), false, true); + bind(&Sensors::batteryVoltageCallback, this)); sensors_map.emplace(std::make_pair(battery_voltage, info)); @@ -194,13 +191,13 @@ void Sensors::batteryVoltageInit() void Sensors::pressDigiInit() { SPIBusConfig spi_cfg{}; - spi_cfg.clock_div = SPIClockDivider::DIV16; + spi_cfg.clockDivider = SPI::ClockDivider::DIV_16; press_digital = new MS5803(spi1_bus, miosix::sensors::ms5803::cs::getPin(), spi_cfg, TEMP_DIVIDER_PRESS_DIGITAL); SensorInfo info("DigitalBarometer", SAMPLE_PERIOD_PRESS_DIGITAL, - bind(&Sensors::pressDigiCallback, this), false, true); + bind(&Sensors::pressDigiCallback, this)); sensors_map.emplace(std::make_pair(press_digital, info)); @@ -210,7 +207,7 @@ void Sensors::pressDigiInit() void Sensors::ADS1118Init() { SPIBusConfig spi_cfg = ADS1118::getDefaultSPIConfig(); - spi_cfg.clock_div = SPIClockDivider::DIV64; + spi_cfg.clockDivider = SPI::ClockDivider::DIV_64; ADS1118::ADS1118Config ads1118Config = ADS1118::ADS1118_DEFAULT_CONFIG; ads1118Config.bits.mode = ADS1118::ADS1118Mode::CONTIN_CONV_MODE; @@ -229,7 +226,7 @@ void Sensors::ADS1118Init() adc_ads1118->enableInput(ADC_CH_VREF, ADC_DR_VREF, ADC_PGA_VREF); SensorInfo info("ADS1118", SAMPLE_PERIOD_ADC_ADS1118, - bind(&Sensors::ADS1118Callback, this), false, true); + bind(&Sensors::ADS1118Callback, this)); sensors_map.emplace(std::make_pair(adc_ads1118, info)); LOG_INFO(log, "ADS1118 setup done!"); @@ -243,7 +240,7 @@ void Sensors::pressPitotInit() PRESS_PITOT_CALIB_SAMPLES_NUM); SensorInfo info("DiffBarometer", SAMPLE_PERIOD_PRESS_PITOT, - bind(&Sensors::pressPitotCallback, this), false, true); + bind(&Sensors::pressPitotCallback, this)); sensors_map.emplace(std::make_pair(press_pitot, info)); @@ -257,7 +254,7 @@ void Sensors::pressDPLVaneInit() press_dpl_vane = new SSCDANN030PAA(voltage_fun, REFERENCE_VOLTAGE); SensorInfo info("DeploymentBarometer", SAMPLE_PERIOD_PRESS_DPL, - bind(&Sensors::pressDPLVaneCallback, this), false, true); + bind(&Sensors::pressDPLVaneCallback, this)); sensors_map.emplace(std::make_pair(press_dpl_vane, info)); @@ -273,7 +270,7 @@ void Sensors::pressStaticInit() PRESS_STATIC_MOVING_AVG_COEFF); SensorInfo info("StaticPortsBarometer", SAMPLE_PERIOD_PRESS_STATIC, - bind(&Sensors::pressStaticCallback, this), false, true); + bind(&Sensors::pressStaticCallback, this)); sensors_map.emplace(std::make_pair(press_static_port, info)); @@ -283,29 +280,29 @@ void Sensors::pressStaticInit() void Sensors::imuBMXInit() { SPIBusConfig spi_cfg; - spi_cfg.clock_div = SPIClockDivider::DIV8; + spi_cfg.clockDivider = SPI::ClockDivider::DIV_8; BMX160Config bmx_config; - bmx_config.fifo_mode = BMX160Config::FifoMode::HEADER; - bmx_config.fifo_watermark = IMU_BMX_FIFO_WATERMARK; - bmx_config.fifo_int = BMX160Config::FifoInterruptPin::PIN_INT1; + bmx_config.fifoMode = BMX160Config::FifoMode::HEADER; + bmx_config.fifoWatermark = IMU_BMX_FIFO_WATERMARK; + bmx_config.fifoInterrupt = BMX160Config::FifoInterruptPin::PIN_INT1; - bmx_config.temp_divider = IMU_BMX_TEMP_DIVIDER; + bmx_config.temperatureDivider = IMU_BMX_TEMP_DIVIDER; - bmx_config.acc_range = IMU_BMX_ACC_FULLSCALE_ENUM; - bmx_config.gyr_range = IMU_BMX_GYRO_FULLSCALE_ENUM; + bmx_config.accelerometerRange = IMU_BMX_ACC_FULLSCALE_ENUM; + bmx_config.gyroscopeRange = IMU_BMX_GYRO_FULLSCALE_ENUM; - bmx_config.acc_odr = IMU_BMX_ACC_GYRO_ODR_ENUM; - bmx_config.gyr_odr = IMU_BMX_ACC_GYRO_ODR_ENUM; - bmx_config.mag_odr = IMU_BMX_MAG_ODR_ENUM; + bmx_config.accelerometerDataRate = IMU_BMX_ACC_GYRO_ODR_ENUM; + bmx_config.gyroscopeDataRate = IMU_BMX_ACC_GYRO_ODR_ENUM; + bmx_config.magnetometerRate = IMU_BMX_MAG_ODR_ENUM; - bmx_config.gyr_unit = BMX160Config::GyroscopeMeasureUnit::RAD; + bmx_config.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD; imu_bmx160 = new BMX160(spi1_bus, miosix::sensors::bmx160::cs::getPin(), bmx_config, spi_cfg); SensorInfo info("BMX160", SAMPLE_PERIOD_IMU_BMX, - bind(&Sensors::imuBMXCallback, this), false, true); + bind(&Sensors::imuBMXCallback, this)); sensors_map.emplace(std::make_pair(imu_bmx160, info)); @@ -353,8 +350,7 @@ void Sensors::imuBMXWithCorrectionInit() imu_bmx160, correctionParameters, BMX160_AXIS_ROTATION); SensorInfo info("BMX160WithCorrection", SAMPLE_PERIOD_IMU_BMX, - bind(&Sensors::imuBMXWithCorrectionCallback, this), false, - true); + bind(&Sensors::imuBMXWithCorrectionCallback, this)); sensors_map.emplace(std::make_pair(imu_bmx160_with_correction, info)); @@ -364,7 +360,7 @@ void Sensors::imuBMXWithCorrectionInit() void Sensors::magLISinit() { SPIBusConfig busConfig; - busConfig.clock_div = SPIClockDivider::DIV32; + busConfig.clockDivider = SPI::ClockDivider::DIV_32; LIS3MDL::Config config; config.odr = MAG_LIS_ODR_ENUM; @@ -375,7 +371,7 @@ void Sensors::magLISinit() busConfig, config); SensorInfo info("LIS3MDL", SAMPLE_PERIOD_MAG_LIS, - bind(&Sensors::magLISCallback, this), false, true); + bind(&Sensors::magLISCallback, this)); sensors_map.emplace(std::make_pair(mag_lis3mdl, info)); @@ -384,10 +380,10 @@ void Sensors::magLISinit() void Sensors::gpsUbloxInit() { - gps_ublox = new UbloxGPS(GPS_BAUD_RATE, GPS_SAMPLE_RATE); + gps_ublox = new UbloxGPSSerial(GPS_BAUD_RATE, GPS_SAMPLE_RATE); SensorInfo info("UbloxGPS", GPS_SAMPLE_PERIOD, - bind(&Sensors::gpsUbloxCallback, this), false, true); + bind(&Sensors::gpsUbloxCallback, this)); sensors_map.emplace(std::make_pair(gps_ublox, info)); @@ -396,12 +392,12 @@ void Sensors::gpsUbloxInit() void Sensors::internalAdcCallback() { - // LoggerService::getInstance()->log(internal_adc->getLastSample()); + // LoggerService::getInstance().log(internal_adc->getLastSample()); } void Sensors::batteryVoltageCallback() { - static float v = battery_voltage->getLastSample().bat_voltage; + static float v = battery_voltage->getLastSample().batVoltage; if (v < BATTERY_MIN_SAFE_VOLTAGE) { battery_critical_counter++; @@ -413,18 +409,18 @@ void Sensors::batteryVoltageCallback() } } - LoggerService::getInstance()->log(battery_voltage->getLastSample()); + LoggerService::getInstance().log(battery_voltage->getLastSample()); } #ifdef HARDWARE_IN_THE_LOOP void Sensors::hilBarometerInit() { - HILTransceiver* simulator = HIL::getInstance()->simulator; + HILTransceiver* simulator = HIL::getInstance().simulator; hil_baro = new HILBarometer(simulator, N_DATA_BARO); SensorInfo info_baro("HILBaro", HIL_BARO_PERIOD, - bind(&Sensors::hilBaroCallback, this), false, true); + bind(&Sensors::hilBaroCallback, this)); sensors_map.emplace(std::make_pair(hil_baro, info_baro)); @@ -432,12 +428,12 @@ void Sensors::hilBarometerInit() } void Sensors::hilImuInit() { - HILTransceiver* simulator = HIL::getInstance()->simulator; + HILTransceiver* simulator = HIL::getInstance().simulator; hil_imu = new HILImu(simulator, N_DATA_IMU); SensorInfo info_imu("HILImu", HIL_IMU_PERIOD, - bind(&Sensors::hilIMUCallback, this), false, true); + bind(&Sensors::hilIMUCallback, this)); sensors_map.emplace(std::make_pair(hil_imu, info_imu)); @@ -446,12 +442,12 @@ void Sensors::hilImuInit() void Sensors::hilGpsInit() { - HILTransceiver* simulator = HIL::getInstance()->simulator; + HILTransceiver* simulator = HIL::getInstance().simulator; hil_gps = new HILGps(simulator, N_DATA_GPS); SensorInfo info_gps("HILGps", HIL_GPS_PERIOD, - bind(&Sensors::hilGPSCallback, this), false, true); + bind(&Sensors::hilGPSCallback, this)); sensors_map.emplace(std::make_pair(hil_gps, info_gps)); @@ -465,11 +461,11 @@ void Sensors::mockSensorsInit() mock_gps = new MockGPS(); SensorInfo info_baro("MockBaro", SAMPLE_PERIOD_PRESS_STATIC, - bind(&Sensors::mockBaroCallback, this), false, true); + bind(&Sensors::mockBaroCallback, this)); SensorInfo info_imu("MockIMU", SAMPLE_PERIOD_IMU_BMX, - bind(&Sensors::mockImuCallback, this), false, true); + bind(&Sensors::mockImuCallback, this)); SensorInfo info_gps("MockGPS", GPS_SAMPLE_PERIOD, - bind(&Sensors::mockGpsCallback, this), false, true); + bind(&Sensors::mockGpsCallback, this)); sensors_map.emplace(std::make_pair(mock_baro, info_baro)); sensors_map.emplace(std::make_pair(mock_imu, info_imu)); @@ -481,43 +477,44 @@ void Sensors::mockSensorsInit() void Sensors::pressDigiCallback() { - LoggerService::getInstance()->log(press_digital->getLastSample()); + LoggerService::getInstance().log(press_digital->getLastSample()); } void Sensors::ADS1118Callback() { - LoggerService::getInstance()->log(adc_ads1118->getLastSample()); + LoggerService::getInstance().log(adc_ads1118->getLastSample()); } void Sensors::pressPitotCallback() { SSCDRRN015PDAData d = press_pitot->getLastSample(); - LoggerService::getInstance()->log(d); + LoggerService::getInstance().log(d); ADAReferenceValues rv = DeathStack::getInstance() - ->state_machines->ada_controller->getReferenceValues(); + .state_machines->ada_controller->getReferenceValues(); float rel_density = aeroutils::relDensity( - press_digital->getLastSample().press, rv.ref_pressure, rv.ref_altitude, - rv.ref_temperature); + press_digital->getLastSample().pressure, rv.ref_pressure, + rv.ref_altitude, rv.ref_temperature); if (rel_density != 0.0f) { - float airspeed = sqrtf(2 * fabs(d.press) / rel_density); + float airspeed = sqrtf(2 * fabs(d.pressure) / rel_density); - AirSpeedPitot aspeed_data{TimestampTimer::getTimestamp(), airspeed}; - LoggerService::getInstance()->log(aspeed_data); + AirSpeedPitot aspeed_data{TimestampTimer::getInstance().getTimestamp(), + airspeed}; + LoggerService::getInstance().log(aspeed_data); } } void Sensors::pressDPLVaneCallback() { - LoggerService::getInstance()->log(press_dpl_vane->getLastSample()); + LoggerService::getInstance().log(press_dpl_vane->getLastSample()); } void Sensors::pressStaticCallback() { - LoggerService::getInstance()->log(press_static_port->getLastSample()); + LoggerService::getInstance().log(press_static_port->getLastSample()); } void Sensors::imuBMXCallback() @@ -525,94 +522,92 @@ void Sensors::imuBMXCallback() uint8_t fifo_size = imu_bmx160->getLastFifoSize(); auto& fifo = imu_bmx160->getLastFifo(); - LoggerService::getInstance()->log(imu_bmx160->getTemperature()); + LoggerService::getInstance().log(imu_bmx160->getTemperature()); for (uint8_t i = 0; i < fifo_size; ++i) { - LoggerService::getInstance()->log(fifo.at(i)); + LoggerService::getInstance().log(fifo.at(i)); } - LoggerService::getInstance()->log(imu_bmx160->getFifoStats()); + LoggerService::getInstance().log(imu_bmx160->getFifoStats()); } void Sensors::imuBMXWithCorrectionCallback() { - LoggerService::getInstance()->log( + LoggerService::getInstance().log( imu_bmx160_with_correction->getLastSample()); } void Sensors::magLISCallback() { - LoggerService::getInstance()->log(mag_lis3mdl->getLastSample()); + LoggerService::getInstance().log(mag_lis3mdl->getLastSample()); } void Sensors::gpsUbloxCallback() { - LoggerService::getInstance()->log(gps_ublox->getLastSample()); + LoggerService::getInstance().log(gps_ublox->getLastSample()); } #ifdef HARDWARE_IN_THE_LOOP void Sensors::hilIMUCallback() { - LoggerService::getInstance()->log(hil_imu->getLastSample()); + LoggerService::getInstance().log(hil_imu->getLastSample()); } void Sensors::hilBaroCallback() { - LoggerService::getInstance()->log(hil_baro->getLastSample()); + LoggerService::getInstance().log(hil_baro->getLastSample()); } void Sensors::hilGPSCallback() { - LoggerService::getInstance()->log(hil_gps->getLastSample()); + LoggerService::getInstance().log(hil_gps->getLastSample()); } #elif defined(USE_MOCK_SENSORS) void Sensors::mockBaroCallback() { - LoggerService::getInstance()->log(mock_imu->getLastSample()); + LoggerService::getInstance().log(mock_imu->getLastSample()); } void Sensors::mockImuCallback() { - LoggerService::getInstance()->log(mock_baro->getLastSample()); + LoggerService::getInstance().log(mock_baro->getLastSample()); } void Sensors::mockGpsCallback() { - LoggerService::getInstance()->log(mock_gps->getLastSample()); + LoggerService::getInstance().log(mock_gps->getLastSample()); } #endif void Sensors::updateSensorsStatus() { - SensorInfo info; - - info = sensor_manager->getSensorInfo(imu_bmx160); - if (!info.is_initialized) + SensorInfo info = sensor_manager->getSensorInfo(imu_bmx160); + if (!info.isInitialized) { status.bmx160 = SensorDriverStatus::DRIVER_ERROR; } info = sensor_manager->getSensorInfo(mag_lis3mdl); - if (!info.is_initialized) + if (!info.isInitialized) { status.lis3mdl = SensorDriverStatus::DRIVER_ERROR; } info = sensor_manager->getSensorInfo(gps_ublox); - if (!info.is_initialized) + if (!info.isInitialized) { status.gps = SensorDriverStatus::DRIVER_ERROR; } info = sensor_manager->getSensorInfo(internal_adc); - if (!info.is_initialized) + if (!info.isInitialized) { status.internal_adc = SensorDriverStatus::DRIVER_ERROR; } info = sensor_manager->getSensorInfo(adc_ads1118); - if (!info.is_initialized) + if (!info.isInitialized) { status.ads1118 = SensorDriverStatus::DRIVER_ERROR; } diff --git a/src/boards/DeathStack/Main/Sensors.h b/src/boards/DeathStack/Main/Sensors.h index 44f335a07..1587e1a87 100644 --- a/src/boards/DeathStack/Main/Sensors.h +++ b/src/boards/DeathStack/Main/Sensors.h @@ -24,16 +24,17 @@ #include <Main/SensorsData.h> #include <diagnostic/PrintLogger.h> -#include <drivers/adc/ADS1118/ADS1118.h> -#include <drivers/adc/InternalADC/InternalADC.h> -#include <drivers/gps/ublox/UbloxGPS.h> +#include <drivers/adc/InternalADC.h> #include <drivers/spi/SPIBusInterface.h> +#include <scheduler/TaskScheduler.h> +#include <sensors/ADS1118/ADS1118.h> #include <sensors/BMX160/BMX160.h> #include <sensors/BMX160/BMX160WithCorrection.h> #include <sensors/LIS3MDL/LIS3MDL.h> #include <sensors/MS5803/MS5803.h> #include <sensors/SensorData.h> #include <sensors/SensorManager.h> +#include <sensors/UbloxGPS/UbloxGPS.h> #include <sensors/analog/battery/BatteryVoltageSensor.h> #include <sensors/analog/current/CurrentSensor.h> #include <sensors/analog/pressure/MPXHZ6130A/MPXHZ6130A.h> diff --git a/src/boards/DeathStack/Main/StateMachines.cpp b/src/boards/DeathStack/Main/StateMachines.cpp index f0f08297e..021e82d27 100644 --- a/src/boards/DeathStack/Main/StateMachines.cpp +++ b/src/boards/DeathStack/Main/StateMachines.cpp @@ -47,7 +47,7 @@ StateMachines::StateMachines(IMUType& imu, PressType& press, GPSType& gps, fmm = new FMMController(); #ifdef HARDWARE_IN_THE_LOOP - HIL::getInstance()->setNAS(&nas_controller->getNAS()); + HIL::getInstance().setNAS(&nas_controller->getNAS()); #endif addAlgorithmsToScheduler(scheduler); @@ -72,14 +72,18 @@ void StateMachines::addAlgorithmsToScheduler(TaskScheduler* scheduler) { uint64_t start_time = miosix::getTick() + 10; - scheduler->add(std::bind(&ADAControllerType::update, ada_controller), - ADA_UPDATE_PERIOD, TASK_ADA_ID, start_time); + scheduler->addTask(std::bind(&ADAControllerType::update, ada_controller), + ADA_UPDATE_PERIOD, TASK_ADA_ID, + TaskScheduler::Policy::RECOVER, start_time); - scheduler->add(std::bind(&NASControllerType::update, nas_controller), - NAS_UPDATE_PERIOD, TASK_NAS_ID, start_time); + scheduler->addTask(std::bind(&NASControllerType::update, nas_controller), + NAS_UPDATE_PERIOD, TASK_NAS_ID, + TaskScheduler::Policy::RECOVER, start_time); - scheduler->add(std::bind(&AirBrakesControllerType::update, arb_controller), - ABK_UPDATE_PERIOD, TASK_ABK_ID, start_time); + scheduler->addTask( + std::bind(&AirBrakesControllerType::update, arb_controller), + ABK_UPDATE_PERIOD, TASK_ABK_ID, TaskScheduler::Policy::RECOVER, + start_time); } void StateMachines::setReferenceTemperature(float t) diff --git a/src/boards/DeathStack/Main/StateMachines.h b/src/boards/DeathStack/Main/StateMachines.h index a00692b76..d109922f1 100644 --- a/src/boards/DeathStack/Main/StateMachines.h +++ b/src/boards/DeathStack/Main/StateMachines.h @@ -24,9 +24,9 @@ #include <LoggerService/LoggerService.h> #include <NavigationAttitudeSystem/NASData.h> -#include <drivers/gps/ublox/UbloxGPS.h> #include <scheduler/TaskScheduler.h> #include <sensors/BMX160/BMX160WithCorrection.h> +#include <sensors/UbloxGPS/UbloxGPS.h> #include <sensors/analog/pressure/MPXHZ6130A/MPXHZ6130A.h> #ifdef HARDWARE_IN_THE_LOOP @@ -112,8 +112,6 @@ public: private: void addAlgorithmsToScheduler(TaskScheduler* scheduler); - - LoggerService& logger = *(LoggerService::getInstance()); }; } // namespace DeathStackBoard \ No newline at end of file diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/InitStates.h b/src/boards/DeathStack/NavigationAttitudeSystem/InitStates.h index a2dff0521..6a4dd11bd 100644 --- a/src/boards/DeathStack/NavigationAttitudeSystem/InitStates.h +++ b/src/boards/DeathStack/NavigationAttitudeSystem/InitStates.h @@ -29,7 +29,6 @@ #pragma once -#include <Common.h> #include <configs/NASConfig.h> #include <diagnostic/PrintLogger.h> #include <math/SkyQuaternion.h> diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/NAS.h b/src/boards/DeathStack/NavigationAttitudeSystem/NAS.h index 6e62ccb2d..9310e21f7 100644 --- a/src/boards/DeathStack/NavigationAttitudeSystem/NAS.h +++ b/src/boards/DeathStack/NavigationAttitudeSystem/NAS.h @@ -40,8 +40,8 @@ #include <NavigationAttitudeSystem/ExtendedKalmanEigen.h> #include <NavigationAttitudeSystem/InitStates.h> #include <NavigationAttitudeSystem/NASData.h> -#include <TimestampTimer.h> #include <diagnostic/PrintLogger.h> +#include <drivers/timer/TimestampTimer.h> #include <math/SkyQuaternion.h> #include <sensors/Sensor.h> @@ -187,7 +187,7 @@ bool NAS<IMU, Press, GPS>::init() initialized = true; - LoggerService::getInstance()->log(getTriadResult()); + LoggerService::getInstance().log(getTriadResult()); return initialized; } @@ -211,52 +211,54 @@ NASData NAS<IMU, Press, GPS>::sampleImpl() Press pressure_data = barometer.getLastSample(); // update ekf with new accel and gyro measures - if (imu_data.accel_timestamp != last_accel_timestamp && - imu_data.gyro_timestamp != last_gyro_timestamp) + if (imu_data.accelerationTimestamp != last_accel_timestamp && + imu_data.angularVelocityTimestamp != last_gyro_timestamp) { - last_accel_timestamp = imu_data.accel_timestamp; - last_gyro_timestamp = imu_data.gyro_timestamp; + last_accel_timestamp = imu_data.accelerationTimestamp; + last_gyro_timestamp = imu_data.angularVelocityTimestamp; - Vector3f accel_readings(imu_data.accel_x, imu_data.accel_y, - imu_data.accel_z); + Vector3f accel_readings(imu_data.accelerationX, imu_data.accelerationY, + imu_data.accelerationZ); filter.predict(accel_readings); - Vector3f gyro_readings(imu_data.gyro_x, imu_data.gyro_y, - imu_data.gyro_z); + Vector3f gyro_readings(imu_data.angularVelocityX, + imu_data.angularVelocityY, + imu_data.angularVelocityZ); filter.predictMEKF(gyro_readings); } // check if new pressure data is available - if (pressure_data.press_timestamp != last_press_timestamp) + if (pressure_data.pressureTimestamp != last_press_timestamp) { - last_press_timestamp = pressure_data.press_timestamp; + last_press_timestamp = pressure_data.pressureTimestamp; - filter.correctBaro(pressure_data.press, ref_values.msl_pressure, + filter.correctBaro(pressure_data.pressure, ref_values.msl_pressure, ref_values.msl_temperature); } // check if new gps data is available and the gps has fix - if (gps_data.gps_timestamp != last_gps_timestamp && gps_data.fix == true) + if (gps_data.gpsTimestamp != last_gps_timestamp && gps_data.fix == true) { - last_gps_timestamp = gps_data.gps_timestamp; + last_gps_timestamp = gps_data.gpsTimestamp; Vector3f gps_readings(gps_data.latitude, gps_data.longitude, gps_data.height); Vector3f gps_ned = geodetic2NED(gps_readings); - Vector4f pos_vel(gps_ned(0), gps_ned(1), gps_data.velocity_north, - gps_data.velocity_east); - filter.correctGPS(pos_vel, gps_data.num_satellites); + Vector4f pos_vel(gps_ned(0), gps_ned(1), gps_data.velocityNorth, + gps_data.velocityEast); + filter.correctGPS(pos_vel, gps_data.satellites); } // check if new magnetometer data is available - if (imu_data.mag_timestamp != last_mag_timestamp) + if (imu_data.magneticFieldTimestamp != last_mag_timestamp) { - Vector3f mag_readings(imu_data.mag_x, imu_data.mag_y, imu_data.mag_z); + Vector3f mag_readings(imu_data.magneticFieldX, imu_data.magneticFieldY, + imu_data.magneticFieldZ); if (mag_readings.norm() < EMF * JAMMING_FACTOR) { - last_mag_timestamp = imu_data.mag_timestamp; + last_mag_timestamp = imu_data.magneticFieldTimestamp; mag_readings.normalize(); filter.correctMEKF(mag_readings); @@ -351,7 +353,7 @@ void NAS<IMU, Press, GPS>::setInitialOrientation(float roll, float pitch, template <typename IMU, typename Press, typename GPS> void NAS<IMU, Press, GPS>::updateNASData() { - nas_data.timestamp = TimestampTimer::getTimestamp(); + nas_data.timestamp = TimestampTimer::getInstance().getTimestamp(); nas_data.x = x(0); nas_data.y = x(1); diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/NASCalibrator.h b/src/boards/DeathStack/NavigationAttitudeSystem/NASCalibrator.h index 7a115db5a..c8d12a06c 100644 --- a/src/boards/DeathStack/NavigationAttitudeSystem/NASCalibrator.h +++ b/src/boards/DeathStack/NavigationAttitudeSystem/NASCalibrator.h @@ -22,8 +22,6 @@ #pragma once -#include <Common.h> -#include <Debug.h> #include <NavigationAttitudeSystem/NASData.h> #include <math/Stats.h> #include <miosix.h> @@ -99,8 +97,8 @@ private: Stats mag_z_stats; // Refernece flags - bool ref_coordinates_set = false; - bool ref_altitude_set = false; + bool ref_coordinates_set = false; + bool ref_altitude_set = false; bool ref_temperature_set = false; }; diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/NASController.h b/src/boards/DeathStack/NavigationAttitudeSystem/NASController.h index 425df09ce..307116324 100644 --- a/src/boards/DeathStack/NavigationAttitudeSystem/NASController.h +++ b/src/boards/DeathStack/NavigationAttitudeSystem/NASController.h @@ -121,11 +121,11 @@ NASController<IMU, Press, GPS>::NASController(Sensor<IMU>& imu, Sensor<GPS>& gps) : NASFsm(&NASCtrl::state_idle), calibrator(CALIBRATION_N_SAMPLES), imu(imu), barometer(baro), gps(gps), nas(imu, baro, gps), - logger(*(LoggerService::getInstance())) + logger(LoggerService::getInstance()) { memset(&status, 0, sizeof(NASStatus)); - sEventBroker->subscribe(this, TOPIC_FLIGHT_EVENTS); - sEventBroker->subscribe(this, TOPIC_NAS); + sEventBroker.subscribe(this, TOPIC_FLIGHT_EVENTS); + sEventBroker.subscribe(this, TOPIC_NAS); status.state = NASState::IDLE; } @@ -133,7 +133,7 @@ NASController<IMU, Press, GPS>::NASController(Sensor<IMU>& imu, template <typename IMU, typename Press, typename GPS> NASController<IMU, Press, GPS>::~NASController() { - sEventBroker->unsubscribe(this); + sEventBroker.unsubscribe(this); } template <typename IMU, typename Press, typename GPS> @@ -157,31 +157,33 @@ void NASController<IMU, Press, GPS>::update() { Lock<FastMutex> l(mutex); - if (imu_data.accel_timestamp != last_accel_timestamp) + if (imu_data.accelerationTimestamp != last_accel_timestamp) { - last_accel_timestamp = imu_data.accel_timestamp; - calibrator.addAccelSample( - imu_data.accel_x, imu_data.accel_y, imu_data.accel_z); + last_accel_timestamp = imu_data.accelerationTimestamp; + calibrator.addAccelSample(imu_data.accelerationX, + imu_data.accelerationY, + imu_data.accelerationZ); } - if (imu_data.mag_timestamp != last_mag_timestamp) + if (imu_data.magneticFieldTimestamp != last_mag_timestamp) { - last_mag_timestamp = imu_data.mag_timestamp; - calibrator.addMagSample(imu_data.mag_x, imu_data.mag_y, - imu_data.mag_z); + last_mag_timestamp = imu_data.magneticFieldTimestamp; + calibrator.addMagSample(imu_data.magneticFieldX, + imu_data.magneticFieldY, + imu_data.magneticFieldZ); } // Add samples to the calibration - if (press_data.press_timestamp != last_press_timestamp) + if (press_data.pressureTimestamp != last_press_timestamp) { - last_press_timestamp = press_data.press_timestamp; - calibrator.addBaroSample(press_data.press); + last_press_timestamp = press_data.pressureTimestamp; + calibrator.addBaroSample(press_data.pressure); } if (gps_data.fix == true && - gps_data.gps_timestamp != last_gps_timestamp) + gps_data.gpsTimestamp != last_gps_timestamp) { - last_gps_timestamp = gps_data.gps_timestamp; + last_gps_timestamp = gps_data.gpsTimestamp; calibrator.addGPSSample(gps_data.latitude, gps_data.longitude); } @@ -240,14 +242,14 @@ void NASController<IMU, Press, GPS>::finalizeCalibration() LOG_INFO(log, "Finalized calibration and TRIAD"); - sEventBroker->post({EV_NAS_READY}, TOPIC_NAS); + sEventBroker.post({EV_NAS_READY}, TOPIC_NAS); } } template <typename IMU, typename Press, typename GPS> void NASController<IMU, Press, GPS>::state_idle(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -275,7 +277,7 @@ void NASController<IMU, Press, GPS>::state_idle(const Event& ev) template <typename IMU, typename Press, typename GPS> void NASController<IMU, Press, GPS>::state_calibrating(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -315,7 +317,7 @@ void NASController<IMU, Press, GPS>::state_calibrating(const Event& ev) template <typename IMU, typename Press, typename GPS> void NASController<IMU, Press, GPS>::state_ready(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -348,7 +350,7 @@ void NASController<IMU, Press, GPS>::state_ready(const Event& ev) template <typename IMU, typename Press, typename GPS> void NASController<IMU, Press, GPS>::state_active(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -376,7 +378,7 @@ void NASController<IMU, Press, GPS>::state_active(const Event& ev) template <typename IMU, typename Press, typename GPS> void NASController<IMU, Press, GPS>::state_end(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { @@ -457,18 +459,18 @@ void NASController<IMU, Press, GPS>::setReferenceAltitude(float altitude) template <typename IMU, typename Press, typename GPS> void NASController<IMU, Press, GPS>::logStatus(NASState state) { - status.timestamp = TimestampTimer::getTimestamp(); + status.timestamp = TimestampTimer::getInstance().getTimestamp(); status.state = state; logger.log(status); - StackLogger::getInstance()->updateStack(THID_NAS_FSM); + StackLogger::getInstance().updateStack(THID_NAS_FSM); } template <typename IMU, typename Press, typename GPS> void NASController<IMU, Press, GPS>::logData() { NASKalmanState kalman_state = nas.getKalmanState(); - kalman_state.timestamp = TimestampTimer::getTimestamp(); + kalman_state.timestamp = TimestampTimer::getInstance().getTimestamp(); logger.log(kalman_state); logger.log(nas.getLastSample()); } diff --git a/src/boards/DeathStack/PinHandler/PinHandler.cpp b/src/boards/DeathStack/PinHandler/PinHandler.cpp index 352c73db9..ef710a411 100644 --- a/src/boards/DeathStack/PinHandler/PinHandler.cpp +++ b/src/boards/DeathStack/PinHandler/PinHandler.cpp @@ -20,12 +20,12 @@ * THE SOFTWARE. */ +#include <DeathStack.h> #include <LoggerService/LoggerService.h> #include <PinHandler/PinHandler.h> #include <diagnostic/PrintLogger.h> #include <events/EventBroker.h> #include <events/Events.h> -#include <DeathStack.h> #include <functional> @@ -40,7 +40,7 @@ namespace DeathStackBoard { PinHandler::PinHandler() - : pin_obs(PIN_POLL_INTERVAL), logger(LoggerService::getInstance()) + : pin_obs(PIN_POLL_INTERVAL), logger(&LoggerService::getInstance()) { // Used for _1, _2. See std::bind cpp reference using namespace std::placeholders; @@ -85,16 +85,17 @@ void PinHandler::onLaunchPinTransition(unsigned int p, unsigned char n) UNUSED(n); #ifdef HARDWARE_IN_THE_LOOP - HIL::getInstance()->signalLiftoff(); + HIL::getInstance().signalLiftoff(); #elif defined(USE_MOCK_SENSORS) - DeathStack::getInstance()->sensors->signalLiftoff(); + DeathStack::getInstance().ensors->signalLiftoff(); #endif - sEventBroker->post(Event{EV_UMBILICAL_DETACHED}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post(Event{EV_UMBILICAL_DETACHED}, TOPIC_FLIGHT_EVENTS); LOG_INFO(log, "Launch pin detached!"); - status_pin_launch.last_detection_time = TimestampTimer::getTimestamp(); + status_pin_launch.last_detection_time = + TimestampTimer::getInstance().getTimestamp(); logger->log(status_pin_launch); } @@ -102,11 +103,12 @@ void PinHandler::onNCPinTransition(unsigned int p, unsigned char n) { UNUSED(p); UNUSED(n); - sEventBroker->post(Event{EV_NC_DETACHED}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post(Event{EV_NC_DETACHED}, TOPIC_FLIGHT_EVENTS); LOG_INFO(log, "Nosecone detached!"); - status_pin_nosecone.last_detection_time = TimestampTimer::getTimestamp(); + status_pin_nosecone.last_detection_time = + TimestampTimer::getInstance().getTimestamp(); logger->log(status_pin_nosecone); } @@ -118,7 +120,8 @@ void PinHandler::onDPLServoPinTransition(unsigned int p, unsigned char n) LOG_INFO(log, "Deployment servo actuated!"); // do not post any event, just log the timestamp - status_pin_dpl_servo.last_detection_time = TimestampTimer::getTimestamp(); + status_pin_dpl_servo.last_detection_time = + TimestampTimer::getInstance().getTimestamp(); logger->log(status_pin_dpl_servo); } @@ -128,8 +131,9 @@ void PinHandler::onLaunchPinStateChange(unsigned int p, unsigned char n, UNUSED(p); UNUSED(n); - status_pin_launch.state = (uint8_t)state; - status_pin_launch.last_state_change = TimestampTimer::getTimestamp(); + status_pin_launch.state = (uint8_t)state; + status_pin_launch.last_state_change = + TimestampTimer::getInstance().getTimestamp(); status_pin_launch.num_state_changes += 1; LOG_INFO(log, @@ -145,8 +149,9 @@ void PinHandler::onNCPinStateChange(unsigned int p, unsigned char n, int state) UNUSED(p); UNUSED(n); - status_pin_nosecone.state = (uint8_t)state; - status_pin_nosecone.last_state_change = TimestampTimer::getTimestamp(); + status_pin_nosecone.state = (uint8_t)state; + status_pin_nosecone.last_state_change = + TimestampTimer::getInstance().getTimestamp(); status_pin_nosecone.num_state_changes += 1; LOG_INFO(log, "Nosecone pin state change at time {}: new state = {}", @@ -161,8 +166,9 @@ void PinHandler::onDPLServoPinStateChange(unsigned int p, unsigned char n, UNUSED(p); UNUSED(n); - status_pin_dpl_servo.state = (uint8_t)state; - status_pin_dpl_servo.last_state_change = TimestampTimer::getTimestamp(); + status_pin_dpl_servo.state = (uint8_t)state; + status_pin_dpl_servo.last_state_change = + TimestampTimer::getInstance().getTimestamp(); status_pin_dpl_servo.num_state_changes += 1; LOG_INFO(log, diff --git a/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.cpp b/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.cpp index aba74b7bb..b34959d82 100644 --- a/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.cpp +++ b/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.cpp @@ -67,7 +67,7 @@ const std::map<uint8_t, uint8_t> tcMap = { void handleMavlinkMessage(MavDriver* mav_driver, const mavlink_message_t& msg) { // log status - DeathStack::getInstance()->radio->logStatus(); + DeathStack::getInstance().radio->logStatus(); // acknowledge sendAck(mav_driver, msg); @@ -83,7 +83,7 @@ void handleMavlinkMessage(MavDriver* mav_driver, const mavlink_message_t& msg) // search for the corresponding event and post it auto it = tcMap.find(commandId); if (it != tcMap.end()) - sEventBroker->post(Event{it->second}, TOPIC_TMTC); + sEventBroker.post(Event{it->second}, TOPIC_TMTC); else LOG_WARN(print_logger, "Unknown NOARG command {:d}", commandId); @@ -100,7 +100,7 @@ void handleMavlinkMessage(MavDriver* mav_driver, const mavlink_message_t& msg) LOG_INFO(print_logger, "Received command CLOSE_LOG"); break; case MAV_CMD_START_LOGGING: - DeathStack::getInstance()->startLogger(); + DeathStack::getInstance().startLogger(); sendTelemetry(mav_driver, MAV_LOGGER_TM_ID); LOG_INFO(print_logger, "Received command START_LOG"); break; @@ -128,8 +128,7 @@ void handleMavlinkMessage(MavDriver* mav_driver, const mavlink_message_t& msg) print_logger, "Received SET_REFERENCE_ALTITUDE command. Ref altitude: {:f} m", alt); - DeathStack::getInstance()->state_machines->setReferenceAltitude( - alt); + DeathStack::getInstance().state_machines->setReferenceAltitude(alt); break; } case MAVLINK_MSG_ID_SET_REFERENCE_TEMPERATURE_TC: @@ -140,7 +139,7 @@ void handleMavlinkMessage(MavDriver* mav_driver, const mavlink_message_t& msg) print_logger, "Received SET_REFERENCE_TEMPERATURE command. Temp: {:f} degC", temp); - DeathStack::getInstance()->state_machines->setReferenceTemperature( + DeathStack::getInstance().state_machines->setReferenceTemperature( temp); break; } @@ -153,7 +152,7 @@ void handleMavlinkMessage(MavDriver* mav_driver, const mavlink_message_t& msg) "Received SET_DEPLOYMENT_ALTITUDE command. Dpl alt: {:f} m", alt); DeathStack::getInstance() - ->state_machines->ada_controller->setDeploymentAltitude(alt); + .state_machines->ada_controller->setDeploymentAltitude(alt); break; } case MAVLINK_MSG_ID_SET_INITIAL_ORIENTATION_TC: @@ -166,7 +165,7 @@ void handleMavlinkMessage(MavDriver* mav_driver, const mavlink_message_t& msg) "Received SET_INITIAL_ORIENTATION command. roll: {:f}, " "pitch: {:f}, yaw: {:f}", roll, pitch, yaw); - DeathStack::getInstance()->state_machines->setInitialOrientation( + DeathStack::getInstance().state_machines->setInitialOrientation( roll, pitch, yaw); break; } @@ -182,7 +181,7 @@ void handleMavlinkMessage(MavDriver* mav_driver, const mavlink_message_t& msg) "Received SET_INITIAL_COORDINATES command. latitude: {:f}, " "longitude: {:f}", latitude, longitude); - DeathStack::getInstance()->state_machines->setInitialCoordinates( + DeathStack::getInstance().state_machines->setInitialCoordinates( latitude, longitude); break; } @@ -191,8 +190,8 @@ void handleMavlinkMessage(MavDriver* mav_driver, const mavlink_message_t& msg) LOG_DEBUG(print_logger, "Received RAW_EVENT command"); // post given event on given topic - sEventBroker->post({mavlink_msg_raw_event_tc_get_Event_id(&msg)}, - mavlink_msg_raw_event_tc_get_Topic_id(&msg)); + sEventBroker.post({mavlink_msg_raw_event_tc_get_Event_id(&msg)}, + mavlink_msg_raw_event_tc_get_Topic_id(&msg)); break; } @@ -205,7 +204,7 @@ void handleMavlinkMessage(MavDriver* mav_driver, const mavlink_message_t& msg) { float pos = mavlink_msg_set_aerobrake_angle_tc_get_angle(&msg); DeathStack::getInstance() - ->state_machines->arb_controller->setAirBrakesPosition(pos); + .state_machines->arb_controller->setAirBrakesPosition(pos); LOG_DEBUG(print_logger, "Received set ab pos: {:.1f} deg", pos); break; @@ -231,11 +230,10 @@ bool sendTelemetry(MavDriver* mav_driver, const uint8_t tm_id) { // enqueue the TM packet taking it from the TM repo (pauses kernel to // guarantee synchronicity) - bool ok = - mav_driver->enqueueMsg(TmRepository::getInstance()->packTM(tm_id)); + bool ok = mav_driver->enqueueMsg(TmRepository::getInstance().packTM(tm_id)); // update status - DeathStack::getInstance()->radio->logStatus(); + DeathStack::getInstance().radio->logStatus(); return ok; } diff --git a/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.h b/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.h index 7ac29dc46..5921a37de 100644 --- a/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.h +++ b/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.h @@ -37,7 +37,7 @@ bool sendTelemetry(MavDriver* mav_driver, const uint8_t tm_id); void logMavlinkStatus(MavDriver* mav_driver); -static LoggerService* logger = LoggerService::getInstance(); +static LoggerService* logger = &LoggerService::getInstance(); static PrintLogger print_logger = Logging::getLogger("deathstack.tmtc.tchandler"); diff --git a/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.cpp b/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.cpp index 77559358c..9d564a18d 100644 --- a/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.cpp +++ b/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.cpp @@ -34,23 +34,23 @@ TMTCController::TMTCController() : FSM(&TMTCController::stateGroundTM, skywardStack(16 * 1024)) { // init FSM - sEventBroker->subscribe(this, TOPIC_FLIGHT_EVENTS); - sEventBroker->subscribe(this, TOPIC_TMTC); + sEventBroker.subscribe(this, TOPIC_FLIGHT_EVENTS); + sEventBroker.subscribe(this, TOPIC_TMTC); LOG_DEBUG(log, "Created TMTCController"); } -TMTCController::~TMTCController() { sEventBroker->unsubscribe(this); } +TMTCController::~TMTCController() { sEventBroker.unsubscribe(this); } bool TMTCController::send(const uint8_t tm_id) { - MavDriver* mav_driver = DeathStack::getInstance()->radio->mav_driver; - TmRepository* tm_repo = DeathStack::getInstance()->radio->tm_repo; + MavDriver* mav_driver = DeathStack::getInstance().radio->mav_driver; + TmRepository* tm_repo = DeathStack::getInstance().radio->tm_repo; // enqueue the TM packet taking it from the TM repo (pauses kernel to // guarantee synchronicity) bool ok = mav_driver->enqueueMsg(tm_repo->packTM(tm_id)); // update status - DeathStack::getInstance()->radio->logStatus(); + DeathStack::getInstance().radio->logStatus(); return ok; } @@ -60,7 +60,7 @@ void TMTCController::sendSerialTelemetry() // TODO : this has a problem, mavlink bytes are printed along with debug // messages, which confuses everything #ifdef DEBUG - TmRepository* tm_repo = DeathStack::getInstance()->radio->tm_repo; + TmRepository* tm_repo = DeathStack::getInstance().radio->tm_repo; uint8_t buf[256]; mavlink_message_t msg = tm_repo->packTM(MAV_HR_TM_ID); uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); @@ -71,29 +71,29 @@ void TMTCController::sendSerialTelemetry() void TMTCController::stateGroundTM(const Event& ev) { - // TmRepository* tm_repo = DeathStack::getInstance()->radio->tm_repo; - switch (ev.sig) + // TmRepository* tm_repo = DeathStack::getInstance().radio->tm_repo; + switch (ev.code) { case EV_ENTRY: { // add periodic events - periodicHrEvId = sEventBroker->postDelayed<HR_TM_GROUND_TIMEOUT>( + periodicHrEvId = sEventBroker.postDelayed<HR_TM_GROUND_TIMEOUT>( Event{EV_SEND_HR_TM}, TOPIC_TMTC); // periodicSensEvId = - // sEventBroker->postDelayed<GROUND_SENS_TM_TIMEOUT>( + // sEventBroker.postDelayed<GROUND_SENS_TM_TIMEOUT>( // Event{EV_SEND_SENS_TM}, TOPIC_TMTC); LOG_DEBUG(log, "Entering stateGroundTM"); // log stack usage - StackLogger::getInstance()->updateStack(THID_TMTC_FSM); + StackLogger::getInstance().updateStack(THID_TMTC_FSM); break; } case EV_EXIT: { // remove periodic events - sEventBroker->removeDelayed(periodicHrEvId); - sEventBroker->removeDelayed(periodicSensEvId); + sEventBroker.removeDelayed(periodicHrEvId); + sEventBroker.removeDelayed(periodicSensEvId); LOG_DEBUG(log, "Exiting stateGroundTM"); break; @@ -101,7 +101,7 @@ void TMTCController::stateGroundTM(const Event& ev) case EV_SEND_HR_TM: { // repost periodic event - periodicHrEvId = sEventBroker->postDelayed<HR_TM_GROUND_TIMEOUT>( + periodicHrEvId = sEventBroker.postDelayed<HR_TM_GROUND_TIMEOUT>( Event{EV_SEND_HR_TM}, TOPIC_TMTC); send(MAV_HR_TM_ID); @@ -112,7 +112,7 @@ void TMTCController::stateGroundTM(const Event& ev) { // LOG_DEBUG(log, "Sending SENS_TM"); // periodicSensEvId = - // sEventBroker->postDelayed<GROUND_SENS_TM_TIMEOUT>( + // sEventBroker.postDelayed<GROUND_SENS_TM_TIMEOUT>( // Event{EV_SEND_SENS_TM}, TOPIC_TMTC); // send tm @@ -142,28 +142,28 @@ void TMTCController::stateGroundTM(const Event& ev) void TMTCController::stateFlightTM(const Event& ev) { - switch (ev.sig) + switch (ev.code) { case EV_ENTRY: { // add periodic events - periodicLrEvId = sEventBroker->postDelayed<LR_TM_TIMEOUT>( + periodicLrEvId = sEventBroker.postDelayed<LR_TM_TIMEOUT>( Event{EV_SEND_LR_TM}, TOPIC_TMTC); - periodicHrEvId = sEventBroker->postDelayed<HR_TM_TIMEOUT>( + periodicHrEvId = sEventBroker.postDelayed<HR_TM_TIMEOUT>( Event{EV_SEND_HR_TM}, TOPIC_TMTC); LOG_DEBUG(log, "Entering stateFlightTM"); // log stack usage - StackLogger::getInstance()->updateStack(THID_TMTC_FSM); + StackLogger::getInstance().updateStack(THID_TMTC_FSM); break; } case EV_EXIT: { // remove periodic events - sEventBroker->removeDelayed(periodicLrEvId); - sEventBroker->removeDelayed(periodicHrEvId); + sEventBroker.removeDelayed(periodicLrEvId); + sEventBroker.removeDelayed(periodicHrEvId); LOG_DEBUG(log, "Exiting stateFlightTM"); break; @@ -171,7 +171,7 @@ void TMTCController::stateFlightTM(const Event& ev) case EV_SEND_HR_TM: { // repost periodic event - periodicHrEvId = sEventBroker->postDelayed<HR_TM_TIMEOUT>( + periodicHrEvId = sEventBroker.postDelayed<HR_TM_TIMEOUT>( Event{EV_SEND_HR_TM}, TOPIC_TMTC); // send tm once 4 packets are filled @@ -186,7 +186,7 @@ void TMTCController::stateFlightTM(const Event& ev) case EV_SEND_LR_TM: { // repost periodic event - periodicLrEvId = sEventBroker->postDelayed<LR_TM_TIMEOUT>( + periodicLrEvId = sEventBroker.postDelayed<LR_TM_TIMEOUT>( Event{EV_SEND_LR_TM}, TOPIC_TMTC); // send low rate tm @@ -207,25 +207,25 @@ void TMTCController::stateFlightTM(const Event& ev) void TMTCController::stateSensorTM(const Event& ev) { - // TmRepository* tm_repo = DeathStack::getInstance()->radio->tm_repo; - switch (ev.sig) + // TmRepository* tm_repo = DeathStack::getInstance().radio->tm_repo; + switch (ev.code) { case EV_ENTRY: { // add periodic events - periodicSensEvId = sEventBroker->postDelayed<SENS_TM_TIMEOUT>( + periodicSensEvId = sEventBroker.postDelayed<SENS_TM_TIMEOUT>( Event{EV_SEND_SENS_TM}, TOPIC_TMTC); LOG_DEBUG(log, "Entering stateSensorTM"); // log stack usage - StackLogger::getInstance()->updateStack(THID_TMTC_FSM); + StackLogger::getInstance().updateStack(THID_TMTC_FSM); break; } case EV_EXIT: { // remove periodic events - sEventBroker->removeDelayed(periodicSensEvId); + sEventBroker.removeDelayed(periodicSensEvId); LOG_DEBUG(log, "Exiting stateSensorTM"); break; @@ -233,7 +233,7 @@ void TMTCController::stateSensorTM(const Event& ev) case EV_SEND_SENS_TM: { // repost periodic event - periodicSensEvId = sEventBroker->postDelayed<SENS_TM_TIMEOUT>( + periodicSensEvId = sEventBroker.postDelayed<SENS_TM_TIMEOUT>( Event{EV_SEND_SENS_TM}, TOPIC_TMTC); send(MAV_SENSORS_TM_ID); @@ -258,25 +258,25 @@ void TMTCController::stateSensorTM(const Event& ev) void TMTCController::stateSerialDebugTM(const Event& ev) { - // TmRepository* tm_repo = DeathStack::getInstance()->radio->tm_repo; - switch (ev.sig) + // TmRepository* tm_repo = DeathStack::getInstance().radio->tm_repo; + switch (ev.code) { case EV_ENTRY: { // add periodic events - periodicHrEvId = sEventBroker->postDelayed<HR_TM_GROUND_TIMEOUT>( + periodicHrEvId = sEventBroker.postDelayed<HR_TM_GROUND_TIMEOUT>( Event{EV_SEND_HR_TM_OVER_SERIAL}, TOPIC_TMTC); LOG_DEBUG(log, "Entering stateSerialDebugTM"); // log stack usage - StackLogger::getInstance()->updateStack(THID_TMTC_FSM); + StackLogger::getInstance().updateStack(THID_TMTC_FSM); break; } case EV_EXIT: { // remove periodic events - sEventBroker->removeDelayed(periodicHrEvId); + sEventBroker.removeDelayed(periodicHrEvId); LOG_DEBUG(log, "Exiting stateSerialDebugTM"); break; @@ -284,7 +284,7 @@ void TMTCController::stateSerialDebugTM(const Event& ev) case EV_SEND_HR_TM_OVER_SERIAL: { // repost periodic event - periodicHrEvId = sEventBroker->postDelayed<HR_TM_GROUND_TIMEOUT>( + periodicHrEvId = sEventBroker.postDelayed<HR_TM_GROUND_TIMEOUT>( Event{EV_SEND_HR_TM_OVER_SERIAL}, TOPIC_TMTC); sendSerialTelemetry(); diff --git a/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.h b/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.h index 880070170..a1fd9d3cd 100644 --- a/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.h +++ b/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.h @@ -77,7 +77,7 @@ private: void stateFlightTM(const Event& ev); void stateSerialDebugTM(const Event& ev); - LoggerService& logger = *(LoggerService::getInstance()); + LoggerService& logger = LoggerService::getInstance(); uint16_t periodicHrEvId = 0; uint16_t periodicLrEvId = 0; diff --git a/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.cpp b/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.cpp index 4edbb81c6..3028f8037 100644 --- a/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.cpp +++ b/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.cpp @@ -21,15 +21,14 @@ * THE SOFTWARE. */ -#include <Constants.h> #include <DeathStack.h> -#include <Debug.h> #include <FlightStatsRecorder/FSRController.h> #include <LoggerService/LoggerService.h> #include <System/TaskID.h> #include <TelemetriesTelecommands/TmRepository.h> #include <configs/SensorsConfig.h> #include <configs/TMTCConfig.h> +#include <utils/Constants.h> using namespace Boardcore; @@ -209,18 +208,18 @@ template <> void TmRepository::update<BatteryVoltageSensorData>( const BatteryVoltageSensorData& t) { - if (t.channel_id == DeathStackBoard::SensorConfigs::ADC_BATTERY_VOLTAGE) + if (t.channelId == DeathStackBoard::SensorConfigs::ADC_BATTERY_VOLTAGE) { - tm_repository.hr_tm.vbat = t.bat_voltage; - tm_repository.sensors_tm.vbat = t.bat_voltage; - tm_repository.adc_tm.bat_voltage = t.bat_voltage; + tm_repository.hr_tm.vbat = t.batVoltage; + tm_repository.sensors_tm.vbat = t.batVoltage; + tm_repository.adc_tm.bat_voltage = t.batVoltage; } } template <> void TmRepository::update<ADS1118Data>(const ADS1118Data& t) { - if (t.channel_id == SensorConfigs::ADC_CH_VREF) + if (t.channelId == SensorConfigs::ADC_CH_VREF) { // tm_repository.wind_tm.pressure_dpl = t.voltage; tm_repository.sensors_tm.vsupply_5v = t.voltage; @@ -232,17 +231,17 @@ void TmRepository::update<ADS1118Data>(const ADS1118Data& t) template <> void TmRepository::update<MS5803Data>(const MS5803Data& t) { - tm_repository.wind_tm.pressure_digital = t.press; - tm_repository.sensors_tm.ms5803_press = t.press; - tm_repository.digital_baro_tm.pressure = t.press; + tm_repository.wind_tm.pressure_digital = t.pressure; + tm_repository.sensors_tm.ms5803_press = t.pressure; + tm_repository.digital_baro_tm.pressure = t.pressure; #if !defined(HARDWARE_IN_THE_LOOP) && !defined(USE_MOCK_SENSORS) - tm_repository.hr_tm.pressure_digi = t.press; -#endif + tm_repository.hr_tm.pressure_digi = t.pressure; +#endif - tm_repository.sensors_tm.ms5803_temp = t.temp; - tm_repository.hr_tm.temperature = t.temp; - tm_repository.digital_baro_tm.temperature = t.temp; + tm_repository.sensors_tm.ms5803_temp = t.temperature; + tm_repository.hr_tm.temperature = t.temperature; + tm_repository.digital_baro_tm.temperature = t.temperature; stats_rec.update(t); } @@ -250,12 +249,12 @@ void TmRepository::update<MS5803Data>(const MS5803Data& t) template <> void TmRepository::update<MPXHZ6130AData>(const MPXHZ6130AData& t) { - tm_repository.wind_tm.pressure_static = t.press; - tm_repository.sensors_tm.static_press = t.press; - tm_repository.adc_tm.static_pressure = t.press; + tm_repository.wind_tm.pressure_static = t.pressure; + tm_repository.sensors_tm.static_press = t.pressure; + tm_repository.adc_tm.static_pressure = t.pressure; #if !defined(HARDWARE_IN_THE_LOOP) && !defined(USE_MOCK_SENSORS) - tm_repository.hr_tm.pressure_static = t.press; + tm_repository.hr_tm.pressure_static = t.pressure; #endif stats_rec.update(t); @@ -264,9 +263,9 @@ void TmRepository::update<MPXHZ6130AData>(const MPXHZ6130AData& t) template <> void TmRepository::update<SSCDRRN015PDAData>(const SSCDRRN015PDAData& t) { - tm_repository.wind_tm.pressure_differential = t.press; - tm_repository.sensors_tm.pitot_press = t.press; - tm_repository.adc_tm.pitot_pressure = t.press; + tm_repository.wind_tm.pressure_differential = t.pressure; + tm_repository.sensors_tm.pitot_press = t.pressure; + tm_repository.adc_tm.pitot_pressure = t.pressure; } template <> @@ -280,10 +279,10 @@ void TmRepository::update<AirSpeedPitot>(const AirSpeedPitot& t) template <> void TmRepository::update<SSCDANN030PAAData>(const SSCDANN030PAAData& t) { - tm_repository.wind_tm.pressure_dpl = t.press; - tm_repository.sensors_tm.dpl_press = t.press; - tm_repository.hr_tm.pressure_dpl = t.press; - tm_repository.adc_tm.dpl_pressure = t.press; + tm_repository.wind_tm.pressure_dpl = t.pressure; + tm_repository.sensors_tm.dpl_press = t.pressure; + tm_repository.hr_tm.pressure_dpl = t.pressure; + tm_repository.adc_tm.dpl_pressure = t.pressure; stats_rec.update(t); } @@ -292,40 +291,40 @@ void TmRepository::update<SSCDANN030PAAData>(const SSCDANN030PAAData& t) template <> void TmRepository::update<BMX160Data>(const BMX160Data& t) { - tm_repository.sensors_tm.bmx160_acc_x = t.accel_x; - tm_repository.sensors_tm.bmx160_acc_y = t.accel_y; - tm_repository.sensors_tm.bmx160_acc_z = t.accel_z; - tm_repository.sensors_tm.bmx160_gyro_x = t.gyro_x; - tm_repository.sensors_tm.bmx160_gyro_y = t.gyro_y; - tm_repository.sensors_tm.bmx160_gyro_z = t.gyro_z; - tm_repository.sensors_tm.bmx160_mag_x = t.mag_x; - tm_repository.sensors_tm.bmx160_mag_y = t.mag_y; - tm_repository.sensors_tm.bmx160_mag_z = t.mag_z; + tm_repository.sensors_tm.bmx160_acc_x = t.accelerationX; + tm_repository.sensors_tm.bmx160_acc_y = t.accelerationY; + tm_repository.sensors_tm.bmx160_acc_z = t.accelerationZ; + tm_repository.sensors_tm.bmx160_gyro_x = t.angularVelocityX; + tm_repository.sensors_tm.bmx160_gyro_y = t.angularVelocityY; + tm_repository.sensors_tm.bmx160_gyro_z = t.angularVelocityZ; + tm_repository.sensors_tm.bmx160_mag_x = t.magneticFieldX; + tm_repository.sensors_tm.bmx160_mag_y = t.magneticFieldY; + tm_repository.sensors_tm.bmx160_mag_z = t.magneticFieldZ; } template <> void TmRepository::update<BMX160WithCorrectionData>( const BMX160WithCorrectionData& t) { - tm_repository.bmx_tm.acc_x = t.accel_x; - tm_repository.bmx_tm.acc_y = t.accel_y; - tm_repository.bmx_tm.acc_z = t.accel_z; - tm_repository.bmx_tm.gyro_x = t.gyro_x; - tm_repository.bmx_tm.gyro_y = t.gyro_y; - tm_repository.bmx_tm.gyro_z = t.gyro_z; - tm_repository.bmx_tm.mag_x = t.mag_x; - tm_repository.bmx_tm.mag_y = t.mag_y; - tm_repository.bmx_tm.mag_z = t.mag_z; - - tm_repository.hr_tm.acc_x = t.accel_x; - tm_repository.hr_tm.acc_y = t.accel_y; - tm_repository.hr_tm.acc_z = t.accel_z; - tm_repository.hr_tm.gyro_x = t.gyro_x; - tm_repository.hr_tm.gyro_y = t.gyro_y; - tm_repository.hr_tm.gyro_z = t.gyro_z; - tm_repository.hr_tm.mag_x = t.mag_x; - tm_repository.hr_tm.mag_y = t.mag_y; - tm_repository.hr_tm.mag_z = t.mag_z; + tm_repository.bmx_tm.acc_x = t.accelerationX; + tm_repository.bmx_tm.acc_y = t.accelerationY; + tm_repository.bmx_tm.acc_z = t.accelerationZ; + tm_repository.bmx_tm.gyro_x = t.angularVelocityX; + tm_repository.bmx_tm.gyro_y = t.angularVelocityY; + tm_repository.bmx_tm.gyro_z = t.angularVelocityZ; + tm_repository.bmx_tm.mag_x = t.magneticFieldX; + tm_repository.bmx_tm.mag_y = t.magneticFieldY; + tm_repository.bmx_tm.mag_z = t.magneticFieldZ; + + tm_repository.hr_tm.acc_x = t.accelerationX; + tm_repository.hr_tm.acc_y = t.accelerationY; + tm_repository.hr_tm.acc_z = t.accelerationZ; + tm_repository.hr_tm.gyro_x = t.angularVelocityX; + tm_repository.hr_tm.gyro_y = t.angularVelocityY; + tm_repository.hr_tm.gyro_z = t.angularVelocityZ; + tm_repository.hr_tm.mag_x = t.magneticFieldX; + tm_repository.hr_tm.mag_y = t.magneticFieldY; + tm_repository.hr_tm.mag_z = t.magneticFieldZ; stats_rec.update(t); } @@ -334,22 +333,22 @@ void TmRepository::update<BMX160WithCorrectionData>( template <> void TmRepository::update<BMX160Temperature>(const BMX160Temperature& t) { - tm_repository.sensors_tm.bmx160_temp = t.temp; - tm_repository.bmx_tm.temp = t.temp; + tm_repository.sensors_tm.bmx160_temp = t.temperature; + tm_repository.bmx_tm.temp = t.temperature; } template <> void TmRepository::update<LIS3MDLData>(const LIS3MDLData& t) { - tm_repository.sensors_tm.lis3mdl_mag_x = t.mag_x; - tm_repository.sensors_tm.lis3mdl_mag_y = t.mag_y; - tm_repository.sensors_tm.lis3mdl_mag_z = t.mag_z; - tm_repository.sensors_tm.lis3mdl_temp = t.temp; + tm_repository.sensors_tm.lis3mdl_mag_x = t.magneticFieldX; + tm_repository.sensors_tm.lis3mdl_mag_y = t.magneticFieldY; + tm_repository.sensors_tm.lis3mdl_mag_z = t.magneticFieldZ; + tm_repository.sensors_tm.lis3mdl_temp = t.temperature; - tm_repository.lis3mdl_tm.mag_x = t.mag_x; - tm_repository.lis3mdl_tm.mag_y = t.mag_y; - tm_repository.lis3mdl_tm.mag_z = t.mag_z; - tm_repository.lis3mdl_tm.temp = t.temp; + tm_repository.lis3mdl_tm.mag_x = t.magneticFieldX; + tm_repository.lis3mdl_tm.mag_y = t.magneticFieldY; + tm_repository.lis3mdl_tm.mag_z = t.magneticFieldZ; + tm_repository.lis3mdl_tm.temp = t.temperature; } #if !defined(HARDWARE_IN_THE_LOOP) && !defined(USE_MOCK_SENSORS) @@ -357,19 +356,19 @@ void TmRepository::update<LIS3MDLData>(const LIS3MDLData& t) * @brief GPS. */ template <> -void TmRepository::update<UbloxGPSData>(const UbloxGPSData& t) +void TmRepository::update<GPSData>(const GPSData& t) { // GPS_TM tm_repository.gps_tm.latitude = t.latitude; tm_repository.gps_tm.longitude = t.longitude; tm_repository.gps_tm.height = t.height; - tm_repository.gps_tm.vel_north = t.velocity_north; - tm_repository.gps_tm.vel_east = t.velocity_east; - tm_repository.gps_tm.vel_down = t.velocity_down; + tm_repository.gps_tm.vel_north = t.velocityNorth; + tm_repository.gps_tm.vel_east = t.velocityEast; + tm_repository.gps_tm.vel_down = t.velocityDown; tm_repository.gps_tm.speed = t.speed; tm_repository.gps_tm.fix = (uint8_t)t.fix; tm_repository.gps_tm.track = t.track; - tm_repository.gps_tm.n_satellites = t.num_satellites; + tm_repository.gps_tm.n_satellites = t.satellites; // HR TM tm_repository.hr_tm.gps_lat = t.latitude; @@ -378,7 +377,7 @@ void TmRepository::update<UbloxGPSData>(const UbloxGPSData& t) tm_repository.hr_tm.gps_fix = (uint8_t)t.fix; // TEST TM - tm_repository.test_tm.gps_nsats = t.num_satellites; + tm_repository.test_tm.gps_nsats = t.satellites; stats_rec.update(t); } @@ -399,9 +398,9 @@ template <> void TmRepository::update<Xbee::ATCommandResponseFrameLog>( const Xbee::ATCommandResponseFrameLog& t) { - if (strncmp(t.at_command, "DB", 2) == 0 && t.command_data_length == 1) + if (strncmp(t.atCommand, "DB", 2) == 0 && t.commandDataLength == 1) { - tm_repository.wind_tm.last_RSSI = -t.command_data[0]; + tm_repository.wind_tm.last_RSSI = -t.commandData[0]; } } @@ -550,23 +549,23 @@ void TmRepository::update<PinStatus>(const PinStatus& t) * @brief Logger. */ template <> -void TmRepository::update<LogStats>(const LogStats& t) +void TmRepository::update<LoggerStats>(const LoggerStats& t) { tm_repository.logger_tm.statLogNumber = t.logNumber; - tm_repository.logger_tm.statTooLargeSamples = t.statTooLargeSamples; - tm_repository.logger_tm.statDroppedSamples = t.statDroppedSamples; - tm_repository.logger_tm.statQueuedSamples = t.statQueuedSamples; - tm_repository.logger_tm.statBufferFilled = t.statBufferFilled; - tm_repository.logger_tm.statBufferWritten = t.statBufferWritten; - tm_repository.logger_tm.statWriteFailed = t.statWriteFailed; - tm_repository.logger_tm.statWriteError = t.statWriteError; - tm_repository.logger_tm.statWriteTime = t.statWriteTime; - tm_repository.logger_tm.statMaxWriteTime = t.statMaxWriteTime; + tm_repository.logger_tm.statTooLargeSamples = t.tooLargeSamples; + tm_repository.logger_tm.statDroppedSamples = t.droppedSamples; + tm_repository.logger_tm.statQueuedSamples = t.queuedSamples; + tm_repository.logger_tm.statBufferFilled = t.buffersFilled; + tm_repository.logger_tm.statBufferWritten = t.buffersWritten; + tm_repository.logger_tm.statWriteFailed = t.writesFailed; + tm_repository.logger_tm.statWriteError = t.lastWriteError; + tm_repository.logger_tm.statWriteTime = t.writeTime; + tm_repository.logger_tm.statMaxWriteTime = t.maxWriteTime; tm_repository.wind_tm.log_num = t.logNumber; - tm_repository.wind_tm.log_status = t.opened ? t.statWriteError : -1000; + tm_repository.wind_tm.log_status = t.lastWriteError; - tm_repository.hr_tm.logger_error = t.opened ? t.statWriteError : -1; + tm_repository.hr_tm.logger_error = t.lastWriteError; } /** @@ -576,21 +575,21 @@ template <> void TmRepository::update<MavlinkStatus>(const MavlinkStatus& t) { // mavchannel stats - tm_repository.tmtc_tm.n_send_queue = t.n_send_queue; - tm_repository.tmtc_tm.max_send_queue = t.max_send_queue; - tm_repository.tmtc_tm.n_send_errors = t.n_send_errors; - tm_repository.tmtc_tm.msg_received = t.mav_stats.msg_received; + tm_repository.tmtc_tm.n_send_queue = t.nSendQueue; + tm_repository.tmtc_tm.max_send_queue = t.maxSendQueue; + tm_repository.tmtc_tm.n_send_errors = t.nSendErrors; + tm_repository.tmtc_tm.msg_received = t.mavStats.msg_received; // mav stats - tm_repository.tmtc_tm.buffer_overrun = t.mav_stats.buffer_overrun; - tm_repository.tmtc_tm.parse_error = t.mav_stats.parse_error; - tm_repository.tmtc_tm.parse_state = t.mav_stats.parse_state; - tm_repository.tmtc_tm.packet_idx = t.mav_stats.packet_idx; - tm_repository.tmtc_tm.current_rx_seq = t.mav_stats.current_rx_seq; - tm_repository.tmtc_tm.current_tx_seq = t.mav_stats.current_tx_seq; + tm_repository.tmtc_tm.buffer_overrun = t.mavStats.buffer_overrun; + tm_repository.tmtc_tm.parse_error = t.mavStats.parse_error; + tm_repository.tmtc_tm.parse_state = t.mavStats.parse_state; + tm_repository.tmtc_tm.packet_idx = t.mavStats.packet_idx; + tm_repository.tmtc_tm.current_rx_seq = t.mavStats.current_rx_seq; + tm_repository.tmtc_tm.current_tx_seq = t.mavStats.current_tx_seq; tm_repository.tmtc_tm.packet_rx_success_count = - t.mav_stats.packet_rx_success_count; + t.mavStats.packet_rx_success_count; tm_repository.tmtc_tm.packet_rx_drop_count = - t.mav_stats.packet_rx_drop_count; + t.mavStats.packet_rx_drop_count; } /** @@ -692,7 +691,7 @@ void TmRepository::update<TaskStatResult>(const TaskStatResult& t) tm_repository.task_stats_tm.task_sensors_6ms_mean = t.periodStats.mean; tm_repository.task_stats_tm.task_sensors_6ms_stddev = - t.periodStats.stdev; + t.periodStats.stdDev; break; case TASK_SENSORS_15_MS_ID: tm_repository.task_stats_tm.task_sensors_15ms_max = @@ -702,7 +701,7 @@ void TmRepository::update<TaskStatResult>(const TaskStatResult& t) tm_repository.task_stats_tm.task_sensors_15ms_mean = t.periodStats.mean; tm_repository.task_stats_tm.task_sensors_15ms_stddev = - t.periodStats.stdev; + t.periodStats.stdDev; break; case TASK_SENSORS_20_MS_ID: tm_repository.task_stats_tm.task_sensors_20ms_max = @@ -712,7 +711,7 @@ void TmRepository::update<TaskStatResult>(const TaskStatResult& t) tm_repository.task_stats_tm.task_sensors_20ms_mean = t.periodStats.mean; tm_repository.task_stats_tm.task_sensors_20ms_stddev = - t.periodStats.stdev; + t.periodStats.stdDev; break; case TASK_SENSORS_24_MS_ID: tm_repository.task_stats_tm.task_sensors_24ms_max = @@ -722,7 +721,7 @@ void TmRepository::update<TaskStatResult>(const TaskStatResult& t) tm_repository.task_stats_tm.task_sensors_24ms_mean = t.periodStats.mean; tm_repository.task_stats_tm.task_sensors_24ms_stddev = - t.periodStats.stdev; + t.periodStats.stdDev; break; case TASK_SENSORS_40_MS_ID: tm_repository.task_stats_tm.task_sensors_40ms_max = @@ -732,7 +731,7 @@ void TmRepository::update<TaskStatResult>(const TaskStatResult& t) tm_repository.task_stats_tm.task_sensors_40ms_mean = t.periodStats.mean; tm_repository.task_stats_tm.task_sensors_40ms_stddev = - t.periodStats.stdev; + t.periodStats.stdDev; break; case TASK_SENSORS_1000_MS_ID: tm_repository.task_stats_tm.task_sensors_1000ms_max = @@ -742,25 +741,25 @@ void TmRepository::update<TaskStatResult>(const TaskStatResult& t) tm_repository.task_stats_tm.task_sensors_1000ms_mean = t.periodStats.mean; tm_repository.task_stats_tm.task_sensors_1000ms_stddev = - t.periodStats.stdev; + t.periodStats.stdDev; break; case TASK_ADA_ID: tm_repository.task_stats_tm.task_ada_max = t.periodStats.maxValue; tm_repository.task_stats_tm.task_ada_min = t.periodStats.minValue; tm_repository.task_stats_tm.task_ada_mean = t.periodStats.mean; - tm_repository.task_stats_tm.task_ada_stddev = t.periodStats.stdev; + tm_repository.task_stats_tm.task_ada_stddev = t.periodStats.stdDev; break; case TASK_ABK_ID: tm_repository.task_stats_tm.task_abk_max = t.periodStats.maxValue; tm_repository.task_stats_tm.task_abk_min = t.periodStats.minValue; tm_repository.task_stats_tm.task_abk_mean = t.periodStats.mean; - tm_repository.task_stats_tm.task_abk_stddev = t.periodStats.stdev; + tm_repository.task_stats_tm.task_abk_stddev = t.periodStats.stdDev; break; case TASK_NAS_ID: tm_repository.task_stats_tm.task_nas_max = t.periodStats.maxValue; tm_repository.task_stats_tm.task_nas_min = t.periodStats.minValue; tm_repository.task_stats_tm.task_nas_mean = t.periodStats.mean; - tm_repository.task_stats_tm.task_nas_stddev = t.periodStats.stdev; + tm_repository.task_stats_tm.task_nas_stddev = t.periodStats.stdDev; break; // case TASK_SCHEDULER_STATS_ID: // tm_repository.task_stats_tm.task_250hz_max = @@ -770,7 +769,7 @@ void TmRepository::update<TaskStatResult>(const TaskStatResult& t) // tm_repository.task_stats_tm.task_250hz_mean = // t.periodStats.mean; // tm_repository.task_stats_tm.task_250hz_stddev = - // t.periodStats.stdev; break; + // t.periodStats.stdDev; break; default: break; diff --git a/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.h b/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.h index fa1077442..c8141b2ac 100644 --- a/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.h +++ b/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.h @@ -29,17 +29,17 @@ #include <FlightModeManager/FMMStatus.h> #include <FlightStatsRecorder/FSRController.h> #include <FlightStatsRecorder/FSRData.h> +#include <Main/SensorsData.h> #include <NavigationAttitudeSystem/NASData.h> #include <PinHandler/PinHandlerData.h> #include <Singleton.h> #include <SystemData.h> -#include <Main/SensorsData.h> #include <TelemetriesTelecommands/Mavlink.h> +#include <configs/TMTCConfig.h> #include <diagnostic/PrintLogger.h> #include <drivers/Xbee/APIFramesLog.h> -#include <drivers/adc/ADS1118/ADS1118Data.h> -#include <drivers/gps/ublox/UbloxGPSData.h> #include <scheduler/TaskSchedulerData.h> +#include <sensors/ADS1118/ADS1118Data.h> #include <sensors/BMX160/BMX160WithCorrectionData.h> #include <sensors/LIS3MDL/LIS3MDLData.h> #include <sensors/MS5803/MS5803Data.h> @@ -49,8 +49,6 @@ #include <sensors/analog/pressure/honeywell/SSCDANN030PAAData.h> #include <sensors/analog/pressure/honeywell/SSCDRRN015PDAData.h> -#include <configs/TMTCConfig.h> - #ifdef HARDWARE_IN_THE_LOOP #include <hardware_in_the_loop/HIL_sensors/HILSensors.h> #elif defined(USE_MOCK_SENSORS) @@ -175,7 +173,7 @@ void TmRepository::update<AirSpeedPitot>(const AirSpeedPitot& t); template <> void TmRepository::update<SSCDANN030PAAData>(const SSCDANN030PAAData& t); -#if !defined(HARDWARE_IN_THE_LOOP) && !defined(USE_MOCK_SENSORS) +#if !defined(HARDWARE_IN_THE_LOOP) && !defined(USE_MOCK_SENSORS) template <> void TmRepository::update<BMX160Data>(const BMX160Data& t); @@ -190,9 +188,9 @@ void TmRepository::update<BMX160Temperature>(const BMX160Temperature& t); template <> void TmRepository::update<LIS3MDLData>(const LIS3MDLData& t); -#if !defined(HARDWARE_IN_THE_LOOP) && !defined(USE_MOCK_SENSORS) +#if !defined(HARDWARE_IN_THE_LOOP) && !defined(USE_MOCK_SENSORS) template <> -void TmRepository::update<UbloxGPSData>(const UbloxGPSData& t); +void TmRepository::update<GPSData>(const GPSData& t); #endif template <> @@ -213,7 +211,7 @@ void TmRepository::update<Xbee::ATCommandResponseFrameLog>( * @brief Logger. */ template <> -void TmRepository::update<LogStats>(const LogStats& t); +void TmRepository::update<LoggerStats>(const LoggerStats& t); /** * @brief Initialization status of the board. diff --git a/src/boards/DeathStack/configs/ADAConfig.h b/src/boards/DeathStack/configs/ADAConfig.h index ce0a20746..2d4734402 100644 --- a/src/boards/DeathStack/configs/ADAConfig.h +++ b/src/boards/DeathStack/configs/ADAConfig.h @@ -74,12 +74,12 @@ static const float KALMAN_INITIAL_ACCELERATION = -500; static const uint8_t KALMAN_STATES_NUM = 3; static const uint8_t KALMAN_OUTPUTS_NUM = 1; -using MatrixNN = Matrix<float, KALMAN_STATES_NUM, KALMAN_STATES_NUM>; -using MatrixPN = Matrix<float, KALMAN_OUTPUTS_NUM, KALMAN_STATES_NUM>; -using MatrixNP = Matrix<float, KALMAN_STATES_NUM, KALMAN_OUTPUTS_NUM>; -using MatrixPP = Matrix<float, KALMAN_OUTPUTS_NUM, KALMAN_OUTPUTS_NUM>; -using CVectorN = Matrix<float, KALMAN_STATES_NUM, 1>; -using CVectorP = Matrix<float, KALMAN_OUTPUTS_NUM, 1>; +using MatrixNN = Eigen::Matrix<float, KALMAN_STATES_NUM, KALMAN_STATES_NUM>; +using MatrixPN = Eigen::Matrix<float, KALMAN_OUTPUTS_NUM, KALMAN_STATES_NUM>; +using MatrixNP = Eigen::Matrix<float, KALMAN_STATES_NUM, KALMAN_OUTPUTS_NUM>; +using MatrixPP = Eigen::Matrix<float, KALMAN_OUTPUTS_NUM, KALMAN_OUTPUTS_NUM>; +using CVectorN = Eigen::Matrix<float, KALMAN_STATES_NUM, 1>; +using CVectorP = Eigen::Matrix<float, KALMAN_OUTPUTS_NUM, 1>; // clang-format off static inline MatrixNN f_init() diff --git a/src/boards/DeathStack/configs/AirBrakesConfig.h b/src/boards/DeathStack/configs/AirBrakesConfig.h index cef109a53..e01b39474 100644 --- a/src/boards/DeathStack/configs/AirBrakesConfig.h +++ b/src/boards/DeathStack/configs/AirBrakesConfig.h @@ -22,9 +22,8 @@ #pragma once -#include <Constants.h> -#include <drivers/HardwareTimer.h> -#include <drivers/pwm/pwm.h> +#include <drivers/timer/PWM.h> +#include <utils/Constants.h> namespace DeathStackBoard { @@ -32,17 +31,15 @@ namespace DeathStackBoard namespace AirBrakesConfigs { -static const PWM::Timer AB_SERVO_TIMER{ - TIM8, &(RCC->APB2ENR), RCC_APB2ENR_TIM8EN, - TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB2)}; - -static constexpr PWMChannel AB_SERVO_PWM_CH = PWMChannel::CH2; +static TIM_TypeDef* const AB_SERVO_TIMER = TIM8; +static constexpr TimerUtils::Channel AB_SERVO_PWM_CH = + TimerUtils::Channel::CHANNEL_2; // Rocket's parameters #ifdef EUROC static constexpr float M = 27.0; /**< rocket's mass */ #else -static constexpr float M = 18.362; /**< rocket's mass */ +static constexpr float M = 18.362; /**< rocket's mass */ #endif static constexpr float D = 0.15; /**< rocket's diameter */ @@ -87,7 +84,7 @@ static constexpr float FILTER_COEFF = 0.9; #ifdef HARDWARE_IN_THE_LOOP static constexpr float ABK_UPDATE_PERIOD = 0.1 * 1000; // ms -> 10 Hz #else -static constexpr float ABK_UPDATE_PERIOD = 0.05 * 1000; // ms -> 20 Hz +static constexpr float ABK_UPDATE_PERIOD = 0.05 * 1000; // ms -> 20 Hz #endif static constexpr float ABK_UPDATE_PERIOD_SECONDS = ABK_UPDATE_PERIOD / 1000; diff --git a/src/boards/DeathStack/configs/CutterConfig.h b/src/boards/DeathStack/configs/CutterConfig.h index 964a59300..daed3af10 100644 --- a/src/boards/DeathStack/configs/CutterConfig.h +++ b/src/boards/DeathStack/configs/CutterConfig.h @@ -22,8 +22,7 @@ #pragma once -#include <drivers/HardwareTimer.h> -#include <drivers/pwm/pwm.h> +#include <drivers/timer/PWM.h> #include <interfaces-impl/hwmapping.h> namespace DeathStackBoard diff --git a/src/boards/DeathStack/configs/DeploymentConfig.h b/src/boards/DeathStack/configs/DeploymentConfig.h index 04f0d71a5..b2fdaace3 100644 --- a/src/boards/DeathStack/configs/DeploymentConfig.h +++ b/src/boards/DeathStack/configs/DeploymentConfig.h @@ -22,8 +22,8 @@ #pragma once -#include <drivers/HardwareTimer.h> -#include <drivers/pwm/pwm.h> +#include <drivers/timer/PWM.h> +#include <drivers/timer/TimerUtils.h> using namespace Boardcore; @@ -33,11 +33,9 @@ namespace DeathStackBoard namespace DeploymentConfigs { -static const PWM::Timer DPL_SERVO_TIMER{ - TIM4, &(RCC->APB1ENR), RCC_APB1ENR_TIM4EN, - TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB1)}; - -static constexpr PWMChannel DPL_SERVO_PWM_CH = PWMChannel::CH1; +static TIM_TypeDef* const DPL_SERVO_TIMER = TIM4; +static constexpr TimerUtils::Channel DPL_SERVO_PWM_CH = + TimerUtils::Channel::CHANNEL_1; static constexpr int NC_OPEN_TIMEOUT = 5000; diff --git a/src/boards/DeathStack/configs/NASConfig.h b/src/boards/DeathStack/configs/NASConfig.h index 0fe5edeff..5c8d00f1c 100644 --- a/src/boards/DeathStack/configs/NASConfig.h +++ b/src/boards/DeathStack/configs/NASConfig.h @@ -22,7 +22,8 @@ #pragma once -#include "Constants.h" +#include <utils/Constants.h> + #include "Eigen/Dense" using namespace Eigen; diff --git a/src/boards/DeathStack/configs/SensorsConfig.h b/src/boards/DeathStack/configs/SensorsConfig.h index e1c82647a..d0cfd9573 100644 --- a/src/boards/DeathStack/configs/SensorsConfig.h +++ b/src/boards/DeathStack/configs/SensorsConfig.h @@ -22,9 +22,9 @@ #pragma once -#include <drivers/adc/ADS1118/ADS1118.h> -#include <drivers/adc/InternalADC/InternalADC.h> +#include <drivers/adc/InternalADC.h> #include <interfaces-impl/hwmapping.h> +#include <sensors/ADS1118/ADS1118.h> #include <sensors/BMX160/BMX160Config.h> #include <sensors/LIS3MDL/LIS3MDL.h> #include <sensors/calibration/Calibration.h> diff --git a/src/boards/Payload/Main/Radio.cpp b/src/boards/Payload/Main/Radio.cpp index 39944320e..31a52fabf 100644 --- a/src/boards/Payload/Main/Radio.cpp +++ b/src/boards/Payload/Main/Radio.cpp @@ -42,9 +42,9 @@ void __attribute__((used)) EXTI10_IRQHandlerImpl() { using namespace PayloadBoard; - /*if (DeathStack::getInstance()->radio->xbee != nullptr) + /*if (DeathStack::getInstance().radio->xbee != nullptr) { - DeathStack::getInstance()->radio->xbee->handleATTNInterrupt(); + DeathStack::getInstance().radio->xbee->handleATTNInterrupt(); }*/ } @@ -55,7 +55,7 @@ Radio::Radio(SPIBusInterface& xbee_bus) : xbee_bus(xbee_bus) { SPIBusConfig xbee_cfg{}; - xbee_cfg.clock_div = SPIClockDivider::DIV16; + xbee_cfg.clockDivider = SPI::ClockDivider::DIV_16; xbee = new Xbee::Xbee(xbee_bus, xbee_cfg, miosix::xbee::cs::getPin(), miosix::xbee::attn::getPin(), @@ -93,11 +93,11 @@ bool Radio::start() void Radio::onXbeeFrameReceived(Xbee::APIFrame& frame) { - //LoggerService& logger = *LoggerService::getInstance(); + // LoggerService& logger = *LoggerService::getInstance(); using namespace Xbee; bool logged = false; - switch (frame.frame_type) + switch (frame.frameType) { case FTYPE_AT_COMMAND: { @@ -105,7 +105,7 @@ void Radio::onXbeeFrameReceived(Xbee::APIFrame& frame) logged = ATCommandFrameLog::toFrameType(frame, &dest); if (logged) { - //logger.log(dest); + // logger.log(dest); } break; } @@ -115,7 +115,7 @@ void Radio::onXbeeFrameReceived(Xbee::APIFrame& frame) logged = ATCommandResponseFrameLog::toFrameType(frame, &dest); if (logged) { - //logger.log(dest); + // logger.log(dest); } break; } @@ -125,7 +125,7 @@ void Radio::onXbeeFrameReceived(Xbee::APIFrame& frame) logged = ModemStatusFrameLog::toFrameType(frame, &dest); if (logged) { - //logger.log(dest); + // logger.log(dest); } break; } @@ -135,7 +135,7 @@ void Radio::onXbeeFrameReceived(Xbee::APIFrame& frame) logged = TXRequestFrameLog::toFrameType(frame, &dest); if (logged) { - //logger.log(dest); + // logger.log(dest); } break; } @@ -145,7 +145,7 @@ void Radio::onXbeeFrameReceived(Xbee::APIFrame& frame) logged = TXStatusFrameLog::toFrameType(frame, &dest); if (logged) { - //logger.log(dest); + // logger.log(dest); } break; } @@ -155,7 +155,7 @@ void Radio::onXbeeFrameReceived(Xbee::APIFrame& frame) logged = RXPacketFrameLog::toFrameType(frame, &dest); if (logged) { - //logger.log(dest); + // logger.log(dest); } break; } @@ -165,16 +165,16 @@ void Radio::onXbeeFrameReceived(Xbee::APIFrame& frame) { APIFrameLog api; APIFrameLog::fromAPIFrame(frame, &api); - //logger.log(api); + // logger.log(api); } } void Radio::logStatus() { - //MavlinkStatus status = mav_driver->getStatus(); - //status.timestamp = TimestampTimer::getTimestamp(); - //LoggerService::getInstance()->log(status); - //LoggerService::getInstance()->log(xbee->getStatus()); + // MavlinkStatus status = mav_driver->getStatus(); + // status.timestamp = TimestampTimer::getInstance().getTimestamp(); + // LoggerService::getInstance().log(status); + // LoggerService::getInstance().log(xbee->getStatus()); } } // namespace PayloadBoard \ No newline at end of file diff --git a/src/boards/Payload/Main/Sensors.cpp b/src/boards/Payload/Main/Sensors.cpp index 48d38f6ed..695403d72 100644 --- a/src/boards/Payload/Main/Sensors.cpp +++ b/src/boards/Payload/Main/Sensors.cpp @@ -22,11 +22,10 @@ //#include <ApogeeDetectionAlgorithm/ADAController.h> #include <Payload/PayloadBoard.h> -#include <Debug.h> //#include <LoggerService/LoggerService.h> -#include <TimestampTimer.h> #include <Payload/configs/SensorsConfig.h> #include <drivers/interrupt/external_interrupts.h> +#include <drivers/timer/TimestampTimer.h> #include <interfaces-impl/hwmapping.h> #include <sensors/Sensor.h> #include <utils/aero/AeroUtils.h> @@ -44,10 +43,10 @@ void __attribute__((used)) EXTI5_IRQHandlerImpl() { using namespace PayloadBoard; - /*if (Payload::getInstance()->sensors->imu_bmx160 != nullptr) + /*if (Payload::getInstance().sensors->imu_bmx160 != nullptr) { - Payload::getInstance()->sensors->imu_bmx160->IRQupdateTimestamp( - TimestampTimer::getTimestamp()); + Payload::getInstance().ensors->imu_bmx160->IRQupdateTimestamp( + TimestampTimer::getInstance().getTimestamp()); }*/ } @@ -72,7 +71,7 @@ Sensors::Sensors(SPIBusInterface& spi1_bus, TaskScheduler* scheduler) internalAdcInit(); batteryVoltageInit(); - sensor_manager = new SensorManager(scheduler, sensors_map); + sensor_manager = new SensorManager(sensors_map, scheduler); } Sensors::~Sensors() @@ -108,7 +107,7 @@ bool Sensors::start() updateSensorsStatus(); } - //LoggerService::getInstance()->log(status); + // LoggerService::getInstance().log(status); return sm_start_result; } @@ -116,7 +115,7 @@ bool Sensors::start() void Sensors::calibrate() { imu_bmx160_with_correction->calibrate(); - //LoggerService::getInstance()->log( + // LoggerService::getInstance().log( // imu_bmx160_with_correction->getGyroscopeBiases()); press_pitot->calibrate(); @@ -127,7 +126,7 @@ void Sensors::calibrate() for (unsigned int i = 0; i < PRESS_STATIC_CALIB_SAMPLES_NUM / 10; i++) { Thread::sleep(SAMPLE_PERIOD_PRESS_DIGITAL); - press_digi_stats.add(press_digital->getLastSample().press); + press_digi_stats.add(press_digital->getLastSample().pressure); } press_static_port->setReferencePressure(press_digi_stats.getStats().mean); press_static_port->calibrate(); @@ -141,12 +140,12 @@ void Sensors::calibrate() void Sensors::internalAdcInit() { - internal_adc = new InternalADC(*ADC3, INTERNAL_ADC_VREF); + internal_adc = new InternalADC(ADC3, INTERNAL_ADC_VREF); internal_adc->enableChannel(ADC_BATTERY_VOLTAGE); SensorInfo info("InternalADC", SAMPLE_PERIOD_INTERNAL_ADC, - bind(&Sensors::internalAdcCallback, this), false, true); + bind(&Sensors::internalAdcCallback, this)); sensors_map.emplace(std::make_pair(internal_adc, info)); LOG_INFO(log, "InternalADC setup done!"); @@ -160,7 +159,7 @@ void Sensors::batteryVoltageInit() new BatteryVoltageSensor(voltage_fun, BATTERY_VOLTAGE_COEFF); SensorInfo info("BatterySensor", SAMPLE_PERIOD_INTERNAL_ADC, - bind(&Sensors::batteryVoltageCallback, this), false, true); + bind(&Sensors::batteryVoltageCallback, this)); sensors_map.emplace(std::make_pair(battery_voltage, info)); @@ -170,13 +169,13 @@ void Sensors::batteryVoltageInit() void Sensors::pressDigiInit() { SPIBusConfig spi_cfg{}; - spi_cfg.clock_div = SPIClockDivider::DIV16; + spi_cfg.clockDivider = SPI::ClockDivider::DIV_16; press_digital = new MS5803(spi1_bus, miosix::sensors::ms5803::cs::getPin(), spi_cfg, TEMP_DIVIDER_PRESS_DIGITAL); SensorInfo info("DigitalBarometer", SAMPLE_PERIOD_PRESS_DIGITAL, - bind(&Sensors::pressDigiCallback, this), false, true); + bind(&Sensors::pressDigiCallback, this)); sensors_map.emplace(std::make_pair(press_digital, info)); @@ -186,7 +185,7 @@ void Sensors::pressDigiInit() void Sensors::ADS1118Init() { SPIBusConfig spi_cfg = ADS1118::getDefaultSPIConfig(); - spi_cfg.clock_div = SPIClockDivider::DIV64; + spi_cfg.clockDivider = SPI::ClockDivider::DIV_64; ADS1118::ADS1118Config ads1118Config = ADS1118::ADS1118_DEFAULT_CONFIG; ads1118Config.bits.mode = ADS1118::ADS1118Mode::CONTIN_CONV_MODE; @@ -205,7 +204,7 @@ void Sensors::ADS1118Init() adc_ads1118->enableInput(ADC_CH_VREF, ADC_DR_VREF, ADC_PGA_VREF); SensorInfo info("ADS1118", SAMPLE_PERIOD_ADC_ADS1118, - bind(&Sensors::ADS1118Callback, this), false, true); + bind(&Sensors::ADS1118Callback, this)); sensors_map.emplace(std::make_pair(adc_ads1118, info)); LOG_INFO(log, "ADS1118 setup done!"); @@ -219,7 +218,7 @@ void Sensors::pressPitotInit() PRESS_PITOT_CALIB_SAMPLES_NUM); SensorInfo info("DiffBarometer", SAMPLE_PERIOD_PRESS_PITOT, - bind(&Sensors::pressPitotCallback, this), false, true); + bind(&Sensors::pressPitotCallback, this)); sensors_map.emplace(std::make_pair(press_pitot, info)); @@ -233,7 +232,7 @@ void Sensors::pressDPLVaneInit() press_dpl_vane = new SSCDANN030PAA(voltage_fun, REFERENCE_VOLTAGE); SensorInfo info("DeploymentBarometer", SAMPLE_PERIOD_PRESS_DPL, - bind(&Sensors::pressDPLVaneCallback, this), false, true); + bind(&Sensors::pressDPLVaneCallback, this)); sensors_map.emplace(std::make_pair(press_dpl_vane, info)); @@ -249,7 +248,7 @@ void Sensors::pressStaticInit() PRESS_STATIC_MOVING_AVG_COEFF); SensorInfo info("StaticPortsBarometer", SAMPLE_PERIOD_PRESS_STATIC, - bind(&Sensors::pressStaticCallback, this), false, true); + bind(&Sensors::pressStaticCallback, this)); sensors_map.emplace(std::make_pair(press_static_port, info)); @@ -259,29 +258,29 @@ void Sensors::pressStaticInit() void Sensors::imuBMXInit() { SPIBusConfig spi_cfg; - spi_cfg.clock_div = SPIClockDivider::DIV8; + spi_cfg.clockDivider = SPI::ClockDivider::DIV_8; BMX160Config bmx_config; - bmx_config.fifo_mode = BMX160Config::FifoMode::HEADER; - bmx_config.fifo_watermark = IMU_BMX_FIFO_WATERMARK; - bmx_config.fifo_int = BMX160Config::FifoInterruptPin::PIN_INT1; + bmx_config.fifoMode = BMX160Config::FifoMode::HEADER; + bmx_config.fifoWatermark = IMU_BMX_FIFO_WATERMARK; + bmx_config.fifoInterrupt = BMX160Config::FifoInterruptPin::PIN_INT1; - bmx_config.temp_divider = IMU_BMX_TEMP_DIVIDER; + bmx_config.temperatureDivider = IMU_BMX_TEMP_DIVIDER; - bmx_config.acc_range = IMU_BMX_ACC_FULLSCALE_ENUM; - bmx_config.gyr_range = IMU_BMX_GYRO_FULLSCALE_ENUM; + bmx_config.accelerometerRange = IMU_BMX_ACC_FULLSCALE_ENUM; + bmx_config.gyroscopeRange = IMU_BMX_GYRO_FULLSCALE_ENUM; - bmx_config.acc_odr = IMU_BMX_ACC_GYRO_ODR_ENUM; - bmx_config.gyr_odr = IMU_BMX_ACC_GYRO_ODR_ENUM; - bmx_config.mag_odr = IMU_BMX_MAG_ODR_ENUM; + bmx_config.accelerometerDataRate = IMU_BMX_ACC_GYRO_ODR_ENUM; + bmx_config.gyroscopeDataRate = IMU_BMX_ACC_GYRO_ODR_ENUM; + bmx_config.magnetometerRate = IMU_BMX_MAG_ODR_ENUM; - bmx_config.gyr_unit = BMX160Config::GyroscopeMeasureUnit::RAD; + bmx_config.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD; imu_bmx160 = new BMX160(spi1_bus, miosix::sensors::bmx160::cs::getPin(), bmx_config, spi_cfg); SensorInfo info("BMX160", SAMPLE_PERIOD_IMU_BMX, - bind(&Sensors::imuBMXCallback, this), false, true); + bind(&Sensors::imuBMXCallback, this)); sensors_map.emplace(std::make_pair(imu_bmx160, info)); @@ -329,8 +328,7 @@ void Sensors::imuBMXWithCorrectionInit() imu_bmx160, correctionParameters, BMX160_AXIS_ROTATION); SensorInfo info("BMX160WithCorrection", SAMPLE_PERIOD_IMU_BMX, - bind(&Sensors::imuBMXWithCorrectionCallback, this), false, - true); + bind(&Sensors::imuBMXWithCorrectionCallback, this)); sensors_map.emplace(std::make_pair(imu_bmx160_with_correction, info)); @@ -340,7 +338,7 @@ void Sensors::imuBMXWithCorrectionInit() void Sensors::magLISinit() { SPIBusConfig busConfig; - busConfig.clock_div = SPIClockDivider::DIV32; + busConfig.clockDivider = SPI::ClockDivider::DIV_32; LIS3MDL::Config config; config.odr = MAG_LIS_ODR_ENUM; @@ -351,7 +349,7 @@ void Sensors::magLISinit() busConfig, config); SensorInfo info("LIS3MDL", SAMPLE_PERIOD_MAG_LIS, - bind(&Sensors::magLISCallback, this), false, true); + bind(&Sensors::magLISCallback, this)); sensors_map.emplace(std::make_pair(mag_lis3mdl, info)); @@ -360,10 +358,10 @@ void Sensors::magLISinit() void Sensors::gpsUbloxInit() { - gps_ublox = new UbloxGPS(GPS_BAUD_RATE, GPS_SAMPLE_RATE); + gps_ublox = new UbloxGPSSerial(GPS_BAUD_RATE, GPS_SAMPLE_RATE); SensorInfo info("UbloxGPS", GPS_SAMPLE_PERIOD, - bind(&Sensors::gpsUbloxCallback, this), false, true); + bind(&Sensors::gpsUbloxCallback, this)); sensors_map.emplace(std::make_pair(gps_ublox, info)); @@ -372,12 +370,12 @@ void Sensors::gpsUbloxInit() void Sensors::internalAdcCallback() { - //LoggerService::getInstance()->log(internal_adc->getLastSample()); + // LoggerService::getInstance().log(internal_adc->getLastSample()); } void Sensors::batteryVoltageCallback() { - static float v = battery_voltage->getLastSample().bat_voltage; + static float v = battery_voltage->getLastSample().batVoltage; if (v < BATTERY_MIN_SAFE_VOLTAGE) { battery_critical_counter++; @@ -389,48 +387,48 @@ void Sensors::batteryVoltageCallback() } } - //LoggerService::getInstance()->log(battery_voltage->getLastSample()); + // LoggerService::getInstance().log(battery_voltage->getLastSample()); } void Sensors::pressDigiCallback() { - //LoggerService::getInstance()->log(press_digital->getLastSample()); + // LoggerService::getInstance().log(press_digital->getLastSample()); } void Sensors::ADS1118Callback() { - //LoggerService::getInstance()->log(adc_ads1118->getLastSample()); + // LoggerService::getInstance().log(adc_ads1118->getLastSample()); } void Sensors::pressPitotCallback() { SSCDRRN015PDAData d = press_pitot->getLastSample(); - //LoggerService::getInstance()->log(d); + // LoggerService::getInstance().log(d); /*ADAReferenceValues rv = DeathStack::getInstance() ->state_machines->ada_controller->getReferenceValues(); float rel_density = aeroutils::relDensity( - press_digital->getLastSample().press, rv.ref_pressure, rv.ref_altitude, - rv.ref_temperature); - if (rel_density != 0.0f) + press_digital->getLastSample().pressure, rv.ref_pressure, + rv.ref_altitude, rv.ref_temperature); if (rel_density != 0.0f) { - float airspeed = sqrtf(2 * fabs(d.press) / rel_density); + float airspeed = sqrtf(2 * fabs(d.pressure) / rel_density); - AirSpeedPitot aspeed_data{TimestampTimer::getTimestamp(), airspeed}; - //LoggerService::getInstance()->log(aspeed_data); + AirSpeedPitot + aspeed_data{TimestampTimer::getInstance().getTimestamp(), airspeed}; + //LoggerService::getInstance().log(aspeed_data); }*/ } void Sensors::pressDPLVaneCallback() { - //LoggerService::getInstance()->log(press_dpl_vane->getLastSample()); + // LoggerService::getInstance().log(press_dpl_vane->getLastSample()); } void Sensors::pressStaticCallback() { - //LoggerService::getInstance()->log(press_static_port->getLastSample()); + // LoggerService::getInstance().log(press_static_port->getLastSample()); } void Sensors::imuBMXCallback() @@ -438,62 +436,60 @@ void Sensors::imuBMXCallback() uint8_t fifo_size = imu_bmx160->getLastFifoSize(); auto& fifo = imu_bmx160->getLastFifo(); - //LoggerService::getInstance()->log(imu_bmx160->getTemperature()); + // LoggerService::getInstance().log(imu_bmx160->getTemperature()); for (uint8_t i = 0; i < fifo_size; ++i) { - //LoggerService::getInstance()->log(fifo.at(i)); + // LoggerService::getInstance().log(fifo.at(i)); } - //LoggerService::getInstance()->log(imu_bmx160->getFifoStats()); + // LoggerService::getInstance().log(imu_bmx160->getFifoStats()); } void Sensors::imuBMXWithCorrectionCallback() { - //LoggerService::getInstance()->log( + // LoggerService::getInstance().log( // imu_bmx160_with_correction->getLastSample()); } void Sensors::magLISCallback() { - //LoggerService::getInstance()->log(mag_lis3mdl->getLastSample()); + // LoggerService::getInstance().log(mag_lis3mdl->getLastSample()); } void Sensors::gpsUbloxCallback() { - //LoggerService::getInstance()->log(gps_ublox->getLastSample()); + // LoggerService::getInstance().log(gps_ublox->getLastSample()); } void Sensors::updateSensorsStatus() { - SensorInfo info; - - info = sensor_manager->getSensorInfo(imu_bmx160); - if (!info.is_initialized) + SensorInfo info = sensor_manager->getSensorInfo(imu_bmx160); + if (!info.isInitialized) { status.bmx160 = SensorDriverStatus::DRIVER_ERROR; } info = sensor_manager->getSensorInfo(mag_lis3mdl); - if (!info.is_initialized) + if (!info.isInitialized) { status.lis3mdl = SensorDriverStatus::DRIVER_ERROR; } info = sensor_manager->getSensorInfo(gps_ublox); - if (!info.is_initialized) + if (!info.isInitialized) { status.gps = SensorDriverStatus::DRIVER_ERROR; } info = sensor_manager->getSensorInfo(internal_adc); - if (!info.is_initialized) + if (!info.isInitialized) { status.internal_adc = SensorDriverStatus::DRIVER_ERROR; } info = sensor_manager->getSensorInfo(adc_ads1118); - if (!info.is_initialized) + if (!info.isInitialized) { status.ads1118 = SensorDriverStatus::DRIVER_ERROR; } diff --git a/src/boards/Payload/Main/Sensors.h b/src/boards/Payload/Main/Sensors.h index 4811c2c2b..0263065e3 100644 --- a/src/boards/Payload/Main/Sensors.h +++ b/src/boards/Payload/Main/Sensors.h @@ -24,16 +24,16 @@ #include <Payload/Main/SensorsData.h> #include <diagnostic/PrintLogger.h> -#include <drivers/adc/ADS1118/ADS1118.h> -#include <drivers/adc/InternalADC/InternalADC.h> -#include <drivers/gps/ublox/UbloxGPS.h> +#include <drivers/adc/InternalADC.h> #include <drivers/spi/SPIBusInterface.h> +#include <sensors/ADS1118/ADS1118.h> #include <sensors/BMX160/BMX160.h> #include <sensors/BMX160/BMX160WithCorrection.h> #include <sensors/LIS3MDL/LIS3MDL.h> #include <sensors/MS5803/MS5803.h> #include <sensors/SensorData.h> #include <sensors/SensorManager.h> +#include <sensors/UbloxGPS/UbloxGPS.h> #include <sensors/analog/battery/BatteryVoltageSensor.h> #include <sensors/analog/current/CurrentSensor.h> #include <sensors/analog/pressure/MPXHZ6130A/MPXHZ6130A.h> diff --git a/src/boards/Payload/PayloadBoard.h b/src/boards/Payload/PayloadBoard.h index c01728f44..dc591cf3b 100644 --- a/src/boards/Payload/PayloadBoard.h +++ b/src/boards/Payload/PayloadBoard.h @@ -22,9 +22,7 @@ #pragma once -#include <Common.h> #include <Payload/PayloadStatus.h> -#include <Debug.h> //#include <LoggerService/LoggerService.h> #include <Payload/Main/Actuators.h> #include <Payload/Main/Bus.h> @@ -36,9 +34,9 @@ #include <System/TaskID.h> #include <events/EventBroker.h> #include <events/EventData.h> -#include <events/utils/EventInjector.h> #include <events/Events.h> #include <events/Topics.h> +#include <events/utils/EventInjector.h> #include <events/utils/EventSniffer.h> #include <functional> @@ -66,11 +64,11 @@ class Payload : public Singleton<Payload> public: // Shared Components EventBroker* broker; - //LoggerService* logger; + // LoggerService* logger; EventSniffer* sniffer; - //StateMachines* state_machines; + // StateMachines* state_machines; Bus* bus; Sensors* sensors; Radio* radio; @@ -116,18 +114,18 @@ public: injector->start(); #endif - //logger->log(status); + // logger->log(status); // If there was an error, signal it to the FMM and light a LED. if (status.payload_board != COMP_OK) { LOG_ERR(log, "Initalization failed\n"); - sEventBroker->post(Event{EV_INIT_ERROR}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post(Event{EV_INIT_ERROR}, TOPIC_FLIGHT_EVENTS); } else { LOG_INFO(log, "Initalization ok"); - sEventBroker->post(Event{EV_INIT_OK}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post(Event{EV_INIT_OK}, TOPIC_FLIGHT_EVENTS); } } @@ -144,7 +142,7 @@ public: status.setError(&PayloadStatus::logger); } - logger->log(logger->getLogger().getLogStats()); + logger->log(logger->getLogger().getLoggerStats()); }*/ private: @@ -154,12 +152,10 @@ private: Payload() { /* Shared components */ - //logger = Singleton<LoggerService>::getInstance(); - //startLogger(); - - TimestampTimer::enableTimestampTimer(); + // logger = Singleton<LoggerService>::getInstance(); + // startLogger(); - broker = sEventBroker; + broker = &sEventBroker; // Bind the logEvent function to the event sniffer in order to log every // event @@ -190,8 +186,8 @@ private: */ /*void logEvent(uint8_t event, uint8_t topic) { - EventData ev{(long long)TimestampTimer::getTimestamp(), event, topic}; - logger->log(ev); + EventData ev{(long long)TimestampTimer::getInstance().getTimestamp(), +event, topic}; logger->log(ev); #ifdef DEBUG // Don't TRACE if event is in the blacklist to avoid cluttering the @@ -213,17 +209,17 @@ private: /*void addSchedulerStatsTask() { // add lambda to log scheduler tasks statistics - scheduler->add( + scheduler.add( [&]() { std::vector<TaskStatResult> scheduler_stats = - scheduler->getTaskStats(); + scheduler.getTaskStats(); for (TaskStatResult stat : scheduler_stats) { logger->log(stat); } - StackLogger::getInstance()->updateStack(THID_TASK_SCHEDULER); + StackLogger::getInstance().updateStack(THID_TASK_SCHEDULER); }, 1000, // 1 hz TASK_SCHEDULER_STATS_ID, miosix::getTick()); diff --git a/src/boards/Payload/PinHandler/PinHandler.cpp b/src/boards/Payload/PinHandler/PinHandler.cpp index 165b0a87a..7fb6eedf2 100644 --- a/src/boards/Payload/PinHandler/PinHandler.cpp +++ b/src/boards/Payload/PinHandler/PinHandler.cpp @@ -23,6 +23,7 @@ //#include <LoggerService/LoggerService.h> #include <Payload/PinHandler/PinHandler.h> #include <diagnostic/PrintLogger.h> +#include <drivers/timer/TimestampTimer.h> #include <events/EventBroker.h> #include <events/Events.h> //#include <Payload/PayloadBoard.h> @@ -36,7 +37,7 @@ namespace PayloadBoard { PinHandler::PinHandler() - : pin_obs(PIN_POLL_INTERVAL) //, logger(LoggerService::getInstance()) + : pin_obs(PIN_POLL_INTERVAL) //, logger(LoggerService::getInstance()) { // Used for _1, _2. See std::bind cpp reference using namespace std::placeholders; @@ -57,12 +58,13 @@ void PinHandler::onNCPinTransition(unsigned int p, unsigned char n) { UNUSED(p); UNUSED(n); - sEventBroker->post(Event{EV_NC_DETACHED}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post(Event{EV_NC_DETACHED}, TOPIC_FLIGHT_EVENTS); LOG_INFO(log, "Nosecone detached!"); - status_pin_nosecone.last_detection_time = TimestampTimer::getTimestamp(); - //logger->log(status_pin_nosecone); + status_pin_nosecone.last_detection_time = + TimestampTimer::getInstance().getTimestamp(); + // logger->log(status_pin_nosecone); } void PinHandler::onNCPinStateChange(unsigned int p, unsigned char n, int state) @@ -70,14 +72,15 @@ void PinHandler::onNCPinStateChange(unsigned int p, unsigned char n, int state) UNUSED(p); UNUSED(n); - status_pin_nosecone.state = (uint8_t)state; - status_pin_nosecone.last_state_change = TimestampTimer::getTimestamp(); + status_pin_nosecone.state = (uint8_t)state; + status_pin_nosecone.last_state_change = + TimestampTimer::getInstance().getTimestamp(); status_pin_nosecone.num_state_changes += 1; LOG_INFO(log, "Nosecone pin state change at time {}: new state = {}", status_pin_nosecone.last_state_change, status_pin_nosecone.state); - //logger->log(status_pin_nosecone); + // logger->log(status_pin_nosecone); } } // namespace PayloadBoard \ No newline at end of file diff --git a/src/boards/Payload/WingControl/WingServo.cpp b/src/boards/Payload/WingControl/WingServo.cpp index 6587d27db..16f534eab 100644 --- a/src/boards/Payload/WingControl/WingServo.cpp +++ b/src/boards/Payload/WingControl/WingServo.cpp @@ -27,34 +27,18 @@ namespace PayloadBoard using namespace WingConfigs; -WingServo::WingServo(PWM::Timer servo_timer, PWMChannel servo_ch) - : ServoInterface(WING_SERVO_MIN_POS, WING_SERVO_MAX_POS), - servo(servo_timer), servo_channel(servo_ch) -{ -} - -WingServo::WingServo(PWM::Timer servo_timer, PWMChannel servo_ch, +WingServo::WingServo(TIM_TypeDef* const timer, TimerUtils::Channel channel, float minPosition, float maxPosition) - : ServoInterface(minPosition, maxPosition), servo(servo_timer), - servo_channel(servo_ch) + : ServoInterface(minPosition, maxPosition), + servo(timer, channel, 50, 500, 2500) { } WingServo::~WingServo() {} -void WingServo::enable() -{ - servo.setMaxPulseWidth(2500); - servo.setMinPulseWidth(500); - servo.enable(servo_channel); - servo.start(); -} +void WingServo::enable() { servo.enable(); } -void WingServo::disable() -{ - servo.stop(); - servo.disable(servo_channel); -} +void WingServo::disable() { servo.disable(); } void WingServo::selfTest() { @@ -80,7 +64,7 @@ void WingServo::setPosition(float angle) { this->currentPosition = angle; // map position to [0;1] interval for the servo driver - servo.setPosition(servo_channel, angle / 180.0f); + servo.setPosition180Deg(angle); } } // namespace PayloadBoard \ No newline at end of file diff --git a/src/boards/Payload/WingControl/WingServo.h b/src/boards/Payload/WingControl/WingServo.h index 2c9f1893b..4c42ddd28 100644 --- a/src/boards/Payload/WingControl/WingServo.h +++ b/src/boards/Payload/WingControl/WingServo.h @@ -22,9 +22,9 @@ #pragma once -#include <common/ServoInterface.h> #include <Payload/configs/WingConfig.h> -#include <drivers/servo/servo.h> +#include <common/ServoInterface.h> +#include <drivers/servo/Servo.h> #include <miosix.h> using namespace Boardcore; @@ -36,10 +36,9 @@ class WingServo : public ServoInterface { public: - WingServo(PWM::Timer servo_timer, PWMChannel servo_ch); - - WingServo(PWM::Timer servo_timer, PWMChannel servo_ch, float minPosition, - float maxPosition); + WingServo(TIM_TypeDef* const timer, TimerUtils::Channel channel, + float minPosition = WingConfigs::WING_SERVO_MIN_POS, + float maxPosition = WingConfigs::WING_SERVO_MAX_POS); virtual ~WingServo(); @@ -54,7 +53,6 @@ public: private: Servo servo; - PWMChannel servo_channel; protected: /** diff --git a/src/boards/Payload/configs/SensorsConfig.h b/src/boards/Payload/configs/SensorsConfig.h index e84959be0..14e657ad6 100644 --- a/src/boards/Payload/configs/SensorsConfig.h +++ b/src/boards/Payload/configs/SensorsConfig.h @@ -22,9 +22,9 @@ #pragma once -#include <drivers/adc/ADS1118/ADS1118.h> -#include <drivers/adc/InternalADC/InternalADC.h> +#include <drivers/adc/InternalADC.h> #include <interfaces-impl/hwmapping.h> +#include <sensors/ADS1118/ADS1118.h> #include <sensors/BMX160/BMX160Config.h> #include <sensors/LIS3MDL/LIS3MDL.h> #include <sensors/calibration/Calibration.h> @@ -36,6 +36,7 @@ namespace PayloadBoard namespace SensorConfigs { + static constexpr float INTERNAL_ADC_VREF = 3.3; static constexpr InternalADC::Channel ADC_BATTERY_VOLTAGE = InternalADC::Channel::CH5; diff --git a/src/boards/Payload/configs/WingConfig.h b/src/boards/Payload/configs/WingConfig.h index 4eb08d4b2..1665891f9 100644 --- a/src/boards/Payload/configs/WingConfig.h +++ b/src/boards/Payload/configs/WingConfig.h @@ -22,9 +22,9 @@ #pragma once -#include <Constants.h> -#include <drivers/HardwareTimer.h> -#include <drivers/pwm/pwm.h> +#include <drivers/timer/PWM.h> +#include <drivers/timer/TimestampTimer.h> +#include <utils/Constants.h> using namespace Boardcore; @@ -34,17 +34,13 @@ namespace PayloadBoard namespace WingConfigs { -static const PWM::Timer WING_SERVO_1_TIMER{ - TIM8, &(RCC->APB2ENR), RCC_APB2ENR_TIM8EN, - TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB2)}; +static TIM_TypeDef* const WING_SERVO_1_TIMER = TIM8; +static constexpr TimerUtils::Channel WING_SERVO_1_PWM_CH = + TimerUtils::Channel::CHANNEL_2; -static constexpr PWMChannel WING_SERVO_1_PWM_CH = PWMChannel::CH2; - -static const PWM::Timer WING_SERVO_2_TIMER{ - TIM8, &(RCC->APB2ENR), RCC_APB2ENR_TIM8EN, - TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB2)}; - -static constexpr PWMChannel WING_SERVO_2_PWM_CH = PWMChannel::CH2; +static const TIM_TypeDef* WING_SERVO_2_TIMER = TIM4; +static constexpr TimerUtils::Channel WING_SERVO_2_PWM_CH = + TimerUtils::Channel::CHANNEL_1; // Wing servo configs static constexpr float WING_SERVO_MAX_POS = 180.0; // deg diff --git a/src/entrypoints/death-stack-x-decoder/LogTypes.h b/src/entrypoints/death-stack-x-decoder/LogTypes.h index a6f1c0fe5..facc537a7 100644 --- a/src/entrypoints/death-stack-x-decoder/LogTypes.h +++ b/src/entrypoints/death-stack-x-decoder/LogTypes.h @@ -23,7 +23,7 @@ #pragma once #include <drivers/adc/ADS1118/ADS1118Data.h> -#include <drivers/gps/ublox/UbloxGPSData.h> +#include <drivers/gps/ublox/GPSData.h> #include <sensors/BMX160/BMX160Data.h> #include <sensors/BMX160/BMX160WithCorrectionData.h> #include <sensors/LIS3MDL/LIS3MDLData.h> @@ -37,13 +37,15 @@ #include <fstream> #include <iostream> -#include "ApogeeDetectionAlgorithm/ADAData.h" #include "AirBrakes/AirBrakesData.h" +#include "ApogeeDetectionAlgorithm/ADAData.h" //#include "AirBrakes/WindData.h" +#include "../../hardware_in_the_loop/HIL_sensors/HILSensorsData.h" #include "DeathStackStatus.h" #include "Deployment/DeploymentData.h" #include "FlightModeManager/FMMStatus.h" -#include "LogStats.h" +#include "FlightStatsRecorder/FSRData.h" +#include "LoggerStats.h" #include "Main/SensorsData.h" #include "NavigationAttitudeSystem/NASData.h" #include "PinHandler/PinHandlerData.h" @@ -56,8 +58,6 @@ #include "events/EventData.h" #include "logger/Deserializer.h" #include "scheduler/TaskSchedulerData.h" -#include "FlightStatsRecorder/FSRData.h" -#include "../../hardware_in_the_loop/HIL_sensors/HILSensorsData.h" // Serialized classes using std::ofstream; @@ -87,7 +87,7 @@ void registerTypes(Deserializer& ds) // Sensors registerType<CurrentSensorData>(ds); registerType<BatteryVoltageSensorData>(ds); - registerType<UbloxGPSData>(ds); + registerType<GPSData>(ds); registerType<BMX160Data>(ds); registerType<BMX160WithCorrectionData>(ds); registerType<BMX160GyroscopeCalibrationBiases>(ds); @@ -107,7 +107,7 @@ void registerTypes(Deserializer& ds) registerType<SensorsStatus>(ds); registerType<FMMStatus>(ds); registerType<PinStatus>(ds); - registerType<LogStats>(ds); + registerType<LoggerStats>(ds); registerType<DeploymentStatus>(ds); registerType<ADAControllerStatus>(ds); @@ -142,7 +142,7 @@ void registerTypes(Deserializer& ds) registerType<AirBrakesAlgorithmData>(ds); registerType<AirBrakesChosenTrajectory>(ds); - // FlightStatsRecorder + // FlightStatsRecorder registerType<LiftOffStats>(ds); registerType<ApogeeStats>(ds); registerType<DrogueDPLStats>(ds); @@ -155,5 +155,5 @@ void registerTypes(Deserializer& ds) // Others registerType<EventData>(ds); - //registerType<WindData>(ds); + // registerType<WindData>(ds); } diff --git a/src/entrypoints/death-stack-x-entry.cpp b/src/entrypoints/death-stack-x-entry.cpp index b21dd0366..428b7d8f3 100644 --- a/src/entrypoints/death-stack-x-entry.cpp +++ b/src/entrypoints/death-stack-x-entry.cpp @@ -21,7 +21,6 @@ */ #include <DeathStack.h> -#include <Debug.h> #include <FlightStatsRecorder/FSRController.h> #include <diagnostic/CpuMeter.h> #include <math/Stats.h> @@ -45,17 +44,17 @@ int main() SystemData system_data; // Instantiate the stack - DeathStack::getInstance()->start(); + DeathStack::getInstance().start(); LOG_INFO(log, "Death stack started"); - LoggerService* logger_service = LoggerService::getInstance(); + LoggerService& logger_service = LoggerService::getInstance(); for (;;) { Thread::sleep(1000); - logger_service->log(logger_service->getLogger().getLogStats()); + logger_service.log(logger_service.getLogger().getLoggerStats()); - StackLogger::getInstance()->updateStack(THID_ENTRYPOINT); + StackLogger::getInstance().updateStack(THID_ENTRYPOINT); system_data.timestamp = miosix::getTick(); system_data.cpu_usage = averageCpuUtilization(); @@ -69,8 +68,8 @@ int main() system_data.min_free_heap = MemoryProfiling::getAbsoluteFreeHeap(); system_data.free_heap = MemoryProfiling::getCurrentFreeHeap(); - logger_service->log(system_data); - - StackLogger::getInstance()->log(); + logger_service.log(system_data); + + StackLogger::getInstance().log(); } } \ No newline at end of file diff --git a/src/entrypoints/death-stack-x-testsuite.cpp b/src/entrypoints/death-stack-x-testsuite.cpp index c2184e33a..b23445418 100644 --- a/src/entrypoints/death-stack-x-testsuite.cpp +++ b/src/entrypoints/death-stack-x-testsuite.cpp @@ -21,18 +21,18 @@ */ #include <AirBrakes/AirBrakesServo.h> -#include <Common.h> #include <Deployment/DeploymentServo.h> -#include <drivers/adc/ADS1118/ADS1118.h> -#include <drivers/adc/InternalADC/InternalADC.h> -#include <drivers/gps/ublox/UbloxGPS.h> +#include <drivers/adc/InternalADC.h> #include <drivers/hbridge/HBridge.h> +#include <drivers/servo/Servo.h> #include <drivers/spi/SPIDriver.h> #include <interfaces-impl/hwmapping.h> #include <miosix.h> +#include <sensors/ADS1118/ADS1118.h> #include <sensors/BMX160/BMX160.h> #include <sensors/LIS3MDL/LIS3MDL.h> #include <sensors/MS5803/MS5803.h> +#include <sensors/UbloxGPS/UbloxGPS.h> #include <sensors/analog/battery/BatteryVoltageSensor.h> #include <sensors/analog/pressure/MPXHZ6130A/MPXHZ6130A.h> #include <sensors/analog/pressure/honeywell/SSCDANN030PAA.h> @@ -44,7 +44,6 @@ #include <vector> #include "PinHandler/PinHandler.h" -#include "drivers/servo/servo.h" #include "math/Stats.h" using namespace std; @@ -73,8 +72,6 @@ int menu(); int main() { - TimestampTimer::enableTimestampTimer(); - switch (menu()) { case 1: diff --git a/src/entrypoints/hardware_in_the_loop/hil-entry.cpp b/src/entrypoints/hardware_in_the_loop/hil-entry.cpp index 42780794c..0ca4f829f 100644 --- a/src/entrypoints/hardware_in_the_loop/hil-entry.cpp +++ b/src/entrypoints/hardware_in_the_loop/hil-entry.cpp @@ -22,7 +22,6 @@ #define EIGEN_NO_MALLOC // enable eigen malloc usage assert -#include <Common.h> #include <events/EventBroker.h> #include <events/Events.h> #include <events/utils/EventCounter.h> @@ -70,7 +69,7 @@ void threadFunc(void* arg) /*-------------- [FPM] Flight Phases Manager --------------*/ HILFlightPhasesManager* flightPhasesManager = - HIL::getInstance()->flightPhasesManager; + HIL::getInstance().flightPhasesManager; /*flightPhasesManager->registerToFlightPhase( SIMULATION_STOPPED, bind(&setIsSimulationRunning, false));*/ @@ -83,7 +82,7 @@ void threadFunc(void* arg) SIMULATION_STOPPED, bind(&TaskScheduler::stop, &scheduler)); /*-------------- [HIL] HILTransceiver --------------*/ - HILTransceiver* simulator = HIL::getInstance()->simulator; + HILTransceiver* simulator = HIL::getInstance().imulator; /*-------------- Sensors & Actuators --------------*/ @@ -130,7 +129,7 @@ void threadFunc(void* arg) NASController<HILImuData, HILBaroData, HILGpsData> nas_controller( *sensors.imu, *sensors.barometer, *sensors.gps); - HIL::getInstance()->setNAS(&nas_controller.getNAS()); + HIL::getInstance().etNAS(&nas_controller.getNAS()); /*-------------- [CA] Control Algorithm --------------*/ AirBrakesController<NASData> airbrakes_controller(nas_controller.getNAS()); @@ -143,7 +142,7 @@ void threadFunc(void* arg) /*-------------- Events --------------*/ - EventCounter counter{*sEventBroker}; + EventCounter counter{sEventBroker}; counter.subscribe(TOPIC_FLIGHT_EVENTS); counter.subscribe(TOPIC_ADA); counter.subscribe(TOPIC_NAS); @@ -187,7 +186,7 @@ void threadFunc(void* arg) /*---------- Starting threads --------*/ - sEventBroker->start(); + sEventBroker.start(); fmm.start(); ada_controller.start(); nas_controller.start(); @@ -196,10 +195,10 @@ void threadFunc(void* arg) // pin_handler.start(); scheduler.start(); // started only the scheduler instead of the SM - sEventBroker->post({EV_INIT_OK}, TOPIC_FMM); + sEventBroker.post({EV_INIT_OK}, TOPIC_FMM); Thread::sleep(100); - HIL::getInstance()->start(); // wait for first message from simulator + HIL::getInstance().tart(); // wait for first message from simulator /*---------- Wait for simulator to startup ----------*/ while (!flightPhasesManager->isSimulationStarted()) @@ -214,15 +213,13 @@ void threadFunc(void* arg) scheduler.stop(); - sEventBroker->post({EV_LANDED}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post({EV_LANDED}, TOPIC_FLIGHT_EVENTS); Thread::sleep(1000); } int main() { - TimestampTimer::enableTimestampTimer(); - Thread::create(threadFunc, STACK_MIN_FOR_SKYWARD, MAIN_PRIORITY, nullptr); unsigned int counter = 0; diff --git a/src/entrypoints/payload-entry.cpp b/src/entrypoints/payload-entry.cpp index ecf8b8c1e..583f1612a 100644 --- a/src/entrypoints/payload-entry.cpp +++ b/src/entrypoints/payload-entry.cpp @@ -20,7 +20,6 @@ * THE SOFTWARE. */ -#include <Debug.h> #include <Payload/PayloadBoard.h> #include <SystemData.h> #include <diagnostic/CpuMeter.h> @@ -44,7 +43,7 @@ int main() SystemData system_data; // Instantiate the payload - Payload::getInstance()->start(); + Payload::getInstance().start(); LOG_INFO(log, "Payload started"); // LoggerService* logger_service = LoggerService::getInstance(); @@ -52,9 +51,9 @@ int main() for (;;) { Thread::sleep(1000); - // logger_service->log(logger_service->getLogger().getLogStats()); + // logger_service->log(logger_service->getLogger().getLoggerStats()); - // StackLogger::getInstance()->updateStack(THID_ENTRYPOINT); + // StackLogger::getInstance().updateStack(THID_ENTRYPOINT); system_data.timestamp = miosix::getTick(); system_data.cpu_usage = averageCpuUtilization(); @@ -70,6 +69,6 @@ int main() // logger_service->log(system_data); - // StackLogger::getInstance()->log(); + // StackLogger::getInstance().log(); } } \ No newline at end of file diff --git a/src/entrypoints/windtunnel-entry.cpp b/src/entrypoints/windtunnel-entry.cpp index 8c6a85792..89e4deeb3 100644 --- a/src/entrypoints/windtunnel-entry.cpp +++ b/src/entrypoints/windtunnel-entry.cpp @@ -33,15 +33,15 @@ int main() { TRACE("Starting windtunnel test....\n"); // Instantiate the stack - DeathStack::getInstance()->start(); + DeathStack::getInstance().start(); TRACE("Running\n"); // bool printed = false; for (;;) { Thread::sleep(1000); - LoggerService::getInstance()->log( - LoggerService::getInstance()->getLogger().getLogStats()); + LoggerService::getInstance().log( + LoggerService::getInstance().getLogger().getLoggerStats()); // if(buf_adc_pitot.size() >= GLOBAL_BUF_LEN && !printed) // { // printed = true; diff --git a/src/entrypoints/windtunnel-test-decoder/LogTypes.h b/src/entrypoints/windtunnel-test-decoder/LogTypes.h index a093dd70e..5b370ad57 100644 --- a/src/entrypoints/windtunnel-test-decoder/LogTypes.h +++ b/src/entrypoints/windtunnel-test-decoder/LogTypes.h @@ -34,7 +34,7 @@ #include "drivers/mavlink/MavlinkStatus.h" #include "events/EventData.h" #include "logger/Deserializer.h" -#include "logger/LogStats.h" +#include "logger/LoggerStats.h" #include "sensors/MS580301BA07/MS580301BA07Data.h" #include "sensors/analog/pressure/MPXHZ6130A/MPXHZ6130AData.h" #include "sensors/analog/pressure/honeywell/SSCDANN030PAAData.h" @@ -59,7 +59,7 @@ void registerType(Deserializer& ds) void registerTypes(Deserializer& ds) { - registerType<LogStats>(ds); + registerType<LoggerStats>(ds); registerType<EventData>(ds); registerType<MS5803Data>(ds); diff --git a/src/hardware_in_the_loop/HILFlightPhasesManager.h b/src/hardware_in_the_loop/HILFlightPhasesManager.h index 0da4add27..d60c9d5db 100644 --- a/src/hardware_in_the_loop/HILFlightPhasesManager.h +++ b/src/hardware_in_the_loop/HILFlightPhasesManager.h @@ -67,7 +67,7 @@ struct Outcomes Outcomes() : t(0), z(0), vz(0) {} Outcomes(float z, float vz) - : t(TimestampTimer::getTimestamp()), z(z), vz(vz) + : t(TimestampTimer::getInstance().getTimestamp()), z(z), vz(vz) { } @@ -90,16 +90,16 @@ public: HILFlightPhasesManager() { updateFlags({0, 0, 0, 0, 0, 0}); - counter_flight_events = new EventCounter(*sEventBroker); + counter_flight_events = new EventCounter(sEventBroker); counter_flight_events->subscribe(TOPIC_FLIGHT_EVENTS); - counter_airbrakes = new EventCounter(*sEventBroker); + counter_airbrakes = new EventCounter(sEventBroker); counter_airbrakes->subscribe(TOPIC_ABK); - counter_ada = new EventCounter(*sEventBroker); + counter_ada = new EventCounter(sEventBroker); counter_ada->subscribe(TOPIC_ADA); - counter_dpl = new EventCounter(*sEventBroker); + counter_dpl = new EventCounter(sEventBroker); counter_dpl->subscribe(TOPIC_DPL); } @@ -135,7 +135,7 @@ public: // set true when the first packet from the simulator arrives if (isSetTrue(SIMULATION_STARTED)) { - t_start = TimestampTimer::getTimestamp(); + t_start = TimestampTimer::getInstance().getTimestamp(); TRACE("[HIL] ------- SIMULATION STARTED ! ------- \n"); changed_flags.push_back(SIMULATION_STARTED); @@ -162,9 +162,8 @@ public: { if (isSetTrue(FLYING)) { - t_liftoff = TimestampTimer::getTimestamp(); - sEventBroker->post({EV_UMBILICAL_DETACHED}, - TOPIC_FLIGHT_EVENTS); + t_liftoff = TimestampTimer::getInstance().getTimestamp(); + sEventBroker.post({EV_UMBILICAL_DETACHED}, TOPIC_FLIGHT_EVENTS); TRACE("[HIL] ------- LIFTOFF ! ------- \n"); changed_flags.push_back(FLYING); @@ -211,7 +210,7 @@ public: else if (isSetTrue(SIMULATION_STOPPED)) { changed_flags.push_back(SIMULATION_STOPPED); - t_stop = TimestampTimer::getTimestamp(); + t_stop = TimestampTimer::getInstance().getTimestamp(); TRACE("[HIL] ------- SIMULATION STOPPED ! -------: %f \n\n\n", (double)t_stop / 1000000.0f); printOutcomes(); diff --git a/src/hardware_in_the_loop/HIL_sensors/HILBarometer.h b/src/hardware_in_the_loop/HIL_sensors/HILBarometer.h index 3c23d7211..16e5d2d5e 100644 --- a/src/hardware_in_the_loop/HIL_sensors/HILBarometer.h +++ b/src/hardware_in_the_loop/HIL_sensors/HILBarometer.h @@ -45,7 +45,7 @@ protected: { HILBaroData tempData; - tempData.press = sensorData->barometer.measures[sampleCounter]; + tempData.pressure = sensorData->barometer.measures[sampleCounter]; tempData.press_timestamp = updateTimestamp(); return tempData; diff --git a/src/hardware_in_the_loop/HIL_sensors/HILSensor.h b/src/hardware_in_the_loop/HIL_sensors/HILSensor.h index 57a1f0768..253e8e135 100644 --- a/src/hardware_in_the_loop/HIL_sensors/HILSensor.h +++ b/src/hardware_in_the_loop/HIL_sensors/HILSensor.h @@ -24,6 +24,7 @@ #include <typeinfo> +#include "HILSensorsData.h" #include "HILTimestampManagement.h" #include "TimestampTimer.h" #include "hardware_in_the_loop/HILConfig.h" @@ -31,7 +32,6 @@ #include "math/Vec3.h" #include "sensors/Sensor.h" #include "sensors/SensorData.h" -#include "HILSensorsData.h" /** * @brief Fake sensor base used for the simulation. Every sensor for the @@ -140,7 +140,7 @@ protected: uint64_t updateTimestamp() { sampleCounter++; - return TimestampTimer::getTimestamp(); + return TimestampTimer::getInstance().getTimestamp(); } /** diff --git a/src/hardware_in_the_loop/simulator_communication/SerialInterface.h b/src/hardware_in_the_loop/simulator_communication/SerialInterface.h index 37857e3d1..e8ba4476b 100644 --- a/src/hardware_in_the_loop/simulator_communication/SerialInterface.h +++ b/src/hardware_in_the_loop/simulator_communication/SerialInterface.h @@ -22,7 +22,6 @@ #pragma once -#include <Common.h> #include <fcntl.h> #include <stdio.h> diff --git a/src/mocksensors/MockGPS.h b/src/mocksensors/MockGPS.h index 17ecf424d..17b04b9a8 100644 --- a/src/mocksensors/MockGPS.h +++ b/src/mocksensors/MockGPS.h @@ -22,10 +22,10 @@ #pragma once -#include <TimestampTimer.h> -#include <sensors/Sensor.h> +#include <drivers/timer/TimestampTimer.h> #include <mocksensors/MockSensorsData.h> #include <mocksensors/lynx_flight_data/lynx_gps_data.h> +#include <sensors/Sensor.h> namespace DeathStackBoard { @@ -54,28 +54,28 @@ public: MockGPSData data; - data.gps_timestamp = TimestampTimer::getTimestamp(); - data.fix = true; + data.gpsTimestamp = TimestampTimer::getInstance().getTimestamp(); + data.fix = true; if (before_liftoff) { - data.latitude = GPS_DATA_LAT[0]; - data.longitude = GPS_DATA_LON[0]; - data.velocity_north = GPS_DATA_VNORD[0]; - data.velocity_east = GPS_DATA_VEAST[0]; - data.velocity_down = GPS_DATA_VDOWN[0]; - data.num_satellites = GPS_DATA_NSATS[0]; - data.fix = true; + data.latitude = GPS_DATA_LAT[0]; + data.longitude = GPS_DATA_LON[0]; + data.velocityNorth = GPS_DATA_VNORD[0]; + data.velocityEast = GPS_DATA_VEAST[0]; + data.velocityDown = GPS_DATA_VDOWN[0]; + data.satellites = GPS_DATA_NSATS[0]; + data.fix = true; } else if (i < GPS_DATA_SIZE) { - data.latitude = GPS_DATA_LAT[i]; - data.longitude = GPS_DATA_LON[i]; - data.velocity_north = GPS_DATA_VNORD[i]; - data.velocity_east = GPS_DATA_VEAST[i]; - data.velocity_down = GPS_DATA_VDOWN[i]; - data.num_satellites = GPS_DATA_NSATS[i]; - data.fix = true; + data.latitude = GPS_DATA_LAT[i]; + data.longitude = GPS_DATA_LON[i]; + data.velocityNorth = GPS_DATA_VNORD[i]; + data.velocityEast = GPS_DATA_VEAST[i]; + data.velocityDown = GPS_DATA_VDOWN[i]; + data.satellites = GPS_DATA_NSATS[i]; + data.fix = true; i++; } diff --git a/src/mocksensors/MockIMU.h b/src/mocksensors/MockIMU.h index 94ac9f373..acd1bf1e3 100644 --- a/src/mocksensors/MockIMU.h +++ b/src/mocksensors/MockIMU.h @@ -22,10 +22,10 @@ #pragma once -#include <TimestampTimer.h> -#include <sensors/Sensor.h> +#include <drivers/timer/TimestampTimer.h> #include <mocksensors/MockSensorsData.h> #include <mocksensors/lynx_flight_data/lynx_imu_data.h> +#include <sensors/Sensor.h> namespace DeathStackBoard { @@ -47,22 +47,22 @@ public: } MockIMUData data; - uint64_t t = TimestampTimer::getTimestamp(); - - data.accel_timestamp = t; - data.accel_x = ACCEL_DATA[index][0]; - data.accel_y = ACCEL_DATA[index][1]; - data.accel_z = ACCEL_DATA[index][2]; - - data.gyro_timestamp = t; - data.gyro_x = GYRO_DATA[index][0]; - data.gyro_y = GYRO_DATA[index][1]; - data.gyro_z = GYRO_DATA[index][2]; - - data.mag_timestamp = t; - data.mag_x = MAG_DATA[index][0]; - data.mag_y = MAG_DATA[index][1]; - data.mag_z = MAG_DATA[index][2]; + uint64_t t = TimestampTimer::getInstance().getTimestamp(); + + data.accelerationTimestamp = t; + data.accelerationX = ACCEL_DATA[index][0]; + data.accelerationY = ACCEL_DATA[index][1]; + data.accelerationZ = ACCEL_DATA[index][2]; + + data.angularVelocityTimestamp = t; + data.angularVelocityX = GYRO_DATA[index][0]; + data.angularVelocityY = GYRO_DATA[index][1]; + data.angularVelocityZ = GYRO_DATA[index][2]; + + data.magneticFieldTimestamp = t; + data.magneticFieldX = MAG_DATA[index][0]; + data.magneticFieldY = MAG_DATA[index][1]; + data.magneticFieldZ = MAG_DATA[index][2]; // when finished, go back to the beginning index = (index + 1) % IMU_DATA_SIZE; diff --git a/src/mocksensors/MockPressureSensor.h b/src/mocksensors/MockPressureSensor.h index 7100f81b9..1a6b7ccc7 100644 --- a/src/mocksensors/MockPressureSensor.h +++ b/src/mocksensors/MockPressureSensor.h @@ -22,11 +22,11 @@ #pragma once -#include <Common.h> #include <mocksensors/MockSensorsData.h> //#include <mocksensors/lynx_flight_data/lynx_press_data.h> #include <mocksensors/lynx_flight_data/lynx_pressure_static_data.h> #include <sensors/Sensor.h> + #include <random> namespace DeathStackBoard @@ -45,31 +45,31 @@ public: { MockPressureData data; - data.press_timestamp = TimestampTimer::getTimestamp(); + data.pressureTimestamp = TimestampTimer::getInstance().getTimestamp(); if (before_liftoff) { - data.press = PRESSURE_STATIC_DATA[0]; + data.pressure = PRESSURE_STATIC_DATA[0]; } else { if (i < PRESSURE_STATIC_DATA_SIZE) { - data.press = PRESSURE_STATIC_DATA[i++]; + data.pressure = PRESSURE_STATIC_DATA[i++]; } else { - data.press = + data.pressure = PRESSURE_STATIC_DATA[PRESSURE_STATIC_DATA_SIZE - 1]; } } if (with_noise) { - data.press = addNoise(data.press); + data.pressure = addNoise(data.pressure); } - data.press = movingAverage(data.press); + data.pressure = movingAverage(data.pressure); return data; } diff --git a/src/mocksensors/MockSensorsData.h b/src/mocksensors/MockSensorsData.h index 4358df593..d9edc2220 100644 --- a/src/mocksensors/MockSensorsData.h +++ b/src/mocksensors/MockSensorsData.h @@ -37,10 +37,12 @@ struct MockIMUData : public AccelerometerData, void print(std::ostream& os) const { - os << accel_timestamp << "," << accel_x << "," << accel_y << "," - << accel_z << "," << gyro_timestamp << "," << gyro_x << "," << gyro_y - << "," << gyro_z << "," << mag_timestamp << "," << mag_x << "," - << mag_y << "," << mag_z << "\n"; + os << accelerationTimestamp << "," << accelerationX << "," + << accelerationY << "," << accelerationZ << "," + << angularVelocityTimestamp << "," << angularVelocityX << "," + << angularVelocityY << "," << angularVelocityZ << "," + << magneticFieldTimestamp << "," << magneticFieldX << "," + << magneticFieldY << "," << magneticFieldZ << "\n"; } }; @@ -53,7 +55,7 @@ struct MockPressureData : public PressureData void print(std::ostream& os) const { - os << press_timestamp << "," << press << "\n"; + os << pressureTimestamp << "," << pressure << "\n"; } }; @@ -65,12 +67,12 @@ struct MockGPSData : public GPSData "velocity_east,velocity_down,speed,track,num_satellites,fix\n"; } - void print(std::ostream &os) const + void print(std::ostream& os) const { - os << gps_timestamp << "," << latitude << "," << longitude << "," - << height << "," << velocity_north << "," << velocity_east << "," - << velocity_down << "," << speed << "," << track << "," - << (int)num_satellites << "," << (int)fix << "\n"; + os << gpsTimestamp << "," << latitude << "," << longitude << "," + << height << "," << velocityNorth << "," << velocityEast << "," + << velocityDown << "," << speed << "," << track << "," + << (int)satellites << "," << (int)fix << "\n"; } }; diff --git a/src/mocksensors/MockSpeedSensor.h b/src/mocksensors/MockSpeedSensor.h index a9dd3b3f9..c7c01cae0 100644 --- a/src/mocksensors/MockSpeedSensor.h +++ b/src/mocksensors/MockSpeedSensor.h @@ -22,10 +22,10 @@ #pragma once -#include <Common.h> #include <mocksensors/MockSensorsData.h> #include <mocksensors/lynx_flight_data/lynx_airspeed_data.h> #include <sensors/Sensor.h> + #include <random> namespace DeathStackBoard @@ -44,7 +44,7 @@ public: { MockSpeedData data; - data.timestamp = TimestampTimer::getTimestamp(); + data.timestamp = TimestampTimer::getInstance().getTimestamp(); if (before_liftoff) { diff --git a/src/mocksensors/lynx_flight_data/lynx_airspeed_data.h b/src/mocksensors/lynx_flight_data/lynx_airspeed_data.h index d0948bffb..7fa3e38da 100644 --- a/src/mocksensors/lynx_flight_data/lynx_airspeed_data.h +++ b/src/mocksensors/lynx_flight_data/lynx_airspeed_data.h @@ -22,8 +22,6 @@ #pragma once -#include <Common.h> - /** * Pitot airspeed data from Lynx flight test in Roccaraso. * Sampled at 42 Hz (24 ms period). diff --git a/src/mocksensors/lynx_flight_data/lynx_gps_data.h b/src/mocksensors/lynx_flight_data/lynx_gps_data.h index a82162aca..3318c3a51 100644 --- a/src/mocksensors/lynx_flight_data/lynx_gps_data.h +++ b/src/mocksensors/lynx_flight_data/lynx_gps_data.h @@ -22,7 +22,7 @@ #pragma once -#include <Common.h> +#include <stdint.h> /** * Ublox Neo-M9N GPS data from Lynx flight test in Roccaraso. diff --git a/src/mocksensors/mock_data/test-mock-sensors.cpp b/src/mocksensors/mock_data/test-mock-sensors.cpp index d21fa7b4e..2181291ea 100644 --- a/src/mocksensors/mock_data/test-mock-sensors.cpp +++ b/src/mocksensors/mock_data/test-mock-sensors.cpp @@ -20,15 +20,12 @@ * THE SOFTWARE. */ -#include <Common.h> #include <mocksensors/MockSensors.h> using namespace DeathStackBoard; int main() { - TimestampTimer::enableTimestampTimer(); - MockGPS gps; MockPressureSensor p_sensor; MockIMU imu; @@ -55,7 +52,7 @@ int main() p_sensor.sample(); PressureData pressure = p_sensor.getLastSample(); TRACE("PRESSURE SAMPLE: \n"); - TRACE("pressure = %f \n\n", pressure.press); + TRACE("pressure = %f \n\n", pressure.pressure); imu.sample(); MockIMUData imu_data = imu.getLastSample(); diff --git a/src/tests/airbrakes/test-airbrakes-interactive.cpp b/src/tests/airbrakes/test-airbrakes-interactive.cpp index f15d6d74f..36fcdf1ae 100644 --- a/src/tests/airbrakes/test-airbrakes-interactive.cpp +++ b/src/tests/airbrakes/test-airbrakes-interactive.cpp @@ -90,7 +90,7 @@ float resetPosition = AirBrakesConfigs::AB_SERVO_MIN_POS; int main() { - sEventBroker->start(); + sEventBroker.start(); miosix::GpioPin pwmPin = miosix::GpioPin(GPIOC_BASE, 7); pwmPin.mode(miosix::Mode::ALTERNATE); @@ -164,13 +164,13 @@ void testAlgorithm() // Start the state machine airbrakesController.start(); sensor.sample(); - EventCounter counter{*sEventBroker}; + EventCounter counter{sEventBroker}; counter.subscribe(TOPIC_FLIGHT_EVENTS); counter.subscribe(TOPIC_ABK); // Start test cout << "Starting airbrakes test\n"; - sEventBroker->post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS); while (counter.getCount(EV_SHADOW_MODE_TIMEOUT) < 1) { Thread::sleep(10); @@ -208,12 +208,12 @@ void testAirBrakes() // Start the state machine airbrakesController.start(); - EventCounter counter{*sEventBroker}; + EventCounter counter{sEventBroker}; counter.subscribe(TOPIC_ABK); // Start test cout << "Starting airbrakes test\n"; - sEventBroker->post({EV_TEST_ABK}, TOPIC_ABK); + sEventBroker.post({EV_TEST_ABK}, TOPIC_ABK); while (counter.getCount(EV_TEST_TIMEOUT) < 1) { diff --git a/src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp b/src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp index d88c8b7c1..74070c368 100644 --- a/src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp +++ b/src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp @@ -26,22 +26,20 @@ #define EIGEN_NO_MALLOC - -#include <catch2/catch.hpp> - #include <Eigen/Dense> +#include <catch2/catch.hpp> #define private public #define protected public -#include <ApogeeDetectionAlgorithm/ADAController.h> #include <ApogeeDetectionAlgorithm/ADAAlgorithm.h> +#include <ApogeeDetectionAlgorithm/ADAController.h> #include <DeathStack.h> -#include <Common.h> #include <events/EventBroker.h> #include <events/Events.h> #include <events/FSM.h> #include <events/utils/EventCounter.h> + #include <algorithm> #include <cmath> #include <iostream> @@ -88,7 +86,8 @@ public: } } - return PressureData{TimestampTimer::getTimestamp(), press}; + return PressureData{TimestampTimer::getInstance().getTimestamp(), + press}; } void signalLiftoff() { before_liftoff = false; } @@ -145,14 +144,12 @@ void checkState(unsigned int i, ADAKalmanState state) TEST_CASE("Testing ada_controller from calibration to first descent phase") { - TimestampTimer::enableTimestampTimer(); - ada_controller = new ADACtrl(mock_baro, mock_gps); TRACE("ADA init : %d \n", ada_controller->start()); // Start event broker and ada_controller - sEventBroker->start(); - EventCounter counter{*sEventBroker}; + sEventBroker.start(); + EventCounter counter{sEventBroker}; counter.subscribe(TOPIC_ADA); // Startup: we should be in idle @@ -160,7 +157,7 @@ TEST_CASE("Testing ada_controller from calibration to first descent phase") REQUIRE(ada_controller->testState(&ADACtrl::state_idle)); // Enter Calibrating and REQUIRE - sEventBroker->post({EV_CALIBRATE_ADA}, TOPIC_ADA); + sEventBroker.post({EV_CALIBRATE_ADA}, TOPIC_ADA); Thread::sleep(100); REQUIRE(ada_controller->testState(&ADACtrl::state_calibrating)); @@ -216,7 +213,7 @@ TEST_CASE("Testing ada_controller from calibration to first descent phase") REQUIRE(ada_controller->testState(&ADACtrl::state_ready)); // Send liftoff event: should be in shadow mode - sEventBroker->post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS); mock_baro.signalLiftoff(); Thread::sleep(100); REQUIRE(ada_controller->testState(&ADACtrl::state_shadowMode)); @@ -230,7 +227,7 @@ TEST_CASE("Testing ada_controller from calibration to first descent phase") mock_baro.sample(); Thread::sleep(5); ada_controller->update(); - float noisy_p = mock_baro.getLastSample().press; + float noisy_p = mock_baro.getLastSample().pressure; // Thread::sleep(100); ADAKalmanState state = ada_controller->ada.getKalmanState(); printf("%d,%f,%f,%f\n", (int)i, noisy_p, state.x0, @@ -252,7 +249,7 @@ TEST_CASE("Testing ada_controller from calibration to first descent phase") mock_baro.sample(); Thread::sleep(5); ada_controller->update(); - float noisy_p = mock_baro.getLastSample().press; + float noisy_p = mock_baro.getLastSample().pressure; // Thread::sleep(100); ADAKalmanState state = ada_controller->ada.getKalmanState(); printf("%d,%f,%f,%f\n", (int)i, noisy_p, state.x0, diff --git a/src/tests/catch/ada/kalman_acc/test-kalman-acc.cpp b/src/tests/catch/ada/kalman_acc/test-kalman-acc.cpp index 8169ac0fe..52accdb46 100644 --- a/src/tests/catch/ada/kalman_acc/test-kalman-acc.cpp +++ b/src/tests/catch/ada/kalman_acc/test-kalman-acc.cpp @@ -27,13 +27,12 @@ #define private public #include <ADA/ADA.h> -#include <Common.h> #include <configs/ADAConfig.h> +#include <catch2/catch.hpp> #include <iostream> #include <random> #include <sstream> -#include <catch2/catch.hpp> #include "test-kalman-acc-data.h" diff --git a/src/tests/catch/ada/test-rogallo-dts.cpp b/src/tests/catch/ada/test-rogallo-dts.cpp index ac887b85e..2770af03b 100644 --- a/src/tests/catch/ada/test-rogallo-dts.cpp +++ b/src/tests/catch/ada/test-rogallo-dts.cpp @@ -119,7 +119,7 @@ TEST_CASE("[RogalloDTS] Test Launch Hazard Circles") TEST_CASE("[Rogallo DTS] Test deployment altitude") { - EventCounter c{*sEventBroker}; + EventCounter c{sEventBroker}; c.subscribe(TOPIC_ADA); RogalloDTS dts; diff --git a/src/tests/catch/catch-tests-entry.cpp b/src/tests/catch/catch-tests-entry.cpp index 389ee6f73..3167810e3 100644 --- a/src/tests/catch/catch-tests-entry.cpp +++ b/src/tests/catch/catch-tests-entry.cpp @@ -55,9 +55,9 @@ #include <miosix.h> +#include <catch2/catch.hpp> #include <cstring> #include <string> -#include <catch2/catch.hpp> #include <vector> using miosix::Thread; @@ -137,7 +137,7 @@ int main() } // Run tests with the provided arguments - int result = Catch::Session().run(argc, argv); + Catch::Session().run(argc, argv); // Clear memory for (size_t i = 0; i < argc; i++) diff --git a/src/tests/catch/fsm/test-ada.cpp b/src/tests/catch/fsm/test-ada.cpp index 51ee14fe9..775ae083f 100644 --- a/src/tests/catch/fsm/test-ada.cpp +++ b/src/tests/catch/fsm/test-ada.cpp @@ -29,9 +29,8 @@ #include <miosix.h> -#include <catch2/catch.hpp> - #include <Eigen/Dense> +#include <catch2/catch.hpp> #define private public #define protected public @@ -53,7 +52,7 @@ public: // This is called at the beginning of each test / section ADAControllerFixture() { - sEventBroker->start(); + sEventBroker.start(); controller = new ADACtrl(mock_baro, mock_gps); controller->start(); } @@ -62,8 +61,8 @@ public: ~ADAControllerFixture() { controller->stop(); - sEventBroker->unsubscribe(controller); - sEventBroker->clearDelayedEvents(); + sEventBroker.unsubscribe(controller); + sEventBroker.clearDelayedEvents(); delete controller; } diff --git a/src/tests/catch/fsm/test-airbrakes.cpp b/src/tests/catch/fsm/test-airbrakes.cpp index 0eb3af90c..51d7ec5bb 100644 --- a/src/tests/catch/fsm/test-airbrakes.cpp +++ b/src/tests/catch/fsm/test-airbrakes.cpp @@ -75,7 +75,7 @@ public: AirBrakesControllerFixture() { controller = new AirBrakesController<InputClass>(sensor); - sEventBroker->start(); + sEventBroker.start(); controller->start(); } @@ -83,8 +83,8 @@ public: ~AirBrakesControllerFixture() { controller->stop(); - sEventBroker->unsubscribe(controller); - sEventBroker->clearDelayedEvents(); + sEventBroker.unsubscribe(controller); + sEventBroker.clearDelayedEvents(); delete controller; } diff --git a/src/tests/catch/fsm/test-deployment.cpp b/src/tests/catch/fsm/test-deployment.cpp index 68d0df735..137f2f925 100644 --- a/src/tests/catch/fsm/test-deployment.cpp +++ b/src/tests/catch/fsm/test-deployment.cpp @@ -29,7 +29,7 @@ #define protected public #include <Deployment/DeploymentController.h> -#include <drivers/servo/servo.h> +#include <drivers/servo/Servo.h> #include <miosix.h> #include <catch2/catch.hpp> @@ -48,7 +48,7 @@ public: DeploymentControllerFixture() { controller = new DeploymentController(&ejection_servo); - sEventBroker->start(); + sEventBroker.start(); controller->start(); } @@ -56,8 +56,8 @@ public: ~DeploymentControllerFixture() { controller->stop(); - sEventBroker->unsubscribe(controller); - sEventBroker->clearDelayedEvents(); + sEventBroker.unsubscribe(controller); + sEventBroker.clearDelayedEvents(); delete controller; } diff --git a/src/tests/catch/fsm/test-flightstatsrecorder.cpp b/src/tests/catch/fsm/test-flightstatsrecorder.cpp index 45e9cee8e..1ca268fbe 100644 --- a/src/tests/catch/fsm/test-flightstatsrecorder.cpp +++ b/src/tests/catch/fsm/test-flightstatsrecorder.cpp @@ -30,9 +30,8 @@ #include <miosix.h> -#include <catch2/catch.hpp> - #include <Eigen/Dense> +#include <catch2/catch.hpp> #define private public #define protected public @@ -51,7 +50,7 @@ public: // This is called at the beginning of each test / section FlightStatsFixture() { - sEventBroker->start(); + sEventBroker.start(); fsm = new FlightStatsRecorder(); fsm->start(); } @@ -60,8 +59,8 @@ public: ~FlightStatsFixture() { fsm->stop(); - sEventBroker->unsubscribe(fsm); - sEventBroker->clearDelayedEvents(); + sEventBroker.unsubscribe(fsm); + sEventBroker.clearDelayedEvents(); delete fsm; } diff --git a/src/tests/catch/fsm/test-fmm.cpp b/src/tests/catch/fsm/test-fmm.cpp index 793a6a547..0b65370bb 100644 --- a/src/tests/catch/fsm/test-fmm.cpp +++ b/src/tests/catch/fsm/test-fmm.cpp @@ -29,12 +29,11 @@ #include <miosix.h> +#include <Eigen/Dense> #include <catch2/catch.hpp> #include "events/Events.h" -#include <Eigen/Dense> - #define private public #define protected public @@ -51,7 +50,7 @@ public: // This is called at the beginning of each test / section FMMFixture() { - sEventBroker->start(); + sEventBroker.start(); fsm = new FMMController(); fsm->start(); } @@ -60,8 +59,8 @@ public: ~FMMFixture() { fsm->stop(); - sEventBroker->unsubscribe(fsm); - sEventBroker->clearDelayedEvents(); + sEventBroker.unsubscribe(fsm); + sEventBroker.clearDelayedEvents(); delete fsm; } diff --git a/src/tests/catch/fsm/test-ignition.cpp b/src/tests/catch/fsm/test-ignition.cpp index 5df0ea6a9..19afdf1c2 100644 --- a/src/tests/catch/fsm/test-ignition.cpp +++ b/src/tests/catch/fsm/test-ignition.cpp @@ -55,8 +55,8 @@ public: } ~IgnitionTestFixture() { - sEventBroker->unsubscribe(ign); - sEventBroker->clearDelayedEvents(); + sEventBroker.unsubscribe(ign); + sEventBroker.clearDelayedEvents(); delete ign; delete can; delete can_mgr; @@ -161,7 +161,7 @@ class IgnitionTestFixture2 public: IgnitionTestFixture2() { - sEventBroker->start(); + sEventBroker.start(); can_mgr = new CanManager(CAN1); can = new CanProxy(can_mgr); ign = new IgnitionController(can); @@ -170,8 +170,8 @@ public: ~IgnitionTestFixture2() { ign->stop(); - sEventBroker->unsubscribe(ign); - sEventBroker->clearDelayedEvents(); + sEventBroker.unsubscribe(ign); + sEventBroker.clearDelayedEvents(); delete ign; delete can; delete can_mgr; @@ -201,7 +201,7 @@ TEST_CASE_METHOD(IgnitionTestFixture2, "Igntiion: Testing IDLE functions") { TRACE("Beginning of section\n"); - EventCounter counter{*sEventBroker}; + EventCounter counter{sEventBroker}; counter.subscribe(TOPIC_FLIGHT_EVENTS); // Sending ign status @@ -251,7 +251,7 @@ TEST_CASE_METHOD(IgnitionTestFixture2, "Igntiion: Testing IDLE functions") // the delayed ev_ign_offline // should be deleted { - EventCounter counter{*sEventBroker}; + EventCounter counter{sEventBroker}; counter.subscribe(TOPIC_FLIGHT_EVENTS); Thread::sleep(TIMEOUT_IGN_OFFLINE / 2); diff --git a/src/tests/catch/fsm/test-nas.cpp b/src/tests/catch/fsm/test-nas.cpp index 1d1b51496..5aea6b14c 100644 --- a/src/tests/catch/fsm/test-nas.cpp +++ b/src/tests/catch/fsm/test-nas.cpp @@ -34,6 +34,7 @@ #include <NavigationAttitudeSystem/NASCalibrator.h> #include <NavigationAttitudeSystem/NASController.h> #include <mocksensors/MockSensors.h> + #include "events/Events.h" #include "utils/testutils/TestHelper.h" @@ -47,7 +48,7 @@ public: // This is called at the beginning of each test / section NASControllerFixture() : controller(mock_imu, mock_baro, mock_gps) { - sEventBroker->start(); + sEventBroker.start(); controller.start(); } @@ -55,8 +56,8 @@ public: ~NASControllerFixture() { controller.stop(); - sEventBroker->unsubscribe(&controller); - sEventBroker->clearDelayedEvents(); + sEventBroker.unsubscribe(&controller); + sEventBroker.clearDelayedEvents(); } protected: diff --git a/src/tests/catch/fsm/test-tmtc.cpp b/src/tests/catch/fsm/test-tmtc.cpp index 65b7478eb..8d6943553 100644 --- a/src/tests/catch/fsm/test-tmtc.cpp +++ b/src/tests/catch/fsm/test-tmtc.cpp @@ -30,9 +30,8 @@ #include <miosix.h> -#include <catch2/catch.hpp> - #include <Eigen/Dense> +#include <catch2/catch.hpp> #define private public #define protected public @@ -51,7 +50,7 @@ public: // This is called at the beginning of each test / section TMTCFixture() { - sEventBroker->start(); + sEventBroker.start(); fsm = new TMTCController(); fsm->start(); } @@ -60,8 +59,8 @@ public: ~TMTCFixture() { fsm->stop(); - sEventBroker->unsubscribe(fsm); - sEventBroker->clearDelayedEvents(); + sEventBroker.unsubscribe(fsm); + sEventBroker.clearDelayedEvents(); delete fsm; } diff --git a/src/tests/catch/nas/test-nas-simulation.cpp b/src/tests/catch/nas/test-nas-simulation.cpp index 0ba5573b6..75fced107 100644 --- a/src/tests/catch/nas/test-nas-simulation.cpp +++ b/src/tests/catch/nas/test-nas-simulation.cpp @@ -64,12 +64,10 @@ void signalLiftoff() TEST_CASE("Testing Navigation System Controller") { - TimestampTimer::enableTimestampTimer(); - - sEventBroker->start(); + sEventBroker.start(); controller.start(); - EventCounter counter{*sEventBroker}; + EventCounter counter{sEventBroker}; counter.subscribe(TOPIC_NAS); // Startup: we should be in idle @@ -77,7 +75,7 @@ TEST_CASE("Testing Navigation System Controller") REQUIRE(controller.testState(&NASCtrl::state_idle)); // Enter Calibrating and REQUIRE - sEventBroker->post(Event{EV_CALIBRATE_NAS}, TOPIC_NAS); + sEventBroker.post(Event{EV_CALIBRATE_NAS}, TOPIC_NAS); Thread::sleep(100); REQUIRE(controller.testState(&NASCtrl::state_calibrating)); @@ -101,9 +99,11 @@ TEST_CASE("Testing Navigation System Controller") MockIMUData imu_data = mock_imu.getLastSample(); // still before liftoff ... NASReferenceValues ref_values{ - press_data.press, gps_data.latitude, gps_data.longitude, - imu_data.accel_x, imu_data.accel_y, imu_data.accel_z, - imu_data.mag_x, imu_data.mag_y, imu_data.mag_z}; + press_data.pressure, gps_data.latitude, + gps_data.longitude, imu_data.accelerationX, + imu_data.accelerationY, imu_data.accelerationZ, + imu_data.magneticFieldX, imu_data.magneticFieldY, + imu_data.magneticFieldZ}; REQUIRE(ref_values == controller.calibrator.getReferenceValues()); // Now we should be in ready @@ -115,7 +115,7 @@ TEST_CASE("Testing Navigation System Controller") Thread::sleep(100); // retry the calibration phase - sEventBroker->post({EV_CALIBRATE_NAS}, TOPIC_NAS); + sEventBroker.post({EV_CALIBRATE_NAS}, TOPIC_NAS); Thread::sleep(100); REQUIRE(controller.testState(&NASCtrl::state_calibrating)); // sample some data for calibration @@ -130,11 +130,14 @@ TEST_CASE("Testing Navigation System Controller") // i.e. the first element of the arrays from which the sensors get the data press_data = mock_baro.getLastSample(); // still before liftoff // (same as SIMULATED_PRESSURE[0]) - gps_data = mock_gps.getLastSample(); // still before liftoff ... - imu_data = mock_imu.getLastSample(); // still before liftoff ... - ref_values = NASReferenceValues{press_data.press, gps_data.latitude, gps_data.longitude, - imu_data.accel_x, imu_data.accel_y, imu_data.accel_z, - imu_data.mag_x, imu_data.mag_y, imu_data.mag_z}; + gps_data = mock_gps.getLastSample(); // still before liftoff ... + imu_data = mock_imu.getLastSample(); // still before liftoff ... + ref_values = + NASReferenceValues{press_data.pressure, gps_data.latitude, + gps_data.longitude, imu_data.accelerationX, + imu_data.accelerationY, imu_data.accelerationZ, + imu_data.magneticFieldX, imu_data.magneticFieldY, + imu_data.magneticFieldZ}; REQUIRE(ref_values == controller.calibrator.getReferenceValues()); // Now we should be in ready @@ -144,7 +147,7 @@ TEST_CASE("Testing Navigation System Controller") REQUIRE(controller.testState(&NASCtrl::state_ready)); // Send liftoff event: should be in state active - sEventBroker->post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS); signalLiftoff(); Thread::sleep(100); REQUIRE(controller.testState(&NASCtrl::state_active)); diff --git a/src/tests/deathstack-boards/test-analog-board.cpp b/src/tests/deathstack-boards/test-analog-board.cpp index bc8dcd064..bf05c3c92 100644 --- a/src/tests/deathstack-boards/test-analog-board.cpp +++ b/src/tests/deathstack-boards/test-analog-board.cpp @@ -36,10 +36,9 @@ * Deatachment pins */ -#include <Common.h> -#include <drivers/adc/ADS1118/ADS1118.h> #include <interfaces-impl/hwmapping.h> #include <miosix.h> +#include <sensors/ADS1118/ADS1118.h> #include <sensors/MS5803/MS5803.h> #include <sensors/analog/pressure/MPXHZ6130A/MPXHZ6130A.h> #include <sensors/analog/pressure/honeywell/SSCDANN030PAA.h> @@ -76,8 +75,6 @@ void sampleAll(); int main() { - TimestampTimer::enableTimestampTimer(); - switch (menu()) { case 1: @@ -148,7 +145,7 @@ void sampleAnalogPressureSensor(ADS1118::ADS1118Mux channel) SPIBus spiBus(SPI1); SPIBusConfig spiConfig = ADS1118::getDefaultSPIConfig(); - spiConfig.clock_div = SPIClockDivider::DIV64; + spiConfig.clockDivider = SPI::ClockDivider::DIV_64; SPISlave spiSlave(spiBus, miosix::sensors::ads1118::cs::getPin(), spiConfig); @@ -169,8 +166,8 @@ void sampleAnalogPressureSensor(ADS1118::ADS1118Mux channel) { ads1118.sample(); pressureSensor.sample(); - printf("%llu,%.2f\n", pressureSensor.getLastSample().press_timestamp, - pressureSensor.getLastSample().press); + printf("%llu,%.2f\n", pressureSensor.getLastSample().pressureTimestamp, + pressureSensor.getLastSample().pressure); miosix::delayMs(1000 / SAMPLING_FREQUENCY); } @@ -183,7 +180,7 @@ void sampleMS5803() // Sensor setup SPIBusConfig spiCfg{}; - spiCfg.clock_div = SPIClockDivider::DIV16; + spiCfg.clockDivider = SPI::ClockDivider::DIV_16; SPIBus spiBus(SPI1); SPISlave spiSlave(spiBus, miosix::sensors::ms5803::cs::getPin(), spiCfg); MS5803 ms5803 = MS5803(spiSlave); @@ -193,7 +190,7 @@ void sampleMS5803() for (int i = 0; i < seconds * SAMPLING_FREQUENCY; i++) { ms5803.sample(); - printf("%.2f\n", ms5803.getLastSample().press); + printf("%.2f\n", ms5803.getLastSample().pressure); miosix::delayMs(1000 / SAMPLING_FREQUENCY); } @@ -208,9 +205,9 @@ void sampleAll() SPIBus spiBus(SPI1); SPIBusConfig adsSpiConfig = ADS1118::getDefaultSPIConfig(); - adsSpiConfig.clock_div = SPIClockDivider::DIV64; + adsSpiConfig.clockDivider = SPI::ClockDivider::DIV_64; SPISlave adsSpiSlave(spiBus, miosix::sensors::ads1118::cs::getPin(), - adsSpiConfig); + adsSpiConfig); ADS1118::ADS1118Config ads1118Config = ADS1118::ADS1118_DEFAULT_CONFIG; ads1118Config.bits.mode = ADS1118::ADS1118Mode::CONTIN_CONV_MODE; @@ -240,10 +237,11 @@ void sampleAll() SUPPLIED_VOLTAGE); SSCDRRN015PDA honeywell2(get_voltage_function_honeywell_2, SUPPLIED_VOLTAGE); - + SPIBusConfig ms5803SpiCfg{}; - ms5803SpiCfg.clock_div = SPIClockDivider::DIV16; - SPISlave ms5803SpiSlave(spiBus, miosix::sensors::ms5803::cs::getPin(), ms5803SpiCfg); + ms5803SpiCfg.clockDivider = SPI::ClockDivider::DIV_16; + SPISlave ms5803SpiSlave(spiBus, miosix::sensors::ms5803::cs::getPin(), + ms5803SpiCfg); MS5803 ms5803 = MS5803(ms5803SpiSlave); ms5803.init(); @@ -257,10 +255,11 @@ void sampleAll() honeywell2.sample(); ms5803.sample(); printf("%llu,%.2f,%.2f,%.2f,%.2f,%.2f\n", - ads1118.getLastSample().adc_timestamp, - npx1.getLastSample().press, npx2.getLastSample().press, - honeywell1.getLastSample().press, - honeywell2.getLastSample().press, ms5803.getLastSample().press); + ads1118.getLastSample().voltageTimestamp, + npx1.getLastSample().pressure, npx2.getLastSample().pressure, + honeywell1.getLastSample().pressure, + honeywell2.getLastSample().pressure, + ms5803.getLastSample().pressure); miosix::delayMs(1000 / SAMPLING_FREQUENCY); } diff --git a/src/tests/deathstack-boards/test-power-board.cpp b/src/tests/deathstack-boards/test-power-board.cpp index acc36ed2f..72d942bbd 100644 --- a/src/tests/deathstack-boards/test-power-board.cpp +++ b/src/tests/deathstack-boards/test-power-board.cpp @@ -29,11 +29,10 @@ */ #include <AirBrakes/AirBrakesServo.h> -#include <Common.h> #include <Deployment/DeploymentServo.h> -#include <drivers/adc/InternalADC/InternalADC.h> +#include <drivers/adc/InternalADC.h> #include <drivers/hbridge/HBridge.h> -#include <drivers/servo/servo.h> +#include <drivers/servo/Servo.h> #include <sensors/analog/battery/BatteryVoltageSensor.h> #include <cstdio> @@ -62,8 +61,6 @@ void sampleBatteryVoltage(); int main() { - TimestampTimer::enableTimestampTimer(); - switch (menu()) { case 1: @@ -120,7 +117,7 @@ void sampleBatteryVoltage() // Sensor setup - InternalADC internalADC = InternalADC(*ADC3, 3.3); + InternalADC internalADC = InternalADC(ADC3, 3.3); internalADC.enableChannel(InternalADC::CH5); internalADC.init(); @@ -138,10 +135,10 @@ void sampleBatteryVoltage() BatteryVoltageSensorData data = batterySensor.getLastSample(); // Calculate a simple linear battery percentage - float batteryPercentage = 100 * (data.bat_voltage - 9.6) / (12.6 - 9.6); + float batteryPercentage = 100 * (data.batVoltage - 9.6) / (12.6 - 9.6); - printf("%llu,%.3f,%.3f,%.1f\n", data.adc_timestamp, data.voltage, - data.bat_voltage, batteryPercentage); + printf("%llu,%.3f,%.3f,%.1f\n", data.voltageTimestamp, data.voltage, + data.batVoltage, batteryPercentage); miosix::delayMs(1000 / SAMPLING_FREQUENCY); } diff --git a/src/tests/deathstack-boards/test-rf-board.cpp b/src/tests/deathstack-boards/test-rf-board.cpp index 6448e6d7a..b24ab76a9 100644 --- a/src/tests/deathstack-boards/test-rf-board.cpp +++ b/src/tests/deathstack-boards/test-rf-board.cpp @@ -31,13 +31,12 @@ * SD card */ -#include <Common.h> -#include <drivers/gps/ublox/UbloxGPS.h> #include <drivers/spi/SPIDriver.h> #include <interfaces-impl/hwmapping.h> #include <miosix.h> #include <sensors/BMX160/BMX160.h> #include <sensors/LIS3MDL/LIS3MDL.h> +#include <sensors/UbloxGPS/UbloxGPS.h> #include <ctime> #include <iostream> @@ -46,6 +45,8 @@ #include "math/Stats.h" +using namespace Boardcore; + namespace SDCardBenchmark { #include "../../skyward-boardcore/src/entrypoints/sdcard-benchmark.cpp" @@ -132,8 +133,8 @@ void sampleLIS3MDL() SPIBus spiBus(SPI1); SPIBusConfig spiConfig; - spiConfig.clock_div = SPIClockDivider::DIV32; - spiConfig.mode = SPIMode::MODE1; + spiConfig.clockDivider = SPI::ClockDivider::DIV_32; + spiConfig.mode = SPI::Mode::MODE_1; LIS3MDL::Config lis3mdlConfig; lis3mdlConfig.odr = LIS3MDL::ODR_560_HZ; @@ -167,9 +168,9 @@ void sampleBMX160() SPIBus spiBus(SPI1); BMX160Config bmx160Config; - bmx160Config.fifo_mode = BMX160Config::FifoMode::DISABLED; - bmx160Config.fifo_int = BMX160Config::FifoInterruptPin::PIN_INT1; - bmx160Config.temp_divider = 1; + bmx160Config.fifoMode = BMX160Config::FifoMode::DISABLED; + bmx160Config.fifoInterrupt = BMX160Config::FifoInterruptPin::PIN_INT1; + bmx160Config.temperatureDivider = 1; BMX160 bmx160 = BMX160(spiBus, miosix::sensors::bmx160::cs::getPin(), bmx160Config); @@ -197,8 +198,8 @@ void sampleAll() SPIBus spiBus(SPI1); SPIBusConfig spiConfig; - spiConfig.clock_div = SPIClockDivider::DIV32; - spiConfig.mode = SPIMode::MODE1; + spiConfig.clockDivider = SPI::ClockDivider::DIV_32; + spiConfig.mode = SPI::Mode::MODE_1; LIS3MDL::Config lis3mdlConfig; lis3mdlConfig.odr = LIS3MDL::ODR_560_HZ; @@ -210,9 +211,9 @@ void sampleAll() lis3mdl.init(); BMX160Config bmx160Config; - bmx160Config.fifo_mode = BMX160Config::FifoMode::DISABLED; - bmx160Config.fifo_int = BMX160Config::FifoInterruptPin::PIN_INT1; - bmx160Config.temp_divider = 1; + bmx160Config.fifoMode = BMX160Config::FifoMode::DISABLED; + bmx160Config.fifoInterrupt = BMX160Config::FifoInterruptPin::PIN_INT1; + bmx160Config.temperatureDivider = 1; BMX160 bmx160 = BMX160(spiBus, miosix::sensors::bmx160::cs::getPin(), bmx160Config); @@ -240,7 +241,8 @@ void sampleGPS() // Sensor setup - UbloxGPS gps(38400); + UbloxGPSSerial gps; + gps.init(); miosix::delayMs(200); gps.start(); diff --git a/src/tests/deathstack-boards/test-stm-board.cpp b/src/tests/deathstack-boards/test-stm-board.cpp index 053619d50..5f1d3133c 100644 --- a/src/tests/deathstack-boards/test-stm-board.cpp +++ b/src/tests/deathstack-boards/test-stm-board.cpp @@ -28,8 +28,6 @@ * External oscillator */ -#include <Common.h> - #include <iostream> #include <sstream> @@ -53,8 +51,6 @@ int askSeconds(); int main() { - TimestampTimer::enableTimestampTimer(); - switch (menu()) { case 1: diff --git a/src/tests/deployment/test-deployment-interactive.cpp b/src/tests/deployment/test-deployment-interactive.cpp index eba886c72..c0bcf55ca 100644 --- a/src/tests/deployment/test-deployment-interactive.cpp +++ b/src/tests/deployment/test-deployment-interactive.cpp @@ -73,7 +73,7 @@ int main() ejection_servo.enable(); ejection_servo.reset(); - sEventBroker->start(); + sEventBroker.start(); string temp; for (;;) @@ -172,7 +172,7 @@ void cuttingSequence() deploymentController.start(); Thread::sleep(1000); - sEventBroker->post({EV_CUT_DROGUE}, TOPIC_DPL); + sEventBroker.post({EV_CUT_DROGUE}, TOPIC_DPL); waitForEvent(EV_CUTTING_TIMEOUT, TOPIC_DPL, waitEventTimeout); @@ -220,12 +220,12 @@ void cutterContinuity() if (cutter == 1) { cout << "Activating primary cutter...\n"; - sEventBroker->post({EV_TEST_CUT_PRIMARY}, TOPIC_DPL); + sEventBroker.post({EV_TEST_CUT_PRIMARY}, TOPIC_DPL); } else { cout << "Activating backup cutter...\n"; - sEventBroker->post({EV_TEST_CUT_BACKUP}, TOPIC_DPL); + sEventBroker.post({EV_TEST_CUT_BACKUP}, TOPIC_DPL); } waitForEvent(EV_CUTTING_TIMEOUT, TOPIC_DPL); @@ -243,7 +243,7 @@ void noseconeEjection() waitUserInput(); - EventCounter counter{*sEventBroker}; + EventCounter counter{sEventBroker}; HBridge primaryCutter{PrimaryCutterEna::getPin(), CUTTER_TIM, CUTTER_CHANNEL_PRIMARY, primaryCutterFrequency, @@ -266,7 +266,7 @@ void noseconeEjection() } deploymentController.start(); - sEventBroker->post({EV_NC_OPEN}, TOPIC_DPL); + sEventBroker.post({EV_NC_OPEN}, TOPIC_DPL); for (;;) { diff --git a/src/tests/drivers/test-cutter.cpp b/src/tests/drivers/test-cutter.cpp index 5b1f45b74..e7eed18de 100644 --- a/src/tests/drivers/test-cutter.cpp +++ b/src/tests/drivers/test-cutter.cpp @@ -20,9 +20,8 @@ * THE SOFTWARE. */ -#include <Common.h> #include <configs/CutterConfig.h> -#include <drivers/adc/InternalADC/InternalADC.h> +#include <drivers/adc/InternalADC.h> #include <drivers/hbridge/HBridge.h> #include <sensors/analog/current/CurrentSensor.h> @@ -40,6 +39,7 @@ */ using namespace std; +using namespace miosix; using namespace Boardcore; using namespace DeathStackBoard::CutterConfig; @@ -51,18 +51,18 @@ constexpr int SAMPLING_FREQUENCY = 20; * They are not useful for activating the COTS ones * (i.e. they are currently used only in the test-cutter entrypoint) */ -static const PWM::Timer CUTTER_TIM{ - TIM9, &(RCC->APB2ENR), RCC_APB2ENR_TIM9EN, - TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB2)}; +static TIM_TypeDef *const CUTTER_TIM = TIM9; -static const PWMChannel CUTTER_CHANNEL_PRIMARY = PWMChannel::CH2; -static const PWMChannel CUTTER_CHANNEL_BACKUP = PWMChannel::CH2; +static const TimerUtils::Channel CUTTER_CHANNEL_PRIMARY = + TimerUtils::Channel::CHANNEL_2; +static const TimerUtils::Channel CUTTER_CHANNEL_BACKUP = + TimerUtils::Channel::CHANNEL_2; static const unsigned int PRIMARY_CUTTER_PWM_FREQUENCY = 10000; // Hz static constexpr float PRIMARY_CUTTER_PWM_DUTY_CYCLE = 0.45f; static const unsigned int BACKUP_CUTTER_PWM_FREQUENCY = 10000; // Hz static constexpr float BACKUP_CUTTER_PWM_DUTY_CYCLE = 0.45f; static constexpr float CUTTER_TEST_PWM_DUTY_CYCLE = 0.1f; -static constexpr int CUT_TEST_DURATION = 1 * 1000; +static constexpr int CUT_TEST_DURATION = 1 * 1000; // This could go into CutterConfig static constexpr InternalADC::Channel ADC_CHANNEL_PRIMARY = @@ -79,13 +79,15 @@ static constexpr float ADC_TO_CURR_COEFF = ADC_TO_CURR_DKILIS / ADC_TO_CURR_RIS; static constexpr float ADC_TO_CURR_OFFSET = ADC_TO_CURR_DKILIS * ADC_TO_CURR_IISOFF; -function<float(float)> adc_to_current = [](float adc_in) { +function<float(float)> adc_to_current = [](float adc_in) +{ return ADC_TO_CURR_DKILIS * (adc_in / ADC_TO_CURR_RIS - ADC_TO_CURR_IISOFF); }; bool finished = false; -void menu(unsigned int *cutterNo, uint32_t *frequency, float *dutyCycle, uint32_t *cutDuration); +void menu(unsigned int *cutterNo, uint32_t *frequency, float *dutyCycle, + uint32_t *cutDuration); void elapsedTimeAndCsense(void *args); @@ -94,9 +96,7 @@ int main() unsigned int cutterNo = 0; uint32_t frequency = 0; float dutyCycle = 0; - uint32_t cutDuration = 0; // for cots cutters - - TimestampTimer::enableTimestampTimer(); + uint32_t cutDuration = 0; // for cots cutters // Set the clock divider for the analog circuitry (/8) ADC->CCR |= ADC_CCR_ADCPRE_0 | ADC_CCR_ADCPRE_1; @@ -107,7 +107,7 @@ int main() // Cutter setup GpioPin *ena_pin; - PWMChannel pwmChannel; + TimerUtils::Channel pwmChannel; if (cutterNo == 3) // COTS cutters { @@ -125,19 +125,17 @@ int main() { // SRAD cutters if (cutterNo == 1) { - ena_pin = new GpioPin( - PrimaryCutterEna::getPin()); + ena_pin = new GpioPin(PrimaryCutterEna::getPin()); pwmChannel = CUTTER_CHANNEL_PRIMARY; } else // if (cutterNo == 2) { - ena_pin = new GpioPin( - BackupCutterEna::getPin()); + ena_pin = new GpioPin(BackupCutterEna::getPin()); pwmChannel = CUTTER_CHANNEL_BACKUP; } - HBridge cutter(*ena_pin, CUTTER_TIM, - pwmChannel, frequency, dutyCycle / 100.0f); + HBridge cutter(*ena_pin, CUTTER_TIM, pwmChannel, frequency, + dutyCycle / 100.0f); // Start the test @@ -221,7 +219,7 @@ void elapsedTimeAndCsense(void *args) int cutterNo = *(unsigned int *)args; // Sensors setup - InternalADC internalADC(*ADC3, 3.3); + InternalADC internalADC(ADC3, 3.3); internalADC.init(); internalADC.enableChannel(ADC_CHANNEL_PRIMARY); internalADC.enableChannel(ADC_CHANNEL_BACKUP); @@ -243,14 +241,14 @@ void elapsedTimeAndCsense(void *args) current_sensor.init(); // Save the cuttent timestamp - uint64_t t = TimestampTimer::getTimestamp() / 1000; + uint64_t t = TimestampTimer::getInstance().getTimestamp() / 1000; uint64_t t0 = t; while (t < t0 + MAX_CUTTING_TIME && !finished) { Thread::sleep(1000 / SAMPLING_FREQUENCY); - t = TimestampTimer::getTimestamp() / 1000; + t = TimestampTimer::getInstance().getTimestamp() / 1000; internalADC.sample(); current_sensor.sample(); diff --git a/src/tests/drivers/test-imus.cpp b/src/tests/drivers/test-imus.cpp index 78c6b4b9d..321068864 100644 --- a/src/tests/drivers/test-imus.cpp +++ b/src/tests/drivers/test-imus.cpp @@ -20,7 +20,6 @@ * THE SOFTWARE. */ -#include <Common.h> #include <interfaces-impl/hwmapping.h> #include "configs/SensorManagerConfig.h" diff --git a/src/tests/drivers/test-mavlink.cpp b/src/tests/drivers/test-mavlink.cpp index 45f2e47a7..8f5aecab8 100644 --- a/src/tests/drivers/test-mavlink.cpp +++ b/src/tests/drivers/test-mavlink.cpp @@ -20,7 +20,6 @@ * THE SOFTWARE. */ -#include <Common.h> #include <configs/TMTCConfig.h> #include <drivers/Xbee/Xbee.h> #include <drivers/gamma868/Gamma868.h> diff --git a/src/tests/drivers/test-servo.cpp b/src/tests/drivers/test-servo.cpp index 4fdf14806..7df7efba4 100644 --- a/src/tests/drivers/test-servo.cpp +++ b/src/tests/drivers/test-servo.cpp @@ -30,6 +30,7 @@ #include <string> using namespace std; +using namespace Boardcore; using namespace DeathStackBoard; int servoMenu(); @@ -51,7 +52,7 @@ float perlin2d(float x, float y, float freq, int depth); int servoChoice; ServoInterface* servo; -PWMChannel channel; +TimerUtils::Channel channel; constexpr int WIGGLE_STEPS = 5; @@ -77,8 +78,6 @@ static int hashh[] = { int main() { - TimestampTimer::enableTimestampTimer(); - servoChoice = servoMenu(); switch (servoChoice) @@ -266,15 +265,17 @@ void servoBreakIn() waitUserInput(); - uint64_t start = TimestampTimer::getTimestamp(); + uint64_t start = TimestampTimer::getInstance().getTimestamp(); - while (TimestampTimer::getTimestamp() - start < minutes * 60 * 1000000) + while (TimestampTimer::getInstance().getTimestamp() - start < + minutes * 60 * 1000000) { - uint64_t start2 = TimestampTimer::getTimestamp(); + uint64_t start2 = TimestampTimer::getInstance().getTimestamp(); float counter = 0; // 10 seconds - while (TimestampTimer::getTimestamp() - start2 < 10 * 1000000) + while (TimestampTimer::getInstance().getTimestamp() - start2 < + 10 * 1000000) { double angolo = servo->MIN_POS + abs(perlin2d(counter, 0, .075, 10)) * diff --git a/src/tests/eigen-test.cpp b/src/tests/eigen-test.cpp index 7a3ebb4d0..52cedb086 100644 --- a/src/tests/eigen-test.cpp +++ b/src/tests/eigen-test.cpp @@ -20,7 +20,6 @@ * THE SOFTWARE. */ -#include <Common.h> #include <miosix.h> #include <Eigen/Dense> diff --git a/src/tests/hardware_in_the_loop/test-HIL+ADA+Aerobrake/test-HIL+ADA+Aerobrake.cpp b/src/tests/hardware_in_the_loop/test-HIL+ADA+Aerobrake/test-HIL+ADA+Aerobrake.cpp index d57fd42a4..132d3426b 100644 --- a/src/tests/hardware_in_the_loop/test-HIL+ADA+Aerobrake/test-HIL+ADA+Aerobrake.cpp +++ b/src/tests/hardware_in_the_loop/test-HIL+ADA+Aerobrake/test-HIL+ADA+Aerobrake.cpp @@ -22,7 +22,6 @@ #define EIGEN_RUNTIME_NO_MALLOC // enable eigen malloc usage assert -#include <Common.h> #include <events/EventBroker.h> #include <events/Events.h> @@ -86,8 +85,6 @@ int main() // crash if eigen dynamically allocates memory Eigen::internal::set_is_malloc_allowed(false); - TimestampTimer::enableTimestampTimer(); - // Definition of the flight phases manager HILFlightPhasesManager *flightPhasesManager = HILFlightPhasesManager::getInstance(); @@ -184,7 +181,7 @@ int main() /*-------------- Events --------------*/ - EventCounter counter{*sEventBroker}; + EventCounter counter{sEventBroker}; counter.subscribe(TOPIC_FLIGHT_EVENTS); counter.subscribe(TOPIC_ADA); @@ -224,7 +221,7 @@ int main() matlab->start(); ada_controller->start(); - sEventBroker->start(); + sEventBroker.start(); scheduler.start(); // started only the scheduler instead of the SM /*---------- Normal execution --------*/ diff --git a/src/tests/hardware_in_the_loop/test-HIL+ADA+AerobrakeController+nas/test-HIL+ADA+AerobrakeController+nas.cpp b/src/tests/hardware_in_the_loop/test-HIL+ADA+AerobrakeController+nas/test-HIL+ADA+AerobrakeController+nas.cpp index 51bb28c8b..03f914046 100644 --- a/src/tests/hardware_in_the_loop/test-HIL+ADA+AerobrakeController+nas/test-HIL+ADA+AerobrakeController+nas.cpp +++ b/src/tests/hardware_in_the_loop/test-HIL+ADA+AerobrakeController+nas/test-HIL+ADA+AerobrakeController+nas.cpp @@ -22,7 +22,6 @@ #define EIGEN_RUNTIME_NO_MALLOC // enable eigen malloc usage assert -#include <Common.h> #include <events/EventBroker.h> #include <events/Events.h> #include <events/utils/EventCounter.h> @@ -80,8 +79,6 @@ int main() // crash if eigen dynamically allocates memory Eigen::internal::set_is_malloc_allowed(false); - TimestampTimer::enableTimestampTimer(); - // Definition of the flight phases manager HILFlightPhasesManager *flightPhasesManager = HILFlightPhasesManager::getInstance(); @@ -170,7 +167,7 @@ int main() /*-------------- Events --------------*/ - EventCounter counter{*sEventBroker}; + EventCounter counter{sEventBroker}; counter.subscribe(TOPIC_FLIGHT_EVENTS); counter.subscribe(TOPIC_ADA); counter.subscribe(TOPIC_NAS); @@ -214,7 +211,7 @@ int main() ada_controller->start(); nas_controller.start(); airbrakeController.start(); - sEventBroker->start(); + sEventBroker.start(); scheduler.start(); // started only the scheduler instead of the SM /*---------- Normal execution --------*/ diff --git a/src/tests/hardware_in_the_loop/test-HIL+ADA/test-HIL+ADA.cpp b/src/tests/hardware_in_the_loop/test-HIL+ADA/test-HIL+ADA.cpp index 276a1ccd5..26f8a4381 100644 --- a/src/tests/hardware_in_the_loop/test-HIL+ADA/test-HIL+ADA.cpp +++ b/src/tests/hardware_in_the_loop/test-HIL+ADA/test-HIL+ADA.cpp @@ -22,16 +22,15 @@ #define EIGEN_RUNTIME_NO_MALLOC // enable eigen malloc usage assert -#include <Common.h> #include <events/EventBroker.h> #include <events/Events.h> #include <algorithm> +#include <catch2/catch.hpp> #include <cmath> #include <iostream> #include <random> #include <sstream> -#include <catch2/catch.hpp> #include "TimestampTimer.h" #include "miosix.h" @@ -91,8 +90,6 @@ int main() // crash if eigen dynamically allocates memory Eigen::internal::set_is_malloc_allowed(false); - TimestampTimer::enableTimestampTimer(); - // Definition of the flight phases manager HILFlightPhasesManager *flightPhasesManager = HILFlightPhasesManager::getInstance(); @@ -208,7 +205,7 @@ int main() /*-------------- Events --------------*/ - EventCounter counter{*sEventBroker}; + EventCounter counter{sEventBroker}; counter.subscribe({TOPIC_FLIGHT_EVENTS}); counter.subscribe({TOPIC_ADA}); @@ -248,7 +245,7 @@ int main() matlab->start(); ada_controller->start(); - sEventBroker->start(); + sEventBroker.start(); scheduler.start(); // started only the scheduler instead of the SM /*---------- Normal execution --------*/ diff --git a/src/tests/hardware_in_the_loop/test-HIL+Aerobrake/test-HIL+Aerobrake.cpp b/src/tests/hardware_in_the_loop/test-HIL+Aerobrake/test-HIL+Aerobrake.cpp index 8f250978b..cf757034e 100644 --- a/src/tests/hardware_in_the_loop/test-HIL+Aerobrake/test-HIL+Aerobrake.cpp +++ b/src/tests/hardware_in_the_loop/test-HIL+Aerobrake/test-HIL+Aerobrake.cpp @@ -20,7 +20,6 @@ * THE SOFTWARE. */ -#include <Common.h> #include <events/EventBroker.h> #include "TimestampTimer.h" @@ -73,8 +72,6 @@ int main() { isSimulationRunning = true; - TimestampTimer::enableTimestampTimer(); - // Definition of the flight phases manager HILFlightPhasesManager *flightPhasesManager = HILFlightPhasesManager::getInstance(); diff --git a/src/tests/hardware_in_the_loop/test-HIL/test-HIL.cpp b/src/tests/hardware_in_the_loop/test-HIL/test-HIL.cpp index 0658ee791..65825262c 100644 --- a/src/tests/hardware_in_the_loop/test-HIL/test-HIL.cpp +++ b/src/tests/hardware_in_the_loop/test-HIL/test-HIL.cpp @@ -20,7 +20,6 @@ * THE SOFTWARE. */ -#include <Common.h> #include <events/EventBroker.h> #include "TimestampTimer.h" @@ -61,8 +60,6 @@ int main() { isSimulationRunning = true; - TimestampTimer::enableTimestampTimer(); - // Definition of the flight phases manager HILFlightPhasesManager *flightPhasesManager = HILFlightPhasesManager::getInstance(); diff --git a/src/tests/hardware_in_the_loop/test-SerialInterface/test-SerialInterface.cpp b/src/tests/hardware_in_the_loop/test-SerialInterface/test-SerialInterface.cpp index 62cf5a6e3..80f8dace6 100644 --- a/src/tests/hardware_in_the_loop/test-SerialInterface/test-SerialInterface.cpp +++ b/src/tests/hardware_in_the_loop/test-SerialInterface/test-SerialInterface.cpp @@ -20,8 +20,6 @@ * THE SOFTWARE. */ -#include <Common.h> - #include "hardware_in_the_loop/HILConfig.h" #include "hardware_in_the_loop/simulator_communication/SerialInterface.h" #include "math/Vec3.h" diff --git a/src/tests/mock_sensors/test-mock-sensors.cpp b/src/tests/mock_sensors/test-mock-sensors.cpp index d21fa7b4e..2181291ea 100644 --- a/src/tests/mock_sensors/test-mock-sensors.cpp +++ b/src/tests/mock_sensors/test-mock-sensors.cpp @@ -20,15 +20,12 @@ * THE SOFTWARE. */ -#include <Common.h> #include <mocksensors/MockSensors.h> using namespace DeathStackBoard; int main() { - TimestampTimer::enableTimestampTimer(); - MockGPS gps; MockPressureSensor p_sensor; MockIMU imu; @@ -55,7 +52,7 @@ int main() p_sensor.sample(); PressureData pressure = p_sensor.getLastSample(); TRACE("PRESSURE SAMPLE: \n"); - TRACE("pressure = %f \n\n", pressure.press); + TRACE("pressure = %f \n\n", pressure.pressure); imu.sample(); MockIMUData imu_data = imu.getLastSample(); diff --git a/src/tests/ram_test/ramtest.cpp b/src/tests/ram_test/ramtest.cpp index d8e0e9862..312252ed9 100644 --- a/src/tests/ram_test/ramtest.cpp +++ b/src/tests/ram_test/ramtest.cpp @@ -1,19 +1,19 @@ - /*************************************************************************** - * Copyright (C) 2012 by Terraneo Federico * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; either version 2 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, see <http://www.gnu.org/licenses/> * - ***************************************************************************/ +/*************************************************************************** + * Copyright (C) 2012 by Terraneo Federico * + * * + * This program is free software; you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation; either version 2 of the License, or * + * (at your option) any later version. * + * * + * This program is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU General Public License for more details. * + * * + * You should have received a copy of the GNU General Public License * + * along with this program; if not, see <http://www.gnu.org/licenses/> * + ***************************************************************************/ // RAM testing code // Useful to test the external RAM of a board. @@ -22,64 +22,78 @@ #include <stdlib.h> #include <string.h> #include <unistd.h> + #include "sha1.h" -const unsigned int ramBase=0xd0000000; //Tune this to the right value -const unsigned int ramSize=8*1024*1024; //Tune this to the right value +const unsigned int ramBase = 0xd0000000; // Tune this to the right value +const unsigned int ramSize = 8 * 1024 * 1024; // Tune this to the right value bool shaCmp(unsigned int a[5], unsigned int b[5]) { - if(memcmp(a,b,20)==0) return false; + if (memcmp(a, b, 20) == 0) + return false; iprintf("Mismatch\n"); - for(int i=0;i<5;i++) iprintf("%04x",a[i]); iprintf("\n"); - for(int i=0;i<5;i++) iprintf("%04x",b[i]); iprintf("\n"); + for (int i = 0; i < 5; i++) + iprintf("%04x", a[i]); + iprintf("\n"); + for (int i = 0; i < 5; i++) + iprintf("%04x", b[i]); + iprintf("\n"); return true; } -template<typename T> bool ramTest() +template <typename T> +bool ramTest() { SHA1 sha; sha.Reset(); - srand(0x7ad3c099*sizeof(T)); //Just to shuffle initialization between tests - for(unsigned int i=ramBase;i<ramBase+ramSize;i+=sizeof(T)) + srand(0x7ad3c099 * + sizeof(T)); // Just to shuffle initialization between tests + for (unsigned int i = ramBase; i < ramBase + ramSize; i += sizeof(T)) { - T *p=reinterpret_cast<T*>(i); - T v=static_cast<T>(rand()); - *p=v; - sha.Input(reinterpret_cast<const unsigned char*>(&v),sizeof(T)); + T *p = reinterpret_cast<T *>(i); + T v = static_cast<T>(rand()); + *p = v; + sha.Input(reinterpret_cast<const unsigned char *>(&v), sizeof(T)); } unsigned int a[5]; sha.Result(a); - sleep(10); //To check SDRAM retention ability + sleep(10); // To check SDRAM retention ability sha.Reset(); - for(unsigned int i=ramBase;i<ramBase+ramSize;i+=sizeof(T)) + for (unsigned int i = ramBase; i < ramBase + ramSize; i += sizeof(T)) { - T *p=reinterpret_cast<T*>(i); - T v=*p; - sha.Input(reinterpret_cast<const unsigned char*>(&v),sizeof(T)); + T *p = reinterpret_cast<T *>(i); + T v = *p; + sha.Input(reinterpret_cast<const unsigned char *>(&v), sizeof(T)); } unsigned int b[5]; sha.Result(b); - return shaCmp(a,b); + return shaCmp(a, b); } int main() { printf("\nBEFORE YOU START:\n"); - printf("1. Have you compiled with the right linker script (e.g. 2m+256k_rom)?\n"); - printf("2. Have you set RamBase and RamSize correctly in this entrypoint?\n"); + printf( + "1. Have you compiled with the right linker script (e.g. " + "2m+256k_rom)?\n"); + printf( + "2. Have you set RamBase and RamSize correctly in this entrypoint?\n"); printf("3. Have you enable the RAM (aka compile with -D__ENABLE_XRAM)?\n"); printf("Press enter to start..."); getchar(); - for(;;) + for (;;) { iprintf("RAM test\nTesting word size transfers\n"); - if(ramTest<unsigned int>()) return 1; + if (ramTest<unsigned int>()) + return 1; iprintf("Testing halfword size transfers\n"); - if(ramTest<unsigned short>()) return 1; + if (ramTest<unsigned short>()) + return 1; iprintf("Testing byte size transfers\n"); - if(ramTest<unsigned char>()) return 1; + if (ramTest<unsigned char>()) + return 1; iprintf("Ok\n"); } } \ No newline at end of file diff --git a/src/tests/test-ada-dpl-simulation.cpp b/src/tests/test-ada-dpl-simulation.cpp index fb0342458..e3763db59 100644 --- a/src/tests/test-ada-dpl-simulation.cpp +++ b/src/tests/test-ada-dpl-simulation.cpp @@ -66,7 +66,8 @@ public: } } - return PressureData{TimestampTimer::getTimestamp(), press}; + return PressureData{TimestampTimer::getInstance().getTimestamp(), + press}; } void signalLiftoff() { before_liftoff = false; } @@ -95,8 +96,6 @@ public: int main() { - TimestampTimer::enableTimestampTimer(); - MockPressureSensor baro; MockGPSSensor gps; @@ -111,8 +110,8 @@ int main() DeploymentController dpl_controller; PinHandler pin_handler; - sEventBroker->start(); - EventCounter counter{*sEventBroker}; + sEventBroker.start(); + EventCounter counter{sEventBroker}; counter.subscribe(TOPIC_ADA); counter.subscribe(TOPIC_FLIGHT_EVENTS); @@ -129,7 +128,7 @@ int main() Thread::sleep(1000); // Enter ADA calibration state - sEventBroker->post({EV_CALIBRATE_ADA}, TOPIC_ADA); + sEventBroker.post({EV_CALIBRATE_ADA}, TOPIC_ADA); Thread::sleep(100); // Send baro calibration samples for ADA @@ -164,7 +163,7 @@ int main() TRACE("Sending EV_LIFTOFF ... \n"); // Send liftoff event - sEventBroker->post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS); baro.signalLiftoff(); bool first_apogee_detection = true; @@ -185,7 +184,7 @@ int main() first_apogee_detection = false; TRACE("Triggering nosecone ejection ... \n\n"); - sEventBroker->post({EV_NC_OPEN}, TOPIC_DPL); + sEventBroker.post({EV_NC_OPEN}, TOPIC_DPL); } if (ada_controller.getStatus().dpl_altitude_reached && @@ -195,7 +194,7 @@ int main() first_dpl_altitude_detection = false; TRACE("Triggering cutting sequence ... \n\n"); - sEventBroker->post({EV_CUT_DROGUE}, TOPIC_DPL); + sEventBroker.post({EV_CUT_DROGUE}, TOPIC_DPL); } } diff --git a/src/tests/test-fmm-interactive.cpp b/src/tests/test-fmm-interactive.cpp index 7ed81fb54..19d488fde 100644 --- a/src/tests/test-fmm-interactive.cpp +++ b/src/tests/test-fmm-interactive.cpp @@ -36,12 +36,12 @@ void printEvent(uint8_t event, uint8_t topic) } int main() { - sEventBroker->start(); + sEventBroker.start(); FMMController* fmm = new FMMController(); fmm->start(); EventSniffer* sniffer = - new EventSniffer(*sEventBroker, TOPIC_LIST, printEvent); + new EventSniffer(sEventBroker, TOPIC_LIST, printEvent); UNUSED(sniffer); // The sniffer's handler will be called by evBroker printf("\nOk\n"); @@ -53,11 +53,11 @@ int main() { printf("Choose an event\n"); printf("ID:\n"); - scanf("%hhu", &(ev.sig)); + scanf("%hhu", &(ev.code)); printf("TOPIC:\n"); scanf("%d", &topic); - sEventBroker->post(ev, topic); + sEventBroker.post(ev, topic); } return 0; diff --git a/src/tests/test-kalman-hitl.cpp b/src/tests/test-kalman-hitl.cpp index f534a48f4..c9954f919 100644 --- a/src/tests/test-kalman-hitl.cpp +++ b/src/tests/test-kalman-hitl.cpp @@ -20,7 +20,6 @@ * THE SOFTWARE. */ -#include <Common.h> #include <EventClasses.h> #include <events/EventBroker.h> #include <events/Events.h> @@ -49,7 +48,7 @@ int main() } // Start event broker and ada - sEventBroker->start(); + sEventBroker.start(); ada.start(); // Read serial @@ -99,15 +98,15 @@ void handleCommands(std::string line) // } else if (command == "EV_TC_RESET_CALIBRATION") { - sEventBroker->post({EV_TC_RESET_CALIBRATION}, TOPIC_ADA); + sEventBroker.post({EV_TC_RESET_CALIBRATION}, TOPIC_ADA); } else if (command == "EV_LIFTOFF") { - sEventBroker->post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS); } else if (command == "EV_APOGEE") { - sEventBroker->post({EV_APOGEE}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post({EV_APOGEE}, TOPIC_FLIGHT_EVENTS); } else if (command == "EV_SET_DPL_ALTITUDE") { @@ -115,11 +114,11 @@ void handleCommands(std::string line) DeploymentAltitudeEvent ev; ev.dplAltitude = alt; ev.sig = EV_TC_SET_DPL_ALTITUDE; - sEventBroker->post(ev, TOPIC_TC); + sEventBroker.post(ev, TOPIC_TC); } else if (command == "EV_DPL_ALTITUDE") { - sEventBroker->post({EV_DPL_ALTITUDE}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post({EV_DPL_ALTITUDE}, TOPIC_FLIGHT_EVENTS); } else { diff --git a/src/tests/test-logproxy.cpp b/src/tests/test-logproxy.cpp index 754109d69..d19b0d029 100644 --- a/src/tests/test-logproxy.cpp +++ b/src/tests/test-logproxy.cpp @@ -19,7 +19,6 @@ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ -#include <Common.h> #include <LoggerService/LoggerService.h> using namespace miosix; diff --git a/src/tests/test-pinhandler.cpp b/src/tests/test-pinhandler.cpp index 93d116ed1..ee9b069c7 100644 --- a/src/tests/test-pinhandler.cpp +++ b/src/tests/test-pinhandler.cpp @@ -20,7 +20,6 @@ * THE SOFTWARE. */ -#include "Common.h" #include "PinHandler/PinHandler.h" using namespace Boardcore; @@ -29,8 +28,6 @@ using namespace DeathStackBoard; int main() { - TimestampTimer::enableTimestampTimer(); - PinHandler pin_handler; if (!pin_handler.start()) diff --git a/src/tests/test-sensormanager.cpp b/src/tests/test-sensormanager.cpp index e2e90c5f6..b2f9e2f20 100644 --- a/src/tests/test-sensormanager.cpp +++ b/src/tests/test-sensormanager.cpp @@ -21,7 +21,6 @@ */ #include "ADA/ADAController.h" -#include "Common.h" #include "LoggerService/LoggerService.h" #include "Radio/TMTCManager.h" #include "SensorManager/SensorManager.h" @@ -34,9 +33,7 @@ using namespace DeathStackBoard; int main() { - TimestampTimer::enableTimestampTimer(); - - sEventBroker->start(); + sEventBroker.start(); Stats s; SensorManager mgr; @@ -58,7 +55,7 @@ int main() printf("CPU: %f%%, min: %f max: %f\n", s.getStats().mean, s.getStats().minValue, s.getStats().maxValue); - LoggerService::getInstance()->stop(); + LoggerService::getInstance().stop(); printf("End\n"); for (;;) diff --git a/src/tests/test-sm+tmtc.cpp b/src/tests/test-sm+tmtc.cpp index 9024a013b..6726bb8fb 100644 --- a/src/tests/test-sm+tmtc.cpp +++ b/src/tests/test-sm+tmtc.cpp @@ -27,7 +27,6 @@ #include <interfaces-impl/hwmapping.h> #include "ADA/ADAController.h" -#include "Common.h" #include "LoggerService/LoggerService.h" #include "SensorManager/SensorManager.h" #include "diagnostic/CpuMeter.h" @@ -50,7 +49,7 @@ int main() // ada.start(); // try // { - // LoggerService::getInstance()->start(); + // LoggerService::getInstance().start(); // } // catch (const std::exception& e) // { @@ -65,10 +64,10 @@ int main() // } // led1::high(); - sEventBroker->start(); + sEventBroker.start(); mgr.start(); - sEventBroker->post({EV_TC_START_SENSOR_LOGGING}, TOPIC_TC); + sEventBroker.post({EV_TC_START_SENSOR_LOGGING}, TOPIC_TC); printf("Wait for calibration to complete.\n"); @@ -80,7 +79,7 @@ int main() TMTCManager* tmtc = new TMTCManager(); tmtc->start(); - sEventBroker->start(); + sEventBroker.start(); Thread::sleep(1000); @@ -94,7 +93,7 @@ int main() Thread::sleep(100); - sEventBroker->post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS); for (;;) { diff --git a/src/tests/test-tmtc.cpp b/src/tests/test-tmtc.cpp index b7eedc555..e46f383e4 100644 --- a/src/tests/test-tmtc.cpp +++ b/src/tests/test-tmtc.cpp @@ -41,10 +41,10 @@ int main() TMTCController* tmtc = new TMTCController(); tmtc->start(); - sEventBroker->start(); + sEventBroker.start(); EventSniffer* sniffer = - new EventSniffer(*sEventBroker, TOPIC_LIST, onEventReceived); + new EventSniffer(sEventBroker, TOPIC_LIST, onEventReceived); Thread::sleep(1000); @@ -54,7 +54,7 @@ int main() Thread::sleep(100); - sEventBroker->post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post({EV_LIFTOFF}, TOPIC_FLIGHT_EVENTS); printf("Press close to reboot...\n"); while (inputs::btn_close::value()) -- GitLab From 524b7776935aa3506903b989417214299055167b Mon Sep 17 00:00:00 2001 From: Alberto Nidasio <alberto.nidasio@skywarder.eu> Date: Tue, 1 Feb 2022 15:12:07 +0100 Subject: [PATCH 4/6] Set up CI/CD and pre commit hooks --- .gitlab-ci.yml | 103 +- .pre-commit-config.yaml | 51 + .vscode/settings.json | 19 +- skyward-boardcore | 2 +- .../DeathStack/AirBrakes/AirBrakesAlgorithm.h | 66 +- .../AirBrakes/AirBrakesController.h | 80 +- .../DeathStack/AirBrakes/AirBrakesServo.cpp | 20 +- .../DeathStack/AirBrakes/AirBrakesServo.h | 15 +- .../trajectories/TrajectoriesEuroc.h | 8 +- .../trajectories/TrajectoriesRoccaraso.h | 7 +- .../ApogeeDetectionAlgorithm/ADAAlgorithm.cpp | 8 +- .../ApogeeDetectionAlgorithm/ADAAlgorithm.h | 10 +- .../ADACalibrator.cpp | 4 +- .../ApogeeDetectionAlgorithm/ADACalibrator.h | 7 +- .../ApogeeDetectionAlgorithm/ADAController.h | 146 +- .../ApogeeDetectionAlgorithm/ADAData.h | 10 +- src/boards/DeathStack/DeathStack.h | 41 +- .../Deployment/DeploymentController.cpp | 8 +- .../Deployment/DeploymentController.h | 17 +- .../DeathStack/Deployment/DeploymentServo.h | 18 +- .../FlightModeManager/FMMController.cpp | 58 +- .../FlightModeManager/FMMController.h | 37 +- .../FlightStatsRecorder/FSRController.cpp | 4 +- .../FlightStatsRecorder/FSRController.h | 31 +- .../DeathStack/LoggerService/LoggerService.h | 17 +- src/boards/DeathStack/Main/Bus.h | 8 +- src/boards/DeathStack/Main/Radio.h | 12 +- src/boards/DeathStack/Main/Sensors.cpp | 6 +- src/boards/DeathStack/Main/Sensors.h | 37 +- src/boards/DeathStack/Main/SensorsData.h | 9 +- src/boards/DeathStack/Main/StateMachines.cpp | 10 +- src/boards/DeathStack/Main/StateMachines.h | 20 +- .../ExtendedKalmanConfig.h | 8 +- .../ExtendedKalmanEigen.cpp | 69 +- .../ExtendedKalmanEigen.h | 89 +- .../NavigationAttitudeSystem/InitStates.h | 91 +- .../DeathStack/NavigationAttitudeSystem/NAS.h | 123 +- .../NavigationAttitudeSystem/NASCalibrator.h | 21 +- .../NavigationAttitudeSystem/NASController.h | 99 +- .../NavigationAttitudeSystem/NASData.h | 13 +- src/boards/DeathStack/PinHandler/PinHandler.h | 9 +- src/boards/DeathStack/System/StackLogger.h | 4 +- src/boards/DeathStack/System/TaskID.h | 7 +- .../TelemetriesTelecommands/Mavlink.h | 6 +- .../TelemetriesTelecommands/TCHandler.h | 6 +- .../TMTCController.cpp | 46 +- .../TelemetriesTelecommands/TMTCController.h | 15 +- .../TelemetriesTelecommands/TmRepository.cpp | 13 +- .../TelemetriesTelecommands/TmRepository.h | 56 +- .../DeathStack/configs/AirBrakesConfig.h | 12 +- .../DeathStack/configs/DeploymentConfig.h | 6 +- .../DeathStack/configs/FlightStatsConfig.h | 4 +- src/boards/DeathStack/configs/MavlinkConfig.h | 2 +- src/boards/DeathStack/configs/NASConfig.h | 10 +- .../DeathStack/configs/PinObserverConfig.h | 23 +- src/boards/DeathStack/configs/SensorsConfig.h | 75 +- src/boards/DeathStack/configs/config.h | 9 +- src/boards/Payload/Main/Bus.h | 8 +- src/boards/Payload/Main/Radio.h | 18 +- src/boards/Payload/Main/Sensors.cpp | 17 +- src/boards/Payload/Main/Sensors.h | 37 +- src/boards/Payload/Main/SensorsData.h | 9 +- src/boards/Payload/PayloadBoard.h | 39 +- src/boards/Payload/PayloadStatus.h | 4 +- src/boards/Payload/PinHandler/PinHandler.h | 15 +- .../Payload/PinHandler/PinHandlerData.h | 2 +- src/boards/Payload/WingControl/WingServo.cpp | 9 +- src/boards/Payload/WingControl/WingServo.h | 8 +- src/boards/Payload/configs/PinHandlerConfig.h | 8 +- src/boards/Payload/configs/SensorsConfig.h | 75 +- src/boards/Payload/configs/WingConfig.h | 14 +- src/common/CanInterfaces.h | 4 +- src/common/events/Events.h | 7 +- .../death-stack-x-decoder/LogTypes.h | 4 +- src/entrypoints/death-stack-x-entry.cpp | 5 +- src/entrypoints/deserializer/logdecoder.cpp | 2 +- .../hardware_in_the_loop/hil-entry.cpp | 6 +- src/entrypoints/lynx-mock-telemetry.cpp | 4 +- src/entrypoints/payload-entry.cpp | 3 +- .../windtunnel-test-decoder/LogTypes.h | 2 - src/hardware_in_the_loop/HIL.h | 13 +- .../HILFlightPhasesManager.h | 15 +- .../HILMockAerobrakeAlgorithm.h | 5 +- .../HIL_algorithms/HILMockNAS.h | 4 +- src/hardware_in_the_loop/HIL_sensors/HILGps.h | 7 +- src/hardware_in_the_loop/HIL_sensors/HILImu.h | 5 +- .../HIL_sensors/HILMagnetometer.h | 9 +- .../HIL_sensors/HILSensor.h | 4 +- .../HIL_sensors/HILSensorsData.h | 28 +- .../simulator_communication/HILTransceiver.h | 6 +- .../simulator_communication/SerialInterface.h | 6 +- src/mocksensors/MockGPS.h | 7 +- src/mocksensors/MockIMU.h | 4 +- src/mocksensors/MockPressureSensor.h | 5 +- src/mocksensors/MockSensorsData.h | 12 +- src/mocksensors/MockSpeedSensor.h | 5 +- .../lynx_flight_data/lynx_airspeed_data.cpp | 1797 ++++++++--------- .../lynx_flight_data/lynx_airspeed_data.h | 4 +- .../lynx_flight_data/lynx_gps_data.cpp | 2 +- .../lynx_flight_data/lynx_gps_data.h | 4 +- .../lynx_flight_data/lynx_imu_data.cpp | 4 +- .../lynx_flight_data/lynx_imu_data.h | 4 +- .../lynx_flight_data/lynx_press_data.cpp | 4 +- .../lynx_flight_data/lynx_press_data.h | 4 +- .../lynx_pressure_static_data.cpp | 4 +- .../lynx_pressure_static_data.h | 4 +- .../airbrakes/test-airbrakes-algorithm.cpp | 6 +- .../airbrakes/test-airbrakes-interactive.cpp | 12 +- .../ada/ada_kalman_p/test-ada-simulation.cpp | 18 +- src/tests/catch/catch-tests-entry.cpp | 6 +- src/tests/catch/fsm/test-ada.cpp | 32 +- src/tests/catch/fsm/test-airbrakes.cpp | 7 +- src/tests/catch/fsm/test-deployment.cpp | 19 +- .../catch/fsm/test-flightstatsrecorder.cpp | 23 +- src/tests/catch/fsm/test-fmm.cpp | 128 +- src/tests/catch/fsm/test-ignition.cpp | 16 +- src/tests/catch/fsm/test-nas.cpp | 18 +- src/tests/catch/fsm/test-tmtc.cpp | 25 +- src/tests/catch/nas/test-nas-simulation.cpp | 1 + .../deathstack-boards/test-analog-board.cpp | 2 + .../deathstack-boards/test-power-board.cpp | 2 + src/tests/drivers/test-cutter.cpp | 15 +- src/tests/drivers/test-mavlink.cpp | 2 +- src/tests/drivers/test-servo.cpp | 4 +- src/tests/eigen-test.cpp | 76 +- .../test-HIL+ADA+AerobrakeController+nas.cpp | 7 +- src/tests/ram_test/ramtest.cpp | 39 +- src/tests/ram_test/sha1.h | 4 +- src/tests/test-ada-dpl-simulation.cpp | 4 +- .../test-airbrakes-algorithm.cpp | 9 +- .../test-airbrakescontroller.cpp | 8 +- src/tests/test-fmm-interactive.cpp | 8 +- src/tests/test-pinhandler.cpp | 3 +- src/tests/test-tmtc.cpp | 5 +- 134 files changed, 2361 insertions(+), 2109 deletions(-) create mode 100644 .pre-commit-config.yaml diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index cc03a6a37..1f0603576 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -1,24 +1,93 @@ -# This file is a template, and might need editing before it works on your project. -# see https://docs.gitlab.com/ce/ci/yaml/README.html for all available options - -# you can delete this line if you're not using Docker -#image: busybox:latest +# Copyright (c) 2021 Skyward Experimental Rocketry +# Authors: Luca Erbetta, Luca Conterio, Alberto Nidasio, Damiano Amatruda +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. variables: GIT_SUBMODULE_STRATEGY: recursive - -build1: - stage: build +stages: + - build-release + - build-debug + - test + - lint + - documentation + +# Stage build-release + +build-release: + stage: build-release + tags: + - miosix + script: + - ./sbs --jobs 2 + +# Stage build-debug + +build-debug: + stage: build-debug + tags: + - miosix + script: + - ./sbs --jobs 2 --debug + +# Stage test + +test: + stage: test + script: + - ./sbs --jobs 2 --test catch-tests-entry + +# Stage lint + +cppcheck: + stage: lint + script: + - ./skyward-boardcore/scripts/linter.py --cppcheck src + +format: + stage: lint + script: + - ./skyward-boardcore/scripts/linter.py --format src + +copyright: + stage: lint + script: + - ./skyward-boardcore/scripts/linter.py --copyright src + +find: + stage: lint + script: + - ./skyward-boardcore/scripts/linter.py --find src + +# Stage documentation + +documentation: + stage: documentation only: - master - - testing + tags: + - copyright script: - - echo "Building r2a-hermes-obsw..." - - ./skyward-boardcore/sbs - -test1: - stage: test - script: - - echo "Running Linter" - - skyward-boardcore/scripts/linter.sh + - echo "Generate documentation to https://documentation.skywarder.eu/${CI_PROJECT_NAME}/${CI_COMMIT_REF_NAME}" + - rm -rf doc/output + - doxygen doc/Doxyfile + - rm -rf /srv/code_documentation/${CI_PROJECT_NAME}/${CI_COMMIT_REF_NAME} + - mkdir -p /srv/code_documentation/${CI_PROJECT_NAME}/ + - mv doc/output/html /srv/code_documentation/${CI_PROJECT_NAME}/${CI_COMMIT_REF_NAME} diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 000000000..391a4509b --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,51 @@ +repos: + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v2.3.0 + hooks: + - id: check-yaml + - id: end-of-file-fixer + - id: trailing-whitespace + - repo: https://gitlab.com/daverona/pre-commit/cpp + rev: 0.8.0 + hooks: + - id: cppcheck + args: [ + --quiet, + --language=c++, + --enable=all, + --inline-suppr, + --suppress=unmatchedSuppression, + --suppress=unusedFunction, + --suppress=missingInclude + ] + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v13.0.0 + hooks: + - id: clang-format + args: [ + -style=file, + --dry-run, + --Werror + ] + - repo: local + hooks: + - id: copyright + name: Copyright + entry: skyward-boardcore/scripts/linter.py + args: [ + --copyright, + src + ] + pass_filenames: false + language: python + - repo: local + hooks: + - id: find + name: Find + entry: skyward-boardcore/scripts/linter.py + args: [ + --find, + src + ] + pass_filenames: false + language: python diff --git a/.vscode/settings.json b/.vscode/settings.json index 08432c713..9855cc51f 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -74,6 +74,23 @@ "dense": "cpp", "unordered_set": "cpp", "hash_map": "cpp", - "valarray": "cpp" + "valarray": "cpp", + "core": "cpp", + "superlusupport": "cpp", + "*.evaluator": "cpp", + "*.traits": "cpp", + "adolcforward": "cpp", + "alignedvector3": "cpp", + "autodiff": "cpp", + "bvh": "cpp", + "eulerangles": "cpp", + "fft": "cpp", + "kroneckerproduct": "cpp", + "mprealsupport": "cpp", + "numericaldiff": "cpp", + "openglsupport": "cpp", + "specialfunctions": "cpp", + "splines": "cpp", + "matrixfunctions": "cpp" } } diff --git a/skyward-boardcore b/skyward-boardcore index 5fc8d0141..715cf492b 160000 --- a/skyward-boardcore +++ b/skyward-boardcore @@ -1 +1 @@ -Subproject commit 5fc8d0141d1881cf7dd690a5308c336166c3118c +Subproject commit 715cf492bb0239f53444ca6475dccbb9f4ac8ec7 diff --git a/src/boards/DeathStack/AirBrakes/AirBrakesAlgorithm.h b/src/boards/DeathStack/AirBrakes/AirBrakesAlgorithm.h index 8f85fdfaa..87ce3e208 100644 --- a/src/boards/DeathStack/AirBrakes/AirBrakesAlgorithm.h +++ b/src/boards/DeathStack/AirBrakes/AirBrakesAlgorithm.h @@ -54,8 +54,6 @@ * Total computation time: | 25.557320 ms | 181.001143 ms */ -using namespace DeathStackBoard::AirBrakesConfigs; - namespace DeathStackBoard { @@ -64,7 +62,8 @@ class AirBrakesControlAlgorithm : public Algorithm { public: - AirBrakesControlAlgorithm(Sensor<T>& sensor, ServoInterface* actuator); + AirBrakesControlAlgorithm(Boardcore::Sensor<T>& sensor, + ServoInterface* actuator); float getEstimatedCd() { return ab_data.estimated_cd; } @@ -152,8 +151,8 @@ public: /** * @brief Compute the necessary airbrakes surface to match the * given the drag force from the Pid. The possible surface values are - * discretized in (S_MIN, S_MAX) with a - * step of S_STEP. For every possible deltaS the + * discretized in (AirBrakesConfigs::S_MIN, AirBrakesConfigs::S_MAX) with a + * step of AirBrakesConfigs::S_STEP. For every possible deltaS the * correspondig drag force is computed with @see getDrag method and the one * that gives lowest error with respect to Pid output is returned. * @@ -220,7 +219,7 @@ private: Trajectory chosenTrajectory; ServoInterface* actuator; - Sensor<T>& sensor; + Boardcore::Sensor<T>& sensor; PIController pid; AirBrakesAlgorithmData algo_data; @@ -237,8 +236,9 @@ private: template <class T> AirBrakesControlAlgorithm<T>::AirBrakesControlAlgorithm( - Sensor<T>& sensor, ServoInterface* actuator) - : actuator(actuator), sensor(sensor), pid(Kp, Ki), + Boardcore::Sensor<T>& sensor, ServoInterface* actuator) + : actuator(actuator), sensor(sensor), + pid(AirBrakesConfigs::Kp, AirBrakesConfigs::Ki), logger(LoggerService::getInstance()) { } @@ -253,7 +253,7 @@ void AirBrakesControlAlgorithm<T>::begin() running = true; - begin_ts = TimestampTimer::getInstance().getTimestamp(); + begin_ts = Boardcore::TimestampTimer::getInstance().getTimestamp(); last_input_ts = (sensor.getLastSample()).timestamp; @@ -275,14 +275,15 @@ void AirBrakesControlAlgorithm<T>::step() alpha = computeAlpha(input, false); } - uint64_t curr_ts = TimestampTimer::getInstance().getTimestamp(); + uint64_t curr_ts = Boardcore::TimestampTimer::getInstance().getTimestamp(); #ifdef EUROC - if (curr_ts - begin_ts < AIRBRAKES_ACTIVATION_AFTER_SHADOW_MODE * 1000) + if (curr_ts - begin_ts < + AirBrakesConfigs::AIRBRAKES_ACTIVATION_AFTER_SHADOW_MODE * 1000) { // limit control to half of the airbrakes surface // this should correspond to a maximum of 17.18° angle on the servo - actuator->set(AB_SERVO_HALF_AREA_POS, true); + actuator->set(AirBrakesConfigs::AB_SERVO_HALF_AREA_POS, true); } else { @@ -352,7 +353,7 @@ TrajectoryPoint AirBrakesControlAlgorithm<T>::chooseTrajectory(float z, for (uint8_t trajectoryIndex = 0; trajectoryIndex < TOT_TRAJECTORIES; trajectoryIndex++) { - Trajectory trajectory(trajectoryIndex, S_MAX); + Trajectory trajectory(trajectoryIndex, AirBrakesConfigs::S_MAX); for (uint32_t pointIndex = 0; pointIndex < trajectory.length(); pointIndex++) @@ -373,7 +374,8 @@ TrajectoryPoint AirBrakesControlAlgorithm<T>::chooseTrajectory(float z, logger.log( AirBrakesChosenTrajectory{chosenTrajectory.getTrajectoryIndex()}); - PrintLogger log = Logging::getLogger("deathstack.fsm.abk"); + Boardcore::PrintLogger log = + Boardcore::Logging::getLogger("deathstack.fsm.abk"); LOG_INFO(log, "Chosen trajectory : {:d} \n", chosenTrajectory.getTrajectoryIndex()); @@ -388,8 +390,9 @@ TrajectoryPoint AirBrakesControlAlgorithm<T>::getSetpoint(float z, float vz) TrajectoryPoint currentPoint(z, vz); float minDistance = INFINITY; - uint32_t start = std::max(indexMinVal + START_INDEX_OFFSET, 0); - uint32_t end = chosenTrajectory.length(); + uint32_t start = + std::max(indexMinVal + AirBrakesConfigs::START_INDEX_OFFSET, 0); + uint32_t end = chosenTrajectory.length(); for (uint32_t pointIndex = start; pointIndex < end; pointIndex++) { @@ -419,14 +422,14 @@ float AirBrakesControlAlgorithm<T>::pidStep(float z, float vz, float vMod, // cd minimum if abk surface is 0 float cd_min = getDrag(vMod, z, 0); // cd maximum if abk surface is maximum - float cd_max = getDrag(vMod, z, S_MAX); + float cd_max = getDrag(vMod, z, AirBrakesConfigs::S_MAX); - float u_min = 0.5 * rho * cd_min * S0 * vz * vMod; - float u_max = 0.5 * rho * cd_max * S0 * vz * vMod; + float u_min = 0.5 * rho * cd_min * AirBrakesConfigs::S0 * vz * vMod; + float u_max = 0.5 * rho * cd_max * AirBrakesConfigs::S0 * vz * vMod; // get reference CD and control action, according to the chosen trajectory float cd_ref = getDrag(vMod, z, chosenTrajectory.getRefSurface()); - float u_ref = 0.5 * rho * cd_ref * S0 * vz * vMod; + float u_ref = 0.5 * rho * cd_ref * AirBrakesConfigs::S0 * vz * vMod; float error = vz - setpoint.getVz(); ab_data.pid_error = error; // for logging @@ -447,10 +450,12 @@ float AirBrakesControlAlgorithm<T>::getSurface(float z, float vz, float vMod, float selected_s = 0; float best_du = INFINITY; - for (float s = S_MIN; s < S_MAX + S_STEP; s += S_STEP) + for (float s = AirBrakesConfigs::S_MIN; + s < AirBrakesConfigs::S_MAX + AirBrakesConfigs::S_STEP; + s += AirBrakesConfigs::S_STEP) { float cd = getDrag(vMod, z, s); - float du = abs(u - (0.5 * rho * S0 * cd * vz * vMod)); + float du = abs(u - (0.5 * rho * AirBrakesConfigs::S0 * cd * vz * vMod)); if (du < best_du) { @@ -468,10 +473,12 @@ float AirBrakesControlAlgorithm<T>::getSurface(float z, float vz, float vMod, template <class T> float AirBrakesControlAlgorithm<T>::getAlpha(float s) { - float alpha_rad = (-B_DELTAS + sqrt(powf(B_DELTAS, 2) + 4 * A_DELTAS * s)) / - (2 * A_DELTAS); + float alpha_rad = (-AirBrakesConfigs::B_DELTAS + + sqrt(powf(AirBrakesConfigs::B_DELTAS, 2) + + 4 * AirBrakesConfigs::A_DELTAS * s)) / + (2 * AirBrakesConfigs::A_DELTAS); - float alpha_deg = alpha_rad * 180.0f / PI; + float alpha_deg = alpha_rad * 180.0f / Boardcore::PI; return alpha_deg; } @@ -479,20 +486,22 @@ float AirBrakesControlAlgorithm<T>::getAlpha(float s) template <class T> float AirBrakesControlAlgorithm<T>::getRho(float h) { - return RHO * expf(-h / Hn); + return AirBrakesConfigs::RHO * expf(-h / AirBrakesConfigs::Hn); } template <class T> float AirBrakesControlAlgorithm<T>::getMach(float vMod, float z) { - float c = Co + ALPHA * z; + float c = AirBrakesConfigs::Co + AirBrakesConfigs::ALPHA * z; return vMod / c; } template <class T> float AirBrakesControlAlgorithm<T>::getExtension(float s) { - return (-B + sqrtf(powf(B, 2) + 4 * A * s)) / (2 * A); + return (-AirBrakesConfigs::B + + sqrtf(powf(AirBrakesConfigs::B, 2) + 4 * AirBrakesConfigs::A * s)) / + (2 * AirBrakesConfigs::A); } template <class T> @@ -510,6 +519,7 @@ float AirBrakesControlAlgorithm<T>::getDrag(float v, float h, float s) powf(mach, 5), powf(mach, 6)}; + using AirBrakesConfigs::coeffs; return coeffs.n000 + coeffs.n100 * pow_mach[1] + coeffs.n200 * pow_mach[2] + coeffs.n300 * pow_mach[3] + coeffs.n400 * pow_mach[4] + coeffs.n500 * pow_mach[5] + coeffs.n600 * pow_mach[6] + diff --git a/src/boards/DeathStack/AirBrakes/AirBrakesController.h b/src/boards/DeathStack/AirBrakes/AirBrakesController.h index 3ddb7aa73..9e359e9cc 100644 --- a/src/boards/DeathStack/AirBrakes/AirBrakesController.h +++ b/src/boards/DeathStack/AirBrakes/AirBrakesController.h @@ -41,21 +41,21 @@ namespace DeathStackBoard * @brief AirBrakes state machine */ template <class T> -class AirBrakesController : public FSM<AirBrakesController<T>> +class AirBrakesController : public Boardcore::FSM<AirBrakesController<T>> { public: - AirBrakesController(Sensor<T>& sensor, + AirBrakesController(Boardcore::Sensor<T>& sensor, ServoInterface* servo = new AirBrakesServo()); ~AirBrakesController(); // Airbrakes FSM states - void state_initialization(const Event& ev); - void state_idle(const Event& ev); - void state_shadowMode(const Event& ev); - void state_enabled(const Event& ev); - void state_end(const Event& ev); - void state_disabled(const Event& ev); - void state_testAirbrakes(const Event& ev); + void state_initialization(const Boardcore::Event& ev); + void state_idle(const Boardcore::Event& ev); + void state_shadowMode(const Boardcore::Event& ev); + void state_enabled(const Boardcore::Event& ev); + void state_end(const Boardcore::Event& ev); + void state_disabled(const Boardcore::Event& ev); + void state_testAirbrakes(const Boardcore::Event& ev); /** * @brief Update the airbrakes control algorithm @@ -84,13 +84,14 @@ private: AirBrakesControlAlgorithm<T> algorithm; uint16_t ev_shadow_mode_timeout_id; - PrintLogger log = Logging::getLogger("deathstack.fsm.arb"); + Boardcore::PrintLogger log = + Boardcore::Logging::getLogger("deathstack.fsm.arb"); }; template <class T> -AirBrakesController<T>::AirBrakesController(Sensor<T>& sensor, +AirBrakesController<T>::AirBrakesController(Boardcore::Sensor<T>& sensor, ServoInterface* servo) - : FSM<AirBrakesController<T>>( + : Boardcore::FSM<AirBrakesController<T>>( &AirBrakesController<T>::state_initialization), servo(servo), algorithm(sensor, servo) { @@ -112,11 +113,11 @@ void AirBrakesController<T>::update() } template <class T> -void AirBrakesController<T>::state_initialization(const Event& ev) +void AirBrakesController<T>::state_initialization(const Boardcore::Event& ev) { switch (ev.code) { - case EV_ENTRY: + case Boardcore::EV_ENTRY: { logStatus(AirBrakesControllerState::INITIALIZATION); @@ -126,7 +127,7 @@ void AirBrakesController<T>::state_initialization(const Event& ev) this->transition(&AirBrakesController<T>::state_idle); break; } - case EV_EXIT: + case Boardcore::EV_EXIT: { break; } @@ -139,18 +140,18 @@ void AirBrakesController<T>::state_initialization(const Event& ev) } template <class T> -void AirBrakesController<T>::state_idle(const Event& ev) +void AirBrakesController<T>::state_idle(const Boardcore::Event& ev) { switch (ev.code) { - case EV_ENTRY: + case Boardcore::EV_ENTRY: { logStatus(AirBrakesControllerState::IDLE); LOG_DEBUG(log, "Eentering state idle"); break; } - case EV_EXIT: + case Boardcore::EV_EXIT: { LOG_DEBUG(log, "Exiting state idle"); break; @@ -183,22 +184,23 @@ void AirBrakesController<T>::state_idle(const Event& ev) } template <class T> -void AirBrakesController<T>::state_shadowMode(const Event& ev) +void AirBrakesController<T>::state_shadowMode(const Boardcore::Event& ev) { switch (ev.code) { - case EV_ENTRY: + case Boardcore::EV_ENTRY: { ev_shadow_mode_timeout_id = - sEventBroker.postDelayed<SHADOW_MODE_DURATION>( - Event{EV_SHADOW_MODE_TIMEOUT}, TOPIC_ABK); + sEventBroker + .postDelayed<AirBrakesConfigs::SHADOW_MODE_DURATION>( + Boardcore::Event{EV_SHADOW_MODE_TIMEOUT}, TOPIC_ABK); logStatus(AirBrakesControllerState::SHADOW_MODE); LOG_DEBUG(log, "Entering state shadow_mode"); break; } - case EV_EXIT: + case Boardcore::EV_EXIT: { LOG_DEBUG(log, "Exiting state shadow_mode"); break; @@ -221,11 +223,11 @@ void AirBrakesController<T>::state_shadowMode(const Event& ev) } template <class T> -void AirBrakesController<T>::state_enabled(const Event& ev) +void AirBrakesController<T>::state_enabled(const Boardcore::Event& ev) { switch (ev.code) { - case EV_ENTRY: + case Boardcore::EV_ENTRY: { algorithm.begin(); @@ -234,7 +236,7 @@ void AirBrakesController<T>::state_enabled(const Event& ev) LOG_DEBUG(log, "Entering state enabled"); break; } - case EV_EXIT: + case Boardcore::EV_EXIT: { LOG_DEBUG(log, "Exiting state enabled"); break; @@ -257,11 +259,11 @@ void AirBrakesController<T>::state_enabled(const Event& ev) } template <class T> -void AirBrakesController<T>::state_end(const Event& ev) +void AirBrakesController<T>::state_end(const Boardcore::Event& ev) { switch (ev.code) { - case EV_ENTRY: + case Boardcore::EV_ENTRY: { algorithm.end(); servo->reset(); @@ -271,7 +273,7 @@ void AirBrakesController<T>::state_end(const Event& ev) LOG_DEBUG(log, "Entering state end"); break; } - case EV_EXIT: + case Boardcore::EV_EXIT: { LOG_DEBUG(log, "Exiting state end"); break; @@ -285,11 +287,11 @@ void AirBrakesController<T>::state_end(const Event& ev) } template <class T> -void AirBrakesController<T>::state_disabled(const Event& ev) +void AirBrakesController<T>::state_disabled(const Boardcore::Event& ev) { switch (ev.code) { - case EV_ENTRY: + case Boardcore::EV_ENTRY: { algorithm.end(); servo->reset(); @@ -299,7 +301,7 @@ void AirBrakesController<T>::state_disabled(const Event& ev) LOG_DEBUG(log, "Entering state disabled"); break; } - case EV_EXIT: + case Boardcore::EV_EXIT: { LOG_DEBUG(log, "Exiting state disabled"); break; @@ -313,11 +315,11 @@ void AirBrakesController<T>::state_disabled(const Event& ev) } template <class T> -void AirBrakesController<T>::state_testAirbrakes(const Event& ev) +void AirBrakesController<T>::state_testAirbrakes(const Boardcore::Event& ev) { switch (ev.code) { - case EV_ENTRY: + case Boardcore::EV_ENTRY: { LOG_DEBUG(log, "Entering state test_airbrakes"); @@ -329,10 +331,10 @@ void AirBrakesController<T>::state_testAirbrakes(const Event& ev) logStatus(AirBrakesControllerState::TEST_AEROBRAKES); - sEventBroker.post(Event{EV_TEST_TIMEOUT}, TOPIC_ABK); + sEventBroker.post(Boardcore::Event{EV_TEST_TIMEOUT}, TOPIC_ABK); break; } - case EV_EXIT: + case Boardcore::EV_EXIT: { LOG_DEBUG(log, "Exiting state test_airbrakes"); break; @@ -392,12 +394,12 @@ void AirBrakesController<T>::incrementallyClose() template <class T> void AirBrakesController<T>::logStatus(AirBrakesControllerState state) { - status.timestamp = TimestampTimer::getInstance().getTimestamp(); + status.timestamp = Boardcore::TimestampTimer::getInstance().getTimestamp(); status.state = state; LoggerService::getInstance().log(status); - StackLogger::getInstance().updateStack(THID_ABK_FSM); + Boardcore::StackLogger::getInstance().updateStack(THID_ABK_FSM); } template <class T> @@ -406,4 +408,4 @@ void AirBrakesController<T>::setAirBrakesPosition(float pos) servo->set(pos); } -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/AirBrakes/AirBrakesServo.cpp b/src/boards/DeathStack/AirBrakes/AirBrakesServo.cpp index b6a2c35cc..2a6a6c071 100644 --- a/src/boards/DeathStack/AirBrakes/AirBrakesServo.cpp +++ b/src/boards/DeathStack/AirBrakes/AirBrakesServo.cpp @@ -13,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -22,22 +22,16 @@ #include <AirBrakes/AirBrakesServo.h> -namespace DeathStackBoard -{ +using namespace DeathStackBoard::AirBrakesConfigs; -AirBrakesServo::AirBrakesServo() - : ServoInterface(AB_SERVO_MIN_POS, AB_SERVO_MAX_POS) -{ -} - -AirBrakesServo::AirBrakesServo(float minPosition, float maxPosition) - : ServoInterface(minPosition, maxPosition) +namespace DeathStackBoard { -} AirBrakesServo::AirBrakesServo(float minPosition, float maxPosition, float resetPosition) - : ServoInterface(minPosition, maxPosition, resetPosition) + : ServoInterface(minPosition, maxPosition, resetPosition), + servo(AB_SERVO_TIMER, Boardcore::TimerUtils::Channel::CHANNEL_2, 500, + 2500, 50) { } @@ -97,4 +91,4 @@ float AirBrakesServo::preprocessPosition(float angle) return angle; } -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/AirBrakes/AirBrakesServo.h b/src/boards/DeathStack/AirBrakes/AirBrakesServo.h index 8551324c3..a33cc8022 100644 --- a/src/boards/DeathStack/AirBrakes/AirBrakesServo.h +++ b/src/boards/DeathStack/AirBrakes/AirBrakesServo.h @@ -36,16 +36,12 @@ namespace DeathStackBoard { -using namespace AirBrakesConfigs; - class AirBrakesServo : public ServoInterface { public: - AirBrakesServo(); - - AirBrakesServo(float minPosition, float maxPosition); - - AirBrakesServo(float minPosition, float maxPosition, float resetPosition); + AirBrakesServo(float minPosition = AirBrakesConfigs::AB_SERVO_MIN_POS, + float maxPosition = AirBrakesConfigs::AB_SERVO_MAX_POS, + float resetPosition = AirBrakesConfigs::AB_SERVO_MIN_POS); virtual ~AirBrakesServo(); @@ -59,8 +55,7 @@ public: void selfTest() override; private: - Servo servo{AirBrakesConfigs::AB_SERVO_TIMER, AB_SERVO_PWM_CH, 50, 500, - 2500}; + Boardcore::Servo servo; #ifdef HARDWARE_IN_THE_LOOP HIL *simulator = HIL::getInstance(); @@ -77,4 +72,4 @@ protected: float preprocessPosition(float angle) override; }; -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/AirBrakes/trajectories/TrajectoriesEuroc.h b/src/boards/DeathStack/AirBrakes/trajectories/TrajectoriesEuroc.h index e5f7decd4..237e6636b 100644 --- a/src/boards/DeathStack/AirBrakes/trajectories/TrajectoriesEuroc.h +++ b/src/boards/DeathStack/AirBrakes/trajectories/TrajectoriesEuroc.h @@ -1,6 +1,5 @@ -/* - * Copyright (c) 2021 Skyward Experimental Rocketry - * Authors: Vincenzo Santomarco +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Vincenzo Santomarco * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,13 +13,12 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ - #pragma once namespace DeathStackBoard diff --git a/src/boards/DeathStack/AirBrakes/trajectories/TrajectoriesRoccaraso.h b/src/boards/DeathStack/AirBrakes/trajectories/TrajectoriesRoccaraso.h index 519bf952a..84c372e32 100644 --- a/src/boards/DeathStack/AirBrakes/trajectories/TrajectoriesRoccaraso.h +++ b/src/boards/DeathStack/AirBrakes/trajectories/TrajectoriesRoccaraso.h @@ -1,6 +1,5 @@ -/* - * Copyright (c) 2021 Skyward Experimental Rocketry - * Authors: Vincenzo Santomarco +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Vincenzo Santomarco * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN diff --git a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.cpp b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.cpp index 126834ac6..d6f52bb09 100644 --- a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.cpp +++ b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.cpp @@ -28,6 +28,8 @@ #include <events/Events.h> #include <utils/aero/AeroUtils.h> +using namespace Boardcore; + namespace DeathStackBoard { @@ -55,8 +57,8 @@ void ADAAlgorithm::updateBaro(float pressure) ada_data.msl_altitude = pressureToAltitude(filter.getState()(0, 0)); ada_data.agl_altitude = altitudeMSLtoAGL(ada_data.msl_altitude); ada_data.vert_speed = aeroutils::verticalSpeed( - filter.getState()(0, 0), filter.getState()(1, 0), - ref_values.msl_pressure, ref_values.msl_temperature); + filter.getState()(0, 0), filter.getState()(1, 0), + ref_values.msl_pressure, ref_values.msl_temperature); #ifdef DEBUG if (counter == 50) @@ -136,4 +138,4 @@ ADAAlgorithm::getKalmanConfig(const float init_pressure) return config; } -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.h b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.h index 329129db0..3736c93ab 100644 --- a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.h +++ b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAAlgorithm.h @@ -95,14 +95,16 @@ private: /** * @brief Method to initialize the kalman configuration structure */ - const KalmanEigen<float, KALMAN_STATES_NUM, - KALMAN_OUTPUTS_NUM>::KalmanConfig + const Boardcore::KalmanEigen<float, ADAConfigs::KALMAN_STATES_NUM, + ADAConfigs::KALMAN_OUTPUTS_NUM>::KalmanConfig getKalmanConfig(const float ref_pressure); // References for pressure to altitude conversion ADAReferenceValues ref_values; - KalmanEigen<float, KALMAN_STATES_NUM, KALMAN_OUTPUTS_NUM> filter; + Boardcore::KalmanEigen<float, ADAConfigs::KALMAN_STATES_NUM, + ADAConfigs::KALMAN_OUTPUTS_NUM> + filter; ADAData ada_data; @@ -115,4 +117,4 @@ private: #endif }; -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.cpp b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.cpp index 4ea7e1031..2925542cd 100644 --- a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.cpp +++ b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.cpp @@ -23,6 +23,8 @@ #include <ApogeeDetectionAlgorithm/ADACalibrator.h> #include <utils/aero/AeroUtils.h> +using namespace Boardcore; + namespace DeathStackBoard { @@ -83,4 +85,4 @@ ADAReferenceValues ADACalibrator::getReferenceValues() return ref_values; } -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.h b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.h index a963aaeaa..5149a9290 100644 --- a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.h +++ b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADACalibrator.h @@ -69,13 +69,14 @@ private: /** * @brief Computes mean std dev etc for calibration of pressure conversion. */ - Stats pressure_stats; + Boardcore::Stats pressure_stats; // Refernece flags bool ref_alt_set = false; bool ref_temp_set = false; - PrintLogger log = Logging::getLogger("deathstack.fsm.ada"); + Boardcore::PrintLogger log = + Boardcore::Logging::getLogger("deathstack.fsm.ada"); }; -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAController.h b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAController.h index d1f7aa655..2c3b2fd14 100644 --- a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAController.h +++ b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAController.h @@ -39,27 +39,26 @@ using miosix::FastMutex; using miosix::Lock; -using namespace Boardcore; - namespace DeathStackBoard { -using namespace ADAConfigs; - template <typename Press, typename GPS> -class ADAController : public FSM<ADAController<Press, GPS>> +class ADAController : public Boardcore::FSM<ADAController<Press, GPS>> { using ADACtrl = ADAController<Press, GPS>; - using ADAFsm = FSM<ADAController<Press, GPS>>; + using ADAFsm = Boardcore::FSM<ADAController<Press, GPS>>; static_assert( - checkIfProduces<Sensor<Press>, PressureData>::value, + Boardcore::checkIfProduces<Boardcore::Sensor<Press>, + Boardcore::PressureData>::value, "Template argument must be a sensor that produces pressure data."); - static_assert(checkIfProduces<Sensor<GPS>, GPSData>::value, + static_assert(Boardcore::checkIfProduces<Boardcore::Sensor<GPS>, + Boardcore::GPSData>::value, "Template argument must be a sensor that produces GPS data."); public: - ADAController(Sensor<Press>& barometer, Sensor<GPS>& gps); + ADAController(Boardcore::Sensor<Press>& barometer, + Boardcore::Sensor<GPS>& gps); ~ADAController(); @@ -106,13 +105,13 @@ public: ADAReferenceValues getReferenceValues() { return ada.getReferenceValues(); } private: - void state_init(const Event& ev); + void state_init(const Boardcore::Event& ev); /** * @brief Idle state: the ADA waits for a command to start calibration. This * is the initial state. */ - void state_idle(const Event& ev); + void state_idle(const Boardcore::Event& ev); /** * @brief Calibrating state: the ADA calibrates the initial Kalman state. @@ -122,7 +121,7 @@ private: * triggered at the first sample update after having set the deployment * altitude and having reached the minimum number of calibration samples. */ - void state_calibrating(const Event& ev); + void state_calibrating(const Boardcore::Event& ev); /** * @brief Ready state: ADA is ready and waiting for liftoff @@ -131,7 +130,7 @@ private: * The exiting transition to the shadow mode state is triggered by the * liftoff event. */ - void state_ready(const Event& ev); + void state_ready(const Boardcore::Event& ev); /** * @brief Shadow mode state: ADA is running and logging apogees detected, @@ -141,7 +140,7 @@ private: * kalman filter followed by a check of vertical speed sign. The exiting * transition to the active state is triggered by a timeout event. */ - void state_shadowMode(const Event& ev); + void state_shadowMode(const Boardcore::Event& ev); /** * @brief Active state: ADA is running and it generates an event whe apogee @@ -152,7 +151,7 @@ private: * transition to the descent state is triggered by the apogee reached event * (NOT self generated!) */ - void state_active(const Event& ev); + void state_active(const Boardcore::Event& ev); /** * @brief Pressure stabilization state: ADA is running and does not trigger @@ -162,7 +161,7 @@ private: * kalman filter. The exiting transition to the drogue descent state is * triggered by a timeout event. */ - void state_pressureStabilization(const Event& ev); + void state_pressureStabilization(const Boardcore::Event& ev); /** * @brief First descent phase state: ADA is running and it generates an @@ -173,7 +172,7 @@ private: * to the stop state is triggered by the parachute deployment altitude * reached event (NOT self generated!) */ - void state_drogueDescent(const Event& ev); + void state_drogueDescent(const Boardcore::Event& ev); /** * @brief End state: ADA is stopped @@ -181,7 +180,7 @@ private: * In this state a call to update() will have no effect. * This is the final state */ - void state_end(const Event& ev); + void state_end(const Boardcore::Event& ev); void updateBaroAccordingToState(float pressure); @@ -205,8 +204,8 @@ private: ADAAlgorithm ada; - Sensor<Press>& barometer; - Sensor<GPS>& gps; + Boardcore::Sensor<Press>& barometer; + Boardcore::Sensor<GPS>& gps; uint64_t last_press_timestamp = 0; uint64_t last_gps_timestamp = 0; @@ -223,13 +222,15 @@ private: 0; // Number of consecutive samples for abk disable LoggerService& logger = LoggerService::getInstance(); // Logger - PrintLogger log = Logging::getLogger("deathstack.fms.ada"); + Boardcore::PrintLogger log = + Boardcore::Logging::getLogger("deathstack.fms.ada"); }; template <typename Press, typename GPS> -ADAController<Press, GPS>::ADAController(Sensor<Press>& barometer, - Sensor<GPS>& gps) - : ADAFsm(&ADACtrl::state_idle, STACK_MIN_FOR_SKYWARD, ADA_PRIORITY), +ADAController<Press, GPS>::ADAController(Boardcore::Sensor<Press>& barometer, + Boardcore::Sensor<GPS>& gps) + : ADAFsm(&ADACtrl::state_idle, Boardcore::STACK_MIN_FOR_SKYWARD, + ADAConfigs::ADA_PRIORITY), ada(ADAReferenceValues{}), barometer(barometer), gps(gps) { // Subscribe to topics @@ -304,7 +305,8 @@ void ADAController<Press, GPS>::updateBaroAccordingToState(float pressure) // Log the altitude & vertical speed but don't use kalman pressure // while we are on the ramp ADAData d; - d.timestamp = TimestampTimer::getInstance().getTimestamp(); + d.timestamp = + Boardcore::TimestampTimer::getInstance().getTimestamp(); d.msl_altitude = ada.pressureToAltitude(pressure); d.agl_altitude = ada.altitudeMSLtoAGL(d.msl_altitude); d.vert_speed = 0; @@ -319,11 +321,13 @@ void ADAController<Press, GPS>::updateBaroAccordingToState(float pressure) ada.updateBaro(pressure); // Check if the vertical speed smaller than the target apogee speed - if (ada.getVerticalSpeed() < APOGEE_VERTICAL_SPEED_TARGET) + if (ada.getVerticalSpeed() < + ADAConfigs::APOGEE_VERTICAL_SPEED_TARGET) { // Log ApogeeDetected apogee{ - TimestampTimer::getInstance().getTimestamp(), status.state}; + Boardcore::TimestampTimer::getInstance().getTimestamp(), + status.state}; logger.log(apogee); } @@ -337,9 +341,10 @@ void ADAController<Press, GPS>::updateBaroAccordingToState(float pressure) ada.updateBaro(pressure); // Check if we reached apogee - if (ada.getVerticalSpeed() < APOGEE_VERTICAL_SPEED_TARGET) + if (ada.getVerticalSpeed() < + ADAConfigs::APOGEE_VERTICAL_SPEED_TARGET) { - if (++n_samples_apogee_detected >= APOGEE_N_SAMPLES) + if (++n_samples_apogee_detected >= ADAConfigs::APOGEE_N_SAMPLES) { // Active state send notifications for apogee sEventBroker.post({EV_ADA_APOGEE_DETECTED}, TOPIC_ADA); @@ -348,7 +353,8 @@ void ADAController<Press, GPS>::updateBaroAccordingToState(float pressure) // Log ApogeeDetected apogee{ - TimestampTimer::getInstance().getTimestamp(), status.state}; + Boardcore::TimestampTimer::getInstance().getTimestamp(), + status.state}; logger.log(apogee); } else if (n_samples_apogee_detected != 0) @@ -357,9 +363,11 @@ void ADAController<Press, GPS>::updateBaroAccordingToState(float pressure) } // Check if we have to disable airbrakes - if (ada.getVerticalSpeed() < ABK_DISABLE_VERTICAL_SPEED_TARGET) + if (ada.getVerticalSpeed() < + ADAConfigs::ABK_DISABLE_VERTICAL_SPEED_TARGET) { - if (++n_samples_abk_disable_detected >= ABK_DISABLE_N_SAMPLES) + if (++n_samples_abk_disable_detected >= + ADAConfigs::ABK_DISABLE_N_SAMPLES) { // Active state send notifications for disabling airbrakes sEventBroker.post({EV_ADA_DISABLE_ABK}, @@ -384,10 +392,12 @@ void ADAController<Press, GPS>::updateBaroAccordingToState(float pressure) if (ada.getAltitudeForDeployment() <= deployment_altitude) { - if (++n_samples_deployment_detected >= DEPLOYMENT_N_SAMPLES) + if (++n_samples_deployment_detected >= + ADAConfigs::DEPLOYMENT_N_SAMPLES) { logger.log(DplAltitudeReached{ - TimestampTimer::getInstance().getTimestamp()}); + Boardcore::TimestampTimer::getInstance() + .getTimestamp()}); } } else if (n_samples_deployment_detected != 0) @@ -406,10 +416,12 @@ void ADAController<Press, GPS>::updateBaroAccordingToState(float pressure) if (ada.getAltitudeForDeployment() <= deployment_altitude) { - if (++n_samples_deployment_detected >= DEPLOYMENT_N_SAMPLES) + if (++n_samples_deployment_detected >= + ADAConfigs::DEPLOYMENT_N_SAMPLES) { logger.log(DplAltitudeReached{ - TimestampTimer::getInstance().getTimestamp()}); + Boardcore::TimestampTimer::getInstance() + .getTimestamp()}); sEventBroker.post({EV_ADA_DPL_ALT_DETECTED}, TOPIC_ADA); } @@ -512,17 +524,17 @@ void ADAController<Press, GPS>::finalizeCalibration() } template <typename Press, typename GPS> -void ADAController<Press, GPS>::state_idle(const Event& ev) +void ADAController<Press, GPS>::state_idle(const Boardcore::Event& ev) { switch (ev.code) { - case EV_ENTRY: + case Boardcore::EV_ENTRY: { LOG_DEBUG(log, "Entering state idle"); logStatus(ADAState::IDLE); break; } - case EV_EXIT: + case Boardcore::EV_EXIT: { LOG_DEBUG(log, "Exiting state idle"); break; @@ -540,11 +552,11 @@ void ADAController<Press, GPS>::state_idle(const Event& ev) } template <typename Press, typename GPS> -void ADAController<Press, GPS>::state_calibrating(const Event& ev) +void ADAController<Press, GPS>::state_calibrating(const Boardcore::Event& ev) { switch (ev.code) { - case EV_ENTRY: + case Boardcore::EV_ENTRY: { { Lock<FastMutex> l(calibrator_mutex); @@ -555,7 +567,7 @@ void ADAController<Press, GPS>::state_calibrating(const Event& ev) LOG_DEBUG(log, "Entering state calibrating"); break; } - case EV_EXIT: + case Boardcore::EV_EXIT: { LOG_DEBUG(log, "Exiting state calibrating"); break; @@ -578,17 +590,17 @@ void ADAController<Press, GPS>::state_calibrating(const Event& ev) } template <typename Press, typename GPS> -void ADAController<Press, GPS>::state_ready(const Event& ev) +void ADAController<Press, GPS>::state_ready(const Boardcore::Event& ev) { switch (ev.code) { - case EV_ENTRY: + case Boardcore::EV_ENTRY: { logStatus(ADAState::READY); LOG_DEBUG(log, "Entering state ready"); break; } - case EV_EXIT: + case Boardcore::EV_EXIT: { LOG_DEBUG(log, "Exiting state ready"); break; @@ -611,20 +623,20 @@ void ADAController<Press, GPS>::state_ready(const Event& ev) } template <typename Press, typename GPS> -void ADAController<Press, GPS>::state_shadowMode(const Event& ev) +void ADAController<Press, GPS>::state_shadowMode(const Boardcore::Event& ev) { switch (ev.code) { - case EV_ENTRY: + case Boardcore::EV_ENTRY: { shadow_delayed_event_id = - sEventBroker.postDelayed<TIMEOUT_ADA_SHADOW_MODE>( + sEventBroker.postDelayed<ADAConfigs::TIMEOUT_ADA_SHADOW_MODE>( {EV_SHADOW_MODE_TIMEOUT}, TOPIC_ADA); logStatus(ADAState::SHADOW_MODE); LOG_DEBUG(log, "Entering state shadowMode"); break; } - case EV_EXIT: + case Boardcore::EV_EXIT: { sEventBroker.removeDelayed(shadow_delayed_event_id); LOG_DEBUG(log, "Exiting state shadowMode"); @@ -643,17 +655,17 @@ void ADAController<Press, GPS>::state_shadowMode(const Event& ev) } template <typename Press, typename GPS> -void ADAController<Press, GPS>::state_active(const Event& ev) +void ADAController<Press, GPS>::state_active(const Boardcore::Event& ev) { switch (ev.code) { - case EV_ENTRY: + case Boardcore::EV_ENTRY: { logStatus(ADAState::ACTIVE); LOG_DEBUG(log, "Entering state active"); break; } - case EV_EXIT: + case Boardcore::EV_EXIT: { LOG_DEBUG(log, "Exiting state active"); break; @@ -671,20 +683,22 @@ void ADAController<Press, GPS>::state_active(const Event& ev) } template <typename Press, typename GPS> -void ADAController<Press, GPS>::state_pressureStabilization(const Event& ev) +void ADAController<Press, GPS>::state_pressureStabilization( + const Boardcore::Event& ev) { switch (ev.code) { - case EV_ENTRY: + case Boardcore::EV_ENTRY: { pressure_delayed_event_id = - sEventBroker.postDelayed<TIMEOUT_ADA_P_STABILIZATION>( - {EV_TIMEOUT_PRESS_STABILIZATION}, TOPIC_ADA); + sEventBroker + .postDelayed<ADAConfigs::TIMEOUT_ADA_P_STABILIZATION>( + {EV_TIMEOUT_PRESS_STABILIZATION}, TOPIC_ADA); logStatus(ADAState::PRESSURE_STABILIZATION); LOG_DEBUG(log, "Entering state pressureStabilization"); break; } - case EV_EXIT: + case Boardcore::EV_EXIT: { sEventBroker.removeDelayed(pressure_delayed_event_id); LOG_DEBUG(log, "Exiting state pressureStabilization"); @@ -703,18 +717,18 @@ void ADAController<Press, GPS>::state_pressureStabilization(const Event& ev) } template <typename Press, typename GPS> -void ADAController<Press, GPS>::state_drogueDescent(const Event& ev) +void ADAController<Press, GPS>::state_drogueDescent(const Boardcore::Event& ev) { switch (ev.code) { - case EV_ENTRY: + case Boardcore::EV_ENTRY: { logStatus(ADAState::DROGUE_DESCENT); LOG_DEBUG(log, "Entering state drogueDescent"); n_samples_deployment_detected = 0; break; } - case EV_EXIT: + case Boardcore::EV_EXIT: { LOG_DEBUG(log, "Exiting state drogueDescent"); break; @@ -735,17 +749,17 @@ void ADAController<Press, GPS>::state_drogueDescent(const Event& ev) } template <typename Press, typename GPS> -void ADAController<Press, GPS>::state_end(const Event& ev) +void ADAController<Press, GPS>::state_end(const Boardcore::Event& ev) { switch (ev.code) { - case EV_ENTRY: + case Boardcore::EV_ENTRY: { LOG_DEBUG(log, "Entering state end"); logStatus(ADAState::END); break; } - case EV_EXIT: + case Boardcore::EV_EXIT: { LOG_DEBUG(log, "Exiting state end"); break; @@ -767,10 +781,10 @@ void ADAController<Press, GPS>::logStatus(ADAState state) template <typename Press, typename GPS> void ADAController<Press, GPS>::logStatus() { - status.timestamp = TimestampTimer::getInstance().getTimestamp(); + status.timestamp = Boardcore::TimestampTimer::getInstance().getTimestamp(); logger.log(status); - StackLogger::getInstance().updateStack(THID_ADA_FSM); + Boardcore::StackLogger::getInstance().updateStack(THID_ADA_FSM); } template <typename Press, typename GPS> @@ -781,4 +795,4 @@ void ADAController<Press, GPS>::logData(const ADAKalmanState& s, logger.log(d); } -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAData.h b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAData.h index 7889a867c..8da1446e9 100644 --- a/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAData.h +++ b/src/boards/DeathStack/ApogeeDetectionAlgorithm/ADAData.h @@ -29,13 +29,9 @@ #include <ostream> -using namespace Boardcore; - namespace DeathStackBoard { -using namespace ADAConfigs; - /** * @brief All possible states of the ADA. */ @@ -159,8 +155,8 @@ struct ADAReferenceValues // Pressure at mean sea level for altitude calculation, to be updated with // launch-day calibration - float msl_pressure = MSL_PRESSURE; - float msl_temperature = MSL_TEMPERATURE; + float msl_pressure = Boardcore::MSL_PRESSURE; + float msl_temperature = Boardcore::MSL_TEMPERATURE; static std::string header() { @@ -201,4 +197,4 @@ struct TargetDeploymentAltitude void print(std::ostream& os) const { os << deployment_altitude << "\n"; } }; -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/DeathStack.h b/src/boards/DeathStack/DeathStack.h index 202703941..640ca446e 100644 --- a/src/boards/DeathStack/DeathStack.h +++ b/src/boards/DeathStack/DeathStack.h @@ -48,7 +48,6 @@ #endif using std::bind; -using namespace Boardcore; namespace DeathStackBoard { @@ -61,7 +60,7 @@ static const std::vector<uint8_t> TRACE_EVENT_BLACKLIST{ * This file provides a simplified way to initialize and monitor all * the components of the DeathStack. */ -class DeathStack : public Singleton<DeathStack> +class DeathStack : public Boardcore::Singleton<DeathStack> { friend class Singleton<DeathStack>; @@ -69,7 +68,7 @@ public: // Shared Components LoggerService* logger; - EventSniffer* sniffer; + Boardcore::EventSniffer* sniffer; StateMachines* state_machines; Bus* bus; @@ -79,7 +78,7 @@ public: PinHandler* pin_handler; - TaskScheduler* scheduler; + Boardcore::TaskScheduler* scheduler; #ifdef HARDWARE_IN_THE_LOOP HIL* hil; @@ -131,12 +130,14 @@ public: if (status.death_stack != COMP_OK) { LOG_ERR(log, "Initalization failed\n"); - sEventBroker.post(Event{EV_INIT_ERROR}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post(Boardcore::Event{EV_INIT_ERROR}, + TOPIC_FLIGHT_EVENTS); } else { LOG_INFO(log, "Initalization ok"); - sEventBroker.post(Event{EV_INIT_OK}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post(Boardcore::Event{EV_INIT_OK}, + TOPIC_FLIGHT_EVENTS); } } @@ -170,16 +171,16 @@ private: // event { using namespace std::placeholders; - sniffer = - new EventSniffer(sEventBroker, TOPIC_LIST, - bind(&DeathStack::logEvent, this, _1, _2)); + sniffer = new Boardcore::EventSniffer( + sEventBroker, TOPIC_LIST, + bind(&DeathStack::logEvent, this, _1, _2)); } #ifdef HARDWARE_IN_THE_LOOP hil = HIL::getInstance(); #endif - scheduler = new TaskScheduler(); + scheduler = new Boardcore::TaskScheduler(); addSchedulerStatsTask(); bus = new Bus(); @@ -204,7 +205,7 @@ private: pin_handler = new PinHandler(); #ifdef DEBUG - injector = new EventInjector(); + injector = new Boardcore::EventInjector(); #endif LOG_INFO(log, "Init finished"); @@ -215,8 +216,9 @@ private: */ void logEvent(uint8_t event, uint8_t topic) { - EventData ev{(long long)TimestampTimer::getInstance().getTimestamp(), - event, topic}; + Boardcore::EventData ev{ + (long long)Boardcore::TimestampTimer::getInstance().getTimestamp(), + event, topic}; logger->log(ev); #ifdef DEBUG @@ -234,7 +236,7 @@ private: #endif } - inline void postEvent(Event ev, uint8_t topic) + inline void postEvent(Boardcore::Event ev, uint8_t topic) { sEventBroker.post(ev, topic); } @@ -245,21 +247,22 @@ private: scheduler->addTask( [&]() { - std::vector<TaskStatResult> scheduler_stats = + std::vector<Boardcore::TaskStatsResult> scheduler_stats = scheduler->getTaskStats(); - for (TaskStatResult stat : scheduler_stats) + for (Boardcore::TaskStatsResult stat : scheduler_stats) logger->log(stat); - StackLogger::getInstance().updateStack(THID_TASK_SCHEDULER); + Boardcore::StackLogger::getInstance().updateStack( + THID_TASK_SCHEDULER); }, 1000, TASK_SCHEDULER_STATS_ID); } - EventInjector* injector; + Boardcore::EventInjector* injector; DeathStackStatus status{}; - PrintLogger log = Logging::getLogger("deathstack"); + Boardcore::PrintLogger log = Boardcore::Logging::getLogger("deathstack"); }; } // namespace DeathStackBoard diff --git a/src/boards/DeathStack/Deployment/DeploymentController.cpp b/src/boards/DeathStack/Deployment/DeploymentController.cpp index b3cfc11f2..e403dc45e 100644 --- a/src/boards/DeathStack/Deployment/DeploymentController.cpp +++ b/src/boards/DeathStack/Deployment/DeploymentController.cpp @@ -29,6 +29,8 @@ #include <events/Events.h> using namespace Boardcore; +using namespace DeathStackBoard::DeploymentConfigs; +using namespace DeathStackBoard::CutterConfig; namespace DeathStackBoard { @@ -150,7 +152,7 @@ void DeploymentController::state_noseconeEjection(const Event& ev) ejectNosecone(); ev_nc_open_timeout_id = sEventBroker.postDelayed<NC_OPEN_TIMEOUT>( - Event{EV_NC_OPEN_TIMEOUT}, TOPIC_DPL); + Boardcore::Event{EV_NC_OPEN_TIMEOUT}, TOPIC_DPL); logStatus(DeploymentControllerState::NOSECONE_EJECTION); break; @@ -191,7 +193,7 @@ void DeploymentController::state_cutting(const Event& ev) startCutting(); ev_nc_cutting_timeout_id = sEventBroker.postDelayed<CUT_DURATION>( - Event{EV_CUTTING_TIMEOUT}, TOPIC_DPL); + Boardcore::Event{EV_CUTTING_TIMEOUT}, TOPIC_DPL); logStatus(DeploymentControllerState::CUTTING); @@ -237,4 +239,4 @@ void DeploymentController::stopCutting() CuttersInput::getPin().low(); } -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/Deployment/DeploymentController.h b/src/boards/DeathStack/Deployment/DeploymentController.h index c1f3363ae..7fade46a3 100644 --- a/src/boards/DeathStack/Deployment/DeploymentController.h +++ b/src/boards/DeathStack/Deployment/DeploymentController.h @@ -31,27 +31,23 @@ #include <events/Events.h> #include <events/FSM.h> -using namespace Boardcore; -using namespace DeathStackBoard::DeploymentConfigs; -using namespace DeathStackBoard::CutterConfig; - namespace DeathStackBoard { /** * @brief Deployment state machine. */ -class DeploymentController : public FSM<DeploymentController> +class DeploymentController : public Boardcore::FSM<DeploymentController> { public: DeploymentController( ServoInterface* ejection_servo = new DeploymentServo()); ~DeploymentController(); - void state_initialization(const Event& ev); - void state_idle(const Event& ev); - void state_noseconeEjection(const Event& ev); - void state_cutting(const Event& ev); + void state_initialization(const Boardcore::Event& ev); + void state_idle(const Boardcore::Event& ev); + void state_noseconeEjection(const Boardcore::Event& ev); + void state_cutting(const Boardcore::Event& ev); void ejectNosecone(); void startCutting(); @@ -65,7 +61,8 @@ private: uint16_t ev_nc_open_timeout_id = 0; uint16_t ev_nc_cutting_timeout_id = 0; - PrintLogger log = Logging::getLogger("deathstack.fsm.dpl"); + Boardcore::PrintLogger log = + Boardcore::Logging::getLogger("deathstack.fsm.dpl"); void logStatus(DeploymentControllerState current_state); }; diff --git a/src/boards/DeathStack/Deployment/DeploymentServo.h b/src/boards/DeathStack/Deployment/DeploymentServo.h index 29632f1b8..d2008e4a9 100644 --- a/src/boards/DeathStack/Deployment/DeploymentServo.h +++ b/src/boards/DeathStack/Deployment/DeploymentServo.h @@ -27,21 +27,19 @@ #include <drivers/servo/Servo.h> #include <miosix.h> -using namespace Boardcore; - namespace DeathStackBoard { -using namespace DeathStackBoard::DeploymentConfigs; - class DeploymentServo : public ServoInterface { public: - Servo servo{DPL_SERVO_TIMER, DPL_SERVO_PWM_CH, 50, 500, 2500}; + Boardcore::Servo servo{DeploymentConfigs::DPL_SERVO_TIMER, + DeploymentConfigs::DPL_SERVO_PWM_CH, 50, 500, 2500}; DeploymentServo() - : ServoInterface(DPL_SERVO_MIN_POS, DPL_SERVO_MAX_POS, - DPL_SERVO_RESET_POS) + : ServoInterface(DeploymentConfigs::DPL_SERVO_MIN_POS, + DeploymentConfigs::DPL_SERVO_MAX_POS, + DeploymentConfigs::DPL_SERVO_RESET_POS) { } @@ -61,7 +59,7 @@ public: { for (int i = 0; i < 3; i++) { - set(RESET_POS - DPL_SERVO_WIGGLE_AMPLITUDE); + set(RESET_POS - DeploymentConfigs::DPL_SERVO_WIGGLE_AMPLITUDE); miosix::Thread::sleep(500); reset(); miosix::Thread::sleep(500); @@ -76,7 +74,7 @@ protected: } private: - float anglePrec = DPL_SERVO_RESET_POS; + float anglePrec = DeploymentConfigs::DPL_SERVO_RESET_POS; }; -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/FlightModeManager/FMMController.cpp b/src/boards/DeathStack/FlightModeManager/FMMController.cpp index 4b2dac76f..b7db74a16 100644 --- a/src/boards/DeathStack/FlightModeManager/FMMController.cpp +++ b/src/boards/DeathStack/FlightModeManager/FMMController.cpp @@ -28,6 +28,8 @@ #include <events/Topics.h> #include <miosix.h> +using namespace Boardcore; + namespace DeathStackBoard { @@ -74,13 +76,13 @@ State FMMController::state_onGround(const Event& ev) case EV_ENTRY: /* Executed everytime state is entered */ { logState(FMMState::ON_GROUND); - LOG_DEBUG(log, "Entering state_onGround"); + LOG_DEBUG(printLogger, "Entering state_onGround"); break; } case EV_INIT: /* This is a super-state, so move to the first sub-state */ { - LOG_DEBUG(log, "Init state_onGround"); + LOG_DEBUG(printLogger, "Init state_onGround"); retState = transition(&FMMController::state_init); @@ -88,7 +90,7 @@ State FMMController::state_onGround(const Event& ev) } case EV_EXIT: /* Executed everytime state is exited */ { - LOG_DEBUG(log, "Exiting state_onGround"); + LOG_DEBUG(printLogger, "Exiting state_onGround"); break; } @@ -121,7 +123,7 @@ State FMMController::state_init(const Event& ev) case EV_ENTRY: /* Executed everytime state is entered */ { logState(FMMState::INIT); - LOG_DEBUG(log, "Entering state_init"); + LOG_DEBUG(printLogger, "Entering state_init"); break; } case EV_INIT: /* No sub-states */ @@ -130,7 +132,7 @@ State FMMController::state_init(const Event& ev) } case EV_EXIT: /* Executed everytime state is exited */ { - LOG_DEBUG(log, "Exit state_init"); + LOG_DEBUG(printLogger, "Exit state_init"); break; } @@ -161,7 +163,7 @@ State FMMController::state_initError(const Event& ev) case EV_ENTRY: /* Executed everytime state is entered */ { logState(FMMState::INIT_ERROR); - LOG_DEBUG(log, "Entering state_initError"); + LOG_DEBUG(printLogger, "Entering state_initError"); break; } case EV_INIT: /* No sub-states */ @@ -170,7 +172,7 @@ State FMMController::state_initError(const Event& ev) } case EV_EXIT: /* Executed everytime state is exited */ { - LOG_DEBUG(log, "Exit state_initError"); + LOG_DEBUG(printLogger, "Exit state_initError"); break; } @@ -196,7 +198,7 @@ State FMMController::state_initDone(const Event& ev) case EV_ENTRY: /* Executed everytime state is entered */ { logState(FMMState::INIT_DONE); - LOG_DEBUG(log, "Entering state_initDone"); + LOG_DEBUG(printLogger, "Entering state_initDone"); break; } case EV_INIT: /* No sub-states */ @@ -205,7 +207,7 @@ State FMMController::state_initDone(const Event& ev) } case EV_EXIT: /* Executed everytime state is exited */ { - LOG_DEBUG(log, "Exit state_initDone"); + LOG_DEBUG(printLogger, "Exit state_initDone"); break; } @@ -237,7 +239,7 @@ State FMMController::state_sensorsCalibration(const Event& ev) { logState(FMMState::SENSORS_CALIBRATION); - LOG_DEBUG(log, "Entering sensors_calibration"); + LOG_DEBUG(printLogger, "Entering sensors_calibration"); DeathStack::getInstance().sensors->calibrate(); sEventBroker.post({EV_SENSORS_READY}, TOPIC_FLIGHT_EVENTS); @@ -250,7 +252,7 @@ State FMMController::state_sensorsCalibration(const Event& ev) } case EV_EXIT: /* Executed everytime state is exited */ { - LOG_DEBUG(log, "Exit sensors_calibration"); + LOG_DEBUG(printLogger, "Exit sensors_calibration"); break; } @@ -285,7 +287,7 @@ State FMMController::state_algosCalibration(const Event& ev) sEventBroker.post({EV_CALIBRATE_ADA}, TOPIC_ADA); sEventBroker.post({EV_CALIBRATE_NAS}, TOPIC_NAS); - LOG_DEBUG(log, "Entering algos_calibration"); + LOG_DEBUG(printLogger, "Entering algos_calibration"); break; } case EV_INIT: /* No sub-state */ @@ -294,7 +296,7 @@ State FMMController::state_algosCalibration(const Event& ev) } case EV_EXIT: /* Executed everytime state is exited */ { - LOG_DEBUG(log, "Exit algos_calibration"); + LOG_DEBUG(printLogger, "Exit algos_calibration"); break; } @@ -346,7 +348,7 @@ State FMMController::state_disarmed(const Event& ev) { sEventBroker.post({EV_DISARMED}, TOPIC_FLIGHT_EVENTS); logState(FMMState::DISARMED); - LOG_DEBUG(log, "Entering disarmed"); + LOG_DEBUG(printLogger, "Entering disarmed"); break; } @@ -356,7 +358,7 @@ State FMMController::state_disarmed(const Event& ev) } case EV_EXIT: /* Executed everytime state is exited */ { - LOG_DEBUG(log, "Exiting disarmed"); + LOG_DEBUG(printLogger, "Exiting disarmed"); break; } @@ -397,7 +399,7 @@ State FMMController::state_armed(const Event& ev) sEventBroker.post({EV_ARMED}, TOPIC_FLIGHT_EVENTS); logState(FMMState::ARMED); - LOG_DEBUG(log, "Entering armed"); + LOG_DEBUG(printLogger, "Entering armed"); break; } case EV_INIT: /* No sub-state */ @@ -406,7 +408,7 @@ State FMMController::state_armed(const Event& ev) } case EV_EXIT: /* Executed everytime state is exited */ { - LOG_DEBUG(log, "Exiting armed"); + LOG_DEBUG(printLogger, "Exiting armed"); break; } @@ -439,7 +441,7 @@ State FMMController::state_testMode(const Event& ev) { logState(FMMState::TESTING); - LOG_DEBUG(log, "Entering testing"); + LOG_DEBUG(printLogger, "Entering testing"); break; } @@ -449,7 +451,7 @@ State FMMController::state_testMode(const Event& ev) } case EV_EXIT: /* Executed everytime state is exited */ { - LOG_DEBUG(log, "Exiting testing"); + LOG_DEBUG(printLogger, "Exiting testing"); break; } @@ -526,19 +528,19 @@ State FMMController::state_flying(const Event& ev) DeathStack::getInstance().ensors->signalLiftoff(); #endif - LOG_DEBUG(log, "Entering flying"); + LOG_DEBUG(printLogger, "Entering flying"); break; } case EV_INIT: { - LOG_DEBUG(log, "Init flying"); + LOG_DEBUG(printLogger, "Init flying"); retState = transition(&FMMController::state_ascending); break; } case EV_EXIT: /* Executed everytime state is exited */ { - LOG_DEBUG(log, "Exiting flying"); + LOG_DEBUG(printLogger, "Exiting flying"); sEventBroker.removeDelayed(end_mission_d_event_id); break; @@ -580,7 +582,7 @@ State FMMController::state_ascending(const Event& ev) case EV_ENTRY: /* Executed everytime state is entered */ { logState(FMMState::ASCENDING); - LOG_DEBUG(log, "Entering ascending"); + LOG_DEBUG(printLogger, "Entering ascending"); break; } case EV_INIT: /* No sub-state */ @@ -589,7 +591,7 @@ State FMMController::state_ascending(const Event& ev) } case EV_EXIT: /* Executed everytime state is exited */ { - LOG_DEBUG(log, "Exit ascending"); + LOG_DEBUG(printLogger, "Exit ascending"); break; } @@ -630,7 +632,7 @@ State FMMController::state_drogueDescent(const Event& ev) sEventBroker.post(Event{EV_NC_OPEN}, TOPIC_DPL); logState(FMMState::DROGUE_DESCENT); - LOG_DEBUG(log, "Entering drogueDescent"); + LOG_DEBUG(printLogger, "Entering drogueDescent"); break; } case EV_INIT: /* No sub-state */ @@ -639,7 +641,7 @@ State FMMController::state_drogueDescent(const Event& ev) } case EV_EXIT: /* Executed everytime state is exited */ { - LOG_DEBUG(log, "Exiting drogueDescent"); + LOG_DEBUG(printLogger, "Exiting drogueDescent"); break; } @@ -671,7 +673,7 @@ State FMMController::state_terminalDescent(const Event& ev) logState(FMMState::TERMINAL_DESCENT); - LOG_DEBUG(log, "Entering terminalDescent"); + LOG_DEBUG(printLogger, "Entering terminalDescent"); break; } case EV_INIT: @@ -709,7 +711,7 @@ State FMMController::state_landed(const Event& ev) sEventBroker.post(Event{EV_LANDED}, TOPIC_FLIGHT_EVENTS); logger.stop(); - LOG_DEBUG(log, "Entering landed"); + LOG_DEBUG(printLogger, "Entering landed"); break; } case EV_INIT: diff --git a/src/boards/DeathStack/FlightModeManager/FMMController.h b/src/boards/DeathStack/FlightModeManager/FMMController.h index 072f459ea..77b73311a 100644 --- a/src/boards/DeathStack/FlightModeManager/FMMController.h +++ b/src/boards/DeathStack/FlightModeManager/FMMController.h @@ -33,92 +33,90 @@ using miosix::FastMutex; using miosix::Lock; -using namespace Boardcore; - namespace DeathStackBoard { /** * @brief Implementation of the Flight Mode Manager Finite State Machine. */ -class FMMController : public HSM<FMMController> +class FMMController : public Boardcore::HSM<FMMController> { public: FMMController(); ~FMMController(); - State state_initialization(const Event& ev); + Boardcore::State state_initialization(const Boardcore::Event& ev); /** * @brief Handle TC_BOARD_RESET and TC_FORCE_LIFTOFF (super-state). */ - State state_onGround(const Event& ev); + Boardcore::State state_onGround(const Boardcore::Event& ev); /** * @brief First state, wait for sensors and objects initialization. */ - State state_init(const Event& ev); + Boardcore::State state_init(const Boardcore::Event& ev); /** * @brief "Low power" state, log only if requested by TC. */ - State state_initDone(const Event& ev); + Boardcore::State state_initDone(const Boardcore::Event& ev); /** * @brief Init error, get stuck. */ - State state_initError(const Event& ev); + Boardcore::State state_initError(const Boardcore::Event& ev); /** * @brief Test mode, listen to serial and print stuff on serial. */ - State state_testMode(const Event& ev); + Boardcore::State state_testMode(const Boardcore::Event& ev); /** * @brief Calibrating sensors. */ - State state_sensorsCalibration(const Event& ev); + Boardcore::State state_sensorsCalibration(const Boardcore::Event& ev); /** * @brief Calibrating ADA and NAS. */ - State state_algosCalibration(const Event& ev); + Boardcore::State state_algosCalibration(const Boardcore::Event& ev); /** * @brief All good, waiting for arm. */ - State state_disarmed(const Event& ev); + Boardcore::State state_disarmed(const Boardcore::Event& ev); /** * @brief Ready to launch, listening detachment pin (or command). */ - State state_armed(const Event& ev); + Boardcore::State state_armed(const Boardcore::Event& ev); /** * @brief Handle TC_OPEN and END_MISSION (super-state). */ - State state_flying(const Event& ev); + Boardcore::State state_flying(const Boardcore::Event& ev); /** * @brief Liftoff. */ - State state_ascending(const Event& ev); + Boardcore::State state_ascending(const Boardcore::Event& ev); /** * @brief Apogee reached, deploy drogue and wait half altitude (or manual * mode). */ - State state_drogueDescent(const Event& ev); + Boardcore::State state_drogueDescent(const Boardcore::Event& ev); /** * @brief Descent super-state. */ - State state_terminalDescent(const Event& ev); + Boardcore::State state_terminalDescent(const Boardcore::Event& ev); /** * @brief Close file descriptors. */ - State state_landed(const Event& ev); + Boardcore::State state_landed(const Boardcore::Event& ev); FMMStatus getStatus() { return status; } @@ -134,7 +132,8 @@ private: bool ada_ready = false; bool nas_ready = false; - PrintLogger log = Logging::getLogger("deathstack.fsm.fmm"); + Boardcore::PrintLogger printLogger = + Boardcore::Logging::getLogger("deathstack.fsm.fmm"); }; } // namespace DeathStackBoard diff --git a/src/boards/DeathStack/FlightStatsRecorder/FSRController.cpp b/src/boards/DeathStack/FlightStatsRecorder/FSRController.cpp index 1205db39d..830443cfd 100644 --- a/src/boards/DeathStack/FlightStatsRecorder/FSRController.cpp +++ b/src/boards/DeathStack/FlightStatsRecorder/FSRController.cpp @@ -29,6 +29,8 @@ #include <cmath> +using namespace Boardcore; + namespace DeathStackBoard { @@ -527,4 +529,4 @@ void FlightStatsRecorder::state_testingCutters(const Event& ev) } */ -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/FlightStatsRecorder/FSRController.h b/src/boards/DeathStack/FlightStatsRecorder/FSRController.h index f00899138..7244d6170 100644 --- a/src/boards/DeathStack/FlightStatsRecorder/FSRController.h +++ b/src/boards/DeathStack/FlightStatsRecorder/FSRController.h @@ -13,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -38,8 +38,6 @@ #include <hardware_in_the_loop/HIL_sensors/HILSensors.h> #endif -using namespace Boardcore; - namespace DeathStackBoard { @@ -51,7 +49,7 @@ namespace DeathStackBoard * flight we are in, and we do so using a state machine and receiving events * from the topic FLIGHT_EVENTS. */ -class FlightStatsRecorder : public FSM<FlightStatsRecorder> +class FlightStatsRecorder : public Boardcore::FSM<FlightStatsRecorder> { public: FlightStatsRecorder(); @@ -59,12 +57,12 @@ public: void update(const ADAKalmanState& t); void update(const ADAData& t); - void update(const GPSData& t); - void update(const BMX160WithCorrectionData& t); + void update(const Boardcore::GPSData& t); + void update(const Boardcore::BMX160WithCorrectionData& t); // void update(const CurrentSensorData& t); - void update(const MS5803Data& t); // digitl baro - void update(const MPXHZ6130AData& t); // static ports baro - void update(const SSCDANN030PAAData& t); // DPL vane baro + void update(const Boardcore::MS5803Data& t); // digitl baro + void update(const Boardcore::MPXHZ6130AData& t); // static ports baro + void update(const Boardcore::SSCDANN030PAAData& t); // DPL vane baro void update(const AirSpeedPitot& t); #ifdef HARDWARE_IN_THE_LOOP @@ -76,27 +74,27 @@ public: /** * @brief Wait for liftoff or deployment. */ - void state_idle(const Event& ev); + void state_idle(const Boardcore::Event& ev); /** * @brief Record stats of the first few seconds of flight. */ - void state_liftOff(const Event& ev); + void state_liftOff(const Boardcore::Event& ev); /** * @brief Record stats for the apogee part of the flight. */ - void state_ascending(const Event& ev); + void state_ascending(const Boardcore::Event& ev); /** * @brief Record stats during drogue deployment. */ - void state_drogueDeployment(const Event& ev); + void state_drogueDeployment(const Boardcore::Event& ev); /** * @brief Stats during main deployment. */ - void state_mainDeployment(const Event& ev); + void state_mainDeployment(const Boardcore::Event& ev); private: LiftOffStats liftoff_stats{}; @@ -109,7 +107,8 @@ private: uint16_t ev_timeout_id = 0; - PrintLogger log = Logging::getLogger("deathstack.fsm.flightstatsrecorder"); + Boardcore::PrintLogger log = + Boardcore::Logging::getLogger("deathstack.fsm.flightstatsrecorder"); }; -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/LoggerService/LoggerService.h b/src/boards/DeathStack/LoggerService/LoggerService.h index c4593aa70..f9d8fb8bd 100644 --- a/src/boards/DeathStack/LoggerService/LoggerService.h +++ b/src/boards/DeathStack/LoggerService/LoggerService.h @@ -26,8 +26,6 @@ #include <TelemetriesTelecommands/TmRepository.h> #include <logger/Logger.h> -using namespace Boardcore; - namespace DeathStackBoard { @@ -37,16 +35,16 @@ namespace DeathStackBoard * struct is called. In this way, the Logger updates the Tm Repository before * logging on SD card. */ -class LoggerService : public Singleton<LoggerService> +class LoggerService : public Boardcore::Singleton<LoggerService> { - friend class Singleton<LoggerService>; + friend class Boardcore::Singleton<LoggerService>; public: /** * @brief Generic log function, to be implemented for each loggable struct. */ template <typename T> - inline LoggerResult log(const T& t) + inline Boardcore::LoggerResult log(const T& t) { { miosix::PauseKernelLock kLock; @@ -79,22 +77,23 @@ public: */ void stop() { logger.stop(); } - Logger& getLogger() { return logger; } + Boardcore::Logger& getLogger() { return logger; } private: /** * @brief Private constructor to enforce the singleton */ LoggerService() - : logger(Logger::getInstance()), tmRepo(TmRepository::getInstance()) + : logger(Boardcore::Logger::getInstance()), + tmRepo(TmRepository::getInstance()) { } ~LoggerService() {} - Logger& logger; // SD loggers + Boardcore::Logger& logger; // SD loggers // FlightStatsRecorder flight_stats{}; TmRepository& tmRepo; }; -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/Main/Bus.h b/src/boards/DeathStack/Main/Bus.h index a0aaed14e..c1cb1e8b7 100644 --- a/src/boards/DeathStack/Main/Bus.h +++ b/src/boards/DeathStack/Main/Bus.h @@ -25,15 +25,13 @@ #include <drivers/spi/SPIBus.h> #include <miosix.h> -using namespace Boardcore; - namespace DeathStackBoard { struct Bus { - SPIBusInterface* spi1 = new SPIBus(SPI1); - SPIBusInterface* spi2 = new SPIBus(SPI2); + Boardcore::SPIBusInterface* spi1 = new Boardcore::SPIBus(SPI1); + Boardcore::SPIBusInterface* spi2 = new Boardcore::SPIBus(SPI2); }; -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/Main/Radio.h b/src/boards/DeathStack/Main/Radio.h index bacf85b57..1afacc24d 100644 --- a/src/boards/DeathStack/Main/Radio.h +++ b/src/boards/DeathStack/Main/Radio.h @@ -25,8 +25,6 @@ #include <TelemetriesTelecommands/Mavlink.h> #include <drivers/Xbee/Xbee.h> -using namespace Boardcore; - namespace DeathStackBoard { @@ -38,10 +36,10 @@ class Radio public: TMTCController* tmtc_manager; TmRepository* tm_repo; - Xbee::Xbee* xbee; + Boardcore::Xbee::Xbee* xbee; MavDriver* mav_driver; - Radio(SPIBusInterface& xbee_bus); + Radio(Boardcore::SPIBusInterface& xbee_bus); ~Radio(); bool start(); @@ -49,9 +47,9 @@ public: void logStatus(); private: - void onXbeeFrameReceived(Xbee::APIFrame& frame); + void onXbeeFrameReceived(Boardcore::Xbee::APIFrame& frame); - SPIBusInterface& xbee_bus; + Boardcore::SPIBusInterface& xbee_bus; }; -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/Main/Sensors.cpp b/src/boards/DeathStack/Main/Sensors.cpp index 78721d919..34eace92a 100644 --- a/src/boards/DeathStack/Main/Sensors.cpp +++ b/src/boards/DeathStack/Main/Sensors.cpp @@ -35,6 +35,7 @@ using std::bind; using std::function; +using namespace Boardcore; // BMX160 Watermark interrupt void __attribute__((used)) EXTI5_IRQHandlerImpl() @@ -50,6 +51,7 @@ void __attribute__((used)) EXTI5_IRQHandlerImpl() namespace DeathStackBoard { + using namespace SensorConfigs; Sensors::Sensors(SPIBusInterface& spi1_bus, TaskScheduler* scheduler) @@ -582,7 +584,7 @@ void Sensors::mockGpsCallback() void Sensors::updateSensorsStatus() { - SensorInfo info = sensor_manager->getSensorInfo(imu_bmx160); + Boardcore::SensorInfo info = sensor_manager->getSensorInfo(imu_bmx160); if (!info.isInitialized) { status.bmx160 = SensorDriverStatus::DRIVER_ERROR; @@ -613,4 +615,4 @@ void Sensors::updateSensorsStatus() } } -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/Main/Sensors.h b/src/boards/DeathStack/Main/Sensors.h index 1587e1a87..65f36dc47 100644 --- a/src/boards/DeathStack/Main/Sensors.h +++ b/src/boards/DeathStack/Main/Sensors.h @@ -50,8 +50,6 @@ #include <mocksensors/MockSensors.h> #endif -using namespace Boardcore; - namespace DeathStackBoard { @@ -62,22 +60,22 @@ namespace DeathStackBoard class Sensors { public: - SensorManager* sensor_manager = nullptr; + Boardcore::SensorManager* sensor_manager = nullptr; - InternalADC* internal_adc = nullptr; - BatteryVoltageSensor* battery_voltage = nullptr; + Boardcore::InternalADC* internal_adc = nullptr; + Boardcore::BatteryVoltageSensor* battery_voltage = nullptr; - MS5803* press_digital = nullptr; + Boardcore::MS5803* press_digital = nullptr; - ADS1118* adc_ads1118 = nullptr; - SSCDANN030PAA* press_dpl_vane = nullptr; - MPXHZ6130A* press_static_port = nullptr; - SSCDRRN015PDA* press_pitot = nullptr; + Boardcore::ADS1118* adc_ads1118 = nullptr; + Boardcore::SSCDANN030PAA* press_dpl_vane = nullptr; + Boardcore::MPXHZ6130A* press_static_port = nullptr; + Boardcore::SSCDRRN015PDA* press_pitot = nullptr; - BMX160* imu_bmx160 = nullptr; - BMX160WithCorrection* imu_bmx160_with_correction = nullptr; - LIS3MDL* mag_lis3mdl = nullptr; - UbloxGPS* gps_ublox = nullptr; + Boardcore::BMX160* imu_bmx160 = nullptr; + Boardcore::BMX160WithCorrection* imu_bmx160_with_correction = nullptr; + Boardcore::LIS3MDL* mag_lis3mdl = nullptr; + Boardcore::UbloxGPS* gps_ublox = nullptr; #ifdef HARDWARE_IN_THE_LOOP HILImu* hil_imu = nullptr; @@ -89,7 +87,8 @@ public: MockGPS* mock_gps = nullptr; #endif - Sensors(SPIBusInterface& spi1_bus, TaskScheduler* scheduler); + Sensors(Boardcore::SPIBusInterface& spi1_bus, + Boardcore::TaskScheduler* scheduler); ~Sensors(); @@ -151,15 +150,15 @@ private: void updateSensorsStatus(); - SPIBusInterface& spi1_bus; + Boardcore::SPIBusInterface& spi1_bus; - SensorManager::SensorMap_t sensors_map; + Boardcore::SensorManager::SensorMap_t sensors_map; SensorsStatus status; - PrintLogger log = Logging::getLogger("sensors"); + Boardcore::PrintLogger log = Boardcore::Logging::getLogger("sensors"); unsigned int battery_critical_counter = 0; }; -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/Main/SensorsData.h b/src/boards/DeathStack/Main/SensorsData.h index 7ff9615e9..ffdb1f03d 100644 --- a/src/boards/DeathStack/Main/SensorsData.h +++ b/src/boards/DeathStack/Main/SensorsData.h @@ -1,6 +1,5 @@ -/** - * Copyright (c) 2021 Skyward Experimental Rocketry - * Authors: Luca Conterio +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -67,4 +66,4 @@ struct SensorsStatus } }; -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/Main/StateMachines.cpp b/src/boards/DeathStack/Main/StateMachines.cpp index 021e82d27..990aaafda 100644 --- a/src/boards/DeathStack/Main/StateMachines.cpp +++ b/src/boards/DeathStack/Main/StateMachines.cpp @@ -73,17 +73,17 @@ void StateMachines::addAlgorithmsToScheduler(TaskScheduler* scheduler) uint64_t start_time = miosix::getTick() + 10; scheduler->addTask(std::bind(&ADAControllerType::update, ada_controller), - ADA_UPDATE_PERIOD, TASK_ADA_ID, + ADAConfigs::ADA_UPDATE_PERIOD, TASK_ADA_ID, TaskScheduler::Policy::RECOVER, start_time); scheduler->addTask(std::bind(&NASControllerType::update, nas_controller), - NAS_UPDATE_PERIOD, TASK_NAS_ID, + NASConfigs::NAS_UPDATE_PERIOD, TASK_NAS_ID, TaskScheduler::Policy::RECOVER, start_time); scheduler->addTask( std::bind(&AirBrakesControllerType::update, arb_controller), - ABK_UPDATE_PERIOD, TASK_ABK_ID, TaskScheduler::Policy::RECOVER, - start_time); + AirBrakesConfigs::ABK_UPDATE_PERIOD, TASK_ABK_ID, + TaskScheduler::Policy::RECOVER, start_time); } void StateMachines::setReferenceTemperature(float t) @@ -108,4 +108,4 @@ void StateMachines::setInitialCoordinates(float latitude, float longitude) nas_controller->setInitialCoordinates(latitude, longitude); } -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/Main/StateMachines.h b/src/boards/DeathStack/Main/StateMachines.h index d109922f1..828f91810 100644 --- a/src/boards/DeathStack/Main/StateMachines.h +++ b/src/boards/DeathStack/Main/StateMachines.h @@ -35,8 +35,6 @@ #include <mocksensors/MockSensors.h> #endif -using namespace Boardcore; - namespace DeathStackBoard { @@ -74,14 +72,14 @@ public: using GPSType = MockGPS; using GPSDataType = MockGPSData; #else - using IMUType = BMX160WithCorrection; - using IMUDataType = BMX160WithCorrectionData; + using IMUType = Boardcore::BMX160WithCorrection; + using IMUDataType = Boardcore::BMX160WithCorrectionData; - using PressType = MPXHZ6130A; - using PressDataType = MPXHZ6130AData; + using PressType = Boardcore::MPXHZ6130A; + using PressDataType = Boardcore::MPXHZ6130AData; - using GPSType = UbloxGPS; - using GPSDataType = UbloxGPSData; + using GPSType = Boardcore::UbloxGPS; + using GPSDataType = Boardcore::UbloxGPSData; #endif using ADAControllerType = ADAController<PressDataType, GPSDataType>; @@ -96,7 +94,7 @@ public: AirBrakesControllerType* arb_controller; StateMachines(IMUType& imu, PressType& press, GPSType& gps, - TaskScheduler* scheduler); + Boardcore::TaskScheduler* scheduler); ~StateMachines(); @@ -111,7 +109,7 @@ public: void setReferenceAltitude(float altitude); private: - void addAlgorithmsToScheduler(TaskScheduler* scheduler); + void addAlgorithmsToScheduler(Boardcore::TaskScheduler* scheduler); }; -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanConfig.h b/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanConfig.h index bfdf5e927..65242fc30 100644 --- a/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanConfig.h +++ b/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanConfig.h @@ -27,8 +27,6 @@ namespace DeathStackBoard { -using namespace Eigen; - // function with 2 vectors as parameters typedef std::function<MatrixXf(VectorXf, VectorXf)> function_2v; // function with 1 vector as parameter @@ -40,8 +38,8 @@ struct ExtendedKalmanConfig uint8_t m; uint8_t p; - Matrix<float, 12, 12> P; - Matrix<float, 6, 6> Q; + Eigen::Matrix<float, 12, 12> P; + Eigen::Matrix<float, 6, 6> Q; Vector<float, 13> x; function_v h; @@ -50,4 +48,4 @@ struct ExtendedKalmanConfig function_v dhdx; }; -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.cpp b/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.cpp index 6ac88983b..c3fe8804b 100644 --- a/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.cpp +++ b/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.cpp @@ -23,6 +23,7 @@ #include <NavigationAttitudeSystem/ExtendedKalmanEigen.h> using namespace Boardcore; +using namespace Eigen; namespace DeathStackBoard { @@ -45,6 +46,7 @@ ExtendedKalmanEigen::ExtendedKalmanEigen() (1.0F / 3.0F) * SIGMA_BETA * SIGMA_BETA * T * T * T) * eye3, (0.5F * SIGMA_BETA * SIGMA_BETA * T * T) * eye3, + // cppcheck-suppress constStatement (0.5F * SIGMA_BETA * SIGMA_BETA * T * T) * eye3, (SIGMA_BETA * SIGMA_BETA * T) * eye3; @@ -56,32 +58,33 @@ ExtendedKalmanEigen::ExtendedKalmanEigen() 0, 0, P_VEL_VERTICAL; P_att = eye3 * P_ATT; P_bias = eye3 * P_BIAS; - P << P_pos, MatrixXf::Zero(3, N - 4), MatrixXf::Zero(3, 3), P_vel, - MatrixXf::Zero(3, N - 7), MatrixXf::Zero(3, 6), P_att, - MatrixXf::Zero(3, N - 10), MatrixXf::Zero(3, 9), P_bias; + P << P_pos, Eigen::MatrixXf::Zero(3, N - 4), Eigen::MatrixXf::Zero(3, 3), P_vel, + Eigen::MatrixXf::Zero(3, N - 7), Eigen::MatrixXf::Zero(3, 6), P_att, + Eigen::MatrixXf::Zero(3, N - 10), Eigen::MatrixXf::Zero(3, 9), P_bias; Q_pos = eye3 * SIGMA_POS; Q_vel = eye3 * SIGMA_VEL; - Q_lin << Q_pos, MatrixXf::Zero(3, NL - 3), MatrixXf::Zero(3, 3), Q_vel; + Q_lin << Q_pos, Eigen::MatrixXf::Zero(3, NL - 3), Eigen::MatrixXf::Zero(3, 3), Q_vel; - F << 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, + F << 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, - MatrixXf::Zero(3, NL); + 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, + Eigen::MatrixXf::Zero(3, NL); F = eye6 + T * F; Ftr = F.transpose(); - H_gps << 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, - 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, + H_gps << 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, + 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, + 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, + // cppcheck-suppress constStatement 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F; H_gpstr = H_gps.transpose(); - Fatt << -eye3, -eye3 * T, Matrix3f::Zero(3, 3), eye3; + Fatt << -eye3, -eye3 * T, Eigen::Matrix3f::Zero(3, 3), eye3; Fatttr = Fatt.transpose(); - Gatt << -eye3, Matrix3f::Zero(3, 3), - Matrix3f::Zero(3, 3), eye3; + Gatt << -eye3, Eigen::Matrix3f::Zero(3, 3), + Eigen::Matrix3f::Zero(3, 3), eye3; Gatttr = Gatt.transpose(); g << 0.0F, 0.0F, aeroutils::constants::g; // [m/s^2] @@ -89,10 +92,10 @@ ExtendedKalmanEigen::ExtendedKalmanEigen() // clang-format on } -void ExtendedKalmanEigen::predict(const Vector3f& u) +void ExtendedKalmanEigen::predict(const Eigen::Vector3f& u) { - Matrix3f A; - Vector3f a; + Eigen::Matrix3f A; + Eigen::Vector3f a; VectorNf x_dot; Plin = P.block<NL, NL>(0, 0); @@ -110,6 +113,7 @@ void ExtendedKalmanEigen::predict(const Vector3f& u) 2.0F * q1 * q2 - 2.0F * q3 * q4, -powf(q1, 2) + powf(q2, 2) - powf(q3, 2) + powf(q4, 2), 2.0F * q2 * q3 + 2.0F * q1 * q4, 2.0F * q1 * q3 + 2.0F * q2 * q4, + // cppcheck-suppress constStatement 2.0F * q2 * q3 - 2.0F * q1 * q4, -powf(q1, 2) - powf(q2, 2) + powf(q3, 2) + powf(q4, 2); @@ -117,7 +121,7 @@ void ExtendedKalmanEigen::predict(const Vector3f& u) g; // Real accelerometers don't measure g during the flight x_dot << x(3), x(4), x(5), a, - MatrixXf::Zero( + Eigen::MatrixXf::Zero( NATT, 1); // The quaternions and the biases don't need to be updated @@ -146,7 +150,7 @@ void ExtendedKalmanEigen::correctBaro(const float y, const float msl_press, temp; H_bar << 0.0F, 0.0F, dp_dx, - MatrixXf::Zero(1, N - 3 - NATT); // Gradient of h_bar + Eigen::MatrixXf::Zero(1, N - 3 - NATT); // Gradient of h_bar S_bar = H_bar * Plin * H_bar.transpose() + R_bar; @@ -189,6 +193,7 @@ void ExtendedKalmanEigen::correctGPS(const Vector4f& y, const uint8_t sats_num) x.head(NL) = x.head(NL) + K_gps * (yned - h_gps); + // cppcheck-suppress unreadVariable res_gps = y - h_gps; } @@ -199,12 +204,12 @@ void ExtendedKalmanEigen::setX(const VectorNf& x) { this->x = x; } /* MULTIPLICATIVE EXTENDED KALMAN FILTER */ -void ExtendedKalmanEigen::predictMEKF(const Vector3f& u) +void ExtendedKalmanEigen::predictMEKF(const Eigen::Vector3f& u) { - Vector3f omega; - Vector3f prev_bias; + Eigen::Vector3f omega; + Eigen::Vector3f prev_bias; Matrix4f omega_mat; - Matrix3f omega_submat; + Eigen::Matrix3f omega_submat; q << x(NL), x(NL + 1), x(NL + 2), x(NL + 3); prev_bias << x(NL + 4), x(NL + 5), x(NL + 6); @@ -212,13 +217,16 @@ void ExtendedKalmanEigen::predictMEKF(const Vector3f& u) omega = u - prev_bias; omega_submat << 0.0F, -omega(2), omega(1), omega(2), 0.0F, -omega(0), + // cppcheck-suppress constStatement -omega(1), omega(0), 0.0F; + // cppcheck-suppress constStatement omega_mat << -omega_submat, omega, -omega.transpose(), 0.0F; q = (eye4 + 0.5F * omega_mat * T) * q; q.normalize(); + // cppcheck-suppress constStatement x.tail(NATT) << q, prev_bias; Patt = P.block<NMEKF, NMEKF>( @@ -230,14 +238,14 @@ void ExtendedKalmanEigen::predictMEKF(const Vector3f& u) Gatt * Q_mag * Gatttr; // Update only the attitude related part of P } -void ExtendedKalmanEigen::correctMEKF(const Vector3f& y) +void ExtendedKalmanEigen::correctMEKF(const Eigen::Vector3f& y) { - Matrix3f A; - Vector3f z; + Eigen::Matrix3f A; + Eigen::Vector3f z; Vector4f r; - Matrix3f z_mat; + Eigen::Matrix3f z_mat; Vector4f u_att; - Vector3f r_sub; + Eigen::Vector3f r_sub; Matrix<float, NMAG, NMAG> Satt; Matrix<float, NMAG, NMEKF> Hatt; Matrix<float, NMEKF, NMAG> Katt; @@ -253,14 +261,16 @@ void ExtendedKalmanEigen::correctMEKF(const Vector3f& y) 2.0F * q1 * q2 - 2.0F * q3 * q4, -powf(q1, 2) + powf(q2, 2) - powf(q3, 2) + powf(q4, 2), 2.0F * q2 * q3 + 2.0F * q1 * q4, 2.0F * q1 * q3 + 2.0F * q2 * q4, + // cppcheck-suppress constStatement 2.0F * q2 * q3 - 2.0F * q1 * q4, -powf(q1, 2) - powf(q2, 2) + powf(q3, 2) + powf(q4, 2); z = A * NED_MAG; + // cppcheck-suppress constStatement z_mat << 0.0F, -z(2), z(1), z(2), 0.0F, -z(0), -z(1), z(0), 0.0F; - Hatt << z_mat, Matrix3f::Zero(3, 3); + Hatt << z_mat, Eigen::Matrix3f::Zero(3, 3); Patt = P.block<NMEKF, NMEKF>(NL, NL); Satt = Hatt * Patt * Hatt.transpose() + R_mag; @@ -273,6 +283,7 @@ void ExtendedKalmanEigen::correctMEKF(const Vector3f& y) r << 0.5F * r_sub, sqrtf(1.0F - 0.25F * r_sub.transpose() * r_sub); + // cppcheck-suppress constStatement q << q1, q2, q3, q4; u_att = quater.quatProd(r, q); float u_norm = u_att.norm(); @@ -290,4 +301,4 @@ void ExtendedKalmanEigen::correctMEKF(const Vector3f& y) Katt * R_mag * Katt.transpose(); } -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.h b/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.h index fda534bb6..4b1f5ca73 100644 --- a/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.h +++ b/src/boards/DeathStack/NavigationAttitudeSystem/ExtendedKalmanEigen.h @@ -28,15 +28,10 @@ #include <Eigen/Dense> -using namespace Eigen; -using namespace Boardcore; - namespace DeathStackBoard { -using namespace NASConfigs; - -using VectorNf = Matrix<float, N, 1>; +using VectorNf = Eigen::Matrix<float, NASConfigs::N, 1>; class ExtendedKalmanEigen { @@ -49,7 +44,7 @@ public: * * @param u 3x1 Vector of the accelerometer readings [ax ay az]. */ - void predict(const Vector3f& u); + void predict(const Eigen::Vector3f& u); /** * @brief EKF correction of the barometer data. @@ -68,21 +63,21 @@ public: * gps_nord_vel, gps_east_vel]. * @param sats_num Number of satellites available */ - void correctGPS(const Vector4f& y, const uint8_t sats_num); + void correctGPS(const Eigen::Vector4f& y, const uint8_t sats_num); /** * @brief Prediction step of the Multiplicative EKF. * * @param u 3x1 Vector of the gyroscope readings [wx wy wz]. */ - void predictMEKF(const Vector3f& u); + void predictMEKF(const Eigen::Vector3f& u); /** * @brief MEKF correction of the magnetometer readings. * * @param y 3x1 Vector of the magnetometer readings [mx my mz]. */ - void correctMEKF(const Vector3f& y); + void correctMEKF(const Eigen::Vector3f& y); /** * @return 13x1 State vector [px py pz vx vy vz qx qy qz qw bx by bz]. @@ -96,42 +91,42 @@ public: private: VectorNf x; - Matrix<float, NP, NP> P; - Matrix<float, NL, NL> F; - Matrix<float, NL, NL> Ftr; - - Matrix3f P_pos; - Matrix3f P_vel; - Matrix3f P_att; - Matrix3f P_bias; - Matrix<float, NL, NL> Plin; - - Matrix3f Q_pos; - Matrix3f Q_vel; - Matrix<float, NL, NL> Q_lin; - - Vector3f g; - Matrix2f eye2; - Matrix3f eye3; - Matrix4f eye4; - Matrix<float, 6, 6> eye6; - - Matrix<float, NBAR, NBAR> R_bar; - - Matrix<float, NGPS, NGPS> R_gps; - Matrix<float, NGPS, NL> H_gps; - Matrix<float, NL, NGPS> H_gpstr; - - Vector4f q; - Matrix<float, NMAG, NMAG> R_mag; - Matrix<float, NMEKF, NMEKF> Q_mag; - Matrix<float, NMEKF, NMEKF> Fatt; - Matrix<float, NMEKF, NMEKF> Fatttr; - Matrix<float, NMEKF, NMEKF> Gatt; - Matrix<float, NMEKF, NMEKF> Gatttr; - Matrix<float, NMEKF, NMEKF> Patt; - - SkyQuaternion quater; + Eigen::Matrix<float, NASConfigs::NP, NASConfigs::NP> P; + Eigen::Matrix<float, NASConfigs::NL, NASConfigs::NL> F; + Eigen::Matrix<float, NASConfigs::NL, NASConfigs::NL> Ftr; + + Eigen::Matrix3f P_pos; + Eigen::Matrix3f P_vel; + Eigen::Matrix3f P_att; + Eigen::Matrix3f P_bias; + Eigen::Matrix<float, NASConfigs::NL, NASConfigs::NL> Plin; + + Eigen::Matrix3f Q_pos; + Eigen::Matrix3f Q_vel; + Eigen::Matrix<float, NASConfigs::NL, NASConfigs::NL> Q_lin; + + Eigen::Vector3f g; + Eigen::Matrix2f eye2; + Eigen::Matrix3f eye3; + Eigen::Matrix4f eye4; + Eigen::Matrix<float, 6, 6> eye6; + + Eigen::Matrix<float, NASConfigs::NBAR, NASConfigs::NBAR> R_bar; + + Eigen::Matrix<float, NASConfigs::NGPS, NASConfigs::NGPS> R_gps; + Eigen::Matrix<float, NASConfigs::NGPS, NASConfigs::NL> H_gps; + Eigen::Matrix<float, NASConfigs::NL, NASConfigs::NGPS> H_gpstr; + + Eigen::Vector4f q; + Eigen::Matrix<float, NASConfigs::NMAG, NASConfigs::NMAG> R_mag; + Eigen::Matrix<float, NASConfigs::NMEKF, NASConfigs::NMEKF> Q_mag; + Eigen::Matrix<float, NASConfigs::NMEKF, NASConfigs::NMEKF> Fatt; + Eigen::Matrix<float, NASConfigs::NMEKF, NASConfigs::NMEKF> Fatttr; + Eigen::Matrix<float, NASConfigs::NMEKF, NASConfigs::NMEKF> Gatt; + Eigen::Matrix<float, NASConfigs::NMEKF, NASConfigs::NMEKF> Gatttr; + Eigen::Matrix<float, NASConfigs::NMEKF, NASConfigs::NMEKF> Patt; + + Boardcore::SkyQuaternion quater; }; -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/InitStates.h b/src/boards/DeathStack/NavigationAttitudeSystem/InitStates.h index 6a4dd11bd..9082ad1e6 100644 --- a/src/boards/DeathStack/NavigationAttitudeSystem/InitStates.h +++ b/src/boards/DeathStack/NavigationAttitudeSystem/InitStates.h @@ -32,17 +32,14 @@ #include <configs/NASConfig.h> #include <diagnostic/PrintLogger.h> #include <math/SkyQuaternion.h> +#include <utils/Debug.h> #include <utils/aero/AeroUtils.h> #include <Eigen/Dense> -using namespace Eigen; - namespace DeathStackBoard { -using namespace NASConfigs; - class InitStates { @@ -56,7 +53,7 @@ public: * @param mag 3x1 magnetometer readings [mx my mz]. * */ - void eCompass(const Vector3f acc, const Vector3f mag); + void eCompass(const Eigen::Vector3f acc, const Eigen::Vector3f mag); /** * @brief triad algorithm to estimate the attitude before the liftoff. @@ -64,7 +61,7 @@ public: * @param acc 3x1 accelerometer readings [ax ay az]. * @param mag 3x1 magnetometer readings [mx my mz]. */ - const Vector3f& triad(Vector3f& acc, Vector3f& mag); + const Eigen::Vector3f& triad(Eigen::Vector3f& acc, Eigen::Vector3f& mag); /** * @brief Initialization of the positions before the liftoff. @@ -84,27 +81,28 @@ public: */ void biasInit(); - Matrix<float, N, 1> getInitX(); + Eigen::Matrix<float, NASConfigs::N, 1> getInitX(); private: - Matrix<float, N, 1> x_init; - Matrix3f R, Rb, Ri; - SkyQuaternion quat; - Vector4f x_quat; - Vector3f eul; - - PrintLogger log = Logging::getLogger("deathstack.nas.initstates"); + Eigen::Matrix<float, NASConfigs::N, 1> x_init; + Eigen::Matrix3f R, Rb, Ri; + Boardcore::SkyQuaternion quat; + Eigen::Vector4f x_quat; + Eigen::Vector3f eul; + + Boardcore::PrintLogger log = + Boardcore::Logging::getLogger("deathstack.nas.initstates"); }; -InitStates::InitStates() { x_init << MatrixXf::Zero(N, 1); } +InitStates::InitStates() { x_init << Eigen::MatrixXf::Zero(NASConfigs::N, 1); } -void InitStates::eCompass(const Vector3f acc, const Vector3f mag) +void InitStates::eCompass(const Eigen::Vector3f acc, const Eigen::Vector3f mag) { // ndr: since this method runs only when the rocket is stationary, there's // no need to add the gravity vector because the accelerometers already // measure it. This is not true if we consider the flying rocket. - Vector3f am(acc.cross(mag)); + Eigen::Vector3f am(acc.cross(mag)); R << am.cross(acc), am, acc; R.col(0).normalize(); @@ -114,46 +112,47 @@ void InitStates::eCompass(const Vector3f acc, const Vector3f mag) x_quat = quat.rotm2quat(R); eul = quat.quat2eul(x_quat); - x_init(NL) = x_quat(0); - x_init(NL + 1) = x_quat(1); - x_init(NL + 2) = x_quat(2); - x_init(NL + 3) = x_quat(3); + x_init(NASConfigs::NL) = x_quat(0); + x_init(NASConfigs::NL + 1) = x_quat(1); + x_init(NASConfigs::NL + 2) = x_quat(2); + x_init(NASConfigs::NL + 3) = x_quat(3); } -const Vector3f& InitStates::triad(Vector3f& acc, Vector3f& mag) +const Eigen::Vector3f& InitStates::triad(Eigen::Vector3f& acc, + Eigen::Vector3f& mag) { LOG_DEBUG(log, "Executing TRIAD"); // The coulmns of the the triad matrices. b:body, i:inertial - Vector3f t1b, t2b, t3b, t1i, t2i, t3i; + Eigen::Vector3f t1b, t2b, t3b, t1i, t2i, t3i; // vettore gravità "vero" in NED, normalizzato : // -1 su asse "down", perché da fermo l'accelerometro // misura la reazione vincolare (rivolta verso l'alto) - Vector3f g_norm(0.0F, 0.0F, -1.0F); + Eigen::Vector3f g_norm(0.0F, 0.0F, -1.0F); acc.normalize(); - Vector3f w1 = acc; + Eigen::Vector3f w1 = acc; mag.normalize(); - Vector3f w2 = mag; + Eigen::Vector3f w2 = mag; - Vector3f v1 = g_norm; - Vector3f v2 = NED_MAG; + Eigen::Vector3f v1 = g_norm; + Eigen::Vector3f v2 = NASConfigs::NED_MAG; - Vector3f Ou1 = w1; - Vector3f Ou2 = w1.cross(w2); + Eigen::Vector3f Ou1 = w1; + Eigen::Vector3f Ou2 = w1.cross(w2); Ou2.normalize(); - Vector3f Ou3 = Ou2.cross(Ou1); + Eigen::Vector3f Ou3 = Ou2.cross(Ou1); - Vector3f R1 = v1; - Vector3f R2 = v1.cross(v2); + Eigen::Vector3f R1 = v1; + Eigen::Vector3f R2 = v1.cross(v2); R2.normalize(); - Vector3f R3 = R2.cross(R1); + Eigen::Vector3f R3 = R2.cross(R1); - Matrix3f Mou; + Eigen::Matrix3f Mou; Mou << Ou1, Ou2, Ou3; - Matrix3f Mr; + Eigen::Matrix3f Mr; Mr << R1, R2, R3; R = Mr * Mou.transpose(); @@ -162,10 +161,10 @@ const Vector3f& InitStates::triad(Vector3f& acc, Vector3f& mag) eul = quat.quat2eul(x_quat); - x_init(NL) = x_quat(0); - x_init(NL + 1) = x_quat(1); - x_init(NL + 2) = x_quat(2); - x_init(NL + 3) = x_quat(3); + x_init(NASConfigs::NL) = x_quat(0); + x_init(NASConfigs::NL + 1) = x_quat(1); + x_init(NASConfigs::NL + 2) = x_quat(2); + x_init(NASConfigs::NL + 3) = x_quat(3); return eul; } @@ -175,7 +174,7 @@ void InitStates::positionInit(const float press, const float ref_press, { x_init(0) = 0.0; x_init(1) = 0.0; - x_init(2) = -aeroutils::relAltitude( + x_init(2) = -Boardcore::aeroutils::relAltitude( press, ref_press, ref_temp); // msl altitude (in NED, so negative) TRACE("\n[NAS] press : %f - z_init : %f\n\n", press, x_init(2)); @@ -192,11 +191,11 @@ void InitStates::biasInit() { // gyro biases set to zero since the sensor performs // self-calibration and the measurements are already compensated - x_init(NL + 4) = 0.0F; - x_init(NL + 5) = 0.0F; - x_init(NL + 6) = 0.0F; + x_init(NASConfigs::NL + 4) = 0.0F; + x_init(NASConfigs::NL + 5) = 0.0F; + x_init(NASConfigs::NL + 6) = 0.0F; } -Matrix<float, N, 1> InitStates::getInitX() { return x_init; } +Eigen::Matrix<float, NASConfigs::N, 1> InitStates::getInitX() { return x_init; } -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/NAS.h b/src/boards/DeathStack/NavigationAttitudeSystem/NAS.h index 9310e21f7..2f920b025 100644 --- a/src/boards/DeathStack/NavigationAttitudeSystem/NAS.h +++ b/src/boards/DeathStack/NavigationAttitudeSystem/NAS.h @@ -48,14 +48,13 @@ namespace DeathStackBoard { -using namespace NASConfigs; - template <typename IMU, typename Press, typename GPS> -class NAS : public Sensor<NASData> +class NAS : public Boardcore::Sensor<NASData> { public: - NAS(Sensor<IMU>& imu, Sensor<Press>& baro, Sensor<GPS>& gps); + NAS(Boardcore::Sensor<IMU>& imu, Boardcore::Sensor<Press>& baro, + Boardcore::Sensor<GPS>& gps); /** * @brief Initialization of the state vector before the liftoff @@ -108,11 +107,11 @@ private: /** * @brief Convert GPS coordinates to NED frame. */ - Vector3f geodetic2NED(const Vector3f& gps_data); + Eigen::Vector3f geodetic2NED(const Eigen::Vector3f& gps_data); - SkyQuaternion quat; /**< Auxiliary functions for quaternions */ + Boardcore::SkyQuaternion quat; /**< Auxiliary functions for quaternions */ - Matrix<float, N, 1> x; /**< Kalman state vector */ + Eigen::Matrix<float, NASConfigs::N, 1> x; /**< Kalman state vector */ NASData nas_data; @@ -123,7 +122,7 @@ private: ExtendedKalmanEigen filter; InitStates states_init; NASReferenceValues ref_values; - Vector3f triad_result_eul; + Eigen::Vector3f triad_result_eul; uint64_t last_gps_timestamp = 0; uint64_t last_accel_timestamp = 0; @@ -133,7 +132,8 @@ private: bool initialized = false; - PrintLogger log = Logging::getLogger("deathstack.fsm.nas"); + Boardcore::PrintLogger log = + Boardcore::Logging::getLogger("deathstack.fsm.nas"); #ifdef DEBUG unsigned int counter = 0; @@ -145,7 +145,7 @@ NAS<IMU, Press, GPS>::NAS(Sensor<IMU>& imu, Sensor<Press>& baro, Sensor<GPS>& gps) : imu(imu), barometer(baro), gps(gps) { - x = Matrix<float, N, 1>::Zero(); + x = Eigen::Matrix<float, NASConfigs::N, 1>::Zero(); } template <typename IMU, typename Press, typename GPS> @@ -156,10 +156,10 @@ bool NAS<IMU, Press, GPS>::init() states_init.velocityInit(); - Vector3f acc_init(ref_values.ref_accel_x, ref_values.ref_accel_y, - ref_values.ref_accel_z); - Vector3f mag_init(ref_values.ref_mag_x, ref_values.ref_mag_y, - ref_values.ref_mag_z); + Eigen::Vector3f acc_init(ref_values.ref_accel_x, ref_values.ref_accel_y, + ref_values.ref_accel_z); + Eigen::Vector3f mag_init(ref_values.ref_mag_x, ref_values.ref_mag_y, + ref_values.ref_mag_z); triad_result_eul = states_init.triad(acc_init, mag_init); @@ -172,8 +172,8 @@ bool NAS<IMU, Press, GPS>::init() updateNASData(); #ifdef DEBUG - Vector4f qua(x(6), x(7), x(8), x(9)); - Vector3f e = quat.quat2eul(qua); + Eigen::Vector4f qua(x(6), x(7), x(8), x(9)); + Eigen::Vector3f e = quat.quat2eul(qua); LOG_DEBUG( log, @@ -217,13 +217,14 @@ NASData NAS<IMU, Press, GPS>::sampleImpl() last_accel_timestamp = imu_data.accelerationTimestamp; last_gyro_timestamp = imu_data.angularVelocityTimestamp; - Vector3f accel_readings(imu_data.accelerationX, imu_data.accelerationY, - imu_data.accelerationZ); + Eigen::Vector3f accel_readings(imu_data.accelerationX, + imu_data.accelerationY, + imu_data.accelerationZ); filter.predict(accel_readings); - Vector3f gyro_readings(imu_data.angularVelocityX, - imu_data.angularVelocityY, - imu_data.angularVelocityZ); + Eigen::Vector3f gyro_readings(imu_data.angularVelocityX, + imu_data.angularVelocityY, + imu_data.angularVelocityZ); filter.predictMEKF(gyro_readings); } @@ -241,22 +242,23 @@ NASData NAS<IMU, Press, GPS>::sampleImpl() { last_gps_timestamp = gps_data.gpsTimestamp; - Vector3f gps_readings(gps_data.latitude, gps_data.longitude, - gps_data.height); - Vector3f gps_ned = geodetic2NED(gps_readings); + Eigen::Vector3f gps_readings(gps_data.latitude, gps_data.longitude, + gps_data.height); + Eigen::Vector3f gps_ned = geodetic2NED(gps_readings); - Vector4f pos_vel(gps_ned(0), gps_ned(1), gps_data.velocityNorth, - gps_data.velocityEast); + Eigen::Vector4f pos_vel(gps_ned(0), gps_ned(1), gps_data.velocityNorth, + gps_data.velocityEast); filter.correctGPS(pos_vel, gps_data.satellites); } // check if new magnetometer data is available if (imu_data.magneticFieldTimestamp != last_mag_timestamp) { - Vector3f mag_readings(imu_data.magneticFieldX, imu_data.magneticFieldY, - imu_data.magneticFieldZ); + Eigen::Vector3f mag_readings(imu_data.magneticFieldX, + imu_data.magneticFieldY, + imu_data.magneticFieldZ); - if (mag_readings.norm() < EMF * JAMMING_FACTOR) + if (mag_readings.norm() < NASConfigs::EMF * NASConfigs::JAMMING_FACTOR) { last_mag_timestamp = imu_data.magneticFieldTimestamp; @@ -291,8 +293,9 @@ NASData NAS<IMU, Press, GPS>::sampleImpl() template <typename IMU, typename Press, typename GPS> NASTriadResult NAS<IMU, Press, GPS>::getTriadResult() { - Matrix<float, N, 1> state = states_init.getInitX(); - // Vector3f e = quat.quat2eul({state(6), state(7), state(8), state(9)}); + Eigen::Matrix<float, NASConfigs::N, 1> state = states_init.getInitX(); + // Eigen::Vector3f e = quat.quat2eul({state(6), state(7), state(8), + // state(9)}); NASTriadResult result; result.x = state(0); @@ -333,11 +336,11 @@ template <typename IMU, typename Press, typename GPS> void NAS<IMU, Press, GPS>::setInitialOrientation(float roll, float pitch, float yaw) { - Vector4f q = quat.eul2quat({yaw, pitch, roll}); - x(NL) = q(0); - x(NL + 1) = q(1); - x(NL + 2) = q(2); - x(NL + 3) = q(3); + Eigen::Vector4f q = quat.eul2quat({yaw, pitch, roll}); + x(NASConfigs::NL) = q(0); + x(NASConfigs::NL + 1) = q(1); + x(NASConfigs::NL + 2) = q(2); + x(NASConfigs::NL + 3) = q(3); LOG_INFO(log, "Initial orientation set to : ({:.2f}, {:.2f}, {:.2f})", roll, pitch, yaw); LOG_DEBUG(log, @@ -353,7 +356,8 @@ void NAS<IMU, Press, GPS>::setInitialOrientation(float roll, float pitch, template <typename IMU, typename Press, typename GPS> void NAS<IMU, Press, GPS>::updateNASData() { - nas_data.timestamp = TimestampTimer::getInstance().getTimestamp(); + nas_data.timestamp = + Boardcore::TimestampTimer::getInstance().getTimestamp(); nas_data.x = x(0); nas_data.y = x(1); @@ -371,29 +375,30 @@ void NAS<IMU, Press, GPS>::updateNASData() } template <typename IMU, typename Press, typename GPS> -Vector3f NAS<IMU, Press, GPS>::geodetic2NED(const Vector3f& gps_data) +Eigen::Vector3f NAS<IMU, Press, GPS>::geodetic2NED( + const Eigen::Vector3f& gps_data) { - float lat0 = ref_values.ref_latitude * DEGREES_TO_RADIANS; - float lon0 = ref_values.ref_longitude * DEGREES_TO_RADIANS; - float lat = gps_data(0) * DEGREES_TO_RADIANS; - float lon = gps_data(1) * DEGREES_TO_RADIANS; + float lat0 = ref_values.ref_latitude * Boardcore::DEGREES_TO_RADIANS; + float lon0 = ref_values.ref_longitude * Boardcore::DEGREES_TO_RADIANS; + float lat = gps_data(0) * Boardcore::DEGREES_TO_RADIANS; + float lon = gps_data(1) * Boardcore::DEGREES_TO_RADIANS; float h = gps_data(2); - float s1 = sin(lat0); - float c1 = cos(lat0); - float s2 = sin(lat); - float c2 = cos(lat); - float p1 = c1 * cos(lon0); - float p2 = c2 * cos(lon); - float q1 = c1 * sin(lon0); - float q2 = c2 * sin(lon); - float w1 = 1 / sqrt(1 - e2 * pow(s1, 2)); - float w2 = 1 / sqrt(1 - e2 * pow(s2, 2)); - float delta_x = - a * (p2 * w2 - p1 * w1) + (h * p2 - ref_values.ref_altitude * p1); - float delta_y = - a * (q2 * w2 - q1 * w1) + (h * q2 - ref_values.ref_altitude * q1); - float delta_z = (1 - e2) * a * (s2 * w2 - s1 * w1) + + float s1 = sin(lat0); + float c1 = cos(lat0); + float s2 = sin(lat); + float c2 = cos(lat); + float p1 = c1 * cos(lon0); + float p2 = c2 * cos(lon); + float q1 = c1 * sin(lon0); + float q2 = c2 * sin(lon); + float w1 = 1 / sqrt(1 - NASConfigs::e2 * pow(s1, 2)); + float w2 = 1 / sqrt(1 - NASConfigs::e2 * pow(s2, 2)); + float delta_x = NASConfigs::a * (p2 * w2 - p1 * w1) + + (h * p2 - ref_values.ref_altitude * p1); + float delta_y = NASConfigs::a * (q2 * w2 - q1 * w1) + + (h * q2 - ref_values.ref_altitude * q1); + float delta_z = (1 - NASConfigs::e2) * NASConfigs::a * (s2 * w2 - s1 * w1) + (h * s2 - ref_values.ref_altitude * s1); // positions in ENU (east, north, up) frame @@ -404,9 +409,9 @@ Vector3f NAS<IMU, Press, GPS>::geodetic2NED(const Vector3f& gps_data) cos(lat0) * sin(lon0) * delta_y + sin(lat0) * delta_z; // positions in NED frame - Vector3f p_ned(p_north, p_east, -p_up); + Eigen::Vector3f p_ned(p_north, p_east, -p_up); return p_ned; } -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/NASCalibrator.h b/src/boards/DeathStack/NavigationAttitudeSystem/NASCalibrator.h index c8d12a06c..a00fe2ed7 100644 --- a/src/boards/DeathStack/NavigationAttitudeSystem/NASCalibrator.h +++ b/src/boards/DeathStack/NavigationAttitudeSystem/NASCalibrator.h @@ -83,18 +83,19 @@ private: NASReferenceValues ref_values{}; - Stats pressure_stats; // Computes mean std dev etc for calibration + Boardcore::Stats + pressure_stats; // Computes mean std dev etc for calibration - Stats gps_lat_stats; - Stats gps_lon_stats; + Boardcore::Stats gps_lat_stats; + Boardcore::Stats gps_lon_stats; - Stats accel_x_stats; - Stats accel_y_stats; - Stats accel_z_stats; + Boardcore::Stats accel_x_stats; + Boardcore::Stats accel_y_stats; + Boardcore::Stats accel_z_stats; - Stats mag_x_stats; - Stats mag_y_stats; - Stats mag_z_stats; + Boardcore::Stats mag_x_stats; + Boardcore::Stats mag_y_stats; + Boardcore::Stats mag_z_stats; // Refernece flags bool ref_coordinates_set = false; @@ -102,4 +103,4 @@ private: bool ref_temperature_set = false; }; -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/NASController.h b/src/boards/DeathStack/NavigationAttitudeSystem/NASController.h index 307116324..e629c165e 100644 --- a/src/boards/DeathStack/NavigationAttitudeSystem/NASController.h +++ b/src/boards/DeathStack/NavigationAttitudeSystem/NASController.h @@ -36,46 +36,48 @@ using miosix::FastMutex; using miosix::Lock; -using namespace Boardcore; - namespace DeathStackBoard { -using namespace NASConfigs; - /** * @brief Navigatioin system state machine */ template <typename IMU, typename Press, typename GPS> -class NASController : public FSM<NASController<IMU, Press, GPS>> +class NASController : public Boardcore::FSM<NASController<IMU, Press, GPS>> { using NASCtrl = NASController<IMU, Press, GPS>; - using NASFsm = FSM<NASCtrl>; + using NASFsm = Boardcore::FSM<NASCtrl>; static_assert( - checkIfProduces<Sensor<IMU>, AccelerometerData>::value, + Boardcore::checkIfProduces<Boardcore::Sensor<IMU>, + Boardcore::AccelerometerData>::value, "Template argument must be a sensor that produces accelerometer data."); static_assert( - checkIfProduces<Sensor<IMU>, GyroscopeData>::value, + Boardcore::checkIfProduces<Boardcore::Sensor<IMU>, + Boardcore::GyroscopeData>::value, "Template argument must be a sensor that produces gyroscope data."); static_assert( - checkIfProduces<Sensor<IMU>, MagnetometerData>::value, + Boardcore::checkIfProduces<Boardcore::Sensor<IMU>, + Boardcore::MagnetometerData>::value, "Template argument must be a sensor that produces magnetometer data."); static_assert( - checkIfProduces<Sensor<Press>, PressureData>::value, + Boardcore::checkIfProduces<Boardcore::Sensor<Press>, + Boardcore::PressureData>::value, "Template argument must be a sensor that produces pressure data."); - static_assert(checkIfProduces<Sensor<GPS>, GPSData>::value, + static_assert(Boardcore::checkIfProduces<Boardcore::Sensor<GPS>, + Boardcore::GPSData>::value, "Template argument must be a sensor that produces GPS data."); public: - NASController(Sensor<IMU>& imu, Sensor<Press>& baro, Sensor<GPS>& gps); + NASController(Boardcore::Sensor<IMU>& imu, Boardcore::Sensor<Press>& baro, + Boardcore::Sensor<GPS>& gps); ~NASController(); - void state_idle(const Event& ev); - void state_calibrating(const Event& ev); - void state_ready(const Event& ev); - void state_active(const Event& ev); - void state_end(const Event& ev); + void state_idle(const Boardcore::Event& ev); + void state_calibrating(const Boardcore::Event& ev); + void state_ready(const Boardcore::Event& ev); + void state_active(const Boardcore::Event& ev); + void state_end(const Boardcore::Event& ev); void setReferenceTemperature(float t); void setInitialOrientation(float roll, float pitch, float yaw); @@ -84,7 +86,7 @@ public: void update(); - Sensor<NASData>& getNAS() { return nas; } + Boardcore::Sensor<NASData>& getNAS() { return nas; } private: void finalizeCalibration(); @@ -100,14 +102,15 @@ private: FastMutex mutex; NASCalibrator calibrator; - Sensor<IMU>& imu; - Sensor<Press>& barometer; - Sensor<GPS>& gps; + Boardcore::Sensor<IMU>& imu; + Boardcore::Sensor<Press>& barometer; + Boardcore::Sensor<GPS>& gps; NAS<IMU, Press, GPS> nas; LoggerService& logger; - PrintLogger log = Logging::getLogger("deathstack.fsm.nas"); + Boardcore::PrintLogger log = + Boardcore::Logging::getLogger("deathstack.fsm.nas"); uint64_t last_gps_timestamp = 0; uint64_t last_accel_timestamp = 0; @@ -116,12 +119,12 @@ private: }; template <typename IMU, typename Press, typename GPS> -NASController<IMU, Press, GPS>::NASController(Sensor<IMU>& imu, - Sensor<Press>& baro, - Sensor<GPS>& gps) - : NASFsm(&NASCtrl::state_idle), calibrator(CALIBRATION_N_SAMPLES), imu(imu), - barometer(baro), gps(gps), nas(imu, baro, gps), - logger(LoggerService::getInstance()) +NASController<IMU, Press, GPS>::NASController(Boardcore::Sensor<IMU>& imu, + Boardcore::Sensor<Press>& baro, + Boardcore::Sensor<GPS>& gps) + : NASFsm(&NASCtrl::state_idle), + calibrator(NASConfigs::CALIBRATION_N_SAMPLES), imu(imu), barometer(baro), + gps(gps), nas(imu, baro, gps), logger(LoggerService::getInstance()) { memset(&status, 0, sizeof(NASStatus)); sEventBroker.subscribe(this, TOPIC_FLIGHT_EVENTS); @@ -247,17 +250,17 @@ void NASController<IMU, Press, GPS>::finalizeCalibration() } template <typename IMU, typename Press, typename GPS> -void NASController<IMU, Press, GPS>::state_idle(const Event& ev) +void NASController<IMU, Press, GPS>::state_idle(const Boardcore::Event& ev) { switch (ev.code) { - case EV_ENTRY: + case Boardcore::EV_ENTRY: { LOG_DEBUG(log, "Entering state idle"); logStatus(NASState::IDLE); break; } - case EV_EXIT: + case Boardcore::EV_EXIT: { LOG_DEBUG(log, "Exiting state idle"); break; @@ -275,11 +278,12 @@ void NASController<IMU, Press, GPS>::state_idle(const Event& ev) } template <typename IMU, typename Press, typename GPS> -void NASController<IMU, Press, GPS>::state_calibrating(const Event& ev) +void NASController<IMU, Press, GPS>::state_calibrating( + const Boardcore::Event& ev) { switch (ev.code) { - case EV_ENTRY: + case Boardcore::EV_ENTRY: { LOG_DEBUG(log, "Entering state calibrating"); logStatus(NASState::CALIBRATING); @@ -292,7 +296,7 @@ void NASController<IMU, Press, GPS>::state_calibrating(const Event& ev) break; } - case EV_EXIT: + case Boardcore::EV_EXIT: { LOG_DEBUG(log, "Exiting state calibrating"); break; @@ -315,17 +319,17 @@ void NASController<IMU, Press, GPS>::state_calibrating(const Event& ev) } template <typename IMU, typename Press, typename GPS> -void NASController<IMU, Press, GPS>::state_ready(const Event& ev) +void NASController<IMU, Press, GPS>::state_ready(const Boardcore::Event& ev) { switch (ev.code) { - case EV_ENTRY: + case Boardcore::EV_ENTRY: { LOG_DEBUG(log, "Entering state ready"); logStatus(NASState::READY); break; } - case EV_EXIT: + case Boardcore::EV_EXIT: { LOG_DEBUG(log, "Exiting state ready"); break; @@ -348,17 +352,17 @@ void NASController<IMU, Press, GPS>::state_ready(const Event& ev) } template <typename IMU, typename Press, typename GPS> -void NASController<IMU, Press, GPS>::state_active(const Event& ev) +void NASController<IMU, Press, GPS>::state_active(const Boardcore::Event& ev) { switch (ev.code) { - case EV_ENTRY: + case Boardcore::EV_ENTRY: { LOG_DEBUG(log, "Entering state active"); logStatus(NASState::ACTIVE); break; } - case EV_EXIT: + case Boardcore::EV_EXIT: { LOG_DEBUG(log, "Exiting state active"); break; @@ -376,17 +380,17 @@ void NASController<IMU, Press, GPS>::state_active(const Event& ev) } template <typename IMU, typename Press, typename GPS> -void NASController<IMU, Press, GPS>::state_end(const Event& ev) +void NASController<IMU, Press, GPS>::state_end(const Boardcore::Event& ev) { switch (ev.code) { - case EV_ENTRY: + case Boardcore::EV_ENTRY: { LOG_DEBUG(log, "Entering state end"); logStatus(NASState::END); break; } - case EV_EXIT: + case Boardcore::EV_EXIT: { LOG_DEBUG(log, "Exiting state end"); break; @@ -459,20 +463,21 @@ void NASController<IMU, Press, GPS>::setReferenceAltitude(float altitude) template <typename IMU, typename Press, typename GPS> void NASController<IMU, Press, GPS>::logStatus(NASState state) { - status.timestamp = TimestampTimer::getInstance().getTimestamp(); + status.timestamp = Boardcore::TimestampTimer::getInstance().getTimestamp(); status.state = state; logger.log(status); - StackLogger::getInstance().updateStack(THID_NAS_FSM); + Boardcore::StackLogger::getInstance().updateStack(THID_NAS_FSM); } template <typename IMU, typename Press, typename GPS> void NASController<IMU, Press, GPS>::logData() { NASKalmanState kalman_state = nas.getKalmanState(); - kalman_state.timestamp = TimestampTimer::getInstance().getTimestamp(); + kalman_state.timestamp = + Boardcore::TimestampTimer::getInstance().getTimestamp(); logger.log(kalman_state); logger.log(nas.getLastSample()); } -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/NavigationAttitudeSystem/NASData.h b/src/boards/DeathStack/NavigationAttitudeSystem/NASData.h index 0a755dbbe..b3bacdc4b 100644 --- a/src/boards/DeathStack/NavigationAttitudeSystem/NASData.h +++ b/src/boards/DeathStack/NavigationAttitudeSystem/NASData.h @@ -30,8 +30,6 @@ #include <iostream> #include <string> -using namespace Boardcore; - namespace DeathStackBoard { @@ -109,7 +107,8 @@ struct NASKalmanState NASKalmanState() {} - NASKalmanState(uint64_t t, const Matrix<float, NASConfigs::N, 1>& state) + NASKalmanState(uint64_t t, + const Eigen::Matrix<float, NASConfigs::N, 1>& state) { timestamp = t; x0 = state(0); @@ -139,9 +138,9 @@ struct NASKalmanState << "," << x9 << "," << x10 << "," << x11 << "," << x12 << "\n"; } - Vector3f toEul() const + Eigen::Vector3f toEul() const { - SkyQuaternion q; + Boardcore::SkyQuaternion q; return q.quat2eul({x6, x7, x8, x9}); } }; @@ -156,8 +155,8 @@ struct NASReferenceValues float ref_pressure = DEFAULT_REFERENCE_PRESSURE; float ref_temperature = DEFAULT_REFERENCE_TEMPERATURE; - float msl_pressure = MSL_PRESSURE; - float msl_temperature = MSL_TEMPERATURE; + float msl_pressure = Boardcore::MSL_PRESSURE; + float msl_temperature = Boardcore::MSL_TEMPERATURE; float ref_altitude = DEFAULT_REFERENCE_ALTITUDE; float ref_latitude = 0.0f; diff --git a/src/boards/DeathStack/PinHandler/PinHandler.h b/src/boards/DeathStack/PinHandler/PinHandler.h index a42a8d167..a0921ec59 100644 --- a/src/boards/DeathStack/PinHandler/PinHandler.h +++ b/src/boards/DeathStack/PinHandler/PinHandler.h @@ -27,8 +27,6 @@ #include <diagnostic/PrintLogger.h> #include <utils/PinObserver.h> -using namespace Boardcore; - namespace DeathStackBoard { @@ -97,10 +95,11 @@ private: PinStatus status_pin_nosecone{ObservedPin::NOSECONE}; PinStatus status_pin_dpl_servo{ObservedPin::DPL_SERVO}; - PinObserver pin_obs; + Boardcore::PinObserver pin_obs; LoggerService* logger; - PrintLogger log = Logging::getLogger("deathstack.pinhandler"); + Boardcore::PrintLogger log = + Boardcore::Logging::getLogger("deathstack.pinhandler"); }; -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/System/StackLogger.h b/src/boards/DeathStack/System/StackLogger.h index 9af6f2150..3b298b1c8 100644 --- a/src/boards/DeathStack/System/StackLogger.h +++ b/src/boards/DeathStack/System/StackLogger.h @@ -33,7 +33,7 @@ namespace DeathStackBoard */ enum LynxThreadIds : uint8_t { - THID_ENTRYPOINT = THID_FIRST_AVAILABLE_ID, + THID_ENTRYPOINT = Boardcore::THID_FIRST_AVAILABLE_ID, THID_DPL_FSM, THID_FMM_FSM, THID_TMTC_FSM, @@ -44,4 +44,4 @@ enum LynxThreadIds : uint8_t THID_TASK_SCHEDULER }; -} \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/System/TaskID.h b/src/boards/DeathStack/System/TaskID.h index 037f42766..0b7247101 100644 --- a/src/boards/DeathStack/System/TaskID.h +++ b/src/boards/DeathStack/System/TaskID.h @@ -1,6 +1,5 @@ -/** - * Copyright (c) 2021 Skyward Experimental Rocketry - * Authors: Luca Conterio +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -43,4 +42,4 @@ enum TaskIDs : uint8_t TASK_NAS_ID = 9 }; -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/TelemetriesTelecommands/Mavlink.h b/src/boards/DeathStack/TelemetriesTelecommands/Mavlink.h index ca8be01c8..e2b37042a 100644 --- a/src/boards/DeathStack/TelemetriesTelecommands/Mavlink.h +++ b/src/boards/DeathStack/TelemetriesTelecommands/Mavlink.h @@ -30,12 +30,12 @@ #include <mavlink_lib/lynx/mavlink.h> #pragma GCC diagnostic pop -#include <drivers/mavlink/MavlinkDriver.h> #include <configs/MavlinkConfig.h> +#include <drivers/mavlink/MavlinkDriver.h> namespace DeathStackBoard { -using MavDriver = MavlinkDriver<MAV_PKT_SIZE, MAV_OUT_QUEUE_LEN>; +using MavDriver = Boardcore::MavlinkDriver<MAV_PKT_SIZE, MAV_OUT_QUEUE_LEN>; -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.h b/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.h index 5921a37de..a160b08f3 100644 --- a/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.h +++ b/src/boards/DeathStack/TelemetriesTelecommands/TCHandler.h @@ -39,7 +39,7 @@ void logMavlinkStatus(MavDriver* mav_driver); static LoggerService* logger = &LoggerService::getInstance(); -static PrintLogger print_logger = - Logging::getLogger("deathstack.tmtc.tchandler"); +static Boardcore::PrintLogger print_logger = + Boardcore::Logging::getLogger("deathstack.tmtc.tchandler"); -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.cpp b/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.cpp index 9d564a18d..ea055cc51 100644 --- a/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.cpp +++ b/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.cpp @@ -27,6 +27,8 @@ #include <cassert> +using namespace Boardcore; + namespace DeathStackBoard { @@ -37,7 +39,7 @@ TMTCController::TMTCController() sEventBroker.subscribe(this, TOPIC_FLIGHT_EVENTS); sEventBroker.subscribe(this, TOPIC_TMTC); - LOG_DEBUG(log, "Created TMTCController"); + LOG_DEBUG(printLogger, "Created TMTCController"); } TMTCController::~TMTCController() { sEventBroker.unsubscribe(this); } @@ -78,12 +80,12 @@ void TMTCController::stateGroundTM(const Event& ev) { // add periodic events periodicHrEvId = sEventBroker.postDelayed<HR_TM_GROUND_TIMEOUT>( - Event{EV_SEND_HR_TM}, TOPIC_TMTC); + Boardcore::Event{EV_SEND_HR_TM}, TOPIC_TMTC); // periodicSensEvId = // sEventBroker.postDelayed<GROUND_SENS_TM_TIMEOUT>( - // Event{EV_SEND_SENS_TM}, TOPIC_TMTC); + // Boardcore::Event{EV_SEND_SENS_TM}, TOPIC_TMTC); - LOG_DEBUG(log, "Entering stateGroundTM"); + LOG_DEBUG(printLogger, "Entering stateGroundTM"); // log stack usage StackLogger::getInstance().updateStack(THID_TMTC_FSM); @@ -95,14 +97,14 @@ void TMTCController::stateGroundTM(const Event& ev) sEventBroker.removeDelayed(periodicHrEvId); sEventBroker.removeDelayed(periodicSensEvId); - LOG_DEBUG(log, "Exiting stateGroundTM"); + LOG_DEBUG(printLogger, "Exiting stateGroundTM"); break; } case EV_SEND_HR_TM: { // repost periodic event periodicHrEvId = sEventBroker.postDelayed<HR_TM_GROUND_TIMEOUT>( - Event{EV_SEND_HR_TM}, TOPIC_TMTC); + Boardcore::Event{EV_SEND_HR_TM}, TOPIC_TMTC); send(MAV_HR_TM_ID); @@ -110,10 +112,10 @@ void TMTCController::stateGroundTM(const Event& ev) } case EV_SEND_SENS_TM: { - // LOG_DEBUG(log, "Sending SENS_TM"); + // LOG_DEBUG(printLogger, "Sending SENS_TM"); // periodicSensEvId = // sEventBroker.postDelayed<GROUND_SENS_TM_TIMEOUT>( - // Event{EV_SEND_SENS_TM}, TOPIC_TMTC); + // Boardcore::Event{EV_SEND_SENS_TM}, TOPIC_TMTC); // send tm send(MAV_SENSORS_TM_ID); @@ -148,11 +150,11 @@ void TMTCController::stateFlightTM(const Event& ev) { // add periodic events periodicLrEvId = sEventBroker.postDelayed<LR_TM_TIMEOUT>( - Event{EV_SEND_LR_TM}, TOPIC_TMTC); + Boardcore::Event{EV_SEND_LR_TM}, TOPIC_TMTC); periodicHrEvId = sEventBroker.postDelayed<HR_TM_TIMEOUT>( - Event{EV_SEND_HR_TM}, TOPIC_TMTC); + Boardcore::Event{EV_SEND_HR_TM}, TOPIC_TMTC); - LOG_DEBUG(log, "Entering stateFlightTM"); + LOG_DEBUG(printLogger, "Entering stateFlightTM"); // log stack usage StackLogger::getInstance().updateStack(THID_TMTC_FSM); @@ -165,14 +167,14 @@ void TMTCController::stateFlightTM(const Event& ev) sEventBroker.removeDelayed(periodicLrEvId); sEventBroker.removeDelayed(periodicHrEvId); - LOG_DEBUG(log, "Exiting stateFlightTM"); + LOG_DEBUG(printLogger, "Exiting stateFlightTM"); break; } case EV_SEND_HR_TM: { // repost periodic event periodicHrEvId = sEventBroker.postDelayed<HR_TM_TIMEOUT>( - Event{EV_SEND_HR_TM}, TOPIC_TMTC); + Boardcore::Event{EV_SEND_HR_TM}, TOPIC_TMTC); // send tm once 4 packets are filled send(MAV_HR_TM_ID); @@ -187,7 +189,7 @@ void TMTCController::stateFlightTM(const Event& ev) { // repost periodic event periodicLrEvId = sEventBroker.postDelayed<LR_TM_TIMEOUT>( - Event{EV_SEND_LR_TM}, TOPIC_TMTC); + Boardcore::Event{EV_SEND_LR_TM}, TOPIC_TMTC); // send low rate tm send(MAV_LR_TM_ID); @@ -214,9 +216,9 @@ void TMTCController::stateSensorTM(const Event& ev) { // add periodic events periodicSensEvId = sEventBroker.postDelayed<SENS_TM_TIMEOUT>( - Event{EV_SEND_SENS_TM}, TOPIC_TMTC); + Boardcore::Event{EV_SEND_SENS_TM}, TOPIC_TMTC); - LOG_DEBUG(log, "Entering stateSensorTM"); + LOG_DEBUG(printLogger, "Entering stateSensorTM"); // log stack usage StackLogger::getInstance().updateStack(THID_TMTC_FSM); @@ -227,14 +229,14 @@ void TMTCController::stateSensorTM(const Event& ev) // remove periodic events sEventBroker.removeDelayed(periodicSensEvId); - LOG_DEBUG(log, "Exiting stateSensorTM"); + LOG_DEBUG(printLogger, "Exiting stateSensorTM"); break; } case EV_SEND_SENS_TM: { // repost periodic event periodicSensEvId = sEventBroker.postDelayed<SENS_TM_TIMEOUT>( - Event{EV_SEND_SENS_TM}, TOPIC_TMTC); + Boardcore::Event{EV_SEND_SENS_TM}, TOPIC_TMTC); send(MAV_SENSORS_TM_ID); @@ -265,9 +267,9 @@ void TMTCController::stateSerialDebugTM(const Event& ev) { // add periodic events periodicHrEvId = sEventBroker.postDelayed<HR_TM_GROUND_TIMEOUT>( - Event{EV_SEND_HR_TM_OVER_SERIAL}, TOPIC_TMTC); + Boardcore::Event{EV_SEND_HR_TM_OVER_SERIAL}, TOPIC_TMTC); - LOG_DEBUG(log, "Entering stateSerialDebugTM"); + LOG_DEBUG(printLogger, "Entering stateSerialDebugTM"); // log stack usage StackLogger::getInstance().updateStack(THID_TMTC_FSM); @@ -278,14 +280,14 @@ void TMTCController::stateSerialDebugTM(const Event& ev) // remove periodic events sEventBroker.removeDelayed(periodicHrEvId); - LOG_DEBUG(log, "Exiting stateSerialDebugTM"); + LOG_DEBUG(printLogger, "Exiting stateSerialDebugTM"); break; } case EV_SEND_HR_TM_OVER_SERIAL: { // repost periodic event periodicHrEvId = sEventBroker.postDelayed<HR_TM_GROUND_TIMEOUT>( - Event{EV_SEND_HR_TM_OVER_SERIAL}, TOPIC_TMTC); + Boardcore::Event{EV_SEND_HR_TM_OVER_SERIAL}, TOPIC_TMTC); sendSerialTelemetry(); diff --git a/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.h b/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.h index a1fd9d3cd..c7bcf00a7 100644 --- a/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.h +++ b/src/boards/DeathStack/TelemetriesTelecommands/TMTCController.h @@ -28,8 +28,6 @@ #include <events/Events.h> #include <events/FSM.h> -using namespace Boardcore; - namespace DeathStackBoard { @@ -42,7 +40,7 @@ namespace DeathStackBoard * - receiving and handling commands coming from GS * - fetching the last logged values when sending telemetries */ -class TMTCController : public FSM<TMTCController> +class TMTCController : public Boardcore::FSM<TMTCController> { public: /** @@ -72,10 +70,10 @@ private: void sendSerialTelemetry(); // State handlers - void stateGroundTM(const Event& ev); - void stateSensorTM(const Event& ev); - void stateFlightTM(const Event& ev); - void stateSerialDebugTM(const Event& ev); + void stateGroundTM(const Boardcore::Event& ev); + void stateSensorTM(const Boardcore::Event& ev); + void stateFlightTM(const Boardcore::Event& ev); + void stateSerialDebugTM(const Boardcore::Event& ev); LoggerService& logger = LoggerService::getInstance(); @@ -86,7 +84,8 @@ private: uint8_t hrPktCounter = 0; - PrintLogger log = Logging::getLogger("deathstack.fsm.tmtc"); + Boardcore::PrintLogger printLogger = + Boardcore::Logging::getLogger("deathstack.fsm.tmtc"); }; } // namespace DeathStackBoard diff --git a/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.cpp b/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.cpp index 3028f8037..018552592 100644 --- a/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.cpp +++ b/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.cpp @@ -1,6 +1,5 @@ -/** - * Copyright (c) 2020 Skyward Experimental Rocketry - * Authors: Alvise de' Faveri Tron +/* Copyright (c) 2020 Skyward Experimental Rocketry + * Author: Alvise de' Faveri Tron * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -436,7 +435,7 @@ void TmRepository::update<NASStatus>(const NASStatus& t) template <> void TmRepository::update<NASKalmanState>(const NASKalmanState& t) { - Vector3f orientation = t.toEul(); + Eigen::Vector3f orientation = t.toEul(); tm_repository.nas_tm.x0 = t.x0; tm_repository.nas_tm.x1 = t.x1; @@ -679,7 +678,7 @@ void TmRepository::update<ADAReferenceValues>(const ADAReferenceValues& t) } template <> -void TmRepository::update<TaskStatResult>(const TaskStatResult& t) +void TmRepository::update<TaskStatsResult>(const TaskStatsResult& t) { switch (t.id) { @@ -963,4 +962,4 @@ void TmRepository::update<MockGPSData>(const MockGPSData& t) } #endif -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.h b/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.h index c8141b2ac..971910748 100644 --- a/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.h +++ b/src/boards/DeathStack/TelemetriesTelecommands/TmRepository.h @@ -65,9 +65,9 @@ namespace DeathStackBoard * WARNING: These packets are updated by the LoggerService. If the * LoggerService is not active, the values inside packets WILL NOT BE UPDATED. */ -class TmRepository : public Singleton<TmRepository> +class TmRepository : public Boardcore::Singleton<TmRepository> { - friend class Singleton<TmRepository>; + friend class Boardcore::Singleton<TmRepository>; public: /** @@ -137,7 +137,8 @@ private: FlightStatsRecorder stats_rec; - PrintLogger log = Logging::getLogger("deathstack.tmrepo"); + Boardcore::PrintLogger log = + Boardcore::Logging::getLogger("deathstack.tmrepo"); }; template <> @@ -156,41 +157,49 @@ void TmRepository::update<AirBrakesAlgorithmData>( const AirBrakesAlgorithmData& t); template <> -void TmRepository::update<ADS1118Data>(const ADS1118Data& t); +void TmRepository::update<Boardcore::ADS1118Data>( + const Boardcore::ADS1118Data& t); template <> -void TmRepository::update<MS5803Data>(const MS5803Data& t); +void TmRepository::update<Boardcore::MS5803Data>( + const Boardcore::MS5803Data& t); template <> -void TmRepository::update<MPXHZ6130AData>(const MPXHZ6130AData& t); +void TmRepository::update<Boardcore::MPXHZ6130AData>( + const Boardcore::MPXHZ6130AData& t); template <> -void TmRepository::update<SSCDRRN015PDAData>(const SSCDRRN015PDAData& t); +void TmRepository::update<Boardcore::SSCDRRN015PDAData>( + const Boardcore::SSCDRRN015PDAData& t); template <> void TmRepository::update<AirSpeedPitot>(const AirSpeedPitot& t); template <> -void TmRepository::update<SSCDANN030PAAData>(const SSCDANN030PAAData& t); +void TmRepository::update<Boardcore::SSCDANN030PAAData>( + const Boardcore::SSCDANN030PAAData& t); #if !defined(HARDWARE_IN_THE_LOOP) && !defined(USE_MOCK_SENSORS) template <> -void TmRepository::update<BMX160Data>(const BMX160Data& t); +void TmRepository::update<Boardcore::BMX160Data>( + const Boardcore::BMX160Data& t); template <> -void TmRepository::update<BMX160WithCorrectionData>( - const BMX160WithCorrectionData& t); +void TmRepository::update<Boardcore::BMX160WithCorrectionData>( + const Boardcore::BMX160WithCorrectionData& t); #endif template <> -void TmRepository::update<BMX160Temperature>(const BMX160Temperature& t); +void TmRepository::update<Boardcore::BMX160Temperature>( + const Boardcore::BMX160Temperature& t); template <> -void TmRepository::update<LIS3MDLData>(const LIS3MDLData& t); +void TmRepository::update<Boardcore::LIS3MDLData>( + const Boardcore::LIS3MDLData& t); #if !defined(HARDWARE_IN_THE_LOOP) && !defined(USE_MOCK_SENSORS) template <> -void TmRepository::update<GPSData>(const GPSData& t); +void TmRepository::update<Boardcore::GPSData>(const Boardcore::GPSData& t); #endif template <> @@ -200,18 +209,19 @@ void TmRepository::update<SensorsStatus>(const SensorsStatus& t); * @brief Battery status, sampled by internal ADC. */ template <> -void TmRepository::update<BatteryVoltageSensorData>( - const BatteryVoltageSensorData& t); +void TmRepository::update<Boardcore::BatteryVoltageSensorData>( + const Boardcore::BatteryVoltageSensorData& t); template <> -void TmRepository::update<Xbee::ATCommandResponseFrameLog>( - const Xbee::ATCommandResponseFrameLog& t); +void TmRepository::update<Boardcore::Xbee::ATCommandResponseFrameLog>( + const Boardcore::Xbee::ATCommandResponseFrameLog& t); /** * @brief Logger. */ template <> -void TmRepository::update<LoggerStats>(const LoggerStats& t); +void TmRepository::update<Boardcore::LoggerStats>( + const Boardcore::LoggerStats& t); /** * @brief Initialization status of the board. @@ -250,7 +260,8 @@ void TmRepository::update<PinStatus>(const PinStatus& t); * @brief TMTCController (Mavlink). */ template <> -void TmRepository::update<MavlinkStatus>(const MavlinkStatus& t); +void TmRepository::update<Boardcore::MavlinkStatus>( + const Boardcore::MavlinkStatus& t); /** * @brief Deployment Controller. @@ -297,7 +308,8 @@ void TmRepository::update<SystemData>(const SystemData& t); * @brief Sensor Manager scheduler. */ template <> -void TmRepository::update<TaskStatResult>(const TaskStatResult& t); +void TmRepository::update<Boardcore::TaskStatsResult>( + const Boardcore::TaskStatsResult& t); /** * @brief FlightStatsRecorder liftoff stats. @@ -343,4 +355,4 @@ template <> void TmRepository::update<MockGPSData>(const MockGPSData& t); #endif -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/configs/AirBrakesConfig.h b/src/boards/DeathStack/configs/AirBrakesConfig.h index e01b39474..0e6715818 100644 --- a/src/boards/DeathStack/configs/AirBrakesConfig.h +++ b/src/boards/DeathStack/configs/AirBrakesConfig.h @@ -13,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -31,9 +31,9 @@ namespace DeathStackBoard namespace AirBrakesConfigs { -static TIM_TypeDef* const AB_SERVO_TIMER = TIM8; -static constexpr TimerUtils::Channel AB_SERVO_PWM_CH = - TimerUtils::Channel::CHANNEL_2; +TIM_TypeDef* const AB_SERVO_TIMER = TIM8; +constexpr Boardcore::TimerUtils::Channel AB_SERVO_PWM_CH = + Boardcore::TimerUtils::Channel::CHANNEL_2; // Rocket's parameters #ifdef EUROC @@ -43,7 +43,7 @@ static constexpr float M = 18.362; /**< rocket's mass */ #endif static constexpr float D = 0.15; /**< rocket's diameter */ -static constexpr float S0 = (PI * D * D) / 4.0; +static constexpr float S0 = (Boardcore::PI * D * D) / 4.0; static constexpr float RHO = 1.225; static constexpr float Hn = 10400.0; static constexpr float Co = 340.3; @@ -151,4 +151,4 @@ const Coefficients coeffs; } // namespace AirBrakesConfigs -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/configs/DeploymentConfig.h b/src/boards/DeathStack/configs/DeploymentConfig.h index b2fdaace3..d8674b673 100644 --- a/src/boards/DeathStack/configs/DeploymentConfig.h +++ b/src/boards/DeathStack/configs/DeploymentConfig.h @@ -25,8 +25,6 @@ #include <drivers/timer/PWM.h> #include <drivers/timer/TimerUtils.h> -using namespace Boardcore; - namespace DeathStackBoard { @@ -34,8 +32,8 @@ namespace DeploymentConfigs { static TIM_TypeDef* const DPL_SERVO_TIMER = TIM4; -static constexpr TimerUtils::Channel DPL_SERVO_PWM_CH = - TimerUtils::Channel::CHANNEL_1; +static constexpr Boardcore::TimerUtils::Channel DPL_SERVO_PWM_CH = + Boardcore::TimerUtils::Channel::CHANNEL_1; static constexpr int NC_OPEN_TIMEOUT = 5000; diff --git a/src/boards/DeathStack/configs/FlightStatsConfig.h b/src/boards/DeathStack/configs/FlightStatsConfig.h index 5ad3133d7..760f70eb6 100644 --- a/src/boards/DeathStack/configs/FlightStatsConfig.h +++ b/src/boards/DeathStack/configs/FlightStatsConfig.h @@ -31,8 +31,8 @@ namespace FlightStatsConfig { static constexpr long long TIMEOUT_LIFTOFF_STATS = 7500; // [ms] static constexpr long long TIMEOUT_APOGEE_STATS = 1000; // [ms] -static constexpr long long TIMEOUT_DROGUE_DPL_STATS = 5000; // [ms] +static constexpr long long TIMEOUT_DROGUE_DPL_STATS = 5000; // [ms] static constexpr long long TIMEOUT_MAIN_DPL_STATS = 10000; // [ms] } // namespace FlightStatsConfig -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/configs/MavlinkConfig.h b/src/boards/DeathStack/configs/MavlinkConfig.h index 946497910..c359de84c 100644 --- a/src/boards/DeathStack/configs/MavlinkConfig.h +++ b/src/boards/DeathStack/configs/MavlinkConfig.h @@ -1,5 +1,5 @@ /* Copyright (c) 2021 Skyward Experimental Rocketry - * Authors: Luca Conterio + * Author: Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal diff --git a/src/boards/DeathStack/configs/NASConfig.h b/src/boards/DeathStack/configs/NASConfig.h index 5c8d00f1c..1421402f5 100644 --- a/src/boards/DeathStack/configs/NASConfig.h +++ b/src/boards/DeathStack/configs/NASConfig.h @@ -26,8 +26,6 @@ #include "Eigen/Dense" -using namespace Eigen; - namespace DeathStackBoard { @@ -117,7 +115,7 @@ static const float EMF = 44.24F; // [uT] micro Tesla // Earth magnetic field, used to // check if there's magnetic jamming -static const Vector3f NED_MAG( +static const Eigen::Vector3f NED_MAG( 0.5923F, -0.0175F, 0.8055F); // Normalized magnetic field vector at Ponte de Sor // Measurement units are not important since it's normalized @@ -133,13 +131,13 @@ static const float EMF = 46.77F; // [uT] micro Tesla // Earth magnetic field, used to // check if there's magnetic jamming -static const Vector3f NED_MAG( +static const Eigen::Vector3f NED_MAG( 0.524848, 0.035602, 0.850451); // Normalized magnetic field vector at Roccaraso #endif // normalized magentic field at Milano -// static const Vector3f NED_MAG(0.4742, 0.025, 0.8801); +// static const Eigen::Vector3f NED_MAG(0.4742, 0.025, 0.8801); // DIMENSIONS OF MATRICES AND VECTORS @@ -167,4 +165,4 @@ static const uint16_t NMEKF = 6; // Dimension used in the MEKF. } // namespace NASConfigs -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/configs/PinObserverConfig.h b/src/boards/DeathStack/configs/PinObserverConfig.h index 69918d762..8e8f7cf38 100644 --- a/src/boards/DeathStack/configs/PinObserverConfig.h +++ b/src/boards/DeathStack/configs/PinObserverConfig.h @@ -26,35 +26,34 @@ #include <miosix.h> #include <utils/PinObserver.h> -using namespace Boardcore; - namespace DeathStackBoard { static const unsigned int PIN_POLL_INTERVAL = 10; // ms // Launch pin config -static const GpioPin launch_pin(miosix::inputs::lp_dtch::getPin()); -static const PinObserver::Transition TRIGGER_LAUNCH_PIN = - PinObserver::Transition::FALLING_EDGE; +static const miosix::GpioPin launch_pin(miosix::inputs::lp_dtch::getPin()); +static const Boardcore::PinObserver::Transition TRIGGER_LAUNCH_PIN = + Boardcore::PinObserver::Transition::FALLING_EDGE; // How many consecutive times the launch pin should be detected as detached // before triggering a launch event. static const unsigned int THRESHOLD_LAUNCH_PIN = 10; // Nosecone detach pin config -static const GpioPin nosecone_pin(miosix::inputs::nc_dtch::getPin()); -static const PinObserver::Transition TRIGGER_NC_DETACH_PIN = - PinObserver::Transition::FALLING_EDGE; +static const miosix::GpioPin nosecone_pin(miosix::inputs::nc_dtch::getPin()); +static const Boardcore::PinObserver::Transition TRIGGER_NC_DETACH_PIN = + Boardcore::PinObserver::Transition::FALLING_EDGE; // How many consecutive times the nosecone pin should be detected as detached // before triggering a nosecone detach event. static const unsigned int THRESHOLD_NC_DETACH_PIN = 10; // Dpl servo actuation pin config -static const GpioPin dpl_servo_pin(miosix::inputs::expulsion_in::getPin()); -static const PinObserver::Transition TRIGGER_DPL_SERVO_PIN = - PinObserver::Transition::RISING_EDGE; +static const miosix::GpioPin dpl_servo_pin( + miosix::inputs::expulsion_in::getPin()); +static const Boardcore::PinObserver::Transition TRIGGER_DPL_SERVO_PIN = + Boardcore::PinObserver::Transition::RISING_EDGE; // How many consecutive times the deployment servo pin should be detected as // detached before triggering a nosecone detach event. static const unsigned int THRESHOLD_DPL_SERVO_PIN = 10; -} // namespace DeathStackBoard \ No newline at end of file +} // namespace DeathStackBoard diff --git a/src/boards/DeathStack/configs/SensorsConfig.h b/src/boards/DeathStack/configs/SensorsConfig.h index d0cfd9573..cb21c92a8 100644 --- a/src/boards/DeathStack/configs/SensorsConfig.h +++ b/src/boards/DeathStack/configs/SensorsConfig.h @@ -37,26 +37,38 @@ namespace DeathStackBoard namespace SensorConfigs { static constexpr float INTERNAL_ADC_VREF = 3.3; -static constexpr InternalADC::Channel ADC_BATTERY_VOLTAGE = - InternalADC::Channel::CH5; +static constexpr Boardcore::InternalADC::Channel ADC_BATTERY_VOLTAGE = + Boardcore::InternalADC::Channel::CH5; static constexpr float BATTERY_VOLTAGE_COEFF = 5.98; static constexpr float BATTERY_MIN_SAFE_VOLTAGE = 10.5; // [V] -static constexpr ADS1118::ADS1118Mux ADC_CH_STATIC_PORT = ADS1118::MUX_AIN0_GND; -static constexpr ADS1118::ADS1118Mux ADC_CH_PITOT_PORT = ADS1118::MUX_AIN1_GND; -static constexpr ADS1118::ADS1118Mux ADC_CH_DPL_PORT = ADS1118::MUX_AIN2_GND; -static constexpr ADS1118::ADS1118Mux ADC_CH_VREF = ADS1118::MUX_AIN3_GND; - -static constexpr ADS1118::ADS1118DataRate ADC_DR_STATIC_PORT = ADS1118::DR_860; -static constexpr ADS1118::ADS1118DataRate ADC_DR_PITOT_PORT = ADS1118::DR_860; -static constexpr ADS1118::ADS1118DataRate ADC_DR_DPL_PORT = ADS1118::DR_860; -static constexpr ADS1118::ADS1118DataRate ADC_DR_VREF = ADS1118::DR_860; - -static constexpr ADS1118::ADS1118Pga ADC_PGA_STATIC_PORT = ADS1118::FSR_6_144; -static constexpr ADS1118::ADS1118Pga ADC_PGA_PITOT_PORT = ADS1118::FSR_6_144; -static constexpr ADS1118::ADS1118Pga ADC_PGA_DPL_PORT = ADS1118::FSR_6_144; -static constexpr ADS1118::ADS1118Pga ADC_PGA_VREF = ADS1118::FSR_6_144; +static constexpr Boardcore::ADS1118::ADS1118Mux ADC_CH_STATIC_PORT = + Boardcore::ADS1118::MUX_AIN0_GND; +static constexpr Boardcore::ADS1118::ADS1118Mux ADC_CH_PITOT_PORT = + Boardcore::ADS1118::MUX_AIN1_GND; +static constexpr Boardcore::ADS1118::ADS1118Mux ADC_CH_DPL_PORT = + Boardcore::ADS1118::MUX_AIN2_GND; +static constexpr Boardcore::ADS1118::ADS1118Mux ADC_CH_VREF = + Boardcore::ADS1118::MUX_AIN3_GND; + +static constexpr Boardcore::ADS1118::ADS1118DataRate ADC_DR_STATIC_PORT = + Boardcore::ADS1118::DR_860; +static constexpr Boardcore::ADS1118::ADS1118DataRate ADC_DR_PITOT_PORT = + Boardcore::ADS1118::DR_860; +static constexpr Boardcore::ADS1118::ADS1118DataRate ADC_DR_DPL_PORT = + Boardcore::ADS1118::DR_860; +static constexpr Boardcore::ADS1118::ADS1118DataRate ADC_DR_VREF = + Boardcore::ADS1118::DR_860; + +static constexpr Boardcore::ADS1118::ADS1118Pga ADC_PGA_STATIC_PORT = + Boardcore::ADS1118::FSR_6_144; +static constexpr Boardcore::ADS1118::ADS1118Pga ADC_PGA_PITOT_PORT = + Boardcore::ADS1118::FSR_6_144; +static constexpr Boardcore::ADS1118::ADS1118Pga ADC_PGA_DPL_PORT = + Boardcore::ADS1118::FSR_6_144; +static constexpr Boardcore::ADS1118::ADS1118Pga ADC_PGA_VREF = + Boardcore::ADS1118::FSR_6_144; // Sampling periods in milliseconds static constexpr unsigned int SAMPLE_PERIOD_INTERNAL_ADC = @@ -77,17 +89,20 @@ static constexpr unsigned int PRESS_PITOT_CALIB_SAMPLES_NUM = 500; static constexpr unsigned int PRESS_STATIC_CALIB_SAMPLES_NUM = 500; static constexpr float PRESS_STATIC_MOVING_AVG_COEFF = 0.95; -static constexpr BMX160Config::AccelerometerRange IMU_BMX_ACC_FULLSCALE_ENUM = - BMX160Config::AccelerometerRange::G_16; -static constexpr BMX160Config::GyroscopeRange IMU_BMX_GYRO_FULLSCALE_ENUM = - BMX160Config::GyroscopeRange::DEG_1000; +static constexpr Boardcore::BMX160Config::AccelerometerRange + IMU_BMX_ACC_FULLSCALE_ENUM = + Boardcore::BMX160Config::AccelerometerRange::G_16; +static constexpr Boardcore::BMX160Config::GyroscopeRange + IMU_BMX_GYRO_FULLSCALE_ENUM = + Boardcore::BMX160Config::GyroscopeRange::DEG_1000; static constexpr unsigned int IMU_BMX_ACC_GYRO_ODR = 1600; -static constexpr BMX160Config::OutputDataRate IMU_BMX_ACC_GYRO_ODR_ENUM = - BMX160Config::OutputDataRate::HZ_1600; +static constexpr Boardcore::BMX160Config::OutputDataRate + IMU_BMX_ACC_GYRO_ODR_ENUM = + Boardcore::BMX160Config::OutputDataRate::HZ_1600; static constexpr unsigned int IMU_BMX_MAG_ODR = 100; -static constexpr BMX160Config::OutputDataRate IMU_BMX_MAG_ODR_ENUM = - BMX160Config::OutputDataRate::HZ_100; +static constexpr Boardcore::BMX160Config::OutputDataRate IMU_BMX_MAG_ODR_ENUM = + Boardcore::BMX160Config::OutputDataRate::HZ_100; static constexpr unsigned int IMU_BMX_FIFO_HEADER_SIZE = 1; static constexpr unsigned int IMU_BMX_ACC_DATA_SIZE = 6; @@ -115,15 +130,17 @@ static constexpr unsigned int SAMPLE_PERIOD_IMU_BMX = static constexpr unsigned int IMU_BMX_TEMP_DIVIDER = 1; // IMU axis rotation -static const AxisOrthoOrientation BMX160_AXIS_ROTATION = { - Direction::NEGATIVE_Z, Direction::POSITIVE_Y}; +static const Boardcore::AxisOrthoOrientation BMX160_AXIS_ROTATION = { + Boardcore::Direction::NEGATIVE_Z, Boardcore::Direction::POSITIVE_Y}; static constexpr char BMX160_CORRECTION_PARAMETERS_FILE[30] = "/sd/bmx160_params.csv"; -static constexpr unsigned int SAMPLE_PERIOD_MAG_LIS = 15; -static constexpr LIS3MDL::ODR MAG_LIS_ODR_ENUM = LIS3MDL::ODR_80_HZ; -static constexpr LIS3MDL::FullScale MAG_LIS_FULLSCALE = LIS3MDL::FS_4_GAUSS; +static constexpr unsigned int SAMPLE_PERIOD_MAG_LIS = 15; +static constexpr Boardcore::LIS3MDL::ODR MAG_LIS_ODR_ENUM = + Boardcore::LIS3MDL::ODR_80_HZ; +static constexpr Boardcore::LIS3MDL::FullScale MAG_LIS_FULLSCALE = + Boardcore::LIS3MDL::FS_4_GAUSS; static constexpr unsigned int GPS_SAMPLE_RATE = 25; static constexpr unsigned int GPS_SAMPLE_PERIOD = 1000 / GPS_SAMPLE_RATE; diff --git a/src/boards/DeathStack/configs/config.h b/src/boards/DeathStack/configs/config.h index 5f6e46777..a420d483f 100644 --- a/src/boards/DeathStack/configs/config.h +++ b/src/boards/DeathStack/configs/config.h @@ -1,6 +1,5 @@ -/** - * Copyright (c) 2019 Skyward Experimental Rocketry - * Authors: Luca Erbetta +/* Copyright (c) 2019 Skyward Experimental Rocketry + * Author: Luca Erbetta * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -30,8 +29,8 @@ static constexpr unsigned int DEFERRED_EVENTS_QUEUE_SIZE = 100; static const float DEFAULT_REFERENCE_ALTITUDE = 160.0f; static const float DEFAULT_REFERENCE_PRESSURE = 100022.4f; #else -static const float DEFAULT_REFERENCE_ALTITUDE = 1420.0f; -static const float DEFAULT_REFERENCE_PRESSURE = 85389.4f; +static const float DEFAULT_REFERENCE_ALTITUDE = 1420.0f; +static const float DEFAULT_REFERENCE_PRESSURE = 85389.4f; #endif static const float DEFAULT_REFERENCE_TEMPERATURE = 288.15f; diff --git a/src/boards/Payload/Main/Bus.h b/src/boards/Payload/Main/Bus.h index dc9bfdbe9..7a865c150 100644 --- a/src/boards/Payload/Main/Bus.h +++ b/src/boards/Payload/Main/Bus.h @@ -25,15 +25,13 @@ #include <drivers/spi/SPIBus.h> #include <miosix.h> -using namespace Boardcore; - namespace PayloadBoard { struct Bus { - SPIBusInterface* spi1 = new SPIBus(SPI1); - SPIBusInterface* spi2 = new SPIBus(SPI2); + Boardcore::SPIBusInterface* spi1 = new Boardcore::SPIBus(SPI1); + Boardcore::SPIBusInterface* spi2 = new Boardcore::SPIBus(SPI2); }; -} // namespace PayloadBoard \ No newline at end of file +} // namespace PayloadBoard diff --git a/src/boards/Payload/Main/Radio.h b/src/boards/Payload/Main/Radio.h index db349d628..ae67a56c6 100644 --- a/src/boards/Payload/Main/Radio.h +++ b/src/boards/Payload/Main/Radio.h @@ -25,20 +25,18 @@ //#include <TelemetriesTelecommands/Mavlink.h> #include <drivers/Xbee/Xbee.h> -using namespace Boardcore; - namespace PayloadBoard { class Radio { public: - //TMTCController* tmtc_manager; - //TmRepository* tm_repo; - Xbee::Xbee* xbee; - //MavDriver* mav_driver; + // TMTCController* tmtc_manager; + // TmRepository* tm_repo; + Boardcore::Xbee::Xbee* xbee; + // MavDriver* mav_driver; - Radio(SPIBusInterface& xbee_bus_); + Radio(Boardcore::SPIBusInterface& xbee_bus_); ~Radio(); bool start(); @@ -46,9 +44,9 @@ public: void logStatus(); private: - void onXbeeFrameReceived(Xbee::APIFrame& frame); + void onXbeeFrameReceived(Boardcore::Xbee::APIFrame& frame); - SPIBusInterface& xbee_bus; + Boardcore::SPIBusInterface& xbee_bus; }; -} // namespace PayloadBoard \ No newline at end of file +} // namespace PayloadBoard diff --git a/src/boards/Payload/Main/Sensors.cpp b/src/boards/Payload/Main/Sensors.cpp index 695403d72..6460aefdd 100644 --- a/src/boards/Payload/Main/Sensors.cpp +++ b/src/boards/Payload/Main/Sensors.cpp @@ -37,6 +37,7 @@ using std::bind; using std::function; using namespace Boardcore; +using namespace PayloadBoard::SensorConfigs; // BMX160 Watermark interrupt void __attribute__((used)) EXTI5_IRQHandlerImpl() @@ -402,7 +403,7 @@ void Sensors::ADS1118Callback() void Sensors::pressPitotCallback() { - SSCDRRN015PDAData d = press_pitot->getLastSample(); + // SSCDRRN015PDAData d = press_pitot->getLastSample(); // LoggerService::getInstance().log(d); /*ADAReferenceValues rv = @@ -433,15 +434,15 @@ void Sensors::pressStaticCallback() void Sensors::imuBMXCallback() { - uint8_t fifo_size = imu_bmx160->getLastFifoSize(); - auto& fifo = imu_bmx160->getLastFifo(); + // uint8_t fifo_size = imu_bmx160->getLastFifoSize(); + // auto& fifo = imu_bmx160->getLastFifo(); // LoggerService::getInstance().log(imu_bmx160->getTemperature()); - for (uint8_t i = 0; i < fifo_size; ++i) - { - // LoggerService::getInstance().log(fifo.at(i)); - } + // for (uint8_t i = 0; i < fifo_size; ++i) + // { + // LoggerService::getInstance().log(fifo.at(i)); + // } // LoggerService::getInstance().log(imu_bmx160->getFifoStats()); } @@ -495,4 +496,4 @@ void Sensors::updateSensorsStatus() } } -} // namespace PayloadBoard \ No newline at end of file +} // namespace PayloadBoard diff --git a/src/boards/Payload/Main/Sensors.h b/src/boards/Payload/Main/Sensors.h index 0263065e3..98118dfa7 100644 --- a/src/boards/Payload/Main/Sensors.h +++ b/src/boards/Payload/Main/Sensors.h @@ -42,8 +42,6 @@ #include <map> -using namespace Boardcore; - namespace PayloadBoard { @@ -54,24 +52,25 @@ namespace PayloadBoard class Sensors { public: - SensorManager* sensor_manager = nullptr; + Boardcore::SensorManager* sensor_manager = nullptr; - InternalADC* internal_adc = nullptr; - BatteryVoltageSensor* battery_voltage = nullptr; + Boardcore::InternalADC* internal_adc = nullptr; + Boardcore::BatteryVoltageSensor* battery_voltage = nullptr; - MS5803* press_digital = nullptr; + Boardcore::MS5803* press_digital = nullptr; - ADS1118* adc_ads1118 = nullptr; - SSCDANN030PAA* press_dpl_vane = nullptr; - MPXHZ6130A* press_static_port = nullptr; - SSCDRRN015PDA* press_pitot = nullptr; + Boardcore::ADS1118* adc_ads1118 = nullptr; + Boardcore::SSCDANN030PAA* press_dpl_vane = nullptr; + Boardcore::MPXHZ6130A* press_static_port = nullptr; + Boardcore::SSCDRRN015PDA* press_pitot = nullptr; - BMX160* imu_bmx160 = nullptr; - BMX160WithCorrection* imu_bmx160_with_correction = nullptr; - LIS3MDL* mag_lis3mdl = nullptr; - UbloxGPS* gps_ublox = nullptr; + Boardcore::BMX160* imu_bmx160 = nullptr; + Boardcore::BMX160WithCorrection* imu_bmx160_with_correction = nullptr; + Boardcore::LIS3MDL* mag_lis3mdl = nullptr; + Boardcore::UbloxGPS* gps_ublox = nullptr; - Sensors(SPIBusInterface& spi1_bus_, TaskScheduler* scheduler); + Sensors(Boardcore::SPIBusInterface& spi1_bus_, + Boardcore::TaskScheduler* scheduler); ~Sensors(); @@ -115,15 +114,15 @@ private: void updateSensorsStatus(); - SPIBusInterface& spi1_bus; + Boardcore::SPIBusInterface& spi1_bus; - SensorManager::SensorMap_t sensors_map; + Boardcore::SensorManager::SensorMap_t sensors_map; SensorsStatus status; - PrintLogger log = Logging::getLogger("sensors"); + Boardcore::PrintLogger log = Boardcore::Logging::getLogger("sensors"); unsigned int battery_critical_counter = 0; }; -} // namespace PayloadBoard \ No newline at end of file +} // namespace PayloadBoard diff --git a/src/boards/Payload/Main/SensorsData.h b/src/boards/Payload/Main/SensorsData.h index c9751d386..c86847fda 100644 --- a/src/boards/Payload/Main/SensorsData.h +++ b/src/boards/Payload/Main/SensorsData.h @@ -1,6 +1,5 @@ -/** - * Copyright (c) 2021 Skyward Experimental Rocketry - * Authors: Luca Conterio +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -67,4 +66,4 @@ struct SensorsStatus } }; -} // namespace PayloadBoard \ No newline at end of file +} // namespace PayloadBoard diff --git a/src/boards/Payload/PayloadBoard.h b/src/boards/Payload/PayloadBoard.h index dc591cf3b..a8e87f1cf 100644 --- a/src/boards/Payload/PayloadBoard.h +++ b/src/boards/Payload/PayloadBoard.h @@ -44,7 +44,6 @@ #include <vector> using std::bind; -using namespace Boardcore; namespace PayloadBoard { @@ -57,16 +56,16 @@ static const std::vector<uint8_t> TRACE_EVENT_BLACKLIST{ * This file provides a simplified way to initialize and monitor all * the components of the Payload. */ -class Payload : public Singleton<Payload> +class Payload : public Boardcore::Singleton<Payload> { - friend class Singleton<Payload>; + friend class Boardcore::Singleton<Payload>; public: // Shared Components - EventBroker* broker; + Boardcore::EventBroker* broker; // LoggerService* logger; - EventSniffer* sniffer; + Boardcore::EventSniffer* sniffer; // StateMachines* state_machines; Bus* bus; @@ -76,7 +75,7 @@ public: PinHandler* pin_handler; - TaskScheduler* scheduler; + Boardcore::TaskScheduler* scheduler; void start() { @@ -120,12 +119,14 @@ public: if (status.payload_board != COMP_OK) { LOG_ERR(log, "Initalization failed\n"); - sEventBroker.post(Event{EV_INIT_ERROR}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post(Boardcore::Event{EV_INIT_ERROR}, + TOPIC_FLIGHT_EVENTS); } else { LOG_INFO(log, "Initalization ok"); - sEventBroker.post(Event{EV_INIT_OK}, TOPIC_FLIGHT_EVENTS); + sEventBroker.post(Boardcore::Event{EV_INIT_OK}, + TOPIC_FLIGHT_EVENTS); } } @@ -165,7 +166,7 @@ private: *broker, TOPIC_LIST, bind(&Payload::logEvent, this, _1, _2)); }*/ - scheduler = new TaskScheduler(); + scheduler = new Boardcore::TaskScheduler(); bus = new Bus(); radio = new Radio(*bus->spi2); @@ -175,7 +176,7 @@ private: pin_handler = new PinHandler(); #ifdef DEBUG - injector = new EventInjector(); + injector = new Boardcore::EventInjector(); #endif LOG_INFO(log, "Init finished"); @@ -199,22 +200,24 @@ event, topic}; logger->log(ev); return; } } - LOG_DEBUG(log, "{:s} on {:s}", getEventString(event), - getTopicString(topic)); -#endif + LOG_DEBUG(log, "{:s} on {:s}", +getEventString(event), getTopicString(topic)); #endif }*/ - inline void postEvent(Event ev, uint8_t topic) { broker->post(ev, topic); } + inline void postEvent(Boardcore::Event ev, uint8_t topic) + { + broker->post(ev, topic); + } /*void addSchedulerStatsTask() { // add lambda to log scheduler tasks statistics scheduler.add( [&]() { - std::vector<TaskStatResult> scheduler_stats = + std::vector<TaskStatsResult> scheduler_stats = scheduler.getTaskStats(); - for (TaskStatResult stat : scheduler_stats) + for (TaskStatsResult stat : scheduler_stats) { logger->log(stat); } @@ -225,10 +228,10 @@ event, topic}; logger->log(ev); TASK_SCHEDULER_STATS_ID, miosix::getTick()); }*/ - EventInjector* injector; + Boardcore::EventInjector* injector; PayloadStatus status{}; - PrintLogger log = Logging::getLogger("Payload"); + Boardcore::PrintLogger log = Boardcore::Logging::getLogger("Payload"); }; } // namespace PayloadBoard diff --git a/src/boards/Payload/PayloadStatus.h b/src/boards/Payload/PayloadStatus.h index 057b8ddee..c8e6e516f 100644 --- a/src/boards/Payload/PayloadStatus.h +++ b/src/boards/Payload/PayloadStatus.h @@ -38,7 +38,7 @@ enum PayloadComponentStatus struct PayloadStatus { // Logic OR of all components errors. - uint8_t payload_board = COMP_OK; + uint8_t payload_board = COMP_OK; uint8_t logger = COMP_OK; uint8_t ev_broker = COMP_OK; @@ -72,4 +72,4 @@ struct PayloadStatus } }; -} // namespace PayloadBoard \ No newline at end of file +} // namespace PayloadBoard diff --git a/src/boards/Payload/PinHandler/PinHandler.h b/src/boards/Payload/PinHandler/PinHandler.h index fdc081e0b..c7abde008 100644 --- a/src/boards/Payload/PinHandler/PinHandler.h +++ b/src/boards/Payload/PinHandler/PinHandler.h @@ -27,15 +27,13 @@ #include <diagnostic/PrintLogger.h> #include <utils/PinObserver.h> -using namespace Boardcore; - namespace PayloadBoard { /** * @brief Forward dec. */ -//class LoggerService; +// class LoggerService; /** * @brief This class contains the handlers for both the launch pin (umbilical) @@ -69,16 +67,17 @@ public: * @param n */ void onNCPinTransition(unsigned int p, unsigned char n); - + void onNCPinStateChange(unsigned int p, unsigned char n, int state); private: PinStatus status_pin_nosecone{ObservedPin::NOSECONE}; - PinObserver pin_obs; + Boardcore::PinObserver pin_obs; - //LoggerService* logger; - PrintLogger log = Logging::getLogger("deathstack.pinhandler"); + // LoggerService* logger; + Boardcore::PrintLogger log = + Boardcore::Logging::getLogger("deathstack.pinhandler"); }; -} // namespace PayloadBoard \ No newline at end of file +} // namespace PayloadBoard diff --git a/src/boards/Payload/PinHandler/PinHandlerData.h b/src/boards/Payload/PinHandler/PinHandlerData.h index e3c0328df..b794ba55d 100644 --- a/src/boards/Payload/PinHandler/PinHandlerData.h +++ b/src/boards/Payload/PinHandler/PinHandlerData.h @@ -30,7 +30,7 @@ namespace PayloadBoard enum class ObservedPin : uint8_t { - NOSECONE = 0 + NOSECONE = 0 }; /** diff --git a/src/boards/Payload/WingControl/WingServo.cpp b/src/boards/Payload/WingControl/WingServo.cpp index 16f534eab..7146eafa4 100644 --- a/src/boards/Payload/WingControl/WingServo.cpp +++ b/src/boards/Payload/WingControl/WingServo.cpp @@ -13,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -22,11 +22,12 @@ #include <Payload/WingControl/WingServo.h> +using namespace Boardcore; +using namespace PayloadBoard::WingConfigs; + namespace PayloadBoard { -using namespace WingConfigs; - WingServo::WingServo(TIM_TypeDef* const timer, TimerUtils::Channel channel, float minPosition, float maxPosition) : ServoInterface(minPosition, maxPosition), @@ -67,4 +68,4 @@ void WingServo::setPosition(float angle) servo.setPosition180Deg(angle); } -} // namespace PayloadBoard \ No newline at end of file +} // namespace PayloadBoard diff --git a/src/boards/Payload/WingControl/WingServo.h b/src/boards/Payload/WingControl/WingServo.h index 4c42ddd28..19edecee7 100644 --- a/src/boards/Payload/WingControl/WingServo.h +++ b/src/boards/Payload/WingControl/WingServo.h @@ -27,8 +27,6 @@ #include <drivers/servo/Servo.h> #include <miosix.h> -using namespace Boardcore; - namespace PayloadBoard { @@ -36,7 +34,7 @@ class WingServo : public ServoInterface { public: - WingServo(TIM_TypeDef* const timer, TimerUtils::Channel channel, + WingServo(TIM_TypeDef* const timer, Boardcore::TimerUtils::Channel channel, float minPosition = WingConfigs::WING_SERVO_MIN_POS, float maxPosition = WingConfigs::WING_SERVO_MAX_POS); @@ -52,7 +50,7 @@ public: void selfTest() override; private: - Servo servo; + Boardcore::Servo servo; protected: /** @@ -63,4 +61,4 @@ protected: void setPosition(float angle) override; }; -} // namespace PayloadBoard \ No newline at end of file +} // namespace PayloadBoard diff --git a/src/boards/Payload/configs/PinHandlerConfig.h b/src/boards/Payload/configs/PinHandlerConfig.h index 76588cbbb..1f00ffe75 100644 --- a/src/boards/Payload/configs/PinHandlerConfig.h +++ b/src/boards/Payload/configs/PinHandlerConfig.h @@ -26,8 +26,6 @@ #include <miosix.h> #include <utils/PinObserver.h> -using namespace Boardcore; - namespace PayloadBoard { @@ -43,8 +41,8 @@ static const unsigned int PIN_POLL_INTERVAL = 10; // ms // Nosecone detach pin config static const GpioPin nosecone_pin(miosix::inputs::nc_dtch::getPin()); -static const PinObserver::Transition TRIGGER_NC_DETACH_PIN = - PinObserver::Transition::FALLING_EDGE; +static const Boardcore::PinObserver::Transition TRIGGER_NC_DETACH_PIN = + Boardcore::PinObserver::Transition::FALLING_EDGE; // How many consecutive times the nosecone pin should be detected as detached // before triggering a nosecone detach event. static const unsigned int THRESHOLD_NC_DETACH_PIN = 10; @@ -57,4 +55,4 @@ static const unsigned int THRESHOLD_NC_DETACH_PIN = 10; // // detached before triggering a nosecone detach event. // static const unsigned int THRESHOLD_DPL_SERVO_PIN = 10; -} // namespace PayloadBoard \ No newline at end of file +} // namespace PayloadBoard diff --git a/src/boards/Payload/configs/SensorsConfig.h b/src/boards/Payload/configs/SensorsConfig.h index 14e657ad6..e00655539 100644 --- a/src/boards/Payload/configs/SensorsConfig.h +++ b/src/boards/Payload/configs/SensorsConfig.h @@ -38,26 +38,38 @@ namespace SensorConfigs { static constexpr float INTERNAL_ADC_VREF = 3.3; -static constexpr InternalADC::Channel ADC_BATTERY_VOLTAGE = - InternalADC::Channel::CH5; +static constexpr Boardcore::InternalADC::Channel ADC_BATTERY_VOLTAGE = + Boardcore::InternalADC::Channel::CH5; static constexpr float BATTERY_VOLTAGE_COEFF = 5.98; static constexpr float BATTERY_MIN_SAFE_VOLTAGE = 10.5; // [V] -static constexpr ADS1118::ADS1118Mux ADC_CH_STATIC_PORT = ADS1118::MUX_AIN0_GND; -static constexpr ADS1118::ADS1118Mux ADC_CH_PITOT_PORT = ADS1118::MUX_AIN1_GND; -static constexpr ADS1118::ADS1118Mux ADC_CH_DPL_PORT = ADS1118::MUX_AIN2_GND; -static constexpr ADS1118::ADS1118Mux ADC_CH_VREF = ADS1118::MUX_AIN3_GND; - -static constexpr ADS1118::ADS1118DataRate ADC_DR_STATIC_PORT = ADS1118::DR_860; -static constexpr ADS1118::ADS1118DataRate ADC_DR_PITOT_PORT = ADS1118::DR_860; -static constexpr ADS1118::ADS1118DataRate ADC_DR_DPL_PORT = ADS1118::DR_860; -static constexpr ADS1118::ADS1118DataRate ADC_DR_VREF = ADS1118::DR_860; - -static constexpr ADS1118::ADS1118Pga ADC_PGA_STATIC_PORT = ADS1118::FSR_6_144; -static constexpr ADS1118::ADS1118Pga ADC_PGA_PITOT_PORT = ADS1118::FSR_6_144; -static constexpr ADS1118::ADS1118Pga ADC_PGA_DPL_PORT = ADS1118::FSR_6_144; -static constexpr ADS1118::ADS1118Pga ADC_PGA_VREF = ADS1118::FSR_6_144; +static constexpr Boardcore::ADS1118::ADS1118Mux ADC_CH_STATIC_PORT = + Boardcore::ADS1118::MUX_AIN0_GND; +static constexpr Boardcore::ADS1118::ADS1118Mux ADC_CH_PITOT_PORT = + Boardcore::ADS1118::MUX_AIN1_GND; +static constexpr Boardcore::ADS1118::ADS1118Mux ADC_CH_DPL_PORT = + Boardcore::ADS1118::MUX_AIN2_GND; +static constexpr Boardcore::ADS1118::ADS1118Mux ADC_CH_VREF = + Boardcore::ADS1118::MUX_AIN3_GND; + +static constexpr Boardcore::ADS1118::ADS1118DataRate ADC_DR_STATIC_PORT = + Boardcore::ADS1118::DR_860; +static constexpr Boardcore::ADS1118::ADS1118DataRate ADC_DR_PITOT_PORT = + Boardcore::ADS1118::DR_860; +static constexpr Boardcore::ADS1118::ADS1118DataRate ADC_DR_DPL_PORT = + Boardcore::ADS1118::DR_860; +static constexpr Boardcore::ADS1118::ADS1118DataRate ADC_DR_VREF = + Boardcore::ADS1118::DR_860; + +static constexpr Boardcore::ADS1118::ADS1118Pga ADC_PGA_STATIC_PORT = + Boardcore::ADS1118::FSR_6_144; +static constexpr Boardcore::ADS1118::ADS1118Pga ADC_PGA_PITOT_PORT = + Boardcore::ADS1118::FSR_6_144; +static constexpr Boardcore::ADS1118::ADS1118Pga ADC_PGA_DPL_PORT = + Boardcore::ADS1118::FSR_6_144; +static constexpr Boardcore::ADS1118::ADS1118Pga ADC_PGA_VREF = + Boardcore::ADS1118::FSR_6_144; // Sampling periods in milliseconds static constexpr unsigned int SAMPLE_PERIOD_INTERNAL_ADC = @@ -78,17 +90,20 @@ static constexpr unsigned int PRESS_PITOT_CALIB_SAMPLES_NUM = 500; static constexpr unsigned int PRESS_STATIC_CALIB_SAMPLES_NUM = 500; static constexpr float PRESS_STATIC_MOVING_AVG_COEFF = 0.95; -static constexpr BMX160Config::AccelerometerRange IMU_BMX_ACC_FULLSCALE_ENUM = - BMX160Config::AccelerometerRange::G_16; -static constexpr BMX160Config::GyroscopeRange IMU_BMX_GYRO_FULLSCALE_ENUM = - BMX160Config::GyroscopeRange::DEG_1000; +static constexpr Boardcore::BMX160Config::AccelerometerRange + IMU_BMX_ACC_FULLSCALE_ENUM = + Boardcore::BMX160Config::AccelerometerRange::G_16; +static constexpr Boardcore::BMX160Config::GyroscopeRange + IMU_BMX_GYRO_FULLSCALE_ENUM = + Boardcore::BMX160Config::GyroscopeRange::DEG_1000; static constexpr unsigned int IMU_BMX_ACC_GYRO_ODR = 1600; -static constexpr BMX160Config::OutputDataRate IMU_BMX_ACC_GYRO_ODR_ENUM = - BMX160Config::OutputDataRate::HZ_1600; +static constexpr Boardcore::BMX160Config::OutputDataRate + IMU_BMX_ACC_GYRO_ODR_ENUM = + Boardcore::BMX160Config::OutputDataRate::HZ_1600; static constexpr unsigned int IMU_BMX_MAG_ODR = 100; -static constexpr BMX160Config::OutputDataRate IMU_BMX_MAG_ODR_ENUM = - BMX160Config::OutputDataRate::HZ_100; +static constexpr Boardcore::BMX160Config::OutputDataRate IMU_BMX_MAG_ODR_ENUM = + Boardcore::BMX160Config::OutputDataRate::HZ_100; static constexpr unsigned int IMU_BMX_FIFO_HEADER_SIZE = 1; static constexpr unsigned int IMU_BMX_ACC_DATA_SIZE = 6; @@ -116,15 +131,17 @@ static constexpr unsigned int SAMPLE_PERIOD_IMU_BMX = static constexpr unsigned int IMU_BMX_TEMP_DIVIDER = 1; // IMU axis rotation -static const AxisOrthoOrientation BMX160_AXIS_ROTATION = { - Direction::NEGATIVE_Z, Direction::POSITIVE_Y}; +static const Boardcore::AxisOrthoOrientation BMX160_AXIS_ROTATION = { + Boardcore::Direction::NEGATIVE_Z, Boardcore::Direction::POSITIVE_Y}; static constexpr char BMX160_CORRECTION_PARAMETERS_FILE[30] = "/sd/bmx160_params.csv"; -static constexpr unsigned int SAMPLE_PERIOD_MAG_LIS = 15; -static constexpr LIS3MDL::ODR MAG_LIS_ODR_ENUM = LIS3MDL::ODR_80_HZ; -static constexpr LIS3MDL::FullScale MAG_LIS_FULLSCALE = LIS3MDL::FS_4_GAUSS; +static constexpr unsigned int SAMPLE_PERIOD_MAG_LIS = 15; +static constexpr Boardcore::LIS3MDL::ODR MAG_LIS_ODR_ENUM = + Boardcore::LIS3MDL::ODR_80_HZ; +static constexpr Boardcore::LIS3MDL::FullScale MAG_LIS_FULLSCALE = + Boardcore::LIS3MDL::FS_4_GAUSS; static constexpr unsigned int GPS_SAMPLE_RATE = 25; static constexpr unsigned int GPS_SAMPLE_PERIOD = 1000 / GPS_SAMPLE_RATE; diff --git a/src/boards/Payload/configs/WingConfig.h b/src/boards/Payload/configs/WingConfig.h index 1665891f9..18e2f1361 100644 --- a/src/boards/Payload/configs/WingConfig.h +++ b/src/boards/Payload/configs/WingConfig.h @@ -13,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -26,8 +26,6 @@ #include <drivers/timer/TimestampTimer.h> #include <utils/Constants.h> -using namespace Boardcore; - namespace PayloadBoard { @@ -35,12 +33,12 @@ namespace WingConfigs { static TIM_TypeDef* const WING_SERVO_1_TIMER = TIM8; -static constexpr TimerUtils::Channel WING_SERVO_1_PWM_CH = - TimerUtils::Channel::CHANNEL_2; +static constexpr Boardcore::TimerUtils::Channel WING_SERVO_1_PWM_CH = + Boardcore::TimerUtils::Channel::CHANNEL_2; static const TIM_TypeDef* WING_SERVO_2_TIMER = TIM4; -static constexpr TimerUtils::Channel WING_SERVO_2_PWM_CH = - TimerUtils::Channel::CHANNEL_1; +static constexpr Boardcore::TimerUtils::Channel WING_SERVO_2_PWM_CH = + Boardcore::TimerUtils::Channel::CHANNEL_1; // Wing servo configs static constexpr float WING_SERVO_MAX_POS = 180.0; // deg @@ -50,4 +48,4 @@ static constexpr float WING_SERVO_WIGGLE_AMPLITUDE = 20.0; // deg } // namespace WingConfigs -} // namespace PayloadBoard \ No newline at end of file +} // namespace PayloadBoard diff --git a/src/common/CanInterfaces.h b/src/common/CanInterfaces.h index b994b38e6..7e19b9ff5 100644 --- a/src/common/CanInterfaces.h +++ b/src/common/CanInterfaces.h @@ -1,5 +1,5 @@ /* Copyright (c) 2015-2018 Skyward Experimental Rocketry - * Authors: Alvise de' Faveri Tron + * Author: Alvise de' Faveri Tron * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -13,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN diff --git a/src/common/events/Events.h b/src/common/events/Events.h index 67abf6b09..6a1b48996 100644 --- a/src/common/events/Events.h +++ b/src/common/events/Events.h @@ -38,16 +38,13 @@ #include "events/Event.h" #include "events/EventBroker.h" -using std::string; -using namespace Boardcore; - /** * Definition of all events in the Board software. * Refer to the Software Design Document. */ enum Events : uint8_t { - EV_ADA_APOGEE_DETECTED = EV_FIRST_SIGNAL, + EV_ADA_APOGEE_DETECTED = Boardcore::EV_FIRST_SIGNAL, EV_ADA_DISABLE_ABK, EV_ADA_DPL_ALT_DETECTED, EV_ADA_READY, @@ -182,4 +179,4 @@ const std::vector<uint8_t> EVENT_LIST{ * @param event * @return string */ -string getEventString(uint8_t event); \ No newline at end of file +std::string getEventString(uint8_t event); diff --git a/src/entrypoints/death-stack-x-decoder/LogTypes.h b/src/entrypoints/death-stack-x-decoder/LogTypes.h index facc537a7..6f2a8c28a 100644 --- a/src/entrypoints/death-stack-x-decoder/LogTypes.h +++ b/src/entrypoints/death-stack-x-decoder/LogTypes.h @@ -62,8 +62,6 @@ // Serialized classes using std::ofstream; -using namespace DeathStackBoard; - template <typename T> void print(T& t, ostream& os) { @@ -79,7 +77,7 @@ void registerType(Deserializer& ds) void registerTypes(Deserializer& ds) { // Disagnostic - registerType<TaskStatResult>(ds); + registerType<TaskStatsResult>(ds); registerType<StackData>(ds); registerType<LoggingString>(ds); registerType<SystemData>(ds); diff --git a/src/entrypoints/death-stack-x-entry.cpp b/src/entrypoints/death-stack-x-entry.cpp index 428b7d8f3..e368fe1fc 100644 --- a/src/entrypoints/death-stack-x-entry.cpp +++ b/src/entrypoints/death-stack-x-entry.cpp @@ -27,6 +27,7 @@ #include <miosix.h> using namespace miosix; +using namespace Boardcore; using namespace DeathStackBoard; // using namespace GlobalBuffers; @@ -56,7 +57,7 @@ int main() StackLogger::getInstance().updateStack(THID_ENTRYPOINT); - system_data.timestamp = miosix::getTick(); + system_data.timestamp = getTick(); system_data.cpu_usage = averageCpuUtilization(); cpu_stat.add(system_data.cpu_usage); @@ -72,4 +73,4 @@ int main() StackLogger::getInstance().log(); } -} \ No newline at end of file +} diff --git a/src/entrypoints/deserializer/logdecoder.cpp b/src/entrypoints/deserializer/logdecoder.cpp index cf6d35e2f..5ce4f3554 100644 --- a/src/entrypoints/deserializer/logdecoder.cpp +++ b/src/entrypoints/deserializer/logdecoder.cpp @@ -41,7 +41,7 @@ using namespace std; using namespace tscpp; -void showUsage(string cmdName) +void showUsage(string& const cmdName) { std::cerr << "Usage: " << cmdName << " {-a | <log_file_name> | -h}" << "Options:\n" diff --git a/src/entrypoints/hardware_in_the_loop/hil-entry.cpp b/src/entrypoints/hardware_in_the_loop/hil-entry.cpp index 0ca4f829f..6f2170f9c 100644 --- a/src/entrypoints/hardware_in_the_loop/hil-entry.cpp +++ b/src/entrypoints/hardware_in_the_loop/hil-entry.cpp @@ -216,6 +216,10 @@ void threadFunc(void* arg) sEventBroker.post({EV_LANDED}, TOPIC_FLIGHT_EVENTS); Thread::sleep(1000); + + delete sensors.imu; + delete sensors.barometer; + delete sensors.gps; } int main() @@ -239,4 +243,4 @@ int main() } return 0; -} \ No newline at end of file +} diff --git a/src/entrypoints/lynx-mock-telemetry.cpp b/src/entrypoints/lynx-mock-telemetry.cpp index ed988227a..aa844fb17 100644 --- a/src/entrypoints/lynx-mock-telemetry.cpp +++ b/src/entrypoints/lynx-mock-telemetry.cpp @@ -105,7 +105,7 @@ void writeMessage(mavlink_message_t& msg) } } -void handleMessage(mavlink_message_t& msg) +void handleMessage(mavlink_message_t& const msg) { mavlink_message_t ack_msg; mavlink_ack_tm_t ack; @@ -153,4 +153,4 @@ int main() writeMessage(buf); Thread::sleepUntil(t + 100); } -} \ No newline at end of file +} diff --git a/src/entrypoints/payload-entry.cpp b/src/entrypoints/payload-entry.cpp index 583f1612a..6afbc4ef6 100644 --- a/src/entrypoints/payload-entry.cpp +++ b/src/entrypoints/payload-entry.cpp @@ -27,6 +27,7 @@ #include <miosix.h> using namespace miosix; +using namespace Boardcore; using namespace PayloadBoard; int main() @@ -71,4 +72,4 @@ int main() // StackLogger::getInstance().log(); } -} \ No newline at end of file +} diff --git a/src/entrypoints/windtunnel-test-decoder/LogTypes.h b/src/entrypoints/windtunnel-test-decoder/LogTypes.h index 5b370ad57..8c6fe459b 100644 --- a/src/entrypoints/windtunnel-test-decoder/LogTypes.h +++ b/src/entrypoints/windtunnel-test-decoder/LogTypes.h @@ -43,8 +43,6 @@ // Serialized classes using std::ofstream; -using namespace DeathStackBoard; - template <typename T> void print(T& t, ostream& os) { diff --git a/src/hardware_in_the_loop/HIL.h b/src/hardware_in_the_loop/HIL.h index c60df3520..317dd127e 100644 --- a/src/hardware_in_the_loop/HIL.h +++ b/src/hardware_in_the_loop/HIL.h @@ -22,11 +22,11 @@ #pragma once -#include "hardware_in_the_loop/HIL_sensors/HILSensors.h" +#include "NavigationAttitudeSystem/NASData.h" +#include "hardware_in_the_loop/HILConfig.h" #include "hardware_in_the_loop/HILFlightPhasesManager.h" +#include "hardware_in_the_loop/HIL_sensors/HILSensors.h" #include "hardware_in_the_loop/simulator_communication/HILTransceiver.h" -#include "hardware_in_the_loop/HILConfig.h" -#include "NavigationAttitudeSystem/NASData.h" /** * @brief Single interface to the hardware-in-the-loop framework. @@ -46,10 +46,7 @@ public: void stop() { simulator->stop(); } - void send(ActuatorData d) - { - simulator->setActuatorData(d); - } + void send(ActuatorData d) { simulator->setActuatorData(d); } /** * @brief method that signals to the simulator that the liftoff pin has @@ -87,4 +84,4 @@ private: flightPhasesManager = new HILFlightPhasesManager(); simulator = new HILTransceiver(flightPhasesManager); } -}; \ No newline at end of file +}; diff --git a/src/hardware_in_the_loop/HILFlightPhasesManager.h b/src/hardware_in_the_loop/HILFlightPhasesManager.h index d60c9d5db..e3416842f 100644 --- a/src/hardware_in_the_loop/HILFlightPhasesManager.h +++ b/src/hardware_in_the_loop/HILFlightPhasesManager.h @@ -22,8 +22,10 @@ #pragma once +#include <drivers/timer/TimestampTimer.h> #include <events/Events.h> #include <events/utils/EventCounter.h> +#include <utils/Debug.h> #include <iostream> #include <map> @@ -33,14 +35,10 @@ #include "HIL_sensors/HILSensors.h" #include "NavigationAttitudeSystem/NASData.h" #include "Singleton.h" -#include "TimestampTimer.h" #include "hardware_in_the_loop/HILConfig.h" #include "miosix.h" #include "sensors/Sensor.h" -using namespace miosix; -using namespace std; - typedef function<void()> TCallback; enum FlightPhases @@ -135,7 +133,7 @@ public: // set true when the first packet from the simulator arrives if (isSetTrue(SIMULATION_STARTED)) { - t_start = TimestampTimer::getInstance().getTimestamp(); + t_start = Boardcore::TimestampTimer::getInstance().getTimestamp(); TRACE("[HIL] ------- SIMULATION STARTED ! ------- \n"); changed_flags.push_back(SIMULATION_STARTED); @@ -162,7 +160,8 @@ public: { if (isSetTrue(FLYING)) { - t_liftoff = TimestampTimer::getInstance().getTimestamp(); + t_liftoff = + Boardcore::TimestampTimer::getInstance().getTimestamp(); sEventBroker.post({EV_UMBILICAL_DETACHED}, TOPIC_FLIGHT_EVENTS); TRACE("[HIL] ------- LIFTOFF ! ------- \n"); @@ -210,7 +209,7 @@ public: else if (isSetTrue(SIMULATION_STOPPED)) { changed_flags.push_back(SIMULATION_STOPPED); - t_stop = TimestampTimer::getInstance().getTimestamp(); + t_stop = Boardcore::TimestampTimer::getInstance().getTimestamp(); TRACE("[HIL] ------- SIMULATION STOPPED ! -------: %f \n\n\n", (double)t_stop / 1000000.0f); printOutcomes(); @@ -353,4 +352,4 @@ private: EventCounter* counter_airbrakes; EventCounter* counter_ada; EventCounter* counter_dpl; -}; \ No newline at end of file +}; diff --git a/src/hardware_in_the_loop/HIL_algorithms/HILMockAerobrakeAlgorithm.h b/src/hardware_in_the_loop/HIL_algorithms/HILMockAerobrakeAlgorithm.h index f9269dfef..fc79e10f8 100644 --- a/src/hardware_in_the_loop/HIL_algorithms/HILMockAerobrakeAlgorithm.h +++ b/src/hardware_in_the_loop/HIL_algorithms/HILMockAerobrakeAlgorithm.h @@ -28,9 +28,6 @@ #include "miosix.h" #include "sensors/Sensor.h" -using namespace DeathStackBoard; -using namespace miosix; - /** * @brief Example of how a control algorithm should be created. * @@ -90,4 +87,4 @@ private: TimestampData lastSample; /**< keeps track of the last timestamp */ Sensor<KD>* kalman; /**< reference to the kalman object */ ServoInterface* servo; /**< reference to the actuator object */ -}; \ No newline at end of file +}; diff --git a/src/hardware_in_the_loop/HIL_algorithms/HILMockNAS.h b/src/hardware_in_the_loop/HIL_algorithms/HILMockNAS.h index 07d212a5a..3057ea203 100644 --- a/src/hardware_in_the_loop/HIL_algorithms/HILMockNAS.h +++ b/src/hardware_in_the_loop/HIL_algorithms/HILMockNAS.h @@ -30,8 +30,6 @@ #include "hardware_in_the_loop/HIL_sensors/HILMagnetometer.h" #include "hardware_in_the_loop/HIL_sensors/HILSensor.h" -using namespace DeathStackBoard; - struct HILNasData : public TimestampData { HILNasData() : TimestampData{0} {} @@ -74,4 +72,4 @@ protected: } NAS<HILImuData, HILBaroData, HILGpsData> *nas; -}; \ No newline at end of file +}; diff --git a/src/hardware_in_the_loop/HIL_sensors/HILGps.h b/src/hardware_in_the_loop/HIL_sensors/HILGps.h index 093e54779..8f3a9b8a7 100644 --- a/src/hardware_in_the_loop/HIL_sensors/HILGps.h +++ b/src/hardware_in_the_loop/HIL_sensors/HILGps.h @@ -47,9 +47,8 @@ protected: /* I make a copy of the vector i have to memorize in the sensor * struct */ - Vec3 matlabData = sensorData->gps.positionMeasures[sampleCounter]; - tempData.latitude = - matlabData.getX(); // divide by earth radius + Vec3 matlabData = sensorData->gps.positionMeasures[sampleCounter]; + tempData.latitude = matlabData.getX(); // divide by earth radius tempData.longitude = matlabData.getY(); tempData.height = matlabData.getZ(); @@ -65,4 +64,4 @@ protected: return tempData; } -}; \ No newline at end of file +}; diff --git a/src/hardware_in_the_loop/HIL_sensors/HILImu.h b/src/hardware_in_the_loop/HIL_sensors/HILImu.h index 8a68c9c7a..9d72b1e0b 100644 --- a/src/hardware_in_the_loop/HIL_sensors/HILImu.h +++ b/src/hardware_in_the_loop/HIL_sensors/HILImu.h @@ -64,7 +64,8 @@ protected: matlabData = sensorData->magnetometer.measures[sampleCounter]; tempData.mag_x = matlabData.getX(); tempData.mag_y = matlabData.getY(); - tempData.mag_z = matlabData.getZ() / 1000.0f; // from nanotesla to microtesla + tempData.mag_z = + matlabData.getZ() / 1000.0f; // from nanotesla to microtesla // only update the timestamp once and use it for all the 3 sensors // (this sensor assumes the same frequency for accel, gyro and mag) @@ -74,4 +75,4 @@ protected: return tempData; } -}; \ No newline at end of file +}; diff --git a/src/hardware_in_the_loop/HIL_sensors/HILMagnetometer.h b/src/hardware_in_the_loop/HIL_sensors/HILMagnetometer.h index 91d4beff5..973b67ebd 100644 --- a/src/hardware_in_the_loop/HIL_sensors/HILMagnetometer.h +++ b/src/hardware_in_the_loop/HIL_sensors/HILMagnetometer.h @@ -49,11 +49,12 @@ protected: * struct */ Vec3 matlabData = sensorData->magnetometer.measures[sampleCounter]; - tempData.mag_x = matlabData.getX(); - tempData.mag_y = matlabData.getY(); - tempData.mag_z = matlabData.getZ() / 1000.0f; // from nanotesla to microtesla + tempData.mag_x = matlabData.getX(); + tempData.mag_y = matlabData.getY(); + tempData.mag_z = + matlabData.getZ() / 1000.0f; // from nanotesla to microtesla tempData.mag_timestamp = updateTimestamp(); return tempData; } -}; \ No newline at end of file +}; diff --git a/src/hardware_in_the_loop/HIL_sensors/HILSensor.h b/src/hardware_in_the_loop/HIL_sensors/HILSensor.h index 253e8e135..299b9a126 100644 --- a/src/hardware_in_the_loop/HIL_sensors/HILSensor.h +++ b/src/hardware_in_the_loop/HIL_sensors/HILSensor.h @@ -140,7 +140,7 @@ protected: uint64_t updateTimestamp() { sampleCounter++; - return TimestampTimer::getInstance().getTimestamp(); + return Boardcore::TimestampTimer::getInstance().getTimestamp(); } /** @@ -156,4 +156,4 @@ protected: int sampleCounter = 0; /**< counter of the next sample to take */ int n_data_sensor; /**< number of samples in every period */ SimulatorData *sensorData; /**< reference to the SensorData structure */ -}; \ No newline at end of file +}; diff --git a/src/hardware_in_the_loop/HIL_sensors/HILSensorsData.h b/src/hardware_in_the_loop/HIL_sensors/HILSensorsData.h index 9acedad4f..9e0655c79 100644 --- a/src/hardware_in_the_loop/HIL_sensors/HILSensorsData.h +++ b/src/hardware_in_the_loop/HIL_sensors/HILSensorsData.h @@ -1,3 +1,24 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ #include <sensors/SensorData.h> @@ -67,9 +88,10 @@ struct HILImuData : public HILAccelData, void print(std::ostream& os) const { - os << accel_timestamp << "," << accel_x << "," << accel_y << "," << accel_z - << "," << gyro_timestamp << "," << gyro_x << "," << gyro_y << "," << gyro_z - << "," << mag_timestamp << "," << mag_x << "," << mag_y << "," << mag_z << "\n"; + os << accel_timestamp << "," << accel_x << "," << accel_y << "," + << accel_z << "," << gyro_timestamp << "," << gyro_x << "," << gyro_y + << "," << gyro_z << "," << mag_timestamp << "," << mag_x << "," + << mag_y << "," << mag_z << "\n"; } }; diff --git a/src/hardware_in_the_loop/simulator_communication/HILTransceiver.h b/src/hardware_in_the_loop/simulator_communication/HILTransceiver.h index 22fd5855f..f69513774 100644 --- a/src/hardware_in_the_loop/simulator_communication/HILTransceiver.h +++ b/src/hardware_in_the_loop/simulator_communication/HILTransceiver.h @@ -22,6 +22,8 @@ #pragma once +#include <utils/Debug.h> + #include "ActiveObject.h" #include "Algorithm.h" #include "SerialInterface.h" @@ -30,8 +32,6 @@ #include "hardware_in_the_loop/HIL_sensors/HILTimestampManagement.h" #include "math/Vec3.h" -using namespace miosix; - /** * @brief HILTranceiver is a Singleton and provides an easy interface for the * control algorithms to send and receive data during a simulation @@ -175,4 +175,4 @@ private: std::vector<HILTimestampManagement *> sensorsTimestamp; FastMutex mutex; ConditionVariable condVar; -}; \ No newline at end of file +}; diff --git a/src/hardware_in_the_loop/simulator_communication/SerialInterface.h b/src/hardware_in_the_loop/simulator_communication/SerialInterface.h index e8ba4476b..81dce8975 100644 --- a/src/hardware_in_the_loop/simulator_communication/SerialInterface.h +++ b/src/hardware_in_the_loop/simulator_communication/SerialInterface.h @@ -24,6 +24,7 @@ #include <fcntl.h> #include <stdio.h> +#include <utils/Debug.h> #include <string> @@ -31,9 +32,6 @@ #include "filesystem/file_access.h" #include "miosix.h" -using namespace std; -using namespace miosix; - /** * @brief Creates and opens a serial port on the board and provides templated * "sendData" and "recvData" functions in order to send and receive any data @@ -142,4 +140,4 @@ private: int baudrate; /**< Baudrate of the serial port */ bool is_init; /**< True if init() already called successfully, false otherwise */ -}; \ No newline at end of file +}; diff --git a/src/mocksensors/MockGPS.h b/src/mocksensors/MockGPS.h index 17b04b9a8..aa28aaa33 100644 --- a/src/mocksensors/MockGPS.h +++ b/src/mocksensors/MockGPS.h @@ -30,7 +30,7 @@ namespace DeathStackBoard { -class MockGPS : public Sensor<MockGPSData> +class MockGPS : public Boardcore::Sensor<MockGPSData> { public: MockGPS() {} @@ -54,8 +54,9 @@ public: MockGPSData data; - data.gpsTimestamp = TimestampTimer::getInstance().getTimestamp(); - data.fix = true; + data.gpsTimestamp = + Boardcore::TimestampTimer::getInstance().getTimestamp(); + data.fix = true; if (before_liftoff) { diff --git a/src/mocksensors/MockIMU.h b/src/mocksensors/MockIMU.h index acd1bf1e3..4f79bebf0 100644 --- a/src/mocksensors/MockIMU.h +++ b/src/mocksensors/MockIMU.h @@ -30,7 +30,7 @@ namespace DeathStackBoard { -class MockIMU : public Sensor<MockIMUData> +class MockIMU : public Boardcore::Sensor<MockIMUData> { public: MockIMU() {} @@ -47,7 +47,7 @@ public: } MockIMUData data; - uint64_t t = TimestampTimer::getInstance().getTimestamp(); + uint64_t t = Boardcore::TimestampTimer::getInstance().getTimestamp(); data.accelerationTimestamp = t; data.accelerationX = ACCEL_DATA[index][0]; diff --git a/src/mocksensors/MockPressureSensor.h b/src/mocksensors/MockPressureSensor.h index 1a6b7ccc7..2d62684b1 100644 --- a/src/mocksensors/MockPressureSensor.h +++ b/src/mocksensors/MockPressureSensor.h @@ -32,7 +32,7 @@ namespace DeathStackBoard { -class MockPressureSensor : public Sensor<MockPressureData> +class MockPressureSensor : public Boardcore::Sensor<MockPressureData> { public: MockPressureSensor(bool with_noise_ = false) : with_noise(with_noise_) {} @@ -45,7 +45,8 @@ public: { MockPressureData data; - data.pressureTimestamp = TimestampTimer::getInstance().getTimestamp(); + data.pressureTimestamp = + Boardcore::TimestampTimer::getInstance().getTimestamp(); if (before_liftoff) { diff --git a/src/mocksensors/MockSensorsData.h b/src/mocksensors/MockSensorsData.h index d9edc2220..de12014dc 100644 --- a/src/mocksensors/MockSensorsData.h +++ b/src/mocksensors/MockSensorsData.h @@ -24,9 +24,9 @@ #include <sensors/SensorData.h> -struct MockIMUData : public AccelerometerData, - public GyroscopeData, - public MagnetometerData +struct MockIMUData : public Boardcore::AccelerometerData, + public Boardcore::GyroscopeData, + public Boardcore::MagnetometerData { static std::string header() { @@ -46,7 +46,7 @@ struct MockIMUData : public AccelerometerData, } }; -struct MockPressureData : public PressureData +struct MockPressureData : public Boardcore::PressureData { static std::string header() { @@ -59,7 +59,7 @@ struct MockPressureData : public PressureData } }; -struct MockGPSData : public GPSData +struct MockGPSData : public Boardcore::GPSData { static std::string header() { @@ -87,4 +87,4 @@ struct MockSpeedData { os << timestamp << "," << speed << "\n"; } -}; \ No newline at end of file +}; diff --git a/src/mocksensors/MockSpeedSensor.h b/src/mocksensors/MockSpeedSensor.h index c7c01cae0..9e2a1458c 100644 --- a/src/mocksensors/MockSpeedSensor.h +++ b/src/mocksensors/MockSpeedSensor.h @@ -31,7 +31,7 @@ namespace DeathStackBoard { -class MockSpeedSensor : public Sensor<MockSpeedData> +class MockSpeedSensor : public Boardcore::Sensor<MockSpeedData> { public: MockSpeedSensor() {} @@ -44,7 +44,8 @@ public: { MockSpeedData data; - data.timestamp = TimestampTimer::getInstance().getTimestamp(); + data.timestamp = + Boardcore::TimestampTimer::getInstance().getTimestamp(); if (before_liftoff) { diff --git a/src/mocksensors/lynx_flight_data/lynx_airspeed_data.cpp b/src/mocksensors/lynx_flight_data/lynx_airspeed_data.cpp index 7af10f441..bd3dbd2cf 100644 --- a/src/mocksensors/lynx_flight_data/lynx_airspeed_data.cpp +++ b/src/mocksensors/lynx_flight_data/lynx_airspeed_data.cpp @@ -23,902 +23,901 @@ #include "lynx_airspeed_data.h" const float AIRSPEED_DATA[AIRSPEED_DATA_SIZE] = { - 12.4113951, 10.6396132, 17.169529, 22.8292465, 10.5444088, - 8.39081669, 10.641614, 11.4718409, 20.4783287, 15.3504314, 15.3513031, - 9.63413239, 25.3731174, 18.9253101, 16.0033855, 24.1376781, 28.4115391, - 27.6845875, 29.4730015, 33.6768837, 34.5762138, 35.450737, 36.8664055, - 36.8693237, 38.7607841, 41.314991, 41.3147278, 42.0521317, 45.778492, - 46.6648064, 47.7517242, 49.6425095, 52.0558586, 52.4533348, 53.8047791, - 55.6794052, 58.0183525, 60.6116638, 60.7854004, 63.1039772, 65.9648132, - 66.4317398, 68.2648849, 70.625473, 71.7813721, 73.9020615, 75.5481796, - 77.4300003, 78.3614883, 80.6872864, 82.9510956, 82.7034836, 86.2424927, - 86.9628677, 88.2577972, 90.9050903, 92.1436462, 94.1384506, 95.7668915, - 97.7961044, 99.5847855, 102.038879, 103.549995, 105.355217, 107.584, - 109.401268, 112.003922, 112.947815, 114.310066, 116.204742, 117.900772, - 119.379234, 121.453636, 123.484711, 124.659866, 126.166412, 127.724335, - 129.257675, 130.638611, 132.462296, 133.884125, 135.494843, 137.123718, - 138.941895, 140.886703, 143.3172, 143.985199, 146.436874, 147.651245, - 149.980957, 151.795288, 152.972382, 155.550827, 157.63028, 158.979904, - 159.744888, 162.516342, 163.158035, 164.464325, 165.421356, 167.523666, - 169.456955, 170.805832, 172.415237, 173.797318, 175.7146, 177.344955, - 179.430923, 181.203964, 182.604797, 184.202942, 183.232407, 183.828201, - 185.775909, 187.161652, 188.274979, 190.970871, 193.048584, 193.552383, - 193.903107, 195.170502, 196.671463, 197.220306, 197.083771, 197.749985, - 198.354935, 199.889923, 200.472031, 201.510834, 201.720306, 201.343964, - 201.127594, 201.470779, 200.658096, 200.537796, 200.431641, 199.389252, - 199.213486, 198.424728, 197.32753, 196.845001, 195.506439, 194.997894, - 194.371918, 193.090515, 193.750916, 192.536942, 192.263809, 191.898621, - 191.349625, 191.828659, 191.363953, 190.446213, 189.351685, 185.858109, - 180.658096, 180.828461, 183.347458, 184.327713, 186.507248, 187.238586, - 187.274399, 188.080185, 187.780655, 186.073151, 186.473251, 184.401474, - 185.514282, 184.814407, 182.69313, 182.772842, 182.772842, 181.386536, - 180.426163, 179.513199, 179.017822, 179.074036, 179.045639, 178.686157, - 177.242355, 177.189133, 176.377136, 176.152817, 175.485916, 174.898453, - 174.489395, 173.92334, 173.084961, 172.043137, 171.424362, 171.075867, - 170.613815, 169.925919, 169.062241, 168.137009, 167.604156, 167.320312, - 166.214432, 166.065033, 166.495438, 165.243683, 164.243286, 163.642975, - 163.649567, 162.826721, 163.247635, 162.502182, 160.487228, 160.60936, - 160.999207, 159.284119, 158.065231, 157.567429, 156.423874, 157.117569, - 154.945602, 156.644455, 156.413177, 155.87352, 155.647827, 154.769485, - 153.900177, 153.991287, 152.611343, 152.811279, 151.95636, 151.686584, - 151.216324, 150.376831, 150.142502, 149.453705, 148.636536, 148.431534, - 147.900177, 147.052231, 146.009003, 145.9505, 145.679123, 144.611954, - 144.45787, 143.784164, 143.393646, 143.088669, 142.234741, 141.963608, - 141.214371, 141.056747, 140.908966, 140.156128, 140.33905, 139.678894, - 138.870056, 138.330856, 137.469437, 135.973679, 135.708847, 135.49617, - 134.939362, 134.409241, 133.690781, 133.235306, 133.319, 132.43364, - 132.332947, 131.808243, 132.145615, 131.843628, 131.656967, 131.189651, - 130.84111, 130.822113, 129.194061, 129.159836, 128.43576, 127.813522, - 127.921944, 126.603584, 125.869659, 126.404015, 125.184059, 124.586815, - 124.220787, 124.580544, 124.249268, 124.175308, 123.479156, 123.30941, - 122.77816, 122.195129, 121.41465, 121.199074, 120.922768, 120.377502, - 120.308357, 119.179604, 119.280205, 119.204651, 118.655434, 117.689667, - 117.722618, 117.010521, 116.232468, 116.834404, 116.555901, 115.722458, - 115.18029, 115.082939, 114.965645, 115.294937, 114.62886, 113.85054, - 113.45314, 113.286819, 112.595406, 112.576797, 111.637688, 111.34462, - 110.939369, 110.725937, 111.067787, 111.307861, 110.192368, 109.503044, - 109.619461, 109.293739, 108.835175, 108.321236, 107.688713, 108.302734, - 107.286201, 107.089714, 106.759506, 106.905746, 106.503944, 105.741417, - 106.211449, 104.930573, 104.959724, 104.948799, 104.656197, 104.231476, - 104.327293, 104.25589, 103.044258, 103.076309, 103.095726, 103.103935, - 103.013115, 102.692505, 102.904404, 102.244812, 102.244812, 100.50853, - 101.400246, 99.6098175, 99.6516418, 99.0769882, 99.6706009, 98.7544479, - 98.1889343, 98.0998154, 97.5229416, 97.294548, 97.5436935, 96.7283325, - 96.6032104, 96.0145493, 95.925087, 95.3586502, 96.0646896, 95.3726273, - 94.9047775, 95.2724152, 94.6777878, 93.9669113, 93.8663254, 93.5023575, - 92.8998337, 92.1656876, 92.1644287, 92.0681839, 91.6967316, 90.8415756, - 90.6017685, 90.6057816, 90.6103745, 89.8713226, 90.7580795, 89.7719803, - 90.0259781, 89.1295776, 89.1429672, 89.0196991, 88.9238129, 88.4012299, - 88.1494522, 87.8901901, 87.9061508, 87.256691, 86.4787445, 86.7329102, - 86.0919266, 86.1069946, 85.7037277, 84.5901794, 85.6158752, 84.9309998, - 82.9900208, 82.935997, 82.1810913, 81.3405228, 81.0612259, 82.0060654, - 82.1019135, 81.2657013, 80.8491669, 80.4293671, 80.7200241, 80.585083, - 80.5918961, 79.8911362, 79.6070709, 79.7559662, 78.9017639, 78.7602005, - 78.9147797, 78.6235962, 78.0473709, 77.4674454, 77.0251999, 77.3284149, - 76.8851242, 76.1434631, 75.9982681, 76.1545715, 76.0106964, 75.5613708, - 74.8006821, 74.9684753, 74.8190384, 74.9822693, 75.1356201, 74.8413315, - 75.1525192, 73.9239197, 74.0867538, 73.1514816, 73.3152008, 72.3738251, - 72.0599976, 70.9409103, 70.9441223, 70.2937088, 70.140213, 69.9797897, - 70.3167725, 69.9882202, 68.8350296, 69.0057449, 68.8466949, 69.1870804, - 68.5203094, 68.0226212, 68.1939926, 68.3654556, 68.2077789, 67.8697815, - 68.0446091, 67.0231018, 66.8602066, 66.6925735, 65.9965057, 65.8304214, - 65.8362885, 65.8388062, 65.8448029, 65.6725159, 64.6115875, 64.6147308, - 64.4388885, 64.0865479, 63.7265358, 63.5475883, 63.1892509, 63.0096741, - 63.7465324, 62.4627075, 62.8401031, 62.6645813, 62.2936783, 62.1134071, - 62.3037453, 61.9358864, 61.1863976, 61.5671234, 61.3853912, 60.0524178, - 60.0564346, 60.0612564, 59.2879105, 59.2919846, 58.899395, 58.7092056, - 58.9122047, 58.321167, 57.9245033, 57.5246315, 57.729763, 57.5287971, - 57.5352554, 57.5404243, 57.7434425, 56.5259209, 56.3256836, 56.3274956, - 55.5005493, 54.8687096, 54.8751602, 55.092392, 55.0932121, 55.0972252, - 55.1003227, 54.8914223, 54.89645, 54.6863785, 55.1166954, 53.6196175, - 53.4029846, 51.8619728, 53.6315536, 52.7581978, 52.5371323, 52.3209801, - 52.323204, 52.3257484, 51.8822556, 50.7485352, 50.0599213, 50.5264931, - 49.3616409, 49.3656807, 49.3667259, 49.3687859, 49.3724022, 49.3762398, - 49.3780861, 48.6644897, 47.9443398, 47.9471207, 47.9490433, 47.7080727, - 47.9534836, 47.9575996, 47.958744, 47.7178268, 46.2292938, 46.2304153, - 46.4842529, 46.234417, 44.9570503, 46.4934654, 45.47789, 44.9655609, - 45.2264938, 44.7079926, 44.7103996, 44.7117538, 44.4523392, 43.1193924, - 43.1209602, 43.1238022, 42.854023, 42.8559265, 42.8587952, 42.85952, - 42.8622475, 41.475563, 41.7597542, 41.4811821, 40.9140892, 41.2012596, - 41.202652, 41.2042427, 41.2053795, 41.4914932, 40.349575, 40.6394196, - 39.4713898, 39.4743118, 39.1792641, 39.4780922, 39.4780922, 39.4808846, - 39.1840248, 39.185955, 39.4847832, 39.4861526, 38.8903236, 39.4900551, - 38.8932724, 39.4927559, 37.6717682, 37.0458107, 37.6758652, 37.3641129, - 37.6788673, 37.0512886, 37.3682556, 37.369503, 37.3701096, 37.3719177, - 37.3730927, 37.6870918, 37.6876945, 37.3767242, 35.4465446, 35.4460869, - 35.4472466, 35.449543, 35.4499092, 35.4511642, 35.4511642, 35.4538002, - 35.4547119, 35.1230011, 33.410881, 33.0581741, 34.4510002, 33.0604782, - 31.9786453, 31.2377892, 30.8595219, 30.8598766, 31.2406349, 31.2420692, - 31.2429924, 31.2429924, 30.8647938, 30.8668213, 30.8664036, 31.6195831, - 31.2468319, 30.869627, 31.2483406, 31.2496529, 30.4884949, 30.8720722, - 31.2515697, 31.6262512, 31.2538109, 31.6281986, 31.6281986, 29.3145161, - 28.5018139, 24.5097885, 24.9864769, 24.9864426, 24.987608, 24.5116386, - 24.9877625, 24.9884148, 25.4550762, 24.9890804, 24.9899445, 25.4569378, - 24.5155354, 24.031086, 24.5171452, 24.9924126, 24.9930611, 21.4468918, - 22.5186787, 21.9900837, 20.8932552, 21.99086, 21.991396, 21.9915123, - 21.9920998, 21.9926891, 21.9928856, 21.9945145, 21.9945145, 20.8961391, - 21.9940243, 21.4522953, 21.994606, 21.4526997, 21.9957924, 21.9958515, - 21.996254, 21.9964314, 21.9965725, 21.9964924, 21.9976387, 20.3273621, - 21.4550629, 19.7407608, 21.4558983, 21.9986305, 21.9989548, 21.9993973, - 21.9991398, 19.7421799, 21.4567852, 20.3296833, 20.3300762, 22.0001965, - 18.5116444, 22.0004635, 17.8649158, 18.5126705, 17.8653278, 21.4595833, - 18.512888, 20.9040699, 19.1385517, 17.8656101, 18.5133038, 21.4594917, - 22.5309448, 22.5318375, 22.5320187, 22.0026321, 22.0030384, 22.0026226, - 22.5323658, 22.003109, 21.4602718, 22.0032024, 17.1941872, 18.513855, - 17.86726, 17.8674145, 17.8668137, 18.5143547, 17.8674564, 17.8672352, - 18.5146198, 18.5145397, 17.8666153, 18.5147762, 18.5147762, 19.1403484, - 18.5144711, 18.5147095, 18.5133171, 18.5145321, 18.514267, 18.5146809, - 18.514204, 19.140276, 17.8670464, 19.1403503, 20.9043922, 19.745657, - 19.1379051, 12.4201765, 13.3371429, 9.14465141, 14.1921349, 3.60264635, - 6.04226494, 13.3364954, 17.1931267, 19.139143, 6.04244566, 18.5128231, - 17.8655987, 18.5122795, 17.8650379, 17.8648739, 18.5121994, 18.5116119, - 17.8652287, 19.7448235, 6.04315424, 13.3384638, 16.4980888, 9.14543152, - 17.1910973, 22.0007267, 25.4744186, 22.0027752, 23.0486393, 25.0114841, - 22.5339508, 18.5152931, 27.6943932, 30.5291271, 22.5358543, 20.9087124, - 7.59722328, 22.0097103, 12.4254007, 20.3355427, 35.5281181, 12.3314304, - 10.3577375, 5.84748268, 15.6973457, 11.3356037, 12.4266768, 3.2577486, - 7.5973978, 3.25779343, 3.25704408, 7.74968767, 14.1891499, 13.2464952, - 9.01299667, 6.03971958, 6.0393858, 13.3299341, 18.5029831, 17.8552227, - 21.9898586, 21.9898586, 18.502203, 17.8566074, 18.5038853, 18.5051804, - 7.74701643, 6.04055595, 7.74729586, 6.04036283, 3.25602245, 7.59292507, - 14.1051168, 6.04049158, 9.14046478, 12.3218737, 9.1409235, 14.1859617, - 6.03975201, 5.84166813, 9.01044941, 6.03848886, 10.2303801, 7.74395752, - 3.59899211, 3.25361276, 3.59889698, 3.59870267, 6.03561115, 10.3407993, - 7.74237823, 13.3237381, 13.3240919, 17.8494205, 18.4972916, 18.4962921, - 19.1214581, 17.8483868, 18.49576, 17.8477268, 18.4927731, 18.492672, - 18.4916954, 17.8443089, 12.406496, 12.4056816, 3.59816217, 6.03494692, - 3.59752965, 6.03406143, 5.83609486, 10.22295, 11.3140211, 11.3134918, - 10.2218494, 10.2215061, 10.2211504, 12.3060255, 11.3137445, 10.2221661, - 11.3136711, 11.3136711, 11.3124952, 15.664196, 14.8965168, 14.0859232, - 10.2211866, 11.3135366, 10.2217808, 9.00197983, 10.2213392, 6.03308296, - 3.59690285, 7.7373147, 3.59669352, 7.73725796, 6.03254986, 3.59651208, - 12.3028679, 10.2183495, 8.99698067, 10.216466, 11.3068304, 11.3062677, - 5.8319993, 7.73274469, 6.02868652, 3.59436488, 3.59407854, 3.59391236, - 6.02781248, 6.02781248, 7.73060513, 6.02756834, 7.73031759, 7.72989035, - 6.02676058, 3.59323215, 6.02651882, 3.59302473, 6.02615547, 7.72881031, - 3.24800467, 6.02536201, 3.59233379, 6.02535677, 7.72803164, 3.59216714, - 8.98922825, 3.24734902, 10.2073698, 11.2969427, 11.2966814, 11.2965622, - 8.98867702, 11.2960491, 10.2066708, 10.2069464, 11.2960768, 11.2956123, - 10.2055216, 11.2948475, 10.2069149, 11.2959919, 11.2962055, 10.2063618, - 11.2964983, 12.2879896, 17.0724678, 17.0715714, 16.3699131, 17.0712872, - 15.6387024, 16.3675041, 10.20292, 10.2030973, 11.2921638, 11.2914381, - 11.2906418, 10.2012043, 10.2003527, 8.98231316, 10.1990118, 5.82227564, - 6.01907253, 6.01874828, 6.01868629, 3.58817148, 12.2762976, 3.58810663, - 3.58776307, 6.01753235, 7.71756935, 6.01720285, 7.71708059, 6.01679659, - 7.5632906, 8.97652531, 5.81914234, 5.81880713, 7.71594667, 3.5868032, - 3.58661366, 5.81885719, 11.2806158, 12.271019, 10.1920471, 11.2795677, - 10.1915274, 10.1918221, 10.1917429, 10.1912527, 11.2786007, 11.2786112, - 11.2781916, 10.1905231, 10.1902819, 11.2775879, 11.2776461, 15.6158009, - 11.2757177, 14.0409632, 11.2755575, 11.274991, 14.8487339, 15.6146145, - 11.2755175, 17.0420246, 17.0419502, 17.0410709, 14.0389328, 13.1812143, - 11.2738237, 10.1854382, 7.7101841, 6.01100254, 6.01100254, 7.70991182, - 11.3741245, 6.01128531, 6.01112461, 6.01021528, 3.58349705, 7.70864153, - 6.01033497, 10.183567, 11.2703085, 16.3360367, 17.0351124, 21.2924366, - 20.170599, 20.170599, 17.7087135, 16.3377495, 10.1839733, 6.01016378, - 12.354497, 13.263361, 14.1137018, 13.2632685, 17.0976734, 13.2626057, - 12.3517237, 12.3511477, 7.70557833, 7.70467758, 7.70467758, 6.00756264, - 6.007164, 6.00702906, 6.00666142, 11.365983, 12.3460808, 13.2555418, - 14.1044979, 13.2529917, 13.2529707, 13.2517376, 3.57969761, 6.00419092, - 6.00360107, 6.00360107, 3.5793159, 10.1701603, 11.2555685, 10.169241, - 10.1685524, 10.1681852, 5.8049345, 6.00042915, 3.23430943, 3.57742977, - 5.99990034, 5.99987411, 5.99963045, 7.69487047, 7.69487047, 5.99846077, - 3.57622719, 5.9984169, 3.57625532, 7.69332075, 3.57635856, 5.9985199, - 3.57627439, 5.80163622, 3.57631397, 7.54045773, 3.2331686, 8.94923306, - 11.2472944, 8.94926929, 5.99862003, 5.99791861, 10.2751884, 9.07659531, - 15.5735836, 5.99738121, 3.57544804, 11.3474636, 5.79976702, 8.946311, - 3.57490706, 5.99591637, 7.6905179, 12.324584, 3.57464027, 3.57431889, - 5.99452639, 3.57412672, 5.99432373, 5.99418545, 5.99430752, 3.5738368, - 7.68710852, 3.57327032, 5.99312401, 5.99244642, 5.99194384, 5.99204922, - 3.57221198, 3.57221198, 5.9910593, 5.99095249, 7.68387556, 5.99070454, - 3.57172537, 3.57166409, 5.99046659, 3.57143831, 7.68247938, 3.57120061, - 5.98979473, 5.9890089, 7.68116999, 5.98880625, 5.98880625, 7.68037462, - 5.98834944, 7.68022776, 5.98784971, 7.52708721, 5.79197502, 10.1455574, - 11.2285032, 10.1458092, 5.79199791, 13.128891, 7.52783871, 13.1292086, - 3.2278235, 3.2278235, 3.22768569, 3.5701232, 3.57018566, 9.06131268, - 3.56986642, 5.98688555, 5.98638582, 7.52399063, 5.98484755, 5.98485136, - 5.98437738, 3.56741905, 5.98319244, 10.1362906, 11.2182264, 3.56646395, - 8.92491913, 5.98152447, 7.67152357, 7.67128038, 3.56601739, 11.2156086, - 3.56605697, 5.98091507, 11.2148094, 7.5176158, 3.22347832, 3.56539583, - 5.97957325, 3.2231431, 5.97923613, 5.97887087, 8.92055225, 5.97849798, - 3.56453681, 3.56443191, 3.56433344, 5.97805643, 7.66717768, 7.51434851, - 7.66710997, 5.97793961, 5.97740984, 7.66646338, 3.22194648, 11.2070274, - 5.97679329, 3.56318951, 5.97603703, 5.97590065, 7.6642375, 3.56273842, - 5.97523069, 5.97500849, 7.66360664, 7.66348839, 3.56244898, 5.97504091, - 5.97509336, 5.97509336, 5.97487879, 5.97498083, 3.56233811, 11.304224, - 14.0306425, 14.0295267, 13.1844702, 10.2325621, 5.97272062, 7.66025352, - 5.97268009, 5.97211075, 7.5076704, 10.1188536, 11.1989279, 3.56036615, - 5.97102213, 7.65805006, 7.65733051, 5.96992445, 5.9700017, 3.55930924, - 7.65694571, 7.65652943, 9.03396606, 10.1145563, 8.9068737, 10.2287149, - 9.03457832, 7.50459671, 12.2718077, 5.97014618, 10.2276001, 11.2961884, - 11.1935053, 3.21725106, 5.96846628, 11.1922293, 16.2208328, 3.55774903, - 5.77160883, 10.1094856, 10.1095581, 5.96686077, 7.50045633, 5.96654606, - 3.21563625, 3.5566566, 9.0260725, 5.96454144, 7.65039444, 5.96486378, - 5.96432686, 7.64937544, 5.96365499, 3.21465659, 5.96285439, 3.55512285, - 7.64714098, 3.55482578, 5.96200752, 3.5544672, 3.55451393, 3.55422473, - 5.96149969, 5.96121264, 5.96128702, 3.55409288, 5.9612956, 5.96124363, - 5.9613657, 5.96145439, 3.55421925, 5.96093798, 3.21309805, 5.96044922, - 5.96043587, 3.55358911, 10.098197, 13.916523, 18.1972065, 8.89144993, - 5.95890617, 7.64157057, 7.48945284, 5.76255131, 5.95762777, 3.21122432, - 5.95686388, 5.95686388, 7.63983297, 3.55138516, 7.63987255, 7.63961267, - 3.55131745, 3.21058846, 9.01285362, 3.55105591, 10.0911579, 14.706398, - 10.0909786, 10.0910883, 11.1683483, 10.0907621, 10.0907621, 8.88502979, - 11.167366, 12.1476259, 11.1651192, 7.4845438, 8.88363934, 5.75845623, - 3.54942799, 3.54939342, 5.95278311, 5.95216179, 5.95157242, 5.95135164, - 7.63264704, 5.95098734, 7.63193941, 5.95037413, 7.63199186, 5.75587749, - 3.54757261, 10.0811577, 3.54731202, 7.63029957, 5.94852304, 5.94873667, - 5.94861269, 5.94800091, 5.94818068, 5.94799852, 5.94799852, 5.94842768, - 3.54655766, 3.5470531, 3.54731011, 11.2570257, 15.5217209, 20.5828362, - 18.2268982, 18.2256298, 18.2252045, 13.1271458, 18.2200317, 17.5805988, - 14.7572603, 3.54469323, 5.94502163, 5.94509554, 3.54447913, 7.62516308, - 7.62472439, 7.62417555, 5.94443512, 5.94343376, 5.94335413, 5.9430685, - 3.54321885, 5.94245577, 5.94235849, 7.62124968, 5.94210148, 5.94195652, - 5.94189501, 5.9417305, 8.99121666, 7.62052822, 13.0257034, 5.74648094, - 7.61990452, 7.6197629, 5.94117498, 3.54200101, 5.94098902, 5.94080925, - 7.61954832, 13.1123629, 5.9404583, 5.94045448, 7.46690226, 7.61790228, - 7.61802912, 8.98751736, 3.54084778, 7.6173501, 5.93858528, 5.93837786, - 7.61608219, 5.93749857, 7.61466932, 5.93706417, 5.93706417, 5.93653202, - 7.61411667, 7.61357117, 7.61343384, 5.9360323, 5.93591213, 5.93569469, - 7.61276579, 10.0564842, 10.0561552, 10.0561571, 10.0554066, 11.1291246, - 11.1294632, 10.0560875, 11.1300077, 10.0553951, 10.0548286, 8.85348892, - 10.0534792, 7.45834875, 5.93319559, 5.93233061, 7.60854006, 5.93191099, - 5.93193197, 13.0926151, 13.9312391, 14.7227669, 13.9307919, 13.9292288, - 7.60701561, 17.5350285, 11.2236567, 10.1626549, 19.3261051, 14.6478043, - 21.0142803, 16.1234341, 11.2231293, 10.0492678, 3.5363307, 13.849905, - 11.1222973, 3.19719362, 14.7214508, 14.7197962, 19.3757839, 13.9270439, - 3.5349977, 13.9246883, 18.1633053, 18.775528, 17.5262432, 17.5264339, - 17.5262012, 18.1605053, 13.8413582, 5.92726326, 19.3104534, 15.3916883, - 5.92809486, 17.4635448, 7.45011139, 10.0427885, 18.715147, 13.0832834, - 3.5340395, 5.92668915, 10.153801, 18.1573048, 8.84122849, 19.8845654, - 5.92488718, 3.53252649, 3.53227448, 15.4549074, 7.59775972, 3.19308734, - 5.9231801, 8.83705902, 5.92234707, 10.035224, 13.9095736, 7.59579182, - 3.53058672, 5.92130327, 5.92097378, 8.95991611, 8.95991611, 7.59323835, - 5.92002964, 7.59272957, 5.91936255, 5.91917467, 5.9190588, 3.52901626, - 5.91884756, 5.91863251, 5.91841841, 5.91790533, 5.91784573, 3.52822089, - 7.58936405, 7.58936405, 5.91709089, 8.82846451, 5.72304678, 7.58827114, - 5.91637754, 7.58782434, 5.91643667, 7.58741617, 7.58728504, 5.91542864, - 5.91536808, 5.91477728, 5.91507101, 5.91467667, 5.91467667, 7.5867424, - 8.82519531, 5.91450262, 5.91396236, 5.91371632, 5.72051144, 5.91476154, - 18.7332764, 19.3259335, 17.4860172, 8.94877434, 13.8869486, 13.0504255, - 11.1875944, 7.58307171, 7.5822835, 7.58143282, 5.91134596, 11.1856537, - 17.4782276, 18.1110134, 3.18628049, 7.58162689, 15.3520947, 3.52491188, - 3.18669963, 5.91144943, 3.52519703, 14.6744404, 18.1133919, 13.0453939, - 10.1236525, 7.57933998, 3.18526816, 7.5782671, 7.577981, 3.52247834, - 5.90714931, 5.90715027, 7.57603884, 3.52158785, 5.90661478, 7.57527542, - 7.57472801, 7.57472801, 7.57452106, 5.90573359, 8.81113243, 5.90509462, - 5.90493393, 7.5733242, 7.57331848, 5.90438271, 3.52005005, 7.57231808, - 5.90374947, 8.80777359, 7.57106209, 7.57073402, 5.90271568, 5.90264273, - 7.57040215, 5.90211487, 5.70867062, 7.56980038, 5.90182209, 5.90195942, - 5.70824623, 5.90112543, 5.901227, 9.99766445, 8.80345154, 5.70712042, - 7.56760883, 5.90027857, 5.70721531, 9.996418, 11.0628948, 8.80273819, - 8.80233288, 8.80211735, 7.56650972, 5.89913464, 5.89922285, 7.56639433, - 5.89928055, 7.56584692, 14.6415634, 13.8529606, 13.8529606, 13.8518286, - 13.8510284, 13.8508673, 14.6368952, 7.56291294, 5.8966918, 5.89646769, - 5.89606333, 5.89575672, 7.56211329, 5.89588976, 5.89584208, 5.89574051, - 5.89574051, 8.79655838, 7.41026831, 3.51455522, 7.56061602, 7.56006813, - 8.79475403, 5.8941164, 3.17706466, 8.79417038, 7.55984545, 13.8435545, - 14.6309185, 18.0602055, 21.46311, 13.8410463, 14.6279182, 5.89305735, - 7.55800104, 21.4567432, 14.6249208, 18.6629696, 18.0538921, 7.55694532, - 13.7587976, 8.79062653, 11.1503181, 19.198225, 18.6029606, 21.4041195, - 15.3740301, 14.5507851, 5.69944525, 3.17618728, 16.0819206, 8.91504288, - 12.1085167, 3.51208878, 5.69738436, 7.55418444, 12.1065035, 7.55293274, - 7.55257845, 5.88781118, 5.69517422, 5.88728333, 9.9748106, 7.55189562, - 7.55066681, 5.88728809, 5.88698292, 5.88703537, 5.88623238, 7.548944, - 7.54850149, 7.54843664, 7.54820967, 5.69214964, 8.77977943, 8.77888489, - 3.17152977, 3.50804186, 3.50766969, 3.50745177, 7.5450263, 5.88237381, - 7.54472256, 3.50729895, 7.54506397, 5.88232136, 7.54403973, 5.88174534, - 7.54376936, 5.8811903, 7.54256535, 5.8807888, 5.8807888, 5.88153028, - 17.3269634, 12.0900116, 9.96597195, 7.54411936, 5.88186884, 3.50686049, - 13.7364082, 8.77676105, 5.68996096, 15.2737284, 15.2755785, 21.8864517, - 18.6330624, 20.295948, 8.775877, 12.8940296, 14.5240755, 15.3424206, - 11.1286554, 5.88024759, 20.8816242, 15.9809971, 14.5172968, 9.96080685, - 14.515296, 7.54014874, 22.3719063, 14.5166759, 11.02388, 9.96124458, - 7.38803196, 11.9899931, 7.53835678, 18.0053711, 7.53725672, 3.16737843, - 8.89224243, 8.89128685, 7.5351553, 7.53442574, 5.87405491, 5.87395048, - 7.53308868, 5.87336445, 5.87308741, 7.53240252, 3.5012641, 7.53184843, - 7.53170776, 9.94887161, 3.5008297, 7.53099155, 5.8717103, 7.38044024, - 9.94781113, 7.38039112, 9.94709587, 8.75895786, 11.0081139, 11.0077763, - 11.0073862, 8.7583437, 9.94490814, 8.75810146, 11.0063667, 9.94497681, - 8.75695705, 8.7569828, 9.94467449, 13.7046757, 9.94400597, 9.94365883, - 11.0043039, 9.94299889, 9.94214058, 9.94256878, 8.75529194, 3.16271257, - 7.52530813, 7.52471828, 8.87760067, 7.52464008, 8.87749767, 5.86629963, - 5.86602926, 5.86596203, 5.86562824, 7.52262306, 7.52262306, 5.86535025, - 5.86464834, 5.86472988, 7.52198124, 7.52195549, 5.86500931, 13.6937132, - 5.86376715, 7.5212779, 5.86410475, 14.5562744, 14.5558281, 20.286005, - 12.9420042, 12.9420042, 12.0523376, 17.9615993, 3.16006231, 13.7703552, - 7.51991224, 8.87221432, 5.8630209, 10.9947348, 14.5522537, 16.0044823, - 17.8985424, 15.2225933, 11.9602118, 5.67070436, 8.87197971, 3.16022062, - 17.3327236, 10.9925241, 7.51751947, 10.9919729, 10.0436144, 16.6122379, - 19.0968742, 16.679575, 5.86168528, 9.93158054, 17.2660236, 13.6850471, - 3.1589725, 15.9292192, 12.8482265, 15.9275255, 14.5443907, 12.0454245, - 12.043993, 16.6088505, 12.8444777, 13.6808891, 5.85871172, 10.9858913, - 7.36373615, 5.85734892, 7.5122447, 18.5502243, 13.7548561, 8.73679829, - 7.50943947, 7.36028481, 5.8540802, 5.85411739, 10.028636, 5.8535862, - 5.85346127, 5.85250616, 5.85248756, 9.916399, 8.73152637, 9.91540909, - 11.9372301, 9.91480827, 3.15411234, 9.91410351, 10.971962, 7.35516357, - 7.50458527, 12.8273458, 10.9705267, 3.48810863, 7.50343084, 7.50357103, - 3.15338063, 12.9113903, 10.0218544, 10.9687567, 9.91088104, 8.85136986, - 7.50158691, 5.84887314, 8.85038567, 5.84821701, 7.50086641, 13.7356548, - 12.0208664, 13.7344933, 13.7342796, 14.5146351, 13.7327528, 13.7327929, - 5.84692144, 3.48604202, 3.15172839, 5.84637213, 3.48564816, 5.65416288, - 12.8166904, 3.48526049, 5.8453927, 3.48515534, 3.15076637, 3.15066004, - 7.49680471, 7.49658298, 7.49641609, 7.4963727, 10.013236, 9.90289497, - 3.15047836, 5.6531353, 3.48437834, 15.1750832, 7.49591303, 13.6464128, - 12.8123856, 15.9518337, 3.14978647, 7.3448987, 3.14948559, 8.71751499, - 13.6418591, 10.9552145, 15.1683817, 15.1688318, 7.34346867, 9.89787674, - 5.84149551, 12.8922119, 7.34139299, 9.89566994, 5.8394556, 7.33998203, - 9.89331341, 15.1611853, 13.6333036, 8.71154499, 3.14724207, 9.89174652, - 11.9093056, 8.71002197, 3.14657927, 19.017168, 11.9070473, 9.8891592, - 8.70848274, 9.88809776, 9.8880415, 9.88718987, 9.88680744, 9.88742828, - 8.70623684, 8.70623684, 10.9411583, 9.88601685, 8.70497227, 8.70520401, - 9.88480377, 9.88524437, 10.9409876, 9.88557434, 8.70570374, 8.70604229, - 9.88658524, 13.6247263, 15.1495647, 14.4064274, 15.1492958, 15.1462002, - 14.402627, 15.8539314, 14.402194, 8.70189762, 9.88022804, 7.47924376, - 5.83120632, 8.82364464, 14.4729824, 8.69941711, 8.82439804, 7.47932005, - 3.14320707, 7.4791069, 16.5240345, 7.47758198, 3.47609878, 14.4699306, - 14.3936262, 5.63830233, 9.87573242, 8.69695187, 3.14167929, 10.9295301, - 5.82779312, 8.69486237, 8.81847572, 3.47427964, 8.81823635, 5.82679272, - 3.14077401, 5.82619143, 5.82637501, 3.47374916, 7.47240067, 7.47214031, - 7.47163057, 5.82537222, 7.47158813, 5.82511091, 7.47028446, 5.82436466, - 13.6800699, 8.81438065, 3.47277617, 5.82415915, 5.82390451, 5.63260746, - 8.81227589, 9.86588478, 9.86631489, 10.9185028, 10.9183207, 9.86495876, - 14.3762379, 8.68683243, 12.7633791, 7.3175931, 8.68562126, 7.31758928, - 7.4660821, 8.68542099, 13.5916128, 9.86256695, 9.86188889, 5.82047939, - 8.68507957, 7.46541977, 7.46502686, 5.81988525, 8.80524349, 9.85774803, - 8.68084335, 9.85773849, 8.68115616, 10.9101753, 9.85814953, 8.68036366, - 10.9089804, 9.85655594, 9.85621262, 8.67896843, 9.85491276, 8.67817783, - 8.67759514, 7.31015348, 8.67685699, 8.6767292, 8.6767292, 8.67611599, - 7.3095603, 8.67613602, 9.85182953, 9.85193729, 8.67585945, 9.85147858, - 8.67547417, 5.62384844, 8.67473984, 11.8603725, 14.35604, 10.9013405, - 8.67401886, 7.30781794, 3.4657104, 3.46579385, 8.67320251, 5.62253523, - 5.62217522, 9.84775925, 8.67211056, 3.46522188, 7.45505095, 13.650631, - 13.6504803, 11.9472103, 7.45400667, 8.79409027, 7.45340538, 13.6466932, - 8.79329967, 7.4521656, 7.45243359, 7.45174742, 7.45118809, 3.46361041, - 5.80932331, 8.79106331, 5.80902815, 7.45007992, 7.44989204, 8.78967667, - 7.44889832, 7.44889832, 7.44834805, 7.4484601, 8.78767967, 13.6386652, - 14.4131374, 7.29913425, 7.44768524, 13.6389961, 8.66486454, 15.0812426, - 9.83869839, 15.0812044, 8.6646452, 20.5739174, 18.3368721, 15.0789013, - 3.46231365, 10.8874598, 5.80624533, 10.8859959, 10.9845505, 5.80489826, - 8.65997314, 8.78267765, 9.83258247, 9.8324728, 12.7214499, 8.65780067, - 12.7210712, 7.29358625, 3.12734723, 9.82996082, 8.65587044, 8.65583801, - 11.8338413, 9.82882977, 3.12701583, 5.80110788, 7.29240561, 7.43994665, - 5.80024099, 7.43808556, 7.43798971, 5.79900932, 9.82524872, 9.82458591, - 10.8727179, 9.82462692, 8.65087032, 7.43639278, 5.79781055, 5.79779339, - 5.7975812, 7.43564558, 7.43540907, 8.77233982, 5.79677963, 7.4341135, - 5.79638863, 8.7716198, 5.79592085, 7.28544378, 5.79576778, 8.77002716, - 3.45542502, 5.79511881, 5.79520941, 7.43236446, 7.43200254, 5.79479361, - 9.92728043, 7.28416014, 8.64587307, 7.43206024, 9.81776905, 5.79380512, - 3.45449662, 3.4540453, 5.79264307, 7.4290123, 7.4288578, 7.42904425, - 5.79170322, 5.79113483, 13.5217352, 7.42803335, 3.45296407, 7.42888308, - 13.6047716, 8.76586437, 15.110465, 20.03512, 9.8121748, 5.79180527, - 13.5237265, 7.27980566, 3.45322418, 13.5222874, 12.6950302, 9.92028236, - 17.6815872, 8.76396084, 7.27882767, 22.0851669, 5.60044765, 11.9009361, - 8.7619009, 15.0342369, 11.8088474, 9.8081007, 7.42443228, 16.4051266, - 16.4060574, 10.8552933, 9.80659771, 7.27552366, 5.59900427, 10.8532782, - 5.59800625, 7.42302227, 15.796587, 13.5907383, 11.8937111, 9.91114998, - 7.41996193, 3.11819744, 5.78472853, 5.78457594, 7.41931486, 7.41880751, - 5.59418917, 7.41782093, 3.11745501, 8.62905502, 5.78347492, 5.78319597, - 5.78286695, 3.11710668, 5.78272486, 3.44764471, 8.75021839, 5.78220654, - 8.7492218, 8.75000477, 5.7819829, 5.59249353, 3.44743705, 13.5007935, - 10.8415785, 3.44717216, 5.78177118, 21.5569477, 18.309206, 7.41312885, - 5.59105015, 9.90161419, 14.344491, 11.879159, 9.90094376, 7.41229677, - 5.77897692, 3.44529057, 10.835639, 7.4111681, 11.7871685, 5.58878803, - 10.833147, 3.44471908, 5.77715158, 7.40965986, 10.8336191, 5.77683592, - 3.11366892, 11.8745394, 10.9304428, 11.873518, 5.77614021, 3.11326933, - 7.40793514, 17.69627, 8.61650372, 7.40613174, 7.40622807, 3.44250846, - 7.40566587, 7.40540695, 7.4053669, 5.77315617, 8.73617458, 5.77283239, - 5.77267218, 3.44156098, 7.40304852, 5.77173042, 7.40273476, 5.77173615, - 7.40210485, 5.77075672, 3.44077015, 8.60992527, 8.60986996, 8.60950375, - 9.77627182, 8.60941505, 9.77604675, 9.77639008, 5.76942825, 7.39938593, - 5.76904154, 7.39928579, 5.76846838, 5.76821804, 3.43903422, 7.39803648, - 7.39770508, 5.76790428, 8.72783279, 5.76705694, 8.72697926, 5.76699162, - 8.60496044, 5.57771158, 8.60423279, 3.10805154, 3.4378283, 7.39480686, - 5.76571417, 5.76509857, 7.39430475, 7.24683619, 7.39415264, 7.39404964, - 3.43715072, 5.76473475, 7.39358902, 17.0452709, 11.8486586, 14.3069363, - 15.7345409, 13.5376711, 12.7208967, 15.7323389, 12.7198877, 16.3974838, - 10.9045601, 3.10662079, 10.9062347, 5.76319361, 20.9866962, 18.1966095, - 7.3908267, 5.7620306, 11.8450851, 5.76132202, 8.59600353, 7.38931561, - 13.5305614, 3.43460512, 14.2986135, 8.71657467, 7.38711691, 7.38703823, - 7.38661289, 5.75870371, 5.75860214, 12.7098904, 13.5243816, 13.5236397, - 13.5237389, 7.38523149, 15.7200203, 18.2381783, 14.218421, 17.0264339, - 10.8962994, 5.75756788, 12.7081823, 5.75714159, 7.23649216, 11.8327971, - 3.10269642, 5.75574493, 5.56741476, 5.75545979, 9.75146198, 5.75511742, - 7.38097477, 5.75450039, 8.70804882, 9.85850811, 3.1017673, 7.38024855, - 7.37978458, 10.8878956, 8.70732117, 13.5131989, 8.70693398, 11.8262529, - 7.37872934, 7.37872934, 5.75190592, 7.37753677, 5.7514081, 8.70358944, - 5.75125217, 7.3763485, 5.75083733, 7.37556267, 8.70227242, 5.75040436, - 8.57962799, 5.74988985, 7.37502003, 3.09944963, 3.42831373, 5.74961567, - 7.37464857, 8.57805634, 5.56080246, 13.4250593, 8.69937897, 3.42752814, - 8.57653999, 7.3726306, 5.5597806, 3.42672896, 3.0978322, 5.74655008, - 8.69602871, 5.74643707, 5.74623203, 5.74569893, 5.55750895, 10.774189, - 7.36925697, 8.57281017, 10.7745285, 7.36829853, 7.22166538, 5.74507713, - 5.74542284, 8.69338703, 7.36738777, 7.36733103, 7.36733103, 7.36636686, - 5.74319935, 7.36565113, 7.36544943, 7.36552715, 5.74228716, 5.74254274, - 12.6742077, 8.68988228, 5.74237394, 5.74235773, 7.36591196, 3.42408347, - 9.73090267, 14.9130735, 12.5920601, 10.7685404, 9.72865582, 3.09493136, - 7.364048, 7.36252928, 7.36227798, 5.73950481, 7.36153841, 8.6858778, - 5.73961067, 7.36086559, 5.73898411, 7.35992146, 3.42130971, 7.21275187, - 7.35890388, 5.73750591, 7.35829687, 3.42033553, 7.35786152, 7.35763025, - 5.73669243, 8.55848408, 9.7185154, 9.71834183, 8.55777645, 11.6998901, - 15.5888481, 18.6850491, 18.1078682, 14.1605101, 14.1606989, 8.5559845, - 9.71669674, 9.71689415, 9.71627617, 11.6987944, 14.8916569, 14.1609287, - 14.8913336, 18.1069279, 15.588521, 18.1044998, 19.7838402, 18.6823082, - 18.1044254, 15.5849552, 15.5845051, 14.8863811, 15.583209, 15.5828428, - 15.5816641, 8.55233002, 8.55243015, 9.71134281, 9.71016407, 10.7459354, - 8.54990101, 3.4165833, 8.54955006, 8.67101288, 7.34855843, 8.67008781, - 7.20189142, 9.70734882, 5.54145098, 7.20091009, 5.54057217, 5.72843313, - 3.08786201, 10.7406721, 8.54628468, 8.6685791, 3.4153161, 5.72809029, - 10.7401762, 8.54476261, 7.34533024, 7.34466743, 5.72627544, 9.7016716, - 14.1385632, 14.8678169, 18.6559753, 19.2146511, 14.8707542, 9.70253181, - 12.6384172, 18.7110023, 24.1363297, 29.9945183, 28.5494843, 24.5701256, - 22.7867966, 20.3358383, 16.9297905, 8.66628265, 3.41471505, 15.5681391, - 3.08670807, 9.70020962, 21.7850838, 8.66056442, 7.34023619, 5.53433371, - 8.53704834, 8.53615284, 5.72105408, 9.69300747, 7.33737898, 7.33769798, - 7.3374176, 5.72054672, 7.19057465, 5.72041559, 8.65637779, 7.33691359, - 8.65639496, 5.72024679, 7.33557701, 7.33642292, 8.65599632, 7.33532858, - 7.33423567, 5.71784258, 7.33382082, 3.40946746, 7.33426523, 8.53205967, - 3.08222651, 8.53118134, 5.71757603, 7.33500862, 9.68956375, 5.71717072, - 8.52994537, 5.71711111, 5.52946949, 5.71709204, 8.52985287, 3.40861797, - 7.33205462, 3.08130717, 5.52933645, 7.33204174, 3.08144379, 7.33185005, - 5.71637297, 8.65053272, 8.5290184, 5.71633625, 5.71606779, 5.52886724, - 3.40799928, 8.52852249, 5.71598864, 5.71596527, 5.71591711, 5.71599197, - 5.71591282, 5.71579123, 7.33102179, 5.71558762, 8.64934444, 9.68376827, - 7.3306036, 5.71549988, 5.52823782, 5.52809668, 5.71524429, 7.33029747, - 5.52787828, 7.33027077, 5.71500874, 8.6485815, 7.32993603, 3.08043671, - 3.0804255, 3.40727091, 7.32977962, 5.52748489, 7.32977104, 3.40712094, - 7.32964754, 7.32955313, 5.71469593, 5.71446753, 7.32917643, 7.32915306, - 7.32907629, 7.32907009, 7.32907867, 7.32907867, 7.32907867, 9.68145275, - 9.68138981, 8.52562809, 8.52546883, 5.5268321, 5.71388054, 8.52541637, - 10.7142067, 8.52504253, 7.32844257, 3.40654755, 7.32812405, 8.64602661, - 7.32803392, 8.52463436, 8.52429867, 3.07958961, 5.71311378, 7.32758713, - 3.40624785, 7.18165255, 5.525877, 7.32721233, 8.52375984, 5.5257287, - 7.18110085, 8.64455318, 7.32691574, 9.67916393, 8.64510822, 8.52336407, - 3.07922649, 3.40596676, 8.52309704, 5.52523184, 3.40578103, 7.1805253, - 3.07907796, 8.52287579, 3.40578437, 7.32650995, 8.52276802, 5.5250659, - 7.18003178, 7.32606411, 9.67736626, 7.32594252, 7.32561922, 5.52457809, - 5.71172333, 7.32587147, 7.32558393, 5.71154308, 3.07864118, 3.40527081, - 7.32534647, 3.40520215, 3.07847238, 8.52123356, 5.7112093, 7.32506227, - 3.40512896, 3.40513992, 7.17894268, 8.52100563, 10.7090998, 7.17876673, - 5.71078348, 7.32468939, 7.32455254, 5.52389622, 9.67565823, 8.52025509, - 3.40475035, 5.71057653, 7.17803335, 5.52343035, 9.67469978, 8.51950264, - 5.52306175, 5.71000004, 7.32362556, 3.40439606, 5.71015453, 8.51970959, - 9.67461109, 8.51972294, 5.71001959, 8.51946449, 8.6409235, 5.52275801, - 3.07758164, 3.07757521, 5.70954037, 9.67390442, 8.51858997, 5.70954752, - 7.17677784, 5.70932627, 8.63984489, 8.51840019, 7.1767087, 8.5179615, - 3.40394759, 3.07742095, 5.52203035, 5.7087965, 10.7046194, 7.17612791, - 7.17605352, 3.40371656, 7.17600727, 3.07709122, 3.40358305, 3.40352035, - 9.67196369, 3.07705402, 8.51684856, 8.51724625, 9.67196083, 9.67133617, - 7.17545891, 9.6715517, 7.17527914, 7.17525196, 5.52117157, 7.17518711, - 8.51605606, 8.51620865, 7.3206954, 7.17480898, 9.67052841, 9.67057133, - 7.17472363, 8.51604271, 9.67036438, 5.70748329, 7.32044363, 7.32005501, - 5.70732069, 7.32006073, 3.40279031, 5.70718479, 3.076262, 3.4026823, - 7.31974554, 5.70699739, 5.70705795, 5.70703506, 7.31975698, 7.31929588, - 7.31947756, 5.51968241, 5.70662832, 5.70674706, 7.17318487, 3.07591915, - 5.70630646, 7.31899405, 7.31910419, 9.66847801, 5.5192728, 5.70630121, - 8.63535118, 5.70634031, 7.31846333, 3.07567096, 5.70600796, 7.31816769, - 8.6349926, 8.63483906, 8.63470936, 7.31814766, 3.40185189, 5.51878786, - 5.70571232, 8.51295757, 9.66702271, 9.6670332, 9.6670332, 7.31749058, - 5.51852226, 8.51237774, 5.70518208, 5.5183301, 5.70519352, 5.51832533, - 8.6332531, 7.31706333, 7.31706762, 10.6971483, 7.17108583, 5.51784039, - 7.31666231, 5.70462227, 7.31663609, 5.70451498, 8.63243198, 5.70440722, - 8.6324091, 7.31635857, 7.31641865, 7.31618404, 5.70417404, 8.63213634, - 5.70418453, 7.31605053, 5.7040391, 5.70409012, 5.70409012, 5.70384455, - 5.70381212, 7.31574726, 7.31546926, 8.63131046, 7.31515789, 7.31518745, - 5.7035346, 7.31520176, 7.31520605, 5.703475, 5.70328999, 8.63070011, - 7.31489944, 8.50935555, 9.66327572, 5.70302248, 5.51628494, 8.6303587, - 8.63008785, 5.70297337, 5.70287991, 3.40010905, 9.66212463, 8.50860977, - 7.16836596, 3.40002346, 5.70242023, 3.3999629, 3.07381845, 5.51570892, - 5.70239353, 7.16796017, 5.51555109, 7.31377888, 9.66144466, 3.3997581, - 3.39975095, 5.70206738, 5.51543951, 9.66102123, 8.50770569, 10.6921635, - 8.50756454, 8.50756454, 9.66057873, 5.70181751, 8.6281929, 5.70176363, - 8.50720215, 9.66036797, 3.39944601, 9.65995979, 8.50679016, 7.3127265, - 7.16693878, 7.31255579, 7.31233501, 5.70138693, 7.31251287, 8.6274128, - 7.31218863, 8.62734795, 8.62716675, 3.39904451, 7.31181049, 7.31180286, - 7.31180239, 7.16593838, 5.7007699, 7.31158733, 5.70069933, 5.7006197, - 5.70048332, 8.50522995, 7.16572523, 8.50513268, 5.70038033, 5.70032215, - 7.3112731, 5.51367331, 3.39864826, 7.31117296, 3.39856672, 3.07254386, - 3.39864731, 8.50465298, 8.50448132, 7.16486073, 7.16486073, 7.31053543, - 7.31048918, 9.65709972, 7.31036806, 3.39822006, 3.39830613, 3.39821124, - 3.07220984, 9.65679741, 5.51281738, 5.69955158, 5.69948959, 9.65639114, - 8.50368118, 7.31002235, 7.16426897, 7.30977631, 3.07203293, 7.30964804, - 7.16408777, 7.30961466, 7.30953741, 3.39781094, 7.30960417, 7.30995417, - 9.65701008, 9.65630341, 7.16406584, 3.07193494, 8.50309563, 9.65539646, - 8.50268841, 3.397614, 8.50216103, 3.07162356, 7.16316128, 7.16308022, - 8.62321949, 7.30839729, 7.30847454, 7.30862045, 5.69812489, 7.30848789, - 8.62301159, 7.30840778, 7.30819464, 8.6227808, 7.30822515, 7.30798149, - 5.69785929, 7.30798006, 7.30792618, 5.69767094, 5.69770193, 5.69770908, - 7.30768824, 3.39708877, 5.69759083, 7.30755043, 5.69751787, 3.39701033, - 7.30748796, 5.69744968, 7.30727673, 7.30733776, 3.39682651, 5.69733477, - 5.51059341, 7.16152716, 3.39675665, 8.50031376, 8.50032902, 3.07087898, - 7.16122484, 7.16122484, 8.49990082, 9.65222645, 5.51036692, 5.51029444, - 5.69687891, 8.49975586, 3.39652658, 5.51016045, 8.49973202, 9.65185261, - 3.07058573, 9.65155411, 3.07050586, 8.62021255, 7.30603552, 9.75903702, - 7.30602264, 8.49890137, 3.07032108, 5.50946045, 3.0703721, 7.30583477, - 7.30546331, 5.69586182, 7.30540466, 5.69594431, 7.30530691, 8.61933327, - 8.61940956, 7.30535555, 7.30503321, 7.30502748, 7.30484152, 5.6955471, - 7.30485249, 7.15925598, 5.50877047, 7.30447531, 5.69532871, 3.06985712, - 7.30446148, 3.06976914, 7.30452633, 5.50860548, 7.15887356, 8.61838341, - 5.50849247, 7.3042264, 8.49676228, 7.30423403, 8.49683285, 7.1584506, - 7.30379486, 5.69461346, 7.30385017, 5.6946125, 3.39526534, 7.30366373, - 7.15801001, 5.69443321, 7.15796804, 10.6775417, 9.64779377, 7.30318737, - 3.39503241, 3.394979, 7.30307388, 5.50757885, 3.06921887, 5.6940217, - 9.64715672, 5.5073576, 3.069067, 5.50735235, 7.15724516, 5.50726223, - 3.39470124, 7.30247498, 8.6161356, 8.61596775, 5.69364119, 7.30220318, - 7.30215263, 8.61557388, 7.30215931, 7.30215693, 7.30219841, 8.61549187, - 7.3020587, 5.69323587, 7.15638161, 5.50663424, 7.30178165, 7.15616465, - 9.64559937, 7.15617275, 7.30167341, 8.49353218, 9.64520741, 8.49363041, - 7.15576696, 7.30117083, 8.61443138, 7.30110645, 9.64477921, 7.30099154, - 5.69239426, 7.15529537, 8.49279308, 7.15523863, 9.64424324, 3.06821012, - 7.15509367, 8.49269676, 8.49250412, 5.50564957, 7.30046129, 3.39364696, - 7.15474939, 9.64367104, 3.06810641, 3.39359593, 8.49229908, 8.49224377, - 5.50551939, 3.0680449, 5.69187927, 3.39356494, 7.29989719, 7.15443802, - 7.29992199, 8.49157715, 5.50493097, 5.50493956, 5.69133043, 3.06765103, - 5.50474787, 8.61234474, 7.29929876, 7.15377283, 10.6713409, 8.49097538, - 3.39300895, 5.6907382, 3.06748652, 5.50434637, 5.69077873, 8.49069977, - 3.06737947, 7.29867887, 5.6907239, 7.29882622, 5.69087553, 7.29930353, - 7.29955053, 7.29959393, 5.69124317, 8.61256218, 8.61248875, 7.2994051, - 7.29961538, 3.39311075, 8.49127769, 5.69112015, 3.39315033, 5.6908989, - 5.69088316, 8.4907074, 7.2983427, 5.50406122, 8.48969841, 5.69001102, - 7.29756498, 9.74756527, 5.50329542, 7.2972908, 9.63954735, 3.39206457, - 9.63961124, 8.48875809, 3.0667243, 5.68925571, 7.29689837, 5.68906593, - 3.39190674, 7.29663801, 5.68902969, 7.29654026, 9.74601555, 7.29637432, - 7.29644966, 5.68871307, 3.06641293, 8.60858154, 8.60858154, 5.68865776, - 7.29608059, 7.29613972, 7.29603624, 7.1505909, 3.06619406, 7.15053701, - 7.15042162, 9.74508572, 7.29583311, 3.06610012, 7.15001535, 7.15003824, - 5.68807459, 5.68807459, 7.29522753, 7.29528952, 5.68791914, 8.60737705, - 5.68771458, 3.39116788, 3.06584191, 5.68765688, 3.39102769, 7.29487276, - 7.29466963, 7.29454947, 8.48577595, 7.29454947, 8.60665989, 3.39088583, - 8.48541641, 7.2944417, 7.29427719, 3.3907299, 5.50086117, 3.06556249, - 7.2940402, 7.29382801, 7.29395199, 5.68695021, 7.29373932, 7.29382277, - 5.68681002, 7.29381704, 8.48447418, 8.48460388, 9.63481712, 8.60543442, - 7.29327774, 7.2933588, 7.29342127, 7.29314089, 9.74167919, 8.60512829, - 8.60495377, 5.6861105, 7.29293919, 7.29285383, 10.6621866, 7.29273987, - 5.6858573, 8.48346901, 3.39004064, 8.4832983, 8.48323631, 8.48339748, - 7.14698744, 3.06470037, 5.49937773, 9.63312244, 8.6038456, 5.68542719, - 7.29191971, 7.29191971, 7.29199934, 7.29192877, 7.2917366, 7.29163313, - 5.68533516, 7.29171515, 7.29139137, 7.29155874, 5.68494654, 7.29152393, - 7.29132175, 7.29125643, 7.2911973, 9.73917007, 7.29119444, 7.29114914, - 7.29100657, 7.29097176, 7.29089594, 5.68463945, 7.14560175, 9.63114834, - 3.06403875, 7.14528179, 3.38911533, 9.63068485, 8.48089695, 5.68425226, - 7.14506865, 3.06389594, 3.38896346, 5.6839056, 8.48051262, 7.28993893, - 7.14467764, 3.06370878, 8.48044777, 9.62982559, 5.4974966, 3.06358171, - 7.14427233, 5.68353081, 3.06349874, 3.06354856, 8.60077763, 3.06346822, - 5.68327951, 7.28935671, 7.2889657, 7.28914356, 9.6288805, 5.68310118, - 8.60004807, 7.28889227, 7.28888083, 5.49684715, 7.2888236, 5.68279886, - 7.288764, 7.288764, 7.28836346, 8.5992918, 7.28818893, 3.06300473, - 7.2883172, 7.28810644, 5.68244076, 7.14273357, 7.14278316, 8.59887981, - 7.1426034, 5.68219185, 5.68204498, 8.59857178, 8.59857178, 7.14231586, - 5.49584293, 7.28761148, 8.59833431, 8.59820366, 8.59802341, 7.287323, - 7.28722334, 5.68155766, 3.06249881, 3.06248307, 3.06247115, 7.2869854, - 7.14160061, 3.38734245, 9.6256609, 5.49511671, 7.28685045, 8.59715271, - 7.28645372, 7.2864151, 7.28642416, 7.28635311, 7.28627062, 5.68088293, - 5.68082094, 7.28616285, 7.28599882, 8.59646702, 5.68064594, 8.59639835, - 5.68060207, 7.28564262, 8.59624386, 7.28575993, 7.28575134, 7.28571844, - 8.59575939, 5.68035793, 8.59611225, 5.68023634, 5.68023205, 5.68004704, - 7.28523064, 7.28523064, 5.68001842, 7.28505278, 8.59520054, 8.59512138, - 8.59511948, 7.28481102, 8.59529781, 7.28446913, 5.67973566, 5.679667, - 7.2845211, 5.6795516, 7.28445101, 8.5946703, 7.28438759, 7.28429794, - 7.28420591, 7.28411484, 7.28388739, 7.28407192, 5.67902899, 8.59405804, - 7.28396082, 7.28371763, 7.28365469, 7.2834816, 7.2836628, 8.59364796, - 7.28342772, 7.28342772, 7.28329754, 7.28345871, 8.59350967, 8.59315014, - 7.28313684, 7.28307009, 5.67845678, 8.59296608, 7.28301525, 7.2828064, - 7.2828393, 8.59268475, 7.28266621, 7.28255367, 7.28255367, 7.28249264, - 8.59241104, 8.59213543, 5.67778254, 7.28205585, 5.67780209, 7.28226757, - 5.67775249, 8.59200668, 7.28182983, 7.28211021, 9.72676086, 5.67750311, - 8.59162521, 7.28180647, 7.2817049, 7.28175116, 8.59154224, 8.59133816, - 8.59134007, 7.28136969, 7.28141689, 5.6770339, 7.28114414, 7.28130388, - 7.28135824, 8.59064388, 5.67697334, 7.2809701, 8.59063911, 8.59063339, - 8.59053802, 7.28089809, 5.67668581, 7.28072929, 8.59018421, 7.28073072, - 5.67657709, 7.28063488, 7.28065014, 8.59002399, 7.280406, 7.28045797, - 8.58985615, 7.28030729, 8.58961201, 8.58974934, 8.58972836, 7.27991867, - 7.27991295, 5.67601109, 7.27988863, 5.67583466, 7.2799058, 7.27987814, - 7.27959394, 7.27970791, 5.67566109, 8.5891304, 7.27969217, 7.27952147, - 7.27936649, 7.27922583, 7.27919674, 7.27942848, 7.27934074, 5.67551661, - 5.67540455, 7.27901697, 7.27914906, 7.27887917, 7.27889633, 5.67508745, - 7.27886629, 7.27886629, 7.27862692, 8.58780956, 7.27848768, 8.58749866, - 5.67458725, 5.67473555, 7.27821875, 8.58719921, 7.27792549, 7.27775717, - 8.58693981, 5.67426872, 7.27794695, 8.58686256, 7.27777052, 8.58660412, - 7.27760696, 5.67413521, 7.27753687, 7.27736712, 8.58640003, 7.27751112, - 7.27741766, 7.27722788, 7.27712297, 5.67393017, 8.58610249, 7.2771821, - 8.58593178, 7.27698135, 5.67377758, 7.27689743, 7.27684689, 7.27676678, - 7.27680969, 8.58567142, 7.27694035, 8.58540249, 7.27674294, 7.27645636, - 7.27640009, 7.27643728, 5.67313528, 8.5851078, 7.27628326, 7.27626657, - 7.276021, 8.58483601, 7.2760067, 7.27603245, 5.67285919, 5.67286682, - 7.27565193, 5.67275143, 7.27571201, 7.27539825, 7.27550745, 7.27558184, - 7.27527952, 7.27527952, 7.2752552, 7.27512074, 7.27513313, 8.58379459, - 7.27526569, 5.67221642, 7.2751112, 7.2747674, 7.27479362, 7.27464962, - 7.27463245, 7.27450418, 7.27443409, 7.27422714, 7.27422714, 5.67163658, - 7.27430725, 7.27431154, 7.27412367, 5.67159796, 5.67140436, 5.67157555, - 8.58245087, 8.58234882, 5.671381, 5.67125845, 8.58214378, 8.58221149, - 8.58221149, 8.58221149, 7.27353621, 7.27372932, 8.5820713, 7.27352142, - 5.6711092, 7.27355719, 5.67094803, 7.27350092, 7.27336311, 8.58151245, - 7.27336454, 5.67081642, 8.58142471, 8.58169079, 8.58169079, 7.27328873, - 5.6707325, 5.67067337, 8.58174229, 5.67085218, 7.27309227, 5.6706109, - 7.27269793, 7.27271032, 8.5809164, 7.27247858, 7.27243996, 7.27226782, - 7.27210712, 7.27210712, 5.66986513, 7.27210903, 8.58000278, 7.271873, - 5.66982937, 7.27184725, 8.57978725, 7.27154636, 8.57953548, 8.5796833, - 8.57945347, 7.27140713, 7.27150345, 7.27127695, 8.5792017, 5.6693716, - 8.57924366, 5.66915131, 7.27121258, 7.27109051, 8.57895756, 7.27115345, - 5.66901112, 7.27089357, 5.66904831, 7.27086782, 7.27083111, 8.57875443, - 7.27106428, 8.57894993, 5.66977692, 7.27214909, 5.66988945, 8.57999039, - 7.27181768, 7.27128601, 8.57917976, 7.27082872, 7.27078581, 8.57881451, - 8.57858276, 7.27065468, 8.57843113, 7.27071047, 7.27071047, 5.66879702, - 7.27090549, 7.27118158, 8.57941723, 7.27182341, 7.27181053, 7.27163839, - 5.66948271, 8.57809925, 8.5777216, 9.71102142, 5.66807318, 7.26970196, - 5.66787386, 8.57713985, 5.66773224, 7.26935768, 7.269279, 7.26934481, - 7.26915216, 8.57688904, 8.57677841, 8.57642937, 7.26909494, 7.26906061, - 7.26902151, 7.26898718, 8.57672405, 8.5763073, 7.26882458, 5.66729212, - 5.66724777, 7.26880407, 7.2685833, 5.66726017, 5.66730022, 5.66737795, - 7.26958942, 9.71054077, 7.26969814, 7.26925039, 7.2689085, 7.26868725, - 7.26872158, 8.57618618, 7.26831388, 7.26827765, 7.26794481, 7.26799202, - 5.66669798, 7.26793242, 7.26780081, 7.26803017, 8.57499313, 8.57492256, - 7.26753139, 7.26748371, 5.66635418, 5.66633749, 8.5748148, 5.66604805, - 7.2672286, 7.26729107, 8.5743351, 7.26704168, 7.26694441, 7.26696014, - 7.26676178, 7.26698589, 7.26688194, 7.26682186, 5.66572285, 7.26662922, - 9.70642567, 8.57377815, 5.66557884, 7.26650429, 7.26663828, 8.57352352, - 8.57339859, 7.26642323, 8.57377243, 5.66539001, 7.26633692, 7.26619768, - 8.57332134, 5.66534138, 7.26617241, 7.26618004, 7.26618004, 5.66522455, - 7.2659688, 7.2658, 7.26574278, 8.57271767, 8.57287216, 9.70515251, - 8.57245636, 8.57258224, 7.2656002, 8.57234669, 5.66476393, 5.66470146, - 7.2655592, 7.2655592, 7.26534557, 7.26533127, 7.26530886, 7.2653203, - 7.26540327, 7.26537752, 8.57209492, 8.57210922, 8.5718956, 8.57183266, - 8.57183075, 7.26497793, 7.26499271, 8.57200241, 7.26517582, 7.26489639, - 5.66424179, 7.2648325, 7.26492786, 8.57171154, 7.26472521, 8.57144547, - 8.57144737, 7.26471519, 7.26462793, 7.26469374, 7.26451159, 9.703578, - 5.6639204, 5.6639204, 7.26432228, 8.57083797, 7.26400566, 8.57071114, - 9.70302963, 7.26415396, 7.26401424, 7.2640028, 8.57073021, 7.26399469, - 7.26404142, 5.66350365, 7.26395273, 7.26390314, 7.26390314, 7.26386452, - 7.26404285, 7.26393414, 7.26357412, 8.57027245, 8.57013988, 8.5700655, - 8.56978798, 7.26318026, 7.2632432, 7.26329851, 7.26309013, 8.56989765, - 7.26324272, 5.66297197, 5.66299725, 7.26351786, 8.57035255, 8.57072067, - 8.57069397, 7.26424217, 7.26418638, 7.26388836, 7.2640667, 7.2640934, - 7.2638855, 8.57019329, 5.6632967, 7.26353216, 7.26353216, 7.26312351, - 5.66272831, 9.70134068, 7.2625246, 8.5691061, 7.26247787, 8.56894398, - 7.26263428, 7.26249743, 7.26251698, 7.26243591, 7.26244164, 7.26230097, - 7.26245451, 8.56879234, 7.26233006, 5.66236639, 7.26285219, 7.26315546, - 7.26316977, 7.26304722, 8.56974411, 8.56950855, 7.26286602, 8.56934738, - 7.26285267, 7.26222086, 9.70018578, 7.2619586, 8.56820774, 7.26159763, - 7.26171827, 8.56776714, 8.56768799, 8.56782532, 8.56767941, 7.26149035, - 5.66135073, 7.2610445, 5.66129827, 7.26097822, 7.26100349, 8.5669651, - 7.26106882, 7.26106882, 5.66111708, 7.26081324, 8.56680489, 8.566782, - 7.26056385, 5.66096687, 7.2607851, 7.26062346, 8.56641197, 7.26057196, - 5.66081905, 7.26049137, 5.66070509, 8.5663147, 7.260355, 5.66067266, - 7.26030159, 5.66066694, 8.56612587, 7.26016903, 5.66065979, 7.26020193, - 7.26015854, 5.66048193, 7.2601285, 5.66040564, 7.26003408, 7.25977516, - 7.25979948, 7.25979948, 7.25990009, 8.56573582, 8.56567383, 8.56564713, - 8.56548023, 8.56536484, 7.25976038, 7.25939655, 8.56517696, 7.25946617, - 7.2592926, 7.25933552, 7.25912762, 7.259233, 7.259233, 7.25921249, - 8.56482887, 8.56491661, 8.5647707, 7.25928497, 7.25927353, 5.65945339, - 7.25881672, 8.56448746, 7.25888014, 7.25897217, 7.25861311, 7.25880337, - 7.25864649, 8.56429958, 8.56421947, 7.25852537, 5.65925407, 7.25833559, - 8.56394577, 7.25835085, 5.65912724, 7.25824785, 8.56373119, 8.56378078, - 7.25805569, 5.65889263, 8.56371403, 8.56366253, 8.56366253, 8.56360912, - 8.56336212, 7.25795984, 7.25789499, 8.56327152, 7.25789213, 7.25780392, - 7.25759506, 7.2575593, 8.56294441, 7.25779676, 7.25740194, 7.25758219, - 8.56288433, 9.69409275, 7.25730848, 7.257442, 7.25724125, 5.6581893, - 7.25718641, 7.25729465, 7.25708008, 8.56261635, 8.56247234, 8.56270409, - 7.25689936, 7.25696611, 7.25700331, 7.25690079, 7.25690079, 8.56233883, - 7.2568922, 7.25666666, 7.25684166, 8.56217766, 7.25662184, 5.65772009, - 8.56178665, 7.25678968, 8.5621624, 7.25704145, 8.56277084, 8.56281567, - 7.25745487, 7.25745487, 7.25763321, 7.25728941, 5.65848398, 7.2572279, - 7.25712967, 7.25696802, 7.25671434, 5.65746641, 7.2564435, 8.56168652, - 7.25604296, 7.25612974, 7.25607109, 8.56153679, 7.25630569, 8.56204414, - 7.25693989, 8.56223011, 8.56249237, 8.56227398, 8.56167507, 7.25631618, - 5.65710688, 7.25574017, 8.56087303, 8.56084633, 7.25568295, 8.56063843, - 8.56056213, 7.25547934, 5.65683031, 7.25538158, 7.25533724, 8.56033134, - 5.65690374, 5.65655518, 7.25524092, 5.65650988, 8.56029987, 7.25498247, - 8.55978966, 7.25478649, 7.25467443, 9.69068527, 8.55987453, 7.25487518, - 7.25471067, 7.2545743, 5.6562438, 8.55962849, 5.65616846, 7.25484991, - 7.25441074, 7.25461817, 7.25455427, 8.55932999, 8.55924225, 9.69005775, - 7.25433588, 7.25433588, 8.55896854, 7.25437307, 7.25407887, 7.25405073, - 7.25412846, 7.25422907, 7.25415277, 7.25408459, 8.5588932, 5.65568542, - 8.55883312, 5.65561199, 8.55877876, 7.25379658, 7.25379658, 8.55869579, - 7.2537303, 7.25386381, 7.25361061, 5.65548229, 7.2535677, 5.65561199, - 7.25340509, 7.25353432, 7.25369596, 7.25324774, 8.55786228, 7.25330639, - 7.2533021, 8.55799294, 7.25335455, 7.25319529, 8.55805206, 8.55793667, - 8.5575819, 5.65506649, 8.55790138, 5.65501547, 7.25293398, 7.2528553, - 8.5573616, 8.55728531, 9.68801403, 8.5574913, 7.25287628, 7.25277567, - 7.25278425, 7.25278997, 7.25253439, 8.55725384, 7.25249672, 8.55700302, - 7.25253105, 8.55709553, 8.5569582, 7.25251722, 8.55688763, 8.5569849, - 8.55723763, 8.55723763, 7.25223207, 8.55659676, 8.55668068, 5.65439987, - 7.25216007, 7.25211763, 7.25200033, 7.25216198, 7.25210667, 7.25187397, - 7.25212336, 7.25184345, 7.25200224, 7.25176382, 8.55617809, 7.25167179, - 7.25205994, 8.55602264, 7.25169182, 8.55579948, 7.25172567, 7.25170994, - 5.65390444, 8.55595493, 7.25141478, 8.55583096, 8.55572987, 8.55553055, - 9.68609142, 7.25145721, 7.25135231, 7.25115252, 5.65349722, 8.55524635, - 8.55536175, 7.25137615, 7.25113249, 8.55534649, 8.55531979, 7.25079775, - 7.25078535, 8.5548172, 8.5549469, 8.55503178, 7.2507925, 7.25085068, - 9.68505383, 7.25059462, 7.25072145, 8.55459213, 8.55461311, 8.55462551, - 7.25028563, 8.55433846, 7.25050592, 7.25021172, 5.65288115, 7.25018215, - 8.55431271, 8.55431271, 8.55459404, 8.55426311, 8.55444622, 8.55416203, - 8.55414963, 7.25023746, 7.25019789, 7.25023174, 9.68446255, 8.55406952, - 8.55403042, 8.5539856, 8.55387115, 8.55405903, 5.65262175, 5.65249205, - 7.24991274, 7.24984884, 7.24983072, 8.55391216, 7.24978399, 7.24971724, - 8.55356598, 7.24967194, 7.24956989, 8.55361843, 8.55347538, 7.24954319, - 7.24937868, 9.68331528, 7.24953318, 5.65228748, 5.652246, 8.55346203, - 8.55348969, 7.24943733, 7.24951839, 9.68309498, 8.55303669, 7.24932718, - 8.55316162, 8.55313492, 8.55292797, 5.6518712, 7.24900436, 7.24881744, - 8.552742, 7.24881029, 8.55268097, 7.24863386, 8.55238819, 8.55251598, - 8.55226612, 8.55213356, 7.2486558, 7.24858665, 7.24843979, 8.5519743, - 7.2485404, 8.55237484, 5.6512804, 9.68210697, 5.65125036, 5.65126991, - 7.24832678, 7.24830437, 8.55171585, 8.55191326, 7.24792719, 5.65123224, - 5.65116167, 5.65111923, 7.10352278, 8.55168629, 7.24795628, 7.24798918, - 5.65098667, 7.24778652, 5.65081835, 7.24775505, 8.55150986, 8.55104733, - 7.24750519, 7.24751616, 7.24740601, 5.65071297, 7.2471199, 8.55104923, - 7.24718094, 7.24718094, 5.65045595, 5.65034771, 7.24705362, 8.55052853, - 8.55055237, 9.68021393, 7.24703074, 7.24700546, 7.2468791, 8.55047607, - 8.55031967, 8.55006409, 7.24671173, 8.55018425, 5.65006161, 7.24654198, - 7.24667978, 8.55024052, 8.55002308, 8.54984283, 7.2463932, 8.549963, - 7.24624062, 5.64966202, 8.54976463, 7.2464385, 7.24616528, 5.64968157, - 7.24625301, 7.24625301, 5.64964485, 7.24600124, 7.24617243, 7.24603462, - 7.24565554, 7.24591446, 8.54931736, 7.24572277, 7.24607801, 7.24639654, - 7.24686337, 5.64997101, 5.46363926, 7.25156784, 3.37094092, 3.3684454, - 8.42868042, 9.57192612, 8.42880917, 8.42844296, 8.42857552, 9.57120419, - 8.42841721, 7.24512768, 7.24531698, 7.24521542, 7.24513388, 8.54863071, - 7.24524355, 7.24524355, 7.2449584, 7.24519587, 8.54811764, 9.67825794, - 8.54873371, 7.10020876, 8.4287529, 8.42825603, 8.42836761, 7.10093164, - 9.57135582, 9.57128143, 9.57122707, 9.5712347, 8.428545, 8.42840672, - 8.42864037, 9.57113743, 8.42865944, 7.10092306, 7.1008482, 9.57125187, - 8.42864418, 8.42878723, 8.42864037, 9.57126236, 9.57108593, 8.42838001, - 8.42839146, 9.57105923, 9.5710125, 9.57113552, 9.57125187, 7.10095406, - 7.1009655, 8.42864895, 8.4284811, 8.42847538, 8.42857075, 8.4283371, - 8.42842674, 8.42862797, 8.42862415, 9.57119846, 8.42851353, 8.42839527, - 8.42869663, 8.42868996, 7.10073709, 7.10088396, 9.57122993, 9.57119274, - 7.1010108, 8.42820644, 7.10094309, 8.428442, 8.42838955, 7.10102987, - 8.42846394, 7.10093975, 8.42840576, 8.42843342, 8.42841434, 7.10092974, - 8.42848778, 9.57113075, 8.42837715, 7.10103798, 8.42836857, 7.10095501, - 8.4285183, 9.57118511, 8.42850304, 8.42868614, 7.10112667, 7.10101271, - 8.4285059, 8.42863846, 7.10092402, 8.42842674, 8.42856693, 9.57147312, - 8.42837334, 8.42853928, 9.57138634, 8.42846012, 8.42863846, 8.42846298, - 8.42854691, 7.10100889, 8.42851162, 8.42870522, 7.10092068, 8.42854214, - 7.1009593, 8.4288168, 7.10105085, 9.57123661, 7.24546003, 7.24555874, - 5.64907312, 7.24532652, 8.54855824, 8.54862595, 7.24536324, 8.54894733, - 8.54878044, 9.67820072, 7.24535894, 9.67810154, 8.54867935, 7.24545479, - 8.548522, 7.24543715, 9.67821407, 7.24544954, 8.54859447, 7.24540424, - 9.67797375, 8.54864597, 9.67788601, 7.24540663, 8.54864693, 7.24544239, - 7.24534893, 7.24559736, 7.24541664, 8.54880142, 8.54874325, 9.67810822, - 8.5487833, 7.24539614, 7.24536085, 8.54876518, 7.24548101, 8.54850388, - 8.5486517, 7.24551249, 8.5488081, 8.54863453, 7.2455368, 8.54872322, - 8.54858303, 9.6779995, 8.54874039, 7.24553061, 8.54874229, 7.24536753, - 8.5488472, 8.5488472, 7.24538708, 5.6490202, 9.67805672, 5.64908886, - 8.54893017, 7.24557877, 7.24542999, 8.54860115, 8.54863644, 7.2454915, - 7.24547625, 8.54874134, 7.24553537, 7.24555111, 8.54884815, 9.67792988, - 7.24539948, 7.2455492, 7.24530125, 7.24537611, 7.24520969, 7.24547386, - 8.5486784, 7.24558544, 8.54885006, 7.24545813, 7.24534845, 7.24542189, - 8.54862404, 9.67794895, 8.54870033, 7.24543905, 9.678298, 7.24546051, - 8.54885578, 7.24557638, 8.54884434, 7.24532843, 9.67792511, 8.54874706, - 5.64910603, 8.54879284, 7.24535847, 9.67820644, 8.54885197, 9.67829227, - 7.24568176, 8.54878044, 8.54878426, 9.67803574, 8.54872894, 7.24547672, - 8.54866505, 8.5488224, 8.54887772, 7.24542904, 7.24542189, 7.24536514, - 7.24555969, 8.54885864, 8.54864979, 9.67793369, 8.54869938, 8.54872608, - 7.24535036, 7.24536753, 7.24552155, 7.24548578, 5.64906788, 7.24538946, - 7.24533701, 7.24556303, 7.24538946, 8.54896259, 7.24564791, 8.54893398, - 8.5488739, 7.24552155, 9.67792606, 7.24553394, 7.24541426, 7.24551487, - 8.54875183, 7.24564695, 8.54863834, 9.67806911, 8.54866695, 3.04500675, - 7.10104704, 9.5712862, 8.42847538, 7.1011796, 8.4287672, 7.10095692, - 7.10102034, 8.42850971, 9.57134724, 8.42857933, 8.42859364, 8.4284935, - 8.4283762, 7.10103607, 7.10089874, 8.42836475, 5.46396208, 3.36807752, - 8.42859268, 7.10104084, 7.100914, 3.04496884, 8.42867851, 8.4285984, - 3.04501152, 5.64914608, 7.10101652, 8.42857456, 8.42857361, 8.42860985, - 8.42871094, 9.57142353, 8.42868614, 7.2454505, 8.4284029, 7.10101032, - 3.36808872, 3.04504347, 8.42857265, 7.10102367, 8.42846966, 5.64913654, - 8.42867661, 5.46405458, 5.4641037, 7.10097075, 7.10097075, 9.57109261, - 5.64907026, 9.57126427, 7.10087252, 5.64899302, 5.46416044, 8.42867565, - 7.10102081, 3.0449779, 3.04504228, 8.42847252, 7.10094595, 8.4284668, - 8.42854977, 8.42854977, 8.42850494, 7.2454505, 8.42854977, 7.1010952, - 9.57125759, 9.5714159, 3.36807466, 5.46410608, 7.10106945, 7.10103035, - 7.10110712, 3.36813903, 7.10105944, 7.10098886, 8.42852306, 8.4284153, - 8.42852592, 8.42850113, 3.0450294, 8.42829037, 8.4286375, 7.24541759, - 7.1008606, 8.42870426, 8.42858601, 3.36807442, 8.54880619, 8.42840481, - 7.10107565, 8.42862511, 7.10099316, 8.42841148, 5.6489954, 9.57114029, - 5.46398115, 7.24545479, 3.36805773, 8.42847729, 7.10098934, 5.46399784, - 3.36807752, 5.46398401, 7.10101175, 7.10096073, 8.42848873, 8.42824173, - 7.24545765, 5.46405935, 5.64890766, 3.3680203, 8.5487566, 9.57122612, - 7.10083675, 5.64896965, 5.46405077, 8.42843437, 8.54873371, 7.24538803, - 7.10098839, 3.04499364, 8.4284668, 5.6490097, 8.42846203, 8.42853737, - 3.04499555, 8.42841721, 8.54864311, 7.10086012, 7.10118818, 5.64909124, - 8.42842484, 7.10094309, 3.36813259, 7.24543285, 8.428545, 7.24546289, - 7.245327, 7.24537659, 8.5482502, 8.54854012, 7.24545431, 7.24539614, - 9.67787552, 9.6777153, 8.5486412, 7.24538374, 7.24531031, 8.54841423, - 8.54879951, 9.67814732, 8.54858971, 8.54860973, 8.5487566, 8.54844952, - 7.24545097, 9.67816162, 7.24541998, 8.54874897, 8.54876995, 8.5487833, - 9.67811584, 8.54877567, 7.24530172, 8.54862404, 8.54862404, 7.24525642, - 7.24555159, 7.24561071, 8.54864693, 8.54877472, 8.5489006, 7.24542665, - 9.67788696, 9.6779623, 8.54860687, 7.24537039, 8.54876709, 8.54876328, - 7.2455492, 7.2455492, 7.24531937, 7.24541426, 8.54865742, 9.67789841, - 8.54874897, 8.54868698, 8.54879856, 8.54867077, 9.67799473, 7.24543953, - 7.24538422, 8.54859543, 8.54849625, 7.24532604, 7.24532604, 8.54859734, - 8.54887295, 8.5487175, 8.54844284, 8.54866409, 8.54863548, 8.54869366, - 9.67795372, 7.24526262, 8.5487442, 7.24523973, 7.24526501, 8.54886913, - 10.6894016, 8.54865932, 7.24516201, 8.54880714, 7.24530602, 8.54856491, - 7.24531555, 8.54861736, 7.24542475, 8.54858685, 8.54860497, 8.54874706, - 8.5486145, 8.54876232, 8.54862118, 7.24545193, 8.5487318, 7.24531507, - 7.24539566, 8.5487566, 7.24530697, 9.6778307, 8.54864216, 8.5487957, - 9.67823124, 8.54878998, 9.67807007, 7.24551582, 7.24530745, 8.54856682, - 8.54863167, 9.67795753, 8.54868317, 8.54869652, 8.54869175, 7.24533987, - 9.6779089, 9.67794037, 8.54869556, 8.54859161, 7.24542713, 8.54850006, - 7.24544525, 7.2452364, 8.54864407, 7.24532986, 8.54858685, 8.54850101, - 8.54864216, 8.54854488, 9.67793751, 9.67785645, 8.54859447, 7.24539995, - 7.24516296, 8.54874802, 7.24538231, 7.10097742, 5.64900589, 8.54846287, - 9.67804337, 9.67804337, 9.67807293, 3.04497218, 7.10076523, 8.42840195, - 9.67793846, 8.42849731, 7.10094786, 5.6490345, 5.64898491, 8.42831802, - 7.24532843, 7.24534321, 7.10093927, 8.54858303, 8.42842102, 8.4285326, - 3.04494166, 7.24514961, 5.46398306, 5.46397352, 3.36808443, 8.42861557, - 7.10091591, 3.36799455, 3.36801362, 5.64899302, 7.24537277, 5.64897776, - 8.54866791, 9.67799759, 8.54859638, 8.42818928, 5.46391582, 9.67796993, - 8.54866982, 7.2453022, 3.36811757, 8.54859352, 8.54864693, 5.46399832, - 8.42839432, 7.10096121, 8.54865074, 7.24533892, 8.54859734, 8.54843903, - 5.46399736, 5.64901304, 8.54851532, 7.10091066, 8.42855453, 8.54862022, - 8.54861546, 9.67809486, 8.54871464, 3.36806703, 3.04499364, 5.6490078, - 8.54854012, 7.2452898, 8.42834663, 9.67762852, 7.24548531, 3.36801696, - 8.54853439, 7.24530888, 8.54851246, 7.10093832, 7.10067272, 8.54868984, - 8.54847813, 3.04493189, 8.5484705, 7.24545097, 3.04500961, 5.46406269, - 3.36800504, 7.10094357, 8.54851151, 8.54866886, 10.5927849, 7.24540377, - 7.10075283, 7.24534369, 8.54843235, 9.67783642, 7.24517918, 8.54879951, - 8.548522, 8.548522, 9.67778301, 8.54860115, 7.10093164, 8.42852306, - 5.64902306, 3.04495406, 7.24548578, 3.04489493, 3.04494214, 7.24534893, - 8.54843903, 5.46406937, 3.36802626, 9.67804337, 7.24543238, 5.64898205, - 8.54842758, 3.04494452, 3.04497337, 7.24522591, 8.54879475, 7.2453618, - 8.42841053, 9.57095623, 7.10093355, 5.46401739, 8.42823792, 8.54849911, - 8.54875183, 5.64911509, 7.24519968, 3.36812043, 7.24549627, 3.04498339, - 7.10086727, 8.42856026, 8.42859173, 3.04481268, 7.24532175, 3.36802101, - 8.42823792, 8.54860115, 3.36796093, 9.67775059, 9.67775059, 9.67768764, - 8.54860115, 9.67777348, 8.54845715, 8.54848385, 7.24536133, 8.54850864, - 8.54850769, 9.67781639, 7.24537849, 7.24520159, 8.54855919, 8.54855537, - 7.24519587, 8.54842854, 9.677948, 9.67787457, 8.5485487, 8.54859638, - 8.54852486, 8.54869843, 8.54856968, 7.24519157, 10.6890306, 8.54838657, - 7.24541616, 8.54855347, 9.67801189, 8.54869461, 8.54869461, 7.24515867, - 8.5485239, 8.54871655, 8.54845047, 8.54845428, 7.24527645, 8.54839706, - 7.24514294, 7.24548101, 8.54843044, 8.54857635, 8.54848385, 8.54835129, - 8.5487299, 8.5487299, 8.54850483, 8.54849625, 8.54861069, 7.24535179, - 8.5484066, 8.54865646, 7.24535894, 7.24524355, 10.6893644, 8.54849148, - 8.54852676, 7.24538612, 7.24532795, 9.67798424, 8.54865551, 10.6891737, - 7.2452693, 8.54872513, 8.54863167, 9.67783546, 8.548522, 8.5486517, - 8.54854679, 8.54851913, 8.54853058, 7.24531841, 8.54833317, 9.67802525, - 9.67797279, 8.54864502, 8.54887867, 8.54854679, 7.24550486, 8.54846764, - 9.67776108, 5.64895296, 10.6895771, 9.67808437, 7.24518299, 9.67793846, - 8.5486908, 9.67781734, 8.54827976, 9.67785645, 8.54854298, 7.2452569, - 8.54874706, 9.67804909, 3.04495716, 7.24530792, 3.04493141, 8.54869556, - 7.24528456, 3.36803365, 9.67791367, 8.54862022, 8.54866791, 7.24512434, - 9.67784405, 8.54853153, 7.10101318, 7.24540567, 8.54863548, 9.67797852, - 8.54854393, 8.42855549, 8.54858875, 9.6777401, 9.67780876, 8.54880238, - 7.24542999, 9.67799664, 9.67793846, 9.67816734, 9.67816734, 5.64909172, - 9.67804813, 9.67784214, 8.54853344, 8.54864025, 5.6489892, 8.54871655, - 7.24519587, 8.54860115, 8.54881001, 7.24530077, 7.24541664, 9.67792988, - 5.46404219, 8.54865074, 8.54857445, 8.54853439, 8.54873848, 8.54854107, - 8.54854488, 8.54865932, 7.24547338, 7.24516535, 8.54849148, 8.54853058, - 9.67776966, 8.5487051, 9.67784119, 8.54872322, 7.24544525, 9.67783737, - 8.54861641, 8.5487442, 8.54861069, 9.6781168, 7.24540949, 7.24533892, - 3.04493976, 8.54883671, 9.67795563, 9.67778397, 8.54867268, 9.6780014, - 8.54862785, 9.67795277, 8.54854679, 8.54880428, 8.54883575, 7.24538469, - 9.67802906, 8.54888344, 9.67798042, 8.54868507, 5.46399879, 10.6895142, - 8.54852676, 8.54869843, 8.54868317, 8.54869843, 3.04499841, 3.36809969, - 7.24543095, 9.67807961, 9.67805576, 9.67798233, 8.54868221, 8.54860497, - 8.54870701, 8.54848385, 8.5486412, 9.67798615, 8.54881287, 9.67815304, - 7.24539518, 5.64905691, 9.67804432, 3.3681438, 9.67823029, 8.54854393, - 8.54850578, 7.24552011, 9.67810535, 8.54876232, 9.67815113, 8.54876328, - 9.67800903, 8.54845333, 8.54876232, 9.67807293, 9.67807293, 7.24555397, - 9.67800236, 8.54891872, 8.54874134, 9.67811108, 8.5487175, 9.6781702, - 8.54857922, 9.67806149, 9.67820644, 9.67806911, 5.46411324, 8.54864311, - 3.36811256, 8.548769, 8.54872608, 7.24541473, 9.67819977, 8.54888153, - 7.24551058, 7.2453866, 8.54862022, 3.04508567, 3.36803555, 5.64911175, - 5.64906168, 7.24531889, 8.54879284, 7.24552441, 8.54881668, 9.67796135, - 9.67796993, 9.67819977, 8.54873562, 5.46410894, 8.42869854, 7.10112619, - 5.46413422, 5.46404457, 7.10100889, 7.10103083, 7.10087013, 8.42854595, - 5.46431112, 7.1013093, 8.42872524, 8.42856789, 8.42877579, 8.4286108, - 7.10108471, 7.10111713, 8.42870522, 7.10117722, 7.10108423, 7.10116291, - 5.46417046, 5.46410227, 8.42852306, 7.10091877, 7.10091877, 8.42876053, - 7.1011095, 8.42865086, 8.42845726, 7.10110044, 8.42866516, 5.46416044, - 7.10095024, 5.46399307, 5.46412134, 7.10102654, 8.42853832, 7.10112858, - 8.42865181, 8.42865181, 7.10094023, 7.10088062, 8.42878056, 7.10099649, - 7.1011157, 7.10101175, 5.46409607, 7.10099077, 7.10110712, 7.1011138, - 7.10096455, 8.4284668, 7.10101175, 5.46410799, 7.10104561, 8.42859745, - 5.46416044, 7.10087252, 5.46405506, 8.42874813, 7.10093355, 5.4641819, - 5.4641881, 5.46416664, 7.10129452, 7.10114193, 7.1008954, 8.42853546, - 5.4641099, 5.4641099, 7.10110474, 7.1011548, 8.42870617, 5.46414089, - 7.10110712, 8.42862034, 5.4642539, 7.10094786, 7.10091734, 7.10112095, - 7.10101986, 8.54879189, 8.54861546, 9.67801952, 8.54868698, 8.54884529, - 8.54895592, 8.54869461, 9.67792988, 9.67812061, 9.67813778, 8.54891109, - 11.6121511, 8.54892159, 7.24555683, 7.24559784, 8.54881954, 9.67842865, - 8.54896736, 7.24565172, 8.54852104, 8.54890347, 8.54875278, 9.67812347, - 10.6893873, 9.67832088, 8.5486536 -}; \ No newline at end of file + 12.4113951, 10.6396132, 17.169529, 22.8292465, 10.5444088, 8.39081669, + 10.641614, 11.4718409, 20.4783287, 15.3504314, 15.3513031, 9.63413239, + 25.3731174, 18.9253101, 16.0033855, 24.1376781, 28.4115391, 27.6845875, + 29.4730015, 33.6768837, 34.5762138, 35.450737, 36.8664055, 36.8693237, + 38.7607841, 41.314991, 41.3147278, 42.0521317, 45.778492, 46.6648064, + 47.7517242, 49.6425095, 52.0558586, 52.4533348, 53.8047791, 55.6794052, + 58.0183525, 60.6116638, 60.7854004, 63.1039772, 65.9648132, 66.4317398, + 68.2648849, 70.625473, 71.7813721, 73.9020615, 75.5481796, 77.4300003, + 78.3614883, 80.6872864, 82.9510956, 82.7034836, 86.2424927, 86.9628677, + 88.2577972, 90.9050903, 92.1436462, 94.1384506, 95.7668915, 97.7961044, + 99.5847855, 102.038879, 103.549995, 105.355217, 107.584, 109.401268, + 112.003922, 112.947815, 114.310066, 116.204742, 117.900772, 119.379234, + 121.453636, 123.484711, 124.659866, 126.166412, 127.724335, 129.257675, + 130.638611, 132.462296, 133.884125, 135.494843, 137.123718, 138.941895, + 140.886703, 143.3172, 143.985199, 146.436874, 147.651245, 149.980957, + 151.795288, 152.972382, 155.550827, 157.63028, 158.979904, 159.744888, + 162.516342, 163.158035, 164.464325, 165.421356, 167.523666, 169.456955, + 170.805832, 172.415237, 173.797318, 175.7146, 177.344955, 179.430923, + 181.203964, 182.604797, 184.202942, 183.232407, 183.828201, 185.775909, + 187.161652, 188.274979, 190.970871, 193.048584, 193.552383, 193.903107, + 195.170502, 196.671463, 197.220306, 197.083771, 197.749985, 198.354935, + 199.889923, 200.472031, 201.510834, 201.720306, 201.343964, 201.127594, + 201.470779, 200.658096, 200.537796, 200.431641, 199.389252, 199.213486, + 198.424728, 197.32753, 196.845001, 195.506439, 194.997894, 194.371918, + 193.090515, 193.750916, 192.536942, 192.263809, 191.898621, 191.349625, + 191.828659, 191.363953, 190.446213, 189.351685, 185.858109, 180.658096, + 180.828461, 183.347458, 184.327713, 186.507248, 187.238586, 187.274399, + 188.080185, 187.780655, 186.073151, 186.473251, 184.401474, 185.514282, + 184.814407, 182.69313, 182.772842, 182.772842, 181.386536, 180.426163, + 179.513199, 179.017822, 179.074036, 179.045639, 178.686157, 177.242355, + 177.189133, 176.377136, 176.152817, 175.485916, 174.898453, 174.489395, + 173.92334, 173.084961, 172.043137, 171.424362, 171.075867, 170.613815, + 169.925919, 169.062241, 168.137009, 167.604156, 167.320312, 166.214432, + 166.065033, 166.495438, 165.243683, 164.243286, 163.642975, 163.649567, + 162.826721, 163.247635, 162.502182, 160.487228, 160.60936, 160.999207, + 159.284119, 158.065231, 157.567429, 156.423874, 157.117569, 154.945602, + 156.644455, 156.413177, 155.87352, 155.647827, 154.769485, 153.900177, + 153.991287, 152.611343, 152.811279, 151.95636, 151.686584, 151.216324, + 150.376831, 150.142502, 149.453705, 148.636536, 148.431534, 147.900177, + 147.052231, 146.009003, 145.9505, 145.679123, 144.611954, 144.45787, + 143.784164, 143.393646, 143.088669, 142.234741, 141.963608, 141.214371, + 141.056747, 140.908966, 140.156128, 140.33905, 139.678894, 138.870056, + 138.330856, 137.469437, 135.973679, 135.708847, 135.49617, 134.939362, + 134.409241, 133.690781, 133.235306, 133.319, 132.43364, 132.332947, + 131.808243, 132.145615, 131.843628, 131.656967, 131.189651, 130.84111, + 130.822113, 129.194061, 129.159836, 128.43576, 127.813522, 127.921944, + 126.603584, 125.869659, 126.404015, 125.184059, 124.586815, 124.220787, + 124.580544, 124.249268, 124.175308, 123.479156, 123.30941, 122.77816, + 122.195129, 121.41465, 121.199074, 120.922768, 120.377502, 120.308357, + 119.179604, 119.280205, 119.204651, 118.655434, 117.689667, 117.722618, + 117.010521, 116.232468, 116.834404, 116.555901, 115.722458, 115.18029, + 115.082939, 114.965645, 115.294937, 114.62886, 113.85054, 113.45314, + 113.286819, 112.595406, 112.576797, 111.637688, 111.34462, 110.939369, + 110.725937, 111.067787, 111.307861, 110.192368, 109.503044, 109.619461, + 109.293739, 108.835175, 108.321236, 107.688713, 108.302734, 107.286201, + 107.089714, 106.759506, 106.905746, 106.503944, 105.741417, 106.211449, + 104.930573, 104.959724, 104.948799, 104.656197, 104.231476, 104.327293, + 104.25589, 103.044258, 103.076309, 103.095726, 103.103935, 103.013115, + 102.692505, 102.904404, 102.244812, 102.244812, 100.50853, 101.400246, + 99.6098175, 99.6516418, 99.0769882, 99.6706009, 98.7544479, 98.1889343, + 98.0998154, 97.5229416, 97.294548, 97.5436935, 96.7283325, 96.6032104, + 96.0145493, 95.925087, 95.3586502, 96.0646896, 95.3726273, 94.9047775, + 95.2724152, 94.6777878, 93.9669113, 93.8663254, 93.5023575, 92.8998337, + 92.1656876, 92.1644287, 92.0681839, 91.6967316, 90.8415756, 90.6017685, + 90.6057816, 90.6103745, 89.8713226, 90.7580795, 89.7719803, 90.0259781, + 89.1295776, 89.1429672, 89.0196991, 88.9238129, 88.4012299, 88.1494522, + 87.8901901, 87.9061508, 87.256691, 86.4787445, 86.7329102, 86.0919266, + 86.1069946, 85.7037277, 84.5901794, 85.6158752, 84.9309998, 82.9900208, + 82.935997, 82.1810913, 81.3405228, 81.0612259, 82.0060654, 82.1019135, + 81.2657013, 80.8491669, 80.4293671, 80.7200241, 80.585083, 80.5918961, + 79.8911362, 79.6070709, 79.7559662, 78.9017639, 78.7602005, 78.9147797, + 78.6235962, 78.0473709, 77.4674454, 77.0251999, 77.3284149, 76.8851242, + 76.1434631, 75.9982681, 76.1545715, 76.0106964, 75.5613708, 74.8006821, + 74.9684753, 74.8190384, 74.9822693, 75.1356201, 74.8413315, 75.1525192, + 73.9239197, 74.0867538, 73.1514816, 73.3152008, 72.3738251, 72.0599976, + 70.9409103, 70.9441223, 70.2937088, 70.140213, 69.9797897, 70.3167725, + 69.9882202, 68.8350296, 69.0057449, 68.8466949, 69.1870804, 68.5203094, + 68.0226212, 68.1939926, 68.3654556, 68.2077789, 67.8697815, 68.0446091, + 67.0231018, 66.8602066, 66.6925735, 65.9965057, 65.8304214, 65.8362885, + 65.8388062, 65.8448029, 65.6725159, 64.6115875, 64.6147308, 64.4388885, + 64.0865479, 63.7265358, 63.5475883, 63.1892509, 63.0096741, 63.7465324, + 62.4627075, 62.8401031, 62.6645813, 62.2936783, 62.1134071, 62.3037453, + 61.9358864, 61.1863976, 61.5671234, 61.3853912, 60.0524178, 60.0564346, + 60.0612564, 59.2879105, 59.2919846, 58.899395, 58.7092056, 58.9122047, + 58.321167, 57.9245033, 57.5246315, 57.729763, 57.5287971, 57.5352554, + 57.5404243, 57.7434425, 56.5259209, 56.3256836, 56.3274956, 55.5005493, + 54.8687096, 54.8751602, 55.092392, 55.0932121, 55.0972252, 55.1003227, + 54.8914223, 54.89645, 54.6863785, 55.1166954, 53.6196175, 53.4029846, + 51.8619728, 53.6315536, 52.7581978, 52.5371323, 52.3209801, 52.323204, + 52.3257484, 51.8822556, 50.7485352, 50.0599213, 50.5264931, 49.3616409, + 49.3656807, 49.3667259, 49.3687859, 49.3724022, 49.3762398, 49.3780861, + 48.6644897, 47.9443398, 47.9471207, 47.9490433, 47.7080727, 47.9534836, + 47.9575996, 47.958744, 47.7178268, 46.2292938, 46.2304153, 46.4842529, + 46.234417, 44.9570503, 46.4934654, 45.47789, 44.9655609, 45.2264938, + 44.7079926, 44.7103996, 44.7117538, 44.4523392, 43.1193924, 43.1209602, + 43.1238022, 42.854023, 42.8559265, 42.8587952, 42.85952, 42.8622475, + 41.475563, 41.7597542, 41.4811821, 40.9140892, 41.2012596, 41.202652, + 41.2042427, 41.2053795, 41.4914932, 40.349575, 40.6394196, 39.4713898, + 39.4743118, 39.1792641, 39.4780922, 39.4780922, 39.4808846, 39.1840248, + 39.185955, 39.4847832, 39.4861526, 38.8903236, 39.4900551, 38.8932724, + 39.4927559, 37.6717682, 37.0458107, 37.6758652, 37.3641129, 37.6788673, + 37.0512886, 37.3682556, 37.369503, 37.3701096, 37.3719177, 37.3730927, + 37.6870918, 37.6876945, 37.3767242, 35.4465446, 35.4460869, 35.4472466, + 35.449543, 35.4499092, 35.4511642, 35.4511642, 35.4538002, 35.4547119, + 35.1230011, 33.410881, 33.0581741, 34.4510002, 33.0604782, 31.9786453, + 31.2377892, 30.8595219, 30.8598766, 31.2406349, 31.2420692, 31.2429924, + 31.2429924, 30.8647938, 30.8668213, 30.8664036, 31.6195831, 31.2468319, + 30.869627, 31.2483406, 31.2496529, 30.4884949, 30.8720722, 31.2515697, + 31.6262512, 31.2538109, 31.6281986, 31.6281986, 29.3145161, 28.5018139, + 24.5097885, 24.9864769, 24.9864426, 24.987608, 24.5116386, 24.9877625, + 24.9884148, 25.4550762, 24.9890804, 24.9899445, 25.4569378, 24.5155354, + 24.031086, 24.5171452, 24.9924126, 24.9930611, 21.4468918, 22.5186787, + 21.9900837, 20.8932552, 21.99086, 21.991396, 21.9915123, 21.9920998, + 21.9926891, 21.9928856, 21.9945145, 21.9945145, 20.8961391, 21.9940243, + 21.4522953, 21.994606, 21.4526997, 21.9957924, 21.9958515, 21.996254, + 21.9964314, 21.9965725, 21.9964924, 21.9976387, 20.3273621, 21.4550629, + 19.7407608, 21.4558983, 21.9986305, 21.9989548, 21.9993973, 21.9991398, + 19.7421799, 21.4567852, 20.3296833, 20.3300762, 22.0001965, 18.5116444, + 22.0004635, 17.8649158, 18.5126705, 17.8653278, 21.4595833, 18.512888, + 20.9040699, 19.1385517, 17.8656101, 18.5133038, 21.4594917, 22.5309448, + 22.5318375, 22.5320187, 22.0026321, 22.0030384, 22.0026226, 22.5323658, + 22.003109, 21.4602718, 22.0032024, 17.1941872, 18.513855, 17.86726, + 17.8674145, 17.8668137, 18.5143547, 17.8674564, 17.8672352, 18.5146198, + 18.5145397, 17.8666153, 18.5147762, 18.5147762, 19.1403484, 18.5144711, + 18.5147095, 18.5133171, 18.5145321, 18.514267, 18.5146809, 18.514204, + 19.140276, 17.8670464, 19.1403503, 20.9043922, 19.745657, 19.1379051, + 12.4201765, 13.3371429, 9.14465141, 14.1921349, 3.60264635, 6.04226494, + 13.3364954, 17.1931267, 19.139143, 6.04244566, 18.5128231, 17.8655987, + 18.5122795, 17.8650379, 17.8648739, 18.5121994, 18.5116119, 17.8652287, + 19.7448235, 6.04315424, 13.3384638, 16.4980888, 9.14543152, 17.1910973, + 22.0007267, 25.4744186, 22.0027752, 23.0486393, 25.0114841, 22.5339508, + 18.5152931, 27.6943932, 30.5291271, 22.5358543, 20.9087124, 7.59722328, + 22.0097103, 12.4254007, 20.3355427, 35.5281181, 12.3314304, 10.3577375, + 5.84748268, 15.6973457, 11.3356037, 12.4266768, 3.2577486, 7.5973978, + 3.25779343, 3.25704408, 7.74968767, 14.1891499, 13.2464952, 9.01299667, + 6.03971958, 6.0393858, 13.3299341, 18.5029831, 17.8552227, 21.9898586, + 21.9898586, 18.502203, 17.8566074, 18.5038853, 18.5051804, 7.74701643, + 6.04055595, 7.74729586, 6.04036283, 3.25602245, 7.59292507, 14.1051168, + 6.04049158, 9.14046478, 12.3218737, 9.1409235, 14.1859617, 6.03975201, + 5.84166813, 9.01044941, 6.03848886, 10.2303801, 7.74395752, 3.59899211, + 3.25361276, 3.59889698, 3.59870267, 6.03561115, 10.3407993, 7.74237823, + 13.3237381, 13.3240919, 17.8494205, 18.4972916, 18.4962921, 19.1214581, + 17.8483868, 18.49576, 17.8477268, 18.4927731, 18.492672, 18.4916954, + 17.8443089, 12.406496, 12.4056816, 3.59816217, 6.03494692, 3.59752965, + 6.03406143, 5.83609486, 10.22295, 11.3140211, 11.3134918, 10.2218494, + 10.2215061, 10.2211504, 12.3060255, 11.3137445, 10.2221661, 11.3136711, + 11.3136711, 11.3124952, 15.664196, 14.8965168, 14.0859232, 10.2211866, + 11.3135366, 10.2217808, 9.00197983, 10.2213392, 6.03308296, 3.59690285, + 7.7373147, 3.59669352, 7.73725796, 6.03254986, 3.59651208, 12.3028679, + 10.2183495, 8.99698067, 10.216466, 11.3068304, 11.3062677, 5.8319993, + 7.73274469, 6.02868652, 3.59436488, 3.59407854, 3.59391236, 6.02781248, + 6.02781248, 7.73060513, 6.02756834, 7.73031759, 7.72989035, 6.02676058, + 3.59323215, 6.02651882, 3.59302473, 6.02615547, 7.72881031, 3.24800467, + 6.02536201, 3.59233379, 6.02535677, 7.72803164, 3.59216714, 8.98922825, + 3.24734902, 10.2073698, 11.2969427, 11.2966814, 11.2965622, 8.98867702, + 11.2960491, 10.2066708, 10.2069464, 11.2960768, 11.2956123, 10.2055216, + 11.2948475, 10.2069149, 11.2959919, 11.2962055, 10.2063618, 11.2964983, + 12.2879896, 17.0724678, 17.0715714, 16.3699131, 17.0712872, 15.6387024, + 16.3675041, 10.20292, 10.2030973, 11.2921638, 11.2914381, 11.2906418, + 10.2012043, 10.2003527, 8.98231316, 10.1990118, 5.82227564, 6.01907253, + 6.01874828, 6.01868629, 3.58817148, 12.2762976, 3.58810663, 3.58776307, + 6.01753235, 7.71756935, 6.01720285, 7.71708059, 6.01679659, 7.5632906, + 8.97652531, 5.81914234, 5.81880713, 7.71594667, 3.5868032, 3.58661366, + 5.81885719, 11.2806158, 12.271019, 10.1920471, 11.2795677, 10.1915274, + 10.1918221, 10.1917429, 10.1912527, 11.2786007, 11.2786112, 11.2781916, + 10.1905231, 10.1902819, 11.2775879, 11.2776461, 15.6158009, 11.2757177, + 14.0409632, 11.2755575, 11.274991, 14.8487339, 15.6146145, 11.2755175, + 17.0420246, 17.0419502, 17.0410709, 14.0389328, 13.1812143, 11.2738237, + 10.1854382, 7.7101841, 6.01100254, 6.01100254, 7.70991182, 11.3741245, + 6.01128531, 6.01112461, 6.01021528, 3.58349705, 7.70864153, 6.01033497, + 10.183567, 11.2703085, 16.3360367, 17.0351124, 21.2924366, 20.170599, + 20.170599, 17.7087135, 16.3377495, 10.1839733, 6.01016378, 12.354497, + 13.263361, 14.1137018, 13.2632685, 17.0976734, 13.2626057, 12.3517237, + 12.3511477, 7.70557833, 7.70467758, 7.70467758, 6.00756264, 6.007164, + 6.00702906, 6.00666142, 11.365983, 12.3460808, 13.2555418, 14.1044979, + 13.2529917, 13.2529707, 13.2517376, 3.57969761, 6.00419092, 6.00360107, + 6.00360107, 3.5793159, 10.1701603, 11.2555685, 10.169241, 10.1685524, + 10.1681852, 5.8049345, 6.00042915, 3.23430943, 3.57742977, 5.99990034, + 5.99987411, 5.99963045, 7.69487047, 7.69487047, 5.99846077, 3.57622719, + 5.9984169, 3.57625532, 7.69332075, 3.57635856, 5.9985199, 3.57627439, + 5.80163622, 3.57631397, 7.54045773, 3.2331686, 8.94923306, 11.2472944, + 8.94926929, 5.99862003, 5.99791861, 10.2751884, 9.07659531, 15.5735836, + 5.99738121, 3.57544804, 11.3474636, 5.79976702, 8.946311, 3.57490706, + 5.99591637, 7.6905179, 12.324584, 3.57464027, 3.57431889, 5.99452639, + 3.57412672, 5.99432373, 5.99418545, 5.99430752, 3.5738368, 7.68710852, + 3.57327032, 5.99312401, 5.99244642, 5.99194384, 5.99204922, 3.57221198, + 3.57221198, 5.9910593, 5.99095249, 7.68387556, 5.99070454, 3.57172537, + 3.57166409, 5.99046659, 3.57143831, 7.68247938, 3.57120061, 5.98979473, + 5.9890089, 7.68116999, 5.98880625, 5.98880625, 7.68037462, 5.98834944, + 7.68022776, 5.98784971, 7.52708721, 5.79197502, 10.1455574, 11.2285032, + 10.1458092, 5.79199791, 13.128891, 7.52783871, 13.1292086, 3.2278235, + 3.2278235, 3.22768569, 3.5701232, 3.57018566, 9.06131268, 3.56986642, + 5.98688555, 5.98638582, 7.52399063, 5.98484755, 5.98485136, 5.98437738, + 3.56741905, 5.98319244, 10.1362906, 11.2182264, 3.56646395, 8.92491913, + 5.98152447, 7.67152357, 7.67128038, 3.56601739, 11.2156086, 3.56605697, + 5.98091507, 11.2148094, 7.5176158, 3.22347832, 3.56539583, 5.97957325, + 3.2231431, 5.97923613, 5.97887087, 8.92055225, 5.97849798, 3.56453681, + 3.56443191, 3.56433344, 5.97805643, 7.66717768, 7.51434851, 7.66710997, + 5.97793961, 5.97740984, 7.66646338, 3.22194648, 11.2070274, 5.97679329, + 3.56318951, 5.97603703, 5.97590065, 7.6642375, 3.56273842, 5.97523069, + 5.97500849, 7.66360664, 7.66348839, 3.56244898, 5.97504091, 5.97509336, + 5.97509336, 5.97487879, 5.97498083, 3.56233811, 11.304224, 14.0306425, + 14.0295267, 13.1844702, 10.2325621, 5.97272062, 7.66025352, 5.97268009, + 5.97211075, 7.5076704, 10.1188536, 11.1989279, 3.56036615, 5.97102213, + 7.65805006, 7.65733051, 5.96992445, 5.9700017, 3.55930924, 7.65694571, + 7.65652943, 9.03396606, 10.1145563, 8.9068737, 10.2287149, 9.03457832, + 7.50459671, 12.2718077, 5.97014618, 10.2276001, 11.2961884, 11.1935053, + 3.21725106, 5.96846628, 11.1922293, 16.2208328, 3.55774903, 5.77160883, + 10.1094856, 10.1095581, 5.96686077, 7.50045633, 5.96654606, 3.21563625, + 3.5566566, 9.0260725, 5.96454144, 7.65039444, 5.96486378, 5.96432686, + 7.64937544, 5.96365499, 3.21465659, 5.96285439, 3.55512285, 7.64714098, + 3.55482578, 5.96200752, 3.5544672, 3.55451393, 3.55422473, 5.96149969, + 5.96121264, 5.96128702, 3.55409288, 5.9612956, 5.96124363, 5.9613657, + 5.96145439, 3.55421925, 5.96093798, 3.21309805, 5.96044922, 5.96043587, + 3.55358911, 10.098197, 13.916523, 18.1972065, 8.89144993, 5.95890617, + 7.64157057, 7.48945284, 5.76255131, 5.95762777, 3.21122432, 5.95686388, + 5.95686388, 7.63983297, 3.55138516, 7.63987255, 7.63961267, 3.55131745, + 3.21058846, 9.01285362, 3.55105591, 10.0911579, 14.706398, 10.0909786, + 10.0910883, 11.1683483, 10.0907621, 10.0907621, 8.88502979, 11.167366, + 12.1476259, 11.1651192, 7.4845438, 8.88363934, 5.75845623, 3.54942799, + 3.54939342, 5.95278311, 5.95216179, 5.95157242, 5.95135164, 7.63264704, + 5.95098734, 7.63193941, 5.95037413, 7.63199186, 5.75587749, 3.54757261, + 10.0811577, 3.54731202, 7.63029957, 5.94852304, 5.94873667, 5.94861269, + 5.94800091, 5.94818068, 5.94799852, 5.94799852, 5.94842768, 3.54655766, + 3.5470531, 3.54731011, 11.2570257, 15.5217209, 20.5828362, 18.2268982, + 18.2256298, 18.2252045, 13.1271458, 18.2200317, 17.5805988, 14.7572603, + 3.54469323, 5.94502163, 5.94509554, 3.54447913, 7.62516308, 7.62472439, + 7.62417555, 5.94443512, 5.94343376, 5.94335413, 5.9430685, 3.54321885, + 5.94245577, 5.94235849, 7.62124968, 5.94210148, 5.94195652, 5.94189501, + 5.9417305, 8.99121666, 7.62052822, 13.0257034, 5.74648094, 7.61990452, + 7.6197629, 5.94117498, 3.54200101, 5.94098902, 5.94080925, 7.61954832, + 13.1123629, 5.9404583, 5.94045448, 7.46690226, 7.61790228, 7.61802912, + 8.98751736, 3.54084778, 7.6173501, 5.93858528, 5.93837786, 7.61608219, + 5.93749857, 7.61466932, 5.93706417, 5.93706417, 5.93653202, 7.61411667, + 7.61357117, 7.61343384, 5.9360323, 5.93591213, 5.93569469, 7.61276579, + 10.0564842, 10.0561552, 10.0561571, 10.0554066, 11.1291246, 11.1294632, + 10.0560875, 11.1300077, 10.0553951, 10.0548286, 8.85348892, 10.0534792, + 7.45834875, 5.93319559, 5.93233061, 7.60854006, 5.93191099, 5.93193197, + 13.0926151, 13.9312391, 14.7227669, 13.9307919, 13.9292288, 7.60701561, + 17.5350285, 11.2236567, 10.1626549, 19.3261051, 14.6478043, 21.0142803, + 16.1234341, 11.2231293, 10.0492678, 3.5363307, 13.849905, 11.1222973, + 3.19719362, 14.7214508, 14.7197962, 19.3757839, 13.9270439, 3.5349977, + 13.9246883, 18.1633053, 18.775528, 17.5262432, 17.5264339, 17.5262012, + 18.1605053, 13.8413582, 5.92726326, 19.3104534, 15.3916883, 5.92809486, + 17.4635448, 7.45011139, 10.0427885, 18.715147, 13.0832834, 3.5340395, + 5.92668915, 10.153801, 18.1573048, 8.84122849, 19.8845654, 5.92488718, + 3.53252649, 3.53227448, 15.4549074, 7.59775972, 3.19308734, 5.9231801, + 8.83705902, 5.92234707, 10.035224, 13.9095736, 7.59579182, 3.53058672, + 5.92130327, 5.92097378, 8.95991611, 8.95991611, 7.59323835, 5.92002964, + 7.59272957, 5.91936255, 5.91917467, 5.9190588, 3.52901626, 5.91884756, + 5.91863251, 5.91841841, 5.91790533, 5.91784573, 3.52822089, 7.58936405, + 7.58936405, 5.91709089, 8.82846451, 5.72304678, 7.58827114, 5.91637754, + 7.58782434, 5.91643667, 7.58741617, 7.58728504, 5.91542864, 5.91536808, + 5.91477728, 5.91507101, 5.91467667, 5.91467667, 7.5867424, 8.82519531, + 5.91450262, 5.91396236, 5.91371632, 5.72051144, 5.91476154, 18.7332764, + 19.3259335, 17.4860172, 8.94877434, 13.8869486, 13.0504255, 11.1875944, + 7.58307171, 7.5822835, 7.58143282, 5.91134596, 11.1856537, 17.4782276, + 18.1110134, 3.18628049, 7.58162689, 15.3520947, 3.52491188, 3.18669963, + 5.91144943, 3.52519703, 14.6744404, 18.1133919, 13.0453939, 10.1236525, + 7.57933998, 3.18526816, 7.5782671, 7.577981, 3.52247834, 5.90714931, + 5.90715027, 7.57603884, 3.52158785, 5.90661478, 7.57527542, 7.57472801, + 7.57472801, 7.57452106, 5.90573359, 8.81113243, 5.90509462, 5.90493393, + 7.5733242, 7.57331848, 5.90438271, 3.52005005, 7.57231808, 5.90374947, + 8.80777359, 7.57106209, 7.57073402, 5.90271568, 5.90264273, 7.57040215, + 5.90211487, 5.70867062, 7.56980038, 5.90182209, 5.90195942, 5.70824623, + 5.90112543, 5.901227, 9.99766445, 8.80345154, 5.70712042, 7.56760883, + 5.90027857, 5.70721531, 9.996418, 11.0628948, 8.80273819, 8.80233288, + 8.80211735, 7.56650972, 5.89913464, 5.89922285, 7.56639433, 5.89928055, + 7.56584692, 14.6415634, 13.8529606, 13.8529606, 13.8518286, 13.8510284, + 13.8508673, 14.6368952, 7.56291294, 5.8966918, 5.89646769, 5.89606333, + 5.89575672, 7.56211329, 5.89588976, 5.89584208, 5.89574051, 5.89574051, + 8.79655838, 7.41026831, 3.51455522, 7.56061602, 7.56006813, 8.79475403, + 5.8941164, 3.17706466, 8.79417038, 7.55984545, 13.8435545, 14.6309185, + 18.0602055, 21.46311, 13.8410463, 14.6279182, 5.89305735, 7.55800104, + 21.4567432, 14.6249208, 18.6629696, 18.0538921, 7.55694532, 13.7587976, + 8.79062653, 11.1503181, 19.198225, 18.6029606, 21.4041195, 15.3740301, + 14.5507851, 5.69944525, 3.17618728, 16.0819206, 8.91504288, 12.1085167, + 3.51208878, 5.69738436, 7.55418444, 12.1065035, 7.55293274, 7.55257845, + 5.88781118, 5.69517422, 5.88728333, 9.9748106, 7.55189562, 7.55066681, + 5.88728809, 5.88698292, 5.88703537, 5.88623238, 7.548944, 7.54850149, + 7.54843664, 7.54820967, 5.69214964, 8.77977943, 8.77888489, 3.17152977, + 3.50804186, 3.50766969, 3.50745177, 7.5450263, 5.88237381, 7.54472256, + 3.50729895, 7.54506397, 5.88232136, 7.54403973, 5.88174534, 7.54376936, + 5.8811903, 7.54256535, 5.8807888, 5.8807888, 5.88153028, 17.3269634, + 12.0900116, 9.96597195, 7.54411936, 5.88186884, 3.50686049, 13.7364082, + 8.77676105, 5.68996096, 15.2737284, 15.2755785, 21.8864517, 18.6330624, + 20.295948, 8.775877, 12.8940296, 14.5240755, 15.3424206, 11.1286554, + 5.88024759, 20.8816242, 15.9809971, 14.5172968, 9.96080685, 14.515296, + 7.54014874, 22.3719063, 14.5166759, 11.02388, 9.96124458, 7.38803196, + 11.9899931, 7.53835678, 18.0053711, 7.53725672, 3.16737843, 8.89224243, + 8.89128685, 7.5351553, 7.53442574, 5.87405491, 5.87395048, 7.53308868, + 5.87336445, 5.87308741, 7.53240252, 3.5012641, 7.53184843, 7.53170776, + 9.94887161, 3.5008297, 7.53099155, 5.8717103, 7.38044024, 9.94781113, + 7.38039112, 9.94709587, 8.75895786, 11.0081139, 11.0077763, 11.0073862, + 8.7583437, 9.94490814, 8.75810146, 11.0063667, 9.94497681, 8.75695705, + 8.7569828, 9.94467449, 13.7046757, 9.94400597, 9.94365883, 11.0043039, + 9.94299889, 9.94214058, 9.94256878, 8.75529194, 3.16271257, 7.52530813, + 7.52471828, 8.87760067, 7.52464008, 8.87749767, 5.86629963, 5.86602926, + 5.86596203, 5.86562824, 7.52262306, 7.52262306, 5.86535025, 5.86464834, + 5.86472988, 7.52198124, 7.52195549, 5.86500931, 13.6937132, 5.86376715, + 7.5212779, 5.86410475, 14.5562744, 14.5558281, 20.286005, 12.9420042, + 12.9420042, 12.0523376, 17.9615993, 3.16006231, 13.7703552, 7.51991224, + 8.87221432, 5.8630209, 10.9947348, 14.5522537, 16.0044823, 17.8985424, + 15.2225933, 11.9602118, 5.67070436, 8.87197971, 3.16022062, 17.3327236, + 10.9925241, 7.51751947, 10.9919729, 10.0436144, 16.6122379, 19.0968742, + 16.679575, 5.86168528, 9.93158054, 17.2660236, 13.6850471, 3.1589725, + 15.9292192, 12.8482265, 15.9275255, 14.5443907, 12.0454245, 12.043993, + 16.6088505, 12.8444777, 13.6808891, 5.85871172, 10.9858913, 7.36373615, + 5.85734892, 7.5122447, 18.5502243, 13.7548561, 8.73679829, 7.50943947, + 7.36028481, 5.8540802, 5.85411739, 10.028636, 5.8535862, 5.85346127, + 5.85250616, 5.85248756, 9.916399, 8.73152637, 9.91540909, 11.9372301, + 9.91480827, 3.15411234, 9.91410351, 10.971962, 7.35516357, 7.50458527, + 12.8273458, 10.9705267, 3.48810863, 7.50343084, 7.50357103, 3.15338063, + 12.9113903, 10.0218544, 10.9687567, 9.91088104, 8.85136986, 7.50158691, + 5.84887314, 8.85038567, 5.84821701, 7.50086641, 13.7356548, 12.0208664, + 13.7344933, 13.7342796, 14.5146351, 13.7327528, 13.7327929, 5.84692144, + 3.48604202, 3.15172839, 5.84637213, 3.48564816, 5.65416288, 12.8166904, + 3.48526049, 5.8453927, 3.48515534, 3.15076637, 3.15066004, 7.49680471, + 7.49658298, 7.49641609, 7.4963727, 10.013236, 9.90289497, 3.15047836, + 5.6531353, 3.48437834, 15.1750832, 7.49591303, 13.6464128, 12.8123856, + 15.9518337, 3.14978647, 7.3448987, 3.14948559, 8.71751499, 13.6418591, + 10.9552145, 15.1683817, 15.1688318, 7.34346867, 9.89787674, 5.84149551, + 12.8922119, 7.34139299, 9.89566994, 5.8394556, 7.33998203, 9.89331341, + 15.1611853, 13.6333036, 8.71154499, 3.14724207, 9.89174652, 11.9093056, + 8.71002197, 3.14657927, 19.017168, 11.9070473, 9.8891592, 8.70848274, + 9.88809776, 9.8880415, 9.88718987, 9.88680744, 9.88742828, 8.70623684, + 8.70623684, 10.9411583, 9.88601685, 8.70497227, 8.70520401, 9.88480377, + 9.88524437, 10.9409876, 9.88557434, 8.70570374, 8.70604229, 9.88658524, + 13.6247263, 15.1495647, 14.4064274, 15.1492958, 15.1462002, 14.402627, + 15.8539314, 14.402194, 8.70189762, 9.88022804, 7.47924376, 5.83120632, + 8.82364464, 14.4729824, 8.69941711, 8.82439804, 7.47932005, 3.14320707, + 7.4791069, 16.5240345, 7.47758198, 3.47609878, 14.4699306, 14.3936262, + 5.63830233, 9.87573242, 8.69695187, 3.14167929, 10.9295301, 5.82779312, + 8.69486237, 8.81847572, 3.47427964, 8.81823635, 5.82679272, 3.14077401, + 5.82619143, 5.82637501, 3.47374916, 7.47240067, 7.47214031, 7.47163057, + 5.82537222, 7.47158813, 5.82511091, 7.47028446, 5.82436466, 13.6800699, + 8.81438065, 3.47277617, 5.82415915, 5.82390451, 5.63260746, 8.81227589, + 9.86588478, 9.86631489, 10.9185028, 10.9183207, 9.86495876, 14.3762379, + 8.68683243, 12.7633791, 7.3175931, 8.68562126, 7.31758928, 7.4660821, + 8.68542099, 13.5916128, 9.86256695, 9.86188889, 5.82047939, 8.68507957, + 7.46541977, 7.46502686, 5.81988525, 8.80524349, 9.85774803, 8.68084335, + 9.85773849, 8.68115616, 10.9101753, 9.85814953, 8.68036366, 10.9089804, + 9.85655594, 9.85621262, 8.67896843, 9.85491276, 8.67817783, 8.67759514, + 7.31015348, 8.67685699, 8.6767292, 8.6767292, 8.67611599, 7.3095603, + 8.67613602, 9.85182953, 9.85193729, 8.67585945, 9.85147858, 8.67547417, + 5.62384844, 8.67473984, 11.8603725, 14.35604, 10.9013405, 8.67401886, + 7.30781794, 3.4657104, 3.46579385, 8.67320251, 5.62253523, 5.62217522, + 9.84775925, 8.67211056, 3.46522188, 7.45505095, 13.650631, 13.6504803, + 11.9472103, 7.45400667, 8.79409027, 7.45340538, 13.6466932, 8.79329967, + 7.4521656, 7.45243359, 7.45174742, 7.45118809, 3.46361041, 5.80932331, + 8.79106331, 5.80902815, 7.45007992, 7.44989204, 8.78967667, 7.44889832, + 7.44889832, 7.44834805, 7.4484601, 8.78767967, 13.6386652, 14.4131374, + 7.29913425, 7.44768524, 13.6389961, 8.66486454, 15.0812426, 9.83869839, + 15.0812044, 8.6646452, 20.5739174, 18.3368721, 15.0789013, 3.46231365, + 10.8874598, 5.80624533, 10.8859959, 10.9845505, 5.80489826, 8.65997314, + 8.78267765, 9.83258247, 9.8324728, 12.7214499, 8.65780067, 12.7210712, + 7.29358625, 3.12734723, 9.82996082, 8.65587044, 8.65583801, 11.8338413, + 9.82882977, 3.12701583, 5.80110788, 7.29240561, 7.43994665, 5.80024099, + 7.43808556, 7.43798971, 5.79900932, 9.82524872, 9.82458591, 10.8727179, + 9.82462692, 8.65087032, 7.43639278, 5.79781055, 5.79779339, 5.7975812, + 7.43564558, 7.43540907, 8.77233982, 5.79677963, 7.4341135, 5.79638863, + 8.7716198, 5.79592085, 7.28544378, 5.79576778, 8.77002716, 3.45542502, + 5.79511881, 5.79520941, 7.43236446, 7.43200254, 5.79479361, 9.92728043, + 7.28416014, 8.64587307, 7.43206024, 9.81776905, 5.79380512, 3.45449662, + 3.4540453, 5.79264307, 7.4290123, 7.4288578, 7.42904425, 5.79170322, + 5.79113483, 13.5217352, 7.42803335, 3.45296407, 7.42888308, 13.6047716, + 8.76586437, 15.110465, 20.03512, 9.8121748, 5.79180527, 13.5237265, + 7.27980566, 3.45322418, 13.5222874, 12.6950302, 9.92028236, 17.6815872, + 8.76396084, 7.27882767, 22.0851669, 5.60044765, 11.9009361, 8.7619009, + 15.0342369, 11.8088474, 9.8081007, 7.42443228, 16.4051266, 16.4060574, + 10.8552933, 9.80659771, 7.27552366, 5.59900427, 10.8532782, 5.59800625, + 7.42302227, 15.796587, 13.5907383, 11.8937111, 9.91114998, 7.41996193, + 3.11819744, 5.78472853, 5.78457594, 7.41931486, 7.41880751, 5.59418917, + 7.41782093, 3.11745501, 8.62905502, 5.78347492, 5.78319597, 5.78286695, + 3.11710668, 5.78272486, 3.44764471, 8.75021839, 5.78220654, 8.7492218, + 8.75000477, 5.7819829, 5.59249353, 3.44743705, 13.5007935, 10.8415785, + 3.44717216, 5.78177118, 21.5569477, 18.309206, 7.41312885, 5.59105015, + 9.90161419, 14.344491, 11.879159, 9.90094376, 7.41229677, 5.77897692, + 3.44529057, 10.835639, 7.4111681, 11.7871685, 5.58878803, 10.833147, + 3.44471908, 5.77715158, 7.40965986, 10.8336191, 5.77683592, 3.11366892, + 11.8745394, 10.9304428, 11.873518, 5.77614021, 3.11326933, 7.40793514, + 17.69627, 8.61650372, 7.40613174, 7.40622807, 3.44250846, 7.40566587, + 7.40540695, 7.4053669, 5.77315617, 8.73617458, 5.77283239, 5.77267218, + 3.44156098, 7.40304852, 5.77173042, 7.40273476, 5.77173615, 7.40210485, + 5.77075672, 3.44077015, 8.60992527, 8.60986996, 8.60950375, 9.77627182, + 8.60941505, 9.77604675, 9.77639008, 5.76942825, 7.39938593, 5.76904154, + 7.39928579, 5.76846838, 5.76821804, 3.43903422, 7.39803648, 7.39770508, + 5.76790428, 8.72783279, 5.76705694, 8.72697926, 5.76699162, 8.60496044, + 5.57771158, 8.60423279, 3.10805154, 3.4378283, 7.39480686, 5.76571417, + 5.76509857, 7.39430475, 7.24683619, 7.39415264, 7.39404964, 3.43715072, + 5.76473475, 7.39358902, 17.0452709, 11.8486586, 14.3069363, 15.7345409, + 13.5376711, 12.7208967, 15.7323389, 12.7198877, 16.3974838, 10.9045601, + 3.10662079, 10.9062347, 5.76319361, 20.9866962, 18.1966095, 7.3908267, + 5.7620306, 11.8450851, 5.76132202, 8.59600353, 7.38931561, 13.5305614, + 3.43460512, 14.2986135, 8.71657467, 7.38711691, 7.38703823, 7.38661289, + 5.75870371, 5.75860214, 12.7098904, 13.5243816, 13.5236397, 13.5237389, + 7.38523149, 15.7200203, 18.2381783, 14.218421, 17.0264339, 10.8962994, + 5.75756788, 12.7081823, 5.75714159, 7.23649216, 11.8327971, 3.10269642, + 5.75574493, 5.56741476, 5.75545979, 9.75146198, 5.75511742, 7.38097477, + 5.75450039, 8.70804882, 9.85850811, 3.1017673, 7.38024855, 7.37978458, + 10.8878956, 8.70732117, 13.5131989, 8.70693398, 11.8262529, 7.37872934, + 7.37872934, 5.75190592, 7.37753677, 5.7514081, 8.70358944, 5.75125217, + 7.3763485, 5.75083733, 7.37556267, 8.70227242, 5.75040436, 8.57962799, + 5.74988985, 7.37502003, 3.09944963, 3.42831373, 5.74961567, 7.37464857, + 8.57805634, 5.56080246, 13.4250593, 8.69937897, 3.42752814, 8.57653999, + 7.3726306, 5.5597806, 3.42672896, 3.0978322, 5.74655008, 8.69602871, + 5.74643707, 5.74623203, 5.74569893, 5.55750895, 10.774189, 7.36925697, + 8.57281017, 10.7745285, 7.36829853, 7.22166538, 5.74507713, 5.74542284, + 8.69338703, 7.36738777, 7.36733103, 7.36733103, 7.36636686, 5.74319935, + 7.36565113, 7.36544943, 7.36552715, 5.74228716, 5.74254274, 12.6742077, + 8.68988228, 5.74237394, 5.74235773, 7.36591196, 3.42408347, 9.73090267, + 14.9130735, 12.5920601, 10.7685404, 9.72865582, 3.09493136, 7.364048, + 7.36252928, 7.36227798, 5.73950481, 7.36153841, 8.6858778, 5.73961067, + 7.36086559, 5.73898411, 7.35992146, 3.42130971, 7.21275187, 7.35890388, + 5.73750591, 7.35829687, 3.42033553, 7.35786152, 7.35763025, 5.73669243, + 8.55848408, 9.7185154, 9.71834183, 8.55777645, 11.6998901, 15.5888481, + 18.6850491, 18.1078682, 14.1605101, 14.1606989, 8.5559845, 9.71669674, + 9.71689415, 9.71627617, 11.6987944, 14.8916569, 14.1609287, 14.8913336, + 18.1069279, 15.588521, 18.1044998, 19.7838402, 18.6823082, 18.1044254, + 15.5849552, 15.5845051, 14.8863811, 15.583209, 15.5828428, 15.5816641, + 8.55233002, 8.55243015, 9.71134281, 9.71016407, 10.7459354, 8.54990101, + 3.4165833, 8.54955006, 8.67101288, 7.34855843, 8.67008781, 7.20189142, + 9.70734882, 5.54145098, 7.20091009, 5.54057217, 5.72843313, 3.08786201, + 10.7406721, 8.54628468, 8.6685791, 3.4153161, 5.72809029, 10.7401762, + 8.54476261, 7.34533024, 7.34466743, 5.72627544, 9.7016716, 14.1385632, + 14.8678169, 18.6559753, 19.2146511, 14.8707542, 9.70253181, 12.6384172, + 18.7110023, 24.1363297, 29.9945183, 28.5494843, 24.5701256, 22.7867966, + 20.3358383, 16.9297905, 8.66628265, 3.41471505, 15.5681391, 3.08670807, + 9.70020962, 21.7850838, 8.66056442, 7.34023619, 5.53433371, 8.53704834, + 8.53615284, 5.72105408, 9.69300747, 7.33737898, 7.33769798, 7.3374176, + 5.72054672, 7.19057465, 5.72041559, 8.65637779, 7.33691359, 8.65639496, + 5.72024679, 7.33557701, 7.33642292, 8.65599632, 7.33532858, 7.33423567, + 5.71784258, 7.33382082, 3.40946746, 7.33426523, 8.53205967, 3.08222651, + 8.53118134, 5.71757603, 7.33500862, 9.68956375, 5.71717072, 8.52994537, + 5.71711111, 5.52946949, 5.71709204, 8.52985287, 3.40861797, 7.33205462, + 3.08130717, 5.52933645, 7.33204174, 3.08144379, 7.33185005, 5.71637297, + 8.65053272, 8.5290184, 5.71633625, 5.71606779, 5.52886724, 3.40799928, + 8.52852249, 5.71598864, 5.71596527, 5.71591711, 5.71599197, 5.71591282, + 5.71579123, 7.33102179, 5.71558762, 8.64934444, 9.68376827, 7.3306036, + 5.71549988, 5.52823782, 5.52809668, 5.71524429, 7.33029747, 5.52787828, + 7.33027077, 5.71500874, 8.6485815, 7.32993603, 3.08043671, 3.0804255, + 3.40727091, 7.32977962, 5.52748489, 7.32977104, 3.40712094, 7.32964754, + 7.32955313, 5.71469593, 5.71446753, 7.32917643, 7.32915306, 7.32907629, + 7.32907009, 7.32907867, 7.32907867, 7.32907867, 9.68145275, 9.68138981, + 8.52562809, 8.52546883, 5.5268321, 5.71388054, 8.52541637, 10.7142067, + 8.52504253, 7.32844257, 3.40654755, 7.32812405, 8.64602661, 7.32803392, + 8.52463436, 8.52429867, 3.07958961, 5.71311378, 7.32758713, 3.40624785, + 7.18165255, 5.525877, 7.32721233, 8.52375984, 5.5257287, 7.18110085, + 8.64455318, 7.32691574, 9.67916393, 8.64510822, 8.52336407, 3.07922649, + 3.40596676, 8.52309704, 5.52523184, 3.40578103, 7.1805253, 3.07907796, + 8.52287579, 3.40578437, 7.32650995, 8.52276802, 5.5250659, 7.18003178, + 7.32606411, 9.67736626, 7.32594252, 7.32561922, 5.52457809, 5.71172333, + 7.32587147, 7.32558393, 5.71154308, 3.07864118, 3.40527081, 7.32534647, + 3.40520215, 3.07847238, 8.52123356, 5.7112093, 7.32506227, 3.40512896, + 3.40513992, 7.17894268, 8.52100563, 10.7090998, 7.17876673, 5.71078348, + 7.32468939, 7.32455254, 5.52389622, 9.67565823, 8.52025509, 3.40475035, + 5.71057653, 7.17803335, 5.52343035, 9.67469978, 8.51950264, 5.52306175, + 5.71000004, 7.32362556, 3.40439606, 5.71015453, 8.51970959, 9.67461109, + 8.51972294, 5.71001959, 8.51946449, 8.6409235, 5.52275801, 3.07758164, + 3.07757521, 5.70954037, 9.67390442, 8.51858997, 5.70954752, 7.17677784, + 5.70932627, 8.63984489, 8.51840019, 7.1767087, 8.5179615, 3.40394759, + 3.07742095, 5.52203035, 5.7087965, 10.7046194, 7.17612791, 7.17605352, + 3.40371656, 7.17600727, 3.07709122, 3.40358305, 3.40352035, 9.67196369, + 3.07705402, 8.51684856, 8.51724625, 9.67196083, 9.67133617, 7.17545891, + 9.6715517, 7.17527914, 7.17525196, 5.52117157, 7.17518711, 8.51605606, + 8.51620865, 7.3206954, 7.17480898, 9.67052841, 9.67057133, 7.17472363, + 8.51604271, 9.67036438, 5.70748329, 7.32044363, 7.32005501, 5.70732069, + 7.32006073, 3.40279031, 5.70718479, 3.076262, 3.4026823, 7.31974554, + 5.70699739, 5.70705795, 5.70703506, 7.31975698, 7.31929588, 7.31947756, + 5.51968241, 5.70662832, 5.70674706, 7.17318487, 3.07591915, 5.70630646, + 7.31899405, 7.31910419, 9.66847801, 5.5192728, 5.70630121, 8.63535118, + 5.70634031, 7.31846333, 3.07567096, 5.70600796, 7.31816769, 8.6349926, + 8.63483906, 8.63470936, 7.31814766, 3.40185189, 5.51878786, 5.70571232, + 8.51295757, 9.66702271, 9.6670332, 9.6670332, 7.31749058, 5.51852226, + 8.51237774, 5.70518208, 5.5183301, 5.70519352, 5.51832533, 8.6332531, + 7.31706333, 7.31706762, 10.6971483, 7.17108583, 5.51784039, 7.31666231, + 5.70462227, 7.31663609, 5.70451498, 8.63243198, 5.70440722, 8.6324091, + 7.31635857, 7.31641865, 7.31618404, 5.70417404, 8.63213634, 5.70418453, + 7.31605053, 5.7040391, 5.70409012, 5.70409012, 5.70384455, 5.70381212, + 7.31574726, 7.31546926, 8.63131046, 7.31515789, 7.31518745, 5.7035346, + 7.31520176, 7.31520605, 5.703475, 5.70328999, 8.63070011, 7.31489944, + 8.50935555, 9.66327572, 5.70302248, 5.51628494, 8.6303587, 8.63008785, + 5.70297337, 5.70287991, 3.40010905, 9.66212463, 8.50860977, 7.16836596, + 3.40002346, 5.70242023, 3.3999629, 3.07381845, 5.51570892, 5.70239353, + 7.16796017, 5.51555109, 7.31377888, 9.66144466, 3.3997581, 3.39975095, + 5.70206738, 5.51543951, 9.66102123, 8.50770569, 10.6921635, 8.50756454, + 8.50756454, 9.66057873, 5.70181751, 8.6281929, 5.70176363, 8.50720215, + 9.66036797, 3.39944601, 9.65995979, 8.50679016, 7.3127265, 7.16693878, + 7.31255579, 7.31233501, 5.70138693, 7.31251287, 8.6274128, 7.31218863, + 8.62734795, 8.62716675, 3.39904451, 7.31181049, 7.31180286, 7.31180239, + 7.16593838, 5.7007699, 7.31158733, 5.70069933, 5.7006197, 5.70048332, + 8.50522995, 7.16572523, 8.50513268, 5.70038033, 5.70032215, 7.3112731, + 5.51367331, 3.39864826, 7.31117296, 3.39856672, 3.07254386, 3.39864731, + 8.50465298, 8.50448132, 7.16486073, 7.16486073, 7.31053543, 7.31048918, + 9.65709972, 7.31036806, 3.39822006, 3.39830613, 3.39821124, 3.07220984, + 9.65679741, 5.51281738, 5.69955158, 5.69948959, 9.65639114, 8.50368118, + 7.31002235, 7.16426897, 7.30977631, 3.07203293, 7.30964804, 7.16408777, + 7.30961466, 7.30953741, 3.39781094, 7.30960417, 7.30995417, 9.65701008, + 9.65630341, 7.16406584, 3.07193494, 8.50309563, 9.65539646, 8.50268841, + 3.397614, 8.50216103, 3.07162356, 7.16316128, 7.16308022, 8.62321949, + 7.30839729, 7.30847454, 7.30862045, 5.69812489, 7.30848789, 8.62301159, + 7.30840778, 7.30819464, 8.6227808, 7.30822515, 7.30798149, 5.69785929, + 7.30798006, 7.30792618, 5.69767094, 5.69770193, 5.69770908, 7.30768824, + 3.39708877, 5.69759083, 7.30755043, 5.69751787, 3.39701033, 7.30748796, + 5.69744968, 7.30727673, 7.30733776, 3.39682651, 5.69733477, 5.51059341, + 7.16152716, 3.39675665, 8.50031376, 8.50032902, 3.07087898, 7.16122484, + 7.16122484, 8.49990082, 9.65222645, 5.51036692, 5.51029444, 5.69687891, + 8.49975586, 3.39652658, 5.51016045, 8.49973202, 9.65185261, 3.07058573, + 9.65155411, 3.07050586, 8.62021255, 7.30603552, 9.75903702, 7.30602264, + 8.49890137, 3.07032108, 5.50946045, 3.0703721, 7.30583477, 7.30546331, + 5.69586182, 7.30540466, 5.69594431, 7.30530691, 8.61933327, 8.61940956, + 7.30535555, 7.30503321, 7.30502748, 7.30484152, 5.6955471, 7.30485249, + 7.15925598, 5.50877047, 7.30447531, 5.69532871, 3.06985712, 7.30446148, + 3.06976914, 7.30452633, 5.50860548, 7.15887356, 8.61838341, 5.50849247, + 7.3042264, 8.49676228, 7.30423403, 8.49683285, 7.1584506, 7.30379486, + 5.69461346, 7.30385017, 5.6946125, 3.39526534, 7.30366373, 7.15801001, + 5.69443321, 7.15796804, 10.6775417, 9.64779377, 7.30318737, 3.39503241, + 3.394979, 7.30307388, 5.50757885, 3.06921887, 5.6940217, 9.64715672, + 5.5073576, 3.069067, 5.50735235, 7.15724516, 5.50726223, 3.39470124, + 7.30247498, 8.6161356, 8.61596775, 5.69364119, 7.30220318, 7.30215263, + 8.61557388, 7.30215931, 7.30215693, 7.30219841, 8.61549187, 7.3020587, + 5.69323587, 7.15638161, 5.50663424, 7.30178165, 7.15616465, 9.64559937, + 7.15617275, 7.30167341, 8.49353218, 9.64520741, 8.49363041, 7.15576696, + 7.30117083, 8.61443138, 7.30110645, 9.64477921, 7.30099154, 5.69239426, + 7.15529537, 8.49279308, 7.15523863, 9.64424324, 3.06821012, 7.15509367, + 8.49269676, 8.49250412, 5.50564957, 7.30046129, 3.39364696, 7.15474939, + 9.64367104, 3.06810641, 3.39359593, 8.49229908, 8.49224377, 5.50551939, + 3.0680449, 5.69187927, 3.39356494, 7.29989719, 7.15443802, 7.29992199, + 8.49157715, 5.50493097, 5.50493956, 5.69133043, 3.06765103, 5.50474787, + 8.61234474, 7.29929876, 7.15377283, 10.6713409, 8.49097538, 3.39300895, + 5.6907382, 3.06748652, 5.50434637, 5.69077873, 8.49069977, 3.06737947, + 7.29867887, 5.6907239, 7.29882622, 5.69087553, 7.29930353, 7.29955053, + 7.29959393, 5.69124317, 8.61256218, 8.61248875, 7.2994051, 7.29961538, + 3.39311075, 8.49127769, 5.69112015, 3.39315033, 5.6908989, 5.69088316, + 8.4907074, 7.2983427, 5.50406122, 8.48969841, 5.69001102, 7.29756498, + 9.74756527, 5.50329542, 7.2972908, 9.63954735, 3.39206457, 9.63961124, + 8.48875809, 3.0667243, 5.68925571, 7.29689837, 5.68906593, 3.39190674, + 7.29663801, 5.68902969, 7.29654026, 9.74601555, 7.29637432, 7.29644966, + 5.68871307, 3.06641293, 8.60858154, 8.60858154, 5.68865776, 7.29608059, + 7.29613972, 7.29603624, 7.1505909, 3.06619406, 7.15053701, 7.15042162, + 9.74508572, 7.29583311, 3.06610012, 7.15001535, 7.15003824, 5.68807459, + 5.68807459, 7.29522753, 7.29528952, 5.68791914, 8.60737705, 5.68771458, + 3.39116788, 3.06584191, 5.68765688, 3.39102769, 7.29487276, 7.29466963, + 7.29454947, 8.48577595, 7.29454947, 8.60665989, 3.39088583, 8.48541641, + 7.2944417, 7.29427719, 3.3907299, 5.50086117, 3.06556249, 7.2940402, + 7.29382801, 7.29395199, 5.68695021, 7.29373932, 7.29382277, 5.68681002, + 7.29381704, 8.48447418, 8.48460388, 9.63481712, 8.60543442, 7.29327774, + 7.2933588, 7.29342127, 7.29314089, 9.74167919, 8.60512829, 8.60495377, + 5.6861105, 7.29293919, 7.29285383, 10.6621866, 7.29273987, 5.6858573, + 8.48346901, 3.39004064, 8.4832983, 8.48323631, 8.48339748, 7.14698744, + 3.06470037, 5.49937773, 9.63312244, 8.6038456, 5.68542719, 7.29191971, + 7.29191971, 7.29199934, 7.29192877, 7.2917366, 7.29163313, 5.68533516, + 7.29171515, 7.29139137, 7.29155874, 5.68494654, 7.29152393, 7.29132175, + 7.29125643, 7.2911973, 9.73917007, 7.29119444, 7.29114914, 7.29100657, + 7.29097176, 7.29089594, 5.68463945, 7.14560175, 9.63114834, 3.06403875, + 7.14528179, 3.38911533, 9.63068485, 8.48089695, 5.68425226, 7.14506865, + 3.06389594, 3.38896346, 5.6839056, 8.48051262, 7.28993893, 7.14467764, + 3.06370878, 8.48044777, 9.62982559, 5.4974966, 3.06358171, 7.14427233, + 5.68353081, 3.06349874, 3.06354856, 8.60077763, 3.06346822, 5.68327951, + 7.28935671, 7.2889657, 7.28914356, 9.6288805, 5.68310118, 8.60004807, + 7.28889227, 7.28888083, 5.49684715, 7.2888236, 5.68279886, 7.288764, + 7.288764, 7.28836346, 8.5992918, 7.28818893, 3.06300473, 7.2883172, + 7.28810644, 5.68244076, 7.14273357, 7.14278316, 8.59887981, 7.1426034, + 5.68219185, 5.68204498, 8.59857178, 8.59857178, 7.14231586, 5.49584293, + 7.28761148, 8.59833431, 8.59820366, 8.59802341, 7.287323, 7.28722334, + 5.68155766, 3.06249881, 3.06248307, 3.06247115, 7.2869854, 7.14160061, + 3.38734245, 9.6256609, 5.49511671, 7.28685045, 8.59715271, 7.28645372, + 7.2864151, 7.28642416, 7.28635311, 7.28627062, 5.68088293, 5.68082094, + 7.28616285, 7.28599882, 8.59646702, 5.68064594, 8.59639835, 5.68060207, + 7.28564262, 8.59624386, 7.28575993, 7.28575134, 7.28571844, 8.59575939, + 5.68035793, 8.59611225, 5.68023634, 5.68023205, 5.68004704, 7.28523064, + 7.28523064, 5.68001842, 7.28505278, 8.59520054, 8.59512138, 8.59511948, + 7.28481102, 8.59529781, 7.28446913, 5.67973566, 5.679667, 7.2845211, + 5.6795516, 7.28445101, 8.5946703, 7.28438759, 7.28429794, 7.28420591, + 7.28411484, 7.28388739, 7.28407192, 5.67902899, 8.59405804, 7.28396082, + 7.28371763, 7.28365469, 7.2834816, 7.2836628, 8.59364796, 7.28342772, + 7.28342772, 7.28329754, 7.28345871, 8.59350967, 8.59315014, 7.28313684, + 7.28307009, 5.67845678, 8.59296608, 7.28301525, 7.2828064, 7.2828393, + 8.59268475, 7.28266621, 7.28255367, 7.28255367, 7.28249264, 8.59241104, + 8.59213543, 5.67778254, 7.28205585, 5.67780209, 7.28226757, 5.67775249, + 8.59200668, 7.28182983, 7.28211021, 9.72676086, 5.67750311, 8.59162521, + 7.28180647, 7.2817049, 7.28175116, 8.59154224, 8.59133816, 8.59134007, + 7.28136969, 7.28141689, 5.6770339, 7.28114414, 7.28130388, 7.28135824, + 8.59064388, 5.67697334, 7.2809701, 8.59063911, 8.59063339, 8.59053802, + 7.28089809, 5.67668581, 7.28072929, 8.59018421, 7.28073072, 5.67657709, + 7.28063488, 7.28065014, 8.59002399, 7.280406, 7.28045797, 8.58985615, + 7.28030729, 8.58961201, 8.58974934, 8.58972836, 7.27991867, 7.27991295, + 5.67601109, 7.27988863, 5.67583466, 7.2799058, 7.27987814, 7.27959394, + 7.27970791, 5.67566109, 8.5891304, 7.27969217, 7.27952147, 7.27936649, + 7.27922583, 7.27919674, 7.27942848, 7.27934074, 5.67551661, 5.67540455, + 7.27901697, 7.27914906, 7.27887917, 7.27889633, 5.67508745, 7.27886629, + 7.27886629, 7.27862692, 8.58780956, 7.27848768, 8.58749866, 5.67458725, + 5.67473555, 7.27821875, 8.58719921, 7.27792549, 7.27775717, 8.58693981, + 5.67426872, 7.27794695, 8.58686256, 7.27777052, 8.58660412, 7.27760696, + 5.67413521, 7.27753687, 7.27736712, 8.58640003, 7.27751112, 7.27741766, + 7.27722788, 7.27712297, 5.67393017, 8.58610249, 7.2771821, 8.58593178, + 7.27698135, 5.67377758, 7.27689743, 7.27684689, 7.27676678, 7.27680969, + 8.58567142, 7.27694035, 8.58540249, 7.27674294, 7.27645636, 7.27640009, + 7.27643728, 5.67313528, 8.5851078, 7.27628326, 7.27626657, 7.276021, + 8.58483601, 7.2760067, 7.27603245, 5.67285919, 5.67286682, 7.27565193, + 5.67275143, 7.27571201, 7.27539825, 7.27550745, 7.27558184, 7.27527952, + 7.27527952, 7.2752552, 7.27512074, 7.27513313, 8.58379459, 7.27526569, + 5.67221642, 7.2751112, 7.2747674, 7.27479362, 7.27464962, 7.27463245, + 7.27450418, 7.27443409, 7.27422714, 7.27422714, 5.67163658, 7.27430725, + 7.27431154, 7.27412367, 5.67159796, 5.67140436, 5.67157555, 8.58245087, + 8.58234882, 5.671381, 5.67125845, 8.58214378, 8.58221149, 8.58221149, + 8.58221149, 7.27353621, 7.27372932, 8.5820713, 7.27352142, 5.6711092, + 7.27355719, 5.67094803, 7.27350092, 7.27336311, 8.58151245, 7.27336454, + 5.67081642, 8.58142471, 8.58169079, 8.58169079, 7.27328873, 5.6707325, + 5.67067337, 8.58174229, 5.67085218, 7.27309227, 5.6706109, 7.27269793, + 7.27271032, 8.5809164, 7.27247858, 7.27243996, 7.27226782, 7.27210712, + 7.27210712, 5.66986513, 7.27210903, 8.58000278, 7.271873, 5.66982937, + 7.27184725, 8.57978725, 7.27154636, 8.57953548, 8.5796833, 8.57945347, + 7.27140713, 7.27150345, 7.27127695, 8.5792017, 5.6693716, 8.57924366, + 5.66915131, 7.27121258, 7.27109051, 8.57895756, 7.27115345, 5.66901112, + 7.27089357, 5.66904831, 7.27086782, 7.27083111, 8.57875443, 7.27106428, + 8.57894993, 5.66977692, 7.27214909, 5.66988945, 8.57999039, 7.27181768, + 7.27128601, 8.57917976, 7.27082872, 7.27078581, 8.57881451, 8.57858276, + 7.27065468, 8.57843113, 7.27071047, 7.27071047, 5.66879702, 7.27090549, + 7.27118158, 8.57941723, 7.27182341, 7.27181053, 7.27163839, 5.66948271, + 8.57809925, 8.5777216, 9.71102142, 5.66807318, 7.26970196, 5.66787386, + 8.57713985, 5.66773224, 7.26935768, 7.269279, 7.26934481, 7.26915216, + 8.57688904, 8.57677841, 8.57642937, 7.26909494, 7.26906061, 7.26902151, + 7.26898718, 8.57672405, 8.5763073, 7.26882458, 5.66729212, 5.66724777, + 7.26880407, 7.2685833, 5.66726017, 5.66730022, 5.66737795, 7.26958942, + 9.71054077, 7.26969814, 7.26925039, 7.2689085, 7.26868725, 7.26872158, + 8.57618618, 7.26831388, 7.26827765, 7.26794481, 7.26799202, 5.66669798, + 7.26793242, 7.26780081, 7.26803017, 8.57499313, 8.57492256, 7.26753139, + 7.26748371, 5.66635418, 5.66633749, 8.5748148, 5.66604805, 7.2672286, + 7.26729107, 8.5743351, 7.26704168, 7.26694441, 7.26696014, 7.26676178, + 7.26698589, 7.26688194, 7.26682186, 5.66572285, 7.26662922, 9.70642567, + 8.57377815, 5.66557884, 7.26650429, 7.26663828, 8.57352352, 8.57339859, + 7.26642323, 8.57377243, 5.66539001, 7.26633692, 7.26619768, 8.57332134, + 5.66534138, 7.26617241, 7.26618004, 7.26618004, 5.66522455, 7.2659688, + 7.2658, 7.26574278, 8.57271767, 8.57287216, 9.70515251, 8.57245636, + 8.57258224, 7.2656002, 8.57234669, 5.66476393, 5.66470146, 7.2655592, + 7.2655592, 7.26534557, 7.26533127, 7.26530886, 7.2653203, 7.26540327, + 7.26537752, 8.57209492, 8.57210922, 8.5718956, 8.57183266, 8.57183075, + 7.26497793, 7.26499271, 8.57200241, 7.26517582, 7.26489639, 5.66424179, + 7.2648325, 7.26492786, 8.57171154, 7.26472521, 8.57144547, 8.57144737, + 7.26471519, 7.26462793, 7.26469374, 7.26451159, 9.703578, 5.6639204, + 5.6639204, 7.26432228, 8.57083797, 7.26400566, 8.57071114, 9.70302963, + 7.26415396, 7.26401424, 7.2640028, 8.57073021, 7.26399469, 7.26404142, + 5.66350365, 7.26395273, 7.26390314, 7.26390314, 7.26386452, 7.26404285, + 7.26393414, 7.26357412, 8.57027245, 8.57013988, 8.5700655, 8.56978798, + 7.26318026, 7.2632432, 7.26329851, 7.26309013, 8.56989765, 7.26324272, + 5.66297197, 5.66299725, 7.26351786, 8.57035255, 8.57072067, 8.57069397, + 7.26424217, 7.26418638, 7.26388836, 7.2640667, 7.2640934, 7.2638855, + 8.57019329, 5.6632967, 7.26353216, 7.26353216, 7.26312351, 5.66272831, + 9.70134068, 7.2625246, 8.5691061, 7.26247787, 8.56894398, 7.26263428, + 7.26249743, 7.26251698, 7.26243591, 7.26244164, 7.26230097, 7.26245451, + 8.56879234, 7.26233006, 5.66236639, 7.26285219, 7.26315546, 7.26316977, + 7.26304722, 8.56974411, 8.56950855, 7.26286602, 8.56934738, 7.26285267, + 7.26222086, 9.70018578, 7.2619586, 8.56820774, 7.26159763, 7.26171827, + 8.56776714, 8.56768799, 8.56782532, 8.56767941, 7.26149035, 5.66135073, + 7.2610445, 5.66129827, 7.26097822, 7.26100349, 8.5669651, 7.26106882, + 7.26106882, 5.66111708, 7.26081324, 8.56680489, 8.566782, 7.26056385, + 5.66096687, 7.2607851, 7.26062346, 8.56641197, 7.26057196, 5.66081905, + 7.26049137, 5.66070509, 8.5663147, 7.260355, 5.66067266, 7.26030159, + 5.66066694, 8.56612587, 7.26016903, 5.66065979, 7.26020193, 7.26015854, + 5.66048193, 7.2601285, 5.66040564, 7.26003408, 7.25977516, 7.25979948, + 7.25979948, 7.25990009, 8.56573582, 8.56567383, 8.56564713, 8.56548023, + 8.56536484, 7.25976038, 7.25939655, 8.56517696, 7.25946617, 7.2592926, + 7.25933552, 7.25912762, 7.259233, 7.259233, 7.25921249, 8.56482887, + 8.56491661, 8.5647707, 7.25928497, 7.25927353, 5.65945339, 7.25881672, + 8.56448746, 7.25888014, 7.25897217, 7.25861311, 7.25880337, 7.25864649, + 8.56429958, 8.56421947, 7.25852537, 5.65925407, 7.25833559, 8.56394577, + 7.25835085, 5.65912724, 7.25824785, 8.56373119, 8.56378078, 7.25805569, + 5.65889263, 8.56371403, 8.56366253, 8.56366253, 8.56360912, 8.56336212, + 7.25795984, 7.25789499, 8.56327152, 7.25789213, 7.25780392, 7.25759506, + 7.2575593, 8.56294441, 7.25779676, 7.25740194, 7.25758219, 8.56288433, + 9.69409275, 7.25730848, 7.257442, 7.25724125, 5.6581893, 7.25718641, + 7.25729465, 7.25708008, 8.56261635, 8.56247234, 8.56270409, 7.25689936, + 7.25696611, 7.25700331, 7.25690079, 7.25690079, 8.56233883, 7.2568922, + 7.25666666, 7.25684166, 8.56217766, 7.25662184, 5.65772009, 8.56178665, + 7.25678968, 8.5621624, 7.25704145, 8.56277084, 8.56281567, 7.25745487, + 7.25745487, 7.25763321, 7.25728941, 5.65848398, 7.2572279, 7.25712967, + 7.25696802, 7.25671434, 5.65746641, 7.2564435, 8.56168652, 7.25604296, + 7.25612974, 7.25607109, 8.56153679, 7.25630569, 8.56204414, 7.25693989, + 8.56223011, 8.56249237, 8.56227398, 8.56167507, 7.25631618, 5.65710688, + 7.25574017, 8.56087303, 8.56084633, 7.25568295, 8.56063843, 8.56056213, + 7.25547934, 5.65683031, 7.25538158, 7.25533724, 8.56033134, 5.65690374, + 5.65655518, 7.25524092, 5.65650988, 8.56029987, 7.25498247, 8.55978966, + 7.25478649, 7.25467443, 9.69068527, 8.55987453, 7.25487518, 7.25471067, + 7.2545743, 5.6562438, 8.55962849, 5.65616846, 7.25484991, 7.25441074, + 7.25461817, 7.25455427, 8.55932999, 8.55924225, 9.69005775, 7.25433588, + 7.25433588, 8.55896854, 7.25437307, 7.25407887, 7.25405073, 7.25412846, + 7.25422907, 7.25415277, 7.25408459, 8.5588932, 5.65568542, 8.55883312, + 5.65561199, 8.55877876, 7.25379658, 7.25379658, 8.55869579, 7.2537303, + 7.25386381, 7.25361061, 5.65548229, 7.2535677, 5.65561199, 7.25340509, + 7.25353432, 7.25369596, 7.25324774, 8.55786228, 7.25330639, 7.2533021, + 8.55799294, 7.25335455, 7.25319529, 8.55805206, 8.55793667, 8.5575819, + 5.65506649, 8.55790138, 5.65501547, 7.25293398, 7.2528553, 8.5573616, + 8.55728531, 9.68801403, 8.5574913, 7.25287628, 7.25277567, 7.25278425, + 7.25278997, 7.25253439, 8.55725384, 7.25249672, 8.55700302, 7.25253105, + 8.55709553, 8.5569582, 7.25251722, 8.55688763, 8.5569849, 8.55723763, + 8.55723763, 7.25223207, 8.55659676, 8.55668068, 5.65439987, 7.25216007, + 7.25211763, 7.25200033, 7.25216198, 7.25210667, 7.25187397, 7.25212336, + 7.25184345, 7.25200224, 7.25176382, 8.55617809, 7.25167179, 7.25205994, + 8.55602264, 7.25169182, 8.55579948, 7.25172567, 7.25170994, 5.65390444, + 8.55595493, 7.25141478, 8.55583096, 8.55572987, 8.55553055, 9.68609142, + 7.25145721, 7.25135231, 7.25115252, 5.65349722, 8.55524635, 8.55536175, + 7.25137615, 7.25113249, 8.55534649, 8.55531979, 7.25079775, 7.25078535, + 8.5548172, 8.5549469, 8.55503178, 7.2507925, 7.25085068, 9.68505383, + 7.25059462, 7.25072145, 8.55459213, 8.55461311, 8.55462551, 7.25028563, + 8.55433846, 7.25050592, 7.25021172, 5.65288115, 7.25018215, 8.55431271, + 8.55431271, 8.55459404, 8.55426311, 8.55444622, 8.55416203, 8.55414963, + 7.25023746, 7.25019789, 7.25023174, 9.68446255, 8.55406952, 8.55403042, + 8.5539856, 8.55387115, 8.55405903, 5.65262175, 5.65249205, 7.24991274, + 7.24984884, 7.24983072, 8.55391216, 7.24978399, 7.24971724, 8.55356598, + 7.24967194, 7.24956989, 8.55361843, 8.55347538, 7.24954319, 7.24937868, + 9.68331528, 7.24953318, 5.65228748, 5.652246, 8.55346203, 8.55348969, + 7.24943733, 7.24951839, 9.68309498, 8.55303669, 7.24932718, 8.55316162, + 8.55313492, 8.55292797, 5.6518712, 7.24900436, 7.24881744, 8.552742, + 7.24881029, 8.55268097, 7.24863386, 8.55238819, 8.55251598, 8.55226612, + 8.55213356, 7.2486558, 7.24858665, 7.24843979, 8.5519743, 7.2485404, + 8.55237484, 5.6512804, 9.68210697, 5.65125036, 5.65126991, 7.24832678, + 7.24830437, 8.55171585, 8.55191326, 7.24792719, 5.65123224, 5.65116167, + 5.65111923, 7.10352278, 8.55168629, 7.24795628, 7.24798918, 5.65098667, + 7.24778652, 5.65081835, 7.24775505, 8.55150986, 8.55104733, 7.24750519, + 7.24751616, 7.24740601, 5.65071297, 7.2471199, 8.55104923, 7.24718094, + 7.24718094, 5.65045595, 5.65034771, 7.24705362, 8.55052853, 8.55055237, + 9.68021393, 7.24703074, 7.24700546, 7.2468791, 8.55047607, 8.55031967, + 8.55006409, 7.24671173, 8.55018425, 5.65006161, 7.24654198, 7.24667978, + 8.55024052, 8.55002308, 8.54984283, 7.2463932, 8.549963, 7.24624062, + 5.64966202, 8.54976463, 7.2464385, 7.24616528, 5.64968157, 7.24625301, + 7.24625301, 5.64964485, 7.24600124, 7.24617243, 7.24603462, 7.24565554, + 7.24591446, 8.54931736, 7.24572277, 7.24607801, 7.24639654, 7.24686337, + 5.64997101, 5.46363926, 7.25156784, 3.37094092, 3.3684454, 8.42868042, + 9.57192612, 8.42880917, 8.42844296, 8.42857552, 9.57120419, 8.42841721, + 7.24512768, 7.24531698, 7.24521542, 7.24513388, 8.54863071, 7.24524355, + 7.24524355, 7.2449584, 7.24519587, 8.54811764, 9.67825794, 8.54873371, + 7.10020876, 8.4287529, 8.42825603, 8.42836761, 7.10093164, 9.57135582, + 9.57128143, 9.57122707, 9.5712347, 8.428545, 8.42840672, 8.42864037, + 9.57113743, 8.42865944, 7.10092306, 7.1008482, 9.57125187, 8.42864418, + 8.42878723, 8.42864037, 9.57126236, 9.57108593, 8.42838001, 8.42839146, + 9.57105923, 9.5710125, 9.57113552, 9.57125187, 7.10095406, 7.1009655, + 8.42864895, 8.4284811, 8.42847538, 8.42857075, 8.4283371, 8.42842674, + 8.42862797, 8.42862415, 9.57119846, 8.42851353, 8.42839527, 8.42869663, + 8.42868996, 7.10073709, 7.10088396, 9.57122993, 9.57119274, 7.1010108, + 8.42820644, 7.10094309, 8.428442, 8.42838955, 7.10102987, 8.42846394, + 7.10093975, 8.42840576, 8.42843342, 8.42841434, 7.10092974, 8.42848778, + 9.57113075, 8.42837715, 7.10103798, 8.42836857, 7.10095501, 8.4285183, + 9.57118511, 8.42850304, 8.42868614, 7.10112667, 7.10101271, 8.4285059, + 8.42863846, 7.10092402, 8.42842674, 8.42856693, 9.57147312, 8.42837334, + 8.42853928, 9.57138634, 8.42846012, 8.42863846, 8.42846298, 8.42854691, + 7.10100889, 8.42851162, 8.42870522, 7.10092068, 8.42854214, 7.1009593, + 8.4288168, 7.10105085, 9.57123661, 7.24546003, 7.24555874, 5.64907312, + 7.24532652, 8.54855824, 8.54862595, 7.24536324, 8.54894733, 8.54878044, + 9.67820072, 7.24535894, 9.67810154, 8.54867935, 7.24545479, 8.548522, + 7.24543715, 9.67821407, 7.24544954, 8.54859447, 7.24540424, 9.67797375, + 8.54864597, 9.67788601, 7.24540663, 8.54864693, 7.24544239, 7.24534893, + 7.24559736, 7.24541664, 8.54880142, 8.54874325, 9.67810822, 8.5487833, + 7.24539614, 7.24536085, 8.54876518, 7.24548101, 8.54850388, 8.5486517, + 7.24551249, 8.5488081, 8.54863453, 7.2455368, 8.54872322, 8.54858303, + 9.6779995, 8.54874039, 7.24553061, 8.54874229, 7.24536753, 8.5488472, + 8.5488472, 7.24538708, 5.6490202, 9.67805672, 5.64908886, 8.54893017, + 7.24557877, 7.24542999, 8.54860115, 8.54863644, 7.2454915, 7.24547625, + 8.54874134, 7.24553537, 7.24555111, 8.54884815, 9.67792988, 7.24539948, + 7.2455492, 7.24530125, 7.24537611, 7.24520969, 7.24547386, 8.5486784, + 7.24558544, 8.54885006, 7.24545813, 7.24534845, 7.24542189, 8.54862404, + 9.67794895, 8.54870033, 7.24543905, 9.678298, 7.24546051, 8.54885578, + 7.24557638, 8.54884434, 7.24532843, 9.67792511, 8.54874706, 5.64910603, + 8.54879284, 7.24535847, 9.67820644, 8.54885197, 9.67829227, 7.24568176, + 8.54878044, 8.54878426, 9.67803574, 8.54872894, 7.24547672, 8.54866505, + 8.5488224, 8.54887772, 7.24542904, 7.24542189, 7.24536514, 7.24555969, + 8.54885864, 8.54864979, 9.67793369, 8.54869938, 8.54872608, 7.24535036, + 7.24536753, 7.24552155, 7.24548578, 5.64906788, 7.24538946, 7.24533701, + 7.24556303, 7.24538946, 8.54896259, 7.24564791, 8.54893398, 8.5488739, + 7.24552155, 9.67792606, 7.24553394, 7.24541426, 7.24551487, 8.54875183, + 7.24564695, 8.54863834, 9.67806911, 8.54866695, 3.04500675, 7.10104704, + 9.5712862, 8.42847538, 7.1011796, 8.4287672, 7.10095692, 7.10102034, + 8.42850971, 9.57134724, 8.42857933, 8.42859364, 8.4284935, 8.4283762, + 7.10103607, 7.10089874, 8.42836475, 5.46396208, 3.36807752, 8.42859268, + 7.10104084, 7.100914, 3.04496884, 8.42867851, 8.4285984, 3.04501152, + 5.64914608, 7.10101652, 8.42857456, 8.42857361, 8.42860985, 8.42871094, + 9.57142353, 8.42868614, 7.2454505, 8.4284029, 7.10101032, 3.36808872, + 3.04504347, 8.42857265, 7.10102367, 8.42846966, 5.64913654, 8.42867661, + 5.46405458, 5.4641037, 7.10097075, 7.10097075, 9.57109261, 5.64907026, + 9.57126427, 7.10087252, 5.64899302, 5.46416044, 8.42867565, 7.10102081, + 3.0449779, 3.04504228, 8.42847252, 7.10094595, 8.4284668, 8.42854977, + 8.42854977, 8.42850494, 7.2454505, 8.42854977, 7.1010952, 9.57125759, + 9.5714159, 3.36807466, 5.46410608, 7.10106945, 7.10103035, 7.10110712, + 3.36813903, 7.10105944, 7.10098886, 8.42852306, 8.4284153, 8.42852592, + 8.42850113, 3.0450294, 8.42829037, 8.4286375, 7.24541759, 7.1008606, + 8.42870426, 8.42858601, 3.36807442, 8.54880619, 8.42840481, 7.10107565, + 8.42862511, 7.10099316, 8.42841148, 5.6489954, 9.57114029, 5.46398115, + 7.24545479, 3.36805773, 8.42847729, 7.10098934, 5.46399784, 3.36807752, + 5.46398401, 7.10101175, 7.10096073, 8.42848873, 8.42824173, 7.24545765, + 5.46405935, 5.64890766, 3.3680203, 8.5487566, 9.57122612, 7.10083675, + 5.64896965, 5.46405077, 8.42843437, 8.54873371, 7.24538803, 7.10098839, + 3.04499364, 8.4284668, 5.6490097, 8.42846203, 8.42853737, 3.04499555, + 8.42841721, 8.54864311, 7.10086012, 7.10118818, 5.64909124, 8.42842484, + 7.10094309, 3.36813259, 7.24543285, 8.428545, 7.24546289, 7.245327, + 7.24537659, 8.5482502, 8.54854012, 7.24545431, 7.24539614, 9.67787552, + 9.6777153, 8.5486412, 7.24538374, 7.24531031, 8.54841423, 8.54879951, + 9.67814732, 8.54858971, 8.54860973, 8.5487566, 8.54844952, 7.24545097, + 9.67816162, 7.24541998, 8.54874897, 8.54876995, 8.5487833, 9.67811584, + 8.54877567, 7.24530172, 8.54862404, 8.54862404, 7.24525642, 7.24555159, + 7.24561071, 8.54864693, 8.54877472, 8.5489006, 7.24542665, 9.67788696, + 9.6779623, 8.54860687, 7.24537039, 8.54876709, 8.54876328, 7.2455492, + 7.2455492, 7.24531937, 7.24541426, 8.54865742, 9.67789841, 8.54874897, + 8.54868698, 8.54879856, 8.54867077, 9.67799473, 7.24543953, 7.24538422, + 8.54859543, 8.54849625, 7.24532604, 7.24532604, 8.54859734, 8.54887295, + 8.5487175, 8.54844284, 8.54866409, 8.54863548, 8.54869366, 9.67795372, + 7.24526262, 8.5487442, 7.24523973, 7.24526501, 8.54886913, 10.6894016, + 8.54865932, 7.24516201, 8.54880714, 7.24530602, 8.54856491, 7.24531555, + 8.54861736, 7.24542475, 8.54858685, 8.54860497, 8.54874706, 8.5486145, + 8.54876232, 8.54862118, 7.24545193, 8.5487318, 7.24531507, 7.24539566, + 8.5487566, 7.24530697, 9.6778307, 8.54864216, 8.5487957, 9.67823124, + 8.54878998, 9.67807007, 7.24551582, 7.24530745, 8.54856682, 8.54863167, + 9.67795753, 8.54868317, 8.54869652, 8.54869175, 7.24533987, 9.6779089, + 9.67794037, 8.54869556, 8.54859161, 7.24542713, 8.54850006, 7.24544525, + 7.2452364, 8.54864407, 7.24532986, 8.54858685, 8.54850101, 8.54864216, + 8.54854488, 9.67793751, 9.67785645, 8.54859447, 7.24539995, 7.24516296, + 8.54874802, 7.24538231, 7.10097742, 5.64900589, 8.54846287, 9.67804337, + 9.67804337, 9.67807293, 3.04497218, 7.10076523, 8.42840195, 9.67793846, + 8.42849731, 7.10094786, 5.6490345, 5.64898491, 8.42831802, 7.24532843, + 7.24534321, 7.10093927, 8.54858303, 8.42842102, 8.4285326, 3.04494166, + 7.24514961, 5.46398306, 5.46397352, 3.36808443, 8.42861557, 7.10091591, + 3.36799455, 3.36801362, 5.64899302, 7.24537277, 5.64897776, 8.54866791, + 9.67799759, 8.54859638, 8.42818928, 5.46391582, 9.67796993, 8.54866982, + 7.2453022, 3.36811757, 8.54859352, 8.54864693, 5.46399832, 8.42839432, + 7.10096121, 8.54865074, 7.24533892, 8.54859734, 8.54843903, 5.46399736, + 5.64901304, 8.54851532, 7.10091066, 8.42855453, 8.54862022, 8.54861546, + 9.67809486, 8.54871464, 3.36806703, 3.04499364, 5.6490078, 8.54854012, + 7.2452898, 8.42834663, 9.67762852, 7.24548531, 3.36801696, 8.54853439, + 7.24530888, 8.54851246, 7.10093832, 7.10067272, 8.54868984, 8.54847813, + 3.04493189, 8.5484705, 7.24545097, 3.04500961, 5.46406269, 3.36800504, + 7.10094357, 8.54851151, 8.54866886, 10.5927849, 7.24540377, 7.10075283, + 7.24534369, 8.54843235, 9.67783642, 7.24517918, 8.54879951, 8.548522, + 8.548522, 9.67778301, 8.54860115, 7.10093164, 8.42852306, 5.64902306, + 3.04495406, 7.24548578, 3.04489493, 3.04494214, 7.24534893, 8.54843903, + 5.46406937, 3.36802626, 9.67804337, 7.24543238, 5.64898205, 8.54842758, + 3.04494452, 3.04497337, 7.24522591, 8.54879475, 7.2453618, 8.42841053, + 9.57095623, 7.10093355, 5.46401739, 8.42823792, 8.54849911, 8.54875183, + 5.64911509, 7.24519968, 3.36812043, 7.24549627, 3.04498339, 7.10086727, + 8.42856026, 8.42859173, 3.04481268, 7.24532175, 3.36802101, 8.42823792, + 8.54860115, 3.36796093, 9.67775059, 9.67775059, 9.67768764, 8.54860115, + 9.67777348, 8.54845715, 8.54848385, 7.24536133, 8.54850864, 8.54850769, + 9.67781639, 7.24537849, 7.24520159, 8.54855919, 8.54855537, 7.24519587, + 8.54842854, 9.677948, 9.67787457, 8.5485487, 8.54859638, 8.54852486, + 8.54869843, 8.54856968, 7.24519157, 10.6890306, 8.54838657, 7.24541616, + 8.54855347, 9.67801189, 8.54869461, 8.54869461, 7.24515867, 8.5485239, + 8.54871655, 8.54845047, 8.54845428, 7.24527645, 8.54839706, 7.24514294, + 7.24548101, 8.54843044, 8.54857635, 8.54848385, 8.54835129, 8.5487299, + 8.5487299, 8.54850483, 8.54849625, 8.54861069, 7.24535179, 8.5484066, + 8.54865646, 7.24535894, 7.24524355, 10.6893644, 8.54849148, 8.54852676, + 7.24538612, 7.24532795, 9.67798424, 8.54865551, 10.6891737, 7.2452693, + 8.54872513, 8.54863167, 9.67783546, 8.548522, 8.5486517, 8.54854679, + 8.54851913, 8.54853058, 7.24531841, 8.54833317, 9.67802525, 9.67797279, + 8.54864502, 8.54887867, 8.54854679, 7.24550486, 8.54846764, 9.67776108, + 5.64895296, 10.6895771, 9.67808437, 7.24518299, 9.67793846, 8.5486908, + 9.67781734, 8.54827976, 9.67785645, 8.54854298, 7.2452569, 8.54874706, + 9.67804909, 3.04495716, 7.24530792, 3.04493141, 8.54869556, 7.24528456, + 3.36803365, 9.67791367, 8.54862022, 8.54866791, 7.24512434, 9.67784405, + 8.54853153, 7.10101318, 7.24540567, 8.54863548, 9.67797852, 8.54854393, + 8.42855549, 8.54858875, 9.6777401, 9.67780876, 8.54880238, 7.24542999, + 9.67799664, 9.67793846, 9.67816734, 9.67816734, 5.64909172, 9.67804813, + 9.67784214, 8.54853344, 8.54864025, 5.6489892, 8.54871655, 7.24519587, + 8.54860115, 8.54881001, 7.24530077, 7.24541664, 9.67792988, 5.46404219, + 8.54865074, 8.54857445, 8.54853439, 8.54873848, 8.54854107, 8.54854488, + 8.54865932, 7.24547338, 7.24516535, 8.54849148, 8.54853058, 9.67776966, + 8.5487051, 9.67784119, 8.54872322, 7.24544525, 9.67783737, 8.54861641, + 8.5487442, 8.54861069, 9.6781168, 7.24540949, 7.24533892, 3.04493976, + 8.54883671, 9.67795563, 9.67778397, 8.54867268, 9.6780014, 8.54862785, + 9.67795277, 8.54854679, 8.54880428, 8.54883575, 7.24538469, 9.67802906, + 8.54888344, 9.67798042, 8.54868507, 5.46399879, 10.6895142, 8.54852676, + 8.54869843, 8.54868317, 8.54869843, 3.04499841, 3.36809969, 7.24543095, + 9.67807961, 9.67805576, 9.67798233, 8.54868221, 8.54860497, 8.54870701, + 8.54848385, 8.5486412, 9.67798615, 8.54881287, 9.67815304, 7.24539518, + 5.64905691, 9.67804432, 3.3681438, 9.67823029, 8.54854393, 8.54850578, + 7.24552011, 9.67810535, 8.54876232, 9.67815113, 8.54876328, 9.67800903, + 8.54845333, 8.54876232, 9.67807293, 9.67807293, 7.24555397, 9.67800236, + 8.54891872, 8.54874134, 9.67811108, 8.5487175, 9.6781702, 8.54857922, + 9.67806149, 9.67820644, 9.67806911, 5.46411324, 8.54864311, 3.36811256, + 8.548769, 8.54872608, 7.24541473, 9.67819977, 8.54888153, 7.24551058, + 7.2453866, 8.54862022, 3.04508567, 3.36803555, 5.64911175, 5.64906168, + 7.24531889, 8.54879284, 7.24552441, 8.54881668, 9.67796135, 9.67796993, + 9.67819977, 8.54873562, 5.46410894, 8.42869854, 7.10112619, 5.46413422, + 5.46404457, 7.10100889, 7.10103083, 7.10087013, 8.42854595, 5.46431112, + 7.1013093, 8.42872524, 8.42856789, 8.42877579, 8.4286108, 7.10108471, + 7.10111713, 8.42870522, 7.10117722, 7.10108423, 7.10116291, 5.46417046, + 5.46410227, 8.42852306, 7.10091877, 7.10091877, 8.42876053, 7.1011095, + 8.42865086, 8.42845726, 7.10110044, 8.42866516, 5.46416044, 7.10095024, + 5.46399307, 5.46412134, 7.10102654, 8.42853832, 7.10112858, 8.42865181, + 8.42865181, 7.10094023, 7.10088062, 8.42878056, 7.10099649, 7.1011157, + 7.10101175, 5.46409607, 7.10099077, 7.10110712, 7.1011138, 7.10096455, + 8.4284668, 7.10101175, 5.46410799, 7.10104561, 8.42859745, 5.46416044, + 7.10087252, 5.46405506, 8.42874813, 7.10093355, 5.4641819, 5.4641881, + 5.46416664, 7.10129452, 7.10114193, 7.1008954, 8.42853546, 5.4641099, + 5.4641099, 7.10110474, 7.1011548, 8.42870617, 5.46414089, 7.10110712, + 8.42862034, 5.4642539, 7.10094786, 7.10091734, 7.10112095, 7.10101986, + 8.54879189, 8.54861546, 9.67801952, 8.54868698, 8.54884529, 8.54895592, + 8.54869461, 9.67792988, 9.67812061, 9.67813778, 8.54891109, 11.6121511, + 8.54892159, 7.24555683, 7.24559784, 8.54881954, 9.67842865, 8.54896736, + 7.24565172, 8.54852104, 8.54890347, 8.54875278, 9.67812347, 10.6893873, + 9.67832088, 8.5486536}; diff --git a/src/mocksensors/lynx_flight_data/lynx_airspeed_data.h b/src/mocksensors/lynx_flight_data/lynx_airspeed_data.h index 7fa3e38da..972c16abe 100644 --- a/src/mocksensors/lynx_flight_data/lynx_airspeed_data.h +++ b/src/mocksensors/lynx_flight_data/lynx_airspeed_data.h @@ -1,5 +1,5 @@ /* Copyright (c) 2021 Skyward Experimental Rocketry - * Authors: Luca Conterio + * Author: Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -27,4 +27,4 @@ * Sampled at 42 Hz (24 ms period). */ static constexpr unsigned AIRSPEED_DATA_SIZE = 5384; -extern const float AIRSPEED_DATA[AIRSPEED_DATA_SIZE]; \ No newline at end of file +extern const float AIRSPEED_DATA[AIRSPEED_DATA_SIZE]; diff --git a/src/mocksensors/lynx_flight_data/lynx_gps_data.cpp b/src/mocksensors/lynx_flight_data/lynx_gps_data.cpp index 693cea75a..14ddef5b5 100644 --- a/src/mocksensors/lynx_flight_data/lynx_gps_data.cpp +++ b/src/mocksensors/lynx_flight_data/lynx_gps_data.cpp @@ -1,5 +1,5 @@ /* Copyright (c) 2021 Skyward Experimental Rocketry - * Authors: Luca Conterio + * Author: Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal diff --git a/src/mocksensors/lynx_flight_data/lynx_gps_data.h b/src/mocksensors/lynx_flight_data/lynx_gps_data.h index 3318c3a51..bac8b29fa 100644 --- a/src/mocksensors/lynx_flight_data/lynx_gps_data.h +++ b/src/mocksensors/lynx_flight_data/lynx_gps_data.h @@ -1,5 +1,5 @@ /* Copyright (c) 2021 Skyward Experimental Rocketry - * Authors: Luca Conterio + * Author: Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -34,4 +34,4 @@ extern const float GPS_DATA_LON[GPS_DATA_SIZE]; extern const float GPS_DATA_VNORD[GPS_DATA_SIZE]; extern const float GPS_DATA_VEAST[GPS_DATA_SIZE]; extern const float GPS_DATA_VDOWN[GPS_DATA_SIZE]; -extern const uint8_t GPS_DATA_NSATS[GPS_DATA_SIZE]; \ No newline at end of file +extern const uint8_t GPS_DATA_NSATS[GPS_DATA_SIZE]; diff --git a/src/mocksensors/lynx_flight_data/lynx_imu_data.cpp b/src/mocksensors/lynx_flight_data/lynx_imu_data.cpp index 1bcd87d3c..93c39119b 100644 --- a/src/mocksensors/lynx_flight_data/lynx_imu_data.cpp +++ b/src/mocksensors/lynx_flight_data/lynx_imu_data.cpp @@ -1,5 +1,5 @@ /* Copyright (c) 2021 Skyward Experimental Rocketry - * Authors: Luca Conterio + * Author: Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -19418,4 +19418,4 @@ const float MAG_DATA[IMU_DATA_SIZE][MOTION_SENSOR_AXIS_NUM] = { {-14.3207197, 20.2702618, 3.25543451}, {-14.1142645, 20.1281185, 3.46541166}, {-14.2086792, 20.2700577, 3.18543696}, -}; \ No newline at end of file +}; diff --git a/src/mocksensors/lynx_flight_data/lynx_imu_data.h b/src/mocksensors/lynx_flight_data/lynx_imu_data.h index 079f707f1..4addd2945 100644 --- a/src/mocksensors/lynx_flight_data/lynx_imu_data.h +++ b/src/mocksensors/lynx_flight_data/lynx_imu_data.h @@ -1,5 +1,5 @@ /* Copyright (c) 2021 Skyward Experimental Rocketry - * Authors: Luca Conterio + * Author: Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -32,4 +32,4 @@ static constexpr unsigned IMU_DATA_SIZE = 6463; static const unsigned MOTION_SENSOR_AXIS_NUM = 3; extern const float ACCEL_DATA[IMU_DATA_SIZE][MOTION_SENSOR_AXIS_NUM]; extern const float GYRO_DATA[IMU_DATA_SIZE][MOTION_SENSOR_AXIS_NUM]; -extern const float MAG_DATA[IMU_DATA_SIZE][MOTION_SENSOR_AXIS_NUM]; \ No newline at end of file +extern const float MAG_DATA[IMU_DATA_SIZE][MOTION_SENSOR_AXIS_NUM]; diff --git a/src/mocksensors/lynx_flight_data/lynx_press_data.cpp b/src/mocksensors/lynx_flight_data/lynx_press_data.cpp index b54217a83..2f5e47678 100644 --- a/src/mocksensors/lynx_flight_data/lynx_press_data.cpp +++ b/src/mocksensors/lynx_flight_data/lynx_press_data.cpp @@ -1,5 +1,5 @@ /* Copyright (c) 2021 Skyward Experimental Rocketry - * Authors: Luca Conterio + * Author: Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -1459,4 +1459,4 @@ const float PRESSURE_DATA[PRESSURE_DATA_SIZE] = { 84713.2891, 84717.0547, 84715.2734, 84715.2734, 84722.1719, 84726.1016, 84721.5234, 84716.8203, 84720.4688, 84720.4688, 84719.8516, 84723.0156, 84719.4922, 84715.6016, 84722.8984, 84722.8984, 84719.1562, -}; \ No newline at end of file +}; diff --git a/src/mocksensors/lynx_flight_data/lynx_press_data.h b/src/mocksensors/lynx_flight_data/lynx_press_data.h index 282a02589..194b19442 100644 --- a/src/mocksensors/lynx_flight_data/lynx_press_data.h +++ b/src/mocksensors/lynx_flight_data/lynx_press_data.h @@ -1,5 +1,5 @@ /* Copyright (c) 2021 Skyward Experimental Rocketry - * Authors: Luca Conterio + * Author: Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -27,4 +27,4 @@ * Sampled at 66 Hz (15 ms period). */ static constexpr unsigned PRESSURE_DATA_SIZE = 8615; -extern const float PRESSURE_DATA[PRESSURE_DATA_SIZE]; \ No newline at end of file +extern const float PRESSURE_DATA[PRESSURE_DATA_SIZE]; diff --git a/src/mocksensors/lynx_flight_data/lynx_pressure_static_data.cpp b/src/mocksensors/lynx_flight_data/lynx_pressure_static_data.cpp index 7b7641fab..59dce9ee8 100644 --- a/src/mocksensors/lynx_flight_data/lynx_pressure_static_data.cpp +++ b/src/mocksensors/lynx_flight_data/lynx_pressure_static_data.cpp @@ -1,5 +1,5 @@ /* Copyright (c) 2021 Skyward Experimental Rocketry - * Authors: Luca Conterio + * Author: Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -921,4 +921,4 @@ const float PRESSURE_STATIC_DATA[PRESSURE_STATIC_DATA_SIZE] = { 87765.6719, 87765.6719, 87765.6719, 87775.625, 87770.6484, 87770.6484, 87760.6953, 87770.6484, 87775.625, 87765.6719, 87755.7109, 87760.6953, 87760.6953, 87765.6719, -}; \ No newline at end of file +}; diff --git a/src/mocksensors/lynx_flight_data/lynx_pressure_static_data.h b/src/mocksensors/lynx_flight_data/lynx_pressure_static_data.h index f28074904..fd2e92fdf 100644 --- a/src/mocksensors/lynx_flight_data/lynx_pressure_static_data.h +++ b/src/mocksensors/lynx_flight_data/lynx_pressure_static_data.h @@ -1,5 +1,5 @@ /* Copyright (c) 2021 Skyward Experimental Rocketry - * Authors: Luca Conterio + * Author: Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -27,4 +27,4 @@ * Sampled at 42 Hz (24 ms period). */ static constexpr unsigned PRESSURE_STATIC_DATA_SIZE = 5384; -extern const float PRESSURE_STATIC_DATA[PRESSURE_STATIC_DATA_SIZE]; \ No newline at end of file +extern const float PRESSURE_STATIC_DATA[PRESSURE_STATIC_DATA_SIZE]; diff --git a/src/tests/airbrakes/test-airbrakes-algorithm.cpp b/src/tests/airbrakes/test-airbrakes-algorithm.cpp index b72741ae3..5ab4f3997 100644 --- a/src/tests/airbrakes/test-airbrakes-algorithm.cpp +++ b/src/tests/airbrakes/test-airbrakes-algorithm.cpp @@ -104,10 +104,6 @@ int main() RCC->APB1ENR |= RCC_APB1ENR_TIM5EN; } - input_t input; - - input = DATA[0].input; - HardwareTimer<uint32_t> hrclock( TIM5, TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB1)); @@ -162,4 +158,4 @@ int main() while (1) { } -} \ No newline at end of file +} diff --git a/src/tests/airbrakes/test-airbrakes-interactive.cpp b/src/tests/airbrakes/test-airbrakes-interactive.cpp index 36fcdf1ae..6b6d8f6eb 100644 --- a/src/tests/airbrakes/test-airbrakes-interactive.cpp +++ b/src/tests/airbrakes/test-airbrakes-interactive.cpp @@ -33,6 +33,8 @@ #include "test_data.h" #include "utils/testutils/TestHelper.h" +using namespace miosix; +using namespace Boardcore; using namespace DeathStackBoard; using namespace std; @@ -92,8 +94,8 @@ int main() { sEventBroker.start(); - miosix::GpioPin pwmPin = miosix::GpioPin(GPIOC_BASE, 7); - pwmPin.mode(miosix::Mode::ALTERNATE); + GpioPin pwmPin = GpioPin(GPIOC_BASE, 7); + pwmPin.mode(Mode::ALTERNATE); pwmPin.alternateFunction(3); string temp; @@ -228,7 +230,6 @@ void testAirBrakes() void wiggleServo() { - string temp; cout << "\n\n** WIGGLE SERVO **\n\n"; waitUserInput(); @@ -246,7 +247,6 @@ void wiggleServo() void setServoFullyOpen() { - string temp; cout << "\n\n** SERVO FULLY OPEN **\n\n"; waitUserInput(); @@ -262,7 +262,6 @@ void setServoFullyOpen() void setServoFullyClosed() { - string temp; cout << "\n\n** SERVO FULLY CLOSED **\n\n"; waitUserInput(); @@ -278,7 +277,6 @@ void setServoFullyClosed() void resetServo() { - string temp; cout << "\n\n** RESET SERVO **\n\n"; waitUserInput(); @@ -371,4 +369,4 @@ void waitUserInput() cout << "Write 'start' to begin the test:\n"; getline(cin, temp); } while (temp != "start"); -} \ No newline at end of file +} diff --git a/src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp b/src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp index 74070c368..2b13a04b5 100644 --- a/src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp +++ b/src/tests/catch/ada/ada_kalman_p/test-ada-simulation.cpp @@ -57,7 +57,7 @@ constexpr unsigned int SHADOW_MODE_END_INDEX = 30; constexpr unsigned int APOGEE_SAMPLE = 382; // Mock sensors for testing purposes -class MockPressureSensor : public Sensor<PressureData> +class MockPressureSensor : public Boardcore::Sensor<Boardcore::PressureData> { public: MockPressureSensor() {} @@ -66,7 +66,7 @@ public: bool selfTest() override { return true; } - PressureData sampleImpl() override + Boardcore::PressureData sampleImpl() override { float press = 0.0; @@ -86,8 +86,8 @@ public: } } - return PressureData{TimestampTimer::getInstance().getTimestamp(), - press}; + return Boardcore::PressureData{ + Boardcore::TimestampTimer::getInstance().getTimestamp(), press}; } void signalLiftoff() { before_liftoff = false; } @@ -106,18 +106,18 @@ private: float quantization(float sample) { return round(sample / LSB) * LSB; } }; -class MockGPSSensor : public Sensor<GPSData> +class MockGPSSensor : public Boardcore::Sensor<Boardcore::GPSData> { public: bool init() { return true; } bool selfTest() { return true; } - GPSData sampleImpl() { return GPSData{}; } + Boardcore::GPSData sampleImpl() { return Boardcore::GPSData{}; } }; MockPressureSensor mock_baro; MockGPSSensor mock_gps; -using ADACtrl = ADAController<PressureData, GPSData>; +using ADACtrl = ADAController<Boardcore::PressureData, Boardcore::GPSData>; ADACtrl *ada_controller; void checkState(unsigned int i, ADAKalmanState state) @@ -149,7 +149,7 @@ TEST_CASE("Testing ada_controller from calibration to first descent phase") // Start event broker and ada_controller sEventBroker.start(); - EventCounter counter{sEventBroker}; + Boardcore::EventCounter counter{sEventBroker}; counter.subscribe(TOPIC_ADA); // Startup: we should be in idle @@ -277,4 +277,4 @@ TEST_CASE("Testing ada_controller from calibration to first descent phase") REQUIRE(ada_controller->testState(&ADACtrl::state_drogueDescent)); } } -} \ No newline at end of file +} diff --git a/src/tests/catch/catch-tests-entry.cpp b/src/tests/catch/catch-tests-entry.cpp index 3167810e3..1b970e126 100644 --- a/src/tests/catch/catch-tests-entry.cpp +++ b/src/tests/catch/catch-tests-entry.cpp @@ -78,13 +78,13 @@ using std::vector; */ vector<string> splitSpaces(string str) { - unsigned int p = 0, p2; + unsigned int p = 0; bool end = false; vector<string> out; do { - p2 = str.find(" ", p); + unsigned int p2 = str.find(" ", p); if (p2 == string::npos) // No match { @@ -152,4 +152,4 @@ int main() { Thread::sleep(10000); } -} \ No newline at end of file +} diff --git a/src/tests/catch/fsm/test-ada.cpp b/src/tests/catch/fsm/test-ada.cpp index 775ae083f..b2cbd74a5 100644 --- a/src/tests/catch/fsm/test-ada.cpp +++ b/src/tests/catch/fsm/test-ada.cpp @@ -53,6 +53,8 @@ public: ADAControllerFixture() { sEventBroker.start(); + // cppcheck-suppress noCopyConstructor + // cppcheck-suppress noOperatorEq controller = new ADACtrl(mock_baro, mock_gps); controller->start(); } @@ -79,7 +81,8 @@ TEST_CASE_METHOD(ADAControllerFixture, "Testing transitions from idle") SECTION("EV_CALIBRATE_ADA -> CALIBRATING") { - REQUIRE(testFSMTransition(*controller, Event{EV_CALIBRATE_ADA}, + REQUIRE(testFSMTransition(*controller, + Boardcore::Event{EV_CALIBRATE_ADA}, &ADACtrl::state_calibrating)); } } @@ -90,13 +93,14 @@ TEST_CASE_METHOD(ADAControllerFixture, "Testing transitions from calibrating") SECTION("EV_CALIBRATE_ADA -> CALIBRATING") { - REQUIRE(testFSMTransition(*controller, Event{EV_CALIBRATE_ADA}, + REQUIRE(testFSMTransition(*controller, + Boardcore::Event{EV_CALIBRATE_ADA}, &ADACtrl::state_calibrating)); } SECTION("EV_ADA_READY -> READY") { - REQUIRE(testFSMTransition(*controller, Event{EV_ADA_READY}, + REQUIRE(testFSMTransition(*controller, Boardcore::Event{EV_ADA_READY}, &ADACtrl::state_ready)); } } @@ -107,13 +111,14 @@ TEST_CASE_METHOD(ADAControllerFixture, "Testing transitions from ready") SECTION("EV_CALIBRATE_ADA -> CALIBRATING") { - REQUIRE(testFSMTransition(*controller, Event{EV_CALIBRATE_ADA}, + REQUIRE(testFSMTransition(*controller, + Boardcore::Event{EV_CALIBRATE_ADA}, &ADACtrl::state_calibrating)); } SECTION("EV_LIFTOFF -> SHADOW_MODE") { - REQUIRE(testFSMTransition(*controller, Event{EV_LIFTOFF}, + REQUIRE(testFSMTransition(*controller, Boardcore::Event{EV_LIFTOFF}, &ADACtrl::state_shadowMode)); } } @@ -124,7 +129,8 @@ TEST_CASE_METHOD(ADAControllerFixture, "Testing transitions from shadow_mode") SECTION("EV_SHADOW_MODE_TIMEOUT -> ACTIVE") { - REQUIRE(testFSMTransition(*controller, Event{EV_SHADOW_MODE_TIMEOUT}, + REQUIRE(testFSMTransition(*controller, + Boardcore::Event{EV_SHADOW_MODE_TIMEOUT}, &ADACtrl::state_active)); } } @@ -135,7 +141,8 @@ TEST_CASE_METHOD(ADAControllerFixture, "Testing transitions from active") SECTION("EV_ADA_APOGEE_DETECTED -> PRESSURE_STABILIZATION") { - REQUIRE(testFSMTransition(*controller, Event{EV_ADA_APOGEE_DETECTED}, + REQUIRE(testFSMTransition(*controller, + Boardcore::Event{EV_ADA_APOGEE_DETECTED}, &ADACtrl::state_pressureStabilization)); } } @@ -147,9 +154,9 @@ TEST_CASE_METHOD(ADAControllerFixture, SECTION("EV_TIMEOUT_PRESS_STABILIZATION -> DROGUE_DESCENT") { - REQUIRE(testFSMTransition(*controller, - Event{EV_TIMEOUT_PRESS_STABILIZATION}, - &ADACtrl::state_drogueDescent)); + REQUIRE(testFSMTransition( + *controller, Boardcore::Event{EV_TIMEOUT_PRESS_STABILIZATION}, + &ADACtrl::state_drogueDescent)); } } @@ -160,7 +167,8 @@ TEST_CASE_METHOD(ADAControllerFixture, SECTION("EV_ADA_DPL_ALT_DETECTED -> END") { - REQUIRE(testFSMTransition(*controller, Event{EV_ADA_DPL_ALT_DETECTED}, + REQUIRE(testFSMTransition(*controller, + Boardcore::Event{EV_ADA_DPL_ALT_DETECTED}, &ADACtrl::state_end)); } } @@ -168,4 +176,4 @@ TEST_CASE_METHOD(ADAControllerFixture, TEST_CASE_METHOD(ADAControllerFixture, "Testing transitions from end") { controller->transition(&ADACtrl::state_end); -} \ No newline at end of file +} diff --git a/src/tests/catch/fsm/test-airbrakes.cpp b/src/tests/catch/fsm/test-airbrakes.cpp index 51d7ec5bb..e9096fffb 100644 --- a/src/tests/catch/fsm/test-airbrakes.cpp +++ b/src/tests/catch/fsm/test-airbrakes.cpp @@ -40,6 +40,7 @@ using miosix::Thread; using namespace DeathStackBoard; +using namespace Boardcore; class InputClass { @@ -59,7 +60,9 @@ private: protected: T sampleImpl() override { + // cppcheck-suppress unassignedVariable T data; + // cppcheck-suppress uninitvar return data; } @@ -74,6 +77,8 @@ public: // This is called at the beginning of each test / section AirBrakesControllerFixture() { + // cppcheck-suppress noCopyConstructor + // cppcheck-suppress noOperatorEq controller = new AirBrakesController<InputClass>(sensor); sEventBroker.start(); controller->start(); @@ -198,4 +203,4 @@ TEST_CASE_METHOD(AirBrakesControllerFixture, testFSMTransition(*controller, {EV_TEST_TIMEOUT}, &AirBrakesController<InputClass>::state_idle)); } -} \ No newline at end of file +} diff --git a/src/tests/catch/fsm/test-deployment.cpp b/src/tests/catch/fsm/test-deployment.cpp index 137f2f925..699e7d9f9 100644 --- a/src/tests/catch/fsm/test-deployment.cpp +++ b/src/tests/catch/fsm/test-deployment.cpp @@ -47,6 +47,8 @@ public: // This is called at the beginning of each test / section DeploymentControllerFixture() { + // cppcheck-suppress noCopyConstructor + // cppcheck-suppress noOperatorEq controller = new DeploymentController(&ejection_servo); sEventBroker.start(); controller->start(); @@ -72,26 +74,27 @@ TEST_CASE_METHOD(DeploymentControllerFixture, "Testing transitions from idle") SECTION("DPL_IDLE -> EV_RESET_SERVO") { - REQUIRE(testFSMTransition(*controller, Event{EV_RESET_SERVO}, + REQUIRE(testFSMTransition(*controller, Boardcore::Event{EV_RESET_SERVO}, &DeploymentController::state_idle)); } SECTION("DPL_IDLE -> EV_WIGGLE_SERVO") { - REQUIRE(testFSMTransition(*controller, Event{EV_WIGGLE_SERVO}, + REQUIRE(testFSMTransition(*controller, + Boardcore::Event{EV_WIGGLE_SERVO}, &DeploymentController::state_idle)); } SECTION("DPL_IDLE -> EV_NC_OPEN") { REQUIRE( - testFSMTransition(*controller, Event{EV_NC_OPEN}, + testFSMTransition(*controller, Boardcore::Event{EV_NC_OPEN}, &DeploymentController::state_noseconeEjection)); } SECTION("DPL_IDLE -> EV_CUT_DROGUE") { - REQUIRE(testFSMTransition(*controller, Event{EV_CUT_DROGUE}, + REQUIRE(testFSMTransition(*controller, Boardcore::Event{EV_CUT_DROGUE}, &DeploymentController::state_cutting)); } } @@ -103,13 +106,14 @@ TEST_CASE_METHOD(DeploymentControllerFixture, SECTION("DPL_NOSECONE_EJECTION -> EV_NC_DETACHED") { - REQUIRE(testFSMTransition(*controller, Event{EV_NC_DETACHED}, + REQUIRE(testFSMTransition(*controller, Boardcore::Event{EV_NC_DETACHED}, &DeploymentController::state_idle)); } SECTION("DPL_NOSECONE_EJECTION -> EV_NC_OPEN_TIMEOUT") { - REQUIRE(testFSMTransition(*controller, Event{EV_NC_OPEN_TIMEOUT}, + REQUIRE(testFSMTransition(*controller, + Boardcore::Event{EV_NC_OPEN_TIMEOUT}, &DeploymentController::state_idle)); } } @@ -121,7 +125,8 @@ TEST_CASE_METHOD(DeploymentControllerFixture, SECTION("DPL_CUTTING -> EV_CUTTING_TIMEOUT") { - REQUIRE(testFSMTransition(*controller, Event{EV_CUTTING_TIMEOUT}, + REQUIRE(testFSMTransition(*controller, + Boardcore::Event{EV_CUTTING_TIMEOUT}, &DeploymentController::state_idle)); } } diff --git a/src/tests/catch/fsm/test-flightstatsrecorder.cpp b/src/tests/catch/fsm/test-flightstatsrecorder.cpp index 1ca268fbe..0f40aced0 100644 --- a/src/tests/catch/fsm/test-flightstatsrecorder.cpp +++ b/src/tests/catch/fsm/test-flightstatsrecorder.cpp @@ -1,6 +1,5 @@ -/* - * Copyright (c) 2021 Skyward Experimental Rocketry - * Authors: Luca Conterio +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -51,6 +50,8 @@ public: FlightStatsFixture() { sEventBroker.start(); + // cppcheck-suppress noCopyConstructor + // cppcheck-suppress noOperatorEq fsm = new FlightStatsRecorder(); fsm->start(); } @@ -72,13 +73,13 @@ TEST_CASE_METHOD(FlightStatsFixture, "Testing transitions from idle") { SECTION("EV_LIFTOFF -> liftoff") { - REQUIRE(testFSMTransition(*fsm, Event{EV_LIFTOFF}, + REQUIRE(testFSMTransition(*fsm, Boardcore::Event{EV_LIFTOFF}, &FlightStatsRecorder::state_liftOff)); } SECTION("EV_DPL_ALTITUDE -> mainDPL") { - REQUIRE(testFSMTransition(*fsm, Event{EV_DPL_ALTITUDE}, + REQUIRE(testFSMTransition(*fsm, Boardcore::Event{EV_DPL_ALTITUDE}, &FlightStatsRecorder::state_mainDeployment)); } } @@ -90,7 +91,7 @@ TEST_CASE_METHOD(FlightStatsFixture, "Testing transitions from liftoff") SECTION("EV_STATS_TIMEOUT -> ascending") { - REQUIRE(testFSMTransition(*fsm, Event{EV_STATS_TIMEOUT}, + REQUIRE(testFSMTransition(*fsm, Boardcore::Event{EV_STATS_TIMEOUT}, &FlightStatsRecorder::state_ascending)); } } @@ -103,7 +104,7 @@ TEST_CASE_METHOD(FlightStatsFixture, "Testing transitions from ascending") SECTION("EV_APOGEE -> drogueDPL") { REQUIRE( - testFSMTransition(*fsm, Event{EV_STATS_TIMEOUT}, + testFSMTransition(*fsm, Boardcore::Event{EV_STATS_TIMEOUT}, &FlightStatsRecorder::state_drogueDeployment)); } } @@ -115,7 +116,7 @@ TEST_CASE_METHOD(FlightStatsFixture, "Testing transitions from drogueDPL") SECTION("EV_STATS_TIMEOUT -> idle") { - REQUIRE(testFSMTransition(*fsm, Event{EV_STATS_TIMEOUT}, + REQUIRE(testFSMTransition(*fsm, Boardcore::Event{EV_STATS_TIMEOUT}, &FlightStatsRecorder::state_idle)); } } @@ -127,7 +128,7 @@ TEST_CASE_METHOD(FlightStatsFixture, "Testing transitions from mainDPL") SECTION("EV_STATS_TIMEOUT -> idle") { - REQUIRE(testFSMTransition(*fsm, Event{EV_STATS_TIMEOUT}, + REQUIRE(testFSMTransition(*fsm, Boardcore::Event{EV_STATS_TIMEOUT}, &FlightStatsRecorder::state_idle)); } -} \ No newline at end of file +} diff --git a/src/tests/catch/fsm/test-fmm.cpp b/src/tests/catch/fsm/test-fmm.cpp index 0b65370bb..dd01595b7 100644 --- a/src/tests/catch/fsm/test-fmm.cpp +++ b/src/tests/catch/fsm/test-fmm.cpp @@ -51,6 +51,8 @@ public: FMMFixture() { sEventBroker.start(); + // cppcheck-suppress noCopyConstructor + // cppcheck-suppress noOperatorEq fsm = new FMMController(); fsm->start(); } @@ -77,7 +79,7 @@ TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_onGround") // I needed one hour of debugging to figure it out :') /* REQUIRE(testHSMTransition( - *fsm, Event{EV_TC_RESET_BOARD}, + *fsm, Boardcore::Event{EV_TC_RESET_BOARD}, &FMMController::state_init)); // initial state of // state_onGround */ @@ -86,7 +88,7 @@ TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_onGround") SECTION("EV_TC_LAUNCH -> FLYING (ASCENDING)") { REQUIRE(testHSMTransition( - *fsm, Event{EV_TC_LAUNCH}, + *fsm, Boardcore::Event{EV_TC_LAUNCH}, &FMMController::state_ascending)); // initial state of // state_flying } @@ -95,28 +97,29 @@ TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_onGround") TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_flying") { // move to state_flying (state_ascending) - fsm->postEvent(Event{EV_INIT_OK}); + fsm->postEvent(Boardcore::Event{EV_INIT_OK}); Thread::sleep(50); - fsm->postEvent(Event{EV_TC_CALIBRATE_SENSORS}); + fsm->postEvent(Boardcore::Event{EV_TC_CALIBRATE_SENSORS}); Thread::sleep(50); - fsm->postEvent(Event{EV_SENSORS_READY}); + fsm->postEvent(Boardcore::Event{EV_SENSORS_READY}); Thread::sleep(50); - fsm->postEvent(Event{EV_CALIBRATION_OK}); + fsm->postEvent(Boardcore::Event{EV_CALIBRATION_OK}); Thread::sleep(50); - fsm->postEvent(Event{EV_TC_ARM}); + fsm->postEvent(Boardcore::Event{EV_TC_ARM}); Thread::sleep(50); - fsm->postEvent(Event{EV_UMBILICAL_DETACHED}); + fsm->postEvent(Boardcore::Event{EV_UMBILICAL_DETACHED}); Thread::sleep(100); SECTION("EV_TC_END_MISSION -> LANDED") { - REQUIRE(testHSMTransition(*fsm, Event{EV_TC_END_MISSION}, + REQUIRE(testHSMTransition(*fsm, Boardcore::Event{EV_TC_END_MISSION}, &FMMController::state_landed)); } SECTION("EV_TIMEOUT_END_MISSION -> LANDED") { - REQUIRE(testHSMTransition(*fsm, Event{EV_TIMEOUT_END_MISSION}, + REQUIRE(testHSMTransition(*fsm, + Boardcore::Event{EV_TIMEOUT_END_MISSION}, &FMMController::state_landed)); } } @@ -125,13 +128,13 @@ TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_init") { SECTION("EV_INIT_OK -> INIT_DONE") { - REQUIRE(testHSMTransition(*fsm, Event{EV_INIT_OK}, + REQUIRE(testHSMTransition(*fsm, Boardcore::Event{EV_INIT_OK}, &FMMController::state_initDone)); } SECTION("EV_INIT_ERROR -> INIT_ERROR") { - REQUIRE(testHSMTransition(*fsm, Event{EV_INIT_ERROR}, + REQUIRE(testHSMTransition(*fsm, Boardcore::Event{EV_INIT_ERROR}, &FMMController::state_initError)); } } @@ -139,12 +142,12 @@ TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_init") TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_initError") { // move to state_initError - fsm->postEvent(Event{EV_INIT_ERROR}); + fsm->postEvent(Boardcore::Event{EV_INIT_ERROR}); Thread::sleep(50); SECTION("EV_TC_FORCE_INIT -> INIT_DONE") { - REQUIRE(testHSMTransition(*fsm, Event{EV_TC_FORCE_INIT}, + REQUIRE(testHSMTransition(*fsm, Boardcore::Event{EV_TC_FORCE_INIT}, &FMMController::state_initDone)); } } @@ -152,18 +155,19 @@ TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_initError") TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_initDone") { // move to state_initDone - fsm->postEvent(Event{EV_INIT_OK}); + fsm->postEvent(Boardcore::Event{EV_INIT_OK}); Thread::sleep(50); SECTION("EV_TC_TEST_MODE -> TEST_MODE") { - REQUIRE(testHSMTransition(*fsm, Event{EV_TC_TEST_MODE}, + REQUIRE(testHSMTransition(*fsm, Boardcore::Event{EV_TC_TEST_MODE}, &FMMController::state_testMode)); } SECTION("EV_TC_CALIBRATE_SENSORS -> SENSORS_CALIBRATION") { - REQUIRE(testHSMTransition(*fsm, Event{EV_TC_CALIBRATE_SENSORS}, + REQUIRE(testHSMTransition(*fsm, + Boardcore::Event{EV_TC_CALIBRATE_SENSORS}, &FMMController::state_sensorsCalibration)); } } @@ -179,20 +183,21 @@ TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_sensorsCalibration") { // move to state_calibrating - fsm->postEvent(Event{EV_INIT_OK}); + fsm->postEvent(Boardcore::Event{EV_INIT_OK}); Thread::sleep(50); - fsm->postEvent(Event{EV_TC_CALIBRATE_SENSORS}); + fsm->postEvent(Boardcore::Event{EV_TC_CALIBRATE_SENSORS}); Thread::sleep(100); SECTION("EV_TC_CALIBRATE_SENSORS -> SENSORS_CALIBRATION") { - REQUIRE(testHSMTransition(*fsm, Event{EV_TC_CALIBRATE_SENSORS}, + REQUIRE(testHSMTransition(*fsm, + Boardcore::Event{EV_TC_CALIBRATE_SENSORS}, &FMMController::state_sensorsCalibration)); } SECTION("EV_SENSORS_READY -> ALGOS_CALIBRATION") { - REQUIRE(testHSMTransition(*fsm, Event{EV_SENSORS_READY}, + REQUIRE(testHSMTransition(*fsm, Boardcore::Event{EV_SENSORS_READY}, &FMMController::state_algosCalibration)); } } @@ -200,22 +205,22 @@ TEST_CASE_METHOD(FMMFixture, TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_algosCalibration") { // move to state_calibrating - fsm->postEvent(Event{EV_INIT_OK}); + fsm->postEvent(Boardcore::Event{EV_INIT_OK}); Thread::sleep(50); - fsm->postEvent(Event{EV_TC_CALIBRATE_SENSORS}); + fsm->postEvent(Boardcore::Event{EV_TC_CALIBRATE_SENSORS}); Thread::sleep(50); - fsm->postEvent(Event{EV_SENSORS_READY}); + fsm->postEvent(Boardcore::Event{EV_SENSORS_READY}); Thread::sleep(100); SECTION("EV_TC_CALIBRATE_ALGOS -> ALGOS_CALIBRATION") { - REQUIRE(testHSMTransition(*fsm, Event{EV_TC_CALIBRATE_ALGOS}, + REQUIRE(testHSMTransition(*fsm, Boardcore::Event{EV_TC_CALIBRATE_ALGOS}, &FMMController::state_algosCalibration)); } SECTION("EV_CALIBRATION_OK -> DISARMED") { - REQUIRE(testHSMTransition(*fsm, Event{EV_CALIBRATION_OK}, + REQUIRE(testHSMTransition(*fsm, Boardcore::Event{EV_CALIBRATION_OK}, &FMMController::state_disarmed)); } } @@ -223,30 +228,31 @@ TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_algosCalibration") TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_disarmed") { // move to state_disarmed - fsm->postEvent(Event{EV_INIT_OK}); + fsm->postEvent(Boardcore::Event{EV_INIT_OK}); Thread::sleep(50); - fsm->postEvent(Event{EV_TC_CALIBRATE_SENSORS}); + fsm->postEvent(Boardcore::Event{EV_TC_CALIBRATE_SENSORS}); Thread::sleep(50); - fsm->postEvent(Event{EV_SENSORS_READY}); + fsm->postEvent(Boardcore::Event{EV_SENSORS_READY}); Thread::sleep(50); - fsm->postEvent(Event{EV_CALIBRATION_OK}); + fsm->postEvent(Boardcore::Event{EV_CALIBRATION_OK}); Thread::sleep(100); SECTION("EV_TC_CALIBRATE_ALGOS -> ALGOS_CALIBRATION") { - REQUIRE(testHSMTransition(*fsm, Event{EV_TC_CALIBRATE_ALGOS}, + REQUIRE(testHSMTransition(*fsm, Boardcore::Event{EV_TC_CALIBRATE_ALGOS}, &FMMController::state_algosCalibration)); } SECTION("EV_TC_CALIBRATE_SENSORS -> SENSORS_CALIBRATION") { - REQUIRE(testHSMTransition(*fsm, Event{EV_TC_CALIBRATE_SENSORS}, + REQUIRE(testHSMTransition(*fsm, + Boardcore::Event{EV_TC_CALIBRATE_SENSORS}, &FMMController::state_sensorsCalibration)); } SECTION("EV_TC_ARM -> ARMED") { - REQUIRE(testHSMTransition(*fsm, Event{EV_TC_ARM}, + REQUIRE(testHSMTransition(*fsm, Boardcore::Event{EV_TC_ARM}, &FMMController::state_armed)); } } @@ -254,32 +260,32 @@ TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_disarmed") TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_armed") { // move to state_armed - fsm->postEvent(Event{EV_INIT_OK}); + fsm->postEvent(Boardcore::Event{EV_INIT_OK}); Thread::sleep(50); - fsm->postEvent(Event{EV_TC_CALIBRATE_SENSORS}); + fsm->postEvent(Boardcore::Event{EV_TC_CALIBRATE_SENSORS}); Thread::sleep(50); - fsm->postEvent(Event{EV_SENSORS_READY}); + fsm->postEvent(Boardcore::Event{EV_SENSORS_READY}); Thread::sleep(50); - fsm->postEvent(Event{EV_CALIBRATION_OK}); + fsm->postEvent(Boardcore::Event{EV_CALIBRATION_OK}); Thread::sleep(50); - fsm->postEvent(Event{EV_TC_ARM}); + fsm->postEvent(Boardcore::Event{EV_TC_ARM}); Thread::sleep(100); SECTION("EV_TC_DISARM -> DISARMED") { - REQUIRE(testHSMTransition(*fsm, Event{EV_TC_DISARM}, + REQUIRE(testHSMTransition(*fsm, Boardcore::Event{EV_TC_DISARM}, &FMMController::state_disarmed)); } SECTION("EV_TC_LAUNCH -> ASCENDING") { - REQUIRE(testHSMTransition(*fsm, Event{EV_TC_LAUNCH}, + REQUIRE(testHSMTransition(*fsm, Boardcore::Event{EV_TC_LAUNCH}, &FMMController::state_ascending)); } SECTION("EV_UMBILICAL_DETACHED -> ASCENDING") { - REQUIRE(testHSMTransition(*fsm, Event{EV_UMBILICAL_DETACHED}, + REQUIRE(testHSMTransition(*fsm, Boardcore::Event{EV_UMBILICAL_DETACHED}, &FMMController::state_ascending)); } } @@ -287,34 +293,35 @@ TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_armed") TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_ascending") { // move to state_ascending - fsm->postEvent(Event{EV_INIT_OK}); + fsm->postEvent(Boardcore::Event{EV_INIT_OK}); Thread::sleep(50); - fsm->postEvent(Event{EV_TC_CALIBRATE_SENSORS}); + fsm->postEvent(Boardcore::Event{EV_TC_CALIBRATE_SENSORS}); Thread::sleep(50); - fsm->postEvent(Event{EV_SENSORS_READY}); + fsm->postEvent(Boardcore::Event{EV_SENSORS_READY}); Thread::sleep(50); - fsm->postEvent(Event{EV_CALIBRATION_OK}); + fsm->postEvent(Boardcore::Event{EV_CALIBRATION_OK}); Thread::sleep(50); - fsm->postEvent(Event{EV_TC_ARM}); + fsm->postEvent(Boardcore::Event{EV_TC_ARM}); Thread::sleep(50); - fsm->postEvent(Event{EV_UMBILICAL_DETACHED}); + fsm->postEvent(Boardcore::Event{EV_UMBILICAL_DETACHED}); Thread::sleep(100); SECTION("EV_ADA_APOGEE_DETECTED -> DROGUE_DESCENT") { - REQUIRE(testHSMTransition(*fsm, Event{EV_ADA_APOGEE_DETECTED}, + REQUIRE(testHSMTransition(*fsm, + Boardcore::Event{EV_ADA_APOGEE_DETECTED}, &FMMController::state_drogueDescent)); } SECTION("EV_ADA_DISABLE_ABK -> ASCENDING") { - REQUIRE(testHSMTransition(*fsm, Event{EV_ADA_DISABLE_ABK}, + REQUIRE(testHSMTransition(*fsm, Boardcore::Event{EV_ADA_DISABLE_ABK}, &FMMController::state_ascending)); } SECTION("EV_TC_NC_OPEN -> DROGUE_DESCENT") { - REQUIRE(testHSMTransition(*fsm, Event{EV_TC_NC_OPEN}, + REQUIRE(testHSMTransition(*fsm, Boardcore::Event{EV_TC_NC_OPEN}, &FMMController::state_drogueDescent)); } } @@ -322,30 +329,31 @@ TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_ascending") TEST_CASE_METHOD(FMMFixture, "Testing transitions from state_drogueDescent") { // move to state_drogueDescent - fsm->postEvent(Event{EV_INIT_OK}); + fsm->postEvent(Boardcore::Event{EV_INIT_OK}); Thread::sleep(50); - fsm->postEvent(Event{EV_TC_CALIBRATE_SENSORS}); + fsm->postEvent(Boardcore::Event{EV_TC_CALIBRATE_SENSORS}); Thread::sleep(50); - fsm->postEvent(Event{EV_SENSORS_READY}); + fsm->postEvent(Boardcore::Event{EV_SENSORS_READY}); Thread::sleep(50); - fsm->postEvent(Event{EV_CALIBRATION_OK}); + fsm->postEvent(Boardcore::Event{EV_CALIBRATION_OK}); Thread::sleep(50); - fsm->postEvent(Event{EV_TC_ARM}); + fsm->postEvent(Boardcore::Event{EV_TC_ARM}); Thread::sleep(50); - fsm->postEvent(Event{EV_UMBILICAL_DETACHED}); + fsm->postEvent(Boardcore::Event{EV_UMBILICAL_DETACHED}); Thread::sleep(50); - fsm->postEvent(Event{EV_ADA_APOGEE_DETECTED}); + fsm->postEvent(Boardcore::Event{EV_ADA_APOGEE_DETECTED}); Thread::sleep(100); SECTION("EV_ADA_DPL_ALT_DETECTED -> TERMINAL_DESCENT") { - REQUIRE(testHSMTransition(*fsm, Event{EV_ADA_DPL_ALT_DETECTED}, + REQUIRE(testHSMTransition(*fsm, + Boardcore::Event{EV_ADA_DPL_ALT_DETECTED}, &FMMController::state_terminalDescent)); } SECTION("EV_TC_CUT_DROGUE -> TERMINAL_DESCENT") { - REQUIRE(testHSMTransition(*fsm, Event{EV_TC_CUT_DROGUE}, + REQUIRE(testHSMTransition(*fsm, Boardcore::Event{EV_TC_CUT_DROGUE}, &FMMController::state_terminalDescent)); } -} \ No newline at end of file +} diff --git a/src/tests/catch/fsm/test-ignition.cpp b/src/tests/catch/fsm/test-ignition.cpp index 19afdf1c2..773072474 100644 --- a/src/tests/catch/fsm/test-ignition.cpp +++ b/src/tests/catch/fsm/test-ignition.cpp @@ -50,8 +50,10 @@ public: IgnitionTestFixture() { can_mgr = new CanManager(CAN1); - can = new CanProxy(can_mgr); - ign = new IgnitionController(can); + // cppcheck-suppress noCopyConstructor + // cppcheck-suppress noOperatorEq + can = new CanProxy(can_mgr); + ign = new IgnitionController(can); } ~IgnitionTestFixture() { @@ -75,7 +77,7 @@ TEST_CASE_METHOD(IgnitionTestFixture, "Testing IDLE transitions") SECTION("IDLE -> END") { - REQUIRE(testFSMTransition(*ign, Event{EV_LIFTOFF}, + REQUIRE(testFSMTransition(*ign, Boardcore::Event{EV_LIFTOFF}, &IgnitionController::stateEnd)); } @@ -163,8 +165,10 @@ public: { sEventBroker.start(); can_mgr = new CanManager(CAN1); - can = new CanProxy(can_mgr); - ign = new IgnitionController(can); + // cppcheck-suppress noCopyConstructor + // cppcheck-suppress noOperatorEq + can = new CanProxy(can_mgr); + ign = new IgnitionController(can); ign->start(); } ~IgnitionTestFixture2() @@ -256,7 +260,7 @@ TEST_CASE_METHOD(IgnitionTestFixture2, "Igntiion: Testing IDLE functions") Thread::sleep(TIMEOUT_IGN_OFFLINE / 2); - REQUIRE(testFSMTransition(*ign, Event{EV_LIFTOFF}, + REQUIRE(testFSMTransition(*ign, Boardcore::Event{EV_LIFTOFF}, &IgnitionController::stateEnd)); Thread::sleep(TIMEOUT_IGN_OFFLINE / 2 + 5); diff --git a/src/tests/catch/fsm/test-nas.cpp b/src/tests/catch/fsm/test-nas.cpp index 5aea6b14c..6dc8de709 100644 --- a/src/tests/catch/fsm/test-nas.cpp +++ b/src/tests/catch/fsm/test-nas.cpp @@ -38,6 +38,7 @@ #include "events/Events.h" #include "utils/testutils/TestHelper.h" +using namespace miosix; using namespace DeathStackBoard; using NASCtrl = NASController<MockIMUData, MockPressureData, MockGPSData>; @@ -74,7 +75,8 @@ TEST_CASE_METHOD(NASControllerFixture, "Testing transitions from idle") SECTION("EV_CALIBRATE_NAS -> CALIBRATING") { - REQUIRE(testFSMTransition(controller, Event{EV_CALIBRATE_NAS}, + REQUIRE(testFSMTransition(controller, + Boardcore::Event{EV_CALIBRATE_NAS}, &NASCtrl::state_calibrating)); } } @@ -85,13 +87,14 @@ TEST_CASE_METHOD(NASControllerFixture, "Testing transitions from calibrating") SECTION("EV_CALIBRATE_NAS -> CALIBRATING") { - REQUIRE(testFSMTransition(controller, Event{EV_CALIBRATE_NAS}, + REQUIRE(testFSMTransition(controller, + Boardcore::Event{EV_CALIBRATE_NAS}, &NASCtrl::state_calibrating)); } SECTION("EV_NAS_READY -> READY") { - REQUIRE(testFSMTransition(controller, Event{EV_NAS_READY}, + REQUIRE(testFSMTransition(controller, Boardcore::Event{EV_NAS_READY}, &NASCtrl::state_ready)); } } @@ -102,13 +105,14 @@ TEST_CASE_METHOD(NASControllerFixture, "Testing transitions from ready") SECTION("EV_LIFTOFF -> ACTIVE") { - REQUIRE(testFSMTransition(controller, Event{EV_LIFTOFF}, + REQUIRE(testFSMTransition(controller, Boardcore::Event{EV_LIFTOFF}, &NASCtrl::state_active)); } SECTION("EV_CALIBRATE_NAS -> CALIBRATING") { - REQUIRE(testFSMTransition(controller, Event{EV_CALIBRATE_NAS}, + REQUIRE(testFSMTransition(controller, + Boardcore::Event{EV_CALIBRATE_NAS}, &NASCtrl::state_calibrating)); } } @@ -119,7 +123,7 @@ TEST_CASE_METHOD(NASControllerFixture, "Testing transitions from active") SECTION("EV_LANDED -> END") { - REQUIRE(testFSMTransition(controller, Event{EV_LANDED}, + REQUIRE(testFSMTransition(controller, Boardcore::Event{EV_LANDED}, &NASCtrl::state_end)); } } @@ -127,4 +131,4 @@ TEST_CASE_METHOD(NASControllerFixture, "Testing transitions from active") TEST_CASE_METHOD(NASControllerFixture, "Testing transitions from end") { controller.transition(&NASCtrl::state_end); -} \ No newline at end of file +} diff --git a/src/tests/catch/fsm/test-tmtc.cpp b/src/tests/catch/fsm/test-tmtc.cpp index 8d6943553..4eb59bd86 100644 --- a/src/tests/catch/fsm/test-tmtc.cpp +++ b/src/tests/catch/fsm/test-tmtc.cpp @@ -1,6 +1,5 @@ -/* - * Copyright (c) 2021 Skyward Experimental Rocketry - * Authors: Luca Conterio +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -14,7 +13,7 @@ * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN @@ -51,6 +50,8 @@ public: TMTCFixture() { sEventBroker.start(); + // cppcheck-suppress noCopyConstructor + // cppcheck-suppress noOperatorEq fsm = new TMTCController(); fsm->start(); } @@ -72,25 +73,25 @@ TEST_CASE_METHOD(TMTCFixture, "Testing transitions from stateGroundTM") { SECTION("EV_TC_START_SENSOR_TM -> stateSensorTM") { - REQUIRE(testFSMTransition(*fsm, Event{EV_TC_START_SENSOR_TM}, + REQUIRE(testFSMTransition(*fsm, Boardcore::Event{EV_TC_START_SENSOR_TM}, &TMTCController::stateSensorTM)); } SECTION("EV_ARMED -> stateSensorTM") { - REQUIRE(testFSMTransition(*fsm, Event{EV_ARMED}, + REQUIRE(testFSMTransition(*fsm, Boardcore::Event{EV_ARMED}, &TMTCController::stateFlightTM)); } SECTION("EV_LIFTOFF -> stateFlightTM") { - REQUIRE(testFSMTransition(*fsm, Event{EV_LIFTOFF}, + REQUIRE(testFSMTransition(*fsm, Boardcore::Event{EV_LIFTOFF}, &TMTCController::stateFlightTM)); } SECTION("EV_TC_SERIAL_TM -> stateSerialDebugTM") { - REQUIRE(testFSMTransition(*fsm, Event{EV_TC_SERIAL_TM}, + REQUIRE(testFSMTransition(*fsm, Boardcore::Event{EV_TC_SERIAL_TM}, &TMTCController::stateSerialDebugTM)); } } @@ -102,7 +103,7 @@ TEST_CASE_METHOD(TMTCFixture, "Testing transitions from stateSensorTM") SECTION("EV_TC_STOP_SENSOR_TM -> stateGroundTM") { - REQUIRE(testFSMTransition(*fsm, Event{EV_TC_STOP_SENSOR_TM}, + REQUIRE(testFSMTransition(*fsm, Boardcore::Event{EV_TC_STOP_SENSOR_TM}, &TMTCController::stateGroundTM)); } } @@ -114,7 +115,7 @@ TEST_CASE_METHOD(TMTCFixture, "Testing transitions from stateFlightTM") SECTION("EV_DISARMED -> stateGroundTM") { - REQUIRE(testFSMTransition(*fsm, Event{EV_DISARMED}, + REQUIRE(testFSMTransition(*fsm, Boardcore::Event{EV_DISARMED}, &TMTCController::stateGroundTM)); } } @@ -126,7 +127,7 @@ TEST_CASE_METHOD(TMTCFixture, "Testing transitions from stateSerialDebugTM") SECTION("EV_TC_RADIO_TM -> stateSerialDebugTM") { - REQUIRE(testFSMTransition(*fsm, Event{EV_TC_RADIO_TM}, + REQUIRE(testFSMTransition(*fsm, Boardcore::Event{EV_TC_RADIO_TM}, &TMTCController::stateGroundTM)); } -} \ No newline at end of file +} diff --git a/src/tests/catch/nas/test-nas-simulation.cpp b/src/tests/catch/nas/test-nas-simulation.cpp index 75fced107..a4e16ce4e 100644 --- a/src/tests/catch/nas/test-nas-simulation.cpp +++ b/src/tests/catch/nas/test-nas-simulation.cpp @@ -37,6 +37,7 @@ #include "events/Events.h" #include "utils/testutils/TestHelper.h" +using namespace Boardcore; using namespace DeathStackBoard; using namespace NASConfigs; diff --git a/src/tests/deathstack-boards/test-analog-board.cpp b/src/tests/deathstack-boards/test-analog-board.cpp index bf05c3c92..93aa30596 100644 --- a/src/tests/deathstack-boards/test-analog-board.cpp +++ b/src/tests/deathstack-boards/test-analog-board.cpp @@ -49,6 +49,8 @@ #include "PinHandler/PinHandler.h" +using namespace Boardcore; + namespace PinHandlerTest { #include "../test-pinhandler.cpp" diff --git a/src/tests/deathstack-boards/test-power-board.cpp b/src/tests/deathstack-boards/test-power-board.cpp index 72d942bbd..bfd28f951 100644 --- a/src/tests/deathstack-boards/test-power-board.cpp +++ b/src/tests/deathstack-boards/test-power-board.cpp @@ -40,6 +40,8 @@ #include <sstream> #include <string> +using namespace Boardcore; + namespace CutterTest { #include "../drivers/test-cutter.cpp" diff --git a/src/tests/drivers/test-cutter.cpp b/src/tests/drivers/test-cutter.cpp index e7eed18de..ed7baa1f2 100644 --- a/src/tests/drivers/test-cutter.cpp +++ b/src/tests/drivers/test-cutter.cpp @@ -23,6 +23,7 @@ #include <configs/CutterConfig.h> #include <drivers/adc/InternalADC.h> #include <drivers/hbridge/HBridge.h> +#include <drivers/timer/TimerUtils.h> #include <sensors/analog/current/CurrentSensor.h> #include <iostream> @@ -40,8 +41,8 @@ using namespace std; using namespace miosix; -using namespace Boardcore; using namespace DeathStackBoard::CutterConfig; +using namespace Boardcore; static constexpr int MAX_CUTTING_TIME = 10 * 1000; // ms constexpr int SAMPLING_FREQUENCY = 20; @@ -53,9 +54,9 @@ constexpr int SAMPLING_FREQUENCY = 20; */ static TIM_TypeDef *const CUTTER_TIM = TIM9; -static const TimerUtils::Channel CUTTER_CHANNEL_PRIMARY = +const TimerUtils::Channel CUTTER_CHANNEL_PRIMARY = TimerUtils::Channel::CHANNEL_2; -static const TimerUtils::Channel CUTTER_CHANNEL_BACKUP = +const TimerUtils::Channel CUTTER_CHANNEL_BACKUP = TimerUtils::Channel::CHANNEL_2; static const unsigned int PRIMARY_CUTTER_PWM_FREQUENCY = 10000; // Hz static constexpr float PRIMARY_CUTTER_PWM_DUTY_CYCLE = 0.45f; @@ -106,7 +107,6 @@ int main() // Cutter setup - GpioPin *ena_pin; TimerUtils::Channel pwmChannel; if (cutterNo == 3) // COTS cutters @@ -122,7 +122,10 @@ int main() CuttersInput::getPin().low(); } else - { // SRAD cutters + { + // SRAD cutters + GpioPin *ena_pin; + if (cutterNo == 1) { ena_pin = new GpioPin(PrimaryCutterEna::getPin()); @@ -145,7 +148,6 @@ int main() cutter.enable(); // Wait for the user to press ENTER or the timer to elapse - string temp; while (!finished) { (void)getchar(); @@ -192,6 +194,7 @@ void menu(unsigned int *cutterNo, uint32_t *frequency, float *dutyCycle, stringstream(temp) >> *dutyCycle; } while (*dutyCycle < 1 || *dutyCycle > 100); + // cppcheck-suppress invalidPrintfArgType_uint printf("\nCutting %s, frequency: %lu, duty cycle: %.1f\n\n", (*cutterNo ? "PRIMARY" : "BACKUP"), *frequency, *dutyCycle); } diff --git a/src/tests/drivers/test-mavlink.cpp b/src/tests/drivers/test-mavlink.cpp index 8f5aecab8..2da9eaa8c 100644 --- a/src/tests/drivers/test-mavlink.cpp +++ b/src/tests/drivers/test-mavlink.cpp @@ -62,7 +62,7 @@ int main() SPIBus xbee_bus(SPI2); device = new Xbee::Xbee(xbee_bus, XbeeCS::getPin(), XbeeATTN::getPin(), - XbeeRST::getPin()); + XbeeRST::getPin()); channel = new MavChannel(device, &onReceive, 250); device->start(); diff --git a/src/tests/drivers/test-servo.cpp b/src/tests/drivers/test-servo.cpp index 7df7efba4..caaaf35c5 100644 --- a/src/tests/drivers/test-servo.cpp +++ b/src/tests/drivers/test-servo.cpp @@ -32,6 +32,8 @@ using namespace std; using namespace Boardcore; using namespace DeathStackBoard; +using namespace DeploymentConfigs; +using namespace AirBrakesConfigs; int servoMenu(); int actionMenu(); @@ -342,4 +344,4 @@ float perlin2d(float x, float y, float freq, int depth) } return fin / div; -} \ No newline at end of file +} diff --git a/src/tests/eigen-test.cpp b/src/tests/eigen-test.cpp index 52cedb086..886553600 100644 --- a/src/tests/eigen-test.cpp +++ b/src/tests/eigen-test.cpp @@ -34,7 +34,7 @@ using namespace Eigen; using namespace miosix; using miosix::Thread; -void nProducts2Mat(int n, MatrixXd& m1, MatrixXd& m2) +void nProducts2Mat(int n, MatrixXd& const m1, MatrixXd& const m2) { HardwareTimer<uint32_t, 2>& hrclock = HardwareTimer<uint32_t, 2>::instance(); @@ -54,7 +54,8 @@ void nProducts2Mat(int n, MatrixXd& m1, MatrixXd& m2) TRACE("\nTime for %d products using 2 matrices: %f [ms] \n\n", n, time); } -void nProducts3Mat(int n, MatrixXd& m1, MatrixXd& m2, MatrixXd& m3) +void nProducts3Mat(int n, MatrixXd& const m1, MatrixXd& const m2, + MatrixXd& const m3) { HardwareTimer<uint32_t, 2>& hrclock = HardwareTimer<uint32_t, 2>::instance(); @@ -74,30 +75,32 @@ void nProducts3Mat(int n, MatrixXd& m1, MatrixXd& m2, MatrixXd& m3) TRACE("\nTime for %d products using 3 matrices: %f [ms] \n\n", n, time); } -void kalmanOperations(MatrixXd& m1, MatrixXd& m2, MatrixXd& m3, MatrixXd& m4, - MatrixXd& m5, MatrixXd& eye, MatrixXd& v1, MatrixXd& v2) +void kalmanOperations(MatrixXd& const m1, MatrixXd& const m2, + MatrixXd& const m3, MatrixXd& const m4, + MatrixXd& const m5, MatrixXd& const eye, + MatrixXd& const v1, MatrixXd& const v2) { HardwareTimer<uint32_t, 2>& hrclock = HardwareTimer<uint32_t, 2>::instance(); hrclock.setPrescaler(127); hrclock.start(); - auto x = v1; - auto P = m2; - auto Q = m3; - auto R = m5; - auto y = v2; + // auto x = v1; + // auto P = m2; + // auto Q = m3; + // auto R = m5; + // auto y = v2; uint32_t t1 = hrclock.tick(); - auto F = 0.5 * m1; - x = F * x; - P = F * P * (F.transpose()) + Q; - auto H = 2 * m4; - auto K = P * H.transpose() * ((H * P * H.transpose() + R).inverse()); - auto U = K * (y.transpose() - H * x); - auto x_new = x + U; - auto P_new = (eye - K * H) * P; + // auto F = 0.5 * m1; + // x = F * x; + // P = F * P * (F.transpose()) + Q; + // auto H = 2 * m4; + // auto K = P * H.transpose() * ((H * P * H.transpose() + R).inverse()); + // auto U = K * (y.transpose() - H * x); + // auto x_new = x + U; + // auto P_new = (eye - K * H) * P; uint32_t t2 = hrclock.tick(); double time = hrclock.toMilliSeconds(t2 - t1); @@ -106,9 +109,10 @@ void kalmanOperations(MatrixXd& m1, MatrixXd& m2, MatrixXd& m3, MatrixXd& m4, TRACE("\nTime for a single kalman cycle: %f [ms] \n\n", time); } -void sparseKalmanOperations(MatrixXd& m1, MatrixXd& m2, MatrixXd& m3, - MatrixXd& m4, MatrixXd& m5, MatrixXd& eye, - MatrixXd& v1, MatrixXd& v2) +void sparseKalmanOperations(MatrixXd& const m1, MatrixXd& const m2, + MatrixXd& const m3, MatrixXd& const m4, + MatrixXd& const m5, MatrixXd& const eye, + MatrixXd& const v1, MatrixXd& const v2) { // H, P and R can't be sparse since their product needs to be inverted. HardwareTimer<uint32_t, 2>& hrclock = @@ -116,23 +120,23 @@ void sparseKalmanOperations(MatrixXd& m1, MatrixXd& m2, MatrixXd& m3, hrclock.setPrescaler(127); hrclock.start(); - auto x = v1; - auto P = m2; - auto Q = m3.sparseView(); - auto R = m5; - auto y = v2; + // auto x = v1; + // auto P = m2; + // auto Q = m3.sparseView(); + // auto R = m5; + // auto y = v2; uint32_t t1 = hrclock.tick(); - auto F = 0.5 * m1.sparseView(); - x = F * x; - P = F * P * (F.transpose()) + Q; - auto H = 2 * m4; - auto K = (P * (H.transpose()) * ((H * P * H.transpose() + R).inverse())) - .sparseView(); - auto U = (K * (y.transpose() - H * x)).sparseView(); - auto x_new = x + U; - auto P_new = (eye - K * H) * P; + // auto F = 0.5 * m1.sparseView(); + // x = F * x; + // P = F * P * (F.transpose()) + Q; + // auto H = 2 * m4; + // auto K = (P * (H.transpose()) * ((H * P * H.transpose() + R).inverse())) + // .sparseView(); + // auto U = (K * (y.transpose() - H * x)).sparseView(); + // auto x_new = x + U; + // auto P_new = (eye - K * H) * P; uint32_t t2 = hrclock.tick(); double time = hrclock.toMilliSeconds(t2 - t1); @@ -153,7 +157,7 @@ void determinant(MatrixXd& m1) uint32_t t1 = hrclock.tick(); - float det = m1.determinant(); + // float det = m1.determinant(); uint32_t t2 = hrclock.tick(); double time = hrclock.toMilliSeconds(t2 - t1); @@ -185,4 +189,4 @@ int main() sparseKalmanOperations(m1, m2, m3, m4, m5, eye, v1, v2); return 0; -} \ No newline at end of file +} diff --git a/src/tests/hardware_in_the_loop/test-HIL+ADA+AerobrakeController+nas/test-HIL+ADA+AerobrakeController+nas.cpp b/src/tests/hardware_in_the_loop/test-HIL+ADA+AerobrakeController+nas/test-HIL+ADA+AerobrakeController+nas.cpp index 03f914046..0a5a2db3d 100644 --- a/src/tests/hardware_in_the_loop/test-HIL+ADA+AerobrakeController+nas/test-HIL+ADA+AerobrakeController+nas.cpp +++ b/src/tests/hardware_in_the_loop/test-HIL+ADA+AerobrakeController+nas/test-HIL+ADA+AerobrakeController+nas.cpp @@ -220,5 +220,10 @@ int main() Thread::sleep(1000 * ADAConfigs::SAMPLING_PERIOD); } + delete state.imu; + delete state.barometer; + delete state.gps; + delete state.nas; + return 0; -} \ No newline at end of file +} diff --git a/src/tests/ram_test/ramtest.cpp b/src/tests/ram_test/ramtest.cpp index 312252ed9..d43e3002b 100644 --- a/src/tests/ram_test/ramtest.cpp +++ b/src/tests/ram_test/ramtest.cpp @@ -1,19 +1,24 @@ -/*************************************************************************** - * Copyright (C) 2012 by Terraneo Federico * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; either version 2 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, see <http://www.gnu.org/licenses/> * - ***************************************************************************/ +/* Copyright (c) 2012 Skyward Experimental Rocketry + * Author: Federico Terraneo + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ // RAM testing code // Useful to test the external RAM of a board. @@ -96,4 +101,4 @@ int main() return 1; iprintf("Ok\n"); } -} \ No newline at end of file +} diff --git a/src/tests/ram_test/sha1.h b/src/tests/ram_test/sha1.h index 5e8280b57..2fe8cf10a 100644 --- a/src/tests/ram_test/sha1.h +++ b/src/tests/ram_test/sha1.h @@ -108,8 +108,8 @@ private: unsigned Length_Low; // Message length in bits unsigned Length_High; // Message length in bits - unsigned char Message_Block[64]; // 512-bit message blocks - int Message_Block_Index; // Index into message block array + unsigned char Message_Block[64] = {}; // 512-bit message blocks + int Message_Block_Index = 0; // Index into message block array bool Computed; // Is the digest computed? bool Corrupted; // Is the message digest corruped? diff --git a/src/tests/test-ada-dpl-simulation.cpp b/src/tests/test-ada-dpl-simulation.cpp index e3763db59..5e3f8a1f2 100644 --- a/src/tests/test-ada-dpl-simulation.cpp +++ b/src/tests/test-ada-dpl-simulation.cpp @@ -23,6 +23,7 @@ #include <ApogeeDetectionAlgorithm/ADAAlgorithm.h> #include <ApogeeDetectionAlgorithm/ADAController.h> #include <Deployment/DeploymentController.h> +#include <utils/Debug.h> #include <random> @@ -32,6 +33,7 @@ #include "events/Events.h" #include "events/utils/EventCounter.h" +using namespace Boardcore; using namespace DeathStackBoard; constexpr float NOISE_STD_DEV = 5; // Noise varaince @@ -132,7 +134,7 @@ int main() Thread::sleep(100); // Send baro calibration samples for ADA - for (unsigned i = 0; i < CALIBRATION_BARO_N_SAMPLES + 5; i++) + for (unsigned i = 0; i < ADAConfigs::CALIBRATION_BARO_N_SAMPLES + 5; i++) { baro.sample(); Thread::sleep(50); diff --git a/src/tests/test-airbrakes-algorithm/test-airbrakes-algorithm.cpp b/src/tests/test-airbrakes-algorithm/test-airbrakes-algorithm.cpp index 5bc101482..835e6e214 100644 --- a/src/tests/test-airbrakes-algorithm/test-airbrakes-algorithm.cpp +++ b/src/tests/test-airbrakes-algorithm/test-airbrakes-algorithm.cpp @@ -46,7 +46,7 @@ public: int error(int alphaTest) { return abs(alphaTest - lastAlpha); } private: - int lastAlpha; + int lastAlpha = 0; }; template <typename T> @@ -98,9 +98,8 @@ int main() RCC->APB1ENR |= RCC_APB1ENR_TIM5EN; } - input_t input; - - input = DATA[0].input; + // input_t input; + // input = DATA[0].input; HardwareTimer<uint32_t> hrclock( TIM5, @@ -146,4 +145,4 @@ int main() "Init time: %f ms\nAvg iter time: %f ms\nTotal computation time: %f " "ms\n", startTime, stepTime / (LEN_TEST - 1), stepTime + startTime); -} \ No newline at end of file +} diff --git a/src/tests/test-airbrakescontroller/test-airbrakescontroller.cpp b/src/tests/test-airbrakescontroller/test-airbrakescontroller.cpp index 4a4a716d5..62ca048db 100644 --- a/src/tests/test-airbrakescontroller/test-airbrakescontroller.cpp +++ b/src/tests/test-airbrakescontroller/test-airbrakescontroller.cpp @@ -43,7 +43,7 @@ public: int error(int alphaTest) { return abs(alphaTest - lastAlpha); } private: - int lastAlpha; + int lastAlpha = 0; }; template <typename T> @@ -96,10 +96,6 @@ int main() RCC->APB1ENR |= RCC_APB1ENR_TIM5EN; } - input_t input; - - input = DATA[0].input; - HardwareTimer<uint32_t> hrclock( TIM5, TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB1)); @@ -139,4 +135,4 @@ int main() "Init time: %f ms\nAvg iter time: %f ms\nTotal computation time: %f " "ms\n", startTime, stepTime / (LEN_TEST - 1), stepTime + startTime); -} \ No newline at end of file +} diff --git a/src/tests/test-fmm-interactive.cpp b/src/tests/test-fmm-interactive.cpp index 19d488fde..33274ebf7 100644 --- a/src/tests/test-fmm-interactive.cpp +++ b/src/tests/test-fmm-interactive.cpp @@ -15,9 +15,9 @@ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISIN\G - * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS - * IN THE SOFTWARE. + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. */ #include <FlightModeManager/FMMController.h> @@ -61,4 +61,4 @@ int main() } return 0; -} \ No newline at end of file +} diff --git a/src/tests/test-pinhandler.cpp b/src/tests/test-pinhandler.cpp index ee9b069c7..4db092bff 100644 --- a/src/tests/test-pinhandler.cpp +++ b/src/tests/test-pinhandler.cpp @@ -20,7 +20,8 @@ * THE SOFTWARE. */ -#include "PinHandler/PinHandler.h" +#include <PinHandler/PinHandler.h> +#include <miosix.h> using namespace Boardcore; diff --git a/src/tests/test-tmtc.cpp b/src/tests/test-tmtc.cpp index e46f383e4..6a7504000 100644 --- a/src/tests/test-tmtc.cpp +++ b/src/tests/test-tmtc.cpp @@ -43,9 +43,6 @@ int main() tmtc->start(); sEventBroker.start(); - EventSniffer* sniffer = - new EventSniffer(sEventBroker, TOPIC_LIST, onEventReceived); - Thread::sleep(1000); printf("\nOk, press open to post liftoff...\n"); @@ -61,4 +58,4 @@ int main() ; miosix::reboot(); -} \ No newline at end of file +} -- GitLab From e1e1a659201daff73fe48bd1a8f72e3feda67381 Mon Sep 17 00:00:00 2001 From: Matteo Pignataro <matteo.pignataro@skywarder.eu> Date: Wed, 4 May 2022 17:24:49 +0200 Subject: [PATCH 5/6] [Payload] First commit --- skyward-boardcore | 2 +- src/boards/Payload/Main/Sensors.cpp | 65 ++++++------ src/boards/Payload/Main/Sensors.h | 51 ++++++--- src/boards/Payload/PayloadBoard.h | 158 +++++++++++++++------------- 4 files changed, 155 insertions(+), 121 deletions(-) diff --git a/skyward-boardcore b/skyward-boardcore index 715cf492b..e86140bd0 160000 --- a/skyward-boardcore +++ b/skyward-boardcore @@ -1 +1 @@ -Subproject commit 715cf492bb0239f53444ca6475dccbb9f4ac8ec7 +Subproject commit e86140bd051199e971b43a925fa94e374abe9825 diff --git a/src/boards/Payload/Main/Sensors.cpp b/src/boards/Payload/Main/Sensors.cpp index 6460aefdd..a521c3dcc 100644 --- a/src/boards/Payload/Main/Sensors.cpp +++ b/src/boards/Payload/Main/Sensors.cpp @@ -22,7 +22,6 @@ //#include <ApogeeDetectionAlgorithm/ADAController.h> #include <Payload/PayloadBoard.h> -//#include <LoggerService/LoggerService.h> #include <Payload/configs/SensorsConfig.h> #include <drivers/interrupt/external_interrupts.h> #include <drivers/timer/TimestampTimer.h> @@ -44,11 +43,11 @@ void __attribute__((used)) EXTI5_IRQHandlerImpl() { using namespace PayloadBoard; - /*if (Payload::getInstance().sensors->imu_bmx160 != nullptr) + if (Payload::getInstance().sensors->imu_bmx160 != nullptr) { - Payload::getInstance().ensors->imu_bmx160->IRQupdateTimestamp( + Payload::getInstance().sensors->imu_bmx160->IRQupdateTimestamp( TimestampTimer::getInstance().getTimestamp()); - }*/ + } } namespace PayloadBoard @@ -59,7 +58,7 @@ using namespace SensorConfigs; Sensors::Sensors(SPIBusInterface& spi1_bus, TaskScheduler* scheduler) : spi1_bus(spi1_bus) { - // sensors are added to the map ordered by increasing period + // Sensors are added to the map ordered by increasing period ADS1118Init(); magLISinit(); imuBMXInit(); @@ -72,7 +71,11 @@ Sensors::Sensors(SPIBusInterface& spi1_bus, TaskScheduler* scheduler) internalAdcInit(); batteryVoltageInit(); + // Initializing the sensor manager with the main task scheduler sensor_manager = new SensorManager(sensors_map, scheduler); + + // Referencing the SDlogger (already started due to PayloadBoard.h) + SDlogger = &Logger::getInstance(); } Sensors::~Sensors() @@ -102,13 +105,14 @@ bool Sensors::start() bool sm_start_result = sensor_manager->start(); - // if not init ok, set failing sensors in sensors status + // If not init ok, set failing sensors in sensors status if (!sm_start_result) { updateSensorsStatus(); } - // LoggerService::getInstance().log(status); + // Log the status about all the sensors + SDlogger.log(status); return sm_start_result; } @@ -116,12 +120,11 @@ bool Sensors::start() void Sensors::calibrate() { imu_bmx160_with_correction->calibrate(); - // LoggerService::getInstance().log( - // imu_bmx160_with_correction->getGyroscopeBiases()); + SDlogger.log(imu_bmx160_with_correction->getGyroscopeBiases()); press_pitot->calibrate(); - // use the digital barometer as a reference to compute the offset of the + // Use the digital barometer as a reference to compute the offset of the // analog one (static ports sensor) Stats press_digi_stats; for (unsigned int i = 0; i < PRESS_STATIC_CALIB_SAMPLES_NUM / 10; i++) @@ -132,7 +135,7 @@ void Sensors::calibrate() press_static_port->setReferencePressure(press_digi_stats.getStats().mean); press_static_port->calibrate(); - // wait differential and static barometers calibration end + // Wait differential and static barometers calibration end while (press_pitot->isCalibrating() && press_static_port->isCalibrating()) { Thread::sleep(10); @@ -371,7 +374,7 @@ void Sensors::gpsUbloxInit() void Sensors::internalAdcCallback() { - // LoggerService::getInstance().log(internal_adc->getLastSample()); + SDlogger.log(internal_adc->getLastSample()); } void Sensors::batteryVoltageCallback() @@ -388,23 +391,23 @@ void Sensors::batteryVoltageCallback() } } - // LoggerService::getInstance().log(battery_voltage->getLastSample()); + SDlogger.log(battery_voltage->getLastSample()); } void Sensors::pressDigiCallback() { - // LoggerService::getInstance().log(press_digital->getLastSample()); + SDlogger.log(press_digital->getLastSample()); } void Sensors::ADS1118Callback() { - // LoggerService::getInstance().log(adc_ads1118->getLastSample()); + SDlogger.log(adc_ads1118->getLastSample()); } void Sensors::pressPitotCallback() { // SSCDRRN015PDAData d = press_pitot->getLastSample(); - // LoggerService::getInstance().log(d); + // SDlogger.log(d); /*ADAReferenceValues rv = DeathStack::getInstance() @@ -418,49 +421,49 @@ void Sensors::pressPitotCallback() AirSpeedPitot aspeed_data{TimestampTimer::getInstance().getTimestamp(), airspeed}; - //LoggerService::getInstance().log(aspeed_data); + //SDlogger.log(aspeed_data); }*/ } void Sensors::pressDPLVaneCallback() { - // LoggerService::getInstance().log(press_dpl_vane->getLastSample()); + SDlogger.log(press_dpl_vane->getLastSample()); } void Sensors::pressStaticCallback() { - // LoggerService::getInstance().log(press_static_port->getLastSample()); + SDlogger.log(press_static_port->getLastSample()); } void Sensors::imuBMXCallback() { - // uint8_t fifo_size = imu_bmx160->getLastFifoSize(); - // auto& fifo = imu_bmx160->getLastFifo(); + uint8_t fifo_size = imu_bmx160->getLastFifoSize(); + auto& fifo = imu_bmx160->getLastFifo(); - // LoggerService::getInstance().log(imu_bmx160->getTemperature()); + SDlogger.log(imu_bmx160->getTemperature()); - // for (uint8_t i = 0; i < fifo_size; ++i) - // { - // LoggerService::getInstance().log(fifo.at(i)); - // } + for (uint8_t i = 0; i < fifo_size; ++i) + { + SDlogger.log(fifo.at(i)); + } - // LoggerService::getInstance().log(imu_bmx160->getFifoStats()); + SDlogger.log(imu_bmx160->getFifoStats()); } void Sensors::imuBMXWithCorrectionCallback() { - // LoggerService::getInstance().log( - // imu_bmx160_with_correction->getLastSample()); + SDlogger.log( + imu_bmx160_with_correction->getLastSample()); } void Sensors::magLISCallback() { - // LoggerService::getInstance().log(mag_lis3mdl->getLastSample()); + SDlogger.log(mag_lis3mdl->getLastSample()); } void Sensors::gpsUbloxCallback() { - // LoggerService::getInstance().log(gps_ublox->getLastSample()); + SDlogger.log(gps_ublox->getLastSample()); } void Sensors::updateSensorsStatus() diff --git a/src/boards/Payload/Main/Sensors.h b/src/boards/Payload/Main/Sensors.h index 98118dfa7..8ac439fd9 100644 --- a/src/boards/Payload/Main/Sensors.h +++ b/src/boards/Payload/Main/Sensors.h @@ -34,8 +34,8 @@ #include <sensors/SensorData.h> #include <sensors/SensorManager.h> #include <sensors/UbloxGPS/UbloxGPS.h> -#include <sensors/analog/battery/BatteryVoltageSensor.h> -#include <sensors/analog/current/CurrentSensor.h> +#include <sensors/analog/BatteryVoltageSensor.h> +#include <sensors/analog/CurrentSensor.h> #include <sensors/analog/pressure/MPXHZ6130A/MPXHZ6130A.h> #include <sensors/analog/pressure/honeywell/SSCDANN030PAA.h> #include <sensors/analog/pressure/honeywell/SSCDRRN015PDA.h> @@ -52,8 +52,15 @@ namespace PayloadBoard class Sensors { public: + /** + * @brief Sensor manager declaration. It is used to sample all the sensors + * automatically with the help of the task scheduler + */ Boardcore::SensorManager* sensor_manager = nullptr; + /** + * @brief On board sensors + */ Boardcore::InternalADC* internal_adc = nullptr; Boardcore::BatteryVoltageSensor* battery_voltage = nullptr; @@ -69,16 +76,45 @@ public: Boardcore::LIS3MDL* mag_lis3mdl = nullptr; Boardcore::UbloxGPS* gps_ublox = nullptr; + /** + * @brief Construct a new Sensors object + * + * @param spi1_bus_ Bus where all the SPI sensors are connected + * @param scheduler Task scheduler + */ Sensors(Boardcore::SPIBusInterface& spi1_bus_, Boardcore::TaskScheduler* scheduler); + /** + * @brief Destroy the Sensors object + */ ~Sensors(); + /** + * @brief It starts the task scheduler and the sensor manager + * + * @return The starting results + */ bool start(); void calibrate(); private: + Boardcore::SPIBusInterface& spi1_bus; + + Boardcore::SensorManager::SensorMap_t sensors_map; + + /** + * @brief Sum of all the sensors status to be logged + */ + SensorsStatus status; + + Boardcore::PrintLogger log = Boardcore::Logging::getLogger("sensors"); + + Boardcore::Logger* SDlogger; + + unsigned int battery_critical_counter = 0; + void internalAdcInit(); void internalAdcCallback(); @@ -113,16 +149,5 @@ private: void gpsUbloxCallback(); void updateSensorsStatus(); - - Boardcore::SPIBusInterface& spi1_bus; - - Boardcore::SensorManager::SensorMap_t sensors_map; - - SensorsStatus status; - - Boardcore::PrintLogger log = Boardcore::Logging::getLogger("sensors"); - - unsigned int battery_critical_counter = 0; }; - } // namespace PayloadBoard diff --git a/src/boards/Payload/PayloadBoard.h b/src/boards/Payload/PayloadBoard.h index a8e87f1cf..8b49a8a7b 100644 --- a/src/boards/Payload/PayloadBoard.h +++ b/src/boards/Payload/PayloadBoard.h @@ -23,7 +23,6 @@ #pragma once #include <Payload/PayloadStatus.h> -//#include <LoggerService/LoggerService.h> #include <Payload/Main/Actuators.h> #include <Payload/Main/Bus.h> #include <Payload/Main/Radio.h> @@ -36,7 +35,6 @@ #include <events/EventData.h> #include <events/Events.h> #include <events/Topics.h> -#include <events/utils/EventInjector.h> #include <events/utils/EventSniffer.h> #include <functional> @@ -48,7 +46,7 @@ using std::bind; namespace PayloadBoard { -// Add heres the events that you don't want to be TRACEd in +// Add here the events that you don't want to be TRACEd in // Payload.logEvent() static const std::vector<uint8_t> TRACE_EVENT_BLACKLIST{ EV_SEND_HR_TM, EV_SEND_LR_TM, EV_SEND_TEST_TM, EV_SEND_SENS_TM}; @@ -63,8 +61,6 @@ class Payload : public Boardcore::Singleton<Payload> public: // Shared Components Boardcore::EventBroker* broker; - // LoggerService* logger; - Boardcore::EventSniffer* sniffer; // StateMachines* state_machines; @@ -79,41 +75,49 @@ public: void start() { + // Start the task scheduler + if (!scheduler->start()) + { + LOG_ERR(log, "Error starting the main task scheduler"); + } + + // Start the event broker if (!broker->start()) { LOG_ERR(log, "Error starting EventBroker"); status.setError(&PayloadStatus::ev_broker); } + // Start the radio /*if (!radio->start()) { LOG_ERR(log, "Error starting radio module"); status.setError(&PayloadStatus::radio); }*/ + // Start all the sensors sampling if (!sensors->start()) { LOG_ERR(log, "Error starting sensors"); status.setError(&PayloadStatus::sensors); } + // Start all the state machines /*if (!state_machines->start()) { LOG_ERR(log, "Error starting state machines"); status.setError(&PayloadStatus::state_machines); }*/ + // Start all the pin observations if (!pin_handler->start()) { LOG_ERR(log, "Error starting PinObserver"); status.setError(&PayloadStatus::pin_obs); } -#ifdef DEBUG - injector->start(); -#endif - - // logger->log(status); + // Log the result of all the init process + SDlogger->log(status); // If there was an error, signal it to the FMM and light a LED. if (status.payload_board != COMP_OK) @@ -130,41 +134,34 @@ public: } } - /*void startLogger() - { - try - { - logger->start(); - LOG_INFO(log, "Logger started"); - } - catch (const std::runtime_error& re) - { - LOG_ERR(log, "SD Logger init error"); - status.setError(&PayloadStatus::logger); - } +private: - logger->log(logger->getLogger().getLoggerStats()); - }*/ + /** + * @brief Status of all the Payload macro components, such as radio, sensors, FSMs.. + */ + PayloadStatus status{}; + + Boardcore::PrintLogger log = Boardcore::Logging::getLogger("Payload"); + Boardcore::Logger* SDlogger; -private: /** * @brief Initialize Everything. */ Payload() { /* Shared components */ - // logger = Singleton<LoggerService>::getInstance(); - // startLogger(); + SDlogger = Singleton<LoggerService>::getInstance(); + startLogger(); broker = &sEventBroker; // Bind the logEvent function to the event sniffer in order to log every // event - /*{ - using namespace std::placeholders; - sniffer = new EventSniffer( - *broker, TOPIC_LIST, bind(&Payload::logEvent, this, _1, _2)); - }*/ + // { + // using namespace std::placeholders; + // sniffer = new EventSniffer( + // *broker, TOPIC_LIST, bind(&Payload::logEvent, this, _1, _2)); + // } scheduler = new Boardcore::TaskScheduler(); @@ -175,63 +172,72 @@ private: pin_handler = new PinHandler(); -#ifdef DEBUG - injector = new Boardcore::EventInjector(); -#endif - LOG_INFO(log, "Init finished"); } /** - * @brief Helpers for debugging purposes. + * @brief Starts the SD logger and logs its status */ - /*void logEvent(uint8_t event, uint8_t topic) + void startSDLogger() { - EventData ev{(long long)TimestampTimer::getInstance().getTimestamp(), -event, topic}; logger->log(ev); - -#ifdef DEBUG - // Don't TRACE if event is in the blacklist to avoid cluttering the - // serial terminal - for (uint8_t bl_ev : TRACE_EVENT_BLACKLIST) + try { - if (bl_ev == event) - { - return; - } + SDlogger->start(); + LOG_INFO(log, "Logger started"); + } + catch (const std::runtime_error& re) + { + LOG_ERR(log, "SD Logger init error"); + status.setError(&PayloadStatus::logger); } - LOG_DEBUG(log, "{:s} on {:s}", -getEventString(event), getTopicString(topic)); #endif - }*/ + + logger->log(SDlogger->getLogger().getLoggerStats()); + } inline void postEvent(Boardcore::Event ev, uint8_t topic) { broker->post(ev, topic); } - /*void addSchedulerStatsTask() - { - // add lambda to log scheduler tasks statistics - scheduler.add( - [&]() { - std::vector<TaskStatsResult> scheduler_stats = - scheduler.getTaskStats(); - - for (TaskStatsResult stat : scheduler_stats) - { - logger->log(stat); - } - - StackLogger::getInstance().updateStack(THID_TASK_SCHEDULER); - }, - 1000, // 1 hz - TASK_SCHEDULER_STATS_ID, miosix::getTick()); - }*/ - - Boardcore::EventInjector* injector; - PayloadStatus status{}; - - Boardcore::PrintLogger log = Boardcore::Logging::getLogger("Payload"); + /** + * @brief Helpers for debugging purposes. + */ +// void logEvent(uint8_t event, uint8_t topic) +// { +// EventData ev{(long long)TimestampTimer::getInstance().getTimestamp(), +// event, topic}; logger->log(ev); + +// #ifdef DEBUG +// // Don't TRACE if event is in the blacklist to avoid cluttering the +// // serial terminal +// for (uint8_t bl_ev : TRACE_EVENT_BLACKLIST) +// { +// if (bl_ev == event) +// { +// return; +// } +// } +// LOG_DEBUG(log, "{:s} on {:s}", +// getEventString(event), getTopicString(topic)); #endif +// } + + // void addSchedulerStatsTask() + // { + // // add lambda to log scheduler tasks statistics + // scheduler.add( + // [&]() { + // std::vector<TaskStatsResult> scheduler_stats = + // scheduler.getTaskStats(); + + // for (TaskStatsResult stat : scheduler_stats) + // { + // logger->log(stat); + // } + + // StackLogger::getInstance().updateStack(THID_TASK_SCHEDULER); + // }, + // 1000, // 1 hz + // TASK_SCHEDULER_STATS_ID, miosix::getTick()); + // } }; - } // namespace PayloadBoard -- GitLab From c13484cb10e79fe0c7046863a32bde94c183f25b Mon Sep 17 00:00:00 2001 From: Matteo Pignataro <matteo.pignataro@skywarder.eu> Date: Wed, 4 May 2022 17:25:39 +0200 Subject: [PATCH 6/6] [Payload] Boardcore and settings update --- .vscode/c_cpp_properties.json | 6 ++++-- .vscode/settings.json | 5 +++-- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index dbed30c87..cd1bb90f6 100755 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -8,8 +8,8 @@ "defines": [ "DEBUG", "_ARCH_CORTEXM4_STM32F4", - "_BOARD_stm32f429zi_skyward_death_stack", - "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack", + "_BOARD_stm32f429zi_skyward_death_stack_x", + "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack_x", "HSE_VALUE=8000000", "SYSCLK_FREQ_168MHz=168000000", "_MIOSIX", @@ -28,6 +28,7 @@ "${workspaceFolder}/skyward-boardcore/src/shared", "${workspaceFolder}/skyward-boardcore/src/tests", "${workspaceFolder}/src/boards/DeathStack", + "${workspaceFolder}/src/boards/Payload", "${workspaceFolder}/src/boards", "${workspaceFolder}/src/common", "${workspaceFolder}/src" @@ -53,6 +54,7 @@ "${workspaceFolder}/skyward-boardcore/src/shared", "${workspaceFolder}/skyward-boardcore/src/tests", "${workspaceFolder}/src/boards/DeathStack", + "${workspaceFolder}/src/boards/Payload", "${workspaceFolder}/src/boards", "${workspaceFolder}/src/tests", "${workspaceFolder}/src/common", diff --git a/.vscode/settings.json b/.vscode/settings.json index 9855cc51f..beefdfb26 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -92,5 +92,6 @@ "specialfunctions": "cpp", "splines": "cpp", "matrixfunctions": "cpp" - } -} + }, + "C_Cpp.errorSquiggles": "Disabled" +} \ No newline at end of file -- GitLab