Permalink
Browse files

import FollowMe into new repo

  • Loading branch information...
pchickey committed Feb 3, 2013
1 parent db8da71 commit 02518ec95ffe3ae5d3714c075dcb439fc07e8b3f
View
@@ -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();
View
@@ -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 //
+
View
@@ -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);
+ }
+}
+
View
@@ -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__
+
View
No changes.
View
@@ -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);
+ }
+ }
+}
+
View
@@ -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__
+
Oops, something went wrong.

0 comments on commit 02518ec

Please sign in to comment.