mirror of https://github.com/ArduPilot/ardupilot
import FollowMe into new repo
This commit is contained in:
Pat Hickey
parent
db8da71f65
commit
02518ec95f
|
|
@ -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();
|
||||||
|
|
@ -0,0 +1 @@
|
||||||
|
include ../mk/Arduino.mk
|
||||||
|
|
@ -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 //
|
||||||
|
|
||||||
|
|
@ -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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -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__
|
||||||
|
|
||||||
|
|
@ -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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -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__
|
||||||
|
|
||||||
|
|
@ -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 */
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
@ -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__
|
||||||
|
|
||||||
|
|
@ -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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -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__
|
||||||
|
|
||||||
|
|
@ -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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -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__
|
||||||
|
|
||||||
Loading…
Reference in New Issue