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