Skip to content
Snippets Groups Projects
Commit 02518ec9 authored by Pat Hickey's avatar Pat Hickey
Browse files

import FollowMe into new repo

parent db8da71f
No related branches found
No related tags found
No related merge requests found
// -*- 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();
include ../mk/Arduino.mk
#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 //
#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);
}
}
#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__
#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);
}
}
}
#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__
#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 */
);
}
#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__
#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);
}
}
#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__
#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;
}
}
#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__
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment