E&R S3 prime
F3
Dependencies: mbed LSM6DS33_GR1
Revision 7:10653d0475f5, committed 2021-09-30
- Comitter:
- gr66
- Date:
- Thu Sep 30 09:39:45 2021 +0000
- Parent:
- 6:b51a20c39f79
- Commit message:
- F3
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Fusion/Fusion.h Thu Sep 30 09:39:45 2021 +0000
@@ -0,0 +1,37 @@
+/**
+ * @file Fusion.h
+ * @author Seb Madgwick
+ * @brief Main header file for the library. This is the only file that needs to
+ * be included when using the library.
+ *
+ * Fusion is an ANSI C99 compliment sensor fusion library for sensor arrays of
+ * gyroscopes, accelerometers, and magnetometers. Fusion was specifically
+ * developed for use with embedded systems and has been optimised for execution
+ * speed. The library includes modules for: attitude and heading reference
+ * system (AHRS) sensor fusion, gyroscope bias correction, and a tilt-
+ * compensated compass.
+ */
+
+#ifndef FUSION_H
+#define FUSION_H
+
+//------------------------------------------------------------------------------
+// Includes
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "FusionAhrs.h"
+#include "FusionBias.h"
+#include "FusionCalibration.h"
+//#include "FusionCompass.h"
+#include "FusionTypes.h"
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+//------------------------------------------------------------------------------
+// End of file
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Fusion/FusionAhrs.c Thu Sep 30 09:39:45 2021 +0000
@@ -0,0 +1,292 @@
+/**
+ * @file FusionAhrs.c
+ * @author Seb Madgwick
+ * @brief The AHRS sensor fusion algorithm to combines gyroscope, accelerometer,
+ * and magnetometer measurements into a single measurement of orientation
+ * relative to the Earth (NWU convention).
+ *
+ * The algorithm behaviour is governed by a gain. A low gain will decrease the
+ * influence of the accelerometer and magnetometer so that the algorithm will
+ * better reject disturbances causes by translational motion and temporary
+ * magnetic distortions. However, a low gain will also increase the risk of
+ * drift due to gyroscope calibration errors. A typical gain value suitable for
+ * most applications is 0.5.
+ *
+ * The algorithm allows the application to define a minimum and maximum valid
+ * magnetic field magnitude. The algorithm will ignore magnetic measurements
+ * that fall outside of this range. This allows the algorithm to reject
+ * magnetic measurements that do not represent the direction of magnetic North.
+ * The typical magnitude of the Earth's magnetic field is between 20 uT and
+ * 70 uT.
+ *
+ * The algorithm can be used without a magnetometer. Measurements of
+ * orientation obtained using only gyroscope and accelerometer measurements
+ * can be expected to drift in the yaw component of orientation only. The
+ * application can reset the drift in yaw by setting the yaw to a specified
+ * angle at any time.
+ *
+ * The algorithm provides the measurement of orientation as a quaternion. The
+ * library includes functions for converting this quaternion to a rotation
+ * matrix and Euler angles.
+ *
+ * The algorithm also provides a measurement of linear acceleration and Earth
+ * acceleration. Linear acceleration is equal to the accelerometer measurement
+ * with the 1 g of gravity removed. Earth acceleration is a measurement of
+ * linear acceleration in the Earth coordinate frame.
+ */
+
+//------------------------------------------------------------------------------
+// Includes
+
+#include "FusionAhrs.h"
+#include <float.h> // FLT_MAX
+#include <math.h> // atan2f, cosf, sinf
+
+//------------------------------------------------------------------------------
+// Definitions
+
+/**
+ * @brief Initial gain used during the initialisation period. The gain used by
+ * each algorithm iteration will ramp down from this initial gain to the
+ * specified algorithm gain over the initialisation period.
+ */
+#define INITIAL_GAIN (10.0f)
+
+/**
+ * @brief Initialisation period (in seconds).
+ */
+#define INITIALISATION_PERIOD (3.0f)
+
+//------------------------------------------------------------------------------
+// Functions
+
+/**
+ * @brief Initialises the AHRS algorithm structure.
+ * @param fusionAhrs AHRS algorithm structure.
+ * @param gain AHRS algorithm gain.
+ */
+void FusionAhrsInitialise(FusionAhrs * const fusionAhrs, const float gain) {
+ fusionAhrs->gain = gain;
+ fusionAhrs->minimumMagneticFieldSquared = 0.0f;
+ fusionAhrs->maximumMagneticFieldSquared = FLT_MAX;
+ fusionAhrs->quaternion = FUSION_QUATERNION_IDENTITY;
+ fusionAhrs->linearAcceleration = FUSION_VECTOR3_ZERO;
+ fusionAhrs->rampedGain = INITIAL_GAIN;
+ fusionAhrs->zeroYawPending = false;
+}
+
+/**
+ * @brief Sets the AHRS algorithm gain. The gain must be equal or greater than
+ * zero.
+ * @param gain AHRS algorithm gain.
+ */
+void FusionAhrsSetGain(FusionAhrs * const fusionAhrs, const float gain) {
+ fusionAhrs->gain = gain;
+}
+
+/**
+ * @brief Sets the minimum and maximum valid magnetic field magnitudes in uT.
+ * @param fusionAhrs AHRS algorithm structure.
+ * @param minimumMagneticField Minimum valid magnetic field magnitude.
+ * @param maximumMagneticField Maximum valid magnetic field magnitude.
+ */
+void FusionAhrsSetMagneticField(FusionAhrs * const fusionAhrs, const float minimumMagneticField, const float maximumMagneticField) {
+ fusionAhrs->minimumMagneticFieldSquared = minimumMagneticField * minimumMagneticField;
+ fusionAhrs->maximumMagneticFieldSquared = maximumMagneticField * maximumMagneticField;
+}
+
+/**
+ * @brief Updates the AHRS algorithm. This function should be called for each
+ * new gyroscope measurement.
+ * @param fusionAhrs AHRS algorithm structure.
+ * @param gyroscope Gyroscope measurement in degrees per second.
+ * @param accelerometer Accelerometer measurement in g.
+ * @param magnetometer Magnetometer measurement in uT.
+ * @param samplePeriod Sample period in seconds. This is the difference in time
+ * between the current and previous gyroscope measurements.
+ */
+void FusionAhrsUpdate(FusionAhrs * const fusionAhrs, const FusionVector3 gyroscope, const FusionVector3 accelerometer, const FusionVector3 magnetometer, const float samplePeriod) {
+#define Q fusionAhrs->quaternion.element // define shorthand label for more readable code
+
+ // Calculate feedback error
+ FusionVector3 halfFeedbackError = FUSION_VECTOR3_ZERO; // scaled by 0.5 to avoid repeated multiplications by 2
+ do {
+ // Abandon feedback calculation if accelerometer measurement invalid
+ if ((accelerometer.axis.x == 0.0f) && (accelerometer.axis.y == 0.0f) && (accelerometer.axis.z == 0.0f)) {
+ break;
+ }
+
+ // Calculate direction of gravity assumed by quaternion
+ const FusionVector3 halfGravity = {
+ .axis.x = Q.x * Q.z - Q.w * Q.y,
+ .axis.y = Q.w * Q.x + Q.y * Q.z,
+ .axis.z = Q.w * Q.w - 0.5f + Q.z * Q.z,
+ }; // equal to 3rd column of rotation matrix representation scaled by 0.5
+
+ // Calculate accelerometer feedback error
+ halfFeedbackError = FusionVectorCrossProduct(FusionVectorFastNormalise(accelerometer), halfGravity);
+
+ // Abandon magnetometer feedback calculation if magnetometer measurement invalid
+ const float magnetometerMagnitudeSquared = FusionVectorMagnitudeSquared(magnetometer);
+ if ((magnetometerMagnitudeSquared < fusionAhrs->minimumMagneticFieldSquared) || (magnetometerMagnitudeSquared > fusionAhrs->maximumMagneticFieldSquared)) {
+ break;
+ }
+
+ // Compute direction of 'magnetic west' assumed by quaternion
+ const FusionVector3 halfWest = {
+ .axis.x = Q.x * Q.y + Q.w * Q.z,
+ .axis.y = Q.w * Q.w - 0.5f + Q.y * Q.y,
+ .axis.z = Q.y * Q.z - Q.w * Q.x
+ }; // equal to 2nd column of rotation matrix representation scaled by 0.5
+
+ // Calculate magnetometer feedback error
+ halfFeedbackError = FusionVectorAdd(halfFeedbackError, FusionVectorCrossProduct(FusionVectorFastNormalise(FusionVectorCrossProduct(accelerometer, magnetometer)), halfWest));
+
+ } while (false);
+
+ // Ramp down gain until initialisation complete
+ if (fusionAhrs->gain == 0) {
+ fusionAhrs->rampedGain = 0; // skip initialisation if gain is zero
+ }
+ float feedbackGain = fusionAhrs->gain;
+ if (fusionAhrs->rampedGain > fusionAhrs->gain) {
+ fusionAhrs->rampedGain -= (INITIAL_GAIN - fusionAhrs->gain) * samplePeriod / INITIALISATION_PERIOD;
+ feedbackGain = fusionAhrs->rampedGain;
+ }
+
+ // Convert gyroscope to radians per second scaled by 0.5
+ FusionVector3 halfGyroscope = FusionVectorMultiplyScalar(gyroscope, 0.5f * FusionDegreesToRadians(1.0f));
+
+ // Apply feedback to gyroscope
+ halfGyroscope = FusionVectorAdd(halfGyroscope, FusionVectorMultiplyScalar(halfFeedbackError, feedbackGain));
+
+ // Integrate rate of change of quaternion
+ fusionAhrs->quaternion = FusionQuaternionAdd(fusionAhrs->quaternion, FusionQuaternionMultiplyVector(fusionAhrs->quaternion, FusionVectorMultiplyScalar(halfGyroscope, samplePeriod)));
+
+ // Normalise quaternion
+ fusionAhrs->quaternion = FusionQuaternionFastNormalise(fusionAhrs->quaternion);
+
+ // Calculate linear acceleration
+ const FusionVector3 gravity = {
+ .axis.x = 2.0f * (Q.x * Q.z - Q.w * Q.y),
+ .axis.y = 2.0f * (Q.w * Q.x + Q.y * Q.z),
+ .axis.z = 2.0f * (Q.w * Q.w - 0.5f + Q.z * Q.z),
+ }; // equal to 3rd column of rotation matrix representation
+ fusionAhrs->linearAcceleration = FusionVectorSubtract(accelerometer, gravity);
+
+#undef Q // undefine shorthand label
+}
+
+/**
+ * @brief Updates the AHRS algorithm. This function should be called for each
+ * new gyroscope measurement.
+ * @param fusionAhrs AHRS algorithm structure.
+ * @param gyroscope Gyroscope measurement in degrees per second.
+ * @param accelerometer Accelerometer measurement in g.
+ * @param samplePeriod Sample period in seconds. This is the difference in time
+ * between the current and previous gyroscope measurements.
+ */
+void FusionAhrsUpdateWithoutMagnetometer(FusionAhrs * const fusionAhrs, const FusionVector3 gyroscope, const FusionVector3 accelerometer, const float samplePeriod) {
+
+ // Update AHRS algorithm
+ FusionAhrsUpdate(fusionAhrs, gyroscope, accelerometer, FUSION_VECTOR3_ZERO, samplePeriod);
+
+ // Zero yaw once initialisation complete
+ if (FusionAhrsIsInitialising(fusionAhrs) == true) {
+ fusionAhrs->zeroYawPending = true;
+ } else {
+ if (fusionAhrs->zeroYawPending == true) {
+ FusionAhrsSetYaw(fusionAhrs, 0.0f);
+ fusionAhrs->zeroYawPending = false;
+ }
+ }
+}
+
+/**
+ * @brief Gets the quaternion describing the sensor relative to the Earth.
+ * @param fusionAhrs AHRS algorithm structure.
+ * @return Quaternion describing the sensor relative to the Earth.
+ */
+FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs * const fusionAhrs) {
+ return FusionQuaternionConjugate(fusionAhrs->quaternion);
+}
+
+/**
+ * @brief Gets the linear acceleration measurement equal to the accelerometer
+ * measurement with the 1 g of gravity removed.
+ * @param fusionAhrs AHRS algorithm structure.
+ * @return Linear acceleration measurement.
+ */
+FusionVector3 FusionAhrsGetLinearAcceleration(const FusionAhrs * const fusionAhrs) {
+ return fusionAhrs->linearAcceleration;
+}
+
+/**
+ * @brief Gets the Earth acceleration measurement equal to linear acceleration
+ * in the Earth coordinate frame.
+ * @param fusionAhrs AHRS algorithm structure.
+ * @return Earth acceleration measurement.
+ */
+FusionVector3 FusionAhrsGetEarthAcceleration(const FusionAhrs * const fusionAhrs) {
+#define Q fusionAhrs->quaternion.element // define shorthand labels for more readable code
+#define A fusionAhrs->linearAcceleration.axis
+ const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations
+ const float qwqx = Q.w * Q.x;
+ const float qwqy = Q.w * Q.y;
+ const float qwqz = Q.w * Q.z;
+ const float qxqy = Q.x * Q.y;
+ const float qxqz = Q.x * Q.z;
+ const float qyqz = Q.y * Q.z;
+ const FusionVector3 earthAcceleration = {
+ .axis.x = 2.0f * ((qwqw - 0.5f + Q.x * Q.x) * A.x + (qxqy - qwqz) * A.y + (qxqz + qwqy) * A.z),
+ .axis.y = 2.0f * ((qxqy + qwqz) * A.x + (qwqw - 0.5f + Q.y * Q.y) * A.y + (qyqz - qwqx) * A.z),
+ .axis.z = 2.0f * ((qxqz - qwqy) * A.x + (qyqz + qwqx) * A.y + (qwqw - 0.5f + Q.z * Q.z) * A.z),
+ }; // transpose of a rotation matrix representation of the quaternion multiplied with the linear acceleration
+ return earthAcceleration;
+#undef Q // undefine shorthand label
+#undef A
+}
+
+/**
+ * @brief Reinitialise the AHRS algorithm.
+ * @param fusionAhrs AHRS algorithm structure.
+ */
+void FusionAhrsReinitialise(FusionAhrs * const fusionAhrs) {
+ fusionAhrs->quaternion = FUSION_QUATERNION_IDENTITY;
+ fusionAhrs->linearAcceleration = FUSION_VECTOR3_ZERO;
+ fusionAhrs->rampedGain = INITIAL_GAIN;
+}
+
+/**
+ * @brief Returns true while the AHRS algorithm is initialising.
+ * @param fusionAhrs AHRS algorithm structure.
+ * @return True while the AHRS algorithm is initialising.
+ */
+bool FusionAhrsIsInitialising(const FusionAhrs * const fusionAhrs) {
+ return fusionAhrs->rampedGain > fusionAhrs->gain;
+}
+
+/**
+ * @brief Sets the yaw component of the orientation measurement provided by the
+ * AHRS algorithm. This function can be used to reset drift in yaw when the
+ * AHRS algorithm is being used without a magnetometer.
+ * @param fusionAhrs AHRS algorithm structure.
+ * @param yaw Yaw angle in degrees.
+ */
+void FusionAhrsSetYaw(FusionAhrs * const fusionAhrs, const float yaw) {
+#define Q fusionAhrs->quaternion.element // define shorthand label for more readable code
+ fusionAhrs->quaternion = FusionQuaternionNormalise(fusionAhrs->quaternion); // quaternion must be normalised accurately (approximation not sufficient)
+ const float inverseYaw = atan2f(Q.x * Q.y + Q.w * Q.z, Q.w * Q.w - 0.5f + Q.x * Q.x); // Euler angle of conjugate
+ const float halfInverseYawMinusOffset = 0.5f * (inverseYaw - FusionDegreesToRadians(yaw));
+ const FusionQuaternion inverseYawQuaternion = {
+ .element.w = cosf(halfInverseYawMinusOffset),
+ .element.x = 0.0f,
+ .element.y = 0.0f,
+ .element.z = -1.0f * sinf(halfInverseYawMinusOffset),
+ };
+ fusionAhrs->quaternion = FusionQuaternionMultiply(inverseYawQuaternion, fusionAhrs->quaternion);
+#undef Q // undefine shorthand label
+}
+
+//------------------------------------------------------------------------------
+// End of file
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Fusion/FusionAhrs.h Thu Sep 30 09:39:45 2021 +0000
@@ -0,0 +1,82 @@
+/**
+ * @file FusionAhrs.h
+ * @author Seb Madgwick
+ * @brief The AHRS sensor fusion algorithm to combines gyroscope, accelerometer,
+ * and magnetometer measurements into a single measurement of orientation
+ * relative to the Earth (NWU convention).
+ *
+ * The algorithm behaviour is governed by a gain. A low gain will decrease the
+ * influence of the accelerometer and magnetometer so that the algorithm will
+ * better reject disturbances causes by translational motion and temporary
+ * magnetic distortions. However, a low gain will also increase the risk of
+ * drift due to gyroscope calibration errors. A typical gain value suitable for
+ * most applications is 0.5.
+ *
+ * The algorithm allows the application to define a minimum and maximum valid
+ * magnetic field magnitude. The algorithm will ignore magnetic measurements
+ * that fall outside of this range. This allows the algorithm to reject
+ * magnetic measurements that do not represent the direction of magnetic North.
+ * The typical magnitude of the Earth's magnetic field is between 20 uT and
+ * 70 uT.
+ *
+ * The algorithm can be used without a magnetometer. Measurements of
+ * orientation obtained using only gyroscope and accelerometer measurements
+ * can be expected to drift in the yaw component of orientation only. The
+ * application can reset the drift in yaw by setting the yaw to a specified
+ * angle at any time.
+ *
+ * The algorithm provides the measurement of orientation as a quaternion. The
+ * library includes functions for converting this quaternion to a rotation
+ * matrix and Euler angles.
+ *
+ * The algorithm also provides a measurement of linear acceleration and Earth
+ * acceleration. Linear acceleration is equal to the accelerometer measurement
+ * with the 1 g of gravity removed. Earth acceleration is a measurement of
+ * linear acceleration in the Earth coordinate frame.
+ */
+
+#ifndef FUSION_AHRS_H
+#define FUSION_AHRS_H
+
+//------------------------------------------------------------------------------
+// Includes
+
+#include "FusionTypes.h"
+#include <stdbool.h>
+
+//------------------------------------------------------------------------------
+// Definitions
+
+/**
+ * @brief AHRS algorithm structure. Structure members are used internally and
+ * should not be used by the user application.
+ */
+typedef struct {
+ float gain;
+ float minimumMagneticFieldSquared;
+ float maximumMagneticFieldSquared;
+ FusionQuaternion quaternion; // describes the Earth relative to the sensor
+ FusionVector3 linearAcceleration;
+ float rampedGain;
+ bool zeroYawPending;
+} FusionAhrs;
+
+//------------------------------------------------------------------------------
+// Function prototypes
+
+void FusionAhrsInitialise(FusionAhrs * const fusionAhrs, const float gain);
+void FusionAhrsSetGain(FusionAhrs * const fusionAhrs, const float gain);
+void FusionAhrsSetMagneticField(FusionAhrs * const fusionAhrs, const float minimumMagneticField, const float maximumMagneticField);
+void FusionAhrsUpdate(FusionAhrs * const fusionAhrs, const FusionVector3 gyroscope, const FusionVector3 accelerometer, const FusionVector3 magnetometer, const float samplePeriod);
+void FusionAhrsUpdateWithoutMagnetometer(FusionAhrs * const fusionAhrs, const FusionVector3 gyroscope, const FusionVector3 accelerometer, const float samplePeriod);
+FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs * const fusionAhrs);
+FusionVector3 FusionAhrsGetLinearAcceleration(const FusionAhrs * const fusionAhrs);
+FusionVector3 FusionAhrsGetEarthAcceleration(const FusionAhrs * const fusionAhrs);
+void FusionAhrsReinitialise(FusionAhrs * const fusionAhrs);
+bool FusionAhrsIsInitialising(const FusionAhrs * const fusionAhrs);
+void FusionAhrsSetYaw(FusionAhrs * const fusionAhrs, const float yaw);
+
+#endif
+
+//------------------------------------------------------------------------------
+// End of file
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Fusion/FusionBias.c Thu Sep 30 09:39:45 2021 +0000
@@ -0,0 +1,89 @@
+/**
+ * @file FusionBias.c
+ * @author Seb Madgwick
+ * @brief The gyroscope bias correction algorithm achieves run-time calibration
+ * of the gyroscope bias. The algorithm will detect when the gyroscope is
+ * stationary for a set period of time and then begin to sample gyroscope
+ * measurements to calculate the bias as an average.
+ */
+
+//------------------------------------------------------------------------------
+// Includes
+
+#include "FusionBias.h"
+#include "math.h" // fabs
+
+//------------------------------------------------------------------------------
+// Definitions
+
+/**
+ * @brief Minimum stationary period (in seconds) after which the the algorithm
+ * becomes active and begins sampling gyroscope measurements.
+ */
+#define STATIONARY_PERIOD (5.0f) // 5.0
+
+/**
+ * @brief Corner frequency (in Hz) of the high-pass filter used to sample the
+ * gyroscope bias.
+ */
+#define CORNER_FREQUENCY (0.02f) //0.02
+
+//------------------------------------------------------------------------------
+// Functions
+
+/**
+ * @brief Initialises the gyroscope bias correction algorithm.
+ * @param fusionBias FusionBias structure.
+ * @param threshold Gyroscope threshold (in degrees per second) below which the
+ * gyroscope is detected stationary.
+ * @param samplePeriod Nominal sample period (in seconds) corresponding the rate
+ * at which the application will update the algorithm.
+ */
+void FusionBiasInitialise(FusionBias * const fusionBias, const float threshold, const float samplePeriod) {
+ fusionBias->threshold = threshold;
+ fusionBias->samplePeriod = samplePeriod;
+ fusionBias->filterCoefficient = (2.0f * M_PI * CORNER_FREQUENCY) * fusionBias->samplePeriod;
+ fusionBias->stationaryTimer = 0.0f;
+ fusionBias->gyroscopeBias = FUSION_VECTOR3_ZERO;
+}
+
+/**
+ * @brief Updates the gyroscope bias correction algorithm and returns the
+ * corrected gyroscope measurement.
+ * @param fusionBias FusionBias structure.
+ * @param gyroscope Gyroscope measurement in degrees per second.
+ * @return Corrected gyroscope measurement in degrees per second.
+ */
+FusionVector3 FusionBiasUpdate(FusionBias * const fusionBias, FusionVector3 gyroscope) {
+
+ // Subtract bias from gyroscope measurement
+ gyroscope = FusionVectorSubtract(gyroscope, fusionBias->gyroscopeBias);
+
+ // Reset stationary timer if gyroscope not stationary
+ if ((fabs(gyroscope.axis.x) > fusionBias->threshold) || (fabs(gyroscope.axis.y) > fusionBias->threshold) || (fabs(gyroscope.axis.z) > fusionBias->threshold)) {
+ fusionBias->stationaryTimer = 0.0f;
+ return gyroscope;
+ }
+
+ // Increment stationary timer while gyroscope stationary
+ if (fusionBias->stationaryTimer < STATIONARY_PERIOD) {
+ fusionBias->stationaryTimer += fusionBias->samplePeriod;
+ return gyroscope;
+ }
+
+ // Adjust bias if stationary timer has elapsed
+ fusionBias->gyroscopeBias = FusionVectorAdd(fusionBias->gyroscopeBias, FusionVectorMultiplyScalar(gyroscope, fusionBias->filterCoefficient));
+ return gyroscope;
+}
+
+/**
+ * @brief Returns true if the gyroscope bias correction algorithm is active.
+ * @param fusionBias FusionBias structure.
+ * @return True if the gyroscope bias correction algorithm is active.
+ */
+bool FusionBiasIsActive(FusionBias * const fusionBias) {
+ return fusionBias->stationaryTimer >= STATIONARY_PERIOD;
+}
+
+//------------------------------------------------------------------------------
+// End of file
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Fusion/FusionBias.h Thu Sep 30 09:39:45 2021 +0000
@@ -0,0 +1,44 @@
+/**
+ * @file FusionBias.h
+ * @author Seb Madgwick
+ * @brief The gyroscope bias correction algorithm achieves run-time calibration
+ * of the gyroscope bias. The algorithm will detect when the gyroscope is
+ * stationary for a set period of time and then begin to sample gyroscope
+ * measurements to calculate the bias as an average.
+ */
+
+#ifndef FUSION_BIAS_H
+#define FUSION_BIAS_H
+
+//------------------------------------------------------------------------------
+// Includes
+
+#include "FusionTypes.h"
+#include <stdbool.h>
+
+//------------------------------------------------------------------------------
+// Definitions
+
+/**
+ * @brief Gyroscope bias correction algorithm structure. Structure members are
+ * used internally and should not be used by the user application.
+ */
+typedef struct {
+ float threshold;
+ float samplePeriod;
+ float filterCoefficient;
+ float stationaryTimer;
+ FusionVector3 gyroscopeBias;
+} FusionBias;
+
+//------------------------------------------------------------------------------
+// Function prototypes
+
+void FusionBiasInitialise(FusionBias * const fusionBias, const float threshold, const float samplePeriod);
+FusionVector3 FusionBiasUpdate(FusionBias * const fusionBias, FusionVector3 gyroscope);
+bool FusionBiasIsActive(FusionBias * const fusionBias);
+
+#endif
+
+//------------------------------------------------------------------------------
+// End of file
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Fusion/FusionCalibration.h Thu Sep 30 09:39:45 2021 +0000
@@ -0,0 +1,49 @@
+/**
+ * @file FusionCalibration.h
+ * @author Seb Madgwick
+ * @brief Gyroscope, accelerometer, and magnetometer calibration model.
+ *
+ * Static inline implementations are used to optimise for increased execution
+ * speed.
+ */
+
+#ifndef FUSION_CALIBRATION_H
+#define FUSION_CALIBRATION_H
+
+//------------------------------------------------------------------------------
+// Includes
+
+#include "FusionTypes.h"
+
+//------------------------------------------------------------------------------
+// Inline functions
+
+/**
+ * @brief Gyroscope and accelerometer calibration model.
+ * @param uncalibrated Uncalibrated gyroscope or accelerometer measurement in
+ * lsb.
+ * @param misalignment Misalignment matrix (may not be a true rotation matrix).
+ * @param sensitivity Sensitivity in g per lsb for an accelerometer and degrees
+ * per second per lsb for a gyroscope.
+ * @param bias Bias in lsb.
+ * @return Calibrated gyroscope or accelerometer measurement.
+ */
+static inline __attribute__((always_inline)) FusionVector3 FusionCalibrationInertial(const FusionVector3 uncalibrated, const FusionRotationMatrix misalignment, const FusionVector3 sensitivity, const FusionVector3 bias) {
+ return FusionRotationMatrixMultiplyVector(misalignment, FusionVectorHadamardProduct(FusionVectorSubtract(uncalibrated, bias), sensitivity));
+}
+
+/**
+ * @brief Magnetometer calibration model.
+ * @param magnetometer Uncalibrated magnetometer measurement in uT.
+ * @param softIronMatrix Soft-iron matrix (may not be a true rotation matrix).
+ * @param hardIronBias Hard-iron bias in uT.
+ * @return Calibrated magnetometer measurement.
+ */
+static inline __attribute__((always_inline)) FusionVector3 FusionCalibrationMagnetic(const FusionVector3 uncalibrated, const FusionRotationMatrix softIronMatrix, const FusionVector3 hardIronBias) {
+ return FusionVectorSubtract(FusionRotationMatrixMultiplyVector(softIronMatrix, uncalibrated), hardIronBias);
+}
+
+#endif
+
+//------------------------------------------------------------------------------
+// End of file
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Fusion/FusionTypes.h Thu Sep 30 09:39:45 2021 +0000
@@ -0,0 +1,473 @@
+/**
+ * @file FusionTypes.h
+ * @author Seb Madgwick
+ * @brief Common types and their associated operations.
+ *
+ * Static inline implementations are used to optimise for increased execution
+ * speed.
+ */
+
+#ifndef FUSION_TYPES_H
+#define FUSION_TYPES_H
+
+//------------------------------------------------------------------------------
+// Includes
+
+#include <math.h> // M_PI, sqrtf, atan2f, asinf
+#include <stdint.h> // int32_t
+
+//------------------------------------------------------------------------------
+// Definitions
+
+/**
+ * @brief Three-dimensional spacial vector.
+ */
+typedef union {
+ float array[3];
+
+ struct {
+ float x;
+ float y;
+ float z;
+ } axis;
+} FusionVector3;
+
+/**
+ * @brief Quaternion. This library uses the conversion of placing the 'w'
+ * element as the first element. Other implementations may place the 'w'
+ * element as the last element.
+ */
+typedef union {
+ float array[4];
+
+ struct {
+ float w;
+ float x;
+ float y;
+ float z;
+ } element;
+} FusionQuaternion;
+
+/**
+ * @brief Rotation matrix in row-major order.
+ * See http://en.wikipedia.org/wiki/Row-major_order
+ */
+typedef union {
+ float array[9];
+
+ struct {
+ float xx;
+ float xy;
+ float xz;
+ float yx;
+ float yy;
+ float yz;
+ float zx;
+ float zy;
+ float zz;
+ } element;
+} FusionRotationMatrix;
+
+/**
+ * @brief Euler angles union. The Euler angles are in the Aerospace sequence
+ * also known as the ZYX sequence.
+ */
+typedef union {
+ float array[3];
+
+ struct {
+ float roll;
+ float pitch;
+ float yaw;
+ } angle;
+} FusionEulerAngles;
+
+/**
+ * @brief Zero-length vector definition.
+ */
+#define FUSION_VECTOR3_ZERO ((FusionVector3){ .array = {0.0f, 0.0f, 0.0f} })
+
+/**
+ * @brief Quaternion identity definition to represent an aligned of
+ * orientation.
+ */
+#define FUSION_QUATERNION_IDENTITY ((FusionQuaternion){ .array = {1.0f, 0.0f, 0.0f, 0.0f} })
+
+/**
+ * @brief Rotation matrix identity definition to represent an aligned of
+ * orientation.
+ */
+#define FUSION_ROTATION_MATRIX_IDENTITY ((FusionRotationMatrix){ .array = {1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f} })
+
+/**
+ * @brief Euler angles zero definition to represent an aligned of orientation.
+ */
+#define FUSION_EULER_ANGLES_ZERO ((FusionEulerAngles){ .array = {0.0f, 0.0f, 0.0f} })
+
+/**
+ * @brief Definition of M_PI. Some compilers may not define this in math.h.
+ */
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+//------------------------------------------------------------------------------
+// Inline functions - Degrees and radians conversion
+
+/**
+ * @brief Converts degrees to radians.
+ * @param degrees Degrees.
+ * @return Radians.
+ */
+static inline __attribute__((always_inline)) float FusionDegreesToRadians(const float degrees) {
+ return degrees * ((float) M_PI / 180.0f);
+}
+
+/**
+ * @brief Converts radians to degrees.
+ * @param radians Radians.
+ * @return Degrees.
+ */
+static inline __attribute__((always_inline)) float FusionRadiansToDegrees(const float radians) {
+ return radians * (180.0f / (float) M_PI);
+}
+
+//------------------------------------------------------------------------------
+// Inline functions - Fast inverse square root
+
+/**
+ * @brief Calculates the reciprocal of the square root.
+ * See http://en.wikipedia.org/wiki/Fast_inverse_square_root
+ * @param x Operand.
+ * @return Reciprocal of the square root of x.
+ */
+static inline __attribute__((always_inline)) float FusionFastInverseSqrt(const float x) {
+ float halfx = 0.5f * x;
+ float y = x;
+ int32_t i = *(int32_t*) & y;
+ i = 0x5f3759df - (i >> 1);
+ y = *(float*) &i;
+ y = y * (1.5f - (halfx * y * y));
+ return y;
+}
+
+//------------------------------------------------------------------------------
+// Inline functions - Vector operations
+
+/**
+ * @brief Adds two vectors.
+ * @param vectorA First vector of the operation.
+ * @param vectorB Second vector of the operation.
+ * @return Sum of vectorA and vectorB.
+ */
+static inline __attribute__((always_inline)) FusionVector3 FusionVectorAdd(const FusionVector3 vectorA, const FusionVector3 vectorB) {
+ FusionVector3 result;
+ result.axis.x = vectorA.axis.x + vectorB.axis.x;
+ result.axis.y = vectorA.axis.y + vectorB.axis.y;
+ result.axis.z = vectorA.axis.z + vectorB.axis.z;
+ return result;
+}
+
+/**
+ * @brief Subtracts two vectors.
+ * @param vectorA First vector of the operation.
+ * @param vectorB Second vector of the operation.
+ * @return vectorB subtracted from vectorA.
+ */
+static inline __attribute__((always_inline)) FusionVector3 FusionVectorSubtract(const FusionVector3 vectorA, const FusionVector3 vectorB) {
+ FusionVector3 result;
+ result.axis.x = vectorA.axis.x - vectorB.axis.x;
+ result.axis.y = vectorA.axis.y - vectorB.axis.y;
+ result.axis.z = vectorA.axis.z - vectorB.axis.z;
+ return result;
+}
+
+/**
+ * @brief Multiplies vector by a scalar.
+ * @param vector Vector to be multiplied.
+ * @param scalar Scalar to be multiplied.
+ * @return Vector multiplied by scalar.
+ */
+static inline __attribute__((always_inline)) FusionVector3 FusionVectorMultiplyScalar(const FusionVector3 vector, const float scalar) {
+ FusionVector3 result;
+ result.axis.x = vector.axis.x * scalar;
+ result.axis.y = vector.axis.y * scalar;
+ result.axis.z = vector.axis.z * scalar;
+ return result;
+}
+
+/**
+ * @brief Calculates the Hadamard product (element-wise multiplication) of two
+ * vectors.
+ * @param vectorA First vector of the operation.
+ * @param vectorB Second vector of the operation.
+ * @return Hadamard product of vectorA and vectorB.
+ */
+static inline __attribute__((always_inline)) FusionVector3 FusionVectorHadamardProduct(const FusionVector3 vectorA, const FusionVector3 vectorB) {
+ FusionVector3 result;
+ result.axis.x = vectorA.axis.x * vectorB.axis.x;
+ result.axis.y = vectorA.axis.y * vectorB.axis.y;
+ result.axis.z = vectorA.axis.z * vectorB.axis.z;
+ return result;
+}
+
+/**
+ * @brief Calculates the cross-product of two vectors.
+ * @param vectorA First vector of the operation.
+ * @param vectorB Second vector of the operation.
+ * @return Cross-product of vectorA and vectorB.
+ */
+static inline __attribute__((always_inline)) FusionVector3 FusionVectorCrossProduct(const FusionVector3 vectorA, const FusionVector3 vectorB) {
+#define A vectorA.axis // define shorthand labels for more readable code
+#define B vectorB.axis
+ FusionVector3 result;
+ result.axis.x = A.y * B.z - A.z * B.y;
+ result.axis.y = A.z * B.x - A.x * B.z;
+ result.axis.z = A.x * B.y - A.y * B.x;
+ return result;
+#undef A // undefine shorthand labels
+#undef B
+}
+
+/**
+ * @brief Calculates the vector magnitude squared.
+ * @param vector Vector of the operation.
+ * @return Vector magnitude squared.
+ */
+static inline __attribute__((always_inline)) float FusionVectorMagnitudeSquared(const FusionVector3 vector) {
+#define V vector.axis // define shorthand label for more readable code
+ return V.x * V.x + V.y * V.y + V.z * V.z;
+#undef V // undefine shorthand label
+}
+
+/**
+ * @brief Calculates the magnitude of a vector.
+ * @param vector Vector to be used in calculation.
+ * @return Vector magnitude.
+ */
+static inline __attribute__((always_inline)) float FusionVectorMagnitude(const FusionVector3 vector) {
+ return sqrtf(FusionVectorMagnitudeSquared(vector));
+}
+
+/**
+ * @brief Normalises a vector to be of unit magnitude.
+ * @param vector Vector to be normalised.
+ * @return Normalised vector.
+ */
+static inline __attribute__((always_inline)) FusionVector3 FusionVectorNormalise(const FusionVector3 vector) {
+ const float magnitudeReciprocal = 1.0f / sqrtf(FusionVectorMagnitudeSquared(vector));
+ return FusionVectorMultiplyScalar(vector, magnitudeReciprocal);
+}
+
+/**
+ * @brief Normalises a vector to be of unit magnitude using the fast inverse
+ * square root approximation.
+ * @param vector Vector to be normalised.
+ * @return Normalised vector.
+ */
+static inline __attribute__((always_inline)) FusionVector3 FusionVectorFastNormalise(const FusionVector3 vector) {
+ const float magnitudeReciprocal = FusionFastInverseSqrt(FusionVectorMagnitudeSquared(vector));
+ return FusionVectorMultiplyScalar(vector, magnitudeReciprocal);
+}
+
+//------------------------------------------------------------------------------
+// Inline functions - Quaternion operations
+
+/**
+ * @brief Adds two quaternions.
+ * @param quaternionA First quaternion of the operation.
+ * @param quaternionB Second quaternion of the operation.
+ * @return Sum of quaternionA and quaternionB.
+ */
+static inline __attribute__((always_inline)) FusionQuaternion FusionQuaternionAdd(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) {
+ FusionQuaternion result;
+ result.element.w = quaternionA.element.w + quaternionB.element.w;
+ result.element.x = quaternionA.element.x + quaternionB.element.x;
+ result.element.y = quaternionA.element.y + quaternionB.element.y;
+ result.element.z = quaternionA.element.z + quaternionB.element.z;
+ return result;
+}
+
+/**
+ * @brief Multiplies two quaternions.
+ * @param quaternionA First quaternion of the operation.
+ * @param quaternionB Second quaternion of the operation.
+ * @return quaternionA multiplied by quaternionB.
+ */
+static inline __attribute__((always_inline)) FusionQuaternion FusionQuaternionMultiply(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) {
+#define A quaternionA.element // define shorthand labels for more readable code
+#define B quaternionB.element
+ FusionQuaternion result;
+ result.element.w = A.w * B.w - A.x * B.x - A.y * B.y - A.z * B.z;
+ result.element.x = A.w * B.x + A.x * B.w + A.y * B.z - A.z * B.y;
+ result.element.y = A.w * B.y - A.x * B.z + A.y * B.w + A.z * B.x;
+ result.element.z = A.w * B.z + A.x * B.y - A.y * B.x + A.z * B.w;
+ return result;
+#undef A // undefine shorthand labels
+#undef B
+}
+
+/**
+ * @brief Multiplies quaternion by a vector. This is a normal quaternion
+ * multiplication where the vector is treated a quaternion with a 'w' element
+ * value of 0. The quaternion is post multiplied by the vector.
+ * @param quaternion Quaternion to be multiplied.
+ * @param vector Vector to be multiplied.
+ * @return Quaternion multiplied by vector.
+ */
+static inline __attribute__((always_inline)) FusionQuaternion FusionQuaternionMultiplyVector(const FusionQuaternion quaternion, const FusionVector3 vector) {
+#define Q quaternion.element // define shorthand labels for more readable code
+#define V vector.axis
+ FusionQuaternion result;
+ result.element.w = -Q.x * V.x - Q.y * V.y - Q.z * V.z;
+ result.element.x = Q.w * V.x + Q.y * V.z - Q.z * V.y;
+ result.element.y = Q.w * V.y - Q.x * V.z + Q.z * V.x;
+ result.element.z = Q.w * V.z + Q.x * V.y - Q.y * V.x;
+ return result;
+#undef Q // undefine shorthand labels
+#undef V
+}
+
+/**
+ * @brief Returns the quaternion conjugate.
+ * @param quaternion Quaternion to be conjugated.
+ * @return Conjugated quaternion.
+ */
+static inline __attribute__((always_inline)) FusionQuaternion FusionQuaternionConjugate(const FusionQuaternion quaternion) {
+ FusionQuaternion conjugate;
+ conjugate.element.w = quaternion.element.w;
+ conjugate.element.x = -1.0f * quaternion.element.x;
+ conjugate.element.y = -1.0f * quaternion.element.y;
+ conjugate.element.z = -1.0f * quaternion.element.z;
+ return conjugate;
+}
+
+/**
+ * @brief Normalises a quaternion to be of unit magnitude.
+ * @param quaternion Quaternion to be normalised.
+ * @return Normalised quaternion.
+ */
+static inline __attribute__((always_inline)) FusionQuaternion FusionQuaternionNormalise(const FusionQuaternion quaternion) {
+#define Q quaternion.element // define shorthand label for more readable code
+ const float magnitudeReciprocal = 1.0f / sqrtf(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z);
+ FusionQuaternion normalisedQuaternion;
+ normalisedQuaternion.element.w = Q.w * magnitudeReciprocal;
+ normalisedQuaternion.element.x = Q.x * magnitudeReciprocal;
+ normalisedQuaternion.element.y = Q.y * magnitudeReciprocal;
+ normalisedQuaternion.element.z = Q.z * magnitudeReciprocal;
+ return normalisedQuaternion;
+#undef Q // undefine shorthand label
+}
+
+/**
+ * @brief Normalises a quaternion to be of unit magnitude using the fast inverse
+ * square root approximation.
+ * @param quaternion Quaternion to be normalised.
+ * @return Normalised quaternion.
+ */
+static inline __attribute__((always_inline)) FusionQuaternion FusionQuaternionFastNormalise(const FusionQuaternion quaternion) {
+#define Q quaternion.element // define shorthand label for more readable code
+ const float magnitudeReciprocal = FusionFastInverseSqrt(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z);
+ FusionQuaternion normalisedQuaternion;
+ normalisedQuaternion.element.w = Q.w * magnitudeReciprocal;
+ normalisedQuaternion.element.x = Q.x * magnitudeReciprocal;
+ normalisedQuaternion.element.y = Q.y * magnitudeReciprocal;
+ normalisedQuaternion.element.z = Q.z * magnitudeReciprocal;
+ return normalisedQuaternion;
+#undef Q // undefine shorthand label
+}
+
+//------------------------------------------------------------------------------
+// Inline functions - Rotation matrix operations
+
+/**
+ * @brief Multiplies two rotation matrices.
+ * @param rotationMatrixA First rotation matrix of the operation.
+ * @param rotationMatrixB Second rotation matrix of the operation.
+ * @return rotationMatrixA with rotationMatrixB.
+ */
+static inline __attribute__((always_inline)) FusionRotationMatrix FusionRotationMatrixMultiply(const FusionRotationMatrix rotationMatrixA, const FusionRotationMatrix rotationMatrixB) {
+#define A rotationMatrixA.element // define shorthand label for more readable code
+#define B rotationMatrixB.element
+ FusionRotationMatrix result;
+ result.element.xx = A.xx * B.xx + A.xy * B.yx + A.xz * B.zx;
+ result.element.xy = A.xx * B.xy + A.xy * B.yy + A.xz * B.zy;
+ result.element.xz = A.xx * B.xz + A.xy * B.yz + A.xz * B.zz;
+ result.element.yx = A.yx * B.xx + A.yy * B.yx + A.yz * B.zx;
+ result.element.yy = A.yx * B.xy + A.yy * B.yy + A.yz * B.zy;
+ result.element.yz = A.yx * B.xz + A.yy * B.yz + A.yz * B.zz;
+ result.element.zx = A.zx * B.xx + A.zy * B.yx + A.zz * B.zx;
+ result.element.zy = A.zx * B.xy + A.zy * B.yy + A.zz * B.zy;
+ result.element.zz = A.zx * B.xz + A.zy * B.yz + A.zz * B.zz;
+ return result;
+#undef A // undefine shorthand label
+#undef B
+}
+
+/**
+ * @brief Multiplies rotation matrix with scalar.
+ * @param rotationMatrix Rotation matrix to be multiplied.
+ * @param vector Vector to be multiplied.
+ * @return Rotation matrix multiplied with scalar.
+ */
+static inline __attribute__((always_inline)) FusionVector3 FusionRotationMatrixMultiplyVector(const FusionRotationMatrix rotationMatrix, const FusionVector3 vector) {
+#define R rotationMatrix.element // define shorthand label for more readable code
+ FusionVector3 result;
+ result.axis.x = R.xx * vector.axis.x + R.xy * vector.axis.y + R.xz * vector.axis.z;
+ result.axis.y = R.yx * vector.axis.x + R.yy * vector.axis.y + R.yz * vector.axis.z;
+ result.axis.z = R.zx * vector.axis.x + R.zy * vector.axis.y + R.zz * vector.axis.z;
+ return result;
+#undef R // undefine shorthand label
+}
+
+//------------------------------------------------------------------------------
+// Inline functions - Conversion operations
+
+/**
+ * @brief Converts a quaternion to a rotation matrix.
+ * @param quaternion Quaternion to be converted.
+ * @return Rotation matrix.
+ */
+static inline __attribute__((always_inline)) FusionRotationMatrix FusionQuaternionToRotationMatrix(const FusionQuaternion quaternion) {
+#define Q quaternion.element // define shorthand label for more readable code
+ const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations
+ const float qwqx = Q.w * Q.x;
+ const float qwqy = Q.w * Q.y;
+ const float qwqz = Q.w * Q.z;
+ const float qxqy = Q.x * Q.y;
+ const float qxqz = Q.x * Q.z;
+ const float qyqz = Q.y * Q.z;
+ FusionRotationMatrix rotationMatrix;
+ rotationMatrix.element.xx = 2.0f * (qwqw - 0.5f + Q.x * Q.x);
+ rotationMatrix.element.xy = 2.0f * (qxqy + qwqz);
+ rotationMatrix.element.xz = 2.0f * (qxqz - qwqy);
+ rotationMatrix.element.yx = 2.0f * (qxqy - qwqz);
+ rotationMatrix.element.yy = 2.0f * (qwqw - 0.5f + Q.y * Q.y);
+ rotationMatrix.element.yz = 2.0f * (qyqz + qwqx);
+ rotationMatrix.element.zx = 2.0f * (qxqz + qwqy);
+ rotationMatrix.element.zy = 2.0f * (qyqz - qwqx);
+ rotationMatrix.element.zz = 2.0f * (qwqw - 0.5f + Q.z * Q.z);
+ return rotationMatrix;
+#undef Q // undefine shorthand label
+}
+
+/**
+ * @brief Converts a quaternion to Euler angles in degrees.
+ * @param quaternion Quaternion to be converted.
+ * @return Euler angles in degrees.
+ */
+static inline __attribute__((always_inline)) FusionEulerAngles FusionQuaternionToEulerAngles(const FusionQuaternion quaternion) {
+#define Q quaternion.element // define shorthand label for more readable code
+ const float qwqwMinusHalf = Q.w * Q.w - 0.5f; // calculate common terms to avoid repeated operations
+ FusionEulerAngles eulerAngles;
+ eulerAngles.angle.roll = FusionRadiansToDegrees(atan2f(Q.y * Q.z - Q.w * Q.x, qwqwMinusHalf + Q.z * Q.z));
+ eulerAngles.angle.pitch = FusionRadiansToDegrees(-1.0f * asinf(2.0f * (Q.x * Q.z + Q.w * Q.y)));
+ eulerAngles.angle.yaw = FusionRadiansToDegrees(atan2f(Q.x * Q.y - Q.w * Q.z, qwqwMinusHalf + Q.x * Q.x));
+ return eulerAngles;
+#undef Q // undefine shorthand label
+}
+
+#endif
+
+//------------------------------------------------------------------------------
+// End of file
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Fusion3.cpp Thu Sep 30 09:39:45 2021 +0000
@@ -0,0 +1,186 @@
+//************************************
+// test fusion
+// ***********************************
+//
+#include "Fusion.h"
+#include "LSM6DS33_GR1.h"
+//
+float samplePeriod = 0.01f; // replace this value with actual sample period in seconds/
+// tracteur
+FusionBias TBias;
+FusionAhrs TAhrs;
+FusionVector3 gTSensitivity = {
+ .axis.x = 1.0f,
+ .axis.y = 1.0f,
+ .axis.z = 1.0f,
+}; // replace these values with actual sensitivity in degrees per second per lsb as specified in gyroscope datasheet
+FusionVector3 aTSensitivity = {
+ .axis.x = 1.0f,
+ .axis.y = 1.0f,
+ .axis.z = 1.0f,
+}; // replace these values with actual sensitivity in g per lsb as specified in accelerometer datasheet
+//
+// outil
+FusionBias OBias;
+FusionAhrs OAhrs;
+FusionVector3 gOSensitivity = {
+ .axis.x = 1.0f,
+ .axis.y = 1.0f,
+ .axis.z = 1.0f,
+}; // replace these values with actual sensitivity in degrees per second per lsb as specified in gyroscope datasheet
+FusionVector3 aOSensitivity = {
+ .axis.x = 1.0f,
+ .axis.y = 1.0f,
+ .axis.z = 1.0f,
+}; // replace these values with actual sensitivity in g per lsb as specified in accelerometer datasheet
+
+
+
+int flag_fusion=0;
+Serial pc(SERIAL_TX, SERIAL_RX); // I/O terminal PC
+DigitalOut led(LED1); // led
+LSM6DS33 AGT(PB_9, PB_8); // accelero gyro tracteur
+
+
+
+float gg[3];
+float aRes,gRes;
+Ticker affich;
+//
+
+//
+signed long round(double value)
+{
+ return((long)floor(value+0.5));
+}
+
+//
+void aff(void)
+{
+ flag_fusion=1;
+ // tracteur
+ // Calibrate gyroscope
+
+}
+//
+int main()
+{
+ wait(1);
+ pc.baud(115200);
+ pc.printf("Hello Fusion \r\n");
+ //aRes = 2.0 / 32768.0;
+ //gRes = 250.0 / 32768.0;
+ //pi=4*atan(1.0);
+ //init IMU
+ AGT.begin(LSM6DS33::G_SCALE_500DPS,LSM6DS33::A_SCALE_8G,LSM6DS33::G_ODR_104,LSM6DS33::A_ODR_104);
+ //wait(0.5);
+ AGT.calibration(1000);
+ //
+ char rr= AGT.readReg(0x15);
+ pc.printf(" reg 0x15 : %x \r\n",rr);
+ rr= AGT.readReg(0x10);
+ pc.printf(" reg 0x10 : %x \r\n",rr);
+ AGT.writeReg(0x10,rr|0x03); // 50HZ
+ rr= AGT.readReg(0x10);
+ pc.printf(" reg 0x10 : %x \r\n",rr);
+ rr= AGT.readReg(0x15);
+ pc.printf(" reg 0x15 : %x \r\n",rr);
+ rr= AGT.readReg(0x11);
+ pc.printf(" reg 0x11 : %x \r\n",rr);
+
+AGT.writeReg(0x13,0x80); // BW SCAL
+rr= AGT.readReg(0x13);
+ pc.printf(" reg 0x13 : %x \r\n",rr);
+ //
+ // Initialise gyroscope bias correction algorithm
+ FusionBiasInitialise(&TBias, 20.0f, samplePeriod); // stationary threshold = 0.5 degrees per second
+ FusionBiasInitialise(&OBias, 20.0f, samplePeriod); // stationary threshold = 0.5 degrees per second
+
+ // Initialise AHRS algorithm
+ FusionAhrsInitialise(&TAhrs, 0.5f); // gain = 0.5
+ FusionAhrsInitialise(&OAhrs, 0.5f); // gain = 0.5
+
+ // The contents of this do while loop should be called for each time new sensor measurements are available
+ //
+ int cpt=0;
+ affich.attach(&aff,0.01);
+ while(1) {
+ // lecture accelero / gyro
+
+ if(flag_fusion==1) {
+ cpt++;
+ AGT.readAccel();
+ AGT.readGyro();
+ AGT.readTemp();
+ FusionVector3 uncalTGyroscope = {
+ .axis.x = AGT.gx-AGT.gx_off, /* replace this value with actual gyroscope x axis measurement in lsb */
+ .axis.y = AGT.gy-AGT.gy_off, /* replace this value with actual gyroscope y axis measurement in lsb */
+ .axis.z = AGT.gz-AGT.gz_off, /* replace this value with actual gyroscope z axis measurement in lsb */
+ //.axis.z = 0, /* replace this value with actual gyroscope z axis measurement in lsb */
+ };
+ FusionVector3 calTGyroscope = FusionCalibrationInertial(uncalTGyroscope, FUSION_ROTATION_MATRIX_IDENTITY, gTSensitivity, FUSION_VECTOR3_ZERO);
+ // Calibrate accelerometer
+ FusionVector3 uncalTAccelerometer = {
+ .axis.x = AGT.ax, /* replace this value with actual accelerometer x axis measurement in lsb */
+ .axis.y = AGT.ay, /* replace this value with actual accelerometer y axis measurement in lsb */
+ .axis.z = AGT.az, /* replace this value with actual accelerometer z axis measurement in lsb */
+ };
+ FusionVector3 calTAccelerometer = FusionCalibrationInertial(uncalTAccelerometer, FUSION_ROTATION_MATRIX_IDENTITY, aTSensitivity, FUSION_VECTOR3_ZERO);
+ // Update gyroscope bias correction algorithm
+ calTGyroscope = FusionBiasUpdate(&TBias, calTGyroscope);
+ led= FusionBiasIsActive(&TBias);
+ // Update AHRS algorithm
+ FusionAhrsUpdateWithoutMagnetometer(&TAhrs, calTGyroscope, calTAccelerometer, samplePeriod);
+//
+ // Outil
+ // Calibrate gyroscope
+ FusionVector3 uncalOGyroscope = {
+ .axis.x = -(AGT.gz-AGT.gz_off), /* replace this value with actual gyroscope x axis measurement in lsb */
+ .axis.y = -(AGT.gy-AGT.gy_off), /* replace this value with actual gyroscope y axis measurement in lsb */
+ .axis.z = -(AGT.gx-AGT.gx_off), /* replace this value with actual gyroscope z axis measurement in lsb */
+ };
+ FusionVector3 calOGyroscope = FusionCalibrationInertial(uncalOGyroscope, FUSION_ROTATION_MATRIX_IDENTITY, gOSensitivity, FUSION_VECTOR3_ZERO);
+ // Calibrate accelerometer
+ FusionVector3 uncalOAccelerometer = {
+ .axis.x = -AGT.az, /* replace this value with actual accelerometer x axis measurement in lsb */
+ .axis.y = -AGT.ay, /* replace this value with actual accelerometer y axis measurement in lsb */
+ .axis.z = -AGT.ax, /* replace this value with actual accelerometer z axis measurement in lsb */
+ };
+ FusionVector3 calOAccelerometer = FusionCalibrationInertial(uncalOAccelerometer, FUSION_ROTATION_MATRIX_IDENTITY, aOSensitivity, FUSION_VECTOR3_ZERO);
+ // Update gyroscope bias correction algorithm
+ calOGyroscope = FusionBiasUpdate(&OBias, calOGyroscope);
+ // Update AHRS algorithm
+ FusionAhrsUpdateWithoutMagnetometer(&OAhrs, calOGyroscope, calOAccelerometer, samplePeriod);
+FusionEulerAngles eulerTAngles = FusionQuaternionToEulerAngles(FusionAhrsGetQuaternion(&TAhrs));
+
+ FusionEulerAngles eulerOAngles = FusionQuaternionToEulerAngles(FusionAhrsGetQuaternion(&OAhrs));
+ flag_fusion=0;
+ if(cpt%1==0){
+ pc.printf("$%f %f;", eulerTAngles.angle.roll, eulerTAngles.angle.pitch);
+ //pc.printf("$%f %f %f %f;", uncalTGyroscope.axis.x,calTGyroscope.axis.x,uncalTGyroscope.axis.y,calTGyroscope.axis.y);
+
+ }
+ }
+ // Print Euler angles
+
+ //printf("Roll = %0.1f, Pitch = %0.1f, Yaw = %0.1f\r\n", eulerAngles.angle.roll, eulerAngles.angle.pitch, eulerAngles.angle.yaw);
+ // pc.printf("$%f %f %f;", eulerAngles.angle.roll, eulerAngles.angle.pitch, eulerAngles.angle.yaw);
+ // pc.printf("$%f %f %f %f;", eulerAngles.angle.roll, eulerAngles.angle.pitch,angle_trac,cc);
+ //pc.printf("$%f %f %f %f ;", eulerTAngles.angle.roll, eulerTAngles.angle.pitch,eulerOAngles.angle.roll, eulerOAngles.angle.pitch);
+ //////////// pc.printf("$%f %f %f;", eulerOAngles.angle.roll, eulerOAngles.angle.pitch,AGT.temperature_c);
+ ////////////pc.printf("$%f %f;", eulerTAngles.angle.roll, eulerTAngles.angle.pitch);
+ //pc.printf("$%f %f %f;", gyroT_out[0],accT_out[0],angle_trac);
+ //pc.printf("$%f %f %f;", AGT.gx-AGT.gx_off,AGT.gy-AGT.gy_off,AGT.gz-AGT.gz_off);
+ //pc.printf("$%f %f %f;", gg[0],gg[1],gg[2]);
+ //
+ //wait(0.1);
+ // pc.printf(">>> %f %f %f \r\n",GXOT,GYOT,GZOT);
+ ////pc.printf(">>> %f %f %f \r\n",AGT.gx_off,AGT.gy_off,AGT.gz_off);
+ // pc.printf("> %f %f %f %f %f %f \r\n",AXT,AYT,AZT,GXT,GYT,GZT);
+ ////pc.printf("< %f %f %f %f %f %f \r\n\r\n",AGT.ax,AGT.ay,AGT.az,AGT.gx-AGT.gx_off,AGT.gy,AGT.gz);
+ // pc.printf("> %f %f %f %f %f %f \r\n",AXM,AYM,AZM,GXM,GYM,GZM);
+ // pc.printf(">>> %f %f %f \r\n",GXOM,GYOM,GZOM);
+ // allichage des differents ecrans
+
+ }
+}
\ No newline at end of file
--- a/LSM6DS33_GR1.lib Tue Jul 06 07:20:20 2021 +0000 +++ b/LSM6DS33_GR1.lib Thu Sep 30 09:39:45 2021 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/gr66/code/LSM6DS33_GR1/#132a63741359 +https://os.mbed.com/teams/ER-S3-prime/code/LSM6DS33_GR1/#704790280bbe
--- a/TB5649.lib Tue Jul 06 07:20:20 2021 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://os.mbed.com/users/gr66/code/TB5649xx/#d77dee9a8b4a
--- a/main_centrale_50.cpp Tue Jul 06 07:20:20 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,904 +0,0 @@
-//************************************
-// VITIACCESS2
-// HUBLEX
-// G. Raynaud
-// Centrale 40
-// 30 juin 2021
-// format message de 8 octets en flash
-// refonte automate
-// mise en place du mode expert
-// modif lecture IMU calcul angle central
-// ***********************************
-//
-//
-#define elec 0 // 1 verin hydraulique 0 verin electrique
-#define mat 0 // 1 : horizontal 0 vertical
-#include "LSM6DS33_GR1.h"
-#include "TB6549.h"
-
-//
-#define VERSION 50
-
-//#define ZM 2 // zone morte +/- zm
-#define INCLINAISON 45 // inclinaison du boitier
-#define TEMPOCMD 1000
-//
-Serial pc(PA_2, PA_3); // I/O terminal PC
-DigitalOut led(PA_15); // led
-#if elec
-DigitalOut VerinG(PF_1) ; // Verin Gauche
-DigitalOut VerinD(PB_3); // Verin Droit
-DigitalOut Verin(PB_5); // Verin
-#else
-unsigned char VerinG=0,VerinD=0,Verin=0;
-Motor motor(PB_5,PA_0,PA_4,PF_1); // commande moteur vérin pwm in1 in2 sb
-#endif
-I2C i2c_lcd(PB_7, PB_6); // SDA, SCL
-LSM6DS33 DOF6(PB_7, PB_6);
-Ticker automate;
-Ticker affich;
-Timer T;
-CAN can(PA_11,PA_12 ); //canRX canTX
-//
-CANMessage msg; // message recu can
-//
-signed char butee_gauche=-21,butee_droite=21;
-signed char flash_s[8]= {0,-20,20,1,10,10,0,0}; //
-signed char zmm=2; // zone morte
-signed char f_fusion=11; // 10 log frequence de coupure angle tract
-signed char beta=11; // coef de compensation de vitesse
-char ack=0;
-
-unsigned char ecran=6;
-unsigned char ihmnbr=0;
-unsigned char ihmcmd=0;
-//double *angle_acc;
-void automat(void);
-unsigned char etat_automate;
-//tableau pour filtrage
-double coefT_acc[2],coefT_gyro[2];
-double accT_in[2],gyroT_in[2];
-double accT_out[2],gyroT_out[2];
-//
-double coefM_acc[2],coefM_gyro[2];
-double accM_in[2],gyroM_in[2];
-double accM_out[2],gyroM_out[2];
-//
-int f_aff=0;
-//correction inclinaison
-double cos_inclT,sin_inclT;
-double cos_inclM,sin_inclM;
-double pi;
-//attitude trac
-double aRes;
-double gRes ;
-double AXT,AYT,AZT,GXT,GYT,GZT; // T pour tracteur
-double GXOT,GYOT,GZOT;
-double AXM,AYM,AZM,GXM,GYM,GZM; // M pour mat
-double GXOM,GYOM,GZOM;
-//
-void coef_filtre_PB(double H0,double f0,double Te,double* coef)
-{
- double PI=4*atan(1.0);
- double tau=1/(2*PI*f0);
- coef[0]=H0/(1+(2*tau/Te));
- coef[1]=(Te-2*tau)/(Te+2*tau);
-}
-//
-void filtre_PB(double* xn,double* yn, double* coef)
-{
- yn[0]=coef[0]*(xn[0]+xn[1])-coef[1]*yn[1];
- xn[1]=xn[0];
- yn[1]=yn[0];
-}
-//
-signed long round(double value)
-{
- return((long)floor(value+0.5));
-}
-//
-signed char consigne=0,consigne_eff=0;
-signed char angle_trac;
-signed char angle_mat;
-double angleM,angleMCT; // angle mat , aggle mat correction tachimetrique
-signed char angle_verin;
-
-void complement()
-{
- double psia=0;
- //mat
-#if mat
- psia=atan2(AYM,AZM)*180/pi;
- gyroM_in[0]=GXM; // mat H
-#else
- psia=atan2(AYM,-AXM)*180/pi;
- gyroM_in[0]=GZM; // mat V
-#endif
- accM_in[0]=psia;
- filtre_PB(accM_in,accM_out,coefM_acc); // filtre accelero X
- //angle_trac=-(char)round(accM_out[0]); ;
- filtre_PB(gyroM_in,gyroM_out,coefM_gyro); // filtre accelero X
- angleM=-(gyroM_out[0]+accM_out[0]);
- angleMCT=angleM-(gyroM_in[0]*beta*0.01); ////BETA 0.11
- angle_mat=(char)round(angleM);
- //trac
- // corection inclinaison
- double gxx=(GZT)*sin_inclT +(GXT)*cos_inclT;
- double gyy=GYT;
- double gzz=(GZT)*cos_inclT -(GXT)*sin_inclT;
- double axx=(AZT)*sin_inclT +(AXT)*cos_inclT;
- double ayy=AYT;
- double azz=(AZT)*cos_inclT -(AXT)*sin_inclT;
- //
- ////psia=atan2(AYT,AZT)*180/pi;
- ////accT_in[0]=psia;
- ////filtre_PB(accT_in,accT_out,coefT_acc); // filtre accelero X
- ////gyroT_in[0]=GXT;
- ////filtre_PB(gyroT_in,gyroT_out,coefT_gyro); // filtre accelero X
- ////angle_trac=-(char)round(gyroT_out[0]+accT_out[0]);
- //
- psia=atan2(ayy,azz)*180/pi;
- accT_in[0]=psia;
- filtre_PB(accT_in,accT_out,coefT_acc); // filtre accelero X
- gyroT_in[0]=gxx;
- filtre_PB(gyroT_in,gyroT_out,coefT_gyro); // filtre accelero X
- angle_trac=-(char)round(gyroT_out[0]+accT_out[0]);
-
-}
-
-//char tab[5]= {0x00,0xcc,0,0,0}; //
-char voyant[4]= {0x00,0x00,0x00,0x00}; //
-char affi[8]= {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x06}; //
-void send(void);
-//
-void autom(void)
-{
- complement();
-}
-void aff(void)
-{
- send();
- f_aff=1;
- automat();
-}
-// fonction can en reception sur ISR
-void rxcan()
-{
- //pc.printf("receive\n");
- can.read(msg);
- if(msg.id==0x22) {
- led=1;
- ihmnbr=msg.data[0];
- ihmcmd=msg.data[1];
- }
- if(msg.id==0x48) { // stocke les donnees transmises depuis la flash
- butee_gauche=msg.data[1];
- butee_droite=msg.data[2];
- zmm=msg.data[3];
- f_fusion=msg.data[4];
- beta=msg.data[5];
- }
- if(msg.id==0x41) { // accéléro depuis outil
- //pc.printf(">>> can id 41 \r\n");
- AXM=(signed short)(msg.data[1] | (msg.data[0] << 8))*aRes ;
- AYM=(signed short)(msg.data[3] | (msg.data[2]<< 8))*aRes ;
- AZM=(signed short)(msg.data[5] | (msg.data[4] << 8))*aRes ;
- //pc.printf(">>> ayt azt axm %f %f %f \r\n",AYM,AZM,AXM);
- }
- if(msg.id==0x42) { // gyro depuis outil
- GXM=((signed short)(msg.data[1] | (msg.data[0] << 8))*gRes )-GXOM;
- GYM=((signed short)(msg.data[3] | (msg.data[2]<< 8))*gRes )-GYOM;
- GZM=((signed short)(msg.data[5] | (msg.data[4] << 8))*gRes )-GZOM;
- }
- if(msg.id==0x43) { // offset depuis outil
- //pc.printf(">>> can id 43 \r\n");
- GXOM=(signed short)(msg.data[1] | (msg.data[0] << 8))*gRes ;
- GYOM=(signed short)(msg.data[3] | (msg.data[2]<< 8))*gRes ;
- GZOM=(signed short)(msg.data[5] | (msg.data[4] << 8))*gRes ;
- //pc.printf(">>> GXOM %f GYOM %f GZOM %f \r\n",GXOM,GYOM,GZOM);
- }
- if(msg.id==0x49) { // envoi depuis outil
- ack=1;
- }
-}
-// fonction d'envoi trame de test can
-void sendx(char msg,char* data,char size)
-{
- //pc.printf("send()\n");
- while(!can.write(CANMessage(msg, data, size)));
-}
-// fonction d'envoi trame de test can
-void send()
-{
- while(!can.write(CANMessage(0x11, voyant, 4))); // envoi de l'etat des voyants
- while(!can.write(CANMessage(0x12, affi, 8))); // envoi de l'etat des voyants
-}
-//
-void automat(void)
-{
- typedef enum {IDLE=0,RAZ=1, ERR=2,
- D=3,DT=4, DB=5,DF=6,
- G=7,GT=8, GB=9,GF=10,
- AA=11,GA=12,DA=13,INV=14,
- ButGAtt=15,ButG=16,
- ButDAtt=17,ButD=18,
- ParkAtt=19,Park=20,
- ButRAZAtt=21,ButRAZ=22,
- EXPATT=23,EXP1=24,EXP0=25,
- P0=26,P0X=27,P0Z=28,
- P1=29,P1X=30,P1Z=31,
- P2=32,P2X=33,P2Z=34,
- P3=35,P3X=36,P3Z=37,
- P4=38,P4X=39,P4Z=40,
- Param=41
- } type_etat;
- typedef enum {n=0x00, dm=0x01, gm=0x02, da=0x05,
- ga=0x06, a=0x04,bd=0x08,bg=0x10,p=0x20,in=0x24
- } type_cmd;
- type_etat etat_actuel = RAZ ; // déclaration état actuel init. à RAZ etat 3
- static type_etat etat_futur = RAZ; // déclaration état futur
- type_cmd bouton=n; // init bouton N : tout relaché
- // gestion des entrees
- angle_verin=angle_mat-angle_trac;
- bouton=(type_cmd)ihmcmd; // lecture des boutons
- // gestion du diagramme de transition
- //
- etat_actuel = etat_futur; // mise à jour de l'état
- etat_automate=(unsigned char)etat_actuel;
- // calul de l'éat futur en fonction de l'état actuel et des entrées
- // ihmcmd = 02:gauche,01:droite,04:auto,10 : but.G, 20 : park, 08 : but.D
- switch(etat_actuel) {
- case RAZ :
- T.reset();
- if(bouton==a) etat_futur = ERR; // si auto activé à la mise en marche etat d'erreur
- else {
- etat_futur=Param;
- T.start();
- }
- break;
- case Param :
- ecran=6;
- if(T.read_ms()>5000) etat_futur=IDLE;
- break;
- case ERR :
- if(bouton==n) etat_futur = IDLE; // mise en attente d'une commande
- break;
- case IDLE :
- if (bouton==gm) etat_futur = GT; // gauche test
- else if (bouton==dm) etat_futur = DT; // droite test
- else if(bouton==a) {
- etat_futur=AA;
- consigne=angle_mat;
- } else if (ihmcmd==0x10) etat_futur=ButGAtt ; // attente butee gauche
- else if (ihmcmd==0x08) etat_futur=ButDAtt ; // attente butee droite
- else if (ihmcmd==0x18) etat_futur=ButRAZAtt ; // attente RAZ butée
- else if (ihmcmd==0x20) etat_futur=ParkAtt ; // attente butee droite
- else if (ihmcmd==0x30) etat_futur=EXPATT ; // attente mode expert
- break;
- //---- gauche ----
- case GT :
- if(angle_verin<=butee_gauche) etat_futur=GB; // butee gauche atteinte ou dépassée
- else etat_futur=G ; // deplacement gauche possible
- break;
- case GB :
- if(bouton==n) etat_futur=IDLE;
- else if(bouton==a) {
- etat_futur=AA;
- //consigne=angle_verin-angle_trac; ////////grgrgr
- consigne=angle_trac+butee_gauche; ////////grgrgr
- } else if(T.read_ms()>5000) etat_futur=GF;
- break;
- case G :
- if (bouton==n) etat_futur = IDLE; // fin gauche
- else if(bouton==a) {
- etat_futur=AA;
- consigne=angle_mat;
- } else if (angle_verin<=butee_gauche) etat_futur=GB; // butee atteinte
- break;
- case GF :
- if (bouton==n) etat_futur = IDLE; // fin forcage gauche
- else if(bouton==a) {
- etat_futur=AA;
- consigne=angle_mat;
- }
- break;
- // ---- droite ----
- case DT :
- if(angle_verin>=butee_droite) etat_futur=DB; // butee droite atteinte ou dépassée
- else etat_futur=D ; // deplacement droite possible
- break;
- case DB :
- if(bouton==n) etat_futur=IDLE;
- else if(bouton==a) {
- etat_futur=AA;
- //consigne=angle_verin-angle_trac;
- consigne=angle_trac+butee_droite;
- } else if(T.read_ms()>5000) etat_futur=DF;
- break;
- case D :
- if (bouton==n) etat_futur = IDLE; // fin droite
- else if(bouton==a) {
- etat_futur=AA;
- consigne=angle_mat;
- } else if (angle_verin>=butee_droite) etat_futur=DB; // butee atteinte
- break;
- case DF :
- if (bouton==n) etat_futur = IDLE; // fin forcage droite
- else if(bouton==a) {
- etat_futur=AA;
- consigne=angle_mat;
- }
- break;
- // ---- auto ----
- case AA :
- if (bouton==n) etat_futur=IDLE; // retour etat d'attente
- else if (bouton==da) etat_futur=DT; // vers test consigne droite
- else if (bouton==ga) etat_futur=GT; // vers test consigne gauche
- else if (bouton==in) {
- etat_futur=INV; // inversion de consigne
- consigne=-consigne;
- } else if (angleMCT >= consigne_eff+zmm) etat_futur=GA; // mouvement gauche
- else if (angleMCT <= consigne_eff-zmm) etat_futur= DA; //mouvement droite
- break;
- case GA:
- if (bouton==n) etat_futur=IDLE; // retour etat manuel
- else if(bouton==da) etat_futur=DT;
- else if (bouton==ga) etat_futur=GT; // vers test consigne gauche
- else if (bouton==in) {
- etat_futur=INV; // inversion de consigne
- consigne=-consigne;
- } else if (angleMCT < consigne_eff+zmm) etat_futur=AA ; // auto //GRGR +zmm
- break;
- case DA :
- if (bouton==n) etat_futur=IDLE; // retour manuel
- else if(bouton==da) etat_futur=DT;
- else if (bouton==ga) etat_futur=GT; // vers test consigne gauche
- else if (bouton==in) {
- etat_futur=INV; // inversion de consigne
- consigne=-consigne;
- } else if (angleMCT > consigne_eff-zmm) etat_futur=AA ; // auto //GRGR +zmm
- break;
- case INV :
- if(bouton==a) etat_futur=AA;
- break;
- // ---- butée ----
- case ButGAtt :
- if(bouton==n) etat_futur=IDLE;
- else if(ihmcmd==0x18) etat_futur=ButRAZAtt;
- else if(ihmcmd==0x30) etat_futur=EXPATT;
- else if((T.read_ms()>TEMPOCMD)&&(angle_verin <0)) { // butéé dans le bon secteur
- etat_futur=ButG;
- flash_s[1]=angle_verin; // nouvelle valeur
- flash_s[2]=butee_droite; // valeur inchangée
- flash_s[3]=zmm; // valeur inchangée
- flash_s[4]=f_fusion;
- flash_s[5]=beta;
- sendx(0x34,(char*)flash_s,8);
- }
- break;
- case ButG :
- if(bouton==n) etat_futur=IDLE;
- break;
- case ButDAtt :
- if(bouton==n) etat_futur=IDLE;
- else if(ihmcmd==0x18) etat_futur=ButRAZAtt;
- else if((T.read_ms()>TEMPOCMD)&&(angle_verin >0)) { // butéé dans le bon secteur
- etat_futur=ButD;
- flash_s[1]=butee_gauche; // valeur inchangée
- flash_s[2]=angle_verin; // nouvelle valeur
- flash_s[3]=zmm; // valeur inchangée
- flash_s[4]=f_fusion;
- flash_s[5]=beta;
- sendx(0x34,(char*)flash_s,8);
- }
- break;
- case ButRAZAtt :
- if(bouton==n) etat_futur=IDLE;
- else if(T.read_ms()>TEMPOCMD) {
- etat_futur=ButRAZ;
- flash_s[1]=-20; // valeur RAZ
- flash_s[2]=20; // valeur RAE
- flash_s[3]=zmm; // valeur inchangée
- flash_s[4]=f_fusion;
- flash_s[5]=beta;
- sendx(0x34,(char*)flash_s,8);
- }
- break;
- case ButRAZ :
- if(bouton==n) etat_futur=IDLE;
- break;
- case ButD :
- if(bouton==n) etat_futur=IDLE;
- break;
- case ParkAtt :
- if(bouton==n) etat_futur=IDLE;
- else if(ihmcmd==0x30) etat_futur=EXPATT;
- else if(T.read_ms()>3000) {
- etat_futur=Park;
- }
- break;
- case Park :
- if(bouton==n) etat_futur=IDLE;
- else if ((angle_verin > +zmm)) {
- VerinG=1;
- VerinD=0;
- } // correction gauche
- else if ((angle_verin < -zmm)) {
- VerinG=0;
- VerinD=1;
- } // correction droite
- else {
- VerinG=0;
- VerinD=0;
- }
- break;
- // ---- expert ----
- case EXPATT :
- if(bouton==n) etat_futur=IDLE;
- //else if(ihmcmd==0x30) etat_futur=EXPATT;
- else if(T.read_ms()>TEMPOCMD) {
- etat_futur=EXP1;
- }
- break;
- case EXP1:
- ecran=1; // modif xx
- if(bouton==n) { // init message flash valeurs courantes
- flash_s[1]= butee_gauche;
- flash_s[2]= butee_droite;
- flash_s[3]= zmm;
- flash_s[4]= f_fusion;
- flash_s[5]= beta;
- etat_futur=P0;
- }
- break;
- case EXP0 :
- ecran=0; // modif xx
- if(bouton==n) {
- sendx(0x34,(char*)flash_s,8);
- etat_futur=IDLE;
- }
- break;
- case P0X :
- if(bouton==n) etat_futur=P0;
- else if(ihmcmd==0x30) etat_futur=EXP0 ; // sortie mode expert
- break;
- case P0 :
- ecran=1;
- if(bouton==p) etat_futur=P1X;
- else if(bouton==bd) {
- if(zmm<10)zmm++;
- flash_s[3]=zmm; // nouvelle valeur
- //sendx(0x34,(char*)flash_s,8);
- etat_futur=P0Z;
- } else if(bouton==bg) {
- if(zmm>0)zmm--;
- flash_s[3]=zmm; // nouvelle valeur
- //sendx(0x34,(char*)flash_s,8);
- etat_futur=P0Z;
- }
- break;
- case P0Z :
- if(bouton==n) etat_futur=P0;
- break;
- case P1X :
- if(bouton==n) etat_futur=P1;
- else if(ihmcmd==0x30) etat_futur=EXP0 ; // sortie mode expert
- break;
- case P1 :
- ecran=2;
- if(bouton==p) etat_futur=P2X;
- else if(bouton==bd) {
- if(butee_gauche<0)butee_gauche++;
- flash_s[1]=butee_gauche; // valeur inchangée
- //sendx(0x34,(char*)flash_s,8);
- etat_futur=P1Z;
- } else if(bouton==bg) {
- if(butee_gauche>-45)butee_gauche--;
- flash_s[1]=butee_gauche; // valeur inchangée
- //sendx(0x34,(char*)flash_s,8);
- etat_futur=P1Z;
- }
- break;
- case P1Z :
- if(bouton==n) etat_futur=P1;
- break;
- case P2X :
- if(bouton==n) etat_futur=P2;
- else if(ihmcmd==0x30) etat_futur=EXP0 ; // sortie mode expert
- break;
- case P2 :
- ecran=3;
- if(bouton==p) etat_futur=P3X;
- else if(bouton==bd) {
- if(butee_droite<45)butee_droite++;
- flash_s[2]=butee_droite;; // valeur inchangée
- //sendx(0x34,(char*)flash_s,8);
- etat_futur=P2Z;
- } else if(bouton==bg) {
- if(butee_droite>0)butee_droite--;
- flash_s[2]=butee_droite;; // valeur inchangée
- //sendx(0x34,(char*)flash_s,8);
- etat_futur=P2Z;
- }
- break;
- case P2Z :
- if(bouton==n) etat_futur=P2;
- break;
- case P3X :
- if(bouton==n) etat_futur=P3;
- else if(ihmcmd==0x30) etat_futur=EXP0 ; // sortie mode expert
- break;
- case P3 :
- ecran=4; // regmage freq angle
- if(bouton==p) etat_futur=P4X;
- else if(bouton==bd) {
- if(f_fusion<10)f_fusion++;
- flash_s[4]=f_fusion;; // valeur inchangée
- //pc.printf("flog %d \r\n",f_fusion);
- etat_futur=P3Z;
- } else if(bouton==bg) {
- if(f_fusion>-10)f_fusion--;
- flash_s[4]=f_fusion;; // valeur inchangée
- //pc.printf("flog %d \r\n",f_fusion);
- etat_futur=P3Z;
- }
- break;
- case P3Z :
- if(bouton==n) etat_futur=P3;
- break;
- case P4X :
- if(bouton==n) etat_futur=P4;
- else if(ihmcmd==0x30) etat_futur=EXP0 ; // sortie mode expert
- break;
- case P4 :
- ecran=5; // regmage freq angle
- if(bouton==p) etat_futur=P0X;
- else if(bouton==bd) {
- if(beta<50)beta++;
- flash_s[5]=beta;; // valeur inchangée
- //sendx(0x34,(char*)flash_s,8);
- //pc.printf("flog %d \r\n",beta);
- //coef_filtre_PB(1.0,pow(10,f_fusion/10.0),0.01,coef_angle); // paramètres filtre angle Ho,Fo,Te,coef
- //coef_filtre_PB(1.0,pow(10,f_fusion/10.0),0.01,coef_acc); //paramètres filtre accelero Ho,Fo,Te,coef
- etat_futur=P4Z;
- } else if(bouton==bg) {
- if(beta>0)beta--;
- flash_s[5]=beta;; // valeur inchangée
- //sendx(0x34,(char*)flash_s,8);
- //pc.printf("flog %d \r\n",beta);
- //coef_filtre_PB(1.0,pow(10,f_fusion/10.0),0.01,coef_angle); // paramètres filtre angle Ho,Fo,Te,coef
- //coef_filtre_PB(1.0,pow(10,f_fusion/10.0),0.01,coef_acc); //paramètres filtre accelero Ho,Fo,Te,coef
- etat_futur=P4Z;
- }
- break;
- case P4Z :
- if(bouton==n) etat_futur=P4;
- break;
-
-
- // default :
- // etat_futur=IDLE;
- // break;
- }
-// valeurs des sorties en fonction de l'état actuel
- switch(etat_actuel) {
- case RAZ :
- case Param :
- VerinG=0;
- VerinD=0;
- voyant[0]=1;
- voyant[1]=1;
- voyant[2]=1;
- voyant[3]=1;
- break;
- case ERR :
- VerinG=0;
- VerinD=0;
- voyant[0]=2;
- voyant[1]=2;
- voyant[2]=2;
- voyant[3]=2;
- break;
- case IDLE :
- VerinG=0;
- VerinD=0;
- voyant[0]=0;
- voyant[1]=0;
- voyant[2]=0;
- voyant[3]=1;
- T.stop();
- T.reset();
- ecran=0;
- break;
- case DB :
- VerinG=0;
- VerinD=0;
- voyant[1]=2;
- T.start();
- break;
- case D : // droite manuel
- VerinG=0;
- VerinD=1;
- voyant[1]=1;
- break;
- case GB :
- VerinG=0;
- VerinD=0;
- voyant[0]=2;
- T.start();
- break;
- case G :
- VerinG=1;
- VerinD=0;
- voyant[0]=1;
- break;
-
- case AA :
- VerinG=0;
- VerinD=0;
- if(consigne_eff != consigne) voyant[2]=2;
- else voyant[2]=1;
- voyant[0]=0;
- voyant[1]=0;
- T.stop();
- T.reset();
- break;
- case GA :
- VerinG=1;
- VerinD=0;
- voyant[0]=1;
- if(consigne_eff != consigne) voyant[2]=2;
- else voyant[2]=1;
- //T.start();
- break;
- case DA :
- VerinG=0;
- VerinD=1;
- voyant[1]=1;
- if(consigne_eff != consigne) voyant[2]=2;
- else voyant[2]=1;
- break;
- case ButGAtt :
- voyant[0]=2;
- T.start();
- break;
- case ButG :
- voyant[0]=1;
- break;
- case ButDAtt :
- voyant[1]=2;
- T.start();
- break;
- case ButD :
- voyant[1]=1;
- break;
- case ParkAtt :
- voyant[2]=2;
- T.start();
- break;
- case Park :
- voyant[2]=1;
- break;
- case ButRAZAtt :
- voyant[1]=2;
- voyant[0]=2;
- T.start();
- break;
- case EXPATT :
- voyant[2]=2;
- voyant[0]=2;
- T.start();
- break;
- case EXP1:
- voyant[2]=1;
- voyant[0]=1;
- break;
- case ButRAZ :
- voyant[1]=1;
- voyant[0]=1;
- break;
- case GT :
- case DT :
- break;
- case GF:
- VerinG=1;
- VerinD=0;
- voyant[0]=3;
- voyant[1]=0;
- voyant[2]=0;
- break;
- case DF:
- VerinG=0;
- VerinD=1;
- voyant[0]=0;
- voyant[1]=3;
- voyant[2]=0;
- break;
- default :
- VerinG=0;
- VerinD=0;
- //Verin=0;
- break;
- }
- Verin=VerinG||VerinD;
- if(angle_trac>=consigne-butee_gauche) consigne_eff=butee_gauche+angle_trac;
- else if(angle_trac<=consigne-butee_droite) consigne_eff=butee_droite+angle_trac;
- else consigne_eff=consigne;
-#if elec
-#else
- if(VerinG) {
- motor.speed(0.5);
- //pc.printf("verinG \r\n");
- } else if(VerinD) {
- motor.speed(-0.5);
- //pc.printf("verinD \r\n");
- } else {
- motor.speed(0);
- //pc.printf("verinStop \r\n");
- }
-#endif
-}
-int main()
-{
- can.frequency(1000000);
- can.attach(&rxcan);
- pc.baud(115200);
- pc.printf("Hello Viti2 \r\n");
- aRes = 2.0 / 32768.0;
- gRes = 250.0 / 32768.0;
- pi=4*atan(1.0);
- //init IMU
- DOF6.begin();
- DOF6.calibration(1000);
- //pc.printf("flog %d \r\n",f_fusion);
-
- // lancement réception can
- wait(0);
- //while(1){
- sendx(0x39,NULL,0); // envoi requéte
- //wait(1);
- //}
- //pc.printf(">>>> att \r\n");
- char xx=1;
- while(!ack) {
- xx=!ack;
- wait(0.1);
- //pc.printf(">>>> ist\r\n");
- } // attente erquéte
- //pc.printf(">>>> OK\r\n");
- // valeurs flash
- //pc.printf("f_fusion %d %f \r\n",f_fusion, pow(10.0,(f_fusion-10.0)/10.0));
- double fc =pow(10.0,(f_fusion-10.0)/10.0);
- //calcul des coefs des filtres
- //pour le gyro
- coef_filtre_PB(1.0/(fc*2.0*pi),fc,0.01,coefT_gyro); // paramètres filtre angle Ho,Fo,Te,coef
- coef_filtre_PB(1.0,fc,0.01,coefT_acc); //paramètres filtre accelero Ho,Fo,Te,coef
- //pour l'accelero
- coef_filtre_PB(1.0/(fc*2.0*pi),fc,0.01,coefM_gyro); // paramètres filtre angle Ho,Fo,Te,coef
- coef_filtre_PB(1.0,fc,0.01,coefM_acc); //paramètres filtre accelero Ho,Fo,Te,coef
-//
- wait(3); // delay attende envoi depuis acc pour init filtre
-//
- cos_inclT=cos(INCLINAISON*(pi/180));
- sin_inclT=sin(INCLINAISON*(pi/180));
-// resolution accel // gyro
- aRes = 2.0 / 32768.0;
- gRes = 250.0 / 32768.0;
- // calcul des offsets gyro tracteur
- GXOT=(signed short)(DOF6.gxol | (DOF6.gxoh << 8))*gRes ;
- GYOT=(signed short)(DOF6.gyol | (DOF6.gyoh << 8))*gRes ;
- GZOT=(signed short)(DOF6.gzol | (DOF6.gzoh << 8))*gRes ;
-
-
- //init des filtres numeriques
- double psia=atan2(AYM,-AXM)*180/pi;
- //pc.printf(">>> aym axm psia %f %f %f \r\n",AYM,AXM,psia);
- accM_in[0]=psia;
- accM_in[1]=psia;
- accM_out[0]=psia;
- accM_out[1]=psia;
- //
- DOF6.readAccel();
- AXT=(signed short)(DOF6.axl | (DOF6.axh << 8))*aRes ;
- AYT=(signed short)(DOF6.ayl | (DOF6.ayh << 8))*aRes ;
- AZT=(signed short)(DOF6.azl | (DOF6.azh << 8))*aRes ;
- psia=atan2(-AYT,AZT)*180/pi;
- //pc.printf(">>> ayt azt psia %f %f %f \r\n",AYT,AZT,psia);
- accT_in[0]=psia;
- accT_in[1]=psia;
- accT_out[0]=psia;
- accT_out[1]=psia;
- //pc.printf(">>> gxom gyom gzom %f %f %f \r\n",GXOM,GYOM,GZOM);
- //xx=1;
- // while(xx) {
- // if(GXOM+GYOM+GZOM !=0)xx=0;
- // wait(0.1);
- //}
- // pc.printf(">>> gxom gyom gzom %f %f %f \r\n",GXOM,GYOM,GZOM);
- // lancement tickers
- automate.attach(&autom,0.01);
- affich.attach(&aff,0.1);
- while(1) {
- // lecture accelero / gyro
- DOF6.readAccel();
- DOF6.readGyro();
- // mise a l'echelle
- AXT=(signed short)(DOF6.axl | (DOF6.axh << 8))*aRes ;
- AYT=(signed short)(DOF6.ayl | (DOF6.ayh << 8))*aRes ;
- AZT=(signed short)(DOF6.azl | (DOF6.azh << 8))*aRes ;
- GXT=((signed short)(DOF6.gxl | (DOF6.gxh << 8))*gRes)-GXOT ;
- GYT=((signed short)(DOF6.gyl | (DOF6.gyh << 8))*gRes)-GYOT ;
- GZT=((signed short)(DOF6.gzl | (DOF6.gzh << 8))*gRes)-GZOT ;
- // pc.printf(">>> %f %f %f \r\n",GXOT,GYOT,GZOT);
- // pc.printf(">>> %f %f %f \r\n",DOF6.gx_off,DOF6.gy_off,DOF6.gz_off);
- // pc.printf("> %f %f %f %f %f %f \r\n",AXT,AYT,AZT,GXT,GYT,GZT);
- //pc.printf("< %f %f %f %f %f %f \r\n\r\n",DOF6.ax,DOF6.ay,DOF6.az,DOF6.gx,DOF6.gy,DOF6.gz);
- // pc.printf("> %f %f %f %f %f %f \r\n",AXM,AYM,AZM,GXM,GYM,GZM);
- // pc.printf(">>> %f %f %f \r\n",GXOM,GYOM,GZOM);
- // allichage des differents ecrans
- if(ecran==0) {
- affi[0]=etat_automate;
- affi[1]=angle_mat;
- affi[2]=angle_trac;
- affi[3]=consigne;
- affi[4]=butee_gauche;
- affi[5]=butee_droite;
- affi[6]=consigne_eff;
- affi[7]=ecran;
- } else if(ecran==1) {
- affi[0]=etat_automate;
- affi[1]=zmm;
- affi[2]=0;
- affi[3]=0;
- affi[4]=0;
- affi[5]=0;
- affi[6]=0;
- affi[7]=ecran;
- } else if(ecran==2) {
- affi[0]=etat_automate;
- affi[1]=butee_gauche;
- affi[2]=0;
- affi[3]=0;
- affi[4]=0;
- affi[5]=0;
- affi[6]=0;
- affi[7]=ecran;
- } else if(ecran==3) {
- affi[0]=etat_automate;
- affi[1]=butee_droite;
- affi[2]=0;
- affi[3]=0;
- affi[4]=0;
- affi[5]=0;
- affi[6]=0;
- affi[7]=ecran;
- } else if(ecran==4) {
- affi[0]=etat_automate;
- affi[1]=f_fusion;
- affi[2]=0;
- affi[3]=0;
- affi[4]=0;
- affi[5]=0;
- affi[6]=0;
- affi[7]=ecran;
- } else if(ecran==5) {
- affi[0]=etat_automate;
- affi[1]=beta;
- affi[2]=0;
- affi[3]=0;
- affi[4]=0;
- affi[5]=0;
- affi[6]=0;
- affi[7]=ecran;
- } else if(ecran==6) {
- affi[0]=zmm;
- affi[1]=butee_gauche;
- affi[2]=butee_droite;
- affi[3]=f_fusion;
- affi[4]=beta;
- affi[5]=0;
- affi[6]=0;
- affi[7]=ecran;
- }
- }
-}
\ No newline at end of file