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()