merge in nyc-dr1-release history after reset to nyc-dr1-dev
diff --git a/firmware/inc/algos/fusion.h b/firmware/inc/algos/fusion.h
index 506cd1c..6bf4878 100644
--- a/firmware/inc/algos/fusion.h
+++ b/firmware/inc/algos/fusion.h
@@ -22,6 +22,7 @@
#include "mat.h"
#include "quat.h"
+#include <stdbool.h>
#include <stdint.h>
#include <sys/types.h>
@@ -51,6 +52,9 @@
uint32_t mCount[3];
uint32_t flags;
+ float trustedMagDuration;
+ bool lastMagInvalid;
+
float fake_mag_decimation;
struct FusionParam param;
};
@@ -61,17 +65,28 @@
FUSION_REINITIALIZE = 1 << 2,
};
+enum MagTrustMode {
+ NORMAL,
+ INITIALIZATION, // right after initialization of fusion
+ BACK_TO_VALID, // when the mag value goes from invalid to valid
+ MANUAL_MAG_CAL // right after a manual calibration
+};
+
void initFusion(struct Fusion *fusion, uint32_t flags);
void fusionHandleGyro(struct Fusion *fusion, const struct Vec3 *w, float dT);
int fusionHandleAcc(struct Fusion *fusion, const struct Vec3 *a, float dT);
-int fusionHandleMag(struct Fusion *fusion, const struct Vec3 *m);
+int fusionHandleMag(struct Fusion *fusion, const struct Vec3 *m, float dT);
+
+// set trust mode of mag sensors depending on scenarios, see MagTrustMode
+void fusionSetMagTrust(struct Fusion *fusion, int mode);
void fusionGetAttitude(const struct Fusion *fusion, struct Vec4 *attitude);
void fusionGetBias(const struct Fusion *fusion, struct Vec3 *bias);
void fusionGetRotationMatrix(const struct Fusion *fusion, struct Mat33 *R);
int fusionHasEstimate(const struct Fusion *fusion);
+
#ifdef __cplusplus
}
#endif
diff --git a/firmware/src/algos/fusion.c b/firmware/src/algos/fusion.c
index 537af51..a786cef 100644
--- a/firmware/src/algos/fusion.c
+++ b/firmware/src/algos/fusion.c
@@ -54,7 +54,7 @@
#define MAX_VALID_MAGNETIC_FIELD 75.0f
#define MAX_VALID_MAGNETIC_FIELD_SQ (MAX_VALID_MAGNETIC_FIELD * MAX_VALID_MAGNETIC_FIELD)
-#define MIN_VALID_MAGNETIC_FIELD 30.0f
+#define MIN_VALID_MAGNETIC_FIELD 20.0f //norminal mag field strength is 25uT in some area
#define MIN_VALID_MAGNETIC_FIELD_SQ (MIN_VALID_MAGNETIC_FIELD * MIN_VALID_MAGNETIC_FIELD)
#define MIN_VALID_CROSS_PRODUCT_MAG 1.0e-3
@@ -62,6 +62,8 @@
#define DELTA_TIME_MARGIN 1.0e-9f
+#define TRUST_DURATION_MANUAL_MAG_CAL 5.f //unit: seconds
+
void initFusion(struct Fusion *fusion, uint32_t flags) {
fusion->flags = flags;
@@ -95,12 +97,16 @@
initVec3(&fusion->mData[0], 0.0f, 0.0f, 0.0f);
initVec3(&fusion->mData[1], 0.0f, 0.0f, 0.0f);
initVec3(&fusion->mData[2], 0.0f, 0.0f, 0.0f);
+
} else {
// mask off disabled sensor bit
fusion->mInitState &= (ACC
| ((fusion->flags & FUSION_USE_MAG) ? MAG : 0)
| ((fusion->flags & FUSION_USE_GYRO) ? GYRO : 0));
}
+
+ fusionSetMagTrust(fusion, NORMAL);
+ fusion->lastMagInvalid = false;
}
int fusionHasEstimate(const struct Fusion *fusion) {
@@ -218,6 +224,8 @@
initZeroMatrix(&fusion->P[0][1]);
initZeroMatrix(&fusion->P[1][0]);
initZeroMatrix(&fusion->P[1][1]);
+
+ fusionSetMagTrust(fusion, INITIALIZATION);
}
return 0;
@@ -576,10 +584,11 @@
return 0;
}
-#define MAG_COS_CONV_FACTOR 0.02f
-#define MAG_COS_CONV_LIMIT 2.f
+#define MAG_COS_CONV_FACTOR 0.02f
+#define MAG_COS_CONV_LIMIT 3.5f
+#define MAG_STDEV_REDUCTION 0.005f // lower stdev means more trust
-int fusionHandleMag(struct Fusion *fusion, const struct Vec3 *m) {
+int fusionHandleMag(struct Fusion *fusion, const struct Vec3 *m, float dT) {
if (!fusion_init_complete(fusion, MAG, m, 0.0f /* dT */)) {
return -EINVAL;
}
@@ -588,6 +597,8 @@
if (magFieldSq > MAX_VALID_MAGNETIC_FIELD_SQ
|| magFieldSq < MIN_VALID_MAGNETIC_FIELD_SQ) {
+ fusionSetMagTrust(fusion, NORMAL);
+ fusion->lastMagInvalid = true;
return -EINVAL;
}
@@ -601,9 +612,16 @@
vec3Cross(&east, m, &up);
if (vec3NormSquared(&east) < MIN_VALID_CROSS_PRODUCT_MAG_SQ) {
+ fusionSetMagTrust(fusion, NORMAL);
+ fusion->lastMagInvalid = true;
return -EINVAL;
}
+ if (fusion->lastMagInvalid) {
+ fusion->lastMagInvalid = false;
+ fusionSetMagTrust(fusion, BACK_TO_VALID);
+ }
+
struct Vec3 north;
vec3Cross(&north, &up, &east);
@@ -616,12 +634,26 @@
struct Vec3 mm;
mat33Apply(&mm, &R, &fusion->Bm);
float cos_err = vec3Dot(&mm, &north);
- cos_err = cos_err < (1.f - MAG_COS_CONV_FACTOR) ?
- (1.f - MAG_COS_CONV_FACTOR) : cos_err;
- float fc;
- fc = (1.f - cos_err) * (1.0f / MAG_COS_CONV_FACTOR * MAG_COS_CONV_LIMIT);
- p *= expf(-fc);
+ if (fusion->trustedMagDuration > 0) {
+ // if the trust mag time period is not finished
+ if (cos_err < (1.f - MAG_COS_CONV_FACTOR/4)) {
+ // if the mag direction and the fusion north has not converged, lower the
+ // standard deviation of mag to speed up convergence.
+ p *= MAG_STDEV_REDUCTION;
+ fusion->trustedMagDuration -= dT;
+ } else {
+ // it has converged already, so no need to keep the trust period any longer
+ fusionSetMagTrust(fusion, NORMAL);
+ }
+ } else {
+ cos_err = cos_err < (1.f - MAG_COS_CONV_FACTOR) ?
+ (1.f - MAG_COS_CONV_FACTOR) : cos_err;
+
+ float fc;
+ fc = (1.f - cos_err) * (1.0f / MAG_COS_CONV_FACTOR * MAG_COS_CONV_LIMIT);
+ p *= expf(-fc);
+ }
}
fusionUpdate(fusion, &north, &fusion->Bm, p);
@@ -629,6 +661,24 @@
return 0;
}
+void fusionSetMagTrust(struct Fusion *fusion, int mode) {
+ switch(mode) {
+ case NORMAL:
+ fusion->trustedMagDuration = 0; // disable
+ break;
+ case INITIALIZATION:
+ case BACK_TO_VALID:
+ fusion->trustedMagDuration = 0; // no special treatment for these two
+ break;
+ case MANUAL_MAG_CAL:
+ fusion->trustedMagDuration = TRUST_DURATION_MANUAL_MAG_CAL;
+ break;
+ default:
+ fusion->trustedMagDuration = 0; // by default it is disable
+ break;
+ }
+}
+
void fusionGetAttitude(const struct Fusion *fusion, struct Vec4 *attitude) {
*attitude = fusion->x0;
}
diff --git a/firmware/src/drivers/orientation/orientation.c b/firmware/src/drivers/orientation/orientation.c
index 7bb06ef..c9bd5e2 100644
--- a/firmware/src/drivers/orientation/orientation.c
+++ b/firmware/src/drivers/orientation/orientation.c
@@ -29,6 +29,7 @@
#include <nanohub_math.h>
#include <algos/fusion.h>
#include <sensors.h>
+#include <variant/inc/sensType.h>
#include <limits.h>
#include <slab.h>
@@ -51,6 +52,7 @@
#define EVT_SENSOR_ACC_DATA_RDY sensorGetMyEventType(SENS_TYPE_ACCEL)
#define EVT_SENSOR_GYR_DATA_RDY sensorGetMyEventType(SENS_TYPE_GYRO)
#define EVT_SENSOR_MAG_DATA_RDY sensorGetMyEventType(SENS_TYPE_MAG)
+#define EVT_SENSOR_MAG_BIAS sensorGetMyEventType(SENS_TYPE_MAG_BIAS)
#define kGravityEarth 9.80665f
#define kRad2deg (180.0f / M_PI)
@@ -478,7 +480,7 @@
case MAG:
initVec3(&m, mTask.samples[MAG][k].x, mTask.samples[MAG][k].y, mTask.samples[MAG][k].z);
- fusionHandleMag(&mTask.fusion, &m);
+ fusionHandleMag(&mTask.fusion, &m, dT);
--mTask.sample_counts[MAG];
if (++k == MAX_NUM_SAMPLES)
@@ -589,6 +591,7 @@
if (sensorRequest(mTask.tid, mTask.magHandle, mTask.raw_sensor_rate[MAG],
mTask.raw_sensor_latency)) {
osEventSubscribe(mTask.tid, EVT_SENSOR_MAG_DATA_RDY);
+ osEventSubscribe(mTask.tid, EVT_SENSOR_MAG_BIAS);
break;
}
}
@@ -760,6 +763,13 @@
fillSamples(ev, GYR);
drainSamples();
break;
+ case EVT_SENSOR_MAG_BIAS:
+ ev = (struct TripleAxisDataEvent *)evtData;
+ if (ev->samples[0].firstSample.biasPresent && mTask.flags & FUSION_FLAG_ENABLED) {
+ //it is a user initiated mag cal event
+ fusionSetMagTrust(&mTask.fusion, MANUAL_MAG_CAL);
+ }
+ break;
case EVT_SENSOR_MAG_DATA_RDY:
ev = (struct TripleAxisDataEvent *)evtData;
fillSamples(ev, MAG);
