summaryrefslogtreecommitdiff
path: root/src/sensor
diff options
context:
space:
mode:
Diffstat (limited to 'src/sensor')
-rw-r--r--src/sensor/registers.hpp267
-rw-r--r--src/sensor/sensor.cpp174
-rw-r--r--src/sensor/sensor.hpp87
3 files changed, 259 insertions, 269 deletions
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
generated by cgit v1.2.3-18-g5258 (git 2.32.0) at 2024-11-22 00:54:19 +0000