private void _write_new_chunk()
{
int new_len = _data.Length;
if (new_len > 25)
{
new_len = 25;
}
var data = _data.Take(new_len);
_data = _data.Skip(new_len).ToArray();
var pk = new CRTPPacket();
pk.set_header(CRTPPort.MEM, 2);
var tmp = new List <byte>(5);
tmp.Add((byte)mem.id);
tmp.AddRange(BitConverter.GetBytes(_current_addr));
_sent_reply = tmp.ToArray();
tmp.AddRange(data);
pk.data = tmp.ToArray();
_sent_packet = pk;
cf.send_packet(pk, _sent_reply, false, 1000);
_addr_add = data.Count();
}
private void _new_packet_cb(CRTPPacket pk)
{
if (pk.channel == (byte)Param.CHANNEL.READ_CHANNEL || pk.channel == (byte)Param.CHANNEL.WRITE_CHANNEL)
{
int var_id;
if (_useV2)
{
var_id = BitConverter.ToUInt16(pk.data, 0);
if (pk.channel == (byte)Param.CHANNEL.READ_CHANNEL)
{
List <byte> data = new List <byte>();
data.AddRange(pk.data.Take(2));
data.AddRange(pk.data.Skip(3));
pk.data = data.ToArray();
}
}
else
{
var_id = pk.data[0];
}
if (pk.channel == (byte)Param.CHANNEL.TOC_CHANNEL && this._req_param == var_id && pk != null)
{
this.updated_callback(pk);
this._req_param = -1;
}
}
}
public void send_packet(CRTPPacket pk, byte[] expected_reply = null, bool resend = false, int timeout = 200)
{
if (this.link != null)
{
if (expected_reply != null && expected_reply.Length > 0 && !resend && this.link.needs_resending)
{
}
else if (resend)
{
}
this.link.send_packet(pk);
}
}
public void request_param_update(int var_id)
{
CRTPPacket pk = new CRTPPacket();
pk.set_header(CRTPPort.PARAM, (byte)Param.CHANNEL.READ_CHANNEL);
if (this._useV2)
{
pk.data = BitConverter.GetBytes((ushort)var_id);
}
else
{
pk.data = BitConverter.GetBytes((byte)var_id);
}
this.request_queue.Enqueue(pk);
}
private void _request_toc_element(int index)
{
CRTPPacket pk = new CRTPPacket();
pk.set_header(this.port, 0);
if (_useV2)
{
pk.data = new byte[] { (byte)CMD_TOC.CMD_TOC_ITEM_V2, (byte)(index & 0xFF), (byte)((index >> 8) & 0xFF) };
this.cf.send_packet(pk, pk.data);
}
else
{
pk.data = new byte[] { (byte)CMD_TOC.CMD_TOC_ELEMENT, (byte)index };
this.cf.send_packet(pk, pk.data);
}
}
CRTPPacket CommanderPacket(float roll, float pitch, float yaw, ushort thrust)
{
CRTPPacket packet = new CRTPPacket();
packet.port = (byte)CRTPPort.COMMANDER;
List <byte> data = new List <byte>(14)
{
};
data.AddRange(BitConverter.GetBytes(roll));
data.AddRange(BitConverter.GetBytes(-pitch));
data.AddRange(BitConverter.GetBytes(yaw));
data.AddRange(BitConverter.GetBytes(thrust));
packet.data = data.ToArray();
return(packet);
}
CRTPPacket VelocityPacket(float vx, float vy, float vz, float yawrate)
{
CRTPPacket packet = new CRTPPacket();
packet.port = (byte)CRTPPort.COMMANDER_GENERIC;
List <byte> data = new List <byte>(17)
{
0x01
};
data.AddRange(BitConverter.GetBytes(vx));
data.AddRange(BitConverter.GetBytes(vy));
data.AddRange(BitConverter.GetBytes(vz));
data.AddRange(BitConverter.GetBytes(yawrate));
packet.data = data.ToArray();
return(packet);
}
CRTPPacket SetPointPacket(float x, float y, float z, int yaw)
{
CRTPPacket packet = new CRTPPacket();
packet.port = (byte)CRTPPort.COMMANDER_GENERIC;
List <byte> data = new List <byte>(17)
{
7
};
data.AddRange(BitConverter.GetBytes(x));
data.AddRange(BitConverter.GetBytes(y));
data.AddRange(BitConverter.GetBytes(z));
data.AddRange(BitConverter.GetBytes(yaw));
packet.data = data.ToArray();
return(packet);
}
public void run()
{
while (!_should_close)
{
CRTPPacket pk = request_queue.Dequeue();
if (this.cf.link != null)
{
if (this._useV2)
{
this._req_param = BitConverter.ToUInt16(pk.data.Take(2).ToArray(), 0);
this.cf.send_packet(pk, pk.data.Take(2).ToArray());
}
else
{
this._req_param = pk.data[0];
this.cf.send_packet(pk, pk.data.Take(1).ToArray());
}
}
}
}
private void _request_new_chunk()
{
int new_len = _bytes_left;
if (new_len > 20)
{
new_len = 20;
}
var pk = new CRTPPacket();
pk.set_header(CRTPPort.MEM, 1);
var data = new List <byte>(6);
data.Add((byte)mem.id);
data.AddRange(BitConverter.GetBytes(_current_addr));
data.Add((byte)new_len);
pk.data = data.ToArray();
cf.send_packet(pk, data.Take(5).ToArray(), false, 1000);
}
public void start()
{
this._useV2 = this.cf.platform.get_protocol_version() >= 4;
this.cf.add_port_callback(port, this._new_packet_cb);
this.state = TOC_STATE.GET_TOC_INFO;
CRTPPacket pk = new CRTPPacket();
pk.set_header(port, 0); //TOC_CHANNEL
if (this._useV2)
{
pk.data = new byte[] { (byte)CMD_TOC.CMD_TOC_INFO_V2 };
this.cf.send_packet(pk, new byte[] { (byte)CMD_TOC.CMD_TOC_INFO_V2 });
}
else
{
pk.data = new byte[] { (byte)CMD_TOC.CMD_TOC_INFO_V2 };
this.cf.send_packet(pk, new byte[] { (byte)CMD_TOC.CMD_TOC_INFO });
}
}
public void run()
{
while (true)
{
if (this.cf.link == null)
{
Thread.Sleep(1000);
continue;
}
CRTPPacket pk = this.cf.link.receive_packet(1000);
if (pk == null)
{
continue;
}
//this.cf.packet_received.call(pk);
bool found = false;
foreach (var cb in this.cb)
{
if (cb.port == (pk.port & cb.port_mask) && cb.channel == (pk.channel & cb.channel_mask))
{
cb.callback(pk);
if (cb.port != 0xFF)
{
found = true;
}
}
}
if (!found)
{
;
}
}
}
private void _new_packet_cb(CRTPPacket packet)
{
byte chan = packet.channel;
if (chan != 0)
{
return;
}
byte[] payload = packet.data.Skip(1).ToArray();
if (state == TOC_STATE.GET_TOC_INFO)
{
if (_useV2)
{
}
else
{
}
}
else if (state == TOC_STATE.GET_TOC_ELEMENT)
{
}
}
private void _new_packet_cb(CRTPPacket packet)
{
}
private void _param_updated(CRTPPacket pk)
{
}
public void set_value(string complete_name, string value)
{
ParamTocElement element = (ParamTocElement)toc.get_element_by_complete_name(complete_name);
if (element != null && element.access == ParamTocElement.RW_ACCESS)
{
int varid = element.ident;
CRTPPacket pk = new CRTPPacket();
pk.set_header(CRTPPort.PARAM, (byte)CHANNEL.WRITE_CHANNEL);
List <byte> data = new List <byte>()
{
};
if (this._useV2)
{
data.AddRange(BitConverter.GetBytes((ushort)varid));
}
else
{
data.AddRange(BitConverter.GetBytes((byte)varid));
}
switch (element.type)
{
case ParamTocElement.ELEMENT_TYPE.int8_t:
data.AddRange(BitConverter.GetBytes(char.Parse(value)));
break;
case ParamTocElement.ELEMENT_TYPE.int16_t:
data.AddRange(BitConverter.GetBytes(short.Parse(value)));
break;
case ParamTocElement.ELEMENT_TYPE.int32_t:
data.AddRange(BitConverter.GetBytes(int.Parse(value)));
break;
case ParamTocElement.ELEMENT_TYPE.int64_t:
data.AddRange(BitConverter.GetBytes(long.Parse(value)));
break;
case ParamTocElement.ELEMENT_TYPE.float_t:
data.AddRange(BitConverter.GetBytes(float.Parse(value)));
break;
case ParamTocElement.ELEMENT_TYPE.double_t:
data.AddRange(BitConverter.GetBytes(double.Parse(value)));
break;
case ParamTocElement.ELEMENT_TYPE.uint8_t:
data.AddRange(BitConverter.GetBytes(byte.Parse(value)));
break;
case ParamTocElement.ELEMENT_TYPE.uint16_t:
data.AddRange(BitConverter.GetBytes(ushort.Parse(value)));
break;
case ParamTocElement.ELEMENT_TYPE.uint32_t:
data.AddRange(BitConverter.GetBytes(uint.Parse(value)));
break;
case ParamTocElement.ELEMENT_TYPE.uint64_t:
data.AddRange(BitConverter.GetBytes(ulong.Parse(value)));
break;
default:
break;
}
this.param_updater.request_param_setvalue(pk);
}
}
public void request_param_setvalue(CRTPPacket pk)
{
request_queue.Enqueue(pk);
}
