Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

ExternalAHRS: support variances in backends #28405

Merged
merged 2 commits into from
Oct 22, 2024
Merged

ExternalAHRS: support variances in backends #28405

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
6aec684
AP_AHRS: support variances from External_AHRS
tridge Oct 14, 2024
fa6c811
AP_ExternalAHRS: support backends with get_variances()
tridge Oct 15, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion libraries/AP_AHRS/AP_AHRS.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3164,7 +3164,7 @@ bool AP_AHRS::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3

#if AP_AHRS_EXTERNAL_ENABLED
case EKFType::EXTERNAL:
return false;
return external.get_variances(velVar, posVar, hgtVar, magVar, tasVar);
#endif
}

Expand Down
84 changes: 82 additions & 2 deletions libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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);
tridge marked this conversation as resolved.
Show resolved Hide resolved
}

void AP_ExternalAHRS::update(void)
Expand Down Expand Up @@ -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
}
Expand Down
1 change: 1 addition & 0 deletions libraries/AP_ExternalAHRS/AP_ExternalAHRS.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -120,6 +120,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
57 changes: 7 additions & 50 deletions libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp
View file
Open in desktop
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() * 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
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h
View file
Open in desktop
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
Open in desktop
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 * 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;
}


Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h
View file
Open in desktop
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
Open in desktop
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() * 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
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h
View file
Open in desktop
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
Loading