Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_ExtneralAHRS: add option to use uncompensated IMU values on vector nav

Browse Source
This commit is contained in:
Iampete1 2022-12-22 21:16:10 +00:00 committed by Andrew Tridgell
parent 9040eac91b
commit 759f035636
5 changed files with 54 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp
View File

@ -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
};

11
libraries/AP_ExternalAHRS/AP_ExternalAHRS.h
View File

@ -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> devtype;
AP_Int16 rate;
AP_Int16 options;
static AP_ExternalAHRS *_singleton;
};

39
libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp
View File

@ -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]);
}

5
libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp
View File

@ -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

1
libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h
View File

@ -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;