From 227327c67855373e59fc4fac18ed0001fd385265 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 15 May 2015 15:32:51 +0900 Subject: [PATCH] FollowMe: removing because no longer supported Android tablets and phones have replaced the need for this device and we haven't been maintaining it. --- FollowMe/FollowMe.pde | 100 ---------------- FollowMe/Makefile | 1 - FollowMe/arducopter_defines.h | 28 ----- FollowMe/downstream.cpp | 41 ------- FollowMe/downstream.h | 10 -- FollowMe/nocore.inoflag | 0 FollowMe/simplegcs.cpp | 98 ---------------- FollowMe/simplegcs.h | 19 --- FollowMe/state.cpp | 212 ---------------------------------- FollowMe/state.h | 88 -------------- FollowMe/upstream.cpp | 39 ------- FollowMe/upstream.h | 10 -- FollowMe/userinput.cpp | 107 ----------------- FollowMe/userinput.h | 71 ------------ 14 files changed, 824 deletions(-) delete mode 100644 FollowMe/FollowMe.pde delete mode 100644 FollowMe/Makefile delete mode 100644 FollowMe/arducopter_defines.h delete mode 100644 FollowMe/downstream.cpp delete mode 100644 FollowMe/downstream.h delete mode 100644 FollowMe/nocore.inoflag delete mode 100644 FollowMe/simplegcs.cpp delete mode 100644 FollowMe/simplegcs.h delete mode 100644 FollowMe/state.cpp delete mode 100644 FollowMe/state.h delete mode 100644 FollowMe/upstream.cpp delete mode 100644 FollowMe/upstream.h delete mode 100644 FollowMe/userinput.cpp delete mode 100644 FollowMe/userinput.h diff --git a/FollowMe/FollowMe.pde b/FollowMe/FollowMe.pde deleted file mode 100644 index 14495ca029..0000000000 --- a/FollowMe/FollowMe.pde +++ /dev/null @@ -1,100 +0,0 @@ -// -*- 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, &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(); diff --git a/FollowMe/Makefile b/FollowMe/Makefile deleted file mode 100644 index c793c78167..0000000000 --- a/FollowMe/Makefile +++ /dev/null @@ -1 +0,0 @@ -include ../mk/Arduino.mk diff --git a/FollowMe/arducopter_defines.h b/FollowMe/arducopter_defines.h deleted file mode 100644 index 95467e81c5..0000000000 --- a/FollowMe/arducopter_defines.h +++ /dev/null @@ -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 // - diff --git a/FollowMe/downstream.cpp b/FollowMe/downstream.cpp deleted file mode 100644 index adee94ed14..0000000000 --- a/FollowMe/downstream.cpp +++ /dev/null @@ -1,41 +0,0 @@ - - -#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 deleted file mode 100644 index 582aa2c684..0000000000 --- a/FollowMe/downstream.h +++ /dev/null @@ -1,10 +0,0 @@ - -#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 deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/FollowMe/simplegcs.cpp b/FollowMe/simplegcs.cpp deleted file mode 100644 index 8b3dafab5d..0000000000 --- a/FollowMe/simplegcs.cpp +++ /dev/null @@ -1,98 +0,0 @@ - -#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) { - - 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); - } - } -} - diff --git a/FollowMe/simplegcs.h b/FollowMe/simplegcs.h deleted file mode 100644 index 72c5b1084e..0000000000 --- a/FollowMe/simplegcs.h +++ /dev/null @@ -1,19 +0,0 @@ - -#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 deleted file mode 100644 index e621420963..0000000000 --- a/FollowMe/state.cpp +++ /dev/null @@ -1,212 +0,0 @@ - -#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 deleted file mode 100644 index 2b5a36f2d4..0000000000 --- a/FollowMe/state.h +++ /dev/null @@ -1,88 +0,0 @@ - -#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 deleted file mode 100644 index bfad419458..0000000000 --- a/FollowMe/upstream.cpp +++ /dev/null @@ -1,39 +0,0 @@ - -#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 deleted file mode 100644 index a478af2fea..0000000000 --- a/FollowMe/upstream.h +++ /dev/null @@ -1,10 +0,0 @@ - -#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 deleted file mode 100644 index 32cd8878d6..0000000000 --- a/FollowMe/userinput.cpp +++ /dev/null @@ -1,107 +0,0 @@ - -#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 deleted file mode 100644 index e5aaad588b..0000000000 --- a/FollowMe/userinput.h +++ /dev/null @@ -1,71 +0,0 @@ - -#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__ -