/// <summary> /// Get param list from apm /// </summary> /// <returns></returns> public Hashtable getParamList() { MainV2.givecomport = true; List<int> missed = new List<int>(); // ryan - re start __mavlink_param_request_read_t rereq = new __mavlink_param_request_read_t(); rereq.target_system = sysid; rereq.target_component = compid; __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; DateTime restart = DateTime.Now; int retrys = 3; int nextid = 0; int a = 0; int z = 5; while (a < z) { if (!(start.AddMilliseconds(5000) > DateTime.Now)) { if (retrys > 0) { Console.WriteLine("getParamList Retry " + retrys + " sys " + sysid + " comp " + compid); generatePacket(MAVLINK_MSG_ID_PARAM_REQUEST_LIST, req); start = DateTime.Now; retrys--; continue; } MainV2.givecomport = false; throw new Exception("Timeout on read - getParamList"); } if (!(restart.AddMilliseconds(1000) > DateTime.Now)) { rereq.param_id = new byte[] {0x0,0x0}; rereq.param_index = (short)nextid; sendPacket(rereq); restart = DateTime.Now; } System.Windows.Forms.Application.DoEvents(); byte[] buffer = readPacket(); if (buffer.Length > 5) { //stopwatch.Reset(); //stopwatch.Start(); if (buffer[5] == MAVLINK_MSG_ID_PARAM_VALUE) { restart = DateTime.Now; start = DateTime.Now; __mavlink_param_value_t par = new __mavlink_param_value_t(); object temp = par; ByteArrayToStructure(buffer, ref temp, 6); par = (__mavlink_param_value_t)temp; z = (par.param_count); if (nextid == (par.param_index)) { nextid++; } else { if (retrys > 0) { generatePacket(MAVLINK_MSG_ID_PARAM_REQUEST_LIST, req); a = 0; nextid = 0; retrys--; continue; } missed.Add(nextid); // for later devel MainV2.givecomport = false; throw new Exception("Missed ID expecting " + nextid + " got " + (par.param_index) + "\nPlease try loading again"); } string st = System.Text.ASCIIEncoding.ASCII.GetString(par.param_id); int pos = st.IndexOf('\0'); if (pos != -1) { st = st.Substring(0, pos); } Console.WriteLine(DateTime.Now.Millisecond + " got param " + (par.param_index) + " of " + (z - 1) + " name: " + st); modifyParamForDisplay(true, st, ref par.param_value); param[st] = (par.param_value); a++; } else { //Console.WriteLine(DateTime.Now + " PC paramlist " + buffer[5] + " " + this.BytesToRead); } //stopwatch.Stop(); //Console.WriteLine("Time elapsed: {0}", stopwatch.Elapsed); } } MainV2.givecomport = false; return param; }
/// <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) { if (!param.ContainsKey(paramname)) { Console.WriteLine("Param doesnt exist " + paramname); return false; } 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); modifyParamForDisplay(false, paramname, ref value); #if MAVLINK10 Array.Resize(ref temp, 16); #else Array.Resize(ref temp, 15); #endif req.param_id = temp; req.param_value = (value); generatePacket(MAVLINK_MSG_ID_PARAM_SET, req); Console.WriteLine("setParam '{0}' = '{1}' sysid {2} compid {3}", paramname, req.param_value, sysid, compid); DateTime start = DateTime.Now; int retrys = 3; while (true) { if (!(start.AddMilliseconds(500) > DateTime.Now)) { if (retrys > 0) { Console.WriteLine("setParam Retry " + retrys); 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) { __mavlink_param_value_t par = new __mavlink_param_value_t(); object tempobj = par; ByteArrayToStructure(buffer, ref tempobj, 6); par = (__mavlink_param_value_t)tempobj; 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) { Console.WriteLine("MAVLINK bad param responce - {0} vs {1}", paramname, st); continue; } modifyParamForDisplay(true, st, ref par.param_value); param[st] = (par.param_value); MainV2.givecomport = false; //System.Threading.Thread.Sleep(100);//(int)(8.5 * 5)); // 8.5ms per byte return true; } } } }