zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

FollowMe: removing because no longer supported 

Android tablets and phones have replaced the need for this device and we
haven't been maintaining it.
mission-4.1.18
Randy Mackay 10 years ago
parent
b0a90df135
commit
227327c678
14 changed files with 0 additions and 824 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 100
      FollowMe/FollowMe.pde
  2. 1
      FollowMe/Makefile
  3. 28
      FollowMe/arducopter_defines.h
  4. 41
      FollowMe/downstream.cpp
  5. 10
      FollowMe/downstream.h
  6. 0
      FollowMe/nocore.inoflag
  7. 98
      FollowMe/simplegcs.cpp
  8. 19
      FollowMe/simplegcs.h
  9. 212
      FollowMe/state.cpp
  10. 88
      FollowMe/state.h
  11. 39
      FollowMe/upstream.cpp
  12. 10
      FollowMe/upstream.h
  13. 107
      FollowMe/userinput.cpp
  14. 71
      FollowMe/userinput.h

100
FollowMe/FollowMe.pde
Unescape View File

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

1
FollowMe/Makefile
Unescape View File

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

28
FollowMe/arducopter_defines.h
Unescape View File

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

41
FollowMe/downstream.cpp
Unescape View File

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

10
FollowMe/downstream.h
Unescape View File

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

0
FollowMe/nocore.inoflag
Unescape View File

98
FollowMe/simplegcs.cpp
Unescape View File

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

19
FollowMe/simplegcs.h
Unescape View File

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

212
FollowMe/state.cpp
Unescape View File

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

88
FollowMe/state.h
Unescape View File

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

39
FollowMe/upstream.cpp
Unescape View File

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

10
FollowMe/upstream.h
Unescape View File

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

107
FollowMe/userinput.cpp
Unescape View File

@ -1,107 +0,0 @@
#include <AP_HAL.h>
#include "userinput.h"
extern const AP_HAL::HAL& hal;
AP_HAL::AnalogSource* UserInput::_joy_x = NULL;
AP_HAL::AnalogSource* UserInput::_joy_y = NULL;
DigitalDebounce* UserInput::_side_btn = NULL;
DigitalDebounce* UserInput::_joy_btn = NULL;
uint32_t UserInput::_last_periodic = 0;
class DigitalInvert : public AP_HAL::DigitalSource {
public:
DigitalInvert(AP_HAL::DigitalSource* p) : _p(p) {}
uint8_t read() { return (_p->read()) ? 0 : 1; }
void mode(uint8_t m) { _p->mode(m); }
void write(uint8_t v) { _p->write( v == 0 ? 1 : 0 ); }
private:
AP_HAL::DigitalSource *_p;
};
void UserInput::init( int side_btn_ch, int joy_x_ch,
int joy_y_ch, int joy_btn_ch) {
_joy_x = hal.analogin->channel(joy_x_ch);
_joy_y = hal.analogin->channel(joy_y_ch);
_side_btn = new DigitalDebounce(
new DigitalInvert(hal.gpio->channel(side_btn_ch)), 100);
_joy_btn = new DigitalDebounce(hal.gpio->channel(joy_btn_ch), 100);
hal.scheduler->register_timer_process(_periodic);
}
void UserInput::print(AP_HAL::BetterStream* s) {
s->printf_P(PSTR("side: %d joy: %f, %f, %d\r\n"),
(int) _side_btn->read(),
_joy_x->read_average(),
_joy_y->read_average(),
(int) _joy_btn->read());
}
void UserInput::_periodic(uint32_t us) {
uint32_t millis = us / 1000;
_side_btn->periodic(millis);
_joy_btn->periodic(millis);
}
bool DigitalDebounce::read() {
switch (_state) {
case STATE_DOWN:
case STATE_RISING:
return false;
break;
case STATE_UP:
case STATE_FALLING:
return true;
break;
}
}
void DigitalDebounce::periodic(uint32_t millis) {
bool latest = _in->read();
uint32_t dt = millis - _last_periodic;
_last_periodic = millis;
switch (_state) {
case STATE_DOWN:
if (latest == true) {
_state = STATE_RISING;
_transition = 0;
}
break;
case STATE_RISING:
if (latest == true) {
_transition += dt;
if (_transition > _thresh_ms) {
_state = STATE_UP;
if (_evt_cb) {
_evt_cb(BUTTON_UP);
}
}
} else {
_state = STATE_DOWN;
}
break;
case STATE_UP:
if (latest == false) {
_state = STATE_FALLING;
_transition = 0;
}
break;
case STATE_FALLING:
if (latest == false) {
_transition += dt;
if (_transition > _thresh_ms) {
_state = STATE_DOWN;
if (_evt_cb) {
_evt_cb(BUTTON_DOWN);
}
}
} else {
_state = STATE_UP;
}
break;
}
}

71
FollowMe/userinput.h
Unescape View File

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