Way Point implementation - 1 : Set Waypoint Function in WinGUI

based on multiwii 2.2, the protocol was implemented in "Serial.ino", in
\  case MSP_SET_WP:
\  ...
\  if (wp_no == 0) {...}
\  else if (wp_no == 16) {...}
\  ...
\  break;
is a statement judging that wp_no is 0(HOME) or 16(HOLD)

but in WinGUI R53, there's no corresponding function to upload a way-point 
packet except 
\  private void MSPquery(int command) {...}
and
\  private void MSPqueryWP(int wp) {...}
so I used these two function to write a new function 
 for uploading the way-point packet, as below

// ===============================================================
// MSPsetWP    based on WinGUI R53, date: 2013/11/7
private void MSPsetWP(int wp_no, Int32 lat, Int32 lon, Int32 alt, Int16 
heading, Int16 tts, byte flag)
{
    byte c = 0;
    byte[] o;
    o = new byte[25];
    // with checksum 
    o[0] = (byte)'$';
    o[1] = (byte)'M';
    o[2] = (byte)'<';
    o[3] = (byte)18;                c ^= o[3];       //one byte payload
    o[4] = (byte)MSP.MSP_SET_WP;    c ^= o[4];
    o[5] = (byte)wp_no;             c ^= o[5];             // way point number
    o[6] = (byte)( lat        &   0xff);  c ^=    o[6];    // latitude
    o[7] = (byte)( (lat>>8)   &   0xff);  c ^=    o[7];
    o[8] = (byte)( (lat>>16)  &   0xff);  c ^=    o[8];
    o[9] = (byte)( (lat>>24)  &   0xff);  c ^=    o[9];
    o[10] = (byte)(  lon      &   0xff);  c ^=    o[10];   // longitude
    o[11] = (byte)( (lon>>8)  &   0xff);  c ^=    o[11];
    o[12] = (byte)( (lon>>16) &   0xff);  c ^=    o[12];
    o[13] = (byte)( (lon>>24) &   0xff);  c ^=    o[13];
    o[14] = (byte)( alt       &   0xff);  c ^=    o[14];   // altitude
    o[15] = (byte)( (alt>>8)  &   0xff);  c ^=    o[15];
    o[16] = (byte)( (alt>>16) &   0xff);  c ^=    o[16];
    o[17] = (byte)( (alt>>24) &   0xff);  c ^=    o[17];
    //  heading/time_to_stay/flag is future function
    //  so it doesn't work now
    heading = tts = flag = 0;
    o[18] = (byte)( heading   &   0xff);  c ^=    o[18];
    o[19] = (byte)( (heading>>8)& 0xff);  c ^=    o[19];
    o[20] = (byte)( tts       &   0xff);  c ^=    o[20];   // time to stay
    o[21] = (byte)( (tts>>8)  &   0xff);  c ^=    o[21];
    o[22] = (byte)flag;                   c ^=    o[22];   // flag
    o[23] = (byte)c;
    serialPort.Write(o, 0, 24);
 }
// MSPsetWP END
// ===============================================================
if the author EOSbandi agree, it's welcome for everyone to use this code
the receive part in on-board multiwii will also release after I made it

reference:
http://multicopter-make.seesaa.net/article/360526447.html
Original issue reported on code.google.com by michaelo...@gmail.com on 7 Nov 2013 at 5:48
GuitarZaan / mw-wingui

Way Point implementation - 1 : Set Waypoint Function in WinGUI #35
