mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_ExternalAHRS: Use filter data to populate EKF status report
* Variances vs uncertainties may still be off * Requires enabling the following packets in SensorConnect: * Position Uncertainty (LLH) * Velocity Uncertainty (NED) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
This commit is contained in:
parent
a8255f6c23
commit
29fde1d6bd
@ -361,11 +361,16 @@ void AP_ExternalAHRS_MicroStrain7::send_status_report(GCS_MAVLINK &link) const
|
|||||||
const float hgt_gate = 4; // represents hz value data is posted at
|
const float hgt_gate = 4; // represents hz value data is posted at
|
||||||
const float mag_var = 0; //we may need to change this to be like the other gates, set to 0 because mag is ignored by the ins filter in vectornav
|
const float mag_var = 0; //we may need to change this to be like the other gates, set to 0 because mag is ignored by the ins filter in vectornav
|
||||||
|
|
||||||
// TODO fix to use NED filter speed accuracy instead of first gnss
|
const float velocity_variance {filter_data.ned_velocity_uncertainty.length() / vel_gate};
|
||||||
// https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/filter_data/data/mip_field_filter_ned_vel_uncertainty.htm
|
const float pos_horiz_variance {filter_data.ned_position_uncertainty.xy().length() / pos_gate};
|
||||||
|
const float pos_vert_variance {filter_data.ned_position_uncertainty.z / hgt_gate};
|
||||||
|
// No terrain alt sensor on MicroStrain7.
|
||||||
|
const float terrain_alt_variance {0};
|
||||||
|
// No airspeed sensor on MicroStrain7.
|
||||||
|
const float airspeed_variance {0};
|
||||||
mavlink_msg_ekf_status_report_send(link.get_chan(), flags,
|
mavlink_msg_ekf_status_report_send(link.get_chan(), flags,
|
||||||
gnss_data[0].speed_accuracy/vel_gate, gnss_data[0].horizontal_position_accuracy/pos_gate, gnss_data[0].vertical_position_accuracy/hgt_gate,
|
velocity_variance, pos_horiz_variance, pos_vert_variance,
|
||||||
mag_var, 0, 0);
|
mag_var, terrain_alt_variance, airspeed_variance);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -60,6 +60,10 @@ enum class FilterPacketField {
|
|||||||
NED_VELOCITY = 0x02,
|
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
|
// 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,
|
ATTITUDE_QUAT = 0x03,
|
||||||
|
// https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/0x82/data/0x08.htm
|
||||||
|
LLH_POSITION_UNCERTAINTY = 0x08,
|
||||||
|
// https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/0x82/data/0x09.htm
|
||||||
|
NED_VELOCITY_UNCERTAINTY = 0x09,
|
||||||
// https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/shared_data/data/mip_field_shared_gps_timestamp.htm
|
// 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,
|
GPS_TIMESTAMP = 0xD3,
|
||||||
};
|
};
|
||||||
@ -295,6 +299,28 @@ void AP_MicroStrain::handle_filter(const MicroStrain_Packet &packet)
|
|||||||
filter_data.attitude_quat.normalize();
|
filter_data.attitude_quat.normalize();
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
case FilterPacketField::LLH_POSITION_UNCERTAINTY: {
|
||||||
|
const float north_pos_acc = be32tofloat_ptr(packet.payload, i+2);
|
||||||
|
const float east_pos_acc = be32tofloat_ptr(packet.payload, i+6);
|
||||||
|
const float down_pos_acc = be32tofloat_ptr(packet.payload, i+10);
|
||||||
|
filter_data.ned_position_uncertainty = Vector3f(
|
||||||
|
north_pos_acc,
|
||||||
|
east_pos_acc,
|
||||||
|
down_pos_acc
|
||||||
|
);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case FilterPacketField::NED_VELOCITY_UNCERTAINTY: {
|
||||||
|
const float north_vel_uncertainty = be32tofloat_ptr(packet.payload, i+2);
|
||||||
|
const float east_vel_uncertainty = be32tofloat_ptr(packet.payload, i+6);
|
||||||
|
const float down_vel_uncertainty = be32tofloat_ptr(packet.payload, i+10);
|
||||||
|
filter_data.ned_velocity_uncertainty = Vector3f(
|
||||||
|
north_vel_uncertainty,
|
||||||
|
east_vel_uncertainty,
|
||||||
|
down_vel_uncertainty
|
||||||
|
);
|
||||||
|
break;
|
||||||
|
}
|
||||||
case FilterPacketField::FILTER_STATUS: {
|
case FilterPacketField::FILTER_STATUS: {
|
||||||
filter_status.state = be16toh_ptr(&packet.payload[i+2]);
|
filter_status.state = be16toh_ptr(&packet.payload[i+2]);
|
||||||
filter_status.mode = be16toh_ptr(&packet.payload[i+4]);
|
filter_status.mode = be16toh_ptr(&packet.payload[i+4]);
|
||||||
|
@ -77,15 +77,16 @@ protected:
|
|||||||
struct {
|
struct {
|
||||||
uint16_t week;
|
uint16_t week;
|
||||||
uint32_t tow_ms;
|
uint32_t tow_ms;
|
||||||
float horizontal_position_accuracy;
|
// 1-sigma position uncertainty in the NED local-level frame [meters].
|
||||||
float vertical_position_accuracy;
|
Vector3f ned_position_uncertainty;
|
||||||
int32_t lon;
|
int32_t lon;
|
||||||
int32_t lat;
|
int32_t lat;
|
||||||
int32_t hae_altitude;
|
int32_t hae_altitude;
|
||||||
float ned_velocity_north;
|
float ned_velocity_north;
|
||||||
float ned_velocity_east;
|
float ned_velocity_east;
|
||||||
float ned_velocity_down;
|
float ned_velocity_down;
|
||||||
float speed_accuracy;
|
// 1-sigma velocity uncertainties in the NED local-level frame.
|
||||||
|
Vector3f ned_velocity_uncertainty;
|
||||||
// 4x1 vector representation of the quaternion describing the orientation of the device with respect to the NED local-level frame.
|
// 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]
|
// NED [Qw, Qx, Qy, Qz]
|
||||||
Quaternion attitude_quat;
|
Quaternion attitude_quat;
|
||||||
|
Loading…
Reference in New Issue
Block a user