diff --git a/Examples/Advanced/main.c b/Examples/Advanced/main.c index d5732c0..5ca8ce0 100644 --- a/Examples/Advanced/main.c +++ b/Examples/Advanced/main.c @@ -30,7 +30,7 @@ int main() { .gain = 0.5f, .accelerationRejection = 10.0f, .magneticRejection = 20.0f, - .rejectionTimeout = 5 * SAMPLE_RATE, /* 5 seconds */ + .recoveryTriggerPeriod = 5 * SAMPLE_RATE, /* 5 seconds */ }; FusionAhrsSetSettings(&ahrs, &settings); diff --git a/Fusion/FusionAhrs.c b/Fusion/FusionAhrs.c index 3572543..fa74190 100644 --- a/Fusion/FusionAhrs.c +++ b/Fusion/FusionAhrs.c @@ -10,7 +10,6 @@ #include // FLT_MAX #include "FusionAhrs.h" -#include "FusionCompass.h" #include // atan2f, cosf, powf, sinf //------------------------------------------------------------------------------ @@ -29,11 +28,13 @@ //------------------------------------------------------------------------------ // Function declarations -static FusionVector HalfGravity(const FusionAhrs *const ahrs); +static inline FusionVector HalfGravity(const FusionAhrs *const ahrs); -static FusionVector HalfMagnetic(const FusionAhrs *const ahrs); +static inline FusionVector HalfMagnetic(const FusionAhrs *const ahrs); -static FusionVector Feedback(const FusionVector sensor, const FusionVector reference); +static inline FusionVector Feedback(const FusionVector sensor, const FusionVector reference); + +static inline int Clamp(const int value, const int min, const int max); //------------------------------------------------------------------------------ // Functions @@ -48,7 +49,7 @@ void FusionAhrsInitialise(FusionAhrs *const ahrs) { .gain = 0.5f, .accelerationRejection = 90.0f, .magneticRejection = 90.0f, - .rejectionTimeout = 0, + .recoveryTriggerPeriod = 0, }; FusionAhrsSetSettings(ahrs, &settings); FusionAhrsReset(ahrs); @@ -67,11 +68,11 @@ void FusionAhrsReset(FusionAhrs *const ahrs) { ahrs->halfAccelerometerFeedback = FUSION_VECTOR_ZERO; ahrs->halfMagnetometerFeedback = FUSION_VECTOR_ZERO; ahrs->accelerometerIgnored = false; - ahrs->accelerationRejectionTimer = 0; - ahrs->accelerationRejectionTimeout = false; + ahrs->accelerationRecoveryTrigger = 0; + ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; ahrs->magnetometerIgnored = false; - ahrs->magneticRejectionTimer = 0; - ahrs->magneticRejectionTimeout = false; + ahrs->magneticRecoveryTrigger = 0; + ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; } /** @@ -84,8 +85,10 @@ void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *con ahrs->settings.gain = settings->gain; ahrs->settings.accelerationRejection = settings->accelerationRejection == 0.0f ? FLT_MAX : powf(0.5f * sinf(FusionDegreesToRadians(settings->accelerationRejection)), 2); ahrs->settings.magneticRejection = settings->magneticRejection == 0.0f ? FLT_MAX : powf(0.5f * sinf(FusionDegreesToRadians(settings->magneticRejection)), 2); - ahrs->settings.rejectionTimeout = settings->rejectionTimeout; - if ((settings->gain == 0.0f) || (settings->rejectionTimeout == 0)) { + ahrs->settings.recoveryTriggerPeriod = settings->recoveryTriggerPeriod; + ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; + ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; + if ((settings->gain == 0.0f) || (settings->recoveryTriggerPeriod == 0)) { ahrs->settings.accelerationRejection = FLT_MAX; ahrs->settings.magneticRejection = FLT_MAX; } @@ -116,7 +119,6 @@ void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, cons if ((ahrs->rampedGain < ahrs->settings.gain) || (ahrs->settings.gain == 0.0f)) { ahrs->rampedGain = ahrs->settings.gain; ahrs->initialising = false; - ahrs->accelerationRejectionTimeout = false; } } @@ -128,25 +130,29 @@ void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, cons ahrs->accelerometerIgnored = true; if (FusionVectorIsZero(accelerometer) == false) { - // Enter acceleration recovery state if acceleration rejection times out - if (ahrs->accelerationRejectionTimer > ahrs->settings.rejectionTimeout) { - const FusionQuaternion quaternion = ahrs->quaternion; - FusionAhrsReset(ahrs); - ahrs->quaternion = quaternion; - ahrs->accelerationRejectionTimer = 0; - ahrs->accelerationRejectionTimeout = true; - } - // Calculate accelerometer feedback scaled by 0.5 ahrs->halfAccelerometerFeedback = Feedback(FusionVectorNormalise(accelerometer), halfGravity); - // Ignore accelerometer if acceleration distortion detected - if ((ahrs->initialising == true) || (FusionVectorMagnitudeSquared(ahrs->halfAccelerometerFeedback) <= ahrs->settings.accelerationRejection)) { - halfAccelerometerFeedback = ahrs->halfAccelerometerFeedback; + // Don't ignore accelerometer if acceleration error below threshold + if ((ahrs->initialising == true) || ((FusionVectorMagnitudeSquared(ahrs->halfAccelerometerFeedback) <= ahrs->settings.accelerationRejection))) { + ahrs->accelerometerIgnored = false; + ahrs->accelerationRecoveryTrigger -= 9; + } else { + ahrs->accelerationRecoveryTrigger += 1; + } + + // Don't ignore accelerometer during acceleration recovery + if (ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout) { + ahrs->accelerationRecoveryTimeout = 0; ahrs->accelerometerIgnored = false; - ahrs->accelerationRejectionTimer -= ahrs->accelerationRejectionTimer >= 10 ? 10 : 0; } else { - ahrs->accelerationRejectionTimer++; + ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; + } + ahrs->accelerationRecoveryTrigger = Clamp(ahrs->accelerationRecoveryTrigger, 0, ahrs->settings.recoveryTriggerPeriod); + + // Apply accelerometer feedback + if (ahrs->accelerometerIgnored == false) { + halfAccelerometerFeedback = ahrs->halfAccelerometerFeedback; } } @@ -155,27 +161,32 @@ void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, cons ahrs->magnetometerIgnored = true; if (FusionVectorIsZero(magnetometer) == false) { - // Set to compass heading if magnetic rejection times out - ahrs->magneticRejectionTimeout = false; - if (ahrs->magneticRejectionTimer > ahrs->settings.rejectionTimeout) { - FusionAhrsSetHeading(ahrs, FusionCompassCalculateHeading(ahrs->settings.convention, halfGravity, magnetometer)); - ahrs->magneticRejectionTimer = 0; - ahrs->magneticRejectionTimeout = true; - } - // Calculate direction of magnetic field indicated by algorithm const FusionVector halfMagnetic = HalfMagnetic(ahrs); // Calculate magnetometer feedback scaled by 0.5 ahrs->halfMagnetometerFeedback = Feedback(FusionVectorNormalise(FusionVectorCrossProduct(halfGravity, magnetometer)), halfMagnetic); - // Ignore magnetometer if magnetic distortion detected - if ((ahrs->initialising == true) || (FusionVectorMagnitudeSquared(ahrs->halfMagnetometerFeedback) <= ahrs->settings.magneticRejection)) { - halfMagnetometerFeedback = ahrs->halfMagnetometerFeedback; + // Don't ignore magnetometer if magnetic error below threshold + if ((ahrs->initialising == true) || ((FusionVectorMagnitudeSquared(ahrs->halfMagnetometerFeedback) <= ahrs->settings.magneticRejection))) { ahrs->magnetometerIgnored = false; - ahrs->magneticRejectionTimer -= ahrs->magneticRejectionTimer >= 10 ? 10 : 0; + ahrs->magneticRecoveryTrigger -= 9; } else { - ahrs->magneticRejectionTimer++; + ahrs->magneticRecoveryTrigger += 1; + } + + // Don't ignore magnetometer during magnetic recovery + if (ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout) { + ahrs->magneticRecoveryTimeout = 0; + ahrs->magnetometerIgnored = false; + } else { + ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; + } + ahrs->magneticRecoveryTrigger = Clamp(ahrs->magneticRecoveryTrigger, 0, ahrs->settings.recoveryTriggerPeriod); + + // Apply magnetometer feedback + if (ahrs->magnetometerIgnored == false) { + halfMagnetometerFeedback = ahrs->halfMagnetometerFeedback; } } @@ -198,7 +209,7 @@ void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, cons * @param ahrs AHRS algorithm structure. * @return Direction of gravity scaled by 0.5. */ -static FusionVector HalfGravity(const FusionAhrs *const ahrs) { +static inline FusionVector HalfGravity(const FusionAhrs *const ahrs) { #define Q ahrs->quaternion.element switch (ahrs->settings.convention) { case FusionConventionNwu: @@ -228,7 +239,7 @@ static FusionVector HalfGravity(const FusionAhrs *const ahrs) { * @param ahrs AHRS algorithm structure. * @return Direction of the magnetic field scaled by 0.5. */ -static FusionVector HalfMagnetic(const FusionAhrs *const ahrs) { +static inline FusionVector HalfMagnetic(const FusionAhrs *const ahrs) { #define Q ahrs->quaternion.element switch (ahrs->settings.convention) { case FusionConventionNwu: { @@ -266,13 +277,30 @@ static FusionVector HalfMagnetic(const FusionAhrs *const ahrs) { * @param reference Reference. * @return Feedback. */ -static FusionVector Feedback(const FusionVector sensor, const FusionVector reference) { +static inline FusionVector Feedback(const FusionVector sensor, const FusionVector reference) { if (FusionVectorDotProduct(sensor, reference) < 0.0f) { // if error is >90 degrees return FusionVectorNormalise(FusionVectorCrossProduct(sensor, reference)); } return FusionVectorCrossProduct(sensor, reference); } +/** + * @brief Returns a value limited to maximum and minimum. + * @param value Value. + * @param min Minimum value. + * @param max Maximum value. + * @return Value limited to maximum and minimum. + */ +static inline int Clamp(const int value, const int min, const int max) { + if (value < min) { + return min; + } + if (value > max) { + return max; + } + return value; +} + /** * @brief Updates the AHRS algorithm using the gyroscope and accelerometer * measurements only. @@ -287,7 +315,7 @@ void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector g FusionAhrsUpdate(ahrs, gyroscope, accelerometer, FUSION_VECTOR_ZERO, deltaTime); // Zero heading during initialisation - if ((ahrs->initialising == true) && (ahrs->accelerationRejectionTimeout == false)) { + if (ahrs->initialising == true) { FusionAhrsSetHeading(ahrs, 0.0f); } } @@ -417,10 +445,10 @@ FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahr const FusionAhrsInternalStates internalStates = { .accelerationError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfAccelerometerFeedback))), .accelerometerIgnored = ahrs->accelerometerIgnored, - .accelerationRejectionTimer = ahrs->settings.rejectionTimeout == 0 ? 0.0f : (float) ahrs->accelerationRejectionTimer / (float) ahrs->settings.rejectionTimeout, + .accelerationRecoveryTrigger = ahrs->settings.recoveryTriggerPeriod == 0 ? 0.0f : (float) ahrs->accelerationRecoveryTrigger / (float) ahrs->settings.recoveryTriggerPeriod, .magneticError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfMagnetometerFeedback))), .magnetometerIgnored = ahrs->magnetometerIgnored, - .magneticRejectionTimer = ahrs->settings.rejectionTimeout == 0 ? 0.0f : (float) ahrs->magneticRejectionTimer / (float) ahrs->settings.rejectionTimeout, + .magneticRecoveryTrigger = ahrs->settings.recoveryTriggerPeriod == 0 ? 0.0f : (float) ahrs->magneticRecoveryTrigger / (float) ahrs->settings.recoveryTriggerPeriod, }; return internalStates; } @@ -431,13 +459,10 @@ FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahr * @return AHRS algorithm flags. */ FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs) { - const unsigned int warningTimeout = ahrs->settings.rejectionTimeout / 4; const FusionAhrsFlags flags = { .initialising = ahrs->initialising, - .accelerationRejectionWarning = ahrs->accelerationRejectionTimer > warningTimeout, - .accelerationRejectionTimeout = ahrs->accelerationRejectionTimeout, - .magneticRejectionWarning = ahrs->magneticRejectionTimer > warningTimeout, - .magneticRejectionTimeout = ahrs->magneticRejectionTimeout, + .accelerationRecovery = ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout, + .magneticRecovery= ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout, }; return flags; } diff --git a/Fusion/FusionAhrs.h b/Fusion/FusionAhrs.h index 179daa7..419fc4c 100644 --- a/Fusion/FusionAhrs.h +++ b/Fusion/FusionAhrs.h @@ -26,7 +26,7 @@ typedef struct { float gain; float accelerationRejection; float magneticRejection; - unsigned int rejectionTimeout; + unsigned int recoveryTriggerPeriod; } FusionAhrsSettings; /** @@ -43,11 +43,11 @@ typedef struct { FusionVector halfAccelerometerFeedback; FusionVector halfMagnetometerFeedback; bool accelerometerIgnored; - unsigned int accelerationRejectionTimer; - bool accelerationRejectionTimeout; + int accelerationRecoveryTrigger; + int accelerationRecoveryTimeout; bool magnetometerIgnored; - unsigned int magneticRejectionTimer; - bool magneticRejectionTimeout; + int magneticRecoveryTrigger; + int magneticRecoveryTimeout; } FusionAhrs; /** @@ -56,10 +56,10 @@ typedef struct { typedef struct { float accelerationError; bool accelerometerIgnored; - float accelerationRejectionTimer; + float accelerationRecoveryTrigger; float magneticError; bool magnetometerIgnored; - float magneticRejectionTimer; + float magneticRecoveryTrigger; } FusionAhrsInternalStates; /** @@ -67,10 +67,8 @@ typedef struct { */ typedef struct { bool initialising; - bool accelerationRejectionWarning; - bool accelerationRejectionTimeout; - bool magneticRejectionWarning; - bool magneticRejectionTimeout; + bool accelerationRecovery; + bool magneticRecovery; } FusionAhrsFlags; //------------------------------------------------------------------------------ diff --git a/Python/Python-C-API/Flags.h b/Python/Python-C-API/Flags.h index f92b61e..bbf001b 100644 --- a/Python/Python-C-API/Flags.h +++ b/Python/Python-C-API/Flags.h @@ -18,28 +18,18 @@ static PyObject *flags_get_initialising(Flags *self) { return build_bool(self->flags.initialising); } -static PyObject *flags_get_acceleration_rejection_warning(Flags *self) { - return build_bool(self->flags.accelerationRejectionWarning); +static PyObject *flags_get_acceleration_recovery(Flags *self) { + return build_bool(self->flags.accelerationRecovery); } -static PyObject *flags_get_acceleration_rejection_timeout(Flags *self) { - return build_bool(self->flags.accelerationRejectionTimeout); -} - -static PyObject *flags_get_magnetic_rejection_warning(Flags *self) { - return build_bool(self->flags.magneticRejectionWarning); -} - -static PyObject *flags_get_magnetic_rejection_timeout(Flags *self) { - return build_bool(self->flags.magneticRejectionTimeout); +static PyObject *flags_get_magnetic_recovery(Flags *self) { + return build_bool(self->flags.magneticRecovery); } static PyGetSetDef flags_get_set[] = { - {"initialising", (getter) flags_get_initialising, NULL, "", NULL}, - {"acceleration_rejection_warning", (getter) flags_get_acceleration_rejection_warning, NULL, "", NULL}, - {"acceleration_rejection_timeout", (getter) flags_get_acceleration_rejection_timeout, NULL, "", NULL}, - {"magnetic_rejection_warning", (getter) flags_get_magnetic_rejection_warning, NULL, "", NULL}, - {"magnetic_rejection_timeout", (getter) flags_get_magnetic_rejection_timeout, NULL, "", NULL}, + {"initialising", (getter) flags_get_initialising, NULL, "", NULL}, + {"acceleration_recovery", (getter) flags_get_acceleration_recovery, NULL, "", NULL}, + {"magnetic_recovery", (getter) flags_get_magnetic_recovery, NULL, "", NULL}, {NULL} /* sentinel */ }; diff --git a/Python/Python-C-API/InternalStates.h b/Python/Python-C-API/InternalStates.h index 6b70884..a028323 100644 --- a/Python/Python-C-API/InternalStates.h +++ b/Python/Python-C-API/InternalStates.h @@ -22,8 +22,8 @@ static PyObject *internal_states_get_accelerometer_ignored(InternalStates *self) return build_bool(self->internal_states.accelerometerIgnored); } -static PyObject *internal_states_get_acceleration_rejection_timer(InternalStates *self) { - return Py_BuildValue("f", self->internal_states.accelerationRejectionTimer); +static PyObject *internal_states_get_acceleration_recovery_trigger(InternalStates *self) { + return Py_BuildValue("f", self->internal_states.accelerationRecoveryTrigger); } static PyObject *internal_states_get_magnetic_error(InternalStates *self) { @@ -34,17 +34,17 @@ static PyObject *internal_states_get_magnetometer_ignored(InternalStates *self) return build_bool(self->internal_states.magnetometerIgnored); } -static PyObject *internal_states_get_magnetic_rejection_timer(InternalStates *self) { - return Py_BuildValue("f", self->internal_states.magneticRejectionTimer); +static PyObject *internal_states_get_magnetic_recovery_trigger(InternalStates *self) { + return Py_BuildValue("f", self->internal_states.magneticRecoveryTrigger); } static PyGetSetDef internal_states_get_set[] = { - {"acceleration_error", (getter) internal_states_get_acceleration_error, NULL, "", NULL}, - {"accelerometer_ignored", (getter) internal_states_get_accelerometer_ignored, NULL, "", NULL}, - {"acceleration_rejection_timer", (getter) internal_states_get_acceleration_rejection_timer, NULL, "", NULL}, - {"magnetic_error", (getter) internal_states_get_magnetic_error, NULL, "", NULL}, - {"magnetometer_ignored", (getter) internal_states_get_magnetometer_ignored, NULL, "", NULL}, - {"magnetic_rejection_timer", (getter) internal_states_get_magnetic_rejection_timer, NULL, "", NULL}, + {"acceleration_error", (getter) internal_states_get_acceleration_error, NULL, "", NULL}, + {"accelerometer_ignored", (getter) internal_states_get_accelerometer_ignored, NULL, "", NULL}, + {"acceleration_recovery_trigger", (getter) internal_states_get_acceleration_recovery_trigger, NULL, "", NULL}, + {"magnetic_error", (getter) internal_states_get_magnetic_error, NULL, "", NULL}, + {"magnetometer_ignored", (getter) internal_states_get_magnetometer_ignored, NULL, "", NULL}, + {"magnetic_recovery_trigger", (getter) internal_states_get_magnetic_recovery_trigger, NULL, "", NULL}, {NULL} /* sentinel */ }; diff --git a/Python/Python-C-API/Settings.h b/Python/Python-C-API/Settings.h index 934caee..c7f1c2d 100644 --- a/Python/Python-C-API/Settings.h +++ b/Python/Python-C-API/Settings.h @@ -13,7 +13,7 @@ typedef struct { static PyObject *settings_new(PyTypeObject *subtype, PyObject *args, PyObject *keywords) { Settings *const self = (Settings *) subtype->tp_alloc(subtype, 0); - const char *const error = PARSE_TUPLE(args, "ifffI", &self->settings.convention, &self->settings.gain, &self->settings.accelerationRejection, &self->settings.magneticRejection, &self->settings.rejectionTimeout); + const char *const error = PARSE_TUPLE(args, "ifffI", &self->settings.convention, &self->settings.gain, &self->settings.accelerationRejection, &self->settings.magneticRejection, &self->settings.recoveryTriggerPeriod); if (error != NULL) { PyErr_SetString(PyExc_TypeError, error); return NULL; @@ -85,27 +85,27 @@ static int settings_set_magnetic_rejection(Settings *self, PyObject *value, void return 0; } -static PyObject *settings_get_rejection_timeout(Settings *self) { - return Py_BuildValue("I", self->settings.rejectionTimeout); +static PyObject *settings_get_recovery_trigger_period(Settings *self) { + return Py_BuildValue("I", self->settings.recoveryTriggerPeriod); } -static int settings_set_rejection_timeout(Settings *self, PyObject *value, void *closure) { - const unsigned int rejection_timeout = (unsigned int) PyFloat_AsDouble(value); +static int settings_set_recovery_trigger_period(Settings *self, PyObject *value, void *closure) { + const unsigned int recovery_trigger_period = (unsigned int) PyFloat_AsDouble(value); if (PyErr_Occurred()) { return -1; } - self->settings.rejectionTimeout = rejection_timeout; + self->settings.recoveryTriggerPeriod = recovery_trigger_period; return 0; } static PyGetSetDef settings_get_set[] = { - {"convention", (getter) settings_get_convention, (setter) settings_set_convention, "", NULL}, - {"gain", (getter) settings_get_gain, (setter) settings_set_gain, "", NULL}, - {"acceleration_rejection", (getter) settings_get_acceleration_rejection, (setter) settings_set_acceleration_rejection, "", NULL}, - {"magnetic_rejection", (getter) settings_get_magnetic_rejection, (setter) settings_set_magnetic_rejection, "", NULL}, - {"rejection_timeout", (getter) settings_get_rejection_timeout, (setter) settings_set_rejection_timeout, "", NULL}, + {"convention", (getter) settings_get_convention, (setter) settings_set_convention, "", NULL}, + {"gain", (getter) settings_get_gain, (setter) settings_set_gain, "", NULL}, + {"acceleration_rejection", (getter) settings_get_acceleration_rejection, (setter) settings_set_acceleration_rejection, "", NULL}, + {"magnetic_rejection", (getter) settings_get_magnetic_rejection, (setter) settings_set_magnetic_rejection, "", NULL}, + {"recovery_trigger_period", (getter) settings_get_recovery_trigger_period, (setter) settings_set_recovery_trigger_period, "", NULL}, {NULL} /* sentinel */ }; diff --git a/Python/advanced_example.py b/Python/advanced_example.py index 1db8d9e..5e0d145 100644 --- a/Python/advanced_example.py +++ b/Python/advanced_example.py @@ -21,14 +21,14 @@ 0.5, # gain 10, # acceleration rejection 20, # magnetic rejection - 5 * sample_rate) # rejection timeout = 5 seconds + 5 * sample_rate) # recovery trigger period = 5 seconds # Process sensor data delta_time = numpy.diff(timestamp, prepend=timestamp[0]) euler = numpy.empty((len(timestamp), 3)) internal_states = numpy.empty((len(timestamp), 6)) -flags = numpy.empty((len(timestamp), 5)) +flags = numpy.empty((len(timestamp), 3)) for index in range(len(timestamp)): gyroscope[index] = offset.update(gyroscope[index]) @@ -40,17 +40,15 @@ ahrs_internal_states = ahrs.internal_states internal_states[index] = numpy.array([ahrs_internal_states.acceleration_error, ahrs_internal_states.accelerometer_ignored, - ahrs_internal_states.acceleration_rejection_timer, + ahrs_internal_states.acceleration_recovery_trigger, ahrs_internal_states.magnetic_error, ahrs_internal_states.magnetometer_ignored, - ahrs_internal_states.magnetic_rejection_timer]) + ahrs_internal_states.magnetic_recovery_trigger]) ahrs_flags = ahrs.flags flags[index] = numpy.array([ahrs_flags.initialising, - ahrs_flags.acceleration_rejection_warning, - ahrs_flags.acceleration_rejection_timeout, - ahrs_flags.magnetic_rejection_warning, - ahrs_flags.magnetic_rejection_timeout]) + ahrs_flags.acceleration_recovery, + ahrs_flags.magnetic_recovery]) def plot_bool(axis, x, y, label): @@ -62,7 +60,7 @@ def plot_bool(axis, x, y, label): # Plot Euler angles -figure, axes = pyplot.subplots(nrows=12, sharex=True, gridspec_kw={"height_ratios": [6, 1, 2, 1, 1, 1, 1, 2, 1, 1, 1, 1]}) +figure, axes = pyplot.subplots(nrows=10, sharex=True, gridspec_kw={"height_ratios": [6, 1, 2, 1, 1, 1, 2, 1, 1, 1]}) figure.suptitle("Euler angles, internal states, and flags") @@ -84,27 +82,25 @@ def plot_bool(axis, x, y, label): plot_bool(axes[3], timestamp, internal_states[:, 1], "Accelerometer ignored") -axes[4].plot(timestamp, internal_states[:, 2], "tab:orange", label="Acceleration rejection timer") +axes[4].plot(timestamp, internal_states[:, 2], "tab:orange", label="Acceleration recovery trigger") axes[4].grid() axes[4].legend() -plot_bool(axes[5], timestamp, flags[:, 1], "Acceleration rejection warning") -plot_bool(axes[6], timestamp, flags[:, 2], "Acceleration rejection timeout") +plot_bool(axes[5], timestamp, flags[:, 1], "Acceleration recovery") # Plot magnetic rejection internal states and flags -axes[7].plot(timestamp, internal_states[:, 3], "tab:olive", label="Magnetic error") -axes[7].set_ylabel("Degrees") -axes[7].grid() -axes[7].legend() +axes[6].plot(timestamp, internal_states[:, 3], "tab:olive", label="Magnetic error") +axes[6].set_ylabel("Degrees") +axes[6].grid() +axes[6].legend() -plot_bool(axes[8], timestamp, internal_states[:, 4], "Magnetometer ignored") +plot_bool(axes[7], timestamp, internal_states[:, 4], "Magnetometer ignored") -axes[9].plot(timestamp, internal_states[:, 5], "tab:orange", label="Magnetic rejection timer") -axes[9].grid() -axes[9].legend() +axes[8].plot(timestamp, internal_states[:, 5], "tab:orange", label="Magnetic recovery trigger") +axes[8].grid() +axes[8].legend() -plot_bool(axes[10], timestamp, flags[:, 3], "Magnetic rejection warning") -plot_bool(axes[11], timestamp, flags[:, 4], "Magnetic rejection timeout") +plot_bool(axes[9], timestamp, flags[:, 2], "Magnetic recovery") if len(sys.argv) == 1: # don't show plots when script run by CI pyplot.show() diff --git a/README.md b/README.md index 9eed811..9af1d76 100644 --- a/README.md +++ b/README.md @@ -18,17 +18,17 @@ The algorithm calculates the orientation as the integration of the gyroscope sum ### Initialisation -Initialisation occurs when the algorithm starts for the first time and after an acceleration rejection timeout. During initialisation, the acceleration and magnetic rejection features are disabled and the gain is ramped down from 10 to the final value over a 3 second period. This allows the measurement of orientation to rapidly converges from an arbitrary initial value to the value indicated by the sensors. +Initialisation occurs when the algorithm starts for the first time. During initialisation, the acceleration and magnetic rejection features are disabled and the gain is ramped down from 10 to the final value over a 3 second period. This allows the measurement of orientation to rapidly converges from an arbitrary initial value to the value indicated by the sensors. The algorithm outputs should be regarded as unreliable during initialisation. ### Acceleration rejection -The acceleration rejection feature reduces the errors that result from the accelerations of linear and rotational motion. Acceleration rejection works by comparing the instantaneous measurement of inclination provided by the accelerometer with the current measurement of inclination indicated by the algorithm output. If the angular difference between these two inclinations is greater than a threshold then the accelerometer will be ignored for this algorithm update. This is equivalent to a dynamic gain that deceases as accelerations increase. +The acceleration rejection feature reduces the errors that result from the accelerations of linear and rotational motion. Acceleration rejection works by calculating an error as the angular difference between the instantaneous measurement of inclination indicated by the accelerometer, and the current measurement of inclination provided by the algorithm output. If the error is greater than a threshold then the accelerometer will be ignored for that algorithm update. This is equivalent to a dynamic gain that deceases as accelerations increase. -Prolonged accelerations from linear and rotational motion may result in the accelerometer being unusable as a measurement of inclination. This is detected by the algorithm as an acceleration rejection timeout. An acceleration rejection timeout occurs when the number of algorithm updates that have ignored the accelerometer exceeds ten times the number that have used the accelerometer within a defined period. If an acceleration rejection timeout occurs then the algorithm will reinitialise. +Prolonged accelerations risk an overdependency on the gyroscope and will trigger an acceleration recovery. Acceleration recovery activates when the error exceeds the threshold for more than 90% of algorithm updates over a period of *t / (0.1p - 9)*, where *t* is the recovery trigger period and *p* is the percentage of algorithm updates where the error exceeds the threshold. The recovery will remain active until the error exceeds the threshold for less than 90% of algorithm updates over the period *-t / (0.1p - 9)*. The accelerometer will be used by every algorithm update during recovery. ### Magnetic rejection -The magnetic rejection feature reduces the errors that result from temporary magnetic distortions. Magnetic rejection works using the same principle as acceleration rejection operating on the magnetometer instead of the accelerometer and by comparing the measurements of heading instead of inclination. A magnetic rejection timeout will not cause the algorithm to reinitialise. If a magnetic rejection timeout occurs then the heading of the algorithm output will be set to the instantaneous measurement of heading provided by the magnetometer. +The magnetic rejection feature reduces the errors that result from temporary magnetic distortions. Magnetic rejection works using the same principle as acceleration rejection, operating on the magnetometer instead of the accelerometer and by comparing the measurements of heading instead of inclination. ### Algorithm outputs @@ -43,39 +43,37 @@ The AHRS algorithm settings are defined by the `FusionAhrsSettings` structure an | `convention` | Earth axes convention (NWD, ENU, or NED). | | `gain` | Determines the influence of the gyroscope relative to other sensors. A value of zero will disable initialisation and the acceleration and magnetic rejection features. A value of 0.5 is appropriate for most applications. | | `accelerationRejection` | Threshold (in degrees) used by the acceleration rejection feature. A value of zero will disable this feature. A value of 10 degrees is appropriate for most applications. | -| `magneticRejection` | Threshold (in degrees) used by the magnetic rejection feature. A value of zero will disable the feature. A value of 20 degrees is appropriate for most applications. | -| `rejectionTimeout` | Acceleration and magnetic rejection timeout period (in samples). A value of zero will disable the acceleration and magnetic rejection features. A period of 5 seconds is appropriate for most applications. | +| `magneticRejection` | Threshold (in degrees) used by the magnetic rejection feature. A value of zero will disable the feature. A value of 10 degrees is appropriate for most applications. | +| `recoveryTriggerPeriod` | Acceleration and magnetic recovery trigger period (in samples). A value of zero will disable the acceleration and magnetic rejection features. A period of 5 seconds is appropriate for most applications. | ### Algorithm internal states The AHRS algorithm internal states are defined by the `FusionAhrsInternalStates` structure and obtained using the `FusionAhrsGetInternalStates` function. -| State | Description | -|------------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| `accelerationError` | Angular error (in degrees) of the instantaneous measurement of inclination provided by the accelerometer. The acceleration rejection feature will ignore the accelerometer if this value exceeds the `accelerationRejection` threshold set in the algorithm settings. | -| `accelerometerIgnored` | `true` if the accelerometer was ignored by the previous algorithm update. | -| `accelerationRejectionTimer` | Acceleration rejection timer value normalised to between 0.0 and 1.0. An acceleration rejection timeout will occur when this value reaches 1.0. | -| `magneticError` | Angular error (in degrees) of the instantaneous measurement of heading provided by the magnetometer. The magnetic rejection feature will ignore the magnetometer if this value exceeds the `magneticRejection` threshold set in the algorithm settings. | -| `magnetometerIgnored` | `true` if the magnetometer was ignored by the previous algorithm update. | -| `magneticRejectionTimer` | Magnetic rejection timer value normalised to between 0.0 and 1.0. A magnetic rejection timeout will occur when this value reaches 1.0. | +| State | Description | +|-------------------------------|----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| `accelerationError` | Angular error (in degrees) of the algorithm output relative to the instantaneous measurement of inclination indicated by the accelerometer. The acceleration rejection feature will ignore the accelerometer if this value exceeds the `accelerationRejection` threshold set in the algorithm settings. | +| `accelerometerIgnored` | `true` if the accelerometer was ignored by the previous algorithm update. | +| `accelerationRecoveryTrigger` | Acceleration recovery trigger value between 0.0 and 1.0. Acceleration recovery will activate when this value reaches 1.0 and then deactivate when when the value reaches 0.0. | +| `magneticError` | Angular error (in degrees) of the algorithm output relative to the instantaneous measurement of heading indicated by the magnetometer. The magnetic rejection feature will ignore the magnetometer if this value exceeds the `magneticRejection` threshold set in the algorithm settings. | +| `magnetometerIgnored` | `true` if the magnetometer was ignored by the previous algorithm update. | +| `magneticRecoveryTrigger` | Magnetic recovery trigger value between 0.0 and 1.0. Magnetic recovery will activate when this value reaches 1.0 and then deactivate when when the value reaches 0.0. | ### Algorithm flags The AHRS algorithm flags are defined by the `FusionAhrsFlags` structure and obtained using the `FusionAhrsGetFlags` function. -| Flag | Description | -|--------------------------------|----------------------------------------------------------------------------------------------------------------------------| -| `initialising` | `true` if the algorithm is initialising. | -| `accelerationRejectionWarning` | `true` if the acceleration rejection timer has exceeded 25% of the `rejectionTimeout` value set in the algorithm settings. | -| `accelerationRejectionTimeout` | `true` if an acceleration rejection timeout has occurred and the algorithm is initialising. | -| `magneticRejectionWarning` | `true` if the magnetic rejection timer has exceeded 25% of the `rejectionTimeout` value set in the algorithm settings. | -| `magneticRejectionTimeout` | `true` if a magnetic rejection timeout has occurred during the previous algorithm update. | +| Flag | Description | +|------------------------|--------------------------------------------| +| `initialising` | `true` if the algorithm is initialising. | +| `accelerationRecovery` | `true` if acceleration recovery is active. | +| `magneticRecovery` | `true` if a magnetic recovery is active. | ## Gyroscope offset correction algorithm The gyroscope offset correction algorithm provides run-time calibration of the gyroscope offset to compensate for variations in temperature and fine-tune existing offset calibration that may already be in place. This algorithm should be used in conjunction with the AHRS algorithm to achieve best performance. -The algorithm calculates the gyroscope offset by detecting the stationary periods that occur naturally in most applications. Gyroscope measurements are sampled during these periods and low-pass filtered to obtain the gyroscope offset. The algorithm requires that gyroscope measurements do not exceed +/-3 degrees per second while stationary. Basic gyroscope offset calibration may be necessary to ensure that the initial offset error plus measurement noise is within these bounds. +The algorithm calculates the gyroscope offset by detecting the stationary periods that occur naturally in most applications. Gyroscope measurements are sampled during these periods and low-pass filtered to obtain the gyroscope offset. The algorithm requires that gyroscope measurements do not exceed +/-3 degrees per second while stationary. Basic gyroscope offset calibration may be necessary to ensure that the initial offset plus measurement noise is within these bounds. ## Sensor calibration