_cnmpc.nmpc_fixedwingdynamics_set_attitude( initial_point[10], initial_point[7], initial_point[8], initial_point[9]) _cnmpc.nmpc_fixedwingdynamics_set_angular_velocity(*initial_point[11:14]) _cnmpc.nmpc_fixedwingdynamics_get_state(state) for i in xrange(500): nmpc.prepare() nmpc.solve( list(state.position) + list(state.velocity) + list(state.attitude) + list(state.angular_velocity)) print "%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f" \ % (i*nmpc.STEP_LENGTH, state.position[0], state.position[1], state.position[2], state.attitude[0], state.attitude[1], state.attitude[2], state.attitude[3]) control_vec = nmpc.get_controls() nmpc.integrate(nmpc.STEP_LENGTH, (ctypes.c_double * 3)(*control_vec)) horizon_point = [a for a in interpolate_reference( (i+nmpc.HORIZON_LENGTH)*nmpc.STEP_LENGTH, xplane_reference_points)] horizon_point.extend([15000, 0, 0]) nmpc.update_horizon(horizon_point[1:])
control_vec = [0, 0, 0] while 1: keys = pygame.event.get() axes = [joystick.get_axis(i) for i in range(joystick.get_numaxes())] axes[0] = -0.5*(axes[0]+0.239) axes[1] = -0.5*(axes[1]+0.239) control_vec = [ (-(axes[3] - 0.35) * 0.8) * 19000, axes[1] - axes[0], axes[1] + axes[0]] # print control_vec nmpc.integrate(TIMESTEP, (ctypes.c_double * 3)(*control_vec)) update = "" update += "set sim/flightmodel/position/local_x %.9f\n" \ % (state.position[1] + position_offset[1]) update += "set sim/flightmodel/position/local_y %.9f\n" \ % -(state.position[2] + position_offset[2]) update += "set sim/flightmodel/position/local_z %.9f\n" \ % -(state.position[0] + position_offset[0]) print repr(state) q = (state.attitude[3], -state.attitude[0], -state.attitude[1], -state.attitude[2]) yaw = math.atan2(2.0 * (q[0] * q[3] + q[1] * q[2]), 1.0 - 2.0 * (q[2] ** 2.0 + q[3] ** 2.0)) pitch = math.asin(2.0 * (q[0] * q[2] - q[3] * q[1])) roll = math.atan2(2.0 * (q[0] * q[1] + q[2] * q[3]), 1.0 - 2.0 * (q[1] ** 2.0 + q[2] ** 2.0))
_cnmpc.nmpc_fixedwingdynamics_set_angular_velocity(0, 0, 0) _cnmpc.nmpc_fixedwingdynamics_get_state(state) control_vec = [0, 0, 0] while 1: keys = pygame.event.get() axes = [joystick.get_axis(i) for i in range(joystick.get_numaxes())] axes[0] = -0.5 * (axes[0] + 0.239) axes[1] = -0.5 * (axes[1] + 0.239) control_vec = [(-(axes[3] - 0.35) * 0.8) * 19000, axes[1] - axes[0], axes[1] + axes[0]] # print control_vec nmpc.integrate(TIMESTEP, (ctypes.c_double * 3)(*control_vec)) update = "" update += "set sim/flightmodel/position/local_x %.9f\n" \ % (state.position[1] + position_offset[1]) update += "set sim/flightmodel/position/local_y %.9f\n" \ % -(state.position[2] + position_offset[2]) update += "set sim/flightmodel/position/local_z %.9f\n" \ % -(state.position[0] + position_offset[0]) print repr(state) q = (state.attitude[3], -state.attitude[0], -state.attitude[1], -state.attitude[2]) yaw = math.atan2(2.0 * (q[0] * q[3] + q[1] * q[2]), 1.0 - 2.0 * (q[2]**2.0 + q[3]**2.0)) pitch = math.asin(2.0 * (q[0] * q[2] - q[3] * q[1]))