diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/gyronardo.cpp | 35 | ||||
| -rw-r--r-- | src/sensor.cpp | 79 | ||||
| -rw-r--r-- | src/sensor.hpp | 41 |
3 files changed, 149 insertions, 6 deletions
diff --git a/src/gyronardo.cpp b/src/gyronardo.cpp index 1a81850..025bb7d 100644 --- a/src/gyronardo.cpp +++ b/src/gyronardo.cpp @@ -4,6 +4,14 @@ Sensor sensor(0x68, &Wire); +/** + * Stops code execution. + */ +void stop() +{ + while (true) {} +} + void setup() { Serial.begin(9600); @@ -21,18 +29,33 @@ void setup() sensor.setGyroSensitivity(1); // 500 degrees/s sensor.setThrottleEnabled(true); - Serial.println("start..."); - // Calibration values - sensor.accel_cal_x = 0.198; - sensor.accel_cal_y = -0.018; - sensor.gyro_cal_x = 0.780; - sensor.gyro_cal_y = -1.495; + Serial.println("Automatically calibrating sensor..."); + bool cal_status = sensor.autoCalibrate(); + + if (!cal_status) + { + Serial.print("Error: Automatic calibration timed out after "); + Serial.print(CALIBRATION_TIMEOUT); + Serial.println(" milliseconds"); + + stop(); + } + + Serial.println("Finished calibrating"); + + Serial.println("Starting..."); } void loop() { + if (!sensor.is_calibrated) + { + return; + } + sensor.read(); + float pitch = sensor.getPitch(); float roll = sensor.getRoll(); diff --git a/src/sensor.cpp b/src/sensor.cpp index e3a2551..be57609 100644 --- a/src/sensor.cpp +++ b/src/sensor.cpp @@ -16,6 +16,8 @@ Sensor::Sensor(uint8_t address, TwoWire *wire) gyro_cal_y = 0; gyro_cal_z = 0; + is_calibrated = false; + _throttle_enabled = true; _throttleTime = DEFAULT_THROTTLE_TIME; @@ -284,6 +286,7 @@ uint8_t Sensor::getGyroSensitivity() float Sensor::getAccelX() { return _accel_raw_x; }; float Sensor::getAccelY() { return _accel_raw_y; }; +float Sensor::getAccelZ() { return _accel_raw_z; }; float Sensor::getAngleX() { return _accel_x; }; float Sensor::getAngleY() { return _accel_y; }; @@ -292,6 +295,7 @@ float Sensor::getTemperature() { return _temperature; }; float Sensor::getGyroX() { return _gyro_raw_x; }; float Sensor::getGyroY() { return _gyro_raw_y; }; +float Sensor::getGyroZ() { return _gyro_raw_z; }; float Sensor::getPitch() { return _pitch; }; float Sensor::getRoll() { return _roll; }; @@ -352,3 +356,78 @@ int16_t Sensor::_readTwoBytes() return response; } + +bool Sensor::autoCalibrate() +{ + bool done = false; + + float accel_x, accel_y, accel_z; + float gyro_x, gyro_y, gyro_z; + + auto cal_start_time = millis(); + + while (!done) + { + if (millis() - cal_start_time >= CALIBRATION_TIMEOUT) + { + return false; + } + + delay(100); + + accel_x = 0; + accel_y = 0; + accel_z = 0; + + gyro_x = 0; + gyro_y = 0; + gyro_z = 0; + + for (int i = 0; i < 20; i++) + { + read(); + + accel_x -= getAccelX(); + accel_y -= getAccelY(); + accel_z -= getAccelZ(); + + gyro_x -= getGyroX(); + gyro_y -= getGyroY(); + gyro_z -= getGyroZ(); + } + + const float adjust_val = 0.05; + + accel_x *= adjust_val; + accel_y *= adjust_val; + accel_z *= adjust_val; + + gyro_x *= adjust_val; + gyro_y *= adjust_val; + gyro_z *= adjust_val; + + if ( + accel_x < ACCEL_CAL_X_MAX && accel_x > ACCEL_CAL_X_MIN && + accel_y < ACCEL_CAL_Y_MAX && accel_y > ACCEL_CAL_Y_MIN && + accel_z < ACCEL_CAL_Z_MAX && accel_z > ACCEL_CAL_Z_MIN && + gyro_x < GYRO_CAL_X_MAX && gyro_x > GYRO_CAL_X_MIN && + gyro_y < GYRO_CAL_Y_MAX && gyro_y > GYRO_CAL_Y_MIN && + gyro_z < GYRO_CAL_Z_MAX && gyro_z > GYRO_CAL_Z_MIN) + { + done = true; + } + + // Adjust calibration values + accel_cal_x += accel_x; + accel_cal_y += accel_y; + accel_cal_z += accel_z; + + gyro_cal_x += gyro_x; + gyro_cal_y += gyro_y; + gyro_cal_z += gyro_z; + } + + is_calibrated = true; + + return true; +} diff --git a/src/sensor.hpp b/src/sensor.hpp index 7ba01bf..3fbc544 100644 --- a/src/sensor.hpp +++ b/src/sensor.hpp @@ -19,6 +19,27 @@ #define RAW_TO_DPS_FACTOR (1.0 / 131.0) #define RAW_TO_G_FACTOR (1.0 / 16384.0) +// Automatic calibration precision +#define ACCEL_CAL_X_MAX 0.002 +#define ACCEL_CAL_X_MIN -0.002 + +#define ACCEL_CAL_Y_MAX 0.002 +#define ACCEL_CAL_Y_MIN -0.002 + +#define ACCEL_CAL_Z_MAX 0.002 +#define ACCEL_CAL_Z_MIN -0.002 + +#define GYRO_CAL_X_MAX 0.2 +#define GYRO_CAL_X_MIN -0.2 + +#define GYRO_CAL_Y_MAX 0.02 +#define GYRO_CAL_Y_MIN -0.02 + +#define GYRO_CAL_Z_MAX 0.08 +#define GYRO_CAL_Z_MIN -0.08 + +#define CALIBRATION_TIMEOUT 120000 // Milliseconds + class Sensor { public: @@ -114,6 +135,11 @@ public: float getAccelY(); /** + * Returns the current Z axis acceleration in g:s (g-force). + */ + float getAccelZ(); + + /** * Returns the current X angle. */ float getAngleX(); @@ -139,6 +165,11 @@ public: float getGyroY(); /** + * Returns the current Z axis in degrees/s. + */ + float getGyroZ(); + + /** * Returns the current pitch angle. */ float getPitch(); @@ -174,6 +205,16 @@ public: */ int16_t getStatus(); + /** + * Automatically calibrates the sensor. + * + * @returns Whether or not the calibration succeeded. + * Will return false if it times out. + */ + bool autoCalibrate(); + + bool is_calibrated; + // Accelerometer calibration values float accel_cal_x; float accel_cal_y; |