mirror of https://github.com/ArduPilot/ardupilot
51 lines
1.4 KiB
Plaintext
51 lines
1.4 KiB
Plaintext
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||
|
|
||
|
// These are function definitions so the Menu can be constructed before the functions
|
||
|
// are defined below. Order matters to the compiler.
|
||
|
static int8_t planner_gcs(uint8_t argc, const Menu::arg *argv);
|
||
|
|
||
|
// Creates a constant array of structs representing menu options
|
||
|
// and stores them in Flash memory, not RAM.
|
||
|
// User enters the string in the console to call the functions on the right.
|
||
|
// See class Menu in AP_Common for implementation details
|
||
|
const struct Menu::command planner_menu_commands[] PROGMEM = {
|
||
|
{"gcs", planner_gcs},
|
||
|
};
|
||
|
|
||
|
// A Macro to create the Menu
|
||
|
MENU(planner_menu, "planner", planner_menu_commands);
|
||
|
|
||
|
int8_t
|
||
|
planner_mode(uint8_t argc, const Menu::arg *argv)
|
||
|
{
|
||
|
Serial.printf_P(PSTR("Planner Mode\n\nThis mode is not intended for manual use\n\n"));
|
||
|
planner_menu.run();
|
||
|
return 0;
|
||
|
}
|
||
|
static int8_t
|
||
|
planner_gcs(uint8_t argc, const Menu::arg *argv)
|
||
|
{
|
||
|
gcs.init(&Serial);
|
||
|
|
||
|
int loopcount = 0;
|
||
|
while (1) {
|
||
|
if (millis()-fast_loopTimer > 19) {
|
||
|
fast_loopTimer = millis();
|
||
|
|
||
|
gcs.update();
|
||
|
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
|
||
|
gcs.data_stream_send(45,1000);
|
||
|
if ((loopcount % 5) == 0) // 10 hz
|
||
|
gcs.data_stream_send(5,45);
|
||
|
if ((loopcount % 16) == 0) { // 3 hz
|
||
|
gcs.data_stream_send(1,5);
|
||
|
gcs.send_message(MSG_HEARTBEAT);
|
||
|
}
|
||
|
#endif
|
||
|
loopcount++;
|
||
|
}
|
||
|
}
|
||
|
return 0;
|
||
|
}
|
||
|
|