diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 159291f0bb2f9f19c0cdbbe7e01c5b695b14fac5..b600d2af98677534acbe0414a25b1022a5d7f720 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -8,7 +8,7 @@ "defines": [ "DEBUG", "_ARCH_CORTEXM4_STM32F4", - "_BOARD_stm32f429zi_stm32f4discovery", + "_BOARD_STM32F429ZI_STM32F4DISCOVERY", "_MIOSIX_BOARDNAME=stm32f429zi_stm32f4discovery", "HSE_VALUE=8000000", "SYSCLK_FREQ_168MHz=168000000", diff --git a/CMakeLists.txt b/CMakeLists.txt index ed560e57e4b1e490afe4781e79a23b95721d933f..2ff5af146481747dfdf70125736764aba2ac10d2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -272,6 +272,9 @@ sbs_target(test-calibration stm32f407vg_stm32f4discovery) add_executable(test-bme280 src/tests/sensors/test-bme280.cpp) sbs_target(test-bme280 stm32f429zi_stm32f4discovery) +add_executable(test-bmp280 src/tests/sensors/test-bmp280.cpp) +sbs_target(test-bmp280 stm32f429zi_stm32f4discovery) + add_executable(test-bmx160 src/tests/sensors/test-bmx160.cpp) sbs_target(test-bmx160 stm32f429zi_skyward_death_stack_x) diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake index 606c04e0e473de590618db29eb6003ac8a9613d1..a37df344373e492d13a75e30d802312f17430004 100644 --- a/cmake/boardcore.cmake +++ b/cmake/boardcore.cmake @@ -43,7 +43,6 @@ foreach(OPT_BOARD ${BOARDS}) ${SBS_BASE}/src/shared/drivers/runcam/Runcam.cpp ${SBS_BASE}/src/shared/drivers/servo/Servo.cpp ${SBS_BASE}/src/shared/drivers/spi/SPITransaction.cpp - ${SBS_BASE}/src/shared/radio/SX1278/SX1278.cpp # Events ${SBS_BASE}/src/shared/events/EventBroker.cpp @@ -60,6 +59,7 @@ foreach(OPT_BOARD ${BOARDS}) ${SBS_BASE}/src/shared/radio/gamma868/Gamma868.cpp ${SBS_BASE}/src/shared/radio/Xbee/APIFrameParser.cpp ${SBS_BASE}/src/shared/radio/Xbee/Xbee.cpp + ${SBS_BASE}/src/shared/radio/SX1278/SX1278.cpp # Scheduler ${SBS_BASE}/src/shared/scheduler/TaskScheduler.cpp @@ -69,6 +69,7 @@ foreach(OPT_BOARD ${BOARDS}) ${SBS_BASE}/src/shared/sensors/ADS131M04/ADS131M04.cpp ${SBS_BASE}/src/shared/sensors/ADS131M04/ADS131M04HighFreq.cpp ${SBS_BASE}/src/shared/sensors/BME280/BME280.cpp + ${SBS_BASE}/src/shared/sensors/BMP280/BMP280.cpp ${SBS_BASE}/src/shared/sensors/BMX160/BMX160.cpp ${SBS_BASE}/src/shared/sensors/BMX160/BMX160WithCorrection.cpp ${SBS_BASE}/src/shared/sensors/calibration/SensorDataExtra.cpp diff --git a/scripts/generators/genutils.py b/scripts/generators/genutils.py index d1f10e41b5a5e1cf7265e98643f548218a0d926e..ea03a91f3a98e8ccb0c94e81b866e5cdd91dee15 100644 --- a/scripts/generators/genutils.py +++ b/scripts/generators/genutils.py @@ -68,6 +68,7 @@ def parse_scxml(file): (topic, event) = camel_case_to_underscores( transition.attrib['event']).split('.') topics.append(topic) + event = topic + "_" + event else: event = camel_case_to_underscores( transition.attrib['event']) diff --git a/src/shared/drivers/servo/Servo.cpp b/src/shared/drivers/servo/Servo.cpp index 3c37bd1fba313b94f36230249d186341b02559d9..ce0f9330209bbd1137bcc5000443a8fcda21618c 100644 --- a/src/shared/drivers/servo/Servo.cpp +++ b/src/shared/drivers/servo/Servo.cpp @@ -51,6 +51,8 @@ void Servo::setPosition(float position) void Servo::setPosition90Deg(float degrees) { setPosition(degrees / 90); } +void Servo::setPosition120Deg(float degrees) { setPosition(degrees / 120); } + void Servo::setPosition180Deg(float degrees) { setPosition(degrees / 180); } void Servo::setPosition360Deg(float degrees) { setPosition(degrees / 360); } diff --git a/src/shared/drivers/servo/Servo.h b/src/shared/drivers/servo/Servo.h index 3d93e7364a606faa6b9546a32cccc61834dceeab..f070f196b1a7a2d2af03534ac90f18f591b6e661 100644 --- a/src/shared/drivers/servo/Servo.h +++ b/src/shared/drivers/servo/Servo.h @@ -57,9 +57,9 @@ public: /** * @brief Prepare the timer and sets the PWM output to the minimum. * - * More specifically, the PWM output is prepared to be equal to minPulse bu + * More specifically, the PWM output is prepared to be equal to minPulse but * it is not enabled! After creating the object the PWM signal is not - * active. This is to ensure + * active. This is to ensure the servo motor doesn't move unexpectedly. * * Note that the timer peripheral's clock is enabled automatically when the * PWM object is created. @@ -93,6 +93,8 @@ public: void setPosition90Deg(float degrees); + void setPosition120Deg(float degrees); + void setPosition180Deg(float degrees); void setPosition360Deg(float degrees); diff --git a/src/shared/drivers/spi/SPI.h b/src/shared/drivers/spi/SPI.h index 2cf55bf809c17090b214a55541b3458e5f514e07..de60c5ed54ae18cfb7156e3a4f31826e6f434ee7 100644 --- a/src/shared/drivers/spi/SPI.h +++ b/src/shared/drivers/spi/SPI.h @@ -340,7 +340,7 @@ inline uint16_t SPI::read16() { return transfer(static_cast<uint16_t>(0)); } inline void SPI::read(uint8_t *data, size_t nBytes) { // Reset the data - for (size_t i = 0; i < nBytes / 2; i++) + for (size_t i = 0; i < nBytes; i++) data[i] = 0; // Read the data diff --git a/src/shared/drivers/stepper/Stepper.h b/src/shared/drivers/stepper/Stepper.h index 80f61e075e5b44f408b03fd1e3d43f9f89bee9f0..fa22b4641be1ce4e4a98c9ce85eac281ea360bce 100644 --- a/src/shared/drivers/stepper/Stepper.h +++ b/src/shared/drivers/stepper/Stepper.h @@ -1,5 +1,5 @@ /* Copyright (c) 2022 Skyward Experimental Rocketry - * Authors: Alberto Nidasio + * Author: Alberto Nidasio * * 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/shared/logger/Logger.cpp b/src/shared/logger/Logger.cpp index 9ba695f576df0c20884a7607b56cce91c1b8c1b9..2709fbcc2f7d3a6e9bf860f180f7036200b4a433 100644 --- a/src/shared/logger/Logger.cpp +++ b/src/shared/logger/Logger.cpp @@ -128,7 +128,12 @@ void Logger::stop() fileNumber = -1; // Reset the fileNumber to an invalid value } -bool Logger::testSDCard() { return ofstream("/sd/test").good(); } +bool Logger::testSDCard() +{ + bool result = ofstream("/sd/test").good(); + std::remove("/sd/test"); + return result; +} int Logger::getCurrentLogNumber() { return fileNumber; } diff --git a/src/shared/radio/MavlinkDriver/MavlinkDriver.h b/src/shared/radio/MavlinkDriver/MavlinkDriver.h index fee9e9b5d0df1b9bb3372b01a2c419b4aa34cd46..9f355c0365769056e174e6557f02e21f69319449 100644 --- a/src/shared/radio/MavlinkDriver/MavlinkDriver.h +++ b/src/shared/radio/MavlinkDriver/MavlinkDriver.h @@ -250,7 +250,8 @@ bool MavlinkDriver<PktLength, OutQueueSize>::enqueueMsg( const mavlink_message_t& msg) { // Convert mavlink message to char array - uint8_t msgtempBuf[PktLength]; + // Use fixed buffer size to avoid overflows + uint8_t msgtempBuf[256]; int msgLen = mavlink_msg_to_send_buffer(msgtempBuf, &msg); // Append message to the queue diff --git a/src/shared/radio/SX1278/SX1278.cpp b/src/shared/radio/SX1278/SX1278.cpp index 6a7f718f422a9a298e225da2f7773b5b76c14062..accd5f31a0b474b5c06d4b4d4a3add72fd79fb2c 100644 --- a/src/shared/radio/SX1278/SX1278.cpp +++ b/src/shared/radio/SX1278/SX1278.cpp @@ -249,14 +249,15 @@ ssize_t SX1278::receive(uint8_t *pkt, size_t max_len) bool SX1278::send(uint8_t *pkt, size_t len) { - // Hacked together larger packets support - while (len > 0) - { - // This shouldn't be needed, but for some reason the device "lies" about - // being ready, so lock up if we are going too fast - rateLimitTx(); + // Packets longer than FIFO_LEN (-1 for the len byte) are not supported + if (len > SX1278Defs::FIFO_LEN - 1) + return false; + + // This shouldn't be needed, but for some reason the device "lies" about + // being ready, so lock up if we are going too fast + rateLimitTx(); - bus_mgr.lock(SX1278BusManager::Mode::MODE_TX); + bus_mgr.lock(SX1278BusManager::Mode::MODE_TX); // Wait for TX ready bus_mgr.waitForIrq(RegIrqFlags::TX_READY); @@ -282,6 +283,11 @@ bool SX1278::send(uint8_t *pkt, size_t len) last_tx = now(); } + // Wait for packet sent + bus_mgr.waitForIrq(RegIrqFlags::PACKET_SENT); + bus_mgr.unlock(); + + last_tx = now(); return true; } diff --git a/src/shared/radio/SX1278/SX1278Defs.h b/src/shared/radio/SX1278/SX1278Defs.h index f38059551c2ca51b04393263044abc0abc004715..06954099644e8a7b6d6b6566e0ad7d2ef1118f3b 100644 --- a/src/shared/radio/SX1278/SX1278Defs.h +++ b/src/shared/radio/SX1278/SX1278Defs.h @@ -34,9 +34,9 @@ namespace SX1278Defs { /** - * @brief Length of the fifo. + * @brief Length of the internal FIFO */ -constexpr size_t FIFO_LEN = 64; +constexpr int FIFO_LEN = 64; /** * @brief Main oscillator frequency (Hz) diff --git a/src/shared/sensors/BME280/BME280.cpp b/src/shared/sensors/BME280/BME280.cpp index 10be66cee23d465072cd20e6d9ec97907a986199..ccc4e90af3aa97187f137a45f28b0e6635ecea39 100644 --- a/src/shared/sensors/BME280/BME280.cpp +++ b/src/shared/sensors/BME280/BME280.cpp @@ -54,16 +54,6 @@ BME280::BME280(SPISlave spiSlave_, BME280Config config_) bool BME280::init() { - // Check if already initialized - if (initialized) - { - LOG_ERR(logger, "Already initialized"); - - lastError = SensorErrors::ALREADY_INIT; - - return false; - } - // Check WHO AM I if (!checkWhoAmI()) { @@ -121,20 +111,19 @@ bool BME280::init() return false; } - initialized = true; return true; } -void BME280::setHumidityOversampling(Oversampling oversampling) +void BME280::setSensorMode(Mode mode) { - config.bits.oversamplingHumidity = oversampling; + config.bits.mode = mode; setConfiguration(); } -void BME280::setSensorMode(Mode mode) +void BME280::setHumidityOversampling(Oversampling oversampling) { - config.bits.mode = mode; + config.bits.oversamplingHumidity = oversampling; setConfiguration(); } @@ -237,12 +226,6 @@ TemperatureData BME280::readTemperature() return lastSample; } -HumidityData BME280::getHumidity() { return lastSample; } - -PressureData BME280::getPressure() { return lastSample; } - -TemperatureData BME280::getTemperature() { return lastSample; } - unsigned int BME280::calculateMaxMeasurementTime(BME280Config config_) { return ceil(1.25 + (2.3 * config_.bits.oversamplingTemperature) + diff --git a/src/shared/sensors/BME280/BME280.h b/src/shared/sensors/BME280/BME280.h index 3dd66cce83dc94db771992ce508560981cdfe1a7..7ab2c6cdd39e4d1973091a4c4c40d24e546369db 100644 --- a/src/shared/sensors/BME280/BME280.h +++ b/src/shared/sensors/BME280/BME280.h @@ -163,12 +163,6 @@ public: */ bool init() override; - /** - * @brief Sets the oversampling for humidity readings, use SKIPPED to - * disable humidity sampling - */ - void setHumidityOversampling(Oversampling oversampling); - /** * @brief Sets the sensor mode * @@ -182,6 +176,12 @@ public: */ void setSensorMode(Mode mode); + /** + * @brief Sets the oversampling for humidity readings, use SKIPPED to + * disable humidity sampling + */ + void setHumidityOversampling(Oversampling oversampling); + /** * @brief Sets the oversampling for pressure readings, use SKIPPED to * disable pressure sampling @@ -220,12 +220,6 @@ public: */ TemperatureData readTemperature(); - HumidityData getHumidity(); - - PressureData getPressure(); - - TemperatureData getTemperature(); - /** * @brief Maximum measurement time formula from datasheet page 51 * @@ -301,8 +295,6 @@ private: BME280Comp compParams; int32_t fineTemperature; // Used in compensation algorithm - bool initialized = false; // Whether the sensor has been initialized - PrintLogger logger = Logging::getLogger("bme280"); }; diff --git a/src/shared/sensors/BME280/BME280Data.h b/src/shared/sensors/BME280/BME280Data.h index 4cfe0e90794942102f34641687ff22763e9d557d..473989e103fe62904a75f1c13a1ee425102f5e4f 100644 --- a/src/shared/sensors/BME280/BME280Data.h +++ b/src/shared/sensors/BME280/BME280Data.h @@ -46,9 +46,8 @@ struct BME280Data : public TemperatureData, static std::string header() { - return "temperatureTimestamp,temp,pressureTimestamp,press,humid_" - "timestamp," - "humid\n"; + return "temperatureTimestamp,temperature,pressureTimestamp,pressure," + "humid_timestamp,humidity\n"; } void print(std::ostream& os) const diff --git a/src/shared/sensors/BMP280/BMP280.cpp b/src/shared/sensors/BMP280/BMP280.cpp new file mode 100644 index 0000000000000000000000000000000000000000..11839de28598dcfc60d049bc5985e4cba3c8f283 --- /dev/null +++ b/src/shared/sensors/BMP280/BMP280.cpp @@ -0,0 +1,309 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * 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 "BMP280.h" + +#include <drivers/timer/TimestampTimer.h> +#include <math.h> + +using namespace std; + +namespace Boardcore +{ + +const BMP280::BMP280Config BMP280::BMP280_DEFAULT_CONFIG = { + 0, 0, SLEEP_MODE, SKIPPED, SKIPPED, 0, FILTER_OFF, STB_TIME_0_5}; + +const BMP280::BMP280Config BMP280::BMP280_CONFIG_ALL_ENABLED = {0, + 0, + NORMAL_MODE, + OVERSAMPLING_16, + OVERSAMPLING_2, + 0, + FILTER_COEFF_16, + STB_TIME_0_5}; + +const BMP280::BMP280Config BMP280::BMP280_CONFIG_TEMP_SINGLE = { + 0, 0, FORCED_MODE, SKIPPED, OVERSAMPLING_1, 0, FILTER_OFF, STB_TIME_0_5}; + +BMP280::BMP280(SPISlave spiSlave_, BMP280Config config_) + : spiSlave(spiSlave_), config(config_) +{ +} + +bool BMP280::init() +{ + // Check WHO AM I + if (!checkWhoAmI()) + { + LOG_ERR(logger, "Invalid WHO AM I"); + + lastError = SensorErrors::INVALID_WHOAMI; + + return false; + } + + loadCompensationParameters(); + + // Read once the temperature to compute fineTemperature + setConfiguration(BMP280_CONFIG_TEMP_SINGLE); + miosix::Thread::sleep( + calculateMaxMeasurementTime(BMP280_CONFIG_TEMP_SINGLE)); + readTemperature(); + + setConfiguration(); + + BMP280Config readBackConfig = readConfiguration(); + + // Check if the configration on the device matches ours + if (config.bytes.ctrlPressureAndTemperature != + readBackConfig.bytes.ctrlPressureAndTemperature || + config.bytes.config != readBackConfig.bytes.config) + { + LOG_ERR(logger, "Device configuration incorrect, setup failed"); + + lastError = SensorErrors::NOT_INIT; + + return false; + } + + return true; +} + +void BMP280::setSensorMode(Mode mode) +{ + config.bits.mode = mode; + + setConfiguration(); +} + +void BMP280::setPressureOversampling(Oversampling oversampling) +{ + config.bits.oversamplingPressure = oversampling; + + setConfiguration(); +} + +void BMP280::setTemperatureOversampling(Oversampling oversampling) +{ + config.bits.oversamplingTemperature = oversampling; + + setConfiguration(); +} + +void BMP280::setFilterCoeff(FilterCoeff filterCoeff) +{ + config.bits.filter = filterCoeff; + + setConfiguration(); +} + +void BMP280::setStandbyTime(StandbyTime standbyTime) +{ + config.bits.standbyTime = standbyTime; + + setConfiguration(); +} + +PressureData BMP280::readPressure() +{ + uint8_t buffer[3]; + int32_t adc_P = 0; + + { + SPITransaction transaction(spiSlave); + + transaction.readRegisters(REG_PRESS_MSB, buffer, 3); + } + + adc_P |= ((uint32_t)buffer[0]) << 12; + adc_P |= ((uint32_t)buffer[1]) << 4; + adc_P |= (buffer[2] >> 4) & 0x0F; + + // Compensate pressure + lastSample.pressureTimestamp = TimestampTimer::getInstance().getTimestamp(); + lastSample.pressure = + (float)compensatePressure(adc_P) / 256; // Convert to Pa + + return lastSample; +} + +TemperatureData BMP280::readTemperature() +{ + uint8_t buffer[3]; + int32_t adcTemperature = 0; + + { + SPITransaction transaction(spiSlave); + + transaction.readRegisters(REG_TEMP_MSB, buffer, 3); + } + + adcTemperature |= ((uint32_t)buffer[0]) << 12; + adcTemperature |= ((uint32_t)buffer[1]) << 4; + adcTemperature |= (buffer[2] >> 4) & 0x0F; + + // Compensate temperature + fineTemperature = computeFineTemperature(adcTemperature); + lastSample.temperatureTimestamp = + TimestampTimer::getInstance().getTimestamp(); + lastSample.temperature = (float)compensateTemperature(fineTemperature) / + 100; // Converto to DegC + + return lastSample; +} + +unsigned int BMP280::calculateMaxMeasurementTime(BMP280Config config_) +{ + // TODO: This folrmula is not present in the BMP280's datasheet, it should + // be checked + return ceil(1.25 + (2.3 * config_.bits.oversamplingTemperature) + + (2.3 * config_.bits.oversamplingPressure + 0.575)); +} + +unsigned int BMP280::getMaxMeasurementTime() +{ + return calculateMaxMeasurementTime(config); +} + +bool BMP280::selfTest() { return checkWhoAmI(); } + +BMP280Data BMP280::sampleImpl() +{ + uint8_t buffer[8]; + int32_t adcTemperature = 0; + int32_t adc_P = 0; + BMP280Data data; + + // TODO: implement selective read! + + // Burst read pressure, temperature and humidity + { + SPITransaction transaction(spiSlave); + + transaction.readRegisters(REG_PRESS_MSB, buffer, 8); + } + + adcTemperature |= ((uint32_t)buffer[3]) << 12; + adcTemperature |= ((uint32_t)buffer[4]) << 4; + adcTemperature |= (buffer[5] >> 4) & 0x0F; + + adc_P |= ((uint32_t)buffer[0]) << 12; + adc_P |= ((uint32_t)buffer[1]) << 4; + adc_P |= (buffer[2] >> 4) & 0x0F; + + // Compensate temperature + fineTemperature = computeFineTemperature(adcTemperature); + data.temperatureTimestamp = TimestampTimer::getInstance().getTimestamp(); + data.temperature = (float)compensateTemperature(fineTemperature) / + 100; // Converto to DegC + + // Compensate pressure + data.pressureTimestamp = TimestampTimer::getInstance().getTimestamp(); + data.pressure = (float)compensatePressure(adc_P) / 256; // Convert to Pa + + return data; +} + +bool BMP280::checkWhoAmI() +{ + SPITransaction transaction(spiSlave); + + uint8_t whoAmIValue = transaction.readRegister(REG_ID); + + return whoAmIValue == REG_ID_VAL; +} + +void BMP280::setConfiguration() { setConfiguration(config); } + +void BMP280::setConfiguration(BMP280Config config_) +{ + SPITransaction transaction(spiSlave); + + transaction.writeRegister(REG_CONFIG & 0x7F, config_.bytes.config); + transaction.writeRegister(REG_CTRL_MEAS & 0x7F, + config_.bytes.ctrlPressureAndTemperature); +} + +BMP280::BMP280Config BMP280::readConfiguration() +{ + BMP280Config tmp; + SPITransaction transaction(spiSlave); + + transaction.readRegisters(REG_STATUS, (uint8_t *)&tmp, 3); + + return tmp; +} + +void BMP280::loadCompensationParameters() +{ + // Read compensation parameters + { + SPITransaction transaction(spiSlave); + + transaction.readRegisters(REG_CALIB_0, (uint8_t *)&compParams, 25); + } +} + +int32_t BMP280::computeFineTemperature(int32_t adcTemperature) +{ + int32_t var1, var2; + var1 = ((((adcTemperature >> 3) - ((int32_t)compParams.bits.dig_T1 << 1))) * + ((int32_t)compParams.bits.dig_T2)) >> + 11; + var2 = (((((adcTemperature >> 4) - ((int32_t)compParams.bits.dig_T1)) * + ((adcTemperature >> 4) - ((int32_t)compParams.bits.dig_T1))) >> + 12) * + ((int32_t)compParams.bits.dig_T3)) >> + 14; + return var1 + var2; +} + +int32_t BMP280::compensateTemperature(int32_t fineTemperature) +{ + return (fineTemperature * 5 + 128) >> 8; +} + +uint32_t BMP280::compensatePressure(int32_t adc_P) +{ + int64_t var1, var2, p; + var1 = ((int64_t)fineTemperature) - 128000; + var2 = var1 * var1 * (int64_t)compParams.bits.dig_P6; + var2 = var2 + ((var1 * (int64_t)compParams.bits.dig_P5) << 17); + var2 = var2 + (((int64_t)compParams.bits.dig_P4) << 35); + var1 = ((var1 * var1 * (int64_t)compParams.bits.dig_P3) >> 8) + + ((var1 * ((int64_t)compParams.bits.dig_P2) << 12)); + var1 = + ((((int64_t)1) << 47) + var1) * ((int64_t)compParams.bits.dig_P1) >> 33; + if (var1 == 0) + { + return 0; // avoid exception caused by division by zero + } + p = 1048576 - adc_P; + p = (((p << 31) - var2) * 3125) / var1; + var1 = (((int64_t)compParams.bits.dig_P9) * (p >> 13) * (p >> 13)) >> 25; + var2 = (((int64_t)compParams.bits.dig_P8) * p) >> 19; + p = ((p + var1 + var2) >> 8) + (((int64_t)compParams.bits.dig_P7) << 4); + return (uint32_t)p; +} + +} // namespace Boardcore diff --git a/src/shared/sensors/BMP280/BMP280.h b/src/shared/sensors/BMP280/BMP280.h new file mode 100644 index 0000000000000000000000000000000000000000..385a40695fc386cc647cd051431198c150f9b835 --- /dev/null +++ b/src/shared/sensors/BMP280/BMP280.h @@ -0,0 +1,273 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * 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 <diagnostic/PrintLogger.h> +#include <drivers/spi/SPIDriver.h> +#include <sensors/Sensor.h> + +#include "BMP280Data.h" + +namespace Boardcore +{ + +class BMP280 : public Sensor<BMP280Data> +{ +public: + enum Oversampling + { + SKIPPED = 0x0, ///< Skipped (output set to 0x8000) + OVERSAMPLING_1 = 0x1, ///< Oversampling x1 + OVERSAMPLING_2 = 0x2, ///< Oversampling x2 + OVERSAMPLING_4 = 0x3, ///< Oversampling x4 + OVERSAMPLING_8 = 0x4, ///< Oversampling x8 + OVERSAMPLING_16 = 0x5, ///< Oversampling x16 + }; + + enum Mode + { + SLEEP_MODE = 0x0, ///< Sleep mode + FORCED_MODE = 0x1, ///< Forced mode + NORMAL_MODE = 0x3 ///< Normal mode + }; + + enum StandbyTime + { + STB_TIME_0_5 = 0x0, ///< 0.5 ms + STB_TIME_62_5 = 0x1, ///< 62.5 ms + STB_TIME_125 = 0x2, ///< 125 ms + STB_TIME_250 = 0x3, ///< 250 ms + STB_TIME_500 = 0x4, ///< 500 ms + STB_TIME_1000 = 0x5, ///< 1000 ms + STB_TIME_20 = 0x6, ///< 20 ms + STB_TIME_40 = 0x7 ///< 40 ms + }; + + enum FilterCoeff + { + FILTER_OFF = 0x0, ///< Filter off + FILTER_COEFF_2 = 0x1, ///< Filter coefficient = 2 + FILTER_COEFF_4 = 0x2, ///< Filter coefficient = 4 + FILTER_COEFF_8 = 0x3, ///< Filter coefficient = 8 + FILTER_COEFF_16 = 0x4 ///< Filter coefficient = 16 + }; + + union BMP280Config + { + struct __attribute__((packed)) BMP280ConfigBits + { + // status + /** + * '1' when the NVM data are being copied to image registers, '0' + * when the copying is done + */ + uint8_t imUpdate : 1; + uint8_t : 2; + /** + * '1' whenever a conversion is running, '0' when the result have + * been transferred to the data registers + */ + uint8_t measuring : 1; + uint8_t : 4; + + Mode mode : 2; ///< Device modes + Oversampling + oversamplingPressure : 3; ///< Oversampling of pressure + Oversampling + oversamplingTemperature : 3; ///< Oversampling of temperature + + // config + uint8_t spi3wEn : 1; ///< Enables 3-wire SPI interface + uint8_t : 1; + FilterCoeff filter : 3; ///< Time constant of the IIR filter + StandbyTime standbyTime : 3; ///< Inactive duration in normal mode + } bits; + + struct + { + uint8_t status; ///< Device status + uint8_t ctrlPressureAndTemperature; ///< Pressure and temperature + ///< options + uint8_t config; ///< Rate, filter and interface options + } bytes; + + uint8_t bytesArray[3]; + }; + + union BMP280Comp + { + struct __attribute__((packed)) + { + uint16_t dig_T1; + int16_t dig_T2; + int16_t dig_T3; + uint16_t dig_P1; + int16_t dig_P2; + int16_t dig_P3; + int16_t dig_P4; + int16_t dig_P5; + int16_t dig_P6; + int16_t dig_P7; + int16_t dig_P8; + int16_t dig_P9; + } bits; + + uint8_t bytesArray[24]; + }; + + static constexpr uint8_t REG_ID_VAL = 0x58; ///< Who am I value + + static const BMP280Config BMP280_DEFAULT_CONFIG; ///< Default register + ///< values + static const BMP280Config BMP280_CONFIG_ALL_ENABLED; ///< Datasheet + ///< values for + ///< indoor + ///< navigation + static const BMP280Config BMP280_CONFIG_TEMP_SINGLE; ///< Temperature + ///< enabled in + ///< forced mode + + explicit BMP280(SPISlave spiSlave_, + BMP280Config config_ = BMP280_CONFIG_ALL_ENABLED); + + /** + * @brief Initialize the device with the specified configuration + */ + bool init() override; + + /** + * @brief Sets the sensor mode + * + * Values: + * - SLEEP_MODE: No measurements are performed + * - FORCED_MODE: A single measurement is performed when this function + * writes the configuration, after the measurement the sensor return to + * sleep mode + * - NORMAL_MODE: Automated cycling between measurements, standby time can + * be set using setStandbyTime() + */ + void setSensorMode(Mode mode); + + /** + * @brief Sets the oversampling for pressure readings, use SKIPPED to + * disable pressure sampling + */ + void setPressureOversampling(Oversampling oversampling); + + /** + * @brief Sets the oversampling for temperature readings, use SKIPPED to + * disable temperature sampling + */ + void setTemperatureOversampling(Oversampling oversampling); + + /** + * @brief Sets the coefficient for the IIR filter (applied to temperature + * and pressure) + */ + void setFilterCoeff(FilterCoeff filterCoeff); + + /** + * @brief Sets the standby time between readings in normal mode + */ + void setStandbyTime(StandbyTime standbyTime); + + /** + * @brief Reads only the pressure, does not set the configuration + */ + PressureData readPressure(); + + /** + * @brief Reads only the temperature, does not set the configuration + */ + TemperatureData readTemperature(); + + /** + * @brief Maximum measurement time formula from datasheet page 51 + * + * @return Time in milliseconds + */ + static unsigned int calculateMaxMeasurementTime(BMP280Config config_); + + unsigned int getMaxMeasurementTime(); + + /** + * @brief Reads the WHO AM I register + * + * @return True if everything ok + */ + bool selfTest() override; + +private: + BMP280Data sampleImpl() override; + + void setConfiguration(); + + void setConfiguration(BMP280Config config_); + + BMP280Config readConfiguration(); + + void loadCompensationParameters(); + + // Compensation algorithm rev.1.1 from Bosh datasheet + + int32_t computeFineTemperature(int32_t adcTemperature); + + int32_t compensateTemperature(int32_t fineTemperature); + + uint32_t compensatePressure(int32_t adcPressure); + /** + * @brief Check the WHO AM I code from the device. + * + * @return true if the device is recognized + */ + bool checkWhoAmI(); + + enum BMP280Registers : uint8_t + { + REG_CALIB_0 = 0x88, + // Calibration register 1-25 + + REG_ID = 0xD0, + REG_RESET = 0xE0, + + REG_STATUS = 0xF3, + REG_CTRL_MEAS = 0xF4, + REG_CONFIG = 0xF5, + + REG_PRESS_MSB = 0xF7, + REG_PRESS_LSB = 0xF8, + REG_PRESS_XLSB = 0xF9, + REG_TEMP_MSB = 0xFA, + REG_TEMP_LSB = 0xFB, + REG_TEMP_XLSB = 0xFC + }; + + const SPISlave spiSlave; + BMP280Config config; + BMP280Comp compParams; + int32_t fineTemperature; // Used in compensation algorithm + + PrintLogger logger = Logging::getLogger("bmp280"); +}; + +} // namespace Boardcore diff --git a/src/shared/sensors/BMP280/BMP280Data.h b/src/shared/sensors/BMP280/BMP280Data.h new file mode 100644 index 0000000000000000000000000000000000000000..cfd3cecc200fb8be00b137bc725a6a62a2776f45 --- /dev/null +++ b/src/shared/sensors/BMP280/BMP280Data.h @@ -0,0 +1,55 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * 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 <sensors/SensorData.h> + +namespace Boardcore +{ + +struct BMP280Data : public TemperatureData, public PressureData +{ + BMP280Data() : TemperatureData{0, 0.0}, PressureData{0, 0.0} {} + + BMP280Data(uint64_t timestamp, float temperature, float pressure, + float humidity) + : TemperatureData{timestamp, temperature}, PressureData{timestamp, + pressure} + + { + } + + static std::string header() + { + return "temperatureTimestamp,temperature,pressureTimestamp,pressure,\n"; + } + + void print(std::ostream& os) const + { + os << temperatureTimestamp << "," << temperature << "," + << pressureTimestamp << "," << pressure << "," + << "\n"; + } +}; + +} // namespace Boardcore diff --git a/src/shared/sensors/UbloxGPS/UbloxGPS.h b/src/shared/sensors/UbloxGPS/UbloxGPS.h index 3f7a1616ef1c0c732527b91ed209964d9af3c893..9495bc6f5fe792774f0bb9a59c32f35c6e97e5b8 100644 --- a/src/shared/sensors/UbloxGPS/UbloxGPS.h +++ b/src/shared/sensors/UbloxGPS/UbloxGPS.h @@ -129,7 +129,7 @@ private: const int defaultBaudrate; // [baud] char gpsFilePath[16]; ///< Allows for a filename of up to 10 characters - int gpsFile; + int gpsFile = -1; mutable miosix::FastMutex mutex; UbloxGPSData threadSample{}; diff --git a/src/shared/utils/collections/SyncPacketQueue.h b/src/shared/utils/collections/SyncPacketQueue.h index ff5e7b360a3f612cf73feae05e0283267d96108f..a2bd35e33855484f33c6b96dae9655492ca77dd5 100644 --- a/src/shared/utils/collections/SyncPacketQueue.h +++ b/src/shared/utils/collections/SyncPacketQueue.h @@ -67,16 +67,15 @@ public: ~Packet() { content.clear(); }; /** - * @brief Try to append a given message to the packet. + * @brief Append a given message to the packet. * * If it's the first message, also set the timestamp. * * @param msg The message to be appended. * @param msgLen Length of msg. - * @return true if the message was appended correctly. - * @return false if there isn't enough space for the message. + * @return How many bytes were actually appended. */ - bool tryAppend(const uint8_t* msg, const size_t msgLen); + size_t append(const uint8_t* msg, size_t msgLen); /** * @brief Mark the packet as ready to be sent. @@ -111,7 +110,7 @@ public: inline bool isReady() const { return ready; } /** - * @return The timestamp of the first successful call to tryAppend(). + * @return The timestamp of the first successful call to append(). */ inline uint64_t timestamp() const { return ts; } @@ -126,7 +125,7 @@ public: inline unsigned int maxSize() const { return len; } /** - * @return How many times the tryAppend() function has been called + * @return How many times the append() function has been called * successfully. */ inline unsigned int getMsgCount() const { return msgCounter; } @@ -148,13 +147,12 @@ private: }; template <unsigned int len> -bool Packet<len>::tryAppend(const uint8_t* msg, const size_t msgLen) +size_t Packet<len>::append(const uint8_t* msg, size_t msgLen) { - if (msgLen == 0 || content.size() + msgLen > len) - { - return false; - } - else + size_t remaining = len - content.size(); + msgLen = std::min(remaining, msgLen); + + if (msgLen != 0) { // Set the packet's timestamp when the first message is inserted if (content.size() == 0) @@ -165,9 +163,9 @@ bool Packet<len>::tryAppend(const uint8_t* msg, const size_t msgLen) // Append the message to the packet content.insert(content.end(), msg, msg + msgLen); msgCounter++; - - return true; } + + return msgLen; } template <unsigned int len> @@ -230,7 +228,7 @@ public: { int dropped = 0; - if (msgLen == 0 || msgLen > pktLen) + if (msgLen == 0) { return -1; } @@ -243,43 +241,37 @@ public: buffer.put(Pkt{}); } - Pkt& last = buffer.last(); - - bool added = false; - - // Add to the current packet only if it isn't already ready - if (!last.isReady()) - { - added = last.tryAppend(msg, msgLen); - } - - // If already ready or cannot fit the new data, add to a new packet - if (!added) + while (msgLen > 0) { - // Mark the packet as ready (in the case it wasn't already) - last.markAsReady(); - - if (buffer.isFull()) + if (buffer.last().isReady()) { - // We have dropped a packet - ++dropped; + if (buffer.isFull()) + { + // We have dropped a packet + ++dropped; + } + + // If the last pkt is ready, append a new one + buffer.put(Pkt{}); + // FIXME(davide.mor): Figure out quantum shenanigans + // uncommenting the following line causes everything to + // break, why? + + // last = buffer.last(); } - // Add a new packet and fill that instead - Pkt& newpkt = buffer.put(Pkt{}); - if (!newpkt.tryAppend(msg, msgLen)) + size_t sentLen = buffer.last().append(msg, msgLen); + + msgLen -= sentLen; + msg += sentLen; + + // Mark as ready if the packet is full + if (buffer.last().isFull()) { - TRACE("Packet is too big!\n"); - return -1; + buffer.last().markAsReady(); } } - // Mark as ready if the packet is full - if (buffer.last().isFull()) - { - buffer.last().markAsReady(); - } - cvNotempty.broadcast(); return dropped; } diff --git a/src/tests/drivers/stepper/test-stepper.cpp b/src/tests/drivers/stepper/test-stepper.cpp index 4072e5269484a11e9fe2606943a468b35aed8573..3b27f5f0f565a98e6c41882148f92e31f2ef2ae2 100644 --- a/src/tests/drivers/stepper/test-stepper.cpp +++ b/src/tests/drivers/stepper/test-stepper.cpp @@ -1,5 +1,5 @@ /* Copyright (c) 2022 Skyward Experimental Rocketry - * Authors: Alberto Nidasio + * Author: Alberto Nidasio * * 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/tests/sensors/test-bme280.cpp b/src/tests/sensors/test-bme280.cpp index ab960d91ffcd84e51929f4deb927f596b4fd6875..afa223a4a46bab257afa859e1f4fd0ea1fad2cf4 100644 --- a/src/tests/sensors/test-bme280.cpp +++ b/src/tests/sensors/test-bme280.cpp @@ -54,15 +54,15 @@ GpioPin csPin = GpioPin(GPIOC_BASE, 1); void initBoard() { // Alternate function configuration for SPI pins - sckPin.mode(miosix::Mode::ALTERNATE); + sckPin.mode(Mode::ALTERNATE); sckPin.alternateFunction(5); // SPI function - mosiPin.mode(miosix::Mode::ALTERNATE); + mosiPin.mode(Mode::ALTERNATE); mosiPin.alternateFunction(5); // SPI function - misoPin.mode(miosix::Mode::ALTERNATE); + misoPin.mode(Mode::ALTERNATE); misoPin.alternateFunction(5); // SPI function // Chip select pin as output starting high - csPin.mode(miosix::Mode::OUTPUT); + csPin.mode(Mode::OUTPUT); csPin.high(); } @@ -98,7 +98,7 @@ int main() { bme280.setSensorMode(BME280::FORCED_MODE); - miosix::Thread::sleep(bme280.getMaxMeasurementTime()); + Thread::sleep(bme280.getMaxMeasurementTime()); bme280.sample(); @@ -106,7 +106,7 @@ int main() bme280.getLastSample().temperature, bme280.getLastSample().pressure, bme280.getLastSample().humidity); - miosix::Thread::sleep(1000); + Thread::sleep(1000); } TRACE("Normal mode\n"); @@ -119,6 +119,6 @@ int main() bme280.getLastSample().temperature, bme280.getLastSample().pressure, bme280.getLastSample().humidity); - miosix::Thread::sleep(50); // 25Hz + Thread::sleep(50); // 25Hz } } diff --git a/src/tests/sensors/test-bmp280.cpp b/src/tests/sensors/test-bmp280.cpp new file mode 100644 index 0000000000000000000000000000000000000000..d83478dede299492b607e2434a70c35b559b0906 --- /dev/null +++ b/src/tests/sensors/test-bmp280.cpp @@ -0,0 +1,107 @@ +/* Copyright (c) 2021 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * 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 <drivers/spi/SPIDriver.h> +#include <drivers/timer/TimestampTimer.h> +#include <miosix.h> +#include <sensors/BMP280/BMP280.h> +#include <utils/Debug.h> + +using namespace miosix; +using namespace Boardcore; + +GpioPin sckPin = GpioPin(GPIOA_BASE, 5); +GpioPin misoPin = GpioPin(GPIOB_BASE, 4); +GpioPin mosiPin = GpioPin(GPIOA_BASE, 7); +GpioPin csPin = GpioPin(GPIOB_BASE, 2); + +void initBoard() +{ + // Alternate function configuration for SPI pins + sckPin.mode(Mode::ALTERNATE); + sckPin.alternateFunction(5); // SPI function + mosiPin.mode(Mode::ALTERNATE); + mosiPin.alternateFunction(5); // SPI function + misoPin.mode(Mode::ALTERNATE); + misoPin.alternateFunction(5); // SPI function + + // Chip select pin as output starting high + csPin.mode(Mode::OUTPUT); + csPin.high(); +} + +int main() +{ + // Enable SPI clock and set gpios + initBoard(); + + // SPI configuration setup + SPIBusConfig spiConfig; + // spiConfig.clockDivider = SPI::ClockDivider::DIV_32; + spiConfig.mode = SPI::Mode::MODE_0; + SPIBus spiBus(SPI1); + SPISlave spiSlave(spiBus, csPin, spiConfig); + + // Device initialization + BMP280 bmp280(spiSlave); + + bmp280.init(); + + // In practice the self test reads the who am i reagister, this is already + // done in init() + if (!bmp280.selfTest()) + { + TRACE("Self test failed!\n"); + + return -1; + } + + // Try forced mode + TRACE("Forced mode\n"); + for (int i = 0; i < 10; i++) + { + bmp280.setSensorMode(BMP280::FORCED_MODE); + + Thread::sleep(bmp280.getMaxMeasurementTime()); + + bmp280.sample(); + + TRACE("temp: %.2f DegC\tpress: %.2f hPa\n", + bmp280.getLastSample().temperature, + bmp280.getLastSample().pressure); + + Thread::sleep(1000); + } + + TRACE("Normal mode\n"); + bmp280.setSensorMode(BMP280::NORMAL_MODE); + while (true) + { + bmp280.sample(); + + TRACE("temp: %.2f DegC\tpress: %.2f Pa\thumid: %.2f %%RH\n", + bmp280.getLastSample().temperature, + bmp280.getLastSample().pressure); + + Thread::sleep(50); // 25Hz + } +}