import FollowMe into new repo

FollowMe/FollowMe.pde

@@ -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();

FollowMe/Makefile

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

FollowMe/arducopter_defines.h

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

FollowMe/downstream.cpp

@@ -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);
+    }
+}
+

FollowMe/downstream.h

@@ -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__
+

FollowMe/simplegcs.cpp

@@ -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);
+        }
+    }
+}
+

FollowMe/simplegcs.h

@@ -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__
+