mirror of https://github.com/ArduPilot/ardupilot
101 lines
2.6 KiB
Plaintext
101 lines
2.6 KiB
Plaintext
// -*- 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();
|