Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_PX4: implement analog input stop pins

Browse Source 
this allows multiple sonars to cooperate without interference
This commit is contained in:
Andrew Tridgell 2014-06-02 08:35:41 +10:00
parent 24325a5c62
commit fa4eb5475a
2 changed files with 72 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
libraries/AP_HAL_PX4/AnalogIn.cpp
View File

@ -18,6 +18,7 @@
#include <uORB/topics/system_power.h>
#include <GCS_MAVLink.h>
#include <errno.h>
#include "GPIO.h"
#define ANLOGIN_DEBUGGING 0
@ -73,6 +74,8 @@ PX4AnalogSource::PX4AnalogSource(int16_t pin, float initial_value) :
_latest_value(initial_value),
_sum_count(0),
_sum_value(0),
_stop_pin(-1),
_settle_time_ms(0),
_sum_ratiometric(0)
{
#ifdef PX4_ANALOG_VCC_5V_PIN
@ -82,6 +85,11 @@ PX4AnalogSource::PX4AnalogSource(int16_t pin, float initial_value) :
#endif
}
void PX4AnalogSource::set_stop_pin(uint8_t p)
{
_stop_pin = p;
}
float PX4AnalogSource::read_average()
{
if (_sum_count == 0) {
@ -186,7 +194,8 @@ void PX4AnalogSource::_add_value(float v, float vcc5V)
PX4AnalogIn::PX4AnalogIn() :
_board_voltage(0),
_servorail_voltage(0),
_power_flags(0)
_power_flags(0),
_current_stop_pin_i(0)
{}
void PX4AnalogIn::init(void* machtnichts)
@ -200,6 +209,40 @@ void PX4AnalogIn::init(void* machtnichts)
_system_power_handle = orb_subscribe(ORB_ID(system_power));
}
/*
move to the next stop pin
*/
void PX4AnalogIn::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 <= PX4_ANALOG_MAX_CHANNELS; i++) {
uint8_t idx = (_current_stop_pin_i + i) % PX4_ANALOG_MAX_CHANNELS;
PX4::PX4AnalogSource *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<PX4_ANALOG_MAX_CHANNELS; j++) {
PX4::PX4AnalogSource *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
*/
@ -215,6 +258,12 @@ void PX4AnalogIn::_timer_tick(void)
struct adc_msg_s buf_adc[PX4_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) {
@ -235,7 +284,16 @@ void PX4AnalogIn::_timer_tick(void)
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->_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();
}
}
}
}
}

15
libraries/AP_HAL_PX4/AnalogIn.h
View File

@ -31,13 +31,15 @@ public:
float voltage_latest();
float voltage_average_ratiometric();
// stop pins not implemented on PX4 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;
@ -68,9 +70,16 @@ private:
uint64_t _battery_timestamp;
uint64_t _servorail_timestamp;
PX4::PX4AnalogSource* _channels[PX4_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_PX4_ANALOGIN_H__