import FollowMe into new repo

FollowMe/FollowMe.pde

@@ -0,0 +1,100 @@
+// -*- 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);
+}
+
+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();

FollowMe/Makefile

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

FollowMe/arducopter_defines.h

@@ -0,0 +1,28 @@
+
+#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 // 
+

FollowMe/downstream.cpp

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

FollowMe/downstream.h

@@ -0,0 +1,10 @@
+
+#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__
+

FollowMe/simplegcs.cpp

@@ -0,0 +1,98 @@
+
+#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) {
+
+    int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
+    if (payload_space < MAVLINK_MSG_ID_STATUSTEXT_LEN) 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);
+        }
+    }
+}
+

FollowMe/simplegcs.h

@@ -0,0 +1,19 @@
+
+#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__
+

FollowMe/state.cpp

@@ -0,0 +1,212 @@
+
+#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 */
+      );
+}

FollowMe/state.h

@@ -0,0 +1,88 @@
+
+#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__
+

FollowMe/upstream.cpp

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

FollowMe/upstream.h

@@ -0,0 +1,10 @@
+
+#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__
+

FollowMe/userinput.cpp

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

FollowMe/userinput.h

@@ -0,0 +1,71 @@
+
+#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__
+