mirror of https://github.com/ArduPilot/ardupilot
108 lines
2.6 KiB
C++
108 lines
2.6 KiB
C++
|
|
#include <AP_HAL.h>
|
|
#include "userinput.h"
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
AP_HAL::AnalogSource* UserInput::_joy_x = NULL;
|
|
AP_HAL::AnalogSource* UserInput::_joy_y = NULL;
|
|
DigitalDebounce* UserInput::_side_btn = NULL;
|
|
DigitalDebounce* UserInput::_joy_btn = NULL;
|
|
|
|
uint32_t UserInput::_last_periodic = 0;
|
|
|
|
class DigitalInvert : public AP_HAL::DigitalSource {
|
|
public:
|
|
DigitalInvert(AP_HAL::DigitalSource* p) : _p(p) {}
|
|
uint8_t read() { return (_p->read()) ? 0 : 1; }
|
|
void mode(uint8_t m) { _p->mode(m); }
|
|
void write(uint8_t v) { _p->write( v == 0 ? 1 : 0 ); }
|
|
private:
|
|
AP_HAL::DigitalSource *_p;
|
|
};
|
|
|
|
void UserInput::init( int side_btn_ch, int joy_x_ch,
|
|
int joy_y_ch, int joy_btn_ch) {
|
|
|
|
_joy_x = hal.analogin->channel(joy_x_ch);
|
|
_joy_y = hal.analogin->channel(joy_y_ch);
|
|
_side_btn = new DigitalDebounce(
|
|
new DigitalInvert(hal.gpio->channel(side_btn_ch)), 100);
|
|
_joy_btn = new DigitalDebounce(hal.gpio->channel(joy_btn_ch), 100);
|
|
hal.scheduler->register_timer_process(_periodic);
|
|
}
|
|
|
|
void UserInput::print(AP_HAL::BetterStream* s) {
|
|
s->printf_P(PSTR("side: %d joy: %f, %f, %d\r\n"),
|
|
(int) _side_btn->read(),
|
|
_joy_x->read_average(),
|
|
_joy_y->read_average(),
|
|
(int) _joy_btn->read());
|
|
}
|
|
|
|
void UserInput::_periodic(uint32_t us) {
|
|
uint32_t millis = us / 1000;
|
|
_side_btn->periodic(millis);
|
|
_joy_btn->periodic(millis);
|
|
}
|
|
|
|
bool DigitalDebounce::read() {
|
|
switch (_state) {
|
|
case STATE_DOWN:
|
|
case STATE_RISING:
|
|
return false;
|
|
break;
|
|
case STATE_UP:
|
|
case STATE_FALLING:
|
|
return true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
void DigitalDebounce::periodic(uint32_t millis) {
|
|
bool latest = _in->read();
|
|
uint32_t dt = millis - _last_periodic;
|
|
_last_periodic = millis;
|
|
switch (_state) {
|
|
case STATE_DOWN:
|
|
if (latest == true) {
|
|
_state = STATE_RISING;
|
|
_transition = 0;
|
|
}
|
|
break;
|
|
case STATE_RISING:
|
|
if (latest == true) {
|
|
_transition += dt;
|
|
if (_transition > _thresh_ms) {
|
|
_state = STATE_UP;
|
|
if (_evt_cb) {
|
|
_evt_cb(BUTTON_UP);
|
|
}
|
|
}
|
|
} else {
|
|
_state = STATE_DOWN;
|
|
}
|
|
break;
|
|
case STATE_UP:
|
|
if (latest == false) {
|
|
_state = STATE_FALLING;
|
|
_transition = 0;
|
|
}
|
|
break;
|
|
case STATE_FALLING:
|
|
if (latest == false) {
|
|
_transition += dt;
|
|
if (_transition > _thresh_ms) {
|
|
_state = STATE_DOWN;
|
|
if (_evt_cb) {
|
|
_evt_cb(BUTTON_DOWN);
|
|
}
|
|
}
|
|
} else {
|
|
_state = STATE_UP;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|