From 4e0806186f5ab7b19b748244706c89c8347d276a Mon Sep 17 00:00:00 2001
From: James Bielman <jamesjb@galois.com>
Date: Mon, 7 Jan 2013 10:48:56 -0800
Subject: [PATCH] AP_HAL_SMACCM: Implement RCInput driver.

---
 libraries/AP_HAL_SMACCM/HAL_SMACCM_Class.cpp |   1 +
 libraries/AP_HAL_SMACCM/RCInput.cpp          | 103 +++++++++++++++----
 libraries/AP_HAL_SMACCM/RCInput.h            |   6 +-
 3 files changed, 89 insertions(+), 21 deletions(-)

diff --git a/libraries/AP_HAL_SMACCM/HAL_SMACCM_Class.cpp b/libraries/AP_HAL_SMACCM/HAL_SMACCM_Class.cpp
index 91101700cd..f720d03bf1 100644
--- a/libraries/AP_HAL_SMACCM/HAL_SMACCM_Class.cpp
+++ b/libraries/AP_HAL_SMACCM/HAL_SMACCM_Class.cpp
@@ -51,6 +51,7 @@ void HAL_SMACCM::init(int argc,char* const argv[]) const
   i2c->begin();
   spi->init(NULL);
   storage->init(NULL);
+  rcin->init(NULL);
 }
 
 const HAL_SMACCM AP_HAL_SMACCM;
diff --git a/libraries/AP_HAL_SMACCM/RCInput.cpp b/libraries/AP_HAL_SMACCM/RCInput.cpp
index 552d827080..a8db128eef 100644
--- a/libraries/AP_HAL_SMACCM/RCInput.cpp
+++ b/libraries/AP_HAL_SMACCM/RCInput.cpp
@@ -1,38 +1,101 @@
 
 #include "RCInput.h"
 
+#include <hwf4/timer.h>
+
 using namespace SMACCM;
+
+/* Constrain captured pulse to be between min and max pulsewidth. */
+static inline uint16_t constrain_pulse(uint16_t p)
+{
+  if (p > RC_INPUT_MAX_PULSEWIDTH)
+    return RC_INPUT_MAX_PULSEWIDTH;
+  if (p < RC_INPUT_MIN_PULSEWIDTH)
+    return RC_INPUT_MIN_PULSEWIDTH;
+
+  return p;
+}
+
 SMACCMRCInput::SMACCMRCInput()
-{}
+{
+}
+
+void SMACCMRCInput::init(void *unused)
+{
+  clear_overrides();
+}
 
-void SMACCMRCInput::init(void* machtnichts)
-{}
+uint8_t SMACCMRCInput::valid()
+{
+  // If any of the overrides are positive, we have valid data.
+  for (int i = 0; i < PPM_MAX_CHANNELS; ++i)
+    if (_override[i] > 0)
+      return true;
 
-uint8_t SMACCMRCInput::valid() {
-    return 0;
+  return timer_is_ppm_valid();
 }
 
-uint16_t SMACCMRCInput::read(uint8_t ch) {
-    if (ch == 3) return 900; /* throttle should be low, for safety */
-    else return 1500;
+// It looks like the APM2 driver clears the PPM sample after this
+// function is called, so we do the same thing here for compatibility.
+uint16_t SMACCMRCInput::read(uint8_t ch)
+{
+  uint16_t result = 0;
+
+  if (_override[ch] != 0) {
+    result = _override[ch];
+  } else {
+    timer_get_ppm_channel(ch, &result);
+    result = constrain_pulse(result);
+  }
+
+  timer_clear_ppm();
+  return constrain_pulse(result);
 }
 
-uint8_t SMACCMRCInput::read(uint16_t* periods, uint8_t len) {
-    for (uint8_t i = 0; i < len; i++){
-        if (i == 3) periods[i] = 900;
-        else periods[i] = 1500;
-    }
-    return len;
+// It looks like the APM2 driver clears the PPM sample after this
+// function is called, so we do the same thing here for compatibility.
+uint8_t SMACCMRCInput::read(uint16_t *periods, uint8_t len)
+{
+  uint8_t result;
+  result = timer_get_ppm(periods, len, NULL);
+
+  for (int i = 0; i < result; ++i) {
+    periods[i] = constrain_pulse(periods[i]);
+    if (_override[i] != 0)
+      periods[i] = _override[i];
+  }
+
+  timer_clear_ppm();
+  return result;
 }
 
-bool SMACCMRCInput::set_overrides(int16_t *overrides, uint8_t len) {
-    return true;
+bool SMACCMRCInput::set_overrides(int16_t *overrides, uint8_t len)
+{
+  bool result = false;
+
+  for (int i = 0; i < len; ++i)
+    result |= set_override(i, overrides[i]);
+
+  return result;
 }
 
-bool SMACCMRCInput::set_override(uint8_t channel, int16_t override) {
-    return true;
+bool SMACCMRCInput::set_override(uint8_t channel, int16_t override)
+{
+  if (override < 0)
+    return false;
+
+  if (channel < PPM_MAX_CHANNELS) {
+    _override[channel] = override;
+    if (override != 0) {
+      return true;
+    }
+  }
+
+  return false;
 }
 
 void SMACCMRCInput::clear_overrides()
-{}
-
+{
+  for (int i = 0; i < PPM_MAX_CHANNELS; ++i)
+    _override[i] = 0;
+}
diff --git a/libraries/AP_HAL_SMACCM/RCInput.h b/libraries/AP_HAL_SMACCM/RCInput.h
index ed1cd06f5d..265d3d589b 100644
--- a/libraries/AP_HAL_SMACCM/RCInput.h
+++ b/libraries/AP_HAL_SMACCM/RCInput.h
@@ -3,11 +3,12 @@
 #define __AP_HAL_SMACCM_RCINPUT_H__
 
 #include <AP_HAL_SMACCM.h>
+#include <hwf4/timer.h>
 
 class SMACCM::SMACCMRCInput : public AP_HAL::RCInput {
 public:
     SMACCMRCInput();
-    void init(void* machtnichts);
+    void init(void *unused);
     uint8_t  valid();
     uint16_t read(uint8_t ch);
     uint8_t read(uint16_t* periods, uint8_t len);
@@ -15,6 +16,9 @@ public:
     bool set_overrides(int16_t *overrides, uint8_t len);
     bool set_override(uint8_t channel, int16_t override);
     void clear_overrides();
+
+private:
+    uint16_t _override[PPM_MAX_CHANNELS];
 };
 
 #endif // __AP_HAL_SMACCM_RCINPUT_H__