Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Browse files

import FollowMe into new repo

  • Loading branch information...
commit 02518ec95ffe3ae5d3714c075dcb439fc07e8b3f 1 parent db8da71
@pchickey pchickey authored
View
100 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();
View
1  FollowMe/Makefile
@@ -0,0 +1 @@
+include ../mk/Arduino.mk
View
28 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 //
+
View
41 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);
+ }
+}
+
View
10 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__
+
View
0  FollowMe/nocore.inoflag
No changes.
View
98 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);
+ }
+ }
+}
+
View
19 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__
+
View
212 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 */
+ );
+}
View
88 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__
+
View
39 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);
+ }
+}
+
View
10 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__
+
View
107 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;
+ }
+}
+
View
71 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__
+
Please sign in to comment.
Something went wrong with that request. Please try again.