/* * 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); } } }
/** * 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); }
/** * 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(" "); }