From 6682e93bf340fb6ac0b2919cb006cd3b34337e79 Mon Sep 17 00:00:00 2001
From: Jacob Walser <jwalser90@gmail.com>
Date: Sat, 26 Nov 2016 01:27:29 -0500
Subject: [PATCH] Sub: Refactor delay()

We don't need a dedicated file only to define a delay function.
This will also provide a preventitive measure for people trying to use
delay() without an understanding of the implications
---
 ArduSub/Sub.h      |  1 -
 ArduSub/compat.cpp |  6 ------
 ArduSub/radio.cpp  |  2 +-
 ArduSub/setup.cpp  |  2 +-
 ArduSub/system.cpp |  4 ++--
 ArduSub/test.cpp   | 20 ++++++++++----------
 6 files changed, 14 insertions(+), 21 deletions(-)
 delete mode 100644 ArduSub/compat.cpp

diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index 3a1608af2b..43b6b8b805 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -697,7 +697,6 @@ private:
     void do_take_picture();
     void log_picture();
     uint8_t mavlink_compassmot(mavlink_channel_t chan);
-    void delay(uint32_t ms);
     bool acro_init(bool ignore_checks);
     void acro_run();
     void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out);
diff --git a/ArduSub/compat.cpp b/ArduSub/compat.cpp
deleted file mode 100644
index a9a4dab3f8..0000000000
--- a/ArduSub/compat.cpp
+++ /dev/null
@@ -1,6 +0,0 @@
-#include "Sub.h"
-
-void Sub::delay(uint32_t ms)
-{
-    hal.scheduler->delay(ms);
-}
diff --git a/ArduSub/radio.cpp b/ArduSub/radio.cpp
index 26c5611cbf..f1a8545d43 100644
--- a/ArduSub/radio.cpp
+++ b/ArduSub/radio.cpp
@@ -88,7 +88,7 @@ void Sub::init_rc_out()
     motors.Init();                                              // motor initialisation
 
     for(uint8_t i = 0; i < 5; i++) {
-        delay(20);
+    	hal.scheduler->delay(20);
         read_radio();
     }
 
diff --git a/ArduSub/setup.cpp b/ArduSub/setup.cpp
index 222d54f24e..e5a3017b0f 100644
--- a/ArduSub/setup.cpp
+++ b/ArduSub/setup.cpp
@@ -51,7 +51,7 @@ int8_t Sub::setup_factory(uint8_t argc, const Menu::arg *argv)
     AP_Param::erase_all();
     cliSerial->printf("\nReboot board");
 
-    delay(1000);
+    hal.scheduler->delay(1000);
 
     for (;; ) {
     }
diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp
index fbfe549d05..d70573b444 100644
--- a/ArduSub/system.cpp
+++ b/ArduSub/system.cpp
@@ -88,7 +88,7 @@ void Sub::init_ardupilot()
         // least one second after powering up. Simplest solution for
         // now is to delay for 1 second. Something more elegant may be
         // added later
-        delay(1000);
+    	hal.scheduler->delay(1000);
     }
 
     // initialise serial port
@@ -226,7 +226,7 @@ void Sub::init_ardupilot()
         // the barometer begins updating when we get the first
         // HIL_STATE message
         gcs_send_text(MAV_SEVERITY_WARNING, "Waiting for first HIL_STATE message");
-        delay(1000);
+        hal.scheduler->delay(1000);
     }
 
     // set INS to HIL mode
diff --git a/ArduSub/test.cpp b/ArduSub/test.cpp
index 76f15e8a3f..8540c06a63 100644
--- a/ArduSub/test.cpp
+++ b/ArduSub/test.cpp
@@ -40,7 +40,7 @@ int8_t Sub::test_baro(uint8_t argc, const Menu::arg *argv)
     init_barometer(true);
 
     while(1) {
-        delay(100);
+    	hal.scheduler->delay(100);
         read_barometer();
 
         if (!barometer.healthy()) {
@@ -92,7 +92,7 @@ int8_t Sub::test_compass(uint8_t argc, const Menu::arg *argv)
     print_hit_enter();
 
     while(1) {
-        delay(20);
+    	hal.scheduler->delay(20);
         if (millis() - fast_loopTimer > 19) {
             delta_ms_fast_loop      = millis() - fast_loopTimer;
             G_Dt                    = (float)delta_ms_fast_loop / 1000.0f;                       // used by DCM integrator
@@ -149,13 +149,13 @@ int8_t Sub::test_ins(uint8_t argc, const Menu::arg *argv)
     Vector3f gyro, accel;
     print_hit_enter();
     cliSerial->printf("INS\n");
-    delay(1000);
+    hal.scheduler->delay(1000);
 
     ahrs.init();
     ins.init(scheduler.get_loop_rate_hz());
     cliSerial->printf("...done\n");
 
-    delay(50);
+    hal.scheduler->delay(50);
 
     while(1) {
         ins.update();
@@ -169,7 +169,7 @@ int8_t Sub::test_ins(uint8_t argc, const Menu::arg *argv)
             (double)gyro.x, (double)gyro.y, (double)gyro.z,
             (double)test);
 
-        delay(40);
+        hal.scheduler->delay(40);
         if(cliSerial->available() > 0) {
             return (0);
         }
@@ -184,7 +184,7 @@ int8_t Sub::test_optflow(uint8_t argc, const Menu::arg *argv)
         print_hit_enter();
 
         while(1) {
-            delay(200);
+        	hal.scheduler->delay(200);
             optflow.update();
             const Vector2f& flowRate = optflow.flowRate();
             cliSerial->printf("flowX : %7.4f\t flowY : %7.4f\t flow qual : %d\n",
@@ -209,19 +209,19 @@ int8_t Sub::test_optflow(uint8_t argc, const Menu::arg *argv)
 int8_t Sub::test_relay(uint8_t argc, const Menu::arg *argv)
 {
     print_hit_enter();
-    delay(1000);
+    hal.scheduler->delay(1000);
 
     while(1) {
         cliSerial->printf("Relay on\n");
         relay.on(0);
-        delay(3000);
+        hal.scheduler->delay(3000);
         if(cliSerial->available() > 0) {
             return (0);
         }
 
         cliSerial->printf("Relay off\n");
         relay.off(0);
-        delay(3000);
+        hal.scheduler->delay(3000);
         if(cliSerial->available() > 0) {
             return (0);
         }
@@ -252,7 +252,7 @@ int8_t Sub::test_rangefinder(uint8_t argc, const Menu::arg *argv)
 
     print_hit_enter();
     while(1) {
-        delay(100);
+    	hal.scheduler->delay(100);
         rangefinder.update();
 
         cliSerial->printf("Primary: status %d distance_cm %d \n", (int)rangefinder.status(), rangefinder.distance_cm());