**-------------------------------------------------------------------------------------------------------
** Reversion :
** Modified By:
** Date :
** Content :
** Notes :
********************************************************************************************************/
"""
# 角度到弧度的转换
D2R = Qfm.D2R
# 仿真参数设置
simPara = Qfm.QuadSimOpt(
# 初值重置方式（随机或者固定）；姿态初值参数（随机就是上限，固定就是设定值）；位置初值参数（同上）
init_mode=Qfm.SimInitType.rand, init_att=np.array([5., 5., 5.]), init_pos=np.array([1., 1., 1.]),
# 仿真运行的最大位置，最大速度，最大姿态角（角度，不是弧度注意），最大角速度（角度每秒）
max_position=8, max_velocity=8, max_attitude=180, max_angular=200,
# 系统噪声，分别在位置环和速度环
sysnoise_bound_pos=0, sysnoise_bound_att=0,
#执行器模式，简单模式没有电机动力学，
actuator_mode=Qfm.ActuatorMode.dynamic
)
# 无人机参数设置，可以为不同的无人机设置不同的参数
uavPara = Qfm.QuadParas(
# 重力加速度；采样时间；机型plus或者x
g=9.8, tim_sample=0.01, structure_type=Qfm.StructureType.quad_plus,
# 无人机臂长（米）；质量（千克）；飞机绕三个轴的转动惯量（千克·平方米）
uav_l=0.45, uav_m=1.5, uav_ixx=1.75e-2, uav_iyy=1.75e-2, uav_izz=3.18e-2,
# 螺旋桨推力系数（牛每平方（弧度每秒）），螺旋桨扭力系数（牛·米每平方（弧度每秒）），旋翼转动惯量，
rotor_ct=1.11e-5, rotor_cm=1.49e-7, rotor_i=9.9e-5,
# 电机转速比例参数（度每秒），电机转速偏置参数（度每秒），电机相应时间（秒）
rotor_cr=646, rotor_wb=166, rotor_t=1.36e-2
)
** Notes :
********************************************************************************************************/
"""
# translate degree to rad
D2R = Qfm.D2R
# set the simulation para
simPara = Qfm.QuadSimOpt(
# initial mode（random or fixed, default is random）；init para for attitude（bound in random mode，
# direct value in fixed mode）；init papa for position (like init_att)
init_mode=Qfm.SimInitType.rand,
init_att=np.array([5., 5., 5.]),
init_pos=np.array([1., 1., 1.]),
# max position(unit is m), max velocity(unit is m/s), max attitude(unit is degree), max angular(unit is deg/s)
max_position=8,
max_velocity=8,
max_attitude=180,
max_angular=200,
# system noise (internal noisy, default is 0)
sysnoise_bound_pos=0,
sysnoise_bound_att=0,
# mode for actuator(motor) (with dynamic or not), enable the sensor system (default is false)
actuator_mode=Qfm.ActuatorMode.dynamic,
enable_sensor_sys=False,
)
# set the para for quadrotor
uavPara = Qfm.QuadParas(
# gravity accelerate；sample period；frame type (plus or x)
g=9.8,
tim_sample=0.01,
structure_type=Qfm.StructureType.quad_plus,
# length of arm(m)；mass(kg)；the moment of inertia around xyz（kg·m^2）
** Module Name: Test_simple
** Module Date: 2019/5/6
** Module Auth: xiaobo
** Version : V0.1
** Description: 'Replace the content between'
**-------------------------------------------------------------------------------------------------------
** Reversion :
** Modified By:
** Date :
** Content :
** Notes :
********************************************************************************************************/
"""
uavPara = Qfm.QuadParas()
# define the simulation parameters
simPara = Qfm.QuadSimOpt()
# define the first uav
quad1 = Qfm.QuadModel(uavPara, simPara)
# simulation begin
print("Simplest simulation begin!")
for i in range(1000):
# set target，i.e., position in x,y,z, and yaw
ref = np.array([0., 0., 1., 0.])
# acquire the state of quadrotor，
# they are 12 degrees' vector，position in xyz，velocity in xyz,
# roll pitch yaw, and angular in roll pitch yaw
stateTemp = quad1.observe()
# calculate the control value; this is a pid controller example
** Version : V0.1
** Description: 'Replace the content between'
**-------------------------------------------------------------------------------------------------------
** Reversion :
** Modified By:
** Date :
** Content :
** Notes :
********************************************************************************************************/
"""
D2R = Qfm.D2R
print("QuadrotorFly Test: ")
# define the quadrotor parameters
uavPara = Qfm.QuadParas()
# define the simulation parameters
simPara = Qfm.QuadSimOpt(init_mode=Qfm.SimInitType.rand,
init_att=np.array([10., 10., 0]),
init_pos=np.array([0, 3, 0]))
# define the data capture
record = MemoryStore.ReplayBuffer(10000, 1)
record.clear()
# define the first uav
quad1 = Qfm.QuadModel(uavPara, simPara)
# define the second uav
quad2 = Qfm.QuadModel(uavPara, simPara)
# gui init
gui = Qfg.QuadrotorFlyGui([quad1, quad2])
# simulation begin
self.state[0:3] = pos_mea[0:3]
self.state[3:6] = np.hstack([vel_mea12, vel_mea3])
return self.state
if __name__ == '__main__':
" used for testing this module"
D2R = Cf.D2R
testFlag = 1
if testFlag == 1:
# from QuadrotorFly import QuadrotorFlyModel as Qfm
q1 = Qfm.QuadModel(
Qfm.QuadParas(),
Qfm.QuadSimOpt(init_mode=Qfm.SimInitType.fixed,
enable_sensor_sys=True,
init_pos=np.array([5, -4, 0]),
init_att=np.array([0, 0, 5])))
# init the estimator
s1 = KalmanFilterSimple()
# set the init state of estimator
s1.reset(q1.state)
# simulation period
t = np.arange(0, 30, 0.01)
ii_len = len(t)
stateRealArr = np.zeros([ii_len, 12])
stateEstArr = np.zeros([ii_len, 12])
meaArr = np.zeros([ii_len, 3])
# set the bias
s1.gyroBias = q1.imu0.gyroBias
"""
class TestPara(Enum):
Test_Module_Dynamic = enum.auto()
Test_Module_Dynamic_Sensor = enum.auto()
Test_Module_Dynamic_CamDown = enum.auto()
D2R = Qfm.D2R
testFlag = TestPara.Test_Module_Dynamic_Sensor
if testFlag == TestPara.Test_Module_Dynamic:
print("QuadrotorFly Dynamic Test: ")
# define the quadrotor parameters
uavPara = Qfm.QuadParas()
# define the simulation parameters
simPara = Qfm.QuadSimOpt(init_mode=Qfm.SimInitType.rand,
init_att=np.array([10., 10., 0]),
init_pos=np.array([0, 3, 0]))
# define the data capture
record = MemoryStore.DataRecord()
record.clear()
# define the first uav
quad1 = Qfm.QuadModel(uavPara, simPara)
# define the second uav
quad2 = Qfm.QuadModel(uavPara, simPara)
# gui init
gui = Qfg.QuadrotorFlyGui([quad1, quad2])
# simulation begin
self.quadGui = QuadrotorFlyGuiUav(self.quads, self.ax)
def render(self):
self.quadGui.render()
plt.pause(0.000000000000001)
if __name__ == '__main__':
import MemoryStore
" used for testing this module"
D2R = Qfm.D2R
testFlag = 1
if testFlag == 1:
# import matplotlib as mpl
print("PID controller test: ")
uavPara = Qfm.QuadParas(structure_type=Qfm.StructureType.quad_plus)
simPara = Qfm.QuadSimOpt(init_mode=Qfm.SimInitType.rand,
init_att=np.array([10., 10., 0]),
init_pos=np.array([0, 3, 0]))
quad1 = Qfm.QuadModel(uavPara, simPara)
record = MemoryStore.DataRecord()
record.clear()
# multi uav test
quad2 = Qfm.QuadModel(uavPara, simPara)
# gui init
gui = QuadrotorFlyGui([quad1, quad2])
# simulation begin
step_cnt = 0
for i in range(1000):