diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/calibration.cpp | 4 | ||||
| -rw-r--r-- | src/calibration.hpp | 51 | ||||
| -rw-r--r-- | src/gyronardo.cpp | 29 | ||||
| -rw-r--r-- | src/sensor/registers.hpp | 267 | ||||
| -rw-r--r-- | src/sensor/sensor.cpp | 174 | ||||
| -rw-r--r-- | src/sensor/sensor.hpp | 87 | ||||
| -rw-r--r-- | src/utils/general.cpp | 27 | ||||
| -rw-r--r-- | src/utils/general.hpp | 33 | ||||
| -rw-r--r-- | src/utils/memory.hpp | 26 | ||||
| -rw-r--r-- | src/utils/memory.tpp | 65 | ||||
| -rw-r--r-- | src/utils/serial.cpp | 31 | ||||
| -rw-r--r-- | src/utils/serial.hpp | 8 | ||||
| -rw-r--r-- | src/utils/smart_string.cpp | 53 | ||||
| -rw-r--r-- | src/utils/smart_string.hpp | 12 | ||||
| -rw-r--r-- | src/utils/time.cpp | 22 | ||||
| -rw-r--r-- | src/utils/time.hpp | 17 |
16 files changed, 523 insertions, 383 deletions
diff --git a/src/calibration.cpp b/src/calibration.cpp index a772ae9..66a52bf 100644 --- a/src/calibration.cpp +++ b/src/calibration.cpp @@ -3,7 +3,7 @@ #include "utils/general.hpp" #include "utils/time.hpp" -SensorCalibrator::SensorCalibrator(unique_ptr<Sensor> &sensor, SerialStream sout) +SensorCalibrator::SensorCalibrator(UniquePtr<Sensor> &sensor, SerialStream sout) : _sensor(sensor), _sout(sout) { _resetValues(); @@ -25,7 +25,7 @@ bool SensorCalibrator::calibrate(unsigned int throttle_time) _resetValues(); - for (int i = 0; i < SENSOR_READ_CNT; i++) + for (unsigned int i = 0U; i < SENSOR_READ_CNT; i++) { _updateValues(); } diff --git a/src/calibration.hpp b/src/calibration.hpp index e66b6fc..e29602a 100644 --- a/src/calibration.hpp +++ b/src/calibration.hpp @@ -1,33 +1,32 @@ -#ifndef CALIBRATION_HPP -#define CALIBRATION_HPP +#pragma once #include "sensor/sensor.hpp" #include "utils/memory.hpp" #include "utils/serial.hpp" // Calibration precision -#define ACCEL_CAL_X_MAX 0.006 -#define ACCEL_CAL_X_MIN -0.006 +constexpr float ACCEL_CAL_X_MAX = 0.006; +constexpr float ACCEL_CAL_X_MIN = -0.006; -#define ACCEL_CAL_Y_MAX 0.006 -#define ACCEL_CAL_Y_MIN -0.006 +constexpr float ACCEL_CAL_Y_MAX = 0.006; +constexpr float ACCEL_CAL_Y_MIN = -0.006; -#define ACCEL_CAL_Z_MAX 0.006 -#define ACCEL_CAL_Z_MIN -0.006 +constexpr float ACCEL_CAL_Z_MAX = 0.006; +constexpr float ACCEL_CAL_Z_MIN = -0.006; -#define GYRO_CAL_X_MAX 0.06 -#define GYRO_CAL_X_MIN -0.06 +constexpr float GYRO_CAL_X_MAX = 0.06; +constexpr float GYRO_CAL_X_MIN = -0.06; -#define GYRO_CAL_Y_MAX 0.06 -#define GYRO_CAL_Y_MIN -0.06 +constexpr float GYRO_CAL_Y_MAX = 0.06; +constexpr float GYRO_CAL_Y_MIN = -0.06; -#define GYRO_CAL_Z_MAX 0.06 -#define GYRO_CAL_Z_MIN -0.06 +constexpr float GYRO_CAL_Z_MAX = 0.06; +constexpr float GYRO_CAL_Z_MIN = -0.06; -#define CALIBRATION_TIMEOUT 120000 // Milliseconds +constexpr uint32_t CALIBRATION_TIMEOUT = 120000; // Milliseconds -#define SENSOR_READ_CNT 20 -#define SENSOR_VAL_ADJUST 0.05 +constexpr unsigned int SENSOR_READ_CNT = 20; +constexpr float SENSOR_VAL_ADJUST = 0.05; /** * Sensor calibrator. @@ -41,7 +40,7 @@ public: * @param sensor A sensor to calibrate * @param sout A Serial output stream */ - SensorCalibrator(unique_ptr<Sensor> &sensor, SerialStream sout); + SensorCalibrator(UniquePtr<Sensor> &sensor, SerialStream sout); /** * Calibrates the sensor. @@ -60,17 +59,15 @@ private: void _adjustCalibration(); - unique_ptr<Sensor> &_sensor; + UniquePtr<Sensor> &_sensor; SerialStream _sout; - float _accel_x; - float _accel_y; - float _accel_z; + double _accel_x = 0; + double _accel_y = 0; + double _accel_z = 0; - float _gyro_x; - float _gyro_y; - float _gyro_z; + double _gyro_x = 0; + double _gyro_y = 0; + double _gyro_z = 0; }; - -#endif diff --git a/src/gyronardo.cpp b/src/gyronardo.cpp index 53a1912..1133f3f 100644 --- a/src/gyronardo.cpp +++ b/src/gyronardo.cpp @@ -1,28 +1,31 @@ -#include "Arduino.h" - -#include "Wire.h" #include "calibration.hpp" #include "sensor/sensor.hpp" #include "utils/memory.hpp" - #include "utils/serial.hpp" -#define LINE_CLEAR_LENGTH 30 +#include <Arduino.h> +#include <Wire.h> + +// constexpr unsigned int LINE_CLEAR_LENGTH = 30U; -#define SENSOR_ADDRESS 0x68 -#define SENSOR_THROTTLE_TIME 50 // milliseconds +constexpr unsigned int SENSOR_ADDRESS = 0x68U; +constexpr unsigned int SENSOR_THROTTLE_TIME = 50U; // milliseconds -unique_ptr<SerialStream> sout_ptr = nullptr; +constexpr unsigned int BAUD_RATE = 9600U; -unique_ptr<Sensor> sensor = nullptr; +constexpr unsigned int SENSOR_RETRY_TIME = 2000U; + +UniquePtr<SerialStream> sout_ptr; + +UniquePtr<Sensor> sensor; void setup() { - sout_ptr = make_unique<SerialStream>(Serial, 9600); + sout_ptr = make_unique<SerialStream>(Serial, BAUD_RATE); auto sout = *sout_ptr; - sensor = make_unique<Sensor>(SENSOR_ADDRESS, &Wire, sout, SENSOR_THROTTLE_TIME); + sensor = make_unique<Sensor>(SENSOR_ADDRESS, Wire, sout, SENSOR_THROTTLE_TIME); while (!sensor->begin()) { @@ -30,11 +33,9 @@ void setup() "milliseconds..." << endl; - delay(2000); + delay(SENSOR_RETRY_TIME); } - sout << "Skit!" << endl; - if (!sensor->setAccelSensitivity(2)) // 8g { sout << "Error: Failed to set the sensor's accelerometer sensitivity. Status: " diff --git a/src/sensor/registers.hpp b/src/sensor/registers.hpp index c1b19b4..fadb62b 100644 --- a/src/sensor/registers.hpp +++ b/src/sensor/registers.hpp @@ -1,137 +1,136 @@ -#ifndef SENSOR_REGISTERS_HPP -#define SENSOR_REGISTERS_HPP +#pragma once + +#include <stdint.h> namespace SensorRegisters { - const uint8_t XG_OFFS_TC = 0x00; - const uint8_t YG_OFFS_TC = 0x01; - const uint8_t ZG_OFFS_TC = 0x02; - - const uint8_t X_FINE_GAIN = 0x03; - const uint8_t Y_FINE_GAIN = 0x04; - const uint8_t Z_FINE_GAIN = 0x05; - - const uint8_t XA_OFFS_H = 0x06; - const uint8_t XA_OFFS_L_TC = 0x07; - const uint8_t YA_OFFS_H = 0x08; - const uint8_t YA_OFFS_L_TC = 0x09; - const uint8_t ZA_OFFS_H = 0x0A; - const uint8_t ZA_OFFS_L_TC = 0x0B; - - const uint8_t SELF_TEST_X = 0x0D; - const uint8_t SELF_TEST_Y = 0x0E; - const uint8_t SELF_TEST_Z = 0x0F; - const uint8_t SELF_TEST_A = 0x10; - - const uint8_t XG_OFFS_USRH = 0x13; - const uint8_t XG_OFFS_USRL = 0x14; - const uint8_t YG_OFFS_USRH = 0x15; - const uint8_t YG_OFFS_USRL = 0x16; - const uint8_t ZG_OFFS_USRH = 0x17; - const uint8_t ZG_OFFS_USRL = 0x18; - - const uint8_t SMPLRT_DIV = 0x19; - const uint8_t CONFIG = 0x1A; - const uint8_t GYRO_CONFIG = 0x1B; - const uint8_t ACCEL_CONFIG = 0x1C; - - const uint8_t FF_THR = 0x1D; - const uint8_t FF_DUR = 0x1E; - const uint8_t MOT_THR = 0x1F; - const uint8_t MOT_DUR = 0x20; - const uint8_t ZRMOT_THR = 0x21; - const uint8_t ZRMOT_DUR = 0x22; - const uint8_t FIFO_EN = 0x23; - - const uint8_t I2C_MST_CTRL = 0x24; - const uint8_t I2C_SLV0_ADDR = 0x25; - const uint8_t I2C_SLV0_REG = 0x26; - const uint8_t I2C_SLV0_CTRL = 0x27; - const uint8_t I2C_SLV1_ADDR = 0x28; - const uint8_t I2C_SLV1_REG = 0x29; - const uint8_t I2C_SLV1_CTRL = 0x2A; - const uint8_t I2C_SLV2_ADDR = 0x2B; - const uint8_t I2C_SLV2_REG = 0x2C; - const uint8_t I2C_SLV2_CTRL = 0x2D; - const uint8_t I2C_SLV3_ADDR = 0x2E; - const uint8_t I2C_SLV3_REG = 0x2F; - const uint8_t I2C_SLV3_CTRL = 0x30; - const uint8_t I2C_SLV4_ADDR = 0x31; - const uint8_t I2C_SLV4_REG = 0x32; - const uint8_t I2C_SLV4_DO = 0x33; - const uint8_t I2C_SLV4_CTRL = 0x34; - const uint8_t I2C_SLV4_DI = 0x35; - const uint8_t I2C_MST_STATUS = 0x36; - - const uint8_t INT_PIN_CFG = 0x37; - const uint8_t INT_ENABLE = 0x38; - const uint8_t DMP_INT_STATUS = 0x39; - const uint8_t INT_STATUS = 0x3A; - - const uint8_t ACCEL_XOUT_H = 0x3B; - const uint8_t ACCEL_XOUT_L = 0x3C; - const uint8_t ACCEL_YOUT_H = 0x3D; - const uint8_t ACCEL_YOUT_L = 0x3E; - const uint8_t ACCEL_ZOUT_H = 0x3F; - const uint8_t ACCEL_ZOUT_L = 0x40; - const uint8_t TEMP_OUT_H = 0x41; - const uint8_t TEMP_OUT_L = 0x42; - const uint8_t GYRO_XOUT_H = 0x43; - const uint8_t GYRO_XOUT_L = 0x44; - const uint8_t GYRO_YOUT_H = 0x45; - const uint8_t GYRO_YOUT_L = 0x46; - const uint8_t GYRO_ZOUT_H = 0x47; - const uint8_t GYRO_ZOUT_L = 0x48; - - const uint8_t EXT_SENS_DATA_00 = 0x49; - const uint8_t EXT_SENS_DATA_01 = 0x4A; - const uint8_t EXT_SENS_DATA_02 = 0x4B; - const uint8_t EXT_SENS_DATA_03 = 0x4C; - const uint8_t EXT_SENS_DATA_04 = 0x4D; - const uint8_t EXT_SENS_DATA_05 = 0x4E; - const uint8_t EXT_SENS_DATA_06 = 0x4F; - const uint8_t EXT_SENS_DATA_07 = 0x50; - const uint8_t EXT_SENS_DATA_08 = 0x51; - const uint8_t EXT_SENS_DATA_09 = 0x52; - const uint8_t EXT_SENS_DATA_10 = 0x53; - const uint8_t EXT_SENS_DATA_11 = 0x54; - const uint8_t EXT_SENS_DATA_12 = 0x55; - const uint8_t EXT_SENS_DATA_13 = 0x56; - const uint8_t EXT_SENS_DATA_14 = 0x57; - const uint8_t EXT_SENS_DATA_15 = 0x58; - const uint8_t EXT_SENS_DATA_16 = 0x59; - const uint8_t EXT_SENS_DATA_17 = 0x5A; - const uint8_t EXT_SENS_DATA_18 = 0x5B; - const uint8_t EXT_SENS_DATA_19 = 0x5C; - const uint8_t EXT_SENS_DATA_20 = 0x5D; - const uint8_t EXT_SENS_DATA_21 = 0x5E; - const uint8_t EXT_SENS_DATA_22 = 0x5F; - const uint8_t EXT_SENS_DATA_23 = 0x60; - - const uint8_t MOT_DETECT_STATUS = 0x61; - - const uint8_t I2C_SLV0_DO = 0x63; - const uint8_t I2C_SLV1_DO = 0x64; - const uint8_t I2C_SLV2_DO = 0x65; - const uint8_t I2C_SLV3_DO = 0x66; - const uint8_t I2C_MST_DELAY_CTRL = 0x67; - - const uint8_t SIGNAL_PATH_RESET = 0x68; - const uint8_t MOT_DETECT_CTRL = 0x69; - const uint8_t USER_CTRL = 0x6A; - - const uint8_t PWR_MGMT_1 = 0x6B; - const uint8_t PWR_MGMT_2 = 0x6C; - const uint8_t BANK_SEL = 0x6D; - const uint8_t MEM_START_ADDR = 0x6E; - const uint8_t MEM_R_W = 0x6F; - - const uint8_t DMP_CFG_1 = 0x70; - const uint8_t DMP_CFG_2 = 0x71; - const uint8_t FIFO_COUNTH = 0x72; - const uint8_t FIFO_COUNTL = 0x73; - const uint8_t FIFO_R_W = 0x74; - const uint8_t WHO_AM_I = 0x75; -} - -#endif + const uint8_t XG_OFFS_TC = 0x00U; + const uint8_t YG_OFFS_TC = 0x01U; + const uint8_t ZG_OFFS_TC = 0x02U; + + const uint8_t X_FINE_GAIN = 0x03U; + const uint8_t Y_FINE_GAIN = 0x04U; + const uint8_t Z_FINE_GAIN = 0x05U; + + const uint8_t XA_OFFS_H = 0x06U; + const uint8_t XA_OFFS_L_TC = 0x07U; + const uint8_t YA_OFFS_H = 0x08U; + const uint8_t YA_OFFS_L_TC = 0x09U; + const uint8_t ZA_OFFS_H = 0x0AU; + const uint8_t ZA_OFFS_L_TC = 0x0BU; + + const uint8_t SELF_TEST_X = 0x0DU; + const uint8_t SELF_TEST_Y = 0x0EU; + const uint8_t SELF_TEST_Z = 0x0FU; + const uint8_t SELF_TEST_A = 0x10U; + + const uint8_t XG_OFFS_USRH = 0x13U; + const uint8_t XG_OFFS_USRL = 0x14U; + const uint8_t YG_OFFS_USRH = 0x15U; + const uint8_t YG_OFFS_USRL = 0x16U; + const uint8_t ZG_OFFS_USRH = 0x17U; + const uint8_t ZG_OFFS_USRL = 0x18U; + + const uint8_t SMPLRT_DIV = 0x19U; + const uint8_t CONFIG = 0x1AU; + const uint8_t GYRO_CONFIG = 0x1BU; + const uint8_t ACCEL_CONFIG = 0x1CU; + + const uint8_t FF_THR = 0x1DU; + const uint8_t FF_DUR = 0x1EU; + const uint8_t MOT_THR = 0x1FU; + const uint8_t MOT_DUR = 0x20U; + const uint8_t ZRMOT_THR = 0x21U; + const uint8_t ZRMOT_DUR = 0x22U; + const uint8_t FIFO_EN = 0x23U; + + const uint8_t I2C_MST_CTRL = 0x24U; + const uint8_t I2C_SLV0_ADDR = 0x25U; + const uint8_t I2C_SLV0_REG = 0x26U; + const uint8_t I2C_SLV0_CTRL = 0x27U; + const uint8_t I2C_SLV1_ADDR = 0x28U; + const uint8_t I2C_SLV1_REG = 0x29U; + const uint8_t I2C_SLV1_CTRL = 0x2AU; + const uint8_t I2C_SLV2_ADDR = 0x2BU; + const uint8_t I2C_SLV2_REG = 0x2CU; + const uint8_t I2C_SLV2_CTRL = 0x2DU; + const uint8_t I2C_SLV3_ADDR = 0x2EU; + const uint8_t I2C_SLV3_REG = 0x2FU; + const uint8_t I2C_SLV3_CTRL = 0x30U; + const uint8_t I2C_SLV4_ADDR = 0x31U; + const uint8_t I2C_SLV4_REG = 0x32U; + const uint8_t I2C_SLV4_DO = 0x33U; + const uint8_t I2C_SLV4_CTRL = 0x34U; + const uint8_t I2C_SLV4_DI = 0x35U; + const uint8_t I2C_MST_STATUS = 0x36U; + + const uint8_t INT_PIN_CFG = 0x37U; + const uint8_t INT_ENABLE = 0x38U; + const uint8_t DMP_INT_STATUS = 0x39U; + const uint8_t INT_STATUS = 0x3AU; + + const uint8_t ACCEL_XOUT_H = 0x3BU; + const uint8_t ACCEL_XOUT_L = 0x3CU; + const uint8_t ACCEL_YOUT_H = 0x3DU; + const uint8_t ACCEL_YOUT_L = 0x3EU; + const uint8_t ACCEL_ZOUT_H = 0x3FU; + const uint8_t ACCEL_ZOUT_L = 0x40U; + const uint8_t TEMP_OUT_H = 0x41U; + const uint8_t TEMP_OUT_L = 0x42U; + const uint8_t GYRO_XOUT_H = 0x43U; + const uint8_t GYRO_XOUT_L = 0x44U; + const uint8_t GYRO_YOUT_H = 0x45U; + const uint8_t GYRO_YOUT_L = 0x46U; + const uint8_t GYRO_ZOUT_H = 0x47U; + const uint8_t GYRO_ZOUT_L = 0x48U; + + const uint8_t EXT_SENS_DATA_00 = 0x49U; + const uint8_t EXT_SENS_DATA_01 = 0x4AU; + const uint8_t EXT_SENS_DATA_02 = 0x4BU; + const uint8_t EXT_SENS_DATA_03 = 0x4CU; + const uint8_t EXT_SENS_DATA_04 = 0x4DU; + const uint8_t EXT_SENS_DATA_05 = 0x4EU; + const uint8_t EXT_SENS_DATA_06 = 0x4FU; + const uint8_t EXT_SENS_DATA_07 = 0x50U; + const uint8_t EXT_SENS_DATA_08 = 0x51U; + const uint8_t EXT_SENS_DATA_09 = 0x52U; + const uint8_t EXT_SENS_DATA_10 = 0x53U; + const uint8_t EXT_SENS_DATA_11 = 0x54U; + const uint8_t EXT_SENS_DATA_12 = 0x55U; + const uint8_t EXT_SENS_DATA_13 = 0x56U; + const uint8_t EXT_SENS_DATA_14 = 0x57U; + const uint8_t EXT_SENS_DATA_15 = 0x58U; + const uint8_t EXT_SENS_DATA_16 = 0x59U; + const uint8_t EXT_SENS_DATA_17 = 0x5AU; + const uint8_t EXT_SENS_DATA_18 = 0x5BU; + const uint8_t EXT_SENS_DATA_19 = 0x5CU; + const uint8_t EXT_SENS_DATA_20 = 0x5DU; + const uint8_t EXT_SENS_DATA_21 = 0x5EU; + const uint8_t EXT_SENS_DATA_22 = 0x5FU; + const uint8_t EXT_SENS_DATA_23 = 0x60U; + + const uint8_t MOT_DETECT_STATUS = 0x61U; + + const uint8_t I2C_SLV0_DO = 0x63U; + const uint8_t I2C_SLV1_DO = 0x64U; + const uint8_t I2C_SLV2_DO = 0x65U; + const uint8_t I2C_SLV3_DO = 0x66U; + const uint8_t I2C_MST_DELAY_CTRL = 0x67U; + + const uint8_t SIGNAL_PATH_RESET = 0x68U; + const uint8_t MOT_DETECT_CTRL = 0x69U; + const uint8_t USER_CTRL = 0x6AU; + + const uint8_t PWR_MGMT_1 = 0x6BU; + const uint8_t PWR_MGMT_2 = 0x6CU; + const uint8_t BANK_SEL = 0x6DU; + const uint8_t MEM_START_ADDR = 0x6EU; + const uint8_t MEM_R_W = 0x6FU; + + const uint8_t DMP_CFG_1 = 0x70U; + const uint8_t DMP_CFG_2 = 0x71U; + const uint8_t FIFO_COUNTH = 0x72U; + const uint8_t FIFO_COUNTL = 0x73U; + const uint8_t FIFO_R_W = 0x74U; + const uint8_t WHO_AM_I = 0x75U; +} // namespace SensorRegisters diff --git a/src/sensor/sensor.cpp b/src/sensor/sensor.cpp index e11a74b..a3b9a5c 100644 --- a/src/sensor/sensor.cpp +++ b/src/sensor/sensor.cpp @@ -1,50 +1,34 @@ #include "sensor.hpp" -#include "../utils/serial.hpp" -#include "../utils/time.hpp" + #include "registers.hpp" +#include "utils/serial.hpp" +#include "utils/time.hpp" -Sensor::Sensor(uint8_t address, TwoWire *wire, SerialStream sout, +Sensor::Sensor(uint8_t address, TwoWire wire, SerialStream sout, unsigned int throttle_time) - : _last_time(0), _sout(sout) + : _wire(wire), + _address(address), + _throttle_enabled(true), + _throttle_time(throttle_time), + _last_time(0), + _status(SensorStatus::OK), + _accel_to_g_force(RAW_TO_G_FACTOR), + _ang_rate_to_dps(RAW_TO_DPS_FACTOR), + _sout(sout) { - _address = address; - _wire = wire; - - // Set default values - accel_cal_x = 0; - accel_cal_y = 0; - accel_cal_z = 0; - - gyro_cal_x = 0; - gyro_cal_y = 0; - gyro_cal_z = 0; - - _throttle_enabled = true; - _throttle_time = throttle_time; - - _status = SensorStatus::OK; - - _accel_sensitivity = 0; - _accel_to_g_force = 1.0 / 16384.0; - - _gyro_sensitivity = 0; - _ang_rate_to_dps = 1.0 / 131.0; - - // Reset the sensor - reset(); } bool Sensor::begin() { - _wire->begin(); + _wire.begin(); if (isConnected()) { - _wire->beginTransmission(_address); - _wire->write(SensorRegisters::PWR_MGMT_1); - _wire->write(SENSOR_WAKEUP); + _wire.beginTransmission(_address); + _wire.write(SensorRegisters::PWR_MGMT_1); + _wire.write(SENSOR_WAKEUP); - return (_wire->endTransmission() == 0); + return (_wire.endTransmission() == 0); } return false; @@ -52,31 +36,13 @@ bool Sensor::begin() bool Sensor::isConnected() { - _wire->beginTransmission(_address); - return (_wire->endTransmission() == 0); -} - -void Sensor::reset() -{ - _accel_raw_x = 0; - _accel_raw_y = 0; - _accel_raw_z = 0; - - _gyro_raw_x = 0; - _gyro_raw_y = 0; - _gyro_raw_z = 0; - - _accel_x = 0; - _accel_y = 0; - - _pitch = 0; - _roll = 0; - _yaw = 0; + _wire.beginTransmission(_address); + return (_wire.endTransmission() == 0); } bool Sensor::read() { - Time now = time_now(); + auto now = time_now(); if (_throttle_enabled && now.diff(_last_time).millisecs() < _throttle_time) { @@ -84,18 +50,20 @@ bool Sensor::read() return false; } - _wire->beginTransmission(_address); - _wire->write(SensorRegisters::ACCEL_XOUT_H); + _wire.beginTransmission(_address); + _wire.write(SensorRegisters::ACCEL_XOUT_H); - if (_wire->endTransmission() != 0) + if (_wire.endTransmission() != 0U) { _status = SensorStatus::ERR_WRITE; return false; } - int8_t response_length = _wire->requestFrom(_address, (uint8_t)14); + uint8_t response_length = _wire.requestFrom(_address, SensorRegisters::SELF_TEST_Y); + + const uint8_t self_test_success = 14U; - if (response_length != 14) + if (response_length != self_test_success) { _status = SensorStatus::ERR_READ; return false; @@ -123,7 +91,7 @@ bool Sensor::read() // Duration interval now.update(); - float duration = now.diff(_last_time).secs(); + auto duration = now.diff(_last_time).secs(); _last_time = now; // Convert raw acceleration to g:s (g-force) @@ -147,12 +115,13 @@ bool Sensor::read() << endl; // Prepare for Pitch Roll Yaw - float accel_y_pow_two = pow(_accel_raw_y, 2); - float accel_z_pow_two = pow(_accel_raw_z, 2); + auto accel_y_pow_two = pow(_accel_raw_y, 2); + auto accel_z_pow_two = pow(_accel_raw_z, 2); - _accel_x = atan2(_accel_raw_y, _accel_raw_z) * 180 / PI; + _accel_x = atan2(_accel_raw_y, _accel_raw_z) * ONE_EIGHTY / PI; - _accel_y = atan2(-_accel_raw_x, sqrt(accel_y_pow_two + accel_z_pow_two)) * 180 / PI; + _accel_y = + atan2(-_accel_raw_x, sqrt(accel_y_pow_two + accel_z_pow_two)) * ONE_EIGHTY / PI; /* _accel_x = @@ -191,8 +160,11 @@ bool Sensor::read() << "Gyro raw z w/o time: " << _gyro_raw_z << "\n" << endl; - _pitch = 0.96 * _gyro_y + 0.04 * _accel_y; - _roll = 0.96 * _gyro_x + 0.04 * _accel_x; + const float gyro_balance = 0.96F; + const float accel_balance = 0.04F; + + _pitch = gyro_balance * _gyro_y + accel_balance * _accel_y; + _roll = gyro_balance * _gyro_x + accel_balance * _accel_x; _yaw = _gyro_z; return true; @@ -201,23 +173,31 @@ bool Sensor::read() bool Sensor::setAccelSensitivity(uint8_t sensitivity) { if (sensitivity > 3) + { sensitivity = 3; + } _accel_sensitivity = sensitivity; uint8_t accel_config = getRegister(SensorRegisters::ACCEL_CONFIG); if (_status != SensorStatus::OK) + { return false; + } // No need to write same value if (((accel_config >> 3) & 3) != _accel_sensitivity) { - accel_config &= 0xE7; + const uint8_t magic = 0xE7U; + + accel_config &= magic; accel_config |= (_accel_sensitivity << 3); if (!setRegister(SensorRegisters::ACCEL_CONFIG, accel_config)) + { return false; + } } // Calculate conversion factor. @@ -229,7 +209,9 @@ bool Sensor::setAccelSensitivity(uint8_t sensitivity) bool Sensor::setGyroSensitivity(uint8_t sensitivity) { if (sensitivity > 3) + { sensitivity = 3; + } _gyro_sensitivity = sensitivity; @@ -243,7 +225,9 @@ bool Sensor::setGyroSensitivity(uint8_t sensitivity) // No need to write same value if (((gyro_config >> 3) & 3) != _gyro_sensitivity) { - gyro_config &= 0xE7; + const uint8_t magic = 0xE7U; + + gyro_config &= magic; gyro_config |= (_gyro_sensitivity << 3); if (!setRegister(SensorRegisters::GYRO_CONFIG, gyro_config)) @@ -257,63 +241,63 @@ bool Sensor::setGyroSensitivity(uint8_t sensitivity) return true; } -float Sensor::getAccelX() +double Sensor::getAccelX() { return _accel_raw_x; } -float Sensor::getAccelY() +double Sensor::getAccelY() { return _accel_raw_y; } -float Sensor::getAccelZ() +double Sensor::getAccelZ() { return _accel_raw_z; } -float Sensor::getAngleX() +double Sensor::getAngleX() { return _accel_x; } -float Sensor::getAngleY() +double Sensor::getAngleY() { return _accel_y; } -float Sensor::getGyroX() +double Sensor::getGyroX() { return _gyro_raw_x; } -float Sensor::getGyroY() +double Sensor::getGyroY() { return _gyro_raw_y; } -float Sensor::getGyroZ() +double Sensor::getGyroZ() { return _gyro_raw_z; } -float Sensor::getPitch() +double Sensor::getPitch() { return _pitch; } -float Sensor::getRoll() +double Sensor::getRoll() { return _roll; } bool Sensor::setRegister(uint8_t reg, uint8_t value) { - _wire->beginTransmission(_address); - _wire->write(reg); - _wire->write(value); + _wire.beginTransmission(_address); + _wire.write(reg); + _wire.write(value); - if (_wire->endTransmission() != 0) + if (_wire.endTransmission() != 0) { _status = SensorStatus::ERR_WRITE; return false; @@ -324,23 +308,23 @@ bool Sensor::setRegister(uint8_t reg, uint8_t value) uint8_t Sensor::getRegister(uint8_t reg) { - _wire->beginTransmission(_address); - _wire->write(reg); + _wire.beginTransmission(_address); + _wire.write(reg); - if (_wire->endTransmission() != 0) + if (_wire.endTransmission() != 0U) { _status = SensorStatus::ERR_WRITE; - return 0; + return 0U; } - uint8_t response_length = _wire->requestFrom(_address, (uint8_t)1); - if (response_length != 1) + uint8_t response_length = _wire.requestFrom(_address, 1U); + if (response_length != 1U) { _status = SensorStatus::ERR_READ; - return 0; + return 0U; } - return _wire->read(); + return _wire.read(); } SensorStatus Sensor::getStatus() @@ -353,8 +337,10 @@ SensorStatus Sensor::getStatus() int16_t Sensor::_readHighLow() { - int16_t high = _wire->read(); - int16_t low = _wire->read(); + const int16_t high = _wire.read(); + const int16_t low = _wire.read(); + + const int8_t bits_in_byte = 8; - return high << 8 | low; + return high << bits_in_byte | low; } diff --git a/src/sensor/sensor.hpp b/src/sensor/sensor.hpp index c982fac..3e56627 100644 --- a/src/sensor/sensor.hpp +++ b/src/sensor/sensor.hpp @@ -1,16 +1,17 @@ -#ifndef SENSOR_HPP -#define SENSOR_HPP +#pragma once -#include "../utils/serial.hpp" -#include "../utils/time.hpp" +#include "utils/serial.hpp" +#include "utils/time.hpp" -#include "Wire.h" #include <Arduino.h> +#include <Wire.h> -#define SENSOR_WAKEUP 0x00 +constexpr uint8_t SENSOR_WAKEUP = 0x00U; -#define RAW_TO_DPS_FACTOR (1.0 / 131.0) -#define RAW_TO_G_FACTOR (1.0 / 16384.0) +constexpr float RAW_TO_DPS_FACTOR = 1.0 / 131.0; +constexpr float RAW_TO_G_FACTOR = 1.0 / 16384.0; + +constexpr unsigned int ONE_EIGHTY = 180; enum class SensorStatus { @@ -35,7 +36,7 @@ public: * @param sout A serial output stream * @param throttle_time A minumum time between sensor reads for the sensor to throttle */ - Sensor(uint8_t address, TwoWire *wire, SerialStream sout, unsigned int throttle_time); + Sensor(uint8_t address, TwoWire wire, SerialStream sout, unsigned int throttle_time); /** * Initializes communication with the sensor. @@ -50,11 +51,6 @@ public: bool isConnected(); /** - * Resets the sensor. - */ - void reset(); - - /** * Reads from the sensor. * * @returns Whether or not it succeeded. @@ -81,52 +77,52 @@ public: /** * Returns the current X axis acceleration in g:s (g-force). */ - float getAccelX(); + double getAccelX(); /** * Returns the current Y axis acceleration in g:s (g-force). */ - float getAccelY(); + double getAccelY(); /** * Returns the current Z axis acceleration in g:s (g-force). */ - float getAccelZ(); + double getAccelZ(); /** * Returns the current X angle. */ - float getAngleX(); + double getAngleX(); /** * Returns the current Y angle. */ - float getAngleY(); + double getAngleY(); /** * Returns the current X axis in degrees/s. */ - float getGyroX(); + double getGyroX(); /** * Returns the current Y axis in degrees/s. */ - float getGyroY(); + double getGyroY(); /** * Returns the current Z axis in degrees/s. */ - float getGyroZ(); + double getGyroZ(); /** * Returns the current pitch angle. */ - float getPitch(); + double getPitch(); /** * Returns the current roll angle. */ - float getRoll(); + double getRoll(); /** * Sets the value of a key in the sensor's register. @@ -150,16 +146,18 @@ public: SensorStatus getStatus(); // Accelerometer calibration values - float accel_cal_x; - float accel_cal_y; - float accel_cal_z; + double accel_cal_x = 0.0F; + double accel_cal_y = 0.0F; + double accel_cal_z = 0.0F; // Gyroscope calibration values - float gyro_cal_x; - float gyro_cal_y; - float gyro_cal_z; + double gyro_cal_x = 0.0F; + double gyro_cal_y = 0.0F; + double gyro_cal_z = 0.0F; private: + TwoWire _wire; + uint8_t _address; bool _throttle_enabled; @@ -169,25 +167,32 @@ private: SensorStatus _status; - uint8_t _accel_sensitivity; + uint8_t _accel_sensitivity = 0U; float _accel_to_g_force; - float _accel_raw_x, _accel_raw_y, _accel_raw_z; - float _accel_x, _accel_y; + double _accel_raw_x = 0; + double _accel_raw_y = 0; + double _accel_raw_z = 0; - uint8_t _gyro_sensitivity; + double _accel_x = 0; + double _accel_y = 0; + + uint8_t _gyro_sensitivity = 0U; float _ang_rate_to_dps; - float _gyro_raw_x, _gyro_raw_y, _gyro_raw_z; - float _gyro_x, _gyro_y, _gyro_z; + double _gyro_raw_x = 0; + double _gyro_raw_y = 0; + double _gyro_raw_z = 0; - float _pitch, _roll, _yaw; + double _gyro_x = 0; + double _gyro_y = 0; + double _gyro_z = 0; - int16_t _readHighLow(); + double _pitch = 0; + double _roll = 0; + double _yaw = 0; - TwoWire *_wire; + int16_t _readHighLow(); SerialStream _sout; }; - -#endif diff --git a/src/utils/general.cpp b/src/utils/general.cpp index bbcd4dd..3464c30 100644 --- a/src/utils/general.cpp +++ b/src/utils/general.cpp @@ -5,10 +5,33 @@ void stop() while (true) {} } -unique_ptr<SmartString> floatToStr(float num, unsigned int width, unsigned int precision) +UniquePtr<SmartString> doubleToStr(double num, unsigned int width, unsigned int precision) { auto str = make_unique<SmartString>(width + precision); - dtostrf(num, width, precision, str->c_str); + + dtostrf(num, static_cast<signed char>(width), precision, str->c_str); + + return str; +} + +UniquePtr<SmartString> intToStr(int num) +{ + auto width = static_cast<unsigned int>(log10(num)); + + auto str = make_unique<SmartString>(width + 1U); + + dtostrf(num, static_cast<signed char>(width + 1U), 0, str->c_str); + + return str; +} + +UniquePtr<SmartString> uintToStr(unsigned int num) +{ + auto width = static_cast<unsigned int>(log10(num)); + + auto str = make_unique<SmartString>(width + 1U); + + dtostrf(num, static_cast<signed char>(width + 1U), 0, str->c_str); return str; } diff --git a/src/utils/general.hpp b/src/utils/general.hpp index 9ad0655..dbdbdfc 100644 --- a/src/utils/general.hpp +++ b/src/utils/general.hpp @@ -9,15 +9,28 @@ void stop(); /** - * Converts a floating point number to a string. + * Converts a double number to a string. * - * @param num A floating point number - * @param width The desired float width - * @param precision The desired float precision - * @returns The float as a string. + * @param num A double number + * @param width The desired double width + * @param precision The desired double precision + * @returns The double as a string. */ -unique_ptr<SmartString> floatToStr( - float num, - unsigned int width = 3, - unsigned int precision = 2 -); +UniquePtr<SmartString> doubleToStr(double num, unsigned int width = 3, + unsigned int precision = 2); + +/** + * Converts a integer to a string. + * + * @param num A number + * @returns The number as a string. + */ +UniquePtr<SmartString> intToStr(int num); + +/** + * Converts a unsigned integer to a string. + * + * @param num A number + * @returns The number as a string. + */ +UniquePtr<SmartString> uintToStr(unsigned int num); diff --git a/src/utils/memory.hpp b/src/utils/memory.hpp index 81e3757..e870b0b 100644 --- a/src/utils/memory.hpp +++ b/src/utils/memory.hpp @@ -1,23 +1,33 @@ #pragma once +#include <stddef.h> + template <typename memType> memType *malloc_s(unsigned int size); template <class Target> -class unique_ptr +class UniquePtr { public: - unique_ptr(Target *target); - ~unique_ptr(); + explicit UniquePtr(); + explicit UniquePtr(Target *target); + UniquePtr(const UniquePtr &unique_ptr); + UniquePtr(UniquePtr &&unique_ptr) noexcept; + + UniquePtr &operator=(const UniquePtr &unique_ptr); + + UniquePtr &operator=(UniquePtr &&unique_ptr) noexcept; + + ~UniquePtr(); - Target operator *() const; - Target *operator ->() const; + Target operator*() const; + Target *operator->() const; private: - Target *_target; + Target *_target = nullptr; }; -template<class Target, typename...Args> -unique_ptr<Target> make_unique(Args&... args); +template <typename Target, typename... Args> +UniquePtr<Target> make_unique(Args... args); #include "memory.tpp" diff --git a/src/utils/memory.tpp b/src/utils/memory.tpp index 276b0b4..6261c81 100644 --- a/src/utils/memory.tpp +++ b/src/utils/memory.tpp @@ -1,8 +1,9 @@ -#include "memory.hpp" +#pragma once -#include "Arduino.h" #include "general.hpp" +#include <Arduino.h> + template <typename memType> memType *malloc_s(unsigned int size) { @@ -18,31 +19,75 @@ memType *malloc_s(unsigned int size) } template <class Target> -unique_ptr<Target>::unique_ptr(Target *target) +UniquePtr<Target>::UniquePtr() = default; + +template <class Target> +UniquePtr<Target>::UniquePtr(Target *target) : _target(target) +{ +} + +template <class Target> +UniquePtr<Target>::UniquePtr(const UniquePtr &unique_ptr) + : _target(new Target(*(unique_ptr._target))) { - this->_target = target; } template <class Target> -unique_ptr<Target>::~unique_ptr() +UniquePtr<Target>::UniquePtr(UniquePtr &&unique_ptr) noexcept + : _target(unique_ptr._target) { - delete this->_target; + unique_ptr._target = nullptr; +} + +template <class Target> +UniquePtr<Target> &UniquePtr<Target>::operator=(const UniquePtr &unique_ptr) +{ + if (&unique_ptr != this) + { + delete _target; + _target = nullptr; + _target = new Target(*(unique_ptr._target)); + } + + return *this; +} + +template <class Target> +UniquePtr<Target> &UniquePtr<Target>::operator=(UniquePtr &&unique_ptr) noexcept +{ + if (&unique_ptr != this) + { + delete _target; + _target = unique_ptr._target; + unique_ptr._target = nullptr; + } + + return *this; +} + +template <class Target> +UniquePtr<Target>::~UniquePtr() +{ + if (this->_target != nullptr) + { + delete this->_target; + } } template <class Target> -Target unique_ptr<Target>::operator*() const +Target UniquePtr<Target>::operator*() const { return *(this->_target); } template <class Target> -Target *unique_ptr<Target>::operator->() const +Target *UniquePtr<Target>::operator->() const { return this->_target; } template <class Target, typename... Args> -unique_ptr<Target> make_unique(Args... args) +UniquePtr<Target> make_unique(Args... args) { - return unique_ptr<Target>(new Target(args...)); + return UniquePtr<Target>(new Target(args...)); } diff --git a/src/utils/serial.cpp b/src/utils/serial.cpp index 11be550..8e4194f 100644 --- a/src/utils/serial.cpp +++ b/src/utils/serial.cpp @@ -2,20 +2,13 @@ #include "general.hpp" -SerialStream::SerialStream(Serial_ serial, unsigned long baud_rate) +SerialStream::SerialStream(Serial_ serial, uint64_t baud_rate) : _serial(serial) { - this->_serial = serial; - this->_serial.begin(baud_rate); while (!this->_serial) {} } -SerialStream::~SerialStream() -{ - Serial.end(); -} - SerialStream &SerialStream::operator<<(const char *str) { this->write(str); @@ -28,9 +21,27 @@ SerialStream &SerialStream::operator<<(const SmartString &str) return *this; } -SerialStream &SerialStream::operator<<(const float num) +SerialStream &SerialStream::operator<<(const double &num) +{ + this->write(doubleToStr(num)->c_str); + return *this; +} + +SerialStream &SerialStream::operator<<(const int &num) +{ + this->write(intToStr(num)->c_str); + return *this; +} + +SerialStream &SerialStream::operator<<(const unsigned int &num) +{ + this->write(uintToStr(num)->c_str); + return *this; +} + +SerialStream &SerialStream::operator<<(const uint32_t &num) { - this->write(floatToStr(num)->c_str); + this->write(uintToStr(num)->c_str); return *this; } diff --git a/src/utils/serial.hpp b/src/utils/serial.hpp index f97e1d8..0d4f929 100644 --- a/src/utils/serial.hpp +++ b/src/utils/serial.hpp @@ -7,12 +7,14 @@ class SerialStream { public: - SerialStream(Serial_ serial, unsigned long baud_rate); - ~SerialStream(); + SerialStream(Serial_ serial, uint64_t baud_rate); SerialStream &operator<<(const char *str); SerialStream &operator<<(const SmartString &str); - SerialStream &operator<<(const float num); + SerialStream &operator<<(const double &num); + SerialStream &operator<<(const int &num); + SerialStream &operator<<(const unsigned int &num); + SerialStream &operator<<(const uint32_t &num); SerialStream &operator<<(void (*manipulator)(SerialStream *)); diff --git a/src/utils/smart_string.cpp b/src/utils/smart_string.cpp index 5258ffe..ac9a862 100644 --- a/src/utils/smart_string.cpp +++ b/src/utils/smart_string.cpp @@ -4,23 +4,62 @@ #include <stdlib.h> -SmartString::SmartString(char *c_string) +SmartString::SmartString(char *c_string) : c_str(c_string) { - this->c_str = c_string; } -SmartString::SmartString(unsigned int size) +SmartString::SmartString(unsigned int size) : c_str(malloc_s<char>(size + 1)) { - this->c_str = malloc_s<char>(size + 1); +} + +SmartString::SmartString(const SmartString &smart_str) + : c_str(malloc_s<char>(strlen(smart_str.c_str) + 1)) +{ + memcpy(c_str, smart_str.c_str, strlen(smart_str.c_str) + 1); +} + +SmartString::SmartString(SmartString &&smart_str) noexcept : c_str(smart_str.c_str) +{ + smart_str.c_str = nullptr; +} + +SmartString &SmartString::operator=(const SmartString &smart_str) +{ + if (&smart_str != this) + { + free(c_str); + c_str = nullptr; + + auto str_size = strlen(smart_str.c_str) + 1; + + c_str = malloc_s<char>(str_size); + memcpy(c_str, smart_str.c_str, str_size); + } + + return *this; +} + +SmartString &SmartString::operator=(SmartString &&smart_str) noexcept +{ + if (&smart_str != this) + { + free(c_str); + c_str = smart_str.c_str; + smart_str.c_str = nullptr; + } + + return *this; } SmartString::~SmartString() { - if (this->c_str != nullptr) - free(this->c_str); + if (c_str != nullptr) + { + free(c_str); + } } SmartString::operator char *() const { - return this->c_str; + return c_str; } diff --git a/src/utils/smart_string.hpp b/src/utils/smart_string.hpp index fcaff98..6390465 100644 --- a/src/utils/smart_string.hpp +++ b/src/utils/smart_string.hpp @@ -3,12 +3,18 @@ class SmartString { public: - SmartString(char *c_str); - SmartString(unsigned int size); + explicit SmartString(char *c_str); + explicit SmartString(unsigned int size); + SmartString(const SmartString &smart_str); + SmartString(SmartString &&smart_str) noexcept; + + SmartString &operator=(const SmartString &smart_str); + + SmartString &operator=(SmartString &&smart_str) noexcept; ~SmartString(); - operator char *() const; + explicit operator char *() const; char *c_str = nullptr; }; diff --git a/src/utils/time.cpp b/src/utils/time.cpp index adc33db..c6d981c 100644 --- a/src/utils/time.cpp +++ b/src/utils/time.cpp @@ -1,9 +1,9 @@ #include "time.hpp" -#include "Arduino.h" -Time::Time(unsigned long time_micros) +#include <Arduino.h> + +Time::Time(uint64_t time_micros) : _time_micros(time_micros) { - _time_micros = time_micros; } void Time::update() @@ -16,19 +16,23 @@ Time Time::diff(Time prev_time) return Time(_time_micros - prev_time.microsecs()); } -unsigned long Time::microsecs() +double Time::secs() { - return _time_micros; + const double micros_to_secs = 0.000001; + + return static_cast<double>(_time_micros) * micros_to_secs; } -unsigned long Time::millisecs() +double Time::millisecs() { - return _time_micros * 0.001; + const double micros_to_millis = 0.001; + + return static_cast<double>(_time_micros) * micros_to_millis; } -float Time::secs() +uint64_t Time::microsecs() { - return _time_micros * 0.000001; + return _time_micros; } Time time_now() diff --git a/src/utils/time.hpp b/src/utils/time.hpp index e0385ef..1577bb7 100644 --- a/src/utils/time.hpp +++ b/src/utils/time.hpp @@ -1,5 +1,6 @@ -#ifndef TIME_HPP -#define TIME_HPP +#pragma once + +#include <stdint.h> /** * A representation of time. @@ -12,7 +13,7 @@ public: * * @param time_micros Time in microseconds */ - Time(unsigned long time_micros); + explicit Time(uint64_t time_micros); /** * Updates the time to the current time. @@ -29,25 +30,23 @@ public: /** * Returns the time in seconds. */ - float secs(); + double secs(); /** * Returns the time in milliseconds. */ - unsigned long millisecs(); + double millisecs(); /** * Returns the time in microseconds. */ - unsigned long microsecs(); + uint64_t microsecs(); private: - unsigned long _time_micros; + uint64_t _time_micros; }; /** * Returns a time object for the time since the program started. */ Time time_now(); - -#endif |