From e42cf918fdb8cbde58d9620f320a1c991977bd2f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 21 Jan 2013 13:55:06 +1100
Subject: [PATCH] HAL_PX4: added AnalogIn driver

this allows airspeed to work on a PX4
---
 libraries/AP_HAL_PX4/AnalogIn.cpp | 132 ++++++++++++++++++++++++++++++
 libraries/AP_HAL_PX4/AnalogIn.h   |  43 ++++++++++
 2 files changed, 175 insertions(+)
 create mode 100644 libraries/AP_HAL_PX4/AnalogIn.cpp
 create mode 100644 libraries/AP_HAL_PX4/AnalogIn.h

diff --git a/libraries/AP_HAL_PX4/AnalogIn.cpp b/libraries/AP_HAL_PX4/AnalogIn.cpp
new file mode 100644
index 0000000000..910801efe1
--- /dev/null
+++ b/libraries/AP_HAL_PX4/AnalogIn.cpp
@@ -0,0 +1,132 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+#include "AnalogIn.h"
+#include <drivers/drv_adc.h>
+#include <stdio.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <nuttx/analog/adc.h>
+#include <poll.h>
+
+#define ANLOGIN_DEBUGGING 0
+
+#if ANLOGIN_DEBUGGING
+ # define Debug(fmt, args ...)  do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
+#else
+ # define Debug(fmt, args ...)
+#endif
+
+extern const AP_HAL::HAL& hal;
+
+using namespace PX4;
+
+int PX4AnalogIn::_adc_fd;
+uint32_t PX4AnalogIn::_last_run;
+PX4AnalogSource* PX4AnalogIn::_channels[PX4_ANALOG_MAX_CHANNELS] = {};
+
+PX4AnalogSource::PX4AnalogSource(int16_t pin, float initial_value, float scale) :
+	_pin(pin),
+    _value(initial_value),
+    _latest_value(initial_value),
+    _sum_count(0),
+    _sum_value(0),
+    _scale(scale)
+{}
+
+float PX4AnalogSource::read_average() 
+{
+    if (_sum_count == 0) {
+        return _value;
+    }
+    hal.scheduler->suspend_timer_procs();
+    _value = _sum_value / _sum_count;
+    _sum_value = 0;
+    _sum_count = 0;
+    hal.scheduler->resume_timer_procs();
+    return _value;
+}
+
+float PX4AnalogSource::read_latest() 
+{
+    return _latest_value;
+}
+
+void PX4AnalogSource::set_pin(uint8_t pin)
+{
+    _pin = pin;
+}
+
+
+PX4AnalogIn::PX4AnalogIn()
+{}
+
+void PX4AnalogIn::init(void* machtnichts)
+{
+	_adc_fd = open(ADC_DEVICE_PATH, O_RDONLY | O_NONBLOCK);
+    if (_adc_fd == -1) {
+        hal.scheduler->panic("Unable to open " ADC_DEVICE_PATH);
+	}
+    hal.scheduler->register_timer_process(_analogin_timer);
+}
+
+/*
+  called at 1kHz
+ */
+void PX4AnalogIn::_analogin_timer(uint32_t now)
+{
+    // read adc at 100Hz
+    uint32_t delta_t = now - _last_run;
+    if (delta_t < 10000) {
+        return;
+    }
+    _last_run = now;
+
+    struct adc_msg_s buf_adc[8];
+
+    /* read all channels available */
+    int ret = read(_adc_fd, &buf_adc, sizeof(buf_adc));
+    if (ret == -1) return;
+
+    // match the incoming channels to the currently active pins
+    for (uint8_t i=0; i<ret/sizeof(buf_adc[0]); i++) {
+        Debug("chan %u value=%u\n",
+              (unsigned)buf_adc[i].am_channel,
+              (unsigned)buf_adc[i].am_data);
+        for (uint8_t j=0; j<PX4_ANALOG_MAX_CHANNELS; j++) {
+            PX4::PX4AnalogSource *c = _channels[j];
+            if (c != NULL && buf_adc[i].am_channel == c->_pin) {
+                c->_latest_value = buf_adc[i].am_data;
+                c->_sum_value += c->_latest_value;
+                c->_sum_count++;
+                if (c->_sum_count == 254) {
+                    c->_sum_value /= 2;
+                    c->_sum_count /= 2;
+                }
+            }
+        }
+    }
+}
+
+AP_HAL::AnalogSource* PX4AnalogIn::channel(int16_t pin) 
+{
+    return channel(pin, 1.0);
+}
+
+AP_HAL::AnalogSource* PX4AnalogIn::channel(int16_t pin, float scale) 
+{
+    for (uint8_t j=0; j<PX4_ANALOG_MAX_CHANNELS; j++) {
+        if (_channels[j] == NULL) {
+            _channels[j] = new PX4AnalogSource(pin, 0.0, scale);
+            return _channels[j];
+        }
+    }
+    hal.console->println("Out of analog channels");
+    return NULL;
+}
+
+#endif // CONFIG_HAL_BOARD
diff --git a/libraries/AP_HAL_PX4/AnalogIn.h b/libraries/AP_HAL_PX4/AnalogIn.h
new file mode 100644
index 0000000000..68b4001236
--- /dev/null
+++ b/libraries/AP_HAL_PX4/AnalogIn.h
@@ -0,0 +1,43 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef __AP_HAL_PX4_ANALOGIN_H__
+#define __AP_HAL_PX4_ANALOGIN_H__
+
+#include <AP_HAL_PX4.h>
+#include <pthread.h>
+
+#define PX4_ANALOG_MAX_CHANNELS 8
+
+class PX4::PX4AnalogSource : public AP_HAL::AnalogSource {
+public:
+    friend class PX4::PX4AnalogIn;
+    PX4AnalogSource(int16_t pin, float initial_value, float scale);
+    float read_average();
+    float read_latest();
+    void set_pin(uint8_t p);
+private:
+    // what pin it is attached to
+    int16_t _pin;
+
+    // what value it has
+    float _value;
+    float _latest_value;
+    uint8_t _sum_count;
+    float _sum_value;
+    float _scale;
+};
+
+class PX4::PX4AnalogIn : public AP_HAL::AnalogIn {
+public:
+    PX4AnalogIn();
+    void init(void* implspecific);
+    AP_HAL::AnalogSource* channel(int16_t pin);
+    AP_HAL::AnalogSource* channel(int16_t pin, float scale);
+
+private:
+    static int _adc_fd;
+    static PX4::PX4AnalogSource* _channels[PX4_ANALOG_MAX_CHANNELS];
+    static void _analogin_timer(uint32_t now);
+    static uint32_t _last_run;
+};
+#endif // __AP_HAL_PX4_ANALOGIN_H__