private void btn_auto_land_Click(object sender, RoutedEventArgs e)
{
if (_isDroneSelect)
{
foreach (Object o in lv_droneList.SelectedItems)
{
TestViewItem t = (TestViewItem)o;
t.drone.Land();
}
}
else
{
//MessageBox.Show("没有选择飞机", "错误", MessageBoxButton.OK);
writeLog("没有选择飞机，请选择飞机", LOG_MODE.LOG_ERROR);
}
}
//更新状态
private void dTimer_Tick(object sender, EventArgs e)
{
//log测试
if (_drones.Count != 0)
{
foreach (TestViewItem t in _drones)
{
t.Updata();
}
}
if (_isAuto && !_isP2P)
{
if (_isDroneSelect)
{
foreach (Object o in lv_droneList.SelectedItems)
{
TestViewItem t = (TestViewItem)o;
t.drone.Hover();
}
}
}
}
//连接按钮点击响应事件
//判断文本框中的IP地址合法
#region 无人机连接
private void bt_link_Click(object sender, RoutedEventArgs e)
{
string[] format = new string[4];
try
{
string s = ".";
format = tb_ip.Text.Split(s.ToCharArray());
for (int i = 0; i < 4; i++)
{
int temp = Convert.ToInt32(format[i]);
if (temp >= 255 || temp < 0)
{
//MessageBox.Show("IP地址非法", "错误", MessageBoxButton.OK);
writeLog("IP地址非法", LOG_MODE.LOG_ERROR);
return;
}
}
}
catch
{
writeLog("IP地址非法", LOG_MODE.LOG_ERROR);
return;
}
//IP重复读判断
foreach (TestViewItem t in _drones)
{
if (t.IP.Equals(tb_ip.Text))
{
writeLog("IP重复 , 飞机连接失败。", LOG_MODE.LOG_ERROR);
return;
}
}
TestViewItem viewItem = new TestViewItem();
viewItem.ID = Convert.ToInt32(format[3]);
viewItem.Name = format[3] + "号飞机";
viewItem.IP = tb_ip.Text;
viewItem.drone = new MyDroneClient(viewItem.IP);
// 设置PID参数
viewItem.P_pitch = -0.3f;
viewItem.P_roll = -0.3f;
viewItem.P_gaz = 0.4f;
viewItem.D_pitch = -0.9f;
viewItem.D_roll = -0.8f;
//设置目标位置信息
if (_drones.Count == 0)
{
String tmp;
tmp = ConfigurationManager.AppSettings["aimX"];
viewItem._aimX = float.Parse(tmp);
tmp = ConfigurationManager.AppSettings["aimY"];
viewItem._aimY = float.Parse(tmp);
tmp = ConfigurationManager.AppSettings["aimZ"];
viewItem._aimZ = float.Parse(tmp);
}
viewItem.drone.NavigationDataAcquired += data => { viewItem.navigationData = data; };
viewItem.drone.Start();
_drones.Add(viewItem);
}
private void p2pTimer_Tick(object sender, EventArgs e)
{
if (_isP2P && _isDroneSelect)
{
foreach (Object o in lv_droneList.SelectedItems)
{
TestViewItem t = (TestViewItem)o;
//if (!t.IsConected)
//{
// return;
//}
//if (t.navigationData.State.HasFlag(NavigationState.Flying))
{
//高度控制
t._controlZ = t.P_gaz * (t._aimZ - t.currentZ) / 1000.0f;
if (t._controlZ > gate)
{
t._controlZ = gate;
}
if (t._controlZ < -gate)
{
t._controlZ = -gate;
}
//X方向控制
t.Xerror[1] = t.Xerror[0];
t.Xerror[0] = (t._aimX - t.currentX) / 1000.0f;
t.Xerror_dot = (t.Xerror[0] - t.Xerror[1]) / T;
t._controlX = t.P_pitch * t.Xerror[0] + t.I_pitch * t.inte_x + t.D_pitch * t.Xerror_dot;
if (t._controlX > gate)
{
t._controlX = gate;
}
if (t._controlX < -gate)
{
t._controlX = -gate;
}
//Y方向控制
t.Yerror[1] = t.Yerror[0];
t.Yerror[0] = (t._aimY - t.currentY) / 1000;
t.Yerror_dot = (t.Yerror[0] - t.Yerror[1]) / T;
t._controlY = t.P_roll * t.Yerror[0] + t.I_roll * t.inte_y + t.D_roll * t.Yerror_dot;
if (t._controlY > gate)
{
t._controlY = gate;
}
if (t._controlY < -gate)
{
t._controlY = -gate;
}
////限幅
//float len = (float)Math.Sqrt(_controlY * _controlY + _controlX * _controlX);
//if (len > gate)
//{
// _controlX = gate * _controlX / len;
// _controlY = gate * _controlY / len;
//}
////坐标变换：
//_controlX = (float)(c * _controlX - s * _controlY);
//_controlY = (float)(c * _controlY + s * _controlX);
writeLog("controlx:" + t._controlX + " controly:" + t._controlY + " controlz:" + t._controlZ, LOG_MODE.LOG_TEST);
//t.drone.Progress(FlightMode.Progressive, roll: t._controlY, pitch: t._controlX, gaz: t._controlZ);
}
}
}
}
