From 050a878935fe8e52f6da84a7d19a5d623a3c3e43 Mon Sep 17 00:00:00 2001
From: Pat Hickey <pat@galois.com>
Date: Tue, 9 Oct 2012 14:01:58 -0700
Subject: [PATCH] AC_PID: Ported to AP_HAL.

---
 libraries/AC_PID/AC_PID.h                     |  7 ++--
 .../examples/AC_PID_test/AC_PID_test.pde      | 39 ++++++++-----------
 .../AC_PID/examples/AC_PID_test/Arduino.h     |  0
 .../examples/AC_PID_test/nocore.inoflag       |  0
 4 files changed, 20 insertions(+), 26 deletions(-)
 create mode 100644 libraries/AC_PID/examples/AC_PID_test/Arduino.h
 create mode 100644 libraries/AC_PID/examples/AC_PID_test/nocore.inoflag

diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h
index af442ee87f..d3122252cc 100644
--- a/libraries/AC_PID/AC_PID.h
+++ b/libraries/AC_PID/AC_PID.h
@@ -3,11 +3,12 @@
 /// @file	AC_PID.h
 /// @brief	Generic PID algorithm, with EEPROM-backed storage of constants.
 
-#ifndef AC_PID_h
-#define AC_PID_h
+#ifndef __AC_PID_H__
+#define __AC_PID_H__
 
 #include <AP_Common.h>
 #include <AP_Param.h>
+#include <stdlib.h>
 #include <math.h>               // for fabs()
 
 /// @class	AC_PID
@@ -140,4 +141,4 @@ private:
     // f_cut = 30 Hz -> _filter =  5.3052e-3
 };
 
-#endif
+#endif // __AC_PID_H__
diff --git a/libraries/AC_PID/examples/AC_PID_test/AC_PID_test.pde b/libraries/AC_PID/examples/AC_PID_test/AC_PID_test.pde
index 453318fce4..2c751b410c 100644
--- a/libraries/AC_PID/examples/AC_PID_test/AC_PID_test.pde
+++ b/libraries/AC_PID/examples/AC_PID_test/AC_PID_test.pde
@@ -3,14 +3,15 @@
  *       2012 Code by Jason Short, Randy Mackay. DIYDrones.com
  */
 
-// includes
-#include <Arduino_Mega_ISR_Registry.h>
-#include <FastSerial.h>
 #include <AP_Common.h>
+
+#include <AP_HAL.h>
+#include <AP_HAL_AVR.h>
 #include <AP_Math.h>
 #include <AP_Param.h>
-#include <APM_RC.h> // ArduPilot RC Library
-#include <AC_PID.h> // ArduPilot Mega RC Library
+#include <AC_PID.h>
+
+const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
 
 // default PID values
 #define TEST_P 1.0
@@ -18,23 +19,13 @@
 #define TEST_D 0.2
 #define TEST_IMAX 10
 
-// Serial ports
-FastSerialPort0(Serial);        // FTDI/console
-
-// Global variables
-Arduino_Mega_ISR_Registry isr_registry;
-APM_RC_APM1 APM_RC;
-
 // setup function
 void setup()
 {
-    Serial.begin(115200);
-    Serial.println("ArduPilot Mega AC_PID library test");
+    hal.uart0->begin(115200);
+    hal.console->println("ArduPilot Mega AC_PID library test");
 
-    isr_registry.init();
-    APM_RC.Init(&isr_registry);          // APM Radio initialization
-
-    delay(1000);
+    hal.scheduler->delay(1000);
 }
 
 // main loop
@@ -49,18 +40,20 @@ void loop()
     float dt = 1000/50;
 
     // display PID gains
-    Serial.printf("P %f  I %f  D %f  imax %f\n", pid.kP(), pid.kI(), pid.kD(), pid.imax());
+    hal.console->printf("P %f  I %f  D %f  imax %f\n", pid.kP(), pid.kI(), pid.kD(), pid.imax());
 
     // capture radio trim
-    radio_trim = APM_RC.InputCh(0);
+    radio_trim = hal.rcin->read(0);
 
     while( true ) {
-        radio_in = APM_RC.InputCh(0);
+        radio_in = hal.rcin->read(0);
         error = radio_in - radio_trim;
         control = pid.get_pid(error, dt);
 
         // display pid results
-        Serial.printf("radio: %d\t err: %d\t pid:%d\n", radio_in, error, control);
-        delay(50);
+        hal.console->printf("radio: %d\t err: %d\t pid:%d\n", radio_in, error, control);
+        hal.scheduler->delay(50);
     }
 }
+
+AP_HAL_MAIN();
diff --git a/libraries/AC_PID/examples/AC_PID_test/Arduino.h b/libraries/AC_PID/examples/AC_PID_test/Arduino.h
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/libraries/AC_PID/examples/AC_PID_test/nocore.inoflag b/libraries/AC_PID/examples/AC_PID_test/nocore.inoflag
new file mode 100644
index 0000000000..e69de29bb2