// 【このホバースクリプトの使い方】
//
// 【モデルの形状に関する条件】
//	(1)	ホバー制御に使うJETはコアと平行（水平）に取り付けること。
//	(2)	XZ平面上で最外郭のJETの内側にモデルの重心が来るようにすること。
//	(3)	ヨー制御するために、TRIMなどの先にJETの付いたものを1つ以上装備すること。
//	(4)	空力重心が重心の少し後ろ側に来るようにすること（推奨事項）。
//
// 【Valの書き方】
//	(5)	ホバー制御に使うJETのPOWER変数は「JP1_1、JP2_1、･･･」という形式にすること。
//	(6)	ヨー制御に使うTRIM等の角度制御変数は「ANG1_1」のみとする。
//		必要があれば、ANG1_1から自力で必要な値を計算すること。
//
// 【Bodyの書き方】
//	(7)	TOP（コア）に「C_1」と名前を付けること。
//	(8)	ホバー制御に使うJET全てに名前を付けること。
//
// 【Luaの書き方】
//	(9)	Luaブロックの以下の部分を、Bodyの内容に応じて書き換えること。
//		・jname[C_1] ：(8)で付けた名前を、対応するVALのPOWER変数の順番に書くこと。
//		・yjname[C_1]：(8)で付けた名前のうち、ヨー制御に使用するもののみを書くこと。
//	＜例＞
//		jname[C_1]={JET1_1,JET2_1,JET3_1,JET4_1,JET5_1,JET6_1}
//		yjname[C_1]={JET1_1,JET3_1}
///////////////////////////////////////////////////////////////////////////////
Val{
	jp1_1(default=0,min=-1000000,max=1000000,step=0,disp=1)
	jp2_1(default=0,min=-1000000,max=1000000,step=0,disp=1)
	jp3_1(default=0,min=-1000000,max=1000000,step=0,disp=1)
	ang_1(default=0,min=-45,max=45,step=0,disp=0)
	col_1(default=#0000FF,step=0,disp=0)
	eff_1(default=0,min=-1,max=10,step=10,disp=0)

	jp1_2(default=0,min=-1000000,max=1000000,step=0,disp=1)
	jp2_2(default=0,min=-1000000,max=1000000,step=0,disp=1)
	jp3_2(default=0,min=-1000000,max=1000000,step=0,disp=1)
	ang_2(default=0,min=-45,max=45,step=0,disp=0)
	col_2(default=#FFFF00,step=0,disp=0)

	jp1_3(default=0,min=-1000000,max=1000000,step=0,disp=1)
	jp2_3(default=0,min=-1000000,max=1000000,step=0,disp=1)
	jp3_3(default=0,min=-1000000,max=1000000,step=0,disp=1)
	jp4_3(default=0,min=-1000000,max=1000000,step=0,disp=1)
	jp5_3(default=0,min=-1000000,max=1000000,step=0,disp=1)
	ang_3(default=0,min=-45,max=45,step=0,disp=0)
	col_3(default=#FF0000,step=0,disp=0)

	key0_1(default=0,min=0,max=2,step=2,disp=0)	// 前進
	key1_1(default=0,min=0,max=2,step=2,disp=0)	// 後進
	key2_1(default=0,min=0,max=2,step=2,disp=0)	// 左ヨー（水平旋回）
	key3_1(default=0,min=0,max=2,step=2,disp=0)	// 右ヨー（水平旋回）
	key4_1(default=0,min=0,max=2,step=2,disp=0)	// 左スライド
	key5_1(default=0,min=0,max=2,step=2,disp=0)	// 右スライド
	key6_1(default=0,min=0,max=2,step=2,disp=0)	// 上昇
	key7_1(default=0,min=0,max=2,step=2,disp=0)	// 下降
	key8_1(default=0,min=0,max=2,step=2,disp=0)	// エンジンストップ
}
///////////////////////////////////////////////////////////////////////////////
Key{
	0:key0_1(step=1)
	1:key1_1(step=1)
	2:key2_1(step=1)
	3:key3_1(step=1)
	4:key4_1(step=1)
	6:key5_1(step=1)
	5:key6_1(step=1)
	8:key7_1(step=1)
	9:key8_1(step=1)
}
///////////////////////////////////////////////////////////////////////////////
Body{
	core(name=C_1,color=col_1,effect=eff_1){
		n:trim(angle=ang_1,color=col_1){
			n:jet(name=JET1_1,angle=0,power=jp1_1,color=col_1){}
		}
		w:jet(name=JET2_1,power=jp2_1,color=col_1){}
		e:jet(name=JET3_1,power=jp3_1,color=col_1){}
	}
	///////////////////////////////////////////////////////////////////////////
	core(name=C_2,color=col_2,effect=eff_1){
		s:trimf(angle=-ang_2,option=1,color=col_2){
			n:jet(name=JET1_2,power=jp1_2,color=col_2){}
		}
		s:rudder(angle=-60,color=col_2){
			n:trimf(angle=-ang_2,option=1,color=col_2){
				s:jet(name=JET2_2,power=jp2_2,color=col_2){}
			}
		}
		s:rudder(angle=60,color=col_2){
			n:trimf(angle=-ang_2,option=1,color=col_2){
				s:jet(name=JET3_2,power=jp3_2,color=col_2){}
			}
		}
	}
	///////////////////////////////////////////////////////////////////////////
	core(name=C_3,color=col_3,effect=eff_1){
		n:trimf(angle=ang_3,option=1,color=col_3){
			n:jet(name=JET5_3,angle=180,power=-jp5_3,color=col_3){}
		}
		s:rudder(angle=-108,color=col_3){
			s:trimf(angle=ang_3,option=1,color=col_3){
				s:jet(name=JET2_3,angle=180,power=-jp2_3,color=col_3){}
			}
		}
		s:rudder(angle=108,color=col_3){
			s:trimf(angle=ang_3,option=1,color=col_3){
				s:jet(name=JET3_3,angle=180,power=-jp3_3,color=col_3){}
			}
		}
		s:rudder(angle=-36,color=col_3){
			s:trimf(angle=ang_3,option=1,color=col_3){
				s:jet(name=JET4_3,angle=180,power=-jp4_3,color=col_3){}
			}
		}
		s:rudder(angle=36,color=col_3){
			s:trimf(angle=ang_3,option=1,color=col_3){
				s:jet(name=JET1_3,angle=180,power=-jp1_3,color=col_3){}
			}
		}
	}
}
///////////////////////////////////////////////////////////////////////////////
Lua{
f_max={}	-- 前進最大速度[m/s]
b_max={}	-- 後進最大速度[m/s]
s_max={}	-- スライド最大速度[m/s]
u_max={}	-- 上昇最大速度[m/s]
d_max={}	-- 下降最大速度[m/s]
p_max={}	-- ピッチ最大角度[degree]
r_max={}	-- ロール最大角度[degree]
y_max={}	-- ヨー最大角速度[degree/s]
mass={}					-- 質量
gx0,gy0,gz0={},{},{}	-- 前フレーム重心座標
gh0={}					-- 前フレームコア高さ
gvx,gvy,gvz={},{},{}	-- 重心速度（ワールド座標系）
gax,gay,gaz={},{},{}	-- 重心加速度（ワールド座標系）
gvlx,gvlz={},{}			-- 重心水平速度（コア座標系）
galx,galz={},{}			-- 重心水平加速度（コア座標系）
gxx0,gxy0,gxz0={},{},{}	-- 前フレームX軸ベクトル
gyx0,gyy0,gyz0={},{},{}	-- 前フレームY軸ベクトル
gzx0,gzy0,gzz0={},{},{}	-- 前フレームZ軸ベクトル
gwx,gwy,gwz={},{},{}	-- コア角速度
gbx,gby,gbz={},{},{}	-- コア角加速度
gix,giy,giz={},{},{}	-- 重心周りの慣性モーメント
pidx,pidy,pidz,pidw={},{},{},{}
jname,yjname={},{}
jpkx,jpky,jpkz={},{},{}
jpix,jpiy,jpiz={},{},{}
jnum,yjnum,yjp={},{},{}
engine={}
tgtx,tgty,tgtz={},{},{}
baseh=_GY(0)
auto=0
-------------------------------------------------------------------------------
-- モデルに応じて書き換えること -----------------------------------------------
jname[C_1]={JET1_1,JET2_1,JET3_1}
yjname[C_1]={JET1_1}

jname[C_2]={JET1_2,JET2_2,JET3_2}
yjname[C_2]={JET1_2,JET2_2,JET3_2}

jname[C_3]={JET1_3,JET2_3,JET3_3,JET4_3,JET5_3}
yjname[C_3]={JET1_3,JET2_3,JET3_3,JET4_3,JET5_3}
-------------------------------------------------------------------------------
f_max[C_1]=200/3.6
b_max[C_1]=50/3.6
s_max[C_1]=50/3.6
u_max[C_1]=50/3.6
d_max[C_1]=50/3.6
p_max[C_1]=30
r_max[C_1]=30
y_max[C_1]=90
gx0[C_1],gy0[C_1],gz0[C_1]=_GX(C_1),_GY(C_1),_GZ(C_1)
gh0[C_1]=2
gvx[C_1],gvy[C_1],gvz[C_1]=0,0,0
gxx0[C_1],gxy0[C_1],gxz0[C_1]=_XX(C_1),_XY(C_1),_XZ(C_1)
gyx0[C_1],gyy0[C_1],gyz0[C_1]=_YX(C_1),_YY(C_1),_YZ(C_1)
gzx0[C_1],gzy0[C_1],gzz0[C_1]=_ZX(C_1),_ZY(C_1),_ZZ(C_1)
gwx[C_1],gwy[C_1],gwz[C_1]=0,0,0
pidx[C_1],pidy[C_1],pidz[C_1],pidw[C_1]=0,0,0,0
yjp[C_1]={}
jpkx[C_1],jpky[C_1],jpkz[C_1]={},{},{}
engine[C_1]=0
tgtx[C_1],tgty[C_1],tgtz[C_1]=_GX(C_1),_GY(C_1),_GZ(C_1)

f_max[C_2]=200/3.6
b_max[C_2]=50/3.6
s_max[C_2]=50/3.6
u_max[C_2]=50/3.6
d_max[C_2]=50/3.6
p_max[C_2]=30
r_max[C_2]=30
y_max[C_2]=90
gx0[C_2],gy0[C_2],gz0[C_2]=_GX(C_2),_GY(C_2),_GZ(C_2)
gh0[C_2]=2
gvx[C_2],gvy[C_2],gvz[C_2]=0,0,0
gxx0[C_2],gxy0[C_2],gxz0[C_2]=_XX(C_2),_XY(C_2),_XZ(C_2)
gyx0[C_2],gyy0[C_2],gyz0[C_2]=_YX(C_2),_YY(C_2),_YZ(C_2)
gzx0[C_2],gzy0[C_2],gzz0[C_2]=_ZX(C_2),_ZY(C_2),_ZZ(C_2)
gwx[C_2],gwy[C_2],gwz[C_2]=0,0,0
pidx[C_2],pidy[C_2],pidz[C_2],pidw[C_2]=0,0,0,0
yjp[C_2]={}
jpkx[C_2],jpky[C_2],jpkz[C_2]={},{},{}
engine[C_2]=0
tgtx[C_2],tgty[C_2],tgtz[C_2]=_GX(C_2),_GY(C_2),_GZ(C_2)

f_max[C_3]=200/3.6
b_max[C_3]=50/3.6
s_max[C_3]=50/3.6
u_max[C_3]=50/3.6
d_max[C_3]=50/3.6
p_max[C_3]=30
r_max[C_3]=30
y_max[C_3]=90
gx0[C_3],gy0[C_3],gz0[C_3]=_GX(C_3),_GY(C_3),_GZ(C_3)
gh0[C_3]=2
gvx[C_3],gvy[C_3],gvz[C_3]=0,0,0
gxx0[C_3],gxy0[C_3],gxz0[C_3]=_XX(C_3),_XY(C_3),_XZ(C_3)
gyx0[C_3],gyy0[C_3],gyz0[C_3]=_YX(C_3),_YY(C_3),_YZ(C_3)
gzx0[C_3],gzy0[C_3],gzz0[C_3]=_ZX(C_3),_ZY(C_3),_ZZ(C_3)
gwx[C_3],gwy[C_3],gwz[C_3]=0,0,0
pidx[C_3],pidy[C_3],pidz[C_3],pidw[C_3]=0,0,0,0
yjp[C_3]={}
jpkx[C_3],jpky[C_3],jpkz[C_3]={},{},{}
engine[C_3]=0
tgtx[C_3],tgty[C_3],tgtz[C_3]=_GX(C_3),_GY(C_3),_GZ(C_3)

-- ローカル慣性モーメント(INDEX=_TYPE(cn)の戻り値) ----------------------------
IX,IY,IZ={},{},{}
IX[ 0],IY[ 0],IZ[ 0]=0.77700012922286987,1.51200044155120850,0.77700012922286987	-- CORE
IX[ 1],IY[ 1],IZ[ 1]=0.77700012922286987,1.51200044155120850,0.77700012922286987	-- CHIP
IX[ 2],IY[ 2],IZ[ 2]=0.77700012922286987,1.51200044155120850,0.77700012922286987	-- RUDDER
IX[ 3],IY[ 3],IZ[ 3]=1.51091003417968750,0.47712945938110352,1.51091003417968750	-- RIM
IX[ 4],IY[ 4],IZ[ 4]=0.85765480995178223,1.65404880046844480,0.85765480995178223	-- WHEEL
IX[ 5],IY[ 5],IZ[ 5]=0.85765480995178223,1.65404880046844480,0.85765480995178223	-- RLW
IX[ 6],IY[ 6],IZ[ 6]=0.77700012922286987,1.51200044155120850,0.77700012922286987	-- TRIM
IX[ 7],IY[ 7],IZ[ 7]=0.85765480995178223,1.65404880046844480,0.85765480995178223	-- JET
IX[ 8],IY[ 8],IZ[ 8]=3.10800051689147950,6.04800176620483400,3.10800051689147950	-- WEIGHT
IX[10],IY[10],IZ[10]=1.55400025844573970,3.02400088310241700,1.55400025844573970	-- ARM
IX[33],IY[33],IZ[33]=0.38062506914138794,0.75600022077560425,0.38062506914138794	-- FRAME
IX[34],IY[34],IZ[34]=0.38062506914138794,0.75600022077560425,0.38062506914138794	-- RUDDERF
IX[35],IY[35],IZ[35]=0.38062506914138794,0.75600022077560425,0.38062506914138794	-- TRIMF

-- 2次元回転変換 --------------------------------------------------------------
function rot2D(x,y,r)
	return x*math.cos(r)-y*math.sin(r),x*math.sin(r)+y*math.cos(r)
end

-- ローカルベクトル→ワールドベクトル変換 -------------------------------------
function getwvec(lx,ly,lz,cn)
	local wx=_XX(cn)*lx+_YX(cn)*ly+_ZX(cn)*lz
	local wy=_XY(cn)*lx+_YY(cn)*ly+_ZY(cn)*lz
	local wz=_XZ(cn)*lx+_YZ(cn)*ly+_ZZ(cn)*lz
	return wx,wy,wz
end

-- ワールドベクトル→ローカルベクトル変換 -------------------------------------
function getlvec(wx,wy,wz,cn)
	local lx=_XX(cn)*wx+_XY(cn)*wy+_XZ(cn)*wz
	local ly=_YX(cn)*wx+_YY(cn)*wy+_YZ(cn)*wz
	local lz=_ZX(cn)*wx+_ZY(cn)*wy+_ZZ(cn)*wz
	return lx,ly,lz
end

-- 角度補正 -------------------------------------------------------------------
function angle_correct(a)
	while(a<-180) do a=a+360 end
	while(a>180) do a=a-360 end
	return a
end

-- 重心速度等 -----------------------------------------------------------------
function gvel(cn)
	local x,y,z,h=_GX(cn),_GY(cn),_GZ(cn),_H(cn)
	local vx,vy,vz=(x-gx0[cn])*30,(y-gy0[cn])*30,(z-gz0[cn])*30
	if(h>=0)and(h<10) then vy,gh0[cn]=math.min(vy,(h-gh0[cn])*30),h end
	if(gh0[cn]<1) then vy=math.min(vy,-1) end
	gvx[cn],gax[cn],gx0[cn]=vx,vx-gvx[cn],x
	gvy[cn],gay[cn],gy0[cn]=vy,vy-gvy[cn],y
	gvz[cn],gaz[cn],gz0[cn]=vz,vz-gvz[cn],z
	gvlz[cn],gvlx[cn]=rot2D(gvz[cn],gvx[cn],-_AY(cn))
	local xx,xy,xz=getlvec(gxx0[cn],gxy0[cn],gxz0[cn],cn)
	local yx,yy,yz=getlvec(gyx0[cn],gyy0[cn],gyz0[cn],cn)
	local zx,zy,zz=getlvec(gzx0[cn],gzy0[cn],gzz0[cn],cn)
	local wx,wy,wz=math.atan2(zy,zz)*30,-math.atan2(zx,zz)*30,math.atan2(yx,yy)*30
	gwx[cn],gbx[cn],gxx0[cn],gxy0[cn],gxz0[cn]=wx,(wx-gwx[cn])*30,_XX(cn),_XY(cn),_XZ(cn)
	gwy[cn],gby[cn],gyx0[cn],gyy0[cn],gyz0[cn]=wy,(wy-gwy[cn])*30,_YX(cn),_YY(cn),_YZ(cn)
	gwz[cn],gbz[cn],gzx0[cn],gzy0[cn],gzz0[cn]=wz,(wz-gwz[cn])*30,_ZX(cn),_ZY(cn),_ZZ(cn)
--	out(18,"WX=",_WX(cn),", GWX=",gwx[cn])
--	out(19,"WY=",_WY(cn),", GWY=",gwy[cn])
--	out(20,"WZ=",_WZ(cn),", GWZ=",gwz[cn])
end

-- 質量計算 -------------------------------------------------------------------
function mass_cal(cn)
	local s=cn
	local m=_M(s)
	for i=s+1,_CHIPS()-1 do
		if(_PARENT(i)==-1) then break end
		m=m+_M(i)
	end
	mass[cn]=m
end

-- チップ単体の慣性モーメント計算 ---------------------------------------------
function cmoment_cal(cn,cc)
	local xx,xy,xz=getlvec(_XX(cn),_XY(cn),_XZ(cn),cc)
	local yx,yy,yz=getlvec(_YX(cn),_YY(cn),_YZ(cn),cc)
	local zx,zy,zz=getlvec(_ZX(cn),_ZY(cn),_ZZ(cn),cc)
	local i=_TYPE(cn)
	local ix=IX[i]*xx^2+IY[i]*yx^2+IZ[i]*zx^2
	local iy=IX[i]*xy^2+IY[i]*yy^2+IZ[i]*zy^2
	local iz=IX[i]*xz^2+IY[i]*yz^2+IZ[i]*zz^2
	return ix,iy,iz
end
-- モデル全体の慣性モーメント計算 ---------------------------------------------
function mmoment_cal(cn)
	local s=cn
	local gx,gy,gz=_GX(cn),_GY(cn),_GZ(cn)
	local ix,iy,iz=0,0,0
	local jx,jy,jz
	local x,y,z
	for i=s,_CHIPS()-1 do
		if(i~=s)and(_PARENT(i)==-1) then break end
		x,y,z=_X(i)-gx,_Y(i)-gy,_Z(i)-gz
		x,y,z=getlvec(x,y,z,cn)
		x,y,z=x^2,y^2,z^2
		jx,jy,jz=cmoment_cal(i,cn)
		ix=ix+_M(i)*(y+z)+jx
		iy=iy+_M(i)*(z+x)+jy
		iz=iz+_M(i)*(x+y)+jz
	end
	gix[cn],giy[cn],giz[cn]=ix,iy,iz
end

-- ジェットパワー係数 ---------------------------------------------------------
function jpk_cal(cn)
	local gx,gy,gz=_GX(cn),_GY(cn),_GZ(cn)
	local jx,jy,jz={},{},{}
	local x,y,z
	local n=1
	while(jname[cn][n]~=nil) do
		x=_X(jname[cn][n])-_GX(cn)
		y=_Y(jname[cn][n])-_GY(cn)
		z=_Z(jname[cn][n])-_GZ(cn)
		jx[n],jy[n],jz[n]=getlvec(x,y,z,cn)
		n=n+1
	end
	n=n-1
	jnum[cn]=n
	local ax,ay,az=0,0,0
	local px,py,pz=0,0,0
	local mx,my,mz=0,0,0
	for i=1,n do
		if(jx[i]>0) then
			px=px+jx[i]
		else
			mx=mx+jx[i]
		end
		ax=ax+math.abs(jx[i])
		if(jz[i]>0) then
			pz=pz+jz[i]
		else
			mz=mz+jz[i]
		end
		az=az+math.abs(jz[i])
	end
	jpix[cn],jpiz[cn]=1/az,1/ax
	for i=1,n do
		if(jx[i]>0) then
			jx[i]=2*mx/ax
		else
			jx[i]=2*px/ax
		end
		if(jz[i]>0) then
			jz[i]=2*mz/az
		else
			jz[i]=2*pz/az
		end
	end
	for i=1,n do
		jy[i]=math.abs(jx[i]*jz[i])
	end
	for i=1,n do
		jpkx[cn][i],jpky[cn][i],jpkz[cn][i]=jx[i],jy[i],jz[i]
	end
	local m=1
	while(yjname[cn][m]~=nil) do
		x=_X(yjname[cn][m])-_GX(cn)
		y=_Y(yjname[cn][m])-_GY(cn)
		z=_Z(yjname[cn][m])-_GZ(cn)
		jx[m],jy[m],jz[m]=getlvec(x,y,z,cn)
		m=m+1
	end
	m=m-1
	yjnum[cn]=m
	for i=1,m do
		ay=ay+math.sqrt(jx[i]^2+jz[i]^2)
	end
	jpiy[cn]=1/ay
	for i=1,n do
		yjp[cn][i]=0
		for j=1,m do
			if(jname[cn][i]==yjname[cn][j]) then
				yjp[cn][i]=1
				break
			end
		end
	end
end

-- 高度調整 -------------------------------------------------------------------
function y_ctrl(cn)
	local jp={}
	local t=mass[cn]
	local kp,ki,kd=-8*t,-0.4*t,-0.4*t
	local pidp,pidd
	if(KEY6_1>0) then
		t=math.min(u_max[cn],gvy[cn]+u_max[cn]*0.1)
	elseif(KEY7_1>0) then
		t=math.max(-d_max[cn],gvy[cn]-d_max[cn]*0.1)
	else
		t=0
	end
	t=gvy[cn]-t
	pidp=kp*t
	pidy[cn]=math.min(mass[cn]*1000,math.max(-mass[cn]*1000,pidy[cn]+ki*t))
	pidd=kd*gay[cn]
	local pid=pidp+pidy[cn]+pidd
	if(_YY(cn)<0) then pid,pidy[cn]=0,0 end	-- 破綻防止
	for i=1,jnum[cn] do
		jp[i]=jpky[cn][i]*pid
	end
--	out(11,string.format("YP=%.2f, YI=%.2f, YD=%.2f",pidp,pidy[cn],pidd))
	return jp
end

-- 左右速度調整 ---------------------------------------------------------------
function x_ctrl(cn)
	local jp={}
	local t=giz[cn]*jpiz[cn]
	local kp,ki,kd=100*t,t,50*t
	local pidp,pidd
	if(KEY4_1>0) then
		t=(gvlx[cn]-s_max[cn])*r_max[cn]/s_max[cn]
	elseif(KEY5_1>0) then
		t=(gvlx[cn]+s_max[cn])*r_max[cn]/s_max[cn]
	else
		t=gvlx[cn]*3
	end
	t=math.min(r_max[cn],math.max(-r_max[cn],t))
	t=_EZ(cn)-math.rad(t)
	pidp=kp*t
	pidx[cn]=math.min(10,math.max(-10,pidx[cn]+ki*t))
	pidd=kd*gwz[cn]
	local pid=pidp+pidx[cn]+pidd
	for i=1,jnum[cn] do
		jp[i]=jpkx[cn][i]*pid
	end
--	out(12,string.format("XP=%.2f, XI=%.2f, XD=%.2f",pidp,pidx[cn],pidd))
	return jp
end

-- 前後速度調整 ---------------------------------------------------------------
function z_ctrl(cn)
	local jp={}
	local t=-gix[cn]*jpix[cn]
	local kp,ki,kd=100*t,t,50*t
	local pidp,pidd
	if(KEY0_1>0) then
		t=(-f_max[cn]-gvlz[cn])*p_max[cn]/f_max[cn]
	elseif(KEY1_1>0) then
		t=(b_max[cn]-gvlz[cn])*p_max[cn]/b_max[cn]
	else
		t=-gvlz[cn]*3
	end
	t=math.min(p_max[cn],math.max(-p_max[cn],t))
	t=_AX(cn)-math.rad(t)
	pidp=kp*t
	pidz[cn]=math.min(10,math.max(-10,pidz[cn]+ki*t))
	pidd=kd*gwx[cn]
	local pid=pidp+pidz[cn]+pidd
	for i=1,jnum[cn] do
		jp[i]=jpkz[cn][i]*pid
	end
--	out(13,string.format("ZP=%.2f, ZI=%.2f, ZD=%.2f",pidp,pidz[cn],pidd))
	return jp
end

-- ヨー調整 -------------------------------------------------------------------
function yaw_ctrl(cn,yaw,jp)
	local yy=0
	for i=1,jnum[cn] do
		yy=yy+jp[i]*yjp[cn][i]
	end
	yy=math.min(1,math.max(-1,yy))
	local t=-0.002*giy[cn]*jpiy[cn]*yy
	local kp,ki,kd=t,0.5*t,t
	local pidp,pidd
	if(KEY2_1>0) then
		t=-y_max[cn]
	elseif(KEY3_1>0) then
		t=y_max[cn]
	else
		t=0
	end
	t=math.deg(gwy[cn])-t
	pidp=kp*t
	pidw[cn]=math.min(5,math.max(-5,pidw[cn]+ki*t))
	pidd=kd*gby[cn]
	local pid=pidp+pidw[cn]+pidd
	yaw=yaw+math.min(2,math.max(-2,pid-yaw))
	yaw=math.min(15,math.max(-15,yaw))
--	out(14,string.format("WP=%.2f, WI=%.2f, WD=%.2f",pidp,pidw[cn],pidd))
	return yaw
end

-- 自動操縦 -------------------------------------------------------------------
function automatic(cn)
	if(_H(cn)>=0)and(_H(cn)<10) then tgty[cn]=math.max(tgty[cn],_GY(cn)+1-_H(cn)) end
	local lx,ly,lz=tgtx[cn]-_GX(cn),tgty[cn]-_GY(cn),tgtz[cn]-_GZ(cn)
	local dst=math.sqrt(lx^2+ly^2+lz^2)
	if(cn==C_1) then
		if(dst<10) then
			tgtx[cn]=math.random()*200-100
			tgty[cn]=math.random()*45+5+baseh
			tgtz[cn]=math.random()*220-110
			if(_H(cn)>=0)and(_H(cn)<10) then tgty[cn]=math.max(tgty[cn],_GY(cn)+1-_H(cn)) end
			lx,ly,lz=tgtx[cn]-_GX(cn),tgty[cn]-_GY(cn),tgtz[cn]-_GZ(cn)
		end
	elseif(cn==C_2) then
		tgtx[cn]=_GX(C_1)+_ZX(C_1)*2
		tgty[cn]=_GY(C_1)
		tgtz[cn]=_GZ(C_1)+_ZZ(C_1)*2
		if(_H(cn)>=0)and(_H(cn)<10) then tgty[cn]=math.max(tgty[cn],_GY(cn)+1-_H(cn)) end
		lx,ly,lz=tgtx[cn]-_GX(cn),tgty[cn]-_GY(cn),tgtz[cn]-_GZ(cn)
		dst=math.sqrt(lx^2+ly^2+lz^2)
		if(dst<3) then
			lx,ly,lz=-_ZX(C_1)*0.5,0,-_ZZ(C_1)*0.5
		end
	else
		tgtx[cn]=_GX(C_2)+_ZX(C_2)*2
		tgty[cn]=_GY(C_2)
		tgtz[cn]=_GZ(C_2)+_ZZ(C_2)*2
		if(_H(cn)>=0)and(_H(cn)<10) then tgty[cn]=math.max(tgty[cn],_GY(cn)+1-_H(cn)) end
		lx,ly,lz=tgtx[cn]-_GX(cn),tgty[cn]-_GY(cn),tgtz[cn]-_GZ(cn)
		dst=math.sqrt(lx^2+ly^2+lz^2)
		if(dst<3) then
			lx,ly,lz=-_ZX(C_2)*0.5,0,-_ZZ(C_2)*0.5
		end
	end
	lx,ly,lz=getlvec(lx,ly,lz,cn)
	for i=0,8 do
		_G["KEY"..i.."_1"]=0
	end
	local lr=math.deg(math.atan2(-lx,-lz))
	local h=tgty[cn]-_GY(cn)
	if(lz<-2+_VZ(cn)*2)and(math.abs(lr)<30) then KEY0_1=1 end
	if(lz> 2+_VZ(cn)*2)and(math.abs(lr)<30) then KEY1_1=1 end
	if(lx> 2+_VX(cn)*2) then KEY4_1=1 end
	if(lx<-2+_VX(cn)*2) then KEY5_1=1 end
	if(lr<-10) then KEY2_1=1 end
	if(lr> 10) then KEY3_1=1 end
	if(h>0) then KEY6_1=1 end
	if(h<-2) then KEY7_1=1 end
	engine[cn]=1
--[[
	if(cn==C_1) then
		_SETCOLOR(COL_1)
	elseif(cn==C_2) then
		_SETCOLOR(COL_2)
	else
		_SETCOLOR(COL_3)
	end
	_MOVE3D(_GX(cn),_GY(cn),_GZ(cn))
	_LINE3D(tgtx[cn],tgty[cn],tgtz[cn])
]]
end

-- ホバー制御 -----------------------------------------------------------------
function hover(cn,yaw)
	local jp={}
	if(mass[cn]==nil) then
		mass_cal(cn)
		jpk_cal(cn)
		mmoment_cal(cn)
	end
	gvel(cn)
	if(auto>0)and(cn==C_1)or(cn~=C_1) then automatic(cn) end
	if(KEY6_1>0) then
		engine[cn]=1
	elseif(KEY8_1>0) then
		engine[cn]=0
		pidx[cn],pidy[cn],pidz[cn],pidw[cn]=0,0,0,0
	end
	if(engine[cn]>0) then
		local js={}
		jp=y_ctrl(cn)
		js=x_ctrl(cn)
		for i=1,jnum[cn] do
			jp[i]=jp[i]+js[i]
		end
		js=z_ctrl(cn)
		for i=1,jnum[cn] do
			jp[i]=jp[i]+js[i]
		end
	else
		for i=1,jnum[cn] do
			jp[i]=0
		end
	end
	yaw=yaw_ctrl(cn,yaw,jp)
	return jp,yaw
end

-- メイン ---------------------------------------------------------------------
function main()
	local jp={}
	if(_KEYDOWN(13)>0) then auto=math.mod(auto+1,2) end
	---------------------------------------------------------------------------
	jp,ANG_1=hover(C_1,ANG_1)
	for i=1,jnum[C_1] do
		_G["JP"..i.."_1"]=jp[i]
	end

	jp,ANG_2=hover(C_2,ANG_2)
	for i=1,jnum[C_2] do
		_G["JP"..i.."_2"]=jp[i]
	end

	jp,ANG_3=hover(C_3,ANG_3)
	for i=1,jnum[C_3] do
		_G["JP"..i.."_3"]=jp[i]
	end

	for i=0,8 do
		_G["KEY"..i.."_1"]=0
	end
	---------------------------------------------------------------------------
	local sp=math.sqrt(gvx[C_1]^2+gvy[C_1]^2+gvz[C_1]^2)*3.6
	out(0,"FPS=",_FPS(),",  Chips=",_CHIPS(),",  Weight=",mass[C_1],"[kg]")
	out(1,"Speed=",sp,"[km/h]")
	out(2,"X=",_GX(C_1),",  Y=",_GY(C_1),",  Z=",_GZ(C_1))
	out(3,"Pitch : Up/Down,  Yaw : Left/Right, Roll : Z/C")
	out(4,"Up/Down : X/S,  Stop : D")
	out(5,"Auto : G")

	if(_KEY(15)>0) then
		EFF_1=-1
	elseif(_KEY(16)>0) then
		for i=_CHIPS()-1,1,-1 do
			t=_BYE(i)
		end
	end
end
}