2011-09-08 22:31:32 -03:00
using System ;
using System.Collections.Generic ;
using System.Text ;
using System.IO.Ports ;
using System.Runtime.InteropServices ;
using System.Collections ; // hashs
using System.Diagnostics ; // stopwatch
using System.Reflection ;
using System.Reflection.Emit ;
using System.IO ;
2012-01-19 10:01:53 -04:00
using System.Drawing ;
2012-02-23 21:38:56 -04:00
using System.Threading ;
using ArdupilotMega.Controls ;
2012-02-20 07:30:47 -04:00
using ArdupilotMega.Mavlink ;
using System.ComponentModel ;
2012-02-26 19:13:23 -04:00
using log4net ;
2011-09-08 22:31:32 -03:00
namespace ArdupilotMega
{
public partial class MAVLink
{
2012-02-26 19:13:23 -04:00
private static readonly ILog log = LogManager . GetLogger ( MethodBase . GetCurrentMethod ( ) . DeclaringType ) ;
2011-09-08 22:31:32 -03:00
public ICommsSerial BaseStream = new SerialPort ( ) ;
2012-02-20 07:30:47 -04:00
private const double CONNECT_TIMEOUT_SECONDS = 30 ;
/// <summary>
/// progress form to handle connect and param requests
/// </summary>
2012-02-23 21:38:56 -04:00
ProgressReporterDialogue frmProgressReporter ;
2012-02-20 07:30:47 -04:00
2011-09-17 10:22:07 -03:00
/// <summary>
/// used for outbound packet sending
/// </summary>
2011-09-08 22:31:32 -03:00
byte packetcount = 0 ;
2012-02-20 07:30:47 -04:00
/// <summary>
/// mavlink remote sysid
/// </summary>
2011-09-08 22:31:32 -03:00
public byte sysid = 0 ;
2012-02-20 07:30:47 -04:00
/// <summary>
/// mavlink remove compid
/// </summary>
2011-09-08 22:31:32 -03:00
public byte compid = 0 ;
2012-02-20 07:30:47 -04:00
/// <summary>
/// storage for whole paramater list
/// </summary>
2011-09-08 22:31:32 -03:00
public Hashtable param = new Hashtable ( ) ;
2012-02-20 07:30:47 -04:00
/// <summary>
/// storage of a previous packet recevied of a specific type
/// </summary>
2011-10-23 06:48:00 -03:00
public byte [ ] [ ] packets = new byte [ 256 ] [ ] ;
2012-02-20 07:30:47 -04:00
/// <summary>
/// used to calc packets per second on any single message type - used for stream rate comparaison
/// </summary>
2011-09-08 22:31:32 -03:00
public double [ ] packetspersecond = new double [ 256 ] ;
2012-02-20 07:30:47 -04:00
/// <summary>
/// time last seen a packet of a type
/// </summary>
2011-09-08 22:31:32 -03:00
DateTime [ ] packetspersecondbuild = new DateTime [ 256 ] ;
2012-02-20 07:30:47 -04:00
/// <summary>
/// used as a serial port write lock
/// </summary>
2011-10-23 06:48:00 -03:00
object objlock = new object ( ) ;
2012-02-20 07:30:47 -04:00
/// <summary>
/// used for a readlock on readpacket
/// </summary>
2011-10-23 06:48:00 -03:00
object readlock = new object ( ) ;
2012-02-20 07:30:47 -04:00
/// <summary>
/// used for tlog file lock
/// </summary>
2011-09-08 22:31:32 -03:00
object logwritelock = new object ( ) ;
2012-02-20 07:30:47 -04:00
/// <summary>
/// time seen of last mavlink packet
/// </summary>
2011-09-08 22:31:32 -03:00
public DateTime lastvalidpacket = DateTime . Now ;
2012-02-20 07:30:47 -04:00
/// <summary>
/// old log support
/// </summary>
2011-09-08 22:31:32 -03:00
bool oldlogformat = false ;
2012-02-20 07:30:47 -04:00
/// <summary>
/// mavlink version
/// </summary>
2011-09-17 10:22:07 -03:00
byte mavlinkversion = 0 ;
2012-02-20 07:30:47 -04:00
/// <summary>
/// mavlink ap type
/// </summary>
2011-12-04 18:43:29 -04:00
public byte aptype = 0 ;
2012-02-20 07:30:47 -04:00
/// <summary>
/// used as a snapshot of what is loaded on the ap atm. - derived from the stream
/// </summary>
2011-09-08 22:31:32 -03:00
public PointLatLngAlt [ ] wps = new PointLatLngAlt [ 200 ] ;
2012-02-20 07:30:47 -04:00
/// <summary>
/// turns on console packet display
/// </summary>
2011-09-08 22:31:32 -03:00
public bool debugmavlink = false ;
2012-02-20 07:30:47 -04:00
/// <summary>
/// enabled read from file mode
/// </summary>
2011-09-08 22:31:32 -03:00
public bool logreadmode = false ;
public DateTime lastlogread = DateTime . MinValue ;
public BinaryReader logplaybackfile = null ;
public BinaryWriter logfile = null ;
int bps1 = 0 ;
int bps2 = 0 ;
public int bps = 0 ;
public DateTime bpstime = DateTime . Now ;
int recvpacketcount = 0 ;
2011-09-17 10:22:07 -03:00
float synclost ;
float packetslost = 0 ;
float packetsnotlost = 0 ;
DateTime packetlosttimer = DateTime . Now ;
2011-09-08 22:31:32 -03:00
//Stopwatch stopwatch = new Stopwatch();
public void Close ( )
{
BaseStream . Close ( ) ;
}
public void Open ( )
{
Open ( false ) ;
}
public void Open ( bool getparams )
{
if ( BaseStream . IsOpen )
return ;
2012-02-23 21:38:56 -04:00
frmProgressReporter = new ProgressReporterDialogue
{
StartPosition = System . Windows . Forms . FormStartPosition . CenterScreen ,
Text = "Connecting Mavlink"
} ;
2012-02-20 07:30:47 -04:00
2012-02-23 21:38:56 -04:00
if ( getparams )
{
frmProgressReporter . DoWork + = FrmProgressReporterDoWorkAndParams ;
}
else
{
frmProgressReporter . DoWork + = FrmProgressReporterDoWorkNOParams ;
}
frmProgressReporter . UpdateProgressAndStatus ( - 1 , "Mavlink Connecting..." ) ;
2012-03-03 20:42:42 -04:00
ThemeManager . ApplyThemeTo ( frmProgressReporter ) ;
2012-02-20 07:30:47 -04:00
2012-02-23 21:38:56 -04:00
frmProgressReporter . RunBackgroundOperationAsync ( ) ;
}
2012-02-20 07:30:47 -04:00
2012-02-23 21:38:56 -04:00
void FrmProgressReporterDoWorkAndParams ( object sender , ProgressWorkerEventArgs e )
{
OpenBg ( true , e ) ;
}
2012-02-20 07:30:47 -04:00
2012-02-23 21:38:56 -04:00
void FrmProgressReporterDoWorkNOParams ( object sender , ProgressWorkerEventArgs e )
{
OpenBg ( false , e ) ;
}
2012-02-20 07:30:47 -04:00
2012-02-23 21:38:56 -04:00
private void OpenBg ( bool getparams , ProgressWorkerEventArgs progressWorkerEventArgs )
{
frmProgressReporter . UpdateProgressAndStatus ( - 1 , "Mavlink Connecting..." ) ;
2011-09-08 22:31:32 -03:00
// reset
sysid = 0 ;
compid = 0 ;
param = new Hashtable ( ) ;
try
{
MainV2 . givecomport = true ;
BaseStream . ReadBufferSize = 4 * 1024 ;
lock ( objlock ) // so we dont have random traffic
{
2012-03-01 09:27:03 -04:00
log . Info ( "Open port with " + BaseStream . PortName + " " + BaseStream . BaudRate ) ;
2011-09-08 22:31:32 -03:00
BaseStream . Open ( ) ;
BaseStream . DiscardInBuffer ( ) ;
2011-12-29 06:31:42 -04:00
BaseStream . toggleDTR ( ) ;
2011-09-08 22:31:32 -03:00
2012-02-23 21:38:56 -04:00
Thread . Sleep ( 1000 ) ;
2011-09-08 22:31:32 -03:00
}
byte [ ] buffer ;
byte [ ] buffer1 ;
DateTime start = DateTime . Now ;
2012-02-20 07:30:47 -04:00
DateTime deadline = start . AddSeconds ( CONNECT_TIMEOUT_SECONDS ) ;
var countDown = new System . Timers . Timer { Interval = 1000 , AutoReset = false } ;
countDown . Elapsed + = ( sender , e ) = >
{
int secondsRemaining = ( deadline - e . SignalTime ) . Seconds ;
2012-02-23 21:38:56 -04:00
//if (Progress != null)
// Progress(-1, string.Format("Trying to connect.\nTimeout in {0}", secondsRemaining));
frmProgressReporter . UpdateProgressAndStatus ( - 1 , string . Format ( "Trying to connect.\nTimeout in {0}" , secondsRemaining ) ) ;
2012-02-20 07:30:47 -04:00
if ( secondsRemaining > 0 ) countDown . Start ( ) ;
} ;
countDown . Start ( ) ;
2011-09-08 22:31:32 -03:00
int count = 0 ;
while ( true )
{
2012-02-23 21:38:56 -04:00
if ( progressWorkerEventArgs . CancelRequested )
{
progressWorkerEventArgs . CancelAcknowledged = true ;
countDown . Stop ( ) ;
if ( BaseStream . IsOpen )
BaseStream . Close ( ) ;
MainV2 . givecomport = false ;
return ;
}
2011-09-08 22:31:32 -03:00
// incase we are in setup mode
BaseStream . WriteLine ( "planner\rgcs\r" ) ;
2012-03-01 09:27:03 -04:00
log . Info ( DateTime . Now . Millisecond + " Start connect loop " ) ;
2012-02-20 07:30:47 -04:00
2011-09-08 22:31:32 -03:00
if ( lastbad [ 0 ] = = '!' & & lastbad [ 1 ] = = 'G' | | lastbad [ 0 ] = = 'G' & & lastbad [ 1 ] = = '!' ) // waiting for gps lock
{
2012-02-23 21:38:56 -04:00
//if (Progress != null)
// Progress(-1, "Waiting for GPS detection..");
frmProgressReporter . UpdateProgressAndStatus ( - 1 , "Waiting for GPS detection.." ) ;
deadline = deadline . AddSeconds ( 5 ) ; // each round is 1.1 seconds
2011-09-08 22:31:32 -03:00
}
2012-02-20 07:30:47 -04:00
if ( DateTime . Now > deadline )
2011-09-08 22:31:32 -03:00
{
2012-02-23 21:38:56 -04:00
//if (Progress != null)
// Progress(-1, "No Heatbeat Packets");
2012-02-20 07:30:47 -04:00
this . Close ( ) ;
2012-02-23 21:38:56 -04:00
progressWorkerEventArgs . ErrorMessage = "No Heatbeat Packets Received" ;
2012-02-20 07:30:47 -04:00
throw new Exception ( "No Mavlink Heartbeat Packets where read from this port - Verify Baud Rate and setup\nIt might also be waiting for GPS Lock\nAPM Planner waits for 2 valid heartbeat packets before connecting" ) ;
2011-09-08 22:31:32 -03:00
}
System . Threading . Thread . Sleep ( 1 ) ;
// incase we are in setup mode
BaseStream . WriteLine ( "planner\rgcs\r" ) ;
buffer = getHeartBeat ( ) ;
// incase we are in setup mode
BaseStream . WriteLine ( "planner\rgcs\r" ) ;
System . Threading . Thread . Sleep ( 1 ) ;
buffer1 = getHeartBeat ( ) ;
try
{
2012-03-01 09:27:03 -04:00
log . Debug ( "MAv Data: len " + buffer . Length + " btr " + BaseStream . BytesToRead ) ;
2011-09-08 22:31:32 -03:00
}
catch { }
count + + ;
if ( buffer . Length > 5 & & buffer1 . Length > 5 & & buffer [ 3 ] = = buffer1 [ 3 ] & & buffer [ 4 ] = = buffer1 [ 4 ] )
{
2012-02-20 07:30:47 -04:00
__mavlink_heartbeat_t hb = buffer . ByteArrayToStructure < __mavlink_heartbeat_t > ( 6 ) ;
2011-09-17 10:22:07 -03:00
mavlinkversion = hb . mavlink_version ;
2011-12-04 18:43:29 -04:00
aptype = hb . type ;
2011-09-17 10:22:07 -03:00
2011-09-08 22:31:32 -03:00
sysid = buffer [ 3 ] ;
compid = buffer [ 4 ] ;
recvpacketcount = buffer [ 2 ] ;
2012-02-26 19:13:23 -04:00
log . InfoFormat ( "ID sys {0} comp {1} ver{2}" , sysid , compid , mavlinkversion ) ;
2011-09-08 22:31:32 -03:00
break ;
}
2012-02-20 07:30:47 -04:00
2011-09-08 22:31:32 -03:00
}
2012-02-20 07:30:47 -04:00
countDown . Stop ( ) ;
2012-02-23 21:38:56 -04:00
// if (Progress != null)
// Progress(-1, "Getting Params.. (sysid " + sysid + " compid " + compid + ") ");
frmProgressReporter . UpdateProgressAndStatus ( 0 , "Getting Params.. (sysid " + sysid + " compid " + compid + ") " ) ;
2012-02-20 07:30:47 -04:00
if ( getparams )
2012-02-23 21:38:56 -04:00
getParamListBG ( ) ;
2012-02-24 07:39:02 -04:00
if ( frmProgressReporter . doWorkArgs . CancelAcknowledged = = true )
{
MainV2 . givecomport = false ;
if ( BaseStream . IsOpen )
BaseStream . Close ( ) ;
return ;
}
2011-09-08 22:31:32 -03:00
}
2011-12-27 19:05:12 -04:00
catch ( Exception e )
{
try
{
BaseStream . Close ( ) ;
}
catch { }
MainV2 . givecomport = false ;
2012-02-23 21:38:56 -04:00
// if (Progress != null)
// Progress(-1, "Connect Failed\n" + e.Message);
if ( string . IsNullOrEmpty ( progressWorkerEventArgs . ErrorMessage ) )
progressWorkerEventArgs . ErrorMessage = "Connect Failed" ;
2012-02-29 09:19:54 -04:00
throw e ;
2011-11-19 20:17:17 -04:00
}
2012-02-23 21:38:56 -04:00
//frmProgressReporter.Close();
2011-09-08 22:31:32 -03:00
MainV2 . givecomport = false ;
2012-02-23 21:38:56 -04:00
frmProgressReporter . UpdateProgressAndStatus ( 100 , "Done." ) ;
2012-02-26 19:13:23 -04:00
log . Info ( "Done open " + sysid + " " + compid ) ;
2011-09-17 12:49:32 -03:00
packetslost = 0 ;
2011-09-08 22:31:32 -03:00
}
byte [ ] getHeartBeat ( )
{
DateTime start = DateTime . Now ;
while ( true )
{
byte [ ] buffer = readPacket ( ) ;
if ( buffer . Length > 5 )
{
2012-03-01 09:27:03 -04:00
log . Info ( "getHB packet received: " + buffer . Length + " btr " + BaseStream . BytesToRead + " type " + buffer [ 5 ] ) ;
2011-09-08 22:31:32 -03:00
if ( buffer [ 5 ] = = MAVLINK_MSG_ID_HEARTBEAT )
{
return buffer ;
}
}
if ( DateTime . Now > start . AddMilliseconds ( 2200 ) ) // was 1200 , now 2.2 sec
return new byte [ 0 ] ;
}
}
2011-11-08 09:22:07 -04:00
public void sendPacket ( object indata )
{
2011-11-12 09:17:26 -04:00
bool run = false ;
2011-11-08 09:22:07 -04:00
byte a = 0 ;
foreach ( Type ty in mavstructs )
{
if ( ty = = indata . GetType ( ) )
{
2011-11-12 09:17:26 -04:00
run = true ;
2011-11-08 09:22:07 -04:00
generatePacket ( a , indata ) ;
return ;
}
a + + ;
}
2011-11-12 09:17:26 -04:00
if ( ! run )
{
2012-02-26 19:13:23 -04:00
log . Info ( "Mavlink : NOT VALID PACKET sendPacket() " + indata . GetType ( ) . ToString ( ) ) ;
2011-11-12 09:17:26 -04:00
}
2011-11-08 09:22:07 -04:00
}
2011-09-08 22:31:32 -03:00
/// <summary>
/// Generate a Mavlink Packet and write to serial
/// </summary>
/// <param name="messageType">type number</param>
/// <param name="indata">struct of data</param>
2011-11-08 09:22:07 -04:00
void generatePacket ( byte messageType , object indata )
2011-09-08 22:31:32 -03:00
{
2011-09-17 10:22:07 -03:00
byte [ ] data ;
if ( mavlinkversion = = 3 )
{
2012-02-20 07:30:47 -04:00
data = MavlinkUtil . StructureToByteArray ( indata ) ;
2011-09-17 10:22:07 -03:00
}
else
{
2012-02-20 07:30:47 -04:00
data = MavlinkUtil . StructureToByteArrayBigEndian ( indata ) ;
2011-09-17 10:22:07 -03:00
}
2011-09-08 22:31:32 -03:00
//Console.WriteLine(DateTime.Now + " PC Doing req "+ messageType + " " + this.BytesToRead);
byte [ ] packet = new byte [ data . Length + 6 + 2 ] ;
2011-09-17 10:22:07 -03:00
if ( mavlinkversion = = 3 )
{
packet [ 0 ] = 254 ;
}
else if ( mavlinkversion = = 2 )
{
packet [ 0 ] = ( byte ) 'U' ;
}
2011-09-08 22:31:32 -03:00
packet [ 1 ] = ( byte ) data . Length ;
packet [ 2 ] = packetcount ;
packet [ 3 ] = 255 ; // this is always 255 - MYGCS
2011-11-08 09:22:07 -04:00
#if MAVLINK10
packet [ 4 ] = ( byte ) MAV_COMPONENT . MAV_COMP_ID_MISSIONPLANNER ;
#else
2011-11-18 10:33:44 -04:00
packet [ 4 ] = ( byte ) MAV_COMPONENT . MAV_COMP_ID_WAYPOINTPLANNER ;
2011-11-08 09:22:07 -04:00
#endif
2011-09-08 22:31:32 -03:00
packet [ 5 ] = messageType ;
int i = 6 ;
foreach ( byte b in data )
{
packet [ i ] = b ;
i + + ;
}
2012-02-20 07:30:47 -04:00
ushort checksum = MavlinkCRC . crc_calculate ( packet , packet [ 1 ] + 6 ) ;
2011-09-17 10:22:07 -03:00
if ( mavlinkversion = = 3 )
{
2012-02-20 07:30:47 -04:00
checksum = MavlinkCRC . crc_accumulate ( MAVLINK_MESSAGE_CRCS [ messageType ] , checksum ) ;
2011-09-17 10:22:07 -03:00
}
2011-09-08 22:31:32 -03:00
byte ck_a = ( byte ) ( checksum & 0xFF ) ; ///< High byte
byte ck_b = ( byte ) ( checksum > > 8 ) ; ///< Low byte
packet [ i ] = ck_a ;
i + = 1 ;
packet [ i ] = ck_b ;
i + = 1 ;
if ( BaseStream . IsOpen )
{
lock ( objlock )
{
BaseStream . Write ( packet , 0 , i ) ;
}
}
try
{
if ( logfile ! = null )
{
lock ( logwritelock )
{
byte [ ] datearray = BitConverter . GetBytes ( ( UInt64 ) ( ( DateTime . UtcNow - new DateTime ( 1970 , 1 , 1 ) ) . TotalMilliseconds * 1000 ) ) ; //ASCIIEncoding.ASCII.GetBytes(DateTime.Now.ToBinary() + ":");
Array . Reverse ( datearray ) ;
logfile . Write ( datearray , 0 , datearray . Length ) ;
logfile . Write ( packet , 0 , i ) ;
2012-01-11 13:33:42 -04:00
logfile . Flush ( ) ;
2011-09-08 22:31:32 -03:00
}
}
}
catch { }
if ( messageType = = ArdupilotMega . MAVLink . MAVLINK_MSG_ID_REQUEST_DATA_STREAM )
{
try
{
BinaryWriter bw = new BinaryWriter ( File . OpenWrite ( "serialsent.raw" ) ) ;
bw . Seek ( 0 , SeekOrigin . End ) ;
bw . Write ( packet , 0 , i ) ;
bw . Write ( ( byte ) '\n' ) ;
bw . Close ( ) ;
}
catch { } // been getting errors from this. people must have it open twice.
}
packetcount + + ;
2011-11-21 20:32:11 -04:00
2011-09-08 22:31:32 -03:00
//System.Threading.Thread.Sleep(1);
}
public bool Write ( string line )
{
lock ( objlock )
{
BaseStream . Write ( line ) ;
}
return true ;
}
2012-02-10 20:04:41 -04:00
public bool setParam ( string paramname , object flag )
{
int value = ( int ) ( float ) param [ paramname ] ;
2012-02-20 07:30:47 -04:00
return setParam ( paramname , value | ( int ) flag ) ;
2012-02-10 20:04:41 -04:00
}
2011-09-08 22:31:32 -03:00
/// <summary>
/// Set parameter on apm
/// </summary>
/// <param name="paramname">name as a string</param>
/// <param name="value"></param>
public bool setParam ( string paramname , float value )
{
2011-11-19 20:17:17 -04:00
if ( ! param . ContainsKey ( paramname ) )
{
2012-02-26 19:13:23 -04:00
log . Info ( "Param doesnt exist " + paramname ) ;
2011-11-19 20:17:17 -04:00
return false ;
}
2011-09-08 22:31:32 -03:00
MainV2 . givecomport = true ;
__mavlink_param_set_t req = new __mavlink_param_set_t ( ) ;
req . target_system = sysid ;
req . target_component = compid ;
byte [ ] temp = ASCIIEncoding . ASCII . GetBytes ( paramname ) ;
2011-09-17 10:22:07 -03:00
modifyParamForDisplay ( false , paramname , ref value ) ;
2011-11-08 09:22:07 -04:00
#if MAVLINK10
Array . Resize ( ref temp , 16 ) ;
#else
2011-11-18 10:33:44 -04:00
Array . Resize ( ref temp , 15 ) ;
2011-11-08 09:22:07 -04:00
#endif
2011-09-08 22:31:32 -03:00
req . param_id = temp ;
req . param_value = ( value ) ;
generatePacket ( MAVLINK_MSG_ID_PARAM_SET , req ) ;
2012-02-26 19:13:23 -04:00
log . InfoFormat ( "setParam '{0}' = '{1}' sysid {2} compid {3}" , paramname , req . param_value , sysid , compid ) ;
2011-09-08 22:31:32 -03:00
DateTime start = DateTime . Now ;
int retrys = 3 ;
while ( true )
{
if ( ! ( start . AddMilliseconds ( 500 ) > DateTime . Now ) )
{
if ( retrys > 0 )
{
2012-02-26 19:13:23 -04:00
log . Info ( "setParam Retry " + retrys ) ;
2011-09-08 22:31:32 -03:00
generatePacket ( MAVLINK_MSG_ID_PARAM_SET , req ) ;
start = DateTime . Now ;
retrys - - ;
continue ;
}
MainV2 . givecomport = false ;
throw new Exception ( "Timeout on read - setParam " + paramname ) ;
}
byte [ ] buffer = readPacket ( ) ;
if ( buffer . Length > 5 )
{
if ( buffer [ 5 ] = = MAVLINK_MSG_ID_PARAM_VALUE )
{
2012-02-20 07:30:47 -04:00
__mavlink_param_value_t par = buffer . ByteArrayToStructure < __mavlink_param_value_t > ( 6 ) ;
2011-09-17 10:22:07 -03:00
string st = System . Text . ASCIIEncoding . ASCII . GetString ( par . param_id ) ;
int pos = st . IndexOf ( '\0' ) ;
if ( pos ! = - 1 )
{
st = st . Substring ( 0 , pos ) ;
}
if ( st ! = paramname )
{
2012-02-26 19:13:23 -04:00
log . InfoFormat ( "MAVLINK bad param responce - {0} vs {1}" , paramname , st ) ;
2011-09-17 10:22:07 -03:00
continue ;
}
2011-09-25 05:03:00 -03:00
modifyParamForDisplay ( true , st , ref par . param_value ) ;
2011-09-17 10:22:07 -03:00
param [ st ] = ( par . param_value ) ;
2011-09-08 22:31:32 -03:00
MainV2 . givecomport = false ;
//System.Threading.Thread.Sleep(100);//(int)(8.5 * 5)); // 8.5ms per byte
return true ;
}
}
}
}
2012-01-19 10:01:53 -04:00
/ *
public Bitmap getImage ( )
{
MemoryStream ms = new MemoryStream ( ) ;
2011-09-08 22:31:32 -03:00
2012-01-19 10:01:53 -04:00
}
* /
2012-02-23 21:38:56 -04:00
public void getParamList ( )
{
frmProgressReporter = new ProgressReporterDialogue
{
StartPosition = System . Windows . Forms . FormStartPosition . CenterScreen ,
Text = "Getting Params"
} ;
frmProgressReporter . DoWork + = FrmProgressReporterGetParams ;
frmProgressReporter . UpdateProgressAndStatus ( - 1 , "Getting Params..." ) ;
2012-03-03 20:42:42 -04:00
ThemeManager . ApplyThemeTo ( frmProgressReporter ) ;
2012-02-23 21:38:56 -04:00
frmProgressReporter . RunBackgroundOperationAsync ( ) ;
}
void FrmProgressReporterGetParams ( object sender , ProgressWorkerEventArgs e )
{
Hashtable old = new Hashtable ( param ) ;
getParamListBG ( ) ;
if ( frmProgressReporter . doWorkArgs . CancelRequested )
{
param = old ;
}
}
2011-09-08 22:31:32 -03:00
/// <summary>
/// Get param list from apm
/// </summary>
/// <returns></returns>
2012-02-23 21:38:56 -04:00
private Hashtable getParamListBG ( )
2011-09-08 22:31:32 -03:00
{
MainV2 . givecomport = true ;
2012-02-24 07:39:02 -04:00
List < int > got = new List < int > ( ) ;
2011-12-27 19:05:12 -04:00
2012-02-23 21:38:56 -04:00
// clear old
param = new Hashtable ( ) ;
int retrys = 3 ;
int param_count = 0 ;
int param_total = 5 ;
goagain :
2011-09-08 22:31:32 -03:00
__mavlink_param_request_list_t req = new __mavlink_param_request_list_t ( ) ;
req . target_system = sysid ;
req . target_component = compid ;
generatePacket ( MAVLINK_MSG_ID_PARAM_REQUEST_LIST , req ) ;
DateTime start = DateTime . Now ;
2011-11-02 21:13:27 -03:00
DateTime restart = DateTime . Now ;
2012-02-24 07:39:02 -04:00
while ( got . Count < param_total )
2011-09-08 22:31:32 -03:00
{
2012-02-23 21:38:56 -04:00
if ( frmProgressReporter . doWorkArgs . CancelRequested )
{
frmProgressReporter . doWorkArgs . CancelAcknowledged = true ;
MainV2 . givecomport = false ;
frmProgressReporter . doWorkArgs . ErrorMessage = "User Canceled" ;
return param ;
}
2011-09-08 22:31:32 -03:00
if ( ! ( start . AddMilliseconds ( 5000 ) > DateTime . Now ) )
{
if ( retrys > 0 )
{
2012-02-26 19:13:23 -04:00
log . InfoFormat ( "getParamList Retry {0} sys {1} comp {2}" , retrys , sysid , compid ) ;
2011-09-08 22:31:32 -03:00
generatePacket ( MAVLINK_MSG_ID_PARAM_REQUEST_LIST , req ) ;
start = DateTime . Now ;
retrys - - ;
continue ;
}
2011-12-27 19:05:12 -04:00
MainV2 . givecomport = false ;
2012-02-24 07:39:02 -04:00
throw new Exception ( "Timeout on read - getParamList " + param_count + " " + param_total ) ;
2011-12-27 19:05:12 -04:00
}
2011-11-08 09:22:07 -04:00
2011-09-08 22:31:32 -03:00
byte [ ] buffer = readPacket ( ) ;
if ( buffer . Length > 5 )
{
2011-12-27 19:05:12 -04:00
//stopwatch.Reset();
//stopwatch.Start();
2011-09-08 22:31:32 -03:00
if ( buffer [ 5 ] = = MAVLINK_MSG_ID_PARAM_VALUE )
{
2011-11-02 21:13:27 -03:00
restart = DateTime . Now ;
2011-09-08 22:31:32 -03:00
start = DateTime . Now ;
2012-02-20 07:30:47 -04:00
__mavlink_param_value_t par = buffer . ByteArrayToStructure < __mavlink_param_value_t > ( 6 ) ;
2011-09-08 22:31:32 -03:00
2012-02-23 21:38:56 -04:00
// set new target
2012-02-20 07:30:47 -04:00
param_total = ( par . param_count ) ;
2011-09-08 22:31:32 -03:00
2012-02-23 21:38:56 -04:00
2012-02-20 07:30:47 -04:00
string paramID = System . Text . ASCIIEncoding . ASCII . GetString ( par . param_id ) ;
2011-09-08 22:31:32 -03:00
2012-02-20 07:30:47 -04:00
int pos = paramID . IndexOf ( '\0' ) ;
if ( pos ! = - 1 )
{
paramID = paramID . Substring ( 0 , pos ) ;
}
2011-12-27 19:05:12 -04:00
2012-02-23 21:38:56 -04:00
// check if we already have it
2012-02-24 07:39:02 -04:00
if ( got . Contains ( par . param_index ) )
{
//Console.WriteLine("Already got '"+paramID+"'");
2012-02-23 21:38:56 -04:00
continue ;
2011-09-08 22:31:32 -03:00
}
2012-02-26 19:13:23 -04:00
log . Info ( DateTime . Now . Millisecond + " got param " + ( par . param_index ) + " of " + ( param_total - 1 ) + " name: " + paramID ) ;
2012-02-23 21:38:56 -04:00
2012-02-20 07:30:47 -04:00
modifyParamForDisplay ( true , paramID , ref par . param_value ) ;
param [ paramID ] = ( par . param_value ) ;
2011-12-27 19:05:12 -04:00
param_count + + ;
2012-02-24 07:39:02 -04:00
got . Add ( par . param_index ) ;
2012-02-23 21:38:56 -04:00
// if (Progress != null)
// Progress((param.Count * 100) / param_total, "Got param " + paramID);
2012-02-24 07:39:02 -04:00
this . frmProgressReporter . UpdateProgressAndStatus ( ( got . Count * 100 ) / param_total , "Got param " + paramID ) ;
2011-12-27 19:05:12 -04:00
}
else
{
2012-02-23 21:38:56 -04:00
//Console.WriteLine(DateTime.Now + " PC paramlist " + buffer[5] + " want " + MAVLINK_MSG_ID_PARAM_VALUE + " btr " + BaseStream.BytesToRead);
2011-09-08 22:31:32 -03:00
}
2011-12-27 19:05:12 -04:00
//stopwatch.Stop();
//Console.WriteLine("Time elapsed: {0}", stopwatch.Elapsed);
2011-09-08 22:31:32 -03:00
}
}
2012-02-23 21:38:56 -04:00
2012-02-24 07:39:02 -04:00
if ( got . Count ! = param_total )
2012-02-23 21:38:56 -04:00
{
if ( retrys > 0 )
{
2012-02-24 07:39:02 -04:00
this . frmProgressReporter . UpdateProgressAndStatus ( ( got . Count * 100 ) / param_total , "Getting missed params" ) ;
2012-02-23 21:38:56 -04:00
retrys - - ;
goto goagain ;
}
throw new Exception ( "Missing Params" ) ;
}
2011-09-08 22:31:32 -03:00
MainV2 . givecomport = false ;
return param ;
}
2011-09-20 21:23:43 -03:00
public static void modifyParamForDisplay ( bool fromapm , string paramname , ref float value )
2011-09-08 22:31:32 -03:00
{
2011-09-17 10:22:07 -03:00
if ( paramname . ToUpper ( ) . EndsWith ( "_IMAX" ) | | paramname . ToUpper ( ) . EndsWith ( "ALT_HOLD_RTL" ) | | paramname . ToUpper ( ) . EndsWith ( "TRIM_ARSPD_CM" )
| | paramname . ToUpper ( ) . EndsWith ( "XTRK_ANGLE_CD" ) | | paramname . ToUpper ( ) . EndsWith ( "LIM_PITCH_MAX" ) | | paramname . ToUpper ( ) . EndsWith ( "LIM_PITCH_MIN" )
| | paramname . ToUpper ( ) . EndsWith ( "LIM_ROLL_CD" ) | | paramname . ToUpper ( ) . EndsWith ( "PITCH_MAX" ) | | paramname . ToUpper ( ) . EndsWith ( "WP_SPEED_MAX" ) )
{
2011-11-08 09:22:07 -04:00
if ( paramname . ToUpper ( ) . EndsWith ( "THR_HOLD_IMAX" ) )
{
2011-09-19 21:04:58 -03:00
return ;
}
2011-09-17 10:22:07 -03:00
if ( fromapm )
{
value / = 100.0f ;
}
else
{
value * = 100.0f ;
}
}
2012-03-06 06:27:43 -04:00
else if ( paramname . ToUpper ( ) . StartsWith ( "TUNE_" ) )
{
if ( fromapm )
{
value / = 1000.0f ;
}
else
{
value * = 1000.0f ;
}
}
2011-09-08 22:31:32 -03:00
}
/// <summary>
/// Stops all requested data packets.
/// </summary>
public void stopall ( bool forget )
{
__mavlink_request_data_stream_t req = new __mavlink_request_data_stream_t ( ) ;
req . target_system = sysid ;
req . target_component = compid ;
req . req_message_rate = 10 ;
req . start_stop = 0 ; // stop
req . req_stream_id = 0 ; // all
// no error on bad
try
{
generatePacket ( MAVLINK_MSG_ID_REQUEST_DATA_STREAM , req ) ;
System . Threading . Thread . Sleep ( 20 ) ;
generatePacket ( MAVLINK_MSG_ID_REQUEST_DATA_STREAM , req ) ;
System . Threading . Thread . Sleep ( 20 ) ;
generatePacket ( MAVLINK_MSG_ID_REQUEST_DATA_STREAM , req ) ;
2012-02-26 19:13:23 -04:00
log . Info ( "Stopall Done" ) ;
2011-09-08 22:31:32 -03:00
}
catch { }
}
public void setWPACK ( )
{
2011-11-08 09:22:07 -04:00
#if MAVLINK10
MAVLink . __mavlink_mission_ack_t req = new MAVLink . __mavlink_mission_ack_t ( ) ;
req . target_system = sysid ;
req . target_component = compid ;
req . type = 0 ;
generatePacket ( MAVLINK_MSG_ID_MISSION_ACK , req ) ;
#else
2011-09-08 22:31:32 -03:00
MAVLink . __mavlink_waypoint_ack_t req = new MAVLink . __mavlink_waypoint_ack_t ( ) ;
req . target_system = sysid ;
req . target_component = compid ;
req . type = 0 ;
2011-11-18 10:33:44 -04:00
generatePacket ( MAVLINK_MSG_ID_WAYPOINT_ACK , req ) ;
2011-11-08 09:22:07 -04:00
#endif
2011-09-08 22:31:32 -03:00
}
public bool setWPCurrent ( ushort index )
{
2011-11-08 09:22:07 -04:00
#if MAVLINK10
MainV2 . givecomport = true ;
byte [ ] buffer ;
__mavlink_mission_set_current_t req = new __mavlink_mission_set_current_t ( ) ;
req . target_system = sysid ;
req . target_component = compid ;
req . seq = index ;
generatePacket ( MAVLINK_MSG_ID_MISSION_SET_CURRENT , req ) ;
DateTime start = DateTime . Now ;
int retrys = 5 ;
while ( true )
{
if ( ! ( start . AddMilliseconds ( 2000 ) > DateTime . Now ) )
{
if ( retrys > 0 )
{
2012-02-26 19:13:23 -04:00
log . Info ( "setWPCurrent Retry " + retrys ) ;
2011-11-08 09:22:07 -04:00
generatePacket ( MAVLINK_MSG_ID_MISSION_SET_CURRENT , req ) ;
start = DateTime . Now ;
retrys - - ;
continue ;
}
MainV2 . givecomport = false ;
throw new Exception ( "Timeout on read - setWPCurrent" ) ;
}
buffer = readPacket ( ) ;
if ( buffer . Length > 5 )
{
if ( buffer [ 5 ] = = MAVLINK_MSG_ID_MISSION_CURRENT )
{
MainV2 . givecomport = false ;
return true ;
}
}
}
}
2011-11-21 20:32:11 -04:00
public bool doCommand ( MAV_CMD actionid , float p1 , float p2 , float p3 , float p4 , float p5 , float p6 , float p7 )
2011-11-08 09:22:07 -04:00
{
MainV2 . givecomport = true ;
byte [ ] buffer ;
__mavlink_command_long_t req = new __mavlink_command_long_t ( ) ;
req . target_system = sysid ;
req . target_component = compid ;
req . command = ( ushort ) actionid ;
2011-11-21 20:32:11 -04:00
req . param1 = p1 ;
req . param2 = p2 ;
req . param3 = p3 ;
req . param4 = p4 ;
req . param5 = p5 ;
req . param6 = p6 ;
req . param7 = p7 ;
2011-11-08 09:22:07 -04:00
generatePacket ( MAVLINK_MSG_ID_COMMAND_LONG , req ) ;
DateTime start = DateTime . Now ;
int retrys = 3 ;
int timeout = 2000 ;
// imu calib take a little while
if ( actionid = = MAV_CMD . PREFLIGHT_CALIBRATION )
{
retrys = 1 ;
timeout = 6000 ;
}
while ( true )
{
if ( ! ( start . AddMilliseconds ( timeout ) > DateTime . Now ) )
{
if ( retrys > 0 )
{
2012-02-26 19:13:23 -04:00
log . Info ( "doAction Retry " + retrys ) ;
2011-11-08 09:22:07 -04:00
generatePacket ( MAVLINK_MSG_ID_COMMAND_LONG , req ) ;
start = DateTime . Now ;
retrys - - ;
continue ;
}
MainV2 . givecomport = false ;
throw new Exception ( "Timeout on read - doAction" ) ;
}
buffer = readPacket ( ) ;
if ( buffer . Length > 5 )
{
if ( buffer [ 5 ] = = MAVLINK_MSG_ID_COMMAND_ACK )
{
2012-02-20 07:30:47 -04:00
var ack = buffer . ByteArrayToStructure < __mavlink_command_ack_t > ( 6 ) ;
2011-11-08 09:22:07 -04:00
if ( ack . result = = ( byte ) MAV_RESULT . MAV_RESULT_ACCEPTED )
{
MainV2 . givecomport = false ;
return true ;
}
else
{
MainV2 . givecomport = false ;
return false ;
}
}
}
}
#else
2011-09-08 22:31:32 -03:00
MainV2 . givecomport = true ;
byte [ ] buffer ;
__mavlink_waypoint_set_current_t req = new __mavlink_waypoint_set_current_t ( ) ;
req . target_system = sysid ;
req . target_component = compid ;
req . seq = index ;
generatePacket ( MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT , req ) ;
DateTime start = DateTime . Now ;
int retrys = 5 ;
while ( true )
{
if ( ! ( start . AddMilliseconds ( 2000 ) > DateTime . Now ) )
{
if ( retrys > 0 )
{
2012-02-26 19:13:23 -04:00
log . Info ( "setWPCurrent Retry " + retrys ) ;
2011-09-08 22:31:32 -03:00
generatePacket ( MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT , req ) ;
start = DateTime . Now ;
retrys - - ;
continue ;
}
MainV2 . givecomport = false ;
throw new Exception ( "Timeout on read - setWPCurrent" ) ;
}
buffer = readPacket ( ) ;
if ( buffer . Length > 5 )
{
if ( buffer [ 5 ] = = MAVLINK_MSG_ID_WAYPOINT_CURRENT )
{
MainV2 . givecomport = false ;
return true ;
}
}
}
}
public bool doAction ( MAV_ACTION actionid )
{
MainV2 . givecomport = true ;
byte [ ] buffer ;
__mavlink_action_t req = new __mavlink_action_t ( ) ;
req . target = sysid ;
req . target_component = compid ;
req . action = ( byte ) actionid ;
generatePacket ( MAVLINK_MSG_ID_ACTION , req ) ;
DateTime start = DateTime . Now ;
int retrys = 3 ;
int timeout = 2000 ;
// imu calib take a little while
if ( actionid = = MAV_ACTION . MAV_ACTION_CALIBRATE_ACC | |
actionid = = MAV_ACTION . MAV_ACTION_CALIBRATE_GYRO | |
actionid = = MAV_ACTION . MAV_ACTION_CALIBRATE_MAG | |
actionid = = MAV_ACTION . MAV_ACTION_CALIBRATE_PRESSURE | |
actionid = = MAV_ACTION . MAV_ACTION_REBOOT )
{
retrys = 1 ;
2012-01-15 05:00:50 -04:00
timeout = 20000 ;
2011-09-08 22:31:32 -03:00
}
while ( true )
{
if ( ! ( start . AddMilliseconds ( timeout ) > DateTime . Now ) )
{
if ( retrys > 0 )
{
2012-02-26 19:13:23 -04:00
log . Info ( "doAction Retry " + retrys ) ;
2011-09-08 22:31:32 -03:00
generatePacket ( MAVLINK_MSG_ID_ACTION , req ) ;
start = DateTime . Now ;
retrys - - ;
continue ;
}
MainV2 . givecomport = false ;
throw new Exception ( "Timeout on read - doAction" ) ;
}
buffer = readPacket ( ) ;
if ( buffer . Length > 5 )
{
if ( buffer [ 5 ] = = MAVLINK_MSG_ID_ACTION_ACK )
{
if ( buffer [ 7 ] = = 1 )
{
MainV2 . givecomport = false ;
return true ;
}
else
{
MainV2 . givecomport = false ;
return false ;
}
}
}
}
2011-11-18 10:33:44 -04:00
2011-11-08 09:22:07 -04:00
#endif
2011-09-08 22:31:32 -03:00
}
public void requestDatastream ( byte id , byte hzrate )
{
double pps = 0 ;
switch ( id )
{
case ( byte ) MAVLink . MAV_DATA_STREAM . MAV_DATA_STREAM_ALL :
break ;
case ( byte ) MAVLink . MAV_DATA_STREAM . MAV_DATA_STREAM_EXTENDED_STATUS :
if ( packetspersecondbuild [ MAVLINK_MSG_ID_SYS_STATUS ] < DateTime . Now . AddSeconds ( - 2 ) )
break ;
pps = packetspersecond [ MAVLINK_MSG_ID_SYS_STATUS ] ;
if ( hzratecheck ( pps , hzrate ) )
{
return ;
}
break ;
case ( byte ) MAVLink . MAV_DATA_STREAM . MAV_DATA_STREAM_EXTRA1 :
if ( packetspersecondbuild [ MAVLINK_MSG_ID_ATTITUDE ] < DateTime . Now . AddSeconds ( - 2 ) )
break ;
pps = packetspersecond [ MAVLINK_MSG_ID_ATTITUDE ] ;
if ( hzratecheck ( pps , hzrate ) )
{
return ;
}
break ;
case ( byte ) MAVLink . MAV_DATA_STREAM . MAV_DATA_STREAM_EXTRA2 :
if ( packetspersecondbuild [ MAVLINK_MSG_ID_VFR_HUD ] < DateTime . Now . AddSeconds ( - 2 ) )
break ;
pps = packetspersecond [ MAVLINK_MSG_ID_VFR_HUD ] ;
if ( hzratecheck ( pps , hzrate ) )
{
return ;
}
break ;
case ( byte ) MAVLink . MAV_DATA_STREAM . MAV_DATA_STREAM_EXTRA3 :
break ;
case ( byte ) MAVLink . MAV_DATA_STREAM . MAV_DATA_STREAM_POSITION :
if ( packetspersecondbuild [ MAVLINK_MSG_ID_GLOBAL_POSITION_INT ] < DateTime . Now . AddSeconds ( - 2 ) )
break ;
pps = packetspersecond [ MAVLINK_MSG_ID_GLOBAL_POSITION_INT ] ;
if ( hzratecheck ( pps , hzrate ) )
{
return ;
}
break ;
case ( byte ) MAVLink . MAV_DATA_STREAM . MAV_DATA_STREAM_RAW_CONTROLLER :
if ( packetspersecondbuild [ MAVLINK_MSG_ID_RC_CHANNELS_SCALED ] < DateTime . Now . AddSeconds ( - 2 ) )
break ;
pps = packetspersecond [ MAVLINK_MSG_ID_RC_CHANNELS_SCALED ] ;
if ( hzratecheck ( pps , hzrate ) )
{
return ;
}
break ;
case ( byte ) MAVLink . MAV_DATA_STREAM . MAV_DATA_STREAM_RAW_SENSORS :
if ( packetspersecondbuild [ MAVLINK_MSG_ID_RAW_IMU ] < DateTime . Now . AddSeconds ( - 2 ) )
break ;
pps = packetspersecond [ MAVLINK_MSG_ID_RAW_IMU ] ;
if ( hzratecheck ( pps , hzrate ) )
{
return ;
}
break ;
case ( byte ) MAVLink . MAV_DATA_STREAM . MAV_DATA_STREAM_RC_CHANNELS :
if ( packetspersecondbuild [ MAVLINK_MSG_ID_RC_CHANNELS_RAW ] < DateTime . Now . AddSeconds ( - 2 ) )
break ;
pps = packetspersecond [ MAVLINK_MSG_ID_RC_CHANNELS_RAW ] ;
if ( hzratecheck ( pps , hzrate ) )
{
return ;
}
break ;
}
//packetspersecond[temp[5]];
if ( pps = = 0 & & hzrate = = 0 )
{
return ;
}
2012-02-26 19:13:23 -04:00
log . InfoFormat ( "Request stream {0} at {1} hz : currently {2}" , Enum . Parse ( typeof ( MAV_DATA_STREAM ) , id . ToString ( ) ) , hzrate , pps ) ;
2011-09-08 22:31:32 -03:00
getDatastream ( id , hzrate ) ;
}
// returns true for ok
bool hzratecheck ( double pps , int hzrate )
{
if ( hzrate = = 0 & & pps = = 0 )
{
return true ;
}
else if ( hzrate = = 1 & & pps > = 0.5 & & pps < = 2 )
{
return true ;
}
else if ( hzrate = = 3 & & pps > = 2 & & hzrate < 5 )
{
return true ;
}
else if ( hzrate = = 10 & & pps > 5 & & hzrate < 15 )
{
return true ;
}
else if ( hzrate > 15 & & pps > 15 )
{
return true ;
}
return false ;
}
void getDatastream ( byte id , byte hzrate )
{
__mavlink_request_data_stream_t req = new __mavlink_request_data_stream_t ( ) ;
req . target_system = sysid ;
req . target_component = compid ;
req . req_message_rate = hzrate ;
req . start_stop = 1 ; // start
req . req_stream_id = id ; // id
generatePacket ( MAVLINK_MSG_ID_REQUEST_DATA_STREAM , req ) ;
generatePacket ( MAVLINK_MSG_ID_REQUEST_DATA_STREAM , req ) ;
}
/// <summary>
/// Returns WP count
/// </summary>
/// <returns></returns>
public byte getWPCount ( )
{
MainV2 . givecomport = true ;
byte [ ] buffer ;
2011-11-08 09:22:07 -04:00
#if MAVLINK10
__mavlink_mission_request_list_t req = new __mavlink_mission_request_list_t ( ) ;
req . target_system = sysid ;
req . target_component = compid ;
// request list
generatePacket ( MAVLINK_MSG_ID_MISSION_REQUEST_LIST , req ) ;
DateTime start = DateTime . Now ;
int retrys = 6 ;
while ( true )
{
if ( ! ( start . AddMilliseconds ( 500 ) > DateTime . Now ) )
{
if ( retrys > 0 )
{
2012-02-26 19:13:23 -04:00
log . Info ( "getWPCount Retry " + retrys + " - giv com " + MainV2 . givecomport ) ;
2011-11-08 09:22:07 -04:00
generatePacket ( MAVLINK_MSG_ID_MISSION_REQUEST_LIST , req ) ;
start = DateTime . Now ;
retrys - - ;
continue ;
}
MainV2 . givecomport = false ;
//return (byte)int.Parse(param["WP_TOTAL"].ToString());
throw new Exception ( "Timeout on read - getWPCount" ) ;
}
buffer = readPacket ( ) ;
if ( buffer . Length > 5 )
{
if ( buffer [ 5 ] = = MAVLINK_MSG_ID_MISSION_COUNT )
{
2012-02-20 07:30:47 -04:00
var count = buffer . ByteArrayToStructure < __mavlink_mission_count_t > ( 6 ) ;
2011-11-08 09:22:07 -04:00
2012-02-26 19:13:23 -04:00
log . Info ( "wpcount: " + count . count ) ;
2011-11-08 09:22:07 -04:00
MainV2 . givecomport = false ;
return ( byte ) count . count ; // should be ushort, but apm has limited wp count < byte
}
else
{
2012-02-26 19:13:23 -04:00
log . Info ( DateTime . Now + " PC wpcount " + buffer [ 5 ] + " need " + MAVLINK_MSG_ID_MISSION_COUNT + " " + this . BaseStream . BytesToRead ) ;
2011-11-08 09:22:07 -04:00
}
}
}
2011-11-18 10:33:44 -04:00
#else
2011-09-08 22:31:32 -03:00
__mavlink_waypoint_request_list_t req = new __mavlink_waypoint_request_list_t ( ) ;
req . target_system = sysid ;
req . target_component = compid ;
// request list
generatePacket ( MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST , req ) ;
DateTime start = DateTime . Now ;
int retrys = 6 ;
while ( true )
{
if ( ! ( start . AddMilliseconds ( 500 ) > DateTime . Now ) )
{
if ( retrys > 0 )
{
2012-02-26 19:13:23 -04:00
log . Info ( "getWPCount Retry " + retrys + " - giv com " + MainV2 . givecomport ) ;
2011-09-08 22:31:32 -03:00
generatePacket ( MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST , req ) ;
start = DateTime . Now ;
retrys - - ;
continue ;
}
MainV2 . givecomport = false ;
//return (byte)int.Parse(param["WP_TOTAL"].ToString());
throw new Exception ( "Timeout on read - getWPCount" ) ;
}
buffer = readPacket ( ) ;
if ( buffer . Length > 5 )
{
if ( buffer [ 5 ] = = MAVLINK_MSG_ID_WAYPOINT_COUNT )
{
2012-02-26 19:13:23 -04:00
log . Info ( "wpcount: " + buffer [ 9 ] ) ;
2011-09-08 22:31:32 -03:00
MainV2 . givecomport = false ;
return buffer [ 9 ] ; // should be ushort, but apm has limited wp count < byte
}
else
{
2012-02-26 19:13:23 -04:00
log . Info ( DateTime . Now + " PC wpcount " + buffer [ 5 ] + " need " + MAVLINK_MSG_ID_WAYPOINT_COUNT + " " + this . BaseStream . BytesToRead ) ;
2011-09-08 22:31:32 -03:00
}
}
}
2011-11-18 10:33:44 -04:00
#endif
2011-09-08 22:31:32 -03:00
}
/// <summary>
/// Gets specfied WP
/// </summary>
/// <param name="index"></param>
/// <returns>WP</returns>
public Locationwp getWP ( ushort index )
{
MainV2 . givecomport = true ;
Locationwp loc = new Locationwp ( ) ;
2011-11-08 09:22:07 -04:00
#if MAVLINK10
__mavlink_mission_request_t req = new __mavlink_mission_request_t ( ) ;
req . target_system = sysid ;
req . target_component = compid ;
req . seq = index ;
//Console.WriteLine("getwp req "+ DateTime.Now.Millisecond);
// request
generatePacket ( MAVLINK_MSG_ID_MISSION_REQUEST , req ) ;
DateTime start = DateTime . Now ;
int retrys = 5 ;
while ( true )
{
if ( ! ( start . AddMilliseconds ( 800 ) > DateTime . Now ) ) // apm times out after 1000ms
{
if ( retrys > 0 )
{
2012-02-26 19:13:23 -04:00
log . Info ( "getWP Retry " + retrys ) ;
2011-11-08 09:22:07 -04:00
generatePacket ( MAVLINK_MSG_ID_MISSION_REQUEST , req ) ;
start = DateTime . Now ;
retrys - - ;
continue ;
}
MainV2 . givecomport = false ;
throw new Exception ( "Timeout on read - getWP" ) ;
}
//Console.WriteLine("getwp read " + DateTime.Now.Millisecond);
byte [ ] buffer = readPacket ( ) ;
//Console.WriteLine("getwp readend " + DateTime.Now.Millisecond);
if ( buffer . Length > 5 )
{
if ( buffer [ 5 ] = = MAVLINK_MSG_ID_MISSION_ITEM )
{
//Console.WriteLine("getwp ans " + DateTime.Now.Millisecond);
//Array.Copy(buffer, 6, buffer, 0, buffer.Length - 6);
2012-02-20 07:30:47 -04:00
var wp = buffer . ByteArrayToStructure < __mavlink_mission_item_t > ( 6 ) ;
2011-11-08 09:22:07 -04:00
#else
2011-09-08 22:31:32 -03:00
__mavlink_waypoint_request_t req = new __mavlink_waypoint_request_t ( ) ;
req . target_system = sysid ;
req . target_component = compid ;
req . seq = index ;
2011-09-14 10:31:00 -03:00
//Console.WriteLine("getwp req "+ DateTime.Now.Millisecond);
2011-09-08 22:31:32 -03:00
// request
generatePacket ( MAVLINK_MSG_ID_WAYPOINT_REQUEST , req ) ;
DateTime start = DateTime . Now ;
int retrys = 5 ;
while ( true )
{
if ( ! ( start . AddMilliseconds ( 800 ) > DateTime . Now ) ) // apm times out after 1000ms
{
if ( retrys > 0 )
{
2012-02-26 19:13:23 -04:00
log . Info ( "getWP Retry " + retrys ) ;
2011-09-08 22:31:32 -03:00
generatePacket ( MAVLINK_MSG_ID_WAYPOINT_REQUEST , req ) ;
start = DateTime . Now ;
retrys - - ;
continue ;
}
MainV2 . givecomport = false ;
throw new Exception ( "Timeout on read - getWP" ) ;
}
2011-09-14 10:31:00 -03:00
//Console.WriteLine("getwp read " + DateTime.Now.Millisecond);
2011-09-08 22:31:32 -03:00
byte [ ] buffer = readPacket ( ) ;
2011-09-14 10:31:00 -03:00
//Console.WriteLine("getwp readend " + DateTime.Now.Millisecond);
2011-09-08 22:31:32 -03:00
if ( buffer . Length > 5 )
{
if ( buffer [ 5 ] = = MAVLINK_MSG_ID_WAYPOINT )
{
2011-09-14 10:31:00 -03:00
//Console.WriteLine("getwp ans " + DateTime.Now.Millisecond);
2012-02-20 07:30:47 -04:00
__mavlink_waypoint_t wp = buffer . ByteArrayToStructure < __mavlink_waypoint_t > ( 6 ) ;
2011-09-08 22:31:32 -03:00
2011-11-08 09:22:07 -04:00
#endif
2011-09-08 22:31:32 -03:00
loc . options = ( byte ) ( wp . frame & 0x1 ) ;
loc . id = ( byte ) ( wp . command ) ;
2011-11-18 10:33:44 -04:00
loc . p1 = ( wp . param1 ) ;
loc . p2 = ( wp . param2 ) ;
loc . p3 = ( wp . param3 ) ;
loc . p4 = ( wp . param4 ) ;
loc . alt = ( ( wp . z ) ) ;
loc . lat = ( ( wp . x ) ) ;
loc . lng = ( ( wp . y ) ) ;
/ *
if ( MainV2 . cs . firmware = = MainV2 . Firmwares . ArduPlane )
2011-09-08 22:31:32 -03:00
{
switch ( loc . id )
{ // Switch to map APM command fields inot MAVLink command fields
case ( byte ) MAV_CMD . LOITER_TURNS :
case ( byte ) MAV_CMD . TAKEOFF :
case ( byte ) MAV_CMD . DO_SET_HOME :
2011-11-08 09:22:07 -04:00
//case (byte)MAV_CMD.DO_SET_ROI:
2011-11-18 10:33:44 -04:00
loc . alt = ( float ) ( ( wp . z ) ) ;
loc . lat = ( float ) ( ( wp . x ) ) ;
loc . lng = ( float ) ( ( wp . y ) ) ;
loc . p1 = ( float ) wp . param1 ;
2011-09-08 22:31:32 -03:00
break ;
case ( byte ) MAV_CMD . CONDITION_CHANGE_ALT :
loc . lat = ( int ) wp . param1 ;
loc . p1 = 0 ;
break ;
case ( byte ) MAV_CMD . LOITER_TIME :
2011-09-10 03:15:14 -03:00
if ( MainV2 . APMFirmware = = MainV2 . Firmwares . ArduPlane )
2011-09-08 22:31:32 -03:00
{
loc . p1 = ( byte ) ( wp . param1 / 10 ) ; // APM loiter time is in ten second increments
}
else
{
loc . p1 = ( byte ) wp . param1 ;
}
break ;
case ( byte ) MAV_CMD . CONDITION_DELAY :
case ( byte ) MAV_CMD . CONDITION_DISTANCE :
loc . lat = ( int ) wp . param1 ;
break ;
case ( byte ) MAV_CMD . DO_JUMP :
loc . lat = ( int ) wp . param2 ;
loc . p1 = ( byte ) wp . param1 ;
break ;
case ( byte ) MAV_CMD . DO_REPEAT_SERVO :
loc . lng = ( int ) wp . param4 ;
goto case ( byte ) MAV_CMD . DO_CHANGE_SPEED ;
case ( byte ) MAV_CMD . DO_REPEAT_RELAY :
case ( byte ) MAV_CMD . DO_CHANGE_SPEED :
loc . lat = ( int ) wp . param3 ;
loc . alt = ( int ) wp . param2 ;
loc . p1 = ( byte ) wp . param1 ;
break ;
case ( byte ) MAV_CMD . DO_SET_PARAMETER :
case ( byte ) MAV_CMD . DO_SET_RELAY :
case ( byte ) MAV_CMD . DO_SET_SERVO :
loc . alt = ( int ) wp . param2 ;
loc . p1 = ( byte ) wp . param1 ;
break ;
case ( byte ) MAV_CMD . WAYPOINT :
loc . p1 = ( byte ) wp . param1 ;
break ;
}
}
2011-11-18 10:33:44 -04:00
* /
2012-02-26 19:13:23 -04:00
log . InfoFormat ( "getWP {0} {1} {2} {3} {4} opt {5}" , loc . id , loc . p1 , loc . alt , loc . lat , loc . lng , loc . options ) ;
2011-09-08 22:31:32 -03:00
break ;
}
else
{
2012-02-26 19:13:23 -04:00
log . Info ( DateTime . Now + " PC getwp " + buffer [ 5 ] ) ;
2011-09-08 22:31:32 -03:00
}
}
}
MainV2 . givecomport = false ;
return loc ;
}
public object DebugPacket ( byte [ ] datin )
{
string text = "" ;
2012-03-06 06:27:43 -04:00
return DebugPacket ( datin , ref text , true ) ;
}
public object DebugPacket ( byte [ ] datin , bool PrintToConsole )
{
string text = "" ;
return DebugPacket ( datin , ref text , PrintToConsole ) ;
}
public object DebugPacket ( byte [ ] datin , ref string text )
{
return DebugPacket ( datin , ref text , true ) ;
2011-09-08 22:31:32 -03:00
}
/// <summary>
/// Print entire decoded packet to console
/// </summary>
/// <param name="datin">packet byte array</param>
/// <returns>struct of data</returns>
2012-03-06 06:27:43 -04:00
public object DebugPacket ( byte [ ] datin , ref string text , bool PrintToConsole )
2011-09-08 22:31:32 -03:00
{
string textoutput ;
try
{
if ( datin . Length > 5 )
{
byte header = datin [ 0 ] ;
byte length = datin [ 1 ] ;
byte seq = datin [ 2 ] ;
byte sysid = datin [ 3 ] ;
byte compid = datin [ 4 ] ;
byte messid = datin [ 5 ] ;
textoutput = string . Format ( "{0:X} {1:X} {2:X} {3:X} {4:X} {5:X} " , header , length , seq , sysid , compid , messid ) ;
object data = Activator . CreateInstance ( mavstructs [ messid ] ) ;
2012-02-20 07:30:47 -04:00
MavlinkUtil . ByteArrayToStructure ( datin , ref data , 6 ) ;
2011-09-08 22:31:32 -03:00
Type test = data . GetType ( ) ;
2012-03-06 06:27:43 -04:00
if ( PrintToConsole )
2011-09-08 22:31:32 -03:00
{
2012-03-06 06:27:43 -04:00
textoutput = textoutput + test . Name + " " ;
2011-09-08 22:31:32 -03:00
2012-03-06 06:27:43 -04:00
foreach ( var field in test . GetFields ( ) )
2011-09-08 22:31:32 -03:00
{
2012-03-06 06:27:43 -04:00
// field.Name has the field's name.
object fieldValue = field . GetValue ( data ) ; // Get value
if ( field . FieldType . IsArray )
{
textoutput = textoutput + field . Name + "=" ;
byte [ ] crap = ( byte [ ] ) fieldValue ;
foreach ( byte fiel in crap )
{
if ( fiel = = 0 )
{
break ;
}
else
{
textoutput = textoutput + ( char ) fiel ;
}
}
textoutput = textoutput + " " ;
}
else
2011-09-08 22:31:32 -03:00
{
2012-03-06 06:27:43 -04:00
textoutput = textoutput + field . Name + "=" + fieldValue . ToString ( ) + " " ;
2011-09-08 22:31:32 -03:00
}
}
2012-03-06 06:27:43 -04:00
textoutput = textoutput + " Len:" + datin . Length + "\r\n" ;
if ( PrintToConsole )
Console . Write ( textoutput ) ;
2011-09-08 22:31:32 -03:00
2012-03-06 06:27:43 -04:00
if ( text ! = null )
text = textoutput ;
}
2011-09-08 22:31:32 -03:00
return data ;
}
}
catch { }
return null ;
}
/// <summary>
/// Sets wp total count
/// </summary>
/// <param name="wp_total"></param>
public void setWPTotal ( ushort wp_total )
{
2011-11-08 09:22:07 -04:00
#if MAVLINK10
MainV2 . givecomport = true ;
__mavlink_mission_count_t req = new __mavlink_mission_count_t ( ) ;
req . target_system = sysid ;
req . target_component = compid ; // MAVLINK_MSG_ID_MISSION_COUNT
req . count = wp_total ;
generatePacket ( MAVLINK_MSG_ID_MISSION_COUNT , req ) ;
DateTime start = DateTime . Now ;
int retrys = 3 ;
while ( true )
{
if ( ! ( start . AddMilliseconds ( 700 ) > DateTime . Now ) )
{
if ( retrys > 0 )
{
2012-02-26 19:13:23 -04:00
log . Info ( "setWPTotal Retry " + retrys ) ;
2011-11-08 09:22:07 -04:00
generatePacket ( MAVLINK_MSG_ID_MISSION_COUNT , req ) ;
start = DateTime . Now ;
retrys - - ;
continue ;
}
MainV2 . givecomport = false ;
throw new Exception ( "Timeout on read - setWPTotal" ) ;
}
byte [ ] buffer = readPacket ( ) ;
if ( buffer . Length > 9 )
{
if ( buffer [ 5 ] = = MAVLINK_MSG_ID_MISSION_REQUEST )
{
2012-02-20 07:30:47 -04:00
var request = buffer . ByteArrayToStructure < __mavlink_mission_request_t > ( 6 ) ;
2011-11-08 09:22:07 -04:00
if ( request . seq = = 0 )
{
if ( param [ "WP_TOTAL" ] ! = null )
param [ "WP_TOTAL" ] = ( float ) wp_total - 1 ;
if ( param [ "CMD_TOTAL" ] ! = null )
param [ "CMD_TOTAL" ] = ( float ) wp_total - 1 ;
MainV2 . givecomport = false ;
return ;
}
}
else
{
//Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]);
}
}
}
#else
2011-09-08 22:31:32 -03:00
MainV2 . givecomport = true ;
__mavlink_waypoint_count_t req = new __mavlink_waypoint_count_t ( ) ;
req . target_system = sysid ;
req . target_component = compid ; // MAVLINK_MSG_ID_WAYPOINT_COUNT
req . count = wp_total ;
generatePacket ( MAVLINK_MSG_ID_WAYPOINT_COUNT , req ) ;
DateTime start = DateTime . Now ;
int retrys = 3 ;
while ( true )
{
if ( ! ( start . AddMilliseconds ( 700 ) > DateTime . Now ) )
{
if ( retrys > 0 )
{
2012-02-26 19:13:23 -04:00
log . Info ( "setWPTotal Retry " + retrys ) ;
2011-09-08 22:31:32 -03:00
generatePacket ( MAVLINK_MSG_ID_WAYPOINT_COUNT , req ) ;
start = DateTime . Now ;
retrys - - ;
continue ;
}
MainV2 . givecomport = false ;
throw new Exception ( "Timeout on read - setWPTotal" ) ;
}
byte [ ] buffer = readPacket ( ) ;
if ( buffer . Length > 9 )
{
2011-11-08 09:22:07 -04:00
if ( buffer [ 5 ] = = MAVLINK_MSG_ID_WAYPOINT_REQUEST )
2011-09-08 22:31:32 -03:00
{
2012-02-20 07:30:47 -04:00
__mavlink_waypoint_request_t request = buffer . ByteArrayToStructure < __mavlink_waypoint_request_t > ( 6 ) ;
2011-11-08 09:22:07 -04:00
if ( request . seq = = 0 )
{
if ( param [ "WP_TOTAL" ] ! = null )
param [ "WP_TOTAL" ] = ( float ) wp_total - 1 ;
if ( param [ "CMD_TOTAL" ] ! = null )
param [ "CMD_TOTAL" ] = ( float ) wp_total - 1 ;
MainV2 . givecomport = false ;
return ;
}
2011-09-08 22:31:32 -03:00
}
else
{
//Console.WriteLine(DateTime.Now + " PC getwp " + buffer[5]);
}
}
}
2011-11-08 09:22:07 -04:00
2011-11-18 10:33:44 -04:00
#endif
2011-09-08 22:31:32 -03:00
}
/// <summary>
/// Save wp to eeprom
/// </summary>
/// <param name="loc">location struct</param>
/// <param name="index">wp no</param>
/// <param name="frame">global or relative</param>
/// <param name="current">0 = no , 2 = guided mode</param>
public void setWP ( Locationwp loc , ushort index , MAV_FRAME frame , byte current )
{
MainV2 . givecomport = true ;
2011-11-18 10:33:44 -04:00
#if MAVLINK10
2011-11-08 09:22:07 -04:00
__mavlink_mission_item_t req = new __mavlink_mission_item_t ( ) ;
2011-11-18 10:33:44 -04:00
#else
__mavlink_waypoint_t req = new __mavlink_waypoint_t ( ) ;
#endif
2011-09-08 22:31:32 -03:00
req . target_system = sysid ;
2011-11-08 09:22:07 -04:00
req . target_component = compid ; // MAVLINK_MSG_ID_MISSION_ITEM
2011-09-08 22:31:32 -03:00
req . command = loc . id ;
req . param1 = loc . p1 ;
req . current = current ;
2011-11-18 10:33:44 -04:00
req . frame = ( byte ) frame ;
req . y = ( float ) ( loc . lng ) ;
req . x = ( float ) ( loc . lat ) ;
req . z = ( float ) ( loc . alt ) ;
2011-09-08 22:31:32 -03:00
2011-11-18 10:33:44 -04:00
req . param1 = loc . p1 ;
req . param2 = loc . p2 ;
req . param3 = loc . p3 ;
req . param4 = loc . p4 ;
/ *
if ( MainV2 . cs . firmware = = MainV2 . Firmwares . ArduPlane )
{
2011-09-08 22:31:32 -03:00
switch ( loc . id )
{ // Switch to map APM command fields inot MAVLink command fields
case ( byte ) MAV_CMD . LOITER_TURNS :
case ( byte ) MAV_CMD . TAKEOFF :
req . param1 = loc . p1 ;
break ;
case ( byte ) MAV_CMD . DO_SET_HOME :
req . param1 = loc . p1 ;
break ;
case ( byte ) MAV_CMD . CONDITION_CHANGE_ALT :
req . param1 = loc . lat ;
req . x = 0 ;
req . y = 0 ;
break ;
case ( byte ) MAV_CMD . LOITER_TIME :
req . param1 = loc . p1 * 10 ; // APM loiter time is in ten second increments
break ;
case ( byte ) MAV_CMD . CONDITION_DELAY :
case ( byte ) MAV_CMD . CONDITION_DISTANCE :
req . param1 = loc . lat ;
break ;
case ( byte ) MAV_CMD . DO_JUMP :
req . param2 = loc . lat ;
req . param1 = loc . p1 ;
break ;
case ( byte ) MAV_CMD . DO_REPEAT_SERVO :
req . param4 = loc . lng ;
goto case ( byte ) MAV_CMD . DO_CHANGE_SPEED ;
case ( byte ) MAV_CMD . DO_REPEAT_RELAY :
case ( byte ) MAV_CMD . DO_CHANGE_SPEED :
req . param3 = loc . lat ;
req . param2 = loc . alt ;
req . param1 = loc . p1 ;
break ;
case ( byte ) MAV_CMD . DO_SET_PARAMETER :
case ( byte ) MAV_CMD . DO_SET_RELAY :
case ( byte ) MAV_CMD . DO_SET_SERVO :
req . param2 = loc . alt ;
req . param1 = loc . p1 ;
break ;
}
}
2011-11-18 10:33:44 -04:00
* /
2011-09-08 22:31:32 -03:00
req . seq = index ;
2012-02-26 19:13:23 -04:00
log . InfoFormat ( "setWP {6} frame {0} cmd {1} p1 {2} x {3} y {4} z {5}" , req . frame , req . command , req . param1 , req . x , req . y , req . z , index ) ;
2011-09-08 22:31:32 -03:00
// request
2011-11-18 10:33:44 -04:00
#if MAVLINK10
2011-11-08 09:22:07 -04:00
generatePacket ( MAVLINK_MSG_ID_MISSION_ITEM , req ) ;
2011-11-18 10:33:44 -04:00
#else
generatePacket ( MAVLINK_MSG_ID_WAYPOINT , req ) ;
#endif
2011-09-08 22:31:32 -03:00
DateTime start = DateTime . Now ;
int retrys = 6 ;
while ( true )
{
if ( ! ( start . AddMilliseconds ( 500 ) > DateTime . Now ) )
{
if ( retrys > 0 )
{
2012-02-26 19:13:23 -04:00
log . Info ( "setWP Retry " + retrys ) ;
2011-11-18 10:33:44 -04:00
#if MAVLINK10
2011-11-08 09:22:07 -04:00
generatePacket ( MAVLINK_MSG_ID_MISSION_ITEM , req ) ;
2011-11-18 10:33:44 -04:00
#else
generatePacket ( MAVLINK_MSG_ID_WAYPOINT , req ) ;
#endif
2011-09-08 22:31:32 -03:00
start = DateTime . Now ;
retrys - - ;
continue ;
}
MainV2 . givecomport = false ;
throw new Exception ( "Timeout on read - setWP" ) ;
}
byte [ ] buffer = readPacket ( ) ;
if ( buffer . Length > 5 )
{
2011-11-18 10:33:44 -04:00
#if MAVLINK10
2011-11-08 09:22:07 -04:00
if ( buffer [ 5 ] = = MAVLINK_MSG_ID_MISSION_ACK )
{
2012-02-20 07:30:47 -04:00
var ans = buffer . ByteArrayToStructure < __mavlink_mission_ack_t > ( 6 ) ;
2011-11-08 09:22:07 -04:00
2012-02-26 19:13:23 -04:00
log . Info ( "set wp " + index + " ACK 47 : " + buffer [ 5 ] + " ans " + Enum . Parse ( typeof ( MAV_MISSION_RESULT ) , ans . type . ToString ( ) ) ) ;
2011-11-08 09:22:07 -04:00
break ;
}
else if ( buffer [ 5 ] = = MAVLINK_MSG_ID_MISSION_REQUEST )
{
2012-02-20 07:30:47 -04:00
var ans = buffer . ByteArrayToStructure < __mavlink_mission_request_t > ( 6 ) ;
2011-11-08 09:22:07 -04:00
if ( ans . seq = = ( index + 1 ) )
{
2012-02-26 19:13:23 -04:00
log . Info ( "set wp doing " + index + " req " + ans . seq + " REQ 40 : " + buffer [ 5 ] ) ;
2011-11-08 09:22:07 -04:00
MainV2 . givecomport = false ;
break ;
}
else
{
2012-02-26 19:13:23 -04:00
log . Info ( "set wp fail doing " + index + " req " + ans . seq + " ACK 47 or REQ 40 : " + buffer [ 5 ] + " seq {0} ts {1} tc {2}" , req . seq , req . target_system , req . target_component ) ;
2011-11-08 09:22:07 -04:00
//break;
}
}
else
{
//Console.WriteLine(DateTime.Now + " PC setwp " + buffer[5]);
}
2011-11-18 10:33:44 -04:00
#else
if ( buffer [ 5 ] = = MAVLINK_MSG_ID_WAYPOINT_ACK )
2011-09-08 22:31:32 -03:00
{ //__mavlink_waypoint_request_t
2012-02-26 19:13:23 -04:00
log . Info ( "set wp " + index + " ACK 47 : " + buffer [ 5 ] ) ;
2011-09-08 22:31:32 -03:00
break ;
}
else if ( buffer [ 5 ] = = MAVLINK_MSG_ID_WAYPOINT_REQUEST )
{
2012-02-20 07:30:47 -04:00
__mavlink_waypoint_request_t ans = buffer . ByteArrayToStructure < __mavlink_waypoint_request_t > ( 6 ) ;
2011-09-08 22:31:32 -03:00
if ( ans . seq = = ( index + 1 ) )
{
2012-02-26 19:13:23 -04:00
log . Info ( "set wp doing " + index + " req " + ans . seq + " REQ 40 : " + buffer [ 5 ] ) ;
2011-09-08 22:31:32 -03:00
MainV2 . givecomport = false ;
break ;
}
else
{
2012-02-26 19:13:23 -04:00
log . InfoFormat ( "set wp fail doing " + index + " req " + ans . seq + " ACK 47 or REQ 40 : " + buffer [ 5 ] + " seq {0} ts {1} tc {2}" , req . seq , req . target_system , req . target_component ) ;
2011-09-08 22:31:32 -03:00
//break;
}
}
else
{
//Console.WriteLine(DateTime.Now + " PC setwp " + buffer[5]);
}
2011-11-18 10:33:44 -04:00
#endif
2011-09-08 22:31:32 -03:00
}
}
}
2011-11-08 09:22:07 -04:00
public void setMountConfigure ( MAV_MOUNT_MODE mountmode , bool stabroll , bool stabpitch , bool stabyaw )
2011-10-16 10:21:36 -03:00
{
__mavlink_mount_configure_t req = new __mavlink_mount_configure_t ( ) ;
req . target_system = sysid ;
req . target_component = compid ;
req . mount_mode = ( byte ) mountmode ;
req . stab_pitch = ( stabpitch = = true ) ? ( byte ) 1 : ( byte ) 0 ;
req . stab_roll = ( stabroll = = true ) ? ( byte ) 1 : ( byte ) 0 ;
req . stab_yaw = ( stabyaw = = true ) ? ( byte ) 1 : ( byte ) 0 ;
generatePacket ( MAVLINK_MSG_ID_MOUNT_CONFIGURE , req ) ;
System . Threading . Thread . Sleep ( 20 ) ;
generatePacket ( MAVLINK_MSG_ID_MOUNT_CONFIGURE , req ) ;
}
2011-11-08 09:22:07 -04:00
public void setMountControl ( double pa , double pb , double pc , bool islatlng )
2011-10-16 10:21:36 -03:00
{
__mavlink_mount_control_t req = new __mavlink_mount_control_t ( ) ;
req . target_system = sysid ;
req . target_component = compid ;
if ( ! islatlng )
{
req . input_a = ( int ) pa ;
req . input_b = ( int ) pb ;
req . input_c = ( int ) pc ;
}
else
{
req . input_a = ( int ) ( pa * 10000000.0 ) ;
req . input_b = ( int ) ( pb * 10000000.0 ) ;
req . input_c = ( int ) ( pc * 100.0 ) ;
}
generatePacket ( MAVLINK_MSG_ID_MOUNT_CONTROL , req ) ;
System . Threading . Thread . Sleep ( 20 ) ;
generatePacket ( MAVLINK_MSG_ID_MOUNT_CONTROL , req ) ;
}
public void setMode ( string modein )
{
2011-11-08 09:22:07 -04:00
#if MAVLINK10
try
{
MAVLink . __mavlink_set_mode_t mode = new MAVLink . __mavlink_set_mode_t ( ) ;
if ( Common . translateMode ( modein , ref mode ) )
{
MainV2 . comPort . generatePacket ( ( byte ) MAVLink . MAVLINK_MSG_ID_SET_MODE , mode ) ;
System . Threading . Thread . Sleep ( 10 ) ;
MainV2 . comPort . generatePacket ( ( byte ) MAVLink . MAVLINK_MSG_ID_SET_MODE , mode ) ;
}
}
catch { System . Windows . Forms . MessageBox . Show ( "Failed to change Modes" ) ; }
#else
2011-10-16 10:21:36 -03:00
try
{
MAVLink . __mavlink_set_nav_mode_t navmode = new MAVLink . __mavlink_set_nav_mode_t ( ) ;
MAVLink . __mavlink_set_mode_t mode = new MAVLink . __mavlink_set_mode_t ( ) ;
if ( Common . translateMode ( modein , ref navmode , ref mode ) )
{
MainV2 . comPort . generatePacket ( ( byte ) MAVLink . MAVLINK_MSG_ID_SET_NAV_MODE , navmode ) ;
System . Threading . Thread . Sleep ( 10 ) ;
MainV2 . comPort . generatePacket ( ( byte ) MAVLink . MAVLINK_MSG_ID_SET_NAV_MODE , navmode ) ;
System . Threading . Thread . Sleep ( 10 ) ;
MainV2 . comPort . generatePacket ( ( byte ) MAVLink . MAVLINK_MSG_ID_SET_MODE , mode ) ;
System . Threading . Thread . Sleep ( 10 ) ;
MainV2 . comPort . generatePacket ( ( byte ) MAVLink . MAVLINK_MSG_ID_SET_MODE , mode ) ;
}
}
catch { System . Windows . Forms . MessageBox . Show ( "Failed to change Modes" ) ; }
2011-11-08 09:22:07 -04:00
#endif
2011-10-16 10:21:36 -03:00
}
2011-09-08 22:31:32 -03:00
/// <summary>
/// used for last bad serial characters
/// </summary>
byte [ ] lastbad = new byte [ 2 ] ;
/// <summary>
/// Serial Reader to read mavlink packets. POLL method
/// </summary>
/// <returns></returns>
public byte [ ] readPacket ( )
{
byte [ ] temp = new byte [ 300 ] ;
int count = 0 ;
int length = 0 ;
int readcount = 0 ;
lastbad = new byte [ 2 ] ;
2012-02-20 19:38:06 -04:00
BaseStream . ReadTimeout = 1200 ; // 1200 ms between chars - the gps detection requires this.
2011-09-08 22:31:32 -03:00
DateTime start = DateTime . Now ;
lock ( readlock )
{
while ( BaseStream . IsOpen | | logreadmode )
{
try
{
if ( readcount > 300 )
{
2012-02-26 19:13:23 -04:00
log . Info ( "MAVLink readpacket No valid mavlink packets" ) ;
2011-09-08 22:31:32 -03:00
break ;
}
readcount + + ;
if ( logreadmode )
{
try
{
if ( logplaybackfile . BaseStream . Position = = 0 )
{
if ( logplaybackfile . PeekChar ( ) = = '-' )
{
oldlogformat = true ;
}
else
{
oldlogformat = false ;
}
}
}
catch { oldlogformat = false ; }
if ( oldlogformat )
{
temp = readlogPacket ( ) ; //old style log
}
else
{
temp = readlogPacketMavlink ( ) ;
}
}
else
{
2011-09-17 12:49:32 -03:00
MainV2 . cs . datetime = DateTime . Now ;
2012-02-20 07:30:47 -04:00
2012-02-26 19:13:23 -04:00
DateTime to = DateTime . Now . AddMilliseconds ( BaseStream . ReadTimeout ) ;
2012-02-20 07:30:47 -04:00
while ( BaseStream . BytesToRead < = 0 )
{
2012-02-26 19:13:23 -04:00
if ( DateTime . Now > to )
2012-02-20 07:30:47 -04:00
{
2012-02-26 19:13:23 -04:00
log . InfoFormat ( "MAVLINK: S wait time out btr {0} len {1}" , BaseStream . BytesToRead , length ) ;
2012-02-20 07:30:47 -04:00
throw new Exception ( "Timeout" ) ;
}
System . Threading . Thread . Sleep ( 1 ) ;
}
2012-02-02 09:18:18 -04:00
if ( BaseStream . IsOpen )
temp [ count ] = ( byte ) BaseStream . ReadByte ( ) ;
2011-09-08 22:31:32 -03:00
}
}
2012-02-26 19:13:23 -04:00
catch ( Exception e ) { log . Info ( "MAVLink readpacket read error: " + e . Message ) ; break ; }
2011-09-08 22:31:32 -03:00
2012-02-02 09:18:18 -04:00
if ( temp [ 0 ] ! = 254 & & temp [ 0 ] ! = 'U' | | lastbad [ 0 ] = = 'I' & & lastbad [ 1 ] = = 'M' | | lastbad [ 1 ] = = 'G' | | lastbad [ 1 ] = = 'A' ) // out of sync "AUTO" "GUIDED" "IMU"
2011-09-08 22:31:32 -03:00
{
2012-01-15 05:00:50 -04:00
if ( temp [ 0 ] > = 0x20 & & temp [ 0 ] < = 127 | | temp [ 0 ] = = '\n' | | temp [ 0 ] = = '\r' )
2011-09-08 22:31:32 -03:00
{
2012-01-15 05:00:50 -04:00
TCPConsole . Write ( temp [ 0 ] ) ;
2011-09-08 22:31:32 -03:00
Console . Write ( ( char ) temp [ 0 ] ) ;
}
count = 0 ;
lastbad [ 0 ] = lastbad [ 1 ] ;
lastbad [ 1 ] = temp [ 0 ] ;
temp [ 1 ] = 0 ;
continue ;
}
// reset count on valid packet
readcount = 0 ;
2011-09-17 10:22:07 -03:00
if ( temp [ 0 ] = = 'U' | | temp [ 0 ] = = 254 )
2011-09-08 22:31:32 -03:00
{
length = temp [ 1 ] + 6 + 2 - 2 ; // data + header + checksum - U - length
if ( count > = 5 | | logreadmode )
{
if ( sysid ! = 0 )
{
if ( sysid ! = temp [ 3 ] | | compid ! = temp [ 4 ] )
{
2012-02-26 19:13:23 -04:00
log . InfoFormat ( "Mavlink Bad Packet (not addressed to this MAV) got {0} {1} vs {2} {3}" , temp [ 3 ] , temp [ 4 ] , sysid , compid ) ;
2011-09-08 22:31:32 -03:00
return new byte [ 0 ] ;
}
}
try
{
if ( logreadmode )
{
}
else
{
2012-02-26 19:13:23 -04:00
DateTime to = DateTime . Now . AddMilliseconds ( BaseStream . ReadTimeout ) ;
2011-09-14 10:31:00 -03:00
while ( BaseStream . BytesToRead < ( length - 4 ) )
2011-09-08 22:31:32 -03:00
{
2012-02-26 19:13:23 -04:00
if ( DateTime . Now > to )
2011-09-08 22:31:32 -03:00
{
2012-02-26 19:13:23 -04:00
log . InfoFormat ( "MAVLINK: L wait time out btr {0} len {1}" , BaseStream . BytesToRead , length ) ;
2011-09-08 22:31:32 -03:00
break ;
}
2011-09-14 10:31:00 -03:00
//Console.WriteLine("data " + 0 + " " + length + " aval " + BaseStream.BytesToRead);
2011-09-08 22:31:32 -03:00
}
2012-02-02 09:18:18 -04:00
if ( BaseStream . IsOpen )
{
int read = BaseStream . Read ( temp , 6 , length - 4 ) ;
}
2011-09-08 22:31:32 -03:00
}
//Console.WriteLine("data " + read + " " + length + " aval " + this.BytesToRead);
count = length + 2 ;
}
catch { break ; }
break ;
}
}
count + + ;
if ( count = = 299 )
break ;
}
} // end readlock
Array . Resize < byte > ( ref temp , count ) ;
2011-09-17 12:49:32 -03:00
if ( packetlosttimer . AddSeconds ( 10 ) < DateTime . Now )
{
packetlosttimer = DateTime . Now ;
packetslost = ( int ) ( packetslost * 0.8f ) ;
packetsnotlost = ( int ) ( packetsnotlost * 0.8f ) ;
}
MainV2 . cs . linkqualitygcs = ( ushort ) ( ( packetsnotlost / ( packetsnotlost + packetslost ) ) * 100 ) ;
2011-09-08 22:31:32 -03:00
if ( bpstime . Second ! = DateTime . Now . Second & & ! logreadmode )
{
2011-12-27 19:05:12 -04:00
// Console.Write("bps {0} loss {1} left {2} mem {3} \n", bps1, synclost, BaseStream.BytesToRead, System.GC.GetTotalMemory(false));
2011-09-08 22:31:32 -03:00
bps2 = bps1 ; // prev sec
bps1 = 0 ; // current sec
bpstime = DateTime . Now ;
}
bps1 + = temp . Length ;
bps = ( bps1 + bps2 ) / 2 ;
if ( temp . Length > = 5 & & temp [ 3 ] = = 255 & & logreadmode ) // gcs packet
{
2011-11-24 20:07:14 -04:00
getWPsfromstream ( ref temp ) ;
2011-09-17 10:22:07 -03:00
return temp ; // new byte[0];
2011-09-08 22:31:32 -03:00
}
2012-02-20 07:30:47 -04:00
ushort crc = MavlinkCRC . crc_calculate ( temp , temp . Length - 2 ) ;
2011-09-08 22:31:32 -03:00
2011-09-17 10:22:07 -03:00
if ( temp . Length > 5 & & temp [ 0 ] = = 254 )
{
2012-02-20 07:30:47 -04:00
crc = MavlinkCRC . crc_accumulate ( MAVLINK_MESSAGE_CRCS [ temp [ 5 ] ] , crc ) ;
2011-09-17 10:22:07 -03:00
}
2011-11-08 09:22:07 -04:00
if ( temp . Length > 5 & & temp [ 1 ] ! = MAVLINK_MESSAGE_LENGTHS [ temp [ 5 ] ] )
{
2012-03-01 09:27:03 -04:00
if ( MAVLINK_MESSAGE_LENGTHS [ temp [ 5 ] ] = = 0 ) // pass for unknown packets
{
}
else
{
log . InfoFormat ( "Mavlink Bad Packet (Len Fail) len {0} pkno {1}" , temp . Length , temp [ 5 ] ) ;
2012-02-02 09:18:18 -04:00
#if MAVLINK10
if ( temp . Length = = 11 & & temp [ 0 ] = = 'U' & & temp [ 5 ] = = 0 )
throw new Exception ( "Mavlink 0.9 Heartbeat, Please upgrade your AP, This planner is for Mavlink 1.0\n\n" ) ;
#endif
2012-03-01 09:27:03 -04:00
return new byte [ 0 ] ;
}
2011-11-08 09:22:07 -04:00
}
2011-09-08 22:31:32 -03:00
if ( temp . Length < 5 | | temp [ temp . Length - 1 ] ! = ( crc > > 8 ) | | temp [ temp . Length - 2 ] ! = ( crc & 0xff ) )
{
int packetno = 0 ;
if ( temp . Length > 5 )
{
packetno = temp [ 5 ] ;
}
2012-02-26 19:13:23 -04:00
log . InfoFormat ( "Mavlink Bad Packet (crc fail) len {0} crc {1} pkno {2}" , temp . Length , crc , packetno ) ;
2011-09-08 22:31:32 -03:00
return new byte [ 0 ] ;
}
try
{
2011-09-17 12:49:32 -03:00
2011-09-17 10:22:07 -03:00
if ( ( temp [ 0 ] = = 'U' | | temp [ 0 ] = = 254 ) & & temp . Length > = temp [ 1 ] )
2011-09-08 22:31:32 -03:00
{
if ( temp [ 2 ] ! = ( ( recvpacketcount + 1 ) % 0x100 ) )
{
2011-09-17 10:22:07 -03:00
synclost + + ; // actualy sync loss's
if ( temp [ 2 ] < ( ( recvpacketcount + 1 ) % 0x100 ) )
{
packetslost + = 0x100 - recvpacketcount + temp [ 2 ] ;
}
else
{
packetslost + = temp [ 2 ] - recvpacketcount ;
}
2012-02-26 19:13:23 -04:00
log . InfoFormat ( "lost {0} pkts {1}" , temp [ 2 ] , ( int ) packetslost ) ;
2011-09-17 10:22:07 -03:00
}
packetsnotlost + + ;
2011-09-08 22:31:32 -03:00
recvpacketcount = temp [ 2 ] ;
//MAVLINK_MSG_ID_GPS_STATUS
//if (temp[5] == MAVLINK_MSG_ID_GPS_STATUS)
// Console.Write(temp[5] + " " + DateTime.Now.Millisecond + " " + packetspersecond[temp[5]] + " " + (DateTime.Now - packetspersecondbuild[temp[5]]).TotalMilliseconds + " \n");
if ( double . IsInfinity ( packetspersecond [ temp [ 5 ] ] ) )
packetspersecond [ temp [ 5 ] ] = 0 ;
packetspersecond [ temp [ 5 ] ] = ( ( ( 1000 / ( ( DateTime . Now - packetspersecondbuild [ temp [ 5 ] ] ) . TotalMilliseconds ) + packetspersecond [ temp [ 5 ] ] ) / 2 ) ) ;
packetspersecondbuild [ temp [ 5 ] ] = DateTime . Now ;
//Console.WriteLine("Packet {0}",temp[5]);
// store packet history
lock ( objlock )
{
packets [ temp [ 5 ] ] = temp ;
}
if ( debugmavlink )
DebugPacket ( temp ) ;
if ( temp [ 5 ] = = MAVLink . MAVLINK_MSG_ID_STATUSTEXT ) // status text
{
2011-11-24 20:07:14 -04:00
string logdata = Encoding . ASCII . GetString ( temp , 7 , temp . Length - 7 ) ;
2011-09-08 22:31:32 -03:00
int ind = logdata . IndexOf ( '\0' ) ;
if ( ind ! = - 1 )
logdata = logdata . Substring ( 0 , ind ) ;
2012-02-26 19:13:23 -04:00
log . Info ( DateTime . Now + " " + logdata ) ;
2011-11-08 09:22:07 -04:00
2011-11-24 20:07:14 -04:00
if ( MainV2 . talk ! = null & & MainV2 . config [ "speechenable" ] ! = null & & MainV2 . config [ "speechenable" ] . ToString ( ) = = "True" )
{
2011-12-19 10:41:23 -04:00
//MainV2.talk.SpeakAsync(logdata);
2011-11-24 20:07:14 -04:00
}
2011-09-08 22:31:32 -03:00
}
2011-11-24 20:07:14 -04:00
getWPsfromstream ( ref temp ) ;
2011-09-08 22:31:32 -03:00
try
{
if ( logfile ! = null )
{
lock ( logwritelock )
{
byte [ ] datearray = BitConverter . GetBytes ( ( UInt64 ) ( ( DateTime . UtcNow - new DateTime ( 1970 , 1 , 1 ) ) . TotalMilliseconds * 1000 ) ) ; //ASCIIEncoding.ASCII.GetBytes(DateTime.Now.ToBinary() + ":");
Array . Reverse ( datearray ) ;
logfile . Write ( datearray , 0 , datearray . Length ) ;
logfile . Write ( temp , 0 , temp . Length ) ;
}
}
}
catch { }
}
}
catch { }
lastvalidpacket = DateTime . Now ;
// Console.Write((DateTime.Now - start).TotalMilliseconds.ToString("00.000") + "\t" + temp.Length + " \r");
return temp ;
}
2011-11-24 20:07:14 -04:00
/// <summary>
/// Used to extract mission from log file
/// </summary>
2012-02-20 07:30:47 -04:00
/// <param name="buffer">packet</param>
void getWPsfromstream ( ref byte [ ] buffer )
2011-11-24 20:07:14 -04:00
{
#if MAVLINK10
2012-02-20 07:30:47 -04:00
if ( buffer [ 5 ] = = MAVLINK_MSG_ID_MISSION_COUNT )
2011-11-24 20:07:14 -04:00
{
// clear old
wps = new PointLatLngAlt [ wps . Length ] ;
}
2012-02-20 07:30:47 -04:00
if ( buffer [ 5 ] = = MAVLink . MAVLINK_MSG_ID_MISSION_ITEM )
2011-11-24 20:07:14 -04:00
{
2012-02-20 07:30:47 -04:00
__mavlink_mission_item_t wp = buffer . ByteArrayToStructure < __mavlink_mission_item_t > ( 6 ) ;
2011-11-24 20:07:14 -04:00
#else
2012-02-20 07:30:47 -04:00
if ( buffer [ 5 ] = = MAVLINK_MSG_ID_WAYPOINT_COUNT )
2011-11-24 20:07:14 -04:00
{
// clear old
wps = new PointLatLngAlt [ wps . Length ] ;
}
2012-02-20 07:30:47 -04:00
if ( buffer [ 5 ] = = MAVLink . MAVLINK_MSG_ID_WAYPOINT )
2011-11-24 20:07:14 -04:00
{
2012-02-20 07:30:47 -04:00
__mavlink_waypoint_t wp = buffer . ByteArrayToStructure < __mavlink_waypoint_t > ( 6 ) ;
2011-11-24 20:07:14 -04:00
#endif
wps [ wp . seq ] = new PointLatLngAlt ( wp . x , wp . y , wp . z , wp . seq . ToString ( ) ) ;
}
}
2011-12-17 18:50:40 -04:00
public PointLatLngAlt getFencePoint ( int no , ref int total )
{
byte [ ] buffer ;
MainV2 . givecomport = true ;
PointLatLngAlt plla = new PointLatLngAlt ( ) ;
__mavlink_fence_fetch_point_t req = new __mavlink_fence_fetch_point_t ( ) ;
req . idx = ( byte ) no ;
req . target_component = compid ;
req . target_system = sysid ;
// request point
generatePacket ( MAVLINK_MSG_ID_FENCE_FETCH_POINT , req ) ;
DateTime start = DateTime . Now ;
int retrys = 3 ;
while ( true )
{
if ( ! ( start . AddMilliseconds ( 500 ) > DateTime . Now ) )
{
if ( retrys > 0 )
{
2012-02-26 19:13:23 -04:00
log . Info ( "getFencePoint Retry " + retrys + " - giv com " + MainV2 . givecomport ) ;
2011-12-17 18:50:40 -04:00
generatePacket ( MAVLINK_MSG_ID_FENCE_FETCH_POINT , req ) ;
start = DateTime . Now ;
retrys - - ;
continue ;
}
MainV2 . givecomport = false ;
throw new Exception ( "Timeout on read - getFencePoint" ) ;
}
buffer = readPacket ( ) ;
if ( buffer . Length > 5 )
{
if ( buffer [ 5 ] = = MAVLINK_MSG_ID_FENCE_POINT )
{
MainV2 . givecomport = false ;
2012-02-20 07:30:47 -04:00
__mavlink_fence_point_t fp = buffer . ByteArrayToStructure < __mavlink_fence_point_t > ( 6 ) ;
2011-12-17 18:50:40 -04:00
plla . Lat = fp . lat ;
plla . Lng = fp . lng ;
plla . Tag = fp . idx . ToString ( ) ;
total = fp . count ;
return plla ;
}
}
}
}
2011-12-27 19:05:12 -04:00
public bool setFencePoint ( byte index , PointLatLngAlt plla , byte fencepointcount )
2011-12-17 18:50:40 -04:00
{
__mavlink_fence_point_t fp = new __mavlink_fence_point_t ( ) ;
fp . idx = index ;
fp . count = fencepointcount ;
fp . lat = ( float ) plla . Lat ;
fp . lng = ( float ) plla . Lng ;
fp . target_component = compid ;
fp . target_system = sysid ;
int retry = 3 ;
while ( retry > 0 )
{
generatePacket ( MAVLINK_MSG_ID_FENCE_POINT , fp ) ;
int counttemp = 0 ;
PointLatLngAlt newfp = getFencePoint ( fp . idx , ref counttemp ) ;
if ( newfp . Lat = = plla . Lat & & newfp . Lng = = fp . lng )
return true ;
retry - - ;
}
return false ;
}
2011-09-08 22:31:32 -03:00
byte [ ] readlogPacket ( )
{
byte [ ] temp = new byte [ 300 ] ;
sysid = 0 ;
int a = 0 ;
while ( a < temp . Length & & logplaybackfile . BaseStream . Position ! = logplaybackfile . BaseStream . Length )
{
temp [ a ] = ( byte ) logplaybackfile . BaseStream . ReadByte ( ) ;
//Console.Write((char)temp[a]);
if ( temp [ a ] = = ':' )
{
break ;
}
a + + ;
if ( temp [ 0 ] ! = '-' )
{
a = 0 ;
}
}
//Console.Write('\n');
//Encoding.ASCII.GetString(temp, 0, a);
string datestring = Encoding . ASCII . GetString ( temp , 0 , a ) ;
//Console.WriteLine(datestring);
long date = Int64 . Parse ( datestring ) ;
DateTime date1 = DateTime . FromBinary ( date ) ;
lastlogread = date1 ;
int length = 5 ;
a = 0 ;
while ( a < length )
{
temp [ a ] = ( byte ) logplaybackfile . BaseStream . ReadByte ( ) ;
if ( a = = 1 )
{
length = temp [ 1 ] + 6 + 2 + 1 ;
}
a + + ;
}
return temp ;
}
byte [ ] readlogPacketMavlink ( )
{
byte [ ] temp = new byte [ 300 ] ;
sysid = 0 ;
//byte[] datearray = BitConverter.GetBytes((ulong)(DateTime.UtcNow - new DateTime(1970, 1, 1)).TotalMilliseconds);
byte [ ] datearray = new byte [ 8 ] ;
logplaybackfile . BaseStream . Read ( datearray , 0 , datearray . Length ) ;
Array . Reverse ( datearray ) ;
DateTime date1 = new DateTime ( 1970 , 1 , 1 , 0 , 0 , 0 , DateTimeKind . Utc ) ;
UInt64 dateint = BitConverter . ToUInt64 ( datearray , 0 ) ;
date1 = date1 . AddMilliseconds ( dateint / 1000 ) ;
2011-12-13 08:52:54 -04:00
lastlogread = date1 . ToLocalTime ( ) ;
2011-09-08 22:31:32 -03:00
2011-09-17 10:22:07 -03:00
MainV2 . cs . datetime = lastlogread ;
2011-09-08 22:31:32 -03:00
int length = 5 ;
int a = 0 ;
while ( a < length )
{
temp [ a ] = ( byte ) logplaybackfile . ReadByte ( ) ;
2011-11-08 09:22:07 -04:00
if ( temp [ 0 ] ! = 'U' & & temp [ 0 ] ! = 254 )
2011-09-08 22:31:32 -03:00
{
2012-02-26 19:13:23 -04:00
log . InfoFormat ( "lost sync byte {0} pos {1}" , temp [ 0 ] , logplaybackfile . BaseStream . Position ) ;
2011-09-08 22:31:32 -03:00
a = 0 ;
continue ;
}
if ( a = = 1 )
{
length = temp [ 1 ] + 6 + 2 ; // 6 header + 2 checksum
}
a + + ;
}
return temp ;
}
}
}