HAL_VRBRAIN: updated analog drivers and scheduler driver to latest official version

libraries/AP_HAL_VRBRAIN/AnalogIn.cpp

@@ -72,6 +72,8 @@ using namespace VRBRAIN;
 
 VRBRAINAnalogSource::VRBRAINAnalogSource(int16_t pin, float initial_value) :
 	_pin(pin),
+    _stop_pin(-1),
+    _settle_time_ms(0),
     _value(initial_value),
     _value_ratiometric(initial_value),
     _latest_value(initial_value),
@@ -81,6 +83,11 @@ VRBRAINAnalogSource::VRBRAINAnalogSource(int16_t pin, float initial_value) :
 {
 }
 
+void VRBRAINAnalogSource::set_stop_pin(uint8_t p)
+{ 
+    _stop_pin = p; 
+}
+
 float VRBRAINAnalogSource::read_average()
 {
     if (_sum_count == 0) {
@@ -183,9 +190,10 @@ void VRBRAINAnalogSource::_add_value(float v, float vcc5V)
 
 
 VRBRAINAnalogIn::VRBRAINAnalogIn() :
+    _current_stop_pin_i(0),
 	_board_voltage(0),
     _servorail_voltage(0),
-    _power_flags(0)    
+    _power_flags(0)
 {}
 
 void VRBRAINAnalogIn::init(void* machtnichts)
@@ -199,6 +207,40 @@ void VRBRAINAnalogIn::init(void* machtnichts)
     _system_power_handle = orb_subscribe(ORB_ID(system_power));
 }
 
+
+/*
+  move to the next stop pin
+ */
+void VRBRAINAnalogIn::next_stop_pin(void)
+{
+    // find the next stop pin. We start one past the current stop pin
+    // and wrap completely, so we do the right thing is there is only
+    // one stop pin
+    for (uint8_t i=1; i <= VRBRAIN_ANALOG_MAX_CHANNELS; i++) {
+        uint8_t idx = (_current_stop_pin_i + i) % VRBRAIN_ANALOG_MAX_CHANNELS;
+        VRBRAIN::VRBRAINAnalogSource *c = _channels[idx];
+        if (c && c->_stop_pin != -1) {
+            // found another stop pin
+            _stop_pin_change_time = hal.scheduler->millis();
+            _current_stop_pin_i = idx;
+
+            // set that pin high
+            hal.gpio->pinMode(c->_stop_pin, 1);
+            hal.gpio->write(c->_stop_pin, 1);
+
+            // set all others low
+            for (uint8_t j=0; j<VRBRAIN_ANALOG_MAX_CHANNELS; j++) {
+                VRBRAIN::VRBRAINAnalogSource *c2 = _channels[j];
+                if (c2 && c2->_stop_pin != -1 && j != idx) {
+                    hal.gpio->pinMode(c2->_stop_pin, 1);
+                    hal.gpio->write(c2->_stop_pin, 0);
+                }
+            }
+            break;
+        }
+    }
+}
+
 /*
   called at 1kHz
  */
@@ -214,6 +256,12 @@ void VRBRAINAnalogIn::_timer_tick(void)
 
     struct adc_msg_s buf_adc[VRBRAIN_ANALOG_MAX_CHANNELS];
 
+    // cope with initial setup of stop pin
+    if (_channels[_current_stop_pin_i] == NULL ||
+        _channels[_current_stop_pin_i]->_stop_pin == -1) {
+        next_stop_pin();
+    }
+
     /* read all channels available */
     int ret = read(_adc_fd, &buf_adc, sizeof(buf_adc));
     if (ret > 0) {
@@ -225,7 +273,16 @@ void VRBRAINAnalogIn::_timer_tick(void)
             for (uint8_t j=0; j<VRBRAIN_ANALOG_MAX_CHANNELS; j++) {
                 VRBRAIN::VRBRAINAnalogSource *c = _channels[j];
                 if (c != NULL && buf_adc[i].am_channel == c->_pin) {
-                    c->_add_value(buf_adc[i].am_data, _board_voltage);
+                    // add a value if either there is no stop pin, or
+                    // the stop pin has been settling for enough time
+                    if (c->_stop_pin == -1 || 
+                        (_current_stop_pin_i == j &&
+                         hal.scheduler->millis() - _stop_pin_change_time > c->_settle_time_ms)) {
+                        c->_add_value(buf_adc[i].am_data, _board_voltage);
+                        if (c->_stop_pin != -1 && _current_stop_pin_i == j) {
+                            next_stop_pin();
+                        }
+                    }
                 }
             }
         }

libraries/AP_HAL_VRBRAIN/AnalogIn.h

@@ -41,13 +41,15 @@ public:
     float voltage_latest();
     float voltage_average_ratiometric();
 
-    // stop pins not implemented on VRBRAIN yet
-    void set_stop_pin(uint8_t p) {}
-    void set_settle_time(uint16_t settle_time_ms) {}
+    // implement stop pins
+    void set_stop_pin(uint8_t p);
+    void set_settle_time(uint16_t settle_time_ms) { _settle_time_ms = settle_time_ms; }
 
 private:
     // what pin it is attached to
     int16_t _pin;
+    int16_t _stop_pin;
+    uint16_t _settle_time_ms;
 
     // what value it has
     float _value;
@@ -78,9 +80,16 @@ private:
     uint64_t _battery_timestamp;
     uint64_t _servorail_timestamp;
     VRBRAIN::VRBRAINAnalogSource* _channels[VRBRAIN_ANALOG_MAX_CHANNELS];
+
+    // what pin is currently held low to stop a sonar from reading
+    uint8_t _current_stop_pin_i;
+    uint32_t _stop_pin_change_time;
+
     uint32_t _last_run;
     float _board_voltage;
     float _servorail_voltage;
     uint16_t _power_flags;
+
+    void next_stop_pin(void);
 };
 #endif // __AP_HAL_VRBRAIN_ANALOGIN_H__

libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp

@@ -118,7 +118,7 @@ HAL_VRBRAIN::HAL_VRBRAIN() :
 bool _vrbrain_thread_should_exit = false;        /**< Daemon exit flag */
 static bool thread_running = false;        /**< Daemon status flag */
 static int daemon_task;                /**< Handle of daemon task / thread */
-static bool ran_overtime;
+bool vrbrain_ran_overtime;
 
 extern const AP_HAL::HAL& hal;
 
@@ -141,7 +141,7 @@ static void set_priority(uint8_t priority)
 static void loop_overtime(void *)
 {
     set_priority(APM_OVERTIME_PRIORITY);
-    ran_overtime = true;
+    vrbrain_ran_overtime = true;
 }
 
 static int main_loop(int argc, char **argv)
@@ -197,14 +197,14 @@ static int main_loop(int argc, char **argv)
 
         loop();
 
-        if (ran_overtime) {
+        if (vrbrain_ran_overtime) {
             /*
               we ran over 1s in loop(), and our priority was lowered
               to let a driver run. Set it back to high priority now.
              */
             set_priority(APM_MAIN_PRIORITY);
             perf_count(perf_overrun);
-            ran_overtime = false;
+            vrbrain_ran_overtime = false;
         }
 
         perf_end(perf_loop);

libraries/AP_HAL_VRBRAIN/Scheduler.cpp

@@ -22,6 +22,7 @@
 #include "Storage.h"
 #include "RCOutput.h"
 #include "RCInput.h"
+#include <AP_Scheduler.h>
 
 using namespace VRBRAIN;
 
@@ -229,8 +230,11 @@ void VRBRAINScheduler::_run_timers(bool called_from_timer_thread)
     _in_timer_proc = false;
 }
 
+extern bool vrbrain_ran_overtime;
+
 void *VRBRAINScheduler::_timer_thread(void)
 {
+    uint32_t last_ran_overtime = 0;
     while (!_hal_initialized) {
         poll(NULL, 0, 1);        
     }
@@ -247,6 +251,12 @@ void *VRBRAINScheduler::_timer_thread(void)
 
         // process any pending RC input requests
         ((VRBRAINRCInput *)hal.rcin)->_timer_tick();
+
+        if (vrbrain_ran_overtime && millis() - last_ran_overtime > 2000) {
+            last_ran_overtime = millis();
+            printf("Overtime in task %d\n", (int)AP_Scheduler::current_task);
+            hal.console->printf("Overtime in task %d\n", (int)AP_Scheduler::current_task);
+        }
     }
     return NULL;
 }