def test_exercise_1_1(drone_class):
student = drone_class()
solution = UDACITYDroneIn3D()
for drone in [student, solution]:
drone.set_propeller_angular_velocities(-5.0, 1.0, -2.0, 0.3)
for om, om_sol in zip(student.omega, solution.omega):
if not close_enough(om, om_sol):
print("Error in exercise 1.1")
return
print("Tests pass - exercise 1.1")
def test_exercise_1_2(drone_class):
student = drone_class()
solution = UDACITYDroneIn3D()
for drone in [student, solution]:
drone.X[3] = 0.1
drone.X[4] = 0.2
drone.X[5] = 0.3
r_stu = student.R()
r_sol = solution.R()
for r1, r2 in zip(r_stu, r_sol):
for v1, v2 in zip(r1, r2):
if not close_enough(v1, v2):
print("Error in exercise 1.2", 'Sol', r_sol, 'Stu', r_stu)
return
print("Tests pass - exercise 1.2")
def test_exercise_3_1(drone_class):
student = drone_class()
solution = UDACITYDroneIn3D()
for drone in [student, solution]:
drone.X[3] = 0.1
drone.X[4] = 0.2
drone.X[5] = 0.3
drone.X[9] = 0.05
drone.X[10] = 0.12
drone.X[11] = 0.23
drone.set_propeller_angular_velocities(-5.0, 1.0, -2.0, 0.3)
ED_stu = student.get_euler_derivatives()
ED_sol = solution.get_euler_derivatives()
for v1, v2 in zip(ED_stu, ED_sol):
if not close_enough(v1, v2):
print("Error in exercise 3.1")
return
print("Tests pass - exercise 3.1")
def test_exercise_1_3(drone_class):
student = drone_class()
solution = UDACITYDroneIn3D()
for drone in [student, solution]:
drone.X[3] = 0.1
drone.X[4] = 0.2
drone.X[5] = 0.3
drone.X[9] = 0.05
drone.X[10] = 0.12
drone.X[11] = 0.23
drone.set_propeller_angular_velocities(-5.0, 1.0, -2.0, 0.3)
acc_stu = student.linear_acceleration()
acc_sol = solution.linear_acceleration()
for v1, v2 in zip(acc_stu, acc_sol):
if not close_enough(v1, v2):
print("Error in exercise 1.3")
return
print("Tests pass - exercise 1.3")
def test_exercise_3_2(drone_class):
student = drone_class()
solution = UDACITYDroneIn3D()
for drone in [student, solution]:
drone.X[3] = 0.1
drone.X[4] = 0.2
drone.X[5] = 0.3
drone.X[6] = -2.3
drone.X[7] = 1.2
drone.X[8] = 0.5
drone.X[9] = 0.05
drone.X[10] = 0.12
drone.X[11] = 0.23
drone.set_propeller_angular_velocities(-5.0, 1.0, -2.0, 0.3)
drone.advance_state(0.1)
for v1, v2 in zip(student.X, solution.X):
if not close_enough(v1, v2):
print("Error in exercise 3.2")
return
print("Tests pass - exercise 3.2")
plt.title('Flight path').set_fontsize(20)
ax.set_xlabel('$x$ [$m$]').set_fontsize(20)
ax.set_ylabel('$y$ [$m$]').set_fontsize(20)
ax.set_zlabel('$z$ [$m$]').set_fontsize(20)
plt.legend([
'Planned yaw',
], fontsize=14)
plt.show()
# how fast the inner loop (Attitude controller) performs calculations
# relative to the outer loops (altitude and position controllers).
inner_loop_relative_to_outer_loop = 10
# creating the drone object
drone = UDACITYDroneIn3D()
# creating the control system object
control_system = UDACITYController(z_k_p=2.0,
z_k_d=1.0,
x_k_p=6.0,
x_k_d=4.0,
y_k_p=6.0,
y_k_d=4.0,
k_p_roll=8.0,
k_p_pitch=8.0,
k_p_yaw=8.0,
k_p_p=20.0,
k_p_q=20.0,
k_p_r=20.0)
def R():
drone = UDACITYDroneIn3D()
drone.X[3] = 0.1
drone.X[4] = 0.2
drone.X[5] = 0.3
return drone.R()