Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: integrate attitude control EKF inertial-frame-reset

Browse Source
This commit is contained in:
Leonard Hall 2018-01-06 17:16:50 +10:30 committed by Randy Mackay
parent f368ff6664
commit a727305a59
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopter/ekf_check.cpp
View File

@ -196,10 +196,10 @@ void Copter::failsafe_ekf_off_event(void)
void Copter::check_ekf_reset() void Copter::check_ekf_reset()
{ {
// check for yaw reset // check for yaw reset
float yaw_angle_change_rad = 0.0f; float yaw_angle_change_rad;
uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad); uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad);
if (new_ekfYawReset_ms != ekfYawReset_ms) { if (new_ekfYawReset_ms != ekfYawReset_ms) {
attitude_control->shift_ef_yaw_target(ToDeg(yaw_angle_change_rad) * 100.0f); attitude_control->inertial_frame_reset();
ekfYawReset_ms = new_ekfYawReset_ms; ekfYawReset_ms = new_ekfYawReset_ms;
Log_Write_Event(DATA_EKF_YAW_RESET); Log_Write_Event(DATA_EKF_YAW_RESET);
} }
@ -207,6 +207,7 @@ void Copter::check_ekf_reset()
#if AP_AHRS_NAVEKF_AVAILABLE #if AP_AHRS_NAVEKF_AVAILABLE
// check for change in primary EKF (log only, AC_WPNav handles position target adjustment) // check for change in primary EKF (log only, AC_WPNav handles position target adjustment)
if ((EKF2.getPrimaryCoreIndex() != ekf_primary_core) && (EKF2.getPrimaryCoreIndex() != -1)) { if ((EKF2.getPrimaryCoreIndex() != ekf_primary_core) && (EKF2.getPrimaryCoreIndex() != -1)) {
attitude_control->inertial_frame_reset();
ekf_primary_core = EKF2.getPrimaryCoreIndex(); ekf_primary_core = EKF2.getPrimaryCoreIndex();
Log_Write_Error(ERROR_SUBSYSTEM_EKF_PRIMARY, ekf_primary_core); Log_Write_Error(ERROR_SUBSYSTEM_EKF_PRIMARY, ekf_primary_core);
gcs().send_text(MAV_SEVERITY_WARNING, "EKF primary changed:%d", (unsigned)ekf_primary_core); gcs().send_text(MAV_SEVERITY_WARNING, "EKF primary changed:%d", (unsigned)ekf_primary_core);