Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

FollowMe: removing because no longer supported 

Android tablets and phones have replaced the need for this device and we
haven't been maintaining it.
This commit is contained in:
Randy Mackay 2015-05-15 15:32:51 +09:00
parent b0a90df135
commit 227327c678
14 changed files with 0 additions and 824 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
FollowMe/FollowMe.pde
View File

@ -1,100 +0,0 @@
// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*-
#include <AP_Common.h>
#include <AP_Progmem.h>
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
#include <AP_Param.h>
#include <AP_Math.h>
#include <GCS_MAVLink.h>
#include <GCS_Console.h>
#include <AP_GPS.h>
#include "simplegcs.h"
#include "downstream.h"
#include "upstream.h"
#include "userinput.h"
#include "state.h"
/* Does the Followme device send a heartbeat? Helpful for debugging. */
#define CONFIG_FOLLOWME_SENDS_HEARTBEAT 1
/* Does the hal console tunnel over mavlink? Requires patched MAVProxy. */
#define CONFIG_FOLLOWME_MAVCONSOLE 0
const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
mavlink_channel_t upstream_channel = MAVLINK_COMM_1;
mavlink_channel_t downstream_channel = MAVLINK_COMM_0;
GPS* gps;
AP_GPS_Auto auto_gps(&gps);
FMStateMachine sm;
UserInput input;
static void sm_on_button_activate(int event) {
if (event == DigitalDebounce::BUTTON_DOWN) {
sm.on_button_activate();
}
}
static void sm_on_button_cancel(int event) {
if (event == DigitalDebounce::BUTTON_DOWN) {
sm.on_button_cancel();
}
}
void setup(void) {
/* Allocate large enough buffers on uart0 to support mavlink */
hal.uartA->begin(57600, 256, 256);
/* Incoming from radio */
hal.uartC->begin(57600, 256, 256);
/* Don't need such big buffers for GPS */
hal.uartB->begin(57600, 256, 16);
/* Setup GCS_Mavlink library's comm 0 port. */
mavlink_comm_0_port = hal.uartA;
/* Setup GCS_Mavlink library's comm 1 port to UART2 (accessible on APM2) */
mavlink_comm_1_port = hal.uartC;
#if CONFIG_FOLLOWME_SENDS_HEARTBEAT
simplegcs_send_heartbeat(downstream_channel);
hal.scheduler->register_timer_process(simplegcs_send_heartbeat_async);
#endif
#if CONFIG_FOLLOWME_MAVCONSOLE
hal.scheduler->register_timer_process(simplegcs_send_console_async);
hal.console->backend_open();
hal.scheduler->delay(1000);
#endif
hal.console->println_P(PSTR("User input init"));
input.init(57, 0, 1, 51);
input.side_btn_event_callback(sm_on_button_activate);
input.joy_btn_event_callback(sm_on_button_cancel);
hal.console->println_P(PSTR("GPS start init"));
auto_gps.init(hal.uartB, GPS::GPS_ENGINE_PEDESTRIAN, &DataFlash);
}
void loop(void) {
if (gps != NULL) {
gps->update();
} else {
auto_gps.update();
}
sm.on_loop(gps);
/* Receive messages off the downstream, send them upstream: */
simplegcs_update(downstream_channel, upstream_handler);
/* Receive messages off the downstream, send them upstream: */
simplegcs_update(upstream_channel, downstream_handler);
}
AP_HAL_MAIN();

1
FollowMe/Makefile
View File

@ -1 +0,0 @@
include ../mk/Arduino.mk

28
FollowMe/arducopter_defines.h
View File

@ -1,28 +0,0 @@
#ifndef __FOLLOWME_ARDUCOPTER_DEFINES_H__
#define __FOLLOWME_ARDUCOPTER_DEFINES_H__
/* These have been taken from the ArduCopter/defines.h. Kinda wish they were
* globally accessible...
* prefixed with MODE_ for namespacing. */
// Auto Pilot modes
// ----------------
#define MODE_STABILIZE 0 // hold level position
#define MODE_ACRO 1 // rate control
#define MODE_ALT_HOLD 2 // AUTO control
#define MODE_AUTO 3 // AUTO control
#define MODE_GUIDED 4 // AUTO control
#define MODE_LOITER 5 // Hold a single location
#define MODE_RTL 6 // AUTO control
#define MODE_CIRCLE 7 // AUTO control
#define MODE_POSITION 8 // AUTO control
#define MODE_LAND 9 // AUTO control
#define MODE_OF_LOITER 10 // Hold a single location using optical flow
// sensor
#define MODE_TOY_A 11 // THOR Enum for Toy mode
#define MODE_TOY_M 12 // THOR Enum for Toy mode
#define MODE_NUM_MODES 13
#endif //

41
FollowMe/downstream.cpp
View File

@ -1,41 +0,0 @@
#include <AP_HAL.h>
#include "downstream.h"
#include "state.h"
extern const AP_HAL::HAL& hal;
extern mavlink_channel_t downstream_channel;
extern FMStateMachine sm;
static void downstream_handle_heartbeat(mavlink_message_t* msg) __attribute__((noinline));
static void downstream_handle_heartbeat(mavlink_message_t* msg) {
mavlink_heartbeat_t pkt;
mavlink_msg_heartbeat_decode(msg, &pkt);
sm.on_downstream_heartbeat(&pkt);
}
static void downstream_handle_gps(mavlink_message_t* msg) __attribute__((noinline));
static void downstream_handle_gps(mavlink_message_t* msg) {
mavlink_gps_raw_int_t pkt;
mavlink_msg_gps_raw_int_decode(msg, &pkt);
sm.on_downstream_gps_raw_int(&pkt);
}
void downstream_handler(mavlink_channel_t from, mavlink_message_t* msg) {
switch (msg->msgid) {
case MAVLINK_MSG_ID_HEARTBEAT:
downstream_handle_heartbeat(msg);
_mavlink_resend_uart(downstream_channel, msg);
break;
case MAVLINK_MSG_ID_GPS_RAW_INT:
downstream_handle_gps(msg);
_mavlink_resend_uart(downstream_channel, msg);
break;
default:
_mavlink_resend_uart(downstream_channel, msg);
}
}

10
FollowMe/downstream.h
View File

@ -1,10 +0,0 @@
#ifndef __FOLLOWME_DOWNSTREAM_H__
#define __FOLLOWME_DOWNSTREAM_H__
#include <GCS_MAVLink.h>
void downstream_handler(mavlink_channel_t from, mavlink_message_t* msg);
#endif // __FOLLOWME_DOWNSTREAM_H__

0
FollowMe/nocore.inoflag
View File

98
FollowMe/simplegcs.cpp
View File

@ -1,98 +0,0 @@
#include <AP_HAL.h>
extern const AP_HAL::HAL& hal;
#include <GCS_MAVLink.h>
#include <GCS_Console.h>
#include "simplegcs.h"
extern mavlink_channel_t downstream_channel;
static volatile uint8_t lock = 0;
void simplegcs_send_console_async(uint32_t machtnichts) {
if (lock) return;
lock = 1;
gcs_console_send(downstream_channel);
lock = 0;
}
void simplegcs_send_heartbeat_async(uint32_t us) {
uint32_t ms = us / 1000;
static uint32_t last_ms = 0;
if (ms - last_ms < 1000) return;
if (lock) return;
last_ms = ms;
lock = 1;
{
uint8_t base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED
| MAV_MODE_FLAG_SAFETY_ARMED
| MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
uint8_t system_status = MAV_STATE_ACTIVE;
uint32_t custom_mode = 5 ; /* Loiter is mode 5. */
mavlink_msg_heartbeat_send(
downstream_channel,
MAV_TYPE_QUADROTOR,
MAV_AUTOPILOT_ARDUPILOTMEGA,
base_mode,
custom_mode,
system_status);
}
lock = 0;
}
void simplegcs_send_heartbeat(mavlink_channel_t chan) {
uint8_t base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED
| MAV_MODE_FLAG_SAFETY_ARMED
| MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
uint8_t system_status = MAV_STATE_ACTIVE;
uint32_t custom_mode = 5 ; /* Loiter is mode 5. */
while(lock);
lock = 1;
mavlink_msg_heartbeat_send(
chan,
MAV_TYPE_QUADROTOR,
MAV_AUTOPILOT_ARDUPILOTMEGA,
base_mode,
custom_mode,
system_status);
lock = 0;
}
bool simplegcs_try_send_statustext(mavlink_channel_t chan, const char *text, int len) {
uint16_t txspace = comm_get_txspace(chan);
if (payload_space < MAVLINK_MSG_ID_STATUSTEXT_LEN+MAVLINK_NUM_NON_PAYLOAD_BYTES) return false;
char statustext[50] = { 0 };
if (len < 50) {
memcpy(statustext, text, len);
}
while(lock);
lock = 1;
mavlink_msg_statustext_send(
chan,
1, /* SEVERITY_LOW */
statustext);
lock = 0;
return true;
}
// -----------------------------------------------------------------------
void simplegcs_update(mavlink_channel_t chan, simplegcs_handler_t handler) {
mavlink_message_t msg;
mavlink_status_t status;
while(comm_get_available(chan)){
uint8_t c = comm_receive_ch(chan);
bool newmsg = mavlink_parse_char(chan, c, &msg, &status);
if (newmsg) {
handler(chan, &msg);
}
}
}

19
FollowMe/simplegcs.h
View File

@ -1,19 +0,0 @@
#ifndef __SIMPLE_GCS_H__
#define __SIMPLE_GCS_H__
#include <GCS_MAVLink.h>
typedef void(*simplegcs_handler_t)(mavlink_channel_t, mavlink_message_t*);
void simplegcs_send_heartbeat(mavlink_channel_t chan);
bool simplegcs_try_send_statustext(mavlink_channel_t chan, const char *text, int len);
void simplegcs_update(mavlink_channel_t chan, simplegcs_handler_t);
void handle_message(mavlink_channel_t chan, mavlink_message_t* msg);
void simplegcs_send_console_async(uint32_t ms);
void simplegcs_send_heartbeat_async(uint32_t ms);
#endif // __SIMPLE_GCS_H__

212
FollowMe/state.cpp
View File

@ -1,212 +0,0 @@
#include "state.h"
#include <AP_HAL.h>
#include <GCS_MAVLink.h>
extern const AP_HAL::HAL& hal;
extern mavlink_channel_t upstream_channel;
extern mavlink_channel_t downstream_channel;
void FMStateMachine::on_upstream_command_long(mavlink_command_long_t* pkt) {
switch(pkt->command) {
case MAV_CMD_NAV_LOITER_UNLIM:
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
case MAV_CMD_NAV_LAND:
case MAV_CMD_MISSION_START:
/* clear out FM control of vehicle */
_on_user_override();
break;
case MAV_CMD_PREFLIGHT_CALIBRATION:
/* i guess do nothing? */
break;
}
}
void FMStateMachine::on_upstream_set_mode(mavlink_set_mode_t* pkt) {
/* mode is set in pkt->custom_mode */
_vehicle_mode = (int8_t) pkt->custom_mode;
/* clear out FM control of vehicle */
_on_user_override();
}
void FMStateMachine::on_downstream_heartbeat(mavlink_heartbeat_t* pkt) {
/* if mode has changed from last set_mode, the user has triggered a change
* via RC switch.
* clear out FM control of vehicle */
bool pktarmed = ((pkt->base_mode & MAV_MODE_FLAG_SAFETY_ARMED) > 0);
int8_t pktmode = (int8_t) pkt->custom_mode;
if ((pktarmed != _vehicle_armed) || (pktmode != _vehicle_mode)) {
_on_user_override();
}
/* update heartbeat millis */
_last_vehicle_hb_millis = hal.scheduler->millis();
/* update local state */
_vehicle_armed = pktarmed;
_vehicle_mode = pktmode;
}
void FMStateMachine::on_downstream_gps_raw_int(mavlink_gps_raw_int_t* pkt) {
/* Keep track of vehicle's latest lat, lon, altitude */
_vehicle_lat = pkt->lat;
_vehicle_lon = pkt->lon;
_vehicle_altitude = pkt->alt;
_vehicle_gps_fix = pkt->fix_type;
}
void FMStateMachine::on_button_activate() {
if (_guiding) return;
/* This action is allowed to swing the state to start guide mode. */
if (_check_guide_valid()) {
_set_guide_offset();
_send_guide();
_guiding = true;
_vehicle_mode = MODE_GUIDED;
hal.console->println_P(PSTR("Button activated, entering guided mode"));
} else {
hal.console->println_P(PSTR("Button activated but insufficient conditions "
"for entering guided mode"));
}
}
void FMStateMachine::on_button_cancel() {
if (!_guiding) return;
_send_loiter();
_guiding = false;
}
void FMStateMachine::on_loop(GPS* gps) {
uint32_t now = hal.scheduler->millis();
if ((_last_run_millis + _loop_period) > now) return;
_last_run_millis = now;
if (gps != NULL) {
_update_local_gps(gps);
}
if (_guiding) {
_send_guide();
}
}
bool FMStateMachine::_check_guide_valid() {
uint32_t now = hal.scheduler->millis();
bool vehicle_gps_valid = (_vehicle_gps_fix == 3);
bool vehicle_hb_valid = (now - _last_vehicle_hb_millis) < 2000;
bool vehicle_mode_valid = _vehicle_armed
&& ( (_vehicle_mode == MODE_LOITER)
||(_vehicle_mode == MODE_ALT_HOLD)
||(_vehicle_mode == MODE_AUTO)
||(_vehicle_mode == MODE_GUIDED)
);
#define DEBUG 1
#if DEBUG
if (!_local_gps_valid) {
hal.console->println_P(PSTR("need valid local gps"));
}
if (!vehicle_gps_valid) {
hal.console->println_P(PSTR("need valid vehicle gps"));
}
if (!vehicle_hb_valid) {
hal.console->println_P(PSTR("need valid vehicle hb"));
}
if (!vehicle_mode_valid) {
hal.console->println_P(PSTR("need valid vehicle mode"));
}
#endif
return _local_gps_valid
&& vehicle_gps_valid
&& vehicle_hb_valid
&& vehicle_mode_valid;
}
void FMStateMachine::_update_local_gps(GPS* gps) {
/* Cause an on_fault_cancel if when local gps has transitioned form
* valid to invalid. */
if (_local_gps_valid && !(gps->status() == GPS::GPS_OK)) {
_on_fault_cancel();
}
_local_gps_valid = (gps->status() == GPS::GPS_OK);
if (gps->new_data) {
_local_gps_lat = gps->latitude;
_local_gps_lon = gps->longitude;
_local_gps_altitude = gps->altitude;
gps->new_data = false;
}
}
void FMStateMachine::_set_guide_offset() {
_offs_lat = 0;
_offs_lon = 0;
_offs_altitude = 1200; /* 12m in centimeters */
}
void FMStateMachine::_on_fault_cancel() {
if (_guiding) {
hal.console->println_P(PSTR("FollowMe: Fault Cancel"));
_send_loiter();
_guiding = false;
}
}
void FMStateMachine::_on_user_override() {
if (_guiding) {
hal.console->println_P(PSTR("FollowMe: User GCS or RC override"));
_guiding = false;
}
}
void FMStateMachine::_send_guide() {
hal.console->println_P(PSTR("FollowMe: Sending guide waypoint packet"));
int32_t lat = _local_gps_lat + _offs_lat;
int32_t lon = _local_gps_lon + _offs_lon;
// int32_t alt = _local_gps_altitude + _offs_altitude;
int32_t alt = _offs_altitude; /* assume above ground. (ArduCopter bug.) */
float x = (float) lat / (float) 1e7; /* lat, lon in deg * 10,000,000 */
float y = (float) lon / (float) 1e7;
float z = (float) alt / (float) 100; /* alt in cm */
hal.console->printf_P(
PSTR("FollowMe: guide x: %f y: %f z: %f\r\n"),
x, y, z);
mavlink_msg_mission_item_send(
upstream_channel, /* mavlink_channel_t chan*/
_target_system, /* uint8_t target_system */
_target_component, /* uint8_t target_component */
0, /* uint16_t seq: always 0, unknown why. */
MAV_FRAME_GLOBAL, /* uint8_t frame: arducopter uninterpreted */
MAV_CMD_NAV_WAYPOINT, /* uint16_t command: arducopter specific */
2, /* uint8_t current: 2 indicates guided mode waypoint */
0, /* uint8_t autocontinue: always 0 */
0, /* float param1 : hold time in seconds */
5, /* float param2 : acceptance radius in meters */
0, /* float param3 : pass through waypoint */
0, /* float param4 : desired yaw angle at waypoint */
x, /* float x : lat degrees */
y, /* float y : lon degrees */
z /* float z : alt meters */
);
}
void FMStateMachine::_send_loiter() {
hal.console->println_P(PSTR("FollowMe: Sending loiter cmd packet"));
mavlink_msg_command_long_send(
upstream_channel, /* mavlink_channel_t chan */
_target_system, /* uint8_t target_system */
_target_component, /* uint8_t target_component */
MAV_CMD_NAV_LOITER_UNLIM, /* uint16_t command: arducopter specific */
0, /* uint8_t confirmation */
0, /* float param1 */
0, /* float param2 */
0, /* float param3 */
0, /* float param4 */
0, /* float param5 */
0, /* float param6 */
0 /* float param7 */
);
}

88
FollowMe/state.h
View File

@ -1,88 +0,0 @@
#ifndef __FOLLOWME_STATE_H__
#define __FOLLOWME_STATE_H__
#include <GCS_MAVLink.h>
#include <AP_GPS.h>
#include "arducopter_defines.h"
class FMStateMachine {
public:
FMStateMachine() :
_last_run_millis(0),
_loop_period(500),
_last_vehicle_hb_millis(0),
_vehicle_mode(MODE_NUM_MODES),
_vehicle_armed(false),
_vehicle_gps_fix(0),
_vehicle_lat(0),
_vehicle_lon(0),
_vehicle_altitude(0),
/* Don't exactly know what these defaults for target system
* and target component mean - they're derived from mavproxy */
_target_system(1),
_target_component(1)
{}
void on_downstream_heartbeat(mavlink_heartbeat_t *pkt);
void on_downstream_gps_raw_int(mavlink_gps_raw_int_t* pkt);
void on_upstream_command_long(mavlink_command_long_t *pkt);
void on_upstream_set_mode(mavlink_set_mode_t* pkt);
void on_loop(GPS* gps);
void on_button_activate();
void on_button_cancel();
private:
/* _send_guide: Send a guide waypoint packet upstream. */
void _send_guide();
/* _send_loiter: Send a setmode loiter packet upstream. */
void _send_loiter();
/* Calculate whether we have sufficient conditions to enter guide mode. */
bool _check_guide_valid();
/* _update_local_gps: Get device's current GPS status and location. Called
* periodically. Can activate _on_fault_cancel(); */
void _update_local_gps(GPS* gps);
void _on_user_override();
void _on_fault_cancel();
void _set_guide_offset();
/* Set once a start guide mode packet has been sent.
* Unset whenever we stop guiding. */
bool _guiding;
/* Scheduling the on_loop periodic updater. */
uint32_t _last_run_millis;
uint32_t _loop_period;
uint32_t _last_vehicle_hb_millis;
int8_t _vehicle_mode;
bool _vehicle_armed;
uint8_t _vehicle_gps_fix;
int32_t _vehicle_lat;
int32_t _vehicle_lon;
int32_t _vehicle_altitude;
uint8_t _target_system;
uint8_t _target_component;
bool _local_gps_valid;
int32_t _local_gps_lat;
int32_t _local_gps_lon;
int32_t _local_gps_altitude;
int32_t _offs_lat;
int32_t _offs_lon;
int32_t _offs_altitude;
};
#endif // __FOLLOWME_STATE_H__

39
FollowMe/upstream.cpp
View File

@ -1,39 +0,0 @@
#include <AP_HAL.h>
#include "upstream.h"
#include "state.h"
extern const AP_HAL::HAL& hal;
extern mavlink_channel_t upstream_channel;
extern FMStateMachine sm;
static void upstream_handle_command_long(mavlink_message_t* msg) __attribute__((noinline));
static void upstream_handle_command_long(mavlink_message_t* msg) {
mavlink_command_long_t pkt;
mavlink_msg_command_long_decode(msg, &pkt);
sm.on_upstream_command_long(&pkt);
}
static void upstream_handle_set_mode(mavlink_message_t* msg) __attribute__((noinline));
static void upstream_handle_set_mode(mavlink_message_t* msg) {
mavlink_set_mode_t pkt;
mavlink_msg_set_mode_decode(msg, &pkt);
sm.on_upstream_set_mode(&pkt);
}
void upstream_handler(mavlink_channel_t from, mavlink_message_t* msg) {
switch (msg->msgid) {
case MAVLINK_MSG_ID_COMMAND_LONG:
upstream_handle_command_long(msg);
_mavlink_resend_uart(upstream_channel, msg);
break;
case MAVLINK_MSG_ID_SET_MODE:
upstream_handle_set_mode(msg);
_mavlink_resend_uart(upstream_channel, msg);
break;
default:
_mavlink_resend_uart(upstream_channel, msg);
}
}

10
FollowMe/upstream.h
View File

@ -1,10 +0,0 @@
#ifndef __FOLLOWME_UPSTREAM_H__
#define __FOLLOWME_UPSTREAM_H__
#include <GCS_MAVLink.h>
void upstream_handler(mavlink_channel_t from, mavlink_message_t* msg);
#endif // __FOLLOWME_UPSTREAM_H__

107
FollowMe/userinput.cpp
View File

@ -1,107 +0,0 @@
#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;
}
}

71
FollowMe/userinput.h
View File

@ -1,71 +0,0 @@
#ifndef __FOLLOWME_USERINPUT_H__
#define __FOLLOWME_USERINPUT_H__
#include <AP_HAL.h>
class DigitalDebounce {
public:
DigitalDebounce(AP_HAL::DigitalSource* in, int thresh_ms) :
_in(in), _thresh_ms(thresh_ms), _state(STATE_UP)
{}
enum Event {
BUTTON_DOWN,
BUTTON_UP
};
enum State {
STATE_DOWN,
STATE_RISING,
STATE_UP,
STATE_FALLING
};
void periodic(uint32_t ms);
void set_callback(void(*evt_cb)(int evt)) {
_evt_cb = evt_cb;
}
bool get_raw() { return _in->read(); }
int get_state() { return _state; }
bool read();
private:
AP_HAL::DigitalSource* _in;
int _thresh_ms;
int _state;
int _transition;
uint32_t _last_periodic;
void(*_evt_cb)(int evt);
};
class UserInput {
public:
static void init(int side_btn_ch, int joy_x_ch, int joy_y_ch, int joy_btn_ch);
static void print(AP_HAL::BetterStream* s);
static float get_joy_x() {
return _joy_x->read_average();
}
static float get_joy_y() {
return _joy_y->read_average();
}
static void side_btn_event_callback(void(*cb)(int)) {
_side_btn->set_callback(cb);
}
static void joy_btn_event_callback(void(*cb)(int)) {
_joy_btn->set_callback(cb);
}
private:
static AP_HAL::AnalogSource* _joy_x;
static AP_HAL::AnalogSource* _joy_y;
static DigitalDebounce* _side_btn;
static DigitalDebounce* _joy_btn;
static void _periodic(uint32_t millis);
static uint32_t _last_periodic;
};
#endif // __FOLLOWME_USERINPUT_H__