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

AP_ExternalAHRS: support backends with get_variances() 

re-implement send_status_report in terms of get_variances and support
EKF failsafe with ExternalAHRS
This commit is contained in:
Andrew Tridgell 2024-10-15 14:18:39 +11:00
parent 04d18f1efb
commit 92693e0237
11 changed files with 123 additions and 212 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp
View File

@ -289,6 +289,17 @@ void AP_ExternalAHRS::get_filter_status(nav_filter_status &status) const
}
}
/*
get estimated variances, return false if not implemented
*/
bool AP_ExternalAHRS::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const
{
if (backend != nullptr) {
return backend->get_variances(velVar, posVar, hgtVar, magVar, tasVar);
}
return false;
}
bool AP_ExternalAHRS::get_gyro(Vector3f &gyro)
{
WITH_SEMAPHORE(state.sem);
@ -312,9 +323,56 @@ bool AP_ExternalAHRS::get_accel(Vector3f &accel)
// send an EKF_STATUS message to GCS
void AP_ExternalAHRS::send_status_report(GCS_MAVLINK &link) const
{
if (backend) {
backend->send_status_report(link);
float velVar, posVar, hgtVar, tasVar;
Vector3f magVar;
if (backend == nullptr || !backend->get_variances(velVar, posVar, hgtVar, magVar, tasVar)) {
return;
}
uint16_t flags = 0;
nav_filter_status filterStatus {};
get_filter_status(filterStatus);
if (filterStatus.flags.attitude) {
flags |= EKF_ATTITUDE;
}
if (filterStatus.flags.horiz_vel) {
flags |= EKF_VELOCITY_HORIZ;
}
if (filterStatus.flags.vert_vel) {
flags |= EKF_VELOCITY_VERT;
}
if (filterStatus.flags.horiz_pos_rel) {
flags |= EKF_POS_HORIZ_REL;
}
if (filterStatus.flags.horiz_pos_abs) {
flags |= EKF_POS_HORIZ_ABS;
}
if (filterStatus.flags.vert_pos) {
flags |= EKF_POS_VERT_ABS;
}
if (filterStatus.flags.terrain_alt) {
flags |= EKF_POS_VERT_AGL;
}
if (filterStatus.flags.const_pos_mode) {
flags |= EKF_CONST_POS_MODE;
}
if (filterStatus.flags.pred_horiz_pos_rel) {
flags |= EKF_PRED_POS_HORIZ_REL;
}
if (filterStatus.flags.pred_horiz_pos_abs) {
flags |= EKF_PRED_POS_HORIZ_ABS;
}
if (!filterStatus.flags.initalized) {
flags |= EKF_UNINITIALIZED;
}
const float mag_var = MAX(magVar.x, MAX(magVar.y, magVar.z));
mavlink_msg_ekf_status_report_send(link.get_chan(), flags,
velVar,
posVar,
hgtVar,
mag_var, 0, 0);
}
void AP_ExternalAHRS::update(void)
@ -357,6 +415,28 @@ void AP_ExternalAHRS::update(void)
state.velocity.x, state.velocity.y, state.velocity.z,
state.location.lat, state.location.lng, state.location.alt*0.01,
filterStatus.value);
// @LoggerMessage: EAHV
// @Description: External AHRS variances
// @Field: TimeUS: Time since system startup
// @Field: Vel: velocity variance
// @Field: Pos: position variance
// @Field: Hgt: height variance
// @Field: MagX: magnetic variance, X
// @Field: MagY: magnetic variance, Y
// @Field: MagZ: magnetic variance, Z
// @Field: TAS: true airspeed variance
float velVar, posVar, hgtVar, tasVar;
Vector3f magVar;
if (backend != nullptr && backend->get_variances(velVar, posVar, hgtVar, magVar, tasVar)) {
AP::logger().WriteStreaming("EAHV", "TimeUS,Vel,Pos,Hgt,MagX,MagY,MagZ,TAS",
"Qfffffff",
AP_HAL::micros64(),
velVar, posVar, hgtVar,
magVar.x, magVar.y, magVar.z,
tasVar);
}
}
#endif // HAL_LOGGING_ENABLED
}

1
libraries/AP_ExternalAHRS/AP_ExternalAHRS.h
View File

@ -120,6 +120,7 @@ public:
bool get_gyro(Vector3f &gyro);
bool get_accel(Vector3f &accel);
void send_status_report(class GCS_MAVLINK &link) const;
bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const;
// update backend
void update();

57
libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp
View File

@ -1110,57 +1110,14 @@ void AP_ExternalAHRS_InertialLabs::get_filter_status(nav_filter_status &status)
status.flags.rejecting_airspeed = (state2.air_data_status & ILABS_AIRDATA_AIRSPEED_FAIL);
}
// send an EKF_STATUS message to GCS
void AP_ExternalAHRS_InertialLabs::send_status_report(GCS_MAVLINK &link) const
// get variances
bool AP_ExternalAHRS_InertialLabs::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const
{
// prepare flags
uint16_t flags = 0;
nav_filter_status filterStatus;
get_filter_status(filterStatus);
if (filterStatus.flags.attitude) {
flags |= EKF_ATTITUDE;
}
if (filterStatus.flags.horiz_vel) {
flags |= EKF_VELOCITY_HORIZ;
}
if (filterStatus.flags.vert_vel) {
flags |= EKF_VELOCITY_VERT;
}
if (filterStatus.flags.horiz_pos_rel) {
flags |= EKF_POS_HORIZ_REL;
}
if (filterStatus.flags.horiz_pos_abs) {
flags |= EKF_POS_HORIZ_ABS;
}
if (filterStatus.flags.vert_pos) {
flags |= EKF_POS_VERT_ABS;
}
if (filterStatus.flags.terrain_alt) {
flags |= EKF_POS_VERT_AGL;
}
if (filterStatus.flags.const_pos_mode) {
flags |= EKF_CONST_POS_MODE;
}
if (filterStatus.flags.pred_horiz_pos_rel) {
flags |= EKF_PRED_POS_HORIZ_REL;
}
if (filterStatus.flags.pred_horiz_pos_abs) {
flags |= EKF_PRED_POS_HORIZ_ABS;
}
if (!filterStatus.flags.initalized) {
flags |= EKF_UNINITIALIZED;
}
// send message
const float vel_gate = 5;
const float pos_gate = 5;
const float hgt_gate = 5;
const float mag_var = 0;
mavlink_msg_ekf_status_report_send(link.get_chan(), flags,
state2.kf_vel_covariance.length()/vel_gate,
state2.kf_pos_covariance.xy().length()/pos_gate,
state2.kf_pos_covariance.z/hgt_gate,
mag_var, 0, 0);
velVar = state2.kf_vel_covariance.length() * vel_gate_scale;
posVar = state2.kf_pos_covariance.xy().length() * pos_gate_scale;
hgtVar = state2.kf_pos_covariance.z * hgt_gate_scale;
tasVar = 0;
return true;
}
#endif // AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED

2
libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h
View File

@ -37,7 +37,7 @@ public:
bool initialised(void) const override;
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
void get_filter_status(nav_filter_status &status) const override;
void send_status_report(class GCS_MAVLINK &link) const override;
bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const override;
// check for new data
void update() override {

55
libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp
View File

@ -268,55 +268,14 @@ void AP_ExternalAHRS_MicroStrain5::get_filter_status(nav_filter_status &status)
}
}
void AP_ExternalAHRS_MicroStrain5::send_status_report(GCS_MAVLINK &link) const
// get variances
bool AP_ExternalAHRS_MicroStrain5::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const
{
// prepare flags
uint16_t flags = 0;
nav_filter_status filterStatus;
get_filter_status(filterStatus);
if (filterStatus.flags.attitude) {
flags |= EKF_ATTITUDE;
}
if (filterStatus.flags.horiz_vel) {
flags |= EKF_VELOCITY_HORIZ;
}
if (filterStatus.flags.vert_vel) {
flags |= EKF_VELOCITY_VERT;
}
if (filterStatus.flags.horiz_pos_rel) {
flags |= EKF_POS_HORIZ_REL;
}
if (filterStatus.flags.horiz_pos_abs) {
flags |= EKF_POS_HORIZ_ABS;
}
if (filterStatus.flags.vert_pos) {
flags |= EKF_POS_VERT_ABS;
}
if (filterStatus.flags.terrain_alt) {
flags |= EKF_POS_VERT_AGL;
}
if (filterStatus.flags.const_pos_mode) {
flags |= EKF_CONST_POS_MODE;
}
if (filterStatus.flags.pred_horiz_pos_rel) {
flags |= EKF_PRED_POS_HORIZ_REL;
}
if (filterStatus.flags.pred_horiz_pos_abs) {
flags |= EKF_PRED_POS_HORIZ_ABS;
}
if (!filterStatus.flags.initalized) {
flags |= EKF_UNINITIALIZED;
}
// send message
const float vel_gate = 4; // represents hz value data is posted at
const float pos_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
mavlink_msg_ekf_status_report_send(link.get_chan(), flags,
gnss_data[gnss_instance].speed_accuracy/vel_gate, gnss_data[gnss_instance].horizontal_position_accuracy/pos_gate, gnss_data[gnss_instance].vertical_position_accuracy/hgt_gate,
mag_var, 0, 0);
velVar = gnss_data[gnss_instance].speed_accuracy * vel_gate_scale;
posVar = gnss_data[gnss_instance].horizontal_position_accuracy * pos_gate_scale;
hgtVar = gnss_data[gnss_instance].vertical_position_accuracy * hgt_gate_scale;
tasVar = 0;
return true;
}

2
libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h
View File

@ -42,7 +42,7 @@ public:
bool initialised(void) const override;
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
void get_filter_status(nav_filter_status &status) const override;
void send_status_report(class GCS_MAVLINK &link) const override;
bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const override;
// check for new data
void update() override {

63
libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp
View File

@ -317,63 +317,14 @@ void AP_ExternalAHRS_MicroStrain7::get_filter_status(nav_filter_status &status)
}
}
void AP_ExternalAHRS_MicroStrain7::send_status_report(GCS_MAVLINK &link) const
// get variances
bool AP_ExternalAHRS_MicroStrain7::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const
{
// prepare flags
uint16_t flags = 0;
nav_filter_status filterStatus;
get_filter_status(filterStatus);
if (filterStatus.flags.attitude) {
flags |= EKF_ATTITUDE;
}
if (filterStatus.flags.horiz_vel) {
flags |= EKF_VELOCITY_HORIZ;
}
if (filterStatus.flags.vert_vel) {
flags |= EKF_VELOCITY_VERT;
}
if (filterStatus.flags.horiz_pos_rel) {
flags |= EKF_POS_HORIZ_REL;
}
if (filterStatus.flags.horiz_pos_abs) {
flags |= EKF_POS_HORIZ_ABS;
}
if (filterStatus.flags.vert_pos) {
flags |= EKF_POS_VERT_ABS;
}
if (filterStatus.flags.terrain_alt) {
flags |= EKF_POS_VERT_AGL;
}
if (filterStatus.flags.const_pos_mode) {
flags |= EKF_CONST_POS_MODE;
}
if (filterStatus.flags.pred_horiz_pos_rel) {
flags |= EKF_PRED_POS_HORIZ_REL;
}
if (filterStatus.flags.pred_horiz_pos_abs) {
flags |= EKF_PRED_POS_HORIZ_ABS;
}
if (!filterStatus.flags.initalized) {
flags |= EKF_UNINITIALIZED;
}
// send message
const float vel_gate = 4; // represents hz value data is posted at
const float pos_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 velocity_variance {filter_data.ned_velocity_uncertainty.length() / vel_gate};
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,
velocity_variance, pos_horiz_variance, pos_vert_variance,
mag_var, terrain_alt_variance, airspeed_variance);
velVar = filter_data.ned_velocity_uncertainty.length() * vel_gate_scale;
posVar = filter_data.ned_position_uncertainty.xy().length() * pos_gate_scale;
hgtVar = filter_data.ned_position_uncertainty.z * hgt_gate_scale;
tasVar = 0;
return true;
}
bool AP_ExternalAHRS_MicroStrain7::times_healthy() const

2
libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h
View File

@ -42,7 +42,7 @@ public:
bool initialised(void) const override;
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
void get_filter_status(nav_filter_status &status) const override;
void send_status_report(class GCS_MAVLINK &link) const override;
bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const override;
// check for new data
void update() override

58
libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp
View File

@ -788,59 +788,15 @@ void AP_ExternalAHRS_VectorNav::get_filter_status(nav_filter_status &status) con
}
}
// send an EKF_STATUS message to GCS
void AP_ExternalAHRS_VectorNav::send_status_report(GCS_MAVLINK &link) const
// get variances
bool AP_ExternalAHRS_VectorNav::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const
{
if (!latest_ins_ekf_packet) {
return;
}
// prepare flags
uint16_t flags = 0;
nav_filter_status filterStatus;
get_filter_status(filterStatus);
if (filterStatus.flags.attitude) {
flags |= EKF_ATTITUDE;
}
if (filterStatus.flags.horiz_vel) {
flags |= EKF_VELOCITY_HORIZ;
}
if (filterStatus.flags.vert_vel) {
flags |= EKF_VELOCITY_VERT;
}
if (filterStatus.flags.horiz_pos_rel) {
flags |= EKF_POS_HORIZ_REL;
}
if (filterStatus.flags.horiz_pos_abs) {
flags |= EKF_POS_HORIZ_ABS;
}
if (filterStatus.flags.vert_pos) {
flags |= EKF_POS_VERT_ABS;
}
if (filterStatus.flags.terrain_alt) {
flags |= EKF_POS_VERT_AGL;
}
if (filterStatus.flags.const_pos_mode) {
flags |= EKF_CONST_POS_MODE;
}
if (filterStatus.flags.pred_horiz_pos_rel) {
flags |= EKF_PRED_POS_HORIZ_REL;
}
if (filterStatus.flags.pred_horiz_pos_abs) {
flags |= EKF_PRED_POS_HORIZ_ABS;
}
if (!filterStatus.flags.initalized) {
flags |= EKF_UNINITIALIZED;
}
// send message
const struct VN_INS_ekf_packet &pkt = *(struct VN_INS_ekf_packet *)latest_ins_ekf_packet;
const float vel_gate = 5;
const float pos_gate = 5;
const float hgt_gate = 5;
const float mag_var = 0;
mavlink_msg_ekf_status_report_send(link.get_chan(), flags,
pkt.velU / vel_gate, pkt.posU / pos_gate, pkt.posU / hgt_gate,
mag_var, 0, 0);
velVar = pkt.velU * vel_gate_scale;
posVar = pkt.posU * pos_gate_scale;
hgtVar = pkt.posU * hgt_gate_scale;
tasVar = 0;
return true;
}
#endif // AP_EXTERNAL_AHRS_VECTORNAV_ENABLED

2
libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h
View File

@ -37,7 +37,7 @@ public:
bool initialised(void) const override;
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
void get_filter_status(nav_filter_status &status) const override;
void send_status_report(class GCS_MAVLINK &link) const override;
bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const override;
// check for new data
void update() override {

9
libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h
View File

@ -41,7 +41,7 @@ public:
virtual bool initialised(void) const = 0;
virtual bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const = 0;
virtual void get_filter_status(nav_filter_status &status) const {}
virtual void send_status_report(class GCS_MAVLINK &link) const {}
virtual bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const { return false; }
// Check for new data.
// This is used when there's not a separate thread for EAHRS.
@ -73,6 +73,13 @@ protected:
*/
bool in_fly_forward(void) const;
/*
scale factors for get_variances() to return normalised values from SI units
*/
const float vel_gate_scale = 0.2;
const float pos_gate_scale = 0.2;
const float hgt_gate_scale = 0.2;
private:
AP_ExternalAHRS &frontend;
};