diff --git a/FollowMe/FollowMe.pde b/FollowMe/FollowMe.pde new file mode 100644 index 0000000000..8b30637e8b --- /dev/null +++ b/FollowMe/FollowMe.pde @@ -0,0 +1,100 @@ +// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*- + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#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(); diff --git a/FollowMe/Makefile b/FollowMe/Makefile new file mode 100644 index 0000000000..c793c78167 --- /dev/null +++ b/FollowMe/Makefile @@ -0,0 +1 @@ +include ../mk/Arduino.mk diff --git a/FollowMe/arducopter_defines.h b/FollowMe/arducopter_defines.h new file mode 100644 index 0000000000..95467e81c5 --- /dev/null +++ b/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 // + diff --git a/FollowMe/downstream.cpp b/FollowMe/downstream.cpp new file mode 100644 index 0000000000..adee94ed14 --- /dev/null +++ b/FollowMe/downstream.cpp @@ -0,0 +1,41 @@ + + +#include + +#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); + } +} + diff --git a/FollowMe/downstream.h b/FollowMe/downstream.h new file mode 100644 index 0000000000..582aa2c684 --- /dev/null +++ b/FollowMe/downstream.h @@ -0,0 +1,10 @@ + +#ifndef __FOLLOWME_DOWNSTREAM_H__ +#define __FOLLOWME_DOWNSTREAM_H__ + +#include + +void downstream_handler(mavlink_channel_t from, mavlink_message_t* msg); + +#endif // __FOLLOWME_DOWNSTREAM_H__ + diff --git a/FollowMe/nocore.inoflag b/FollowMe/nocore.inoflag new file mode 100644 index 0000000000..e69de29bb2 diff --git a/FollowMe/simplegcs.cpp b/FollowMe/simplegcs.cpp new file mode 100644 index 0000000000..2c2c25fe0f --- /dev/null +++ b/FollowMe/simplegcs.cpp @@ -0,0 +1,98 @@ + +#include +extern const AP_HAL::HAL& hal; + +#include +#include +#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); + } + } +} + diff --git a/FollowMe/simplegcs.h b/FollowMe/simplegcs.h new file mode 100644 index 0000000000..72c5b1084e --- /dev/null +++ b/FollowMe/simplegcs.h @@ -0,0 +1,19 @@ + +#ifndef __SIMPLE_GCS_H__ +#define __SIMPLE_GCS_H__ + +#include + +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__ + diff --git a/FollowMe/state.cpp b/FollowMe/state.cpp new file mode 100644 index 0000000000..e621420963 --- /dev/null +++ b/FollowMe/state.cpp @@ -0,0 +1,212 @@ + +#include "state.h" +#include +#include + +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 */ + ); +} diff --git a/FollowMe/state.h b/FollowMe/state.h new file mode 100644 index 0000000000..2b5a36f2d4 --- /dev/null +++ b/FollowMe/state.h @@ -0,0 +1,88 @@ + +#ifndef __FOLLOWME_STATE_H__ +#define __FOLLOWME_STATE_H__ + +#include +#include + +#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__ + diff --git a/FollowMe/upstream.cpp b/FollowMe/upstream.cpp new file mode 100644 index 0000000000..bfad419458 --- /dev/null +++ b/FollowMe/upstream.cpp @@ -0,0 +1,39 @@ + +#include + +#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); + } +} + diff --git a/FollowMe/upstream.h b/FollowMe/upstream.h new file mode 100644 index 0000000000..a478af2fea --- /dev/null +++ b/FollowMe/upstream.h @@ -0,0 +1,10 @@ + +#ifndef __FOLLOWME_UPSTREAM_H__ +#define __FOLLOWME_UPSTREAM_H__ + +#include + +void upstream_handler(mavlink_channel_t from, mavlink_message_t* msg); + +#endif // __FOLLOWME_UPSTREAM_H__ + diff --git a/FollowMe/userinput.cpp b/FollowMe/userinput.cpp new file mode 100644 index 0000000000..32cd8878d6 --- /dev/null +++ b/FollowMe/userinput.cpp @@ -0,0 +1,107 @@ + +#include +#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; + } +} + diff --git a/FollowMe/userinput.h b/FollowMe/userinput.h new file mode 100644 index 0000000000..e5aaad588b --- /dev/null +++ b/FollowMe/userinput.h @@ -0,0 +1,71 @@ + +#ifndef __FOLLOWME_USERINPUT_H__ +#define __FOLLOWME_USERINPUT_H__ + +#include + +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__ +