Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_ExternalAHRS: Populate orientation from filter 

* Populating AHRS orientation from IMU was not correct

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
This commit is contained in:
Ryan Friedman 2024-02-02 15:18:04 -07:00 committed by Andrew Tridgell
parent 8a3af4b018
commit ea0156f72a
4 changed files with 17 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_ExternalAHRS/AP_ExternalAHRS.h
View File

@ -90,7 +90,7 @@ public:
Vector3f accel;
Vector3f gyro;
Quaternion quat;
Quaternion quat; // NED
Location location;
Vector3f velocity;
Location origin;

6
libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp
View File

@ -134,9 +134,6 @@ void AP_ExternalAHRS_MicroStrain7::post_imu() const
WITH_SEMAPHORE(state.sem);
state.accel = imu_data.accel;
state.gyro = imu_data.gyro;
state.quat = imu_data.quat;
state.have_quaternion = true;
}
{
@ -188,6 +185,9 @@ void AP_ExternalAHRS_MicroStrain7::post_filter() const
// Use GNSS 0 even though it may be bad.
state.location = Location{filter_data.lat, filter_data.lon, gnss_data[0].msl_altitude, Location::AltFrame::ABSOLUTE};
state.have_location = true;
state.quat = filter_data.attitude_quat;
state.have_quaternion = true;
}
for (int instance = 0; instance < NUM_GNSS_INSTANCES; instance++) {

10
libraries/AP_ExternalAHRS/MicroStrain_common.cpp
View File

@ -58,6 +58,8 @@ enum class FilterPacketField {
FILTER_STATUS = 0x10,
LLH_POSITION = 0x01,
NED_VELOCITY = 0x02,
// https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/filter_data/data/mip_field_filter_attitude_quaternion.htm
ATTITUDE_QUAT = 0x03,
// https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/shared_data/data/mip_field_shared_gps_timestamp.htm
GPS_TIMESTAMP = 0xD3,
};
@ -288,6 +290,11 @@ void AP_MicroStrain::handle_filter(const MicroStrain_Packet &packet)
filter_data.ned_velocity_down = be32tofloat_ptr(packet.payload, i+10);
break;
}
case FilterPacketField::ATTITUDE_QUAT: {
filter_data.attitude_quat = populate_quaternion(packet.payload, i+2);
filter_data.attitude_quat.normalize();
break;
}
case FilterPacketField::FILTER_STATUS: {
filter_status.state = be16toh_ptr(&packet.payload[i+2]);
filter_status.mode = be16toh_ptr(&packet.payload[i+4]);
@ -309,6 +316,9 @@ Vector3f AP_MicroStrain::populate_vector3f(const uint8_t *data, uint8_t offset)
Quaternion AP_MicroStrain::populate_quaternion(const uint8_t *data, uint8_t offset)
{
// https://github.com/clemense/quaternion-conventions
// AP follows W + Xi + Yj + Zk format.
// Microstrain follows the same
return Quaternion {
be32tofloat_ptr(data, offset),
be32tofloat_ptr(data, offset+4),

3
libraries/AP_ExternalAHRS/MicroStrain_common.h
View File

@ -86,6 +86,9 @@ protected:
float ned_velocity_east;
float ned_velocity_down;
float speed_accuracy;
// 4x1 vector representation of the quaternion describing the orientation of the device with respect to the NED local-level frame.
// NED [Qw, Qx, Qy, Qz]
Quaternion attitude_quat;
} filter_data;
enum class DescriptorSet {