/*
* send vel cmd to UAV every 0.5s to keep it moving
*/
private void timer_flush_Tick(object sender, EventArgs e)
{
DFrame msg = new DFrame();
msg.source_id = gcs.id;
msg.route = 1;
msg.payload[0] = 0x02; // this is a cmd msg
foreach (uav u in uav_list)
{
msg.target_id = u.id;
if (u.flush_vel == true)
{
msg.payload[1] = 12; // vel command
float vel = 1;
byte[] he = BitConverter.GetBytes(vel);
msg.payload[2] = he[0];
msg.payload[3] = he[1];
msg.payload[4] = he[2];
msg.payload[5] = he[3];
msg.len = 6;
msg.send(serial_ch1);
}
}
}
/**
* @brief Push new DFrame msgs to all subscribers
* @param msg: DFrame msg to be published
* @retval: none
*/
public void publish(DFrame msg)
{
DFrame tmp;
int i = 0;
for (i = 0; i < queue_cnt; i++)
{
queue_list[i].Enqueue(msg);
lock (this)
{
if (queue_list[i].Count >= limit_list[i])
{
queue_list[i].TryDequeue(out tmp);
}
}
}
}
/**
* background worker decoding datalink frame from serial_ch1 raw inputs
*/
private void backgroundWorker_serial_ch1_DoWork(object sender, DoWorkEventArgs e)
{
const int state_waiting = 0;
const int state_starting = 1;
const int state_addr = 2;
const int state_routing = 3;
const int state_len = 4;
const int state_payload = 5;
const int state_CRC = 6;
byte starting_mark = Convert.ToByte('$');
byte addr = 0;
byte addr_mask_target = 0x0F;
byte addr_mask_source = 0xF0;
byte PayloadLenCnt = 0;
UInt16 PayloadLen = 0;
int i = 0;
byte CRCCnt = 0;
byte[] CRCBuf = new byte[4];
UInt32 CRC_received;
UInt32 crcval;
int state = 0;
byte sys_id = gcs.id;
DFrame msg = new DFrame();
while (true)
{
byte[] tmp_buf;
if (queue_serial_ch1_raw.TryDequeue(out tmp_buf))
{
// trying to decode & extract DFrame
foreach (byte tmp in tmp_buf)
{
if (state == state_waiting)
{
if (tmp == starting_mark)
{
state = state_starting;
}
}
else if (state == state_starting)
{
if (tmp == starting_mark)
{
/* have found a new frame */
state = state_addr;
}
else
{
state = state_waiting;
}
}
else if (state == state_addr)
{
addr = tmp;
addr = (byte)(addr & addr_mask_target);
if (addr == sys_id || addr == 0x00)
{
/* if we are the target, accept this message */
msg.source_id = (byte)((tmp & addr_mask_source) >> 4);
msg.target_id = (byte)(tmp & addr_mask_target);
state = state_routing;
}
else
{
/* if we are not the target, reject this message */
state = state_waiting;
}
}
else if (state == state_routing)
{
msg.route = tmp;
state = state_len;
}
else if (state == state_len)
{
/* collecting 2-byte payload length */
if (PayloadLenCnt == 0)
{
PayloadLen = (UInt16)(tmp << 8);
PayloadLenCnt += 1;
}
else
{
PayloadLen |= tmp;
PayloadLenCnt = 0;
msg.len = PayloadLen;
if (PayloadLen >= 1024 || PayloadLen == 0)
{
// length incorrect
state = state_waiting;
}
else
{
i = 0;
state = state_payload;
}
}
}
else if (state == state_payload)
{
msg.payload[i] = tmp;
i += 1;
if (i == PayloadLen)
{
msg.payload[i] = Convert.ToByte('\0');
i = 0;
CRCCnt = 0;
CRC_received = 0;
state = state_CRC;
}
}
else if (state == state_CRC)
{
/* collecting CRC bytes */
CRCBuf[3 - CRCCnt] = tmp;
CRCCnt += 1;
if (CRCCnt == 4)
{
// calculate CRC locally
crcval = crc32_byte(msg.payload, PayloadLen);
byte[] he = BitConverter.GetBytes(crcval);
if (he.SequenceEqual(CRCBuf))
{
// CRC successful
//df_msg.Enqueue(msg);
// some routine actions according to incoming msg
dframe_decoder(msg);
crcval = 0;
}
else
{
// CRC fail
crcval = 1;
}
state = state_waiting;
}
}
}
}
} // end of while loop
} // end of backgroundWorker_serial_ch1_DoWork
/**
* @bried Retrive the latest DFrame msg published
* @param id: registration is obtained when registering new subscriber
* @param msg: stores retrived msg
* @retval bool: whether the operation is successful or not
*/
public bool subscribe(int id, out DFrame msg)
{
return(queue_list[id].TryDequeue(out msg));
}
/**
* request home location of uav
*/
private string cmd_gethome(string[] cmd_words)
{
string res = "";
// check input
if (cmd_words.Length < 2)
{
res = " usage: gethome uav_name";
return(res);
}
// check uav_name validity
uav utmp = new uav();
bool tfound = false;
foreach (uav u in uav_list)
{
if (u.name == cmd_words[1] && u.status != "self")
{
utmp = u;
tfound = true;
if (utmp.status == "disconnected")
{
res = " " + utmp.name + " must be connected first.\r\n";
return(res);
}
}
}
if (!tfound)
{
res = " uav_name is invalid\r\n";
res += " use showuav to list available uav_names\r\n";
return(res);
}
// construct and send cmd to uav
DFrame msg = new DFrame();
msg.source_id = gcs.id;
msg.target_id = utmp.id;
msg.route = 1;
msg.payload[0] = 0x02; // this is a cmd msg
msg.payload[1] = 8; // cmd_id is 0x08 (gethome)
msg.len = 2;
// wait for uav reply
lock (this)
{
msg.send(serial_ch1);
msg_cmd_ack_received = false;
}
uav_cmd_wait.WaitOne(5000);
// analyse reply
if (msg_cmd_ack_received)
{
if (msg_cmd_ack.payload[2] == 84)
{
res = " action succeeded \r\n";
}
else if (msg_cmd_ack.payload[2] == 70)
{
res = " action failed: ";
res += Convert.ToString(msg_cmd_ack.payload[3]);
res += "\r\n meaning: ";
res += cmd_ack_explain(Convert.ToInt32(Convert.ToString(msg_cmd_ack.payload[3])));
}
else
{
res = " unknown reply";
}
}
else
{
res = " timeout";
}
return(res);
}
/**
* Check uav status a few times per second, using timer tick
*/
private void timer_uav_Tick(object sender, EventArgs e)
{
DFrame msg = new DFrame();
double time_now = epoch_now();
// broadcast a heartbeat
if (time_now - last_heartbeat >= 1)
{
last_heartbeat = time_now;
msg.source_id = gcs.id;
msg.target_id = 0x00;
msg.len = 1;
msg.route = 0;
msg.payload[0] = 0x00;
if (serial_ch1.IsOpen)
{
msg.send(serial_ch1);
}
}
// check uav connection status
foreach (uav u in uav_list)
{
if (u.status != "self")
{
Label label_conn, label_mode, label_arm, label_att, label_vfr, label_fix, label_bat;
if (u.name == "son")
{
label_conn = label_son_conn;
label_arm = label_son_arm;
label_mode = label_son_mode;
label_att = label_son_att;
label_vfr = label_son_vfr;
label_fix = label_son_fix;
label_bat = label_son_bat;
}
else
{
label_conn = label_mom_conn;
label_arm = label_mom_arm;
label_mode = label_mom_mode;
label_att = label_mom_att;
label_vfr = label_mom_vfr;
label_fix = label_mom_fix;
label_bat = label_mom_bat;
}
// detect uav connection
if (time_now - u.last_msg_time >= uav_timeout_sec)
{
if (u.status == "connected")
{
// issue a warning if connection is lost halfway
}
u.status = "disconnected";
label_conn.Text = "Disconnected";
label_conn.BackColor = System.Drawing.Color.Red;
}
else
{
u.status = "connected";
label_conn.Text = "Connected";
label_conn.BackColor = System.Drawing.Color.Lime;
}
// uav arm status
if ((u.base_mode_flag >> 7) != 0)
{
label_arm.Text = "Armed";
label_arm.BackColor = System.Drawing.Color.Lime;
}
else
{
label_arm.Text = "Disrmed";
label_arm.BackColor = System.Drawing.Color.Red;
}
//uav mode update
string mode;
if (u.custom_mode == 0)
{
mode = "Mode: 0 stabilize";
}
else if (u.custom_mode == 2)
{
mode = "Mode: 2 altitude hold";
}
else if (u.custom_mode == 3)
{
mode = "Mode: 3 auto";
}
else if (u.custom_mode == 4)
{
mode = "Mode: 4 guided";
}
else if (u.custom_mode == 5)
{
mode = "Mode: 5 loiter";
}
else if (u.custom_mode == 6)
{
mode = "Mode: 6 RTL";
}
else if (u.custom_mode == 9)
{
mode = "Mode: 9 land";
}
else if (u.custom_mode == 16)
{
mode = "Mode: 16 position hold";
}
else
{
mode = "Mode: unknown";
}
label_mode.Text = mode;
// GPD fix update
string type = "No Info";
if (u.gps_fix_type == 0 | u.gps_fix_type == 1)
{
type = "No Fix";
label_fix.BackColor = System.Drawing.Color.Red;
}
else if (u.gps_fix_type == 2)
{
type = "2D Fixed";
label_fix.BackColor = System.Drawing.Color.Red;
}
else if (u.gps_fix_type == 3)
{
type = "3D Fixed";
label_fix.BackColor = System.Drawing.Color.Lime;
}
else if (u.gps_fix_type == 4)
{
type = "DGPS";
label_fix.BackColor = System.Drawing.Color.Red;
}
label_fix.Text = "GPS Fix: " + type;
// update UAV attutude
label_att.Text = "Roll: " + Convert.ToString(Math.Round(u.roll * 180 / Math.PI, 2)) + "\r\n" +
"Pitch: " + Convert.ToString(Math.Round(u.pitch * 180 / Math.PI, 2)) + "\r\n" +
"Yaw: " + Convert.ToString(Math.Round(u.yaw * 180 / Math.PI, 2));
// update UAV VFR
label_vfr.Text = "heading: " + Convert.ToString(u.heading) + "\r\n" +
"air speed: " + Convert.ToString(Math.Round(u.aspeed, 2)) + "\r\n" +
"gnd speed: " + Convert.ToString(Math.Round(u.gspeed, 2));
label_bat.Text = "battery: " + Convert.ToString(u.bat_remaining) + "%";
}
}// end of foreachss
}
/**
* change uav mode
*/
private string cmd_setmode(string[] cmd_words)
{
string res = "";
// check input
if (cmd_words.Length < 2)
{
res = " setmode uav_name mode: set the mode of uav_name\r\n";
res += " setmode -l: list all available mode";
return(res);
}
// list available modes
if (cmd_words[1] == "-l")
{
res = " list of all modes";
res += " setmode accepts either num or string \r\n";
res += " num |string |explanation \r\n";
res += " -------|----------|------------- \r\n";
res += " 0 |stabilize | \r\n";
res += " 2 |althold |altitude hold \r\n";
res += " 3 |auto | \r\n";
res += " 4 |guided | \r\n";
res += " 5 |loiter | \r\n";
res += " 6 |rtl | \r\n";
res += " 9 |land | \r\n";
res += " 16 |poshold |position hold \r\n";
return(res);
}
// check uav_name validity
uav utmp = new uav();
bool tfound = false;
foreach (uav u in uav_list)
{
if (u.name == cmd_words[1] && u.status != "self")
{
utmp = u;
tfound = true;
if (utmp.status == "disconnected")
{
res = " " + utmp.name + " must be connected first.\r\n";
return(res);
}
}
}
if (!tfound)
{
res = " uav_name is invalid\r\n";
res += " use showuav to list available uav_names\r\n";
return(res);
}
// retrive mode
byte mav_mode = 0;
if (cmd_words[2] == "stabilize" || cmd_words[2] == "0")
{
mav_mode = 0; // stabilize mode APM
}
else if (cmd_words[2] == "althold" || cmd_words[2] == "2")
{
mav_mode = 2; // altitude hold mode APM
}
else if (cmd_words[2] == "auto" || cmd_words[2] == "3")
{
mav_mode = 3; // auto mode APM
}
else if (cmd_words[2] == "guided" || cmd_words[2] == "4")
{
mav_mode = 4; // guided mode APM
}
else if (cmd_words[2] == "loiter" || cmd_words[2] == "5")
{
mav_mode = 5; // loiter mode APM
}
else if (cmd_words[2] == "rtl" || cmd_words[2] == "6")
{
mav_mode = 6; // return to launch (RTL) mode APM
}
else if (cmd_words[2] == "land" || cmd_words[2] == "9")
{
mav_mode = 9; // land mode APM
}
else if (cmd_words[2] == "poshold" || cmd_words[2] == "16")
{
mav_mode = 16; // position hold mode APM
}
else
{
res = " unknown mode \r\n";
return(res);
}
// construct and send cmd to uav
DFrame msg = new DFrame();
msg.source_id = gcs.id;
msg.target_id = utmp.id;
msg.route = 1;
msg.payload[0] = 0x02; // this is a cmd msg
msg.payload[1] = 0x07; // cmd_id is 0x07 (setmode)
msg.payload[2] = mav_mode; // target mode, see MAV_MODE
msg.len = 3;
// wait for uav reply
lock (this)
{
msg.send(serial_ch1);
msg_cmd_ack_received = false;
}
uav_cmd_wait.WaitOne(5000);
// analyse reply
if (msg_cmd_ack_received)
{
if (msg_cmd_ack.payload[2] == 84)
{
res = " action succeeded \r\n";
}
else if (msg_cmd_ack.payload[2] == 70)
{
res = " action failed \r\n";
}
else
{
res = " unknown reply";
}
}
else
{
res = " timeout";
}
return(res);
}
/**
* go to a position relative to local NED
*/
private string cmd_goto(string[] cmd_words)
{
string res = "";
// check input
if (cmd_words.Length < 3)
{
res = " too few arguments";
return(res);
}
// check uav_name validity
uav utmp = new uav();
bool tfound = false;
foreach (uav u in uav_list)
{
if (u.name == cmd_words[1] && u.status != "self")
{
utmp = u;
tfound = true;
if (utmp.status == "disconnected")
{
res = " " + utmp.name + " must be connected first.\r\n";
return(res);
}
}
}
if (!tfound)
{
res = " uav_name is invalid\r\n";
res += " use showuav to list available uav_names\r\n";
return(res);
}
double[] param = new double[6];
double.TryParse(cmd_words[2], out param[0]); // x, y, z
double.TryParse(cmd_words[3], out param[1]);
double.TryParse(cmd_words[4], out param[2]);
double.TryParse(cmd_words[5], out param[3]); // vx, vy, vz
double.TryParse(cmd_words[6], out param[4]);
double.TryParse(cmd_words[7], out param[5]);
// construct and send cmd to uav
DFrame msg = new DFrame();
msg.source_id = gcs.id;
msg.target_id = utmp.id;
msg.route = 1;
msg.payload[0] = 0x02; // this is a cmd msg
msg.payload[1] = 0x02; // cmd_id is 0x02 (goto)
int i = 0;
foreach (double dd in param)
{
byte[] he = BitConverter.GetBytes(dd);
msg.payload[2 + i * 8] = he[0];
msg.payload[3 + i * 8] = he[1];
msg.payload[4 + i * 8] = he[2];
msg.payload[5 + i * 8] = he[3];
msg.payload[6 + i * 8] = he[4];
msg.payload[7 + i * 8] = he[5];
msg.payload[8 + i * 8] = he[6];
msg.payload[9 + i * 8] = he[7];
i += 1;
}
msg.len = 50;
msg.send(serial_ch1);
res = " command transmitted \r\n";
return(res);
}
/**
* uav takeoff
*/
private string cmd_takeoff(string[] cmd_words)
{
string res = "";
// check input
if (cmd_words.Length < 3)
{
res = " Usage:\r\n";
res += " takeoff uav_name alt: command uav_name to take off ac current location\r\n";
res += " and fly to target altitude given by alt (uint: meter)\r\n";
return(res);
}
// check uav_name validity
uav utmp = new uav();
bool tfound = false;
foreach (uav u in uav_list)
{
if (u.name == cmd_words[1] && u.status != "self")
{
utmp = u;
tfound = true;
if (utmp.status == "disconnected")
{
res = " " + utmp.name + " must be connected first.\r\n";
return(res);
}
}
}
if (!tfound)
{
res = " uav_name is invalid\r\n";
res += " use showuav to list available uav_names\r\n";
return(res);
}
float target_alt;
float.TryParse(cmd_words[2], out target_alt);
byte[] he = BitConverter.GetBytes(target_alt);
// construct and send cmd to uav
DFrame msg = new DFrame();
msg.source_id = gcs.id;
msg.target_id = utmp.id;
msg.route = 1;
msg.payload[0] = 0x02; // this is a cmd msg
msg.payload[1] = 0x00; // cmd_id is 0x00 (takeoff)
msg.payload[2] = he[0]; // target alt, 4-byte float;
msg.payload[3] = he[1];
msg.payload[4] = he[2];
msg.payload[5] = he[3];
msg.len = 6;
// wait for uav reply
lock (this)
{
msg.send(serial_ch1);
msg_cmd_ack_received = false;
}
uav_cmd_wait.WaitOne(5000);
lock (msg_cmd_ack)
{
if (msg_cmd_ack_received)
{
if (msg_cmd_ack.payload[2] == 84)
{
res = " action successful, retval: ";
res += Convert.ToString(msg_cmd_ack.payload[3]) + "\r\n";
}
else if (msg_cmd_ack.payload[2] == 70)
{
res = " action failed, retval: ";
res += Convert.ToString(msg_cmd_ack.payload[3]);
res += "\r\n meaning: ";
res += cmd_ack_explain(Convert.ToInt32(Convert.ToString(msg_cmd_ack.payload[3])));
}
else
{
res = " unknown reply";
}
}
else
{
res = " timeout";
}
}
return(res);
}
/**
* Arm or disarm the uav
*/
private string cmd_arm(string[] cmd_words)
{
string res = "";
bool action = false;
// check input
if (cmd_words.Length < 3)
{
res = " Usage:\r\n";
res += " arm uav_name status: check arming status of uav_name\r\n";
res += " arm uav_name arm: arm uav_name\r\n";
res += " arm uav_name disarm: disarm uav_name\r\n";
return(res);
}
// check uav_name validity
uav utmp = new uav();
bool tfound = false;
foreach (uav u in uav_list)
{
if (u.name == cmd_words[1] && u.status != "self")
{
utmp = u;
tfound = true;
}
}
if (!tfound)
{
res = " uav_name is invalid\r\n";
res += " use showuav to list available uav_names\r\n";
return(res);
}
// check operation validity
if (cmd_words[2] == "status")
{
res = " " + cmd_words[1] + " is ";
res += (utmp.armed) ? "armed \r\n" : "not armed\r\n";
}
else if (cmd_words[2] == "arm")
{
if (utmp.status == "disconnected")
{
//res = " " + utmp.name + " must be connected first.\r\n";
//return res;
}
action = true;
}
else if (cmd_words[2] == "disarm")
{
if (utmp.status == "disconnected")
{
//res = " " + utmp.name + " must be connected first.\r\n";
//return res;
}
action = false;
}
else
{
res = " action in invalid\r\n";
res += " Usage:\r\n";
res += " arm uav_name status: check arming status of uav_name\r\n";
res += " arm uav_name arm: arm uav_name\r\n";
res += " arm uav_name disarm: disarm uav_name\r\n";
return(res);
}
// construct and send cmd to uav
DFrame msg = new DFrame();
msg.source_id = gcs.id;
msg.target_id = utmp.id;
msg.route = 1;
msg.payload[0] = 0x02; // this is a cmd msg
msg.payload[1] = 0x06; // cmd_id is 0x06 (arm)
msg.payload[2] = (action) ? Convert.ToByte('T') : Convert.ToByte('F');
msg.len = 3;
// wait for uav reply
lock (msg_cmd_ack)
{
msg_cmd_ack_received = false;
msg.send(serial_ch1);
}
uav_cmd_wait.WaitOne(2000);
lock (msg_cmd_ack)
{
if (msg_cmd_ack_received)
{
if (msg_cmd_ack.payload[2] == 84)
{
res = " action successful, retval: ";
res += Convert.ToString(msg_cmd_ack.payload[3]) + "\r\n";
}
else if (msg_cmd_ack.payload[2] == 70)
{
res = " action failed, retval: ";
res += Convert.ToString(msg_cmd_ack.payload[3]);
res += "\r\n meaning: ";
res += cmd_ack_explain(Convert.ToInt32(Convert.ToString(msg_cmd_ack.payload[3])));
}
else
{
res = " unknown reply";
}
}
else
{
res = " timeout";
}
}
return(res);
}
private void dframe_decoder(DFrame msg)
{
// update msg reception time
foreach (uav u in uav_list)
{
if (u.id == msg.source_id)
{
lock (u)
{
u.last_msg_time = epoch_now();
// attitude
if (msg.payload[0] == 30)
{
u.roll = BitConverter.ToSingle(msg.payload, 1);
u.pitch = BitConverter.ToSingle(msg.payload, 5);
u.yaw = BitConverter.ToSingle(msg.payload, 9);
}
// global position int
if (msg.payload[0] == 33)
{
u.lat = (float)(BitConverter.ToInt32(msg.payload, 1)) / 10000000;
u.lon = (float)(BitConverter.ToInt32(msg.payload, 5)) / 10000000;
u.alt = (float)(BitConverter.ToInt32(msg.payload, 9)) / 1000;
u.vx = (float)(BitConverter.ToInt32(msg.payload, 13)) / 100;
u.vy = (float)(BitConverter.ToInt32(msg.payload, 15)) / 100;
u.vz = (float)(BitConverter.ToInt32(msg.payload, 17)) / 100;
}
// VFR hud
if (msg.payload[0] == 74)
{
u.aspeed = BitConverter.ToSingle(msg.payload, 1);
u.gspeed = BitConverter.ToSingle(msg.payload, 5);
u.heading = BitConverter.ToInt16(msg.payload, 9);
u.throttle = BitConverter.ToUInt16(msg.payload, 11);
}
// battery
if (msg.payload[0] == 75)
{
u.bat_volt = (float)(BitConverter.ToInt16(msg.payload, 1) / 100);
u.bat_amp = (float)(BitConverter.ToInt16(msg.payload, 3) / 100);
u.bat_remaining = (int)BitConverter.ToInt16(msg.payload, 5);
}
// heartbeat
if (msg.payload[0] == 0)
{
u.custom_mode = (int)BitConverter.ToUInt32(msg.payload, 1);
u.base_mode_flag = msg.payload[5];
u.gps_fix_type = msg.payload[7];
}
// home location
if (msg.payload[0] == 242)
{
}
// img
if (msg.payload[0] == 11)
{
img_raw_buff.Enqueue(msg.payload);
}
}
}
}
// look for pending replies
lock (msg_cmd_ack)
{
if (msg.payload[0] == 3 && !msg_cmd_ack_received)
{
msg_cmd_ack_received = true;
msg_cmd_ack = msg;
uav_cmd_wait.Set();
}
}
}
/**
* ask uav to change its yaw
*/
private string cmd_turn(string[] cmd_words)
{
string res = "";
// check input
if (cmd_words.Length < 4)
{
res = " change the UAV's yaw (heading)\r\n";
res += " usage: turn uav_name direction deg \r\n";
res += " uav_name: name of uav to turn\r\n";
res += " direction: abs = absolute \r\n";
res += " cw = relative, clockwise \r\n";
res += " ccw = relative, counter clockwise \r\n";
res += " deg: number of degrees to turn (0~360)\r\n";
return(res);
}
// check uav_name validity
uav utmp = new uav();
bool tfound = false;
foreach (uav u in uav_list)
{
if (u.name == cmd_words[1] && u.status != "self")
{
utmp = u;
tfound = true;
if (utmp.status == "disconnected")
{
res = " " + utmp.name + " must be connected first.\r\n";
return(res);
}
}
}
if (!tfound)
{
res = " uav_name is invalid\r\n";
res += " use showuav to list available uav_names\r\n";
return(res);
}
// construct and send cmd to uav
DFrame msg = new DFrame();
msg.source_id = gcs.id;
msg.target_id = utmp.id;
msg.route = 1;
msg.payload[0] = 0x02; // this is a cmd msg
msg.payload[1] = 10; // cmd_id is 0x08 (gethome)
if (cmd_words[2] == "abs")
{
msg.payload[2] = 0;
}
else if (cmd_words[2] == "cw")
{
msg.payload[2] = 1;
}
else if (cmd_words[2] == "ccw")
{
msg.payload[2] = 2;
}
else
{
res = " unknown turning direction\r\n";
res += " use one of the following: abs, cw, cww\r\n";
return(res);
}
UInt16 deg;
UInt16.TryParse(cmd_words[3], out deg);
if (deg > 360)
{
res = " angle of turn: 0~360 \r\n";
return(res);
}
byte[] he = BitConverter.GetBytes(deg);
msg.payload[3] = he[0];
msg.payload[4] = he[1];
msg.len = 5;
// wait for uav reply
lock (this)
{
msg.send(serial_ch1);
msg_cmd_ack_received = false;
}
uav_cmd_wait.WaitOne(5000);
// analyse reply
if (msg_cmd_ack_received)
{
if (msg_cmd_ack.payload[2] == 84)
{
res = " action succeeded \r\n";
}
else if (msg_cmd_ack.payload[2] == 70)
{
res = " action failed: ";
res += Convert.ToString(msg_cmd_ack.payload[3]);
res += "\r\n meaning: ";
res += cmd_ack_explain(Convert.ToInt32(Convert.ToString(msg_cmd_ack.payload[3])));
}
else
{
res = " unknown reply";
}
}
else
{
res = " timeout";
}
return(res);
}
private string cmd_img_trans_test(string[] cmd_words)
{
double t1 = epoch_now();
AddDataObj_terminal = new AddDataDelegate(AddDataMethod_terminal);
string res = "";
DFrame msg = new DFrame();
// initiate img transmission
msg.source_id = gcs.id;
msg.target_id = 0x03;
msg.route = 0;
msg.payload[0] = 10; // img transfer request
msg.payload[1] = Convert.ToByte('N');
msg.len = 2;
msg.send(serial_ch1);
// wait for first pkg, which tells the size of the img
byte[] tmp;
int img_size = 0;
Int16 pkg_num = 0;
Int16 block_size = 0;
int timeout_cnt = 0;
while (timeout_cnt < 300)
{
if (img_raw_buff.TryDequeue(out tmp))
{
if (tmp[1] != 0xFF | tmp[2] != 0xFF)
{
continue;
}
pkg_num = BitConverter.ToInt16(tmp, 3);
img_size = BitConverter.ToInt32(tmp, 5);
block_size = BitConverter.ToInt16(tmp, 9);
res = " img request accepted \r\n";
res += " number of pkg: " + Convert.ToString(pkg_num) + "\r\n";
res += " size of img: " + Convert.ToString(img_size) + "\r\n";
res += " img block size: " + Convert.ToString(block_size) + "\r\n";
TextBoxTerminal.Invoke(this.AddDataObj_terminal, res);
break;
}
else
{
timeout_cnt += 1;
Thread.Sleep(1);
}
}
if (timeout_cnt >= 300)
{
res = " timeout\r\n";
TextBoxTerminal.Invoke(this.AddDataObj_terminal, res);
return("");
}
// receive the rest of the img
Int16 pkg_cnt = 0;
const int max_try = 10;
int try_cnt = 0;
byte[] img_buf = new byte[img_size];
List <Int16> received_pkg = new List <Int16>();
msg.payload[1] = Convert.ToByte('R');
msg.len = 4;
for (pkg_cnt = 0; pkg_cnt < pkg_num; pkg_cnt++)
{
try_cnt = 0;
while (try_cnt < max_try)
{
msg.payload[2] = (byte)(pkg_cnt & 0x00FF);
msg.payload[3] = (byte)(pkg_cnt >> 8);
res = "requesting pkg " + Convert.ToString(pkg_cnt) + "\r\n";
TextBoxTerminal.Invoke(this.AddDataObj_terminal, res);
msg.send(serial_ch1);
timeout_cnt = 0;
while (timeout_cnt < 400)
{
if (img_raw_buff.TryDequeue(out tmp))
{
if (BitConverter.ToInt16(tmp, 1) != pkg_cnt)
{
continue;
}
Int16 pkg_cnt_get = BitConverter.ToInt16(tmp, 1);
received_pkg.Add(pkg_cnt);
// copy received pkg to local buffer
int pkg_len = (pkg_cnt == pkg_num - 1) ? img_size % block_size : block_size;
Buffer.BlockCopy(tmp, 3,
img_buf, pkg_cnt * block_size, pkg_len);
res = " received pkg: " + Convert.ToString(pkg_cnt_get) + "\r\n";
TextBoxTerminal.Invoke(this.AddDataObj_terminal, res);
break;
}
else
{
timeout_cnt += 1;
Thread.Sleep(1);
}
}
if (timeout_cnt >= 400)
{
try_cnt += 1;
//Thread.Sleep(1);
}
else
{
break;
}
}
}
// check whether the reception is complete
// request re-transmission of missing pkgs
// write picture to file
BinaryWriter img_write = new BinaryWriter(File.Open("try.jpg", FileMode.Create));
img_write.Write(img_buf);
img_write.Close();
res = "time take: " + Convert.ToString(Math.Round(epoch_now() - t1, 2)) + " seconds";
TextBoxTerminal.Invoke(this.AddDataObj_terminal, res);
return(" ");
}
