diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index 32eee83b6c..3dfb879817 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -60,7 +60,14 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = { // @Units: Hz // @User: Standard AP_GROUPINFO("_RATE", 2, AP_ExternalAHRS, rate, 50), - + + // @Param: _OPTIONS + // @DisplayName: External AHRS options + // @Description: External AHRS options bitmask + // @Bitmask: 0:Vector Nav use uncompensated values for accel gyro and mag. + // @User: Standard + AP_GROUPINFO("_OPTIONS", 3, AP_ExternalAHRS, options, 0), + AP_GROUPEND }; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index 037b520081..32b76701d7 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -33,6 +33,7 @@ class AP_ExternalAHRS { public: friend class AP_ExternalAHRS_backend; + friend class AP_ExternalAHRS_VectorNav; AP_ExternalAHRS(); @@ -132,12 +133,20 @@ public: Vector3f gyro; float temperature; } ins_data_message_t; - + +protected: + + enum class OPTIONS { + VN_UNCOMP_IMU = 1U << 0, + }; + bool option_is_set(OPTIONS option) const { return (options.get() & int32_t(option)) != 0; } + private: AP_ExternalAHRS_backend *backend; AP_Enum devtype; AP_Int16 rate; + AP_Int16 options; static AP_ExternalAHRS *_singleton; }; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp index 3f15fdfd14..ba79fb680e 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp @@ -450,10 +450,17 @@ void AP_ExternalAHRS_VectorNav::process_packet1(const uint8_t *b) last_pkt1_ms = AP_HAL::millis(); *last_pkt1 = pkt1; + const bool use_uncomp = option_is_set(AP_ExternalAHRS::OPTIONS::VN_UNCOMP_IMU); + { WITH_SEMAPHORE(state.sem); - state.accel = Vector3f{pkt1.accel[0], pkt1.accel[1], pkt1.accel[2]}; - state.gyro = Vector3f{pkt1.gyro[0], pkt1.gyro[1], pkt1.gyro[2]}; + if (use_uncomp) { + state.accel = Vector3f{pkt1.uncompAccel[0], pkt1.uncompAccel[1], pkt1.uncompAccel[2]}; + state.gyro = Vector3f{pkt1.uncompAngRate[0], pkt1.uncompAngRate[1], pkt1.uncompAngRate[2]}; + } else { + state.accel = Vector3f{pkt1.accel[0], pkt1.accel[1], pkt1.accel[2]}; + state.gyro = Vector3f{pkt1.gyro[0], pkt1.gyro[1], pkt1.gyro[2]}; + } state.quat = Quaternion{pkt1.quaternion[3], pkt1.quaternion[0], pkt1.quaternion[1], pkt1.quaternion[2]}; state.have_quaternion = true; @@ -582,10 +589,18 @@ void AP_ExternalAHRS_VectorNav::process_packet_VN_100(const uint8_t *b) const struct VN_100_packet1 &pkt = *(struct VN_100_packet1 *)b; last_pkt1_ms = AP_HAL::millis(); + + const bool use_uncomp = option_is_set(AP_ExternalAHRS::OPTIONS::VN_UNCOMP_IMU); + { WITH_SEMAPHORE(state.sem); - state.accel = Vector3f{pkt.accel[0], pkt.accel[1], pkt.accel[2]}; - state.gyro = Vector3f{pkt.gyro[0], pkt.gyro[1], pkt.gyro[2]}; + if (use_uncomp) { + state.accel = Vector3f{pkt.uncompAccel[0], pkt.uncompAccel[1], pkt.uncompAccel[2]}; + state.gyro = Vector3f{pkt.uncompAngRate[0], pkt.uncompAngRate[1], pkt.uncompAngRate[2]}; + } else { + state.accel = Vector3f{pkt.accel[0], pkt.accel[1], pkt.accel[2]}; + state.gyro = Vector3f{pkt.gyro[0], pkt.gyro[1], pkt.gyro[2]}; + } state.quat = Quaternion{pkt.quaternion[3], pkt.quaternion[0], pkt.quaternion[1], pkt.quaternion[2]}; state.have_quaternion = true; @@ -604,7 +619,11 @@ void AP_ExternalAHRS_VectorNav::process_packet_VN_100(const uint8_t *b) { AP_ExternalAHRS::mag_data_message_t mag; - mag.field = Vector3f{pkt.mag[0], pkt.mag[1], pkt.mag[2]}; + if (use_uncomp) { + mag.field = Vector3f{pkt.uncompMag[0], pkt.uncompMag[1], pkt.uncompMag[2]}; + } else { + mag.field = Vector3f{pkt.mag[0], pkt.mag[1], pkt.mag[2]}; + } mag.field *= 1000; // to mGauss AP::compass().handle_external(mag); @@ -644,10 +663,12 @@ void AP_ExternalAHRS_VectorNav::process_packet_VN_100(const uint8_t *b) "Qfffffffffffffff", AP_HAL::micros64(), pkt.temp, pkt.pressure*1e3, - pkt.mag[0], pkt.mag[1], pkt.mag[2], - pkt.accel[0], pkt.accel[1], pkt.accel[2], - pkt.gyro[0], pkt.gyro[1], pkt.gyro[2], - pkt.quaternion[0], pkt.quaternion[1], pkt.quaternion[2], pkt.quaternion[3]); + use_uncomp ? pkt.uncompMag[0] : pkt.mag[0], + use_uncomp ? pkt.uncompMag[1] : pkt.mag[1], + use_uncomp ? pkt.uncompMag[2] : pkt.mag[2], + state.accel[0], state.accel[1], state.accel[2], + state.gyro[0], state.gyro[1], state.gyro[2], + state.quat[0], state.quat[1], state.quat[2], state.quat[3]); } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp index f544238bc4..eaff493f5a 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp @@ -32,5 +32,10 @@ uint16_t AP_ExternalAHRS_backend::get_rate(void) const return frontend.get_IMU_rate(); } +bool AP_ExternalAHRS_backend::option_is_set(AP_ExternalAHRS::OPTIONS option) const +{ + return frontend.option_is_set(option); +} + #endif // HAL_EXTERNAL_AHRS_ENABLED diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h index 15a8f353bd..b35d099c2f 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h @@ -45,6 +45,7 @@ public: protected: AP_ExternalAHRS::state_t &state; uint16_t get_rate(void) const; + bool option_is_set(AP_ExternalAHRS::OPTIONS option) const; private: AP_ExternalAHRS &frontend;