From f35a94a73079bb74684ef8e7cb507b2a610cc3c9 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 12 Aug 2021 10:46:07 +1000 Subject: [PATCH] ArduSub: DCM handles centrifugal correction application internally now --- ArduSub/AP_Arming_Sub.cpp | 4 ---- ArduSub/Parameters.cpp | 2 -- 2 files changed, 6 deletions(-) diff --git a/ArduSub/AP_Arming_Sub.cpp b/ArduSub/AP_Arming_Sub.cpp index 312d38c980..f3edcbec7d 100644 --- a/ArduSub/AP_Arming_Sub.cpp +++ b/ArduSub/AP_Arming_Sub.cpp @@ -126,8 +126,6 @@ bool AP_Arming_Sub::arm(AP_Arming::Method method, bool do_arming_checks) } } - // enable gps velocity based centrefugal force compensation - ahrs.set_correct_centrifugal(true); hal.util->set_soft_armed(true); // enable output to motors @@ -187,8 +185,6 @@ bool AP_Arming_Sub::disarm(const AP_Arming::Method method, bool do_disarm_checks AP::logger().set_vehicle_armed(false); - // disable gps velocity based centrefugal force compensation - ahrs.set_correct_centrifugal(false); hal.util->set_soft_armed(false); // clear input holds diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 7d70b1daa9..d1bed58b2f 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -688,8 +688,6 @@ void Sub::load_parameters() AP_HAL::panic("Bad var table"); } - // disable centrifugal force correction, it will be enabled as part of the arming process - ahrs.set_correct_centrifugal(false); hal.util->set_soft_armed(false); if (!g.format_version.load() ||