Skip to content

Commit

Permalink
AP_ExternalAHRS: support backends with get_variances()
Browse files Browse the repository at this point in the history 
re-implement send_status_report in terms of get_variances and support
EKF failsafe with ExternalAHRS
  • Loading branch information
@tridge
tridge committed Oct 14, 2024
1 parent 957f5f0 commit 4f8209f
Show file tree
Hide file tree
Showing 13 changed files with 129 additions and 212 deletions.
87 changes: 85 additions & 2 deletions libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -306,6 +306,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);
Expand All @@ -331,9 +342,59 @@ 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 vel_gate = 5;
const float pos_gate = 5;
const float hgt_gate = 5;
const float mag_var = MAX(magVar.x, MAX(magVar.y, magVar.z));
mavlink_msg_ekf_status_report_send(link.get_chan(), flags,
velVar / vel_gate,
posVar / pos_gate,
hgtVar / hgt_gate,
mag_var, 0, 0);
}

void AP_ExternalAHRS::update(void)
Expand Down Expand Up @@ -376,6 +437,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
}
Expand Down
1 change: 1 addition & 0 deletions libraries/AP_ExternalAHRS/AP_ExternalAHRS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,6 +125,7 @@ class AP_ExternalAHRS {
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();
Expand Down
8 changes: 8 additions & 0 deletions libraries/AP_ExternalAHRS/AP_ExternalAHRS_CINS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,4 +72,12 @@ void AP_ExternalAHRS_CINS::get_filter_status(nav_filter_status &status) const
}
}

/*
get filter variances
*/
bool AP_ExternalAHRS_CINS::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const
{
return cins.get_variances(velVar, posVar, hgtVar, magVar, tasVar);
}

#endif // AP_EXTERNAL_AHRS_CINS_ENABLED
2 changes: 2 additions & 0 deletions libraries/AP_ExternalAHRS/AP_ExternalAHRS_CINS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,8 @@ class AP_ExternalAHRS_CINS : public AP_ExternalAHRS_backend {

void get_filter_status(nav_filter_status &status) const override;

bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const override;

// check for new data
void update() override;

Expand Down
57 changes: 7 additions & 50 deletions libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
posVar = state2.kf_pos_covariance.xy().length();
hgtVar = state2.kf_pos_covariance.z;
tasVar = 0;
return true;
}

#endif // AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend {
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 {
Expand Down
55 changes: 7 additions & 48 deletions libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
posVar = gnss_data[gnss_instance].horizontal_position_accuracy;
hgtVar = gnss_data[gnss_instance].vertical_position_accuracy;
tasVar = 0;
return true;
}


Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ class AP_ExternalAHRS_MicroStrain5: public AP_ExternalAHRS_backend, public AP_Mi
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 {
Expand Down
63 changes: 7 additions & 56 deletions libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
posVar = filter_data.ned_position_uncertainty.xy().length();
hgtVar = filter_data.ned_position_uncertainty.z;
tasVar = 0;
return true;
}

bool AP_ExternalAHRS_MicroStrain7::times_healthy() const
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ class AP_ExternalAHRS_MicroStrain7: public AP_ExternalAHRS_backend, public AP_Mi
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
Expand Down
Loading

0 comments on commit 4f8209f

Please sign in to comment.