// Created 2011 By Colin G http://www.diydrones.com/profile/ColinG
// Modified 2013 by Colin G for Ardustation Mega
#define COMMANDFIELDWIDTH 18
#define CMDVALCOUNT 6
PROGMEM const prog_char textCommands[] =
//123456789012345678
"Restart Mission\n"
"Request Parameters\n"
"Stop Data Stream\n"
"Re-request Stream\n"
"Set GCS Home\n"
"Reset UAV Home";//\n"
//"Fly in a square";
PROGMEM const prog_char confirmReset_Index[] =
//01234567890123456789
" This will reset\n"
" the mission\n"
" Press OK to\n"
" Confirm";
PROGMEM const prog_char confirmRequest_Params[] =
//01234567890123456789
" This will request\n"
" the parameters\n"
" Press OK to\n"
" Confirm";
PROGMEM const prog_char confirmStop_Stream[] =
//01234567890123456789
" This will stop\n"
" the data stream\n"
" Press OK to\n"
" Confirm";
PROGMEM const prog_char confirmStart_Stream[] =
//01234567890123456789
" This will restart\n"
" the data stream\n"
" Press OK to\n"
" Confirm";
PROGMEM const prog_char confirmSet_GCSHome[] =
//01234567890123456789
"This will initialise\n"
" the GCS's Home\n"
"position. Ensure the\n"
"UAV is next to GCS";
PROGMEM const prog_char confirmReset_Home[] =
//01234567890123456789
"This will initialise\n"
"the Aircraft's Home\n"
"position. Ensure it\n"
"is **on the GROUND**";
// struct msg_command_upload {
// uint8_t action;
// uint16_t itemNumber;
// uint16_t listLength;
// uint8_t commandID;
// uint8_t p1;
// int32_t p2;
// int32_t p3;
// int32_t p4;
//////////////////////////////////////////////////////////////////////////////
// Commands page
////////////////////////////////////////////////////////////////////////////////
PageCommands::PageCommands()
{
/// Copy the header and types to internal storage
_textCommands = textCommands;
// Set up the local LCD def
_linecount = LCD_ROWS-2;
_cmdLCD = gText(0, 16, LCD_COLUMNS, _linecount, SystemFont5x7);
}
uint8_t
PageCommands::_enter()
{
// Draw the header
GLCD.CursorTo(0, 0);
GLCD.SelectFont(Arial_bold_14);
GLCD.Printf_P(PSTR("UAV Commands"));
GLCD.SelectFont(System5x7);
// Draw the contents
_drawLocal();
}
uint8_t
PageCommands::_refresh_med()
{
}
uint8_t
PageCommands::_refresh_slow()
{
}
//void
//PageCommands::_update(void)
//{
// // don't waste time redrawing if we have no changes to announce
// if (!_updated)
// return;
//
// // don't redraw too often
// if ((millis() - _lastRedraw) < STATUS_UPDATE_INTERVAL)
// return;
//
// // redraw the page
// _render();
// _updated = false;
// _lastRedraw = millis();
//}
void
PageCommands::_drawLocal(void)
{
uint8_t c;
uint8_t i,j,k;
uint8_t row;
const prog_char *str;
str = _textCommands;
_cmdLCD.ClearArea();
row = 0;
i=0;
for(j=0;j<_stateFirstVal+_linecount;j++) { //Need to go from zero to read through the string
if (j>=_stateFirstVal)
_cmdLCD.CursorTo(1,j-_stateFirstVal);
k=0;
for (;;) {
c = pgm_read_byte_near(_textCommands + i++);
if (0 == c)
break;
if ('\n' == c)
break;
else {
if (j>=_stateFirstVal && j<CMDVALCOUNT && k++<COMMANDFIELDWIDTH)
_cmdLCD.write(c);
}
}
}
// redraw the "choosing" marker
_paintMarker();
}
/// Draw the "choosing" marker
void
PageCommands::_paintMarker(void)
{
if (_state > 0 && _state < 100) {
_cmdLCD.CursorTo(0,_state-1 - _stateFirstVal);
_cmdLCD.write('>');
}
}
/// remove the "choosing" marker
void
PageCommands::_clearMarker(void)
{
if (_state > 0 && _state < 200) {
_cmdLCD.CursorTo(0,_state-1 - _stateFirstVal);
_cmdLCD.write(' ');
}
}
void
PageCommands::_commandConfirm(void)
{
switch (_state-201) {
case 0: // Reset waypoint to zero
_commandConfirmMessage(confirmReset_Index);
break;
case 1: // Request all the parameters
_commandConfirmMessage(confirmRequest_Params);
break;
case 2: // Stop data stream
_commandConfirmMessage(confirmStop_Stream);
break;
case 3: // Restart data stream
_commandConfirmMessage(confirmStart_Stream);
break;
case 4: // Set GCS Home
_commandConfirmMessage(confirmSet_GCSHome);
break;
case 5: // Set UAV Home
_commandConfirmMessage(confirmReset_Home);
break;
}
}
void
PageCommands::_commandConfirmMessage(const prog_char *str)
{
uint8_t c;
uint8_t row;
_cmdLCD.ClearArea();
row = 0;
for (;;) {
c = pgm_read_byte_near(str++);
if (0 == c)
break;
if ('\n' == c) {
_cmdLCD.CursorTo(0, ++row);
continue;
}
// emit
_cmdLCD.write(c);
}
}
void
PageCommands::_commandSend(void)
{
uint8_t i;
mavlink_message_t msg;
// struct BinComm::msg_command_upload command;
switch (_state - 201)
{
case 0: // Reset waypoint index
// mavlink_msg_waypoint_request_list_pack(0xFF, 0xFA, &msg, 1, 1);
// Restart the mission by setting the current waypoint to waypoint 1
// mavlink_msg_waypoint_set_current_pack(0xFF, 0xFA, &msg, 1, 1, 1);
//mavlink_msg_waypoint_set_current_send(MAVLINK_COMM_1, 1, 1, 1);
// comm.send(&msg);
break;
case 1: // Request parameters
gcs3.param_request(0);
// mavlink_msg_param_request_list_pack(0xFF, 0xFA, &msg, 1, 1);
// mavlink_msg_param_request_list_send(MAVLINK_COMM_0, 1, 1);
// mavlink_msg_param_request_list_send(MAVLINK_COMM_1, 1, 1);
// comm.send(&msg);
break;
case 2: // Stop Stream
//mavlink_msg_request_data_stream_pack(0xFF, 0xFA, &msg, 1, 1, MAV_DATA_STREAM_ALL, 0, 0);
// mavlink_msg_request_data_stream_send(MAVLINK_COMM_1, 1, 1, MAV_DATA_STREAM_ALL, 0, 0);
// comm.send(&msg);
break;
case 3: // Start Stream
gcs3.data_stream_request();
break;
case 4: // GCS Home
// gcsLat = mavGPS.lat;
// gcsLon = mavGPS.lon;
// gcsAlt = mavGPS.alt;
break;
case 5: // UAV Home
// mavlink_msg_command_pack(0xFF, 0xFA, &msg, 1, 1, MAV_CMD_DO_SET_HOME, 0, 1, 0, 0, 0);
// uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z)
// if (mavWptCount.count != 0) {
//
//// mavlink_msg_waypoint_count_pack(0xFF, 0xFA, &msg, 1, 1, mavWptCount.count);
//// comm.send(&msg);
//
//// mavlink_msg_waypoint_pack(0xFF, 0xFA, &msg, 1, 1, 0, MAV_FRAME_GLOBAL, MAV_CMD_DO_SET_HOME, 0, 0, 1, 0, 0, 0, 0, 0, 0);
//
//// mavlink_msg_waypoint_pack(0xFF, 0xFA, &msg, 1, 1, 0, MAV_FRAME_GLOBAL, MAV_CMD_NAV_WAYPOINT, 1, 1, 0, 0, 0, 0, mavGPS.lat, mavGPS.lon, mavGPS.alt);
//// comm.send(&msg);
//
// delay(50);
//
//// mavlink_msg_waypoint_request_pack(0xFF, 0xFA, &msg, 1, 1, 0);
//// comm.send(&msg);
//
// }
break;
}
}
uint8_t
PageCommands::_interact(uint8_t buttonid)
{
_clearMarker();
switch (buttonid) {
case B_UP:
// Navigation
if (_state == 0) {
if (_stateFirstVal > 0) {
_stateFirstVal --;
_updated = true;
}
}
// Picking
else if (_state > 1 && _state < 100) {
if (_state == (_stateFirstVal + 1)) {
_stateFirstVal--;
_updated = true;
}
_state--;
}
// Confirming
else if (_state > 200)
return 0;
break;
case B_DOWN:
// Navigation
if (_state == 0) {
if (_stateFirstVal < CMDVALCOUNT-_linecount) {
_stateFirstVal++;
_updated = true;
}
}
// Picking
else if (_state > 0 && _state < CMDVALCOUNT) {
if (_state == (_stateFirstVal + _linecount)) {
_stateFirstVal++;
_updated = true;
}
_state++;
}
// Confirming
else if (_state > 200)
return 0;
break;
case B_OK:
// Enter picking mode
if (_state == 0) {
_state = _stateFirstVal + 1;
}
// Choose command
else if(_state < 100) {
// Update the state
_state += 200;
_commandConfirm();
return 0;
}
// Confirming
else if(_state > 200) {
// Save the value
_commandSend();
_state = 0;
_updated = true;
_drawLocal(); // Redraw the main screen
return 0; // Leave before we draw the marker again
}
break;
case B_LEFT:
// Navigation
if (_state == 0)
Pages::move(-1);
else // Selection, ignore
return 0;
break;
case B_RIGHT:
// Navigation
if (_state == 0)
Pages::move(1);
else // Selection, ignore
return 0;
break;
case B_CANCEL:
if (_state == 0) {
Pages::move(0);
return 0; // avoid drawing the cursor
}
else {
if (_state > 200)
_drawLocal(); // Redraw the main screen
_state = 0;
_updated = true;
}
break;
}
_paintMarker();
}