From 66ad7ff8e7434d729340a91c30e30228ff7cd04e Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Tue, 6 Jul 2021 16:10:33 -0300 Subject: [PATCH] Sub: use scheduler.get_loop_period_s() instead of MAIN_LOOP_SECONDS --- ArduSub/Sub.cpp | 4 ++-- ArduSub/config.h | 1 - 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp index 7879019bd6..c55a6cd1a3 100644 --- a/ArduSub/Sub.cpp +++ b/ArduSub/Sub.cpp @@ -32,8 +32,8 @@ Sub::Sub() auto_yaw_mode(AUTO_YAW_LOOK_AT_NEXT_WP), inertial_nav(ahrs), ahrs_view(ahrs, ROTATION_NONE), - attitude_control(ahrs_view, aparm, motors, MAIN_LOOP_SECONDS), - pos_control(ahrs_view, inertial_nav, motors, attitude_control, MAIN_LOOP_SECONDS), + attitude_control(ahrs_view, aparm, motors, scheduler.get_loop_period_s()), + pos_control(ahrs_view, inertial_nav, motors, attitude_control, scheduler.get_loop_period_s()), wp_nav(inertial_nav, ahrs_view, pos_control, attitude_control), loiter_nav(inertial_nav, ahrs_view, pos_control, attitude_control), circle_nav(inertial_nav, ahrs_view, pos_control), diff --git a/ArduSub/config.h b/ArduSub/config.h index 8febdaee95..63a7bc6c97 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -14,7 +14,6 @@ // run at 400Hz on all systems # define MAIN_LOOP_RATE 400 -# define MAIN_LOOP_SECONDS 0.0025f #ifndef SURFACE_DEPTH_DEFAULT # define SURFACE_DEPTH_DEFAULT -10.0f // pressure sensor reading 10cm depth means craft is considered surfaced