async def run(): """ Does Offboard control using attitude commands. """ drone = System() await drone.connect(system_address="udp://:14540") print("Waiting for drone to connect...") async for state in drone.core.connection_state(): if state.is_connected: print(f"Drone discovered with UUID: {state.uuid}") break print("-- Arming") await drone.action.arm() print("-- Setting initial setpoint") await drone.offboard.set_attitude(Attitude(0.0, 0.0, 0.0, 0.0)) print("-- Starting offboard") try: await drone.offboard.start() except OffboardError as error: print(f"Starting offboard mode failed with error code: \ {error._result.result}") print("-- Disarming") await drone.action.disarm() return print("-- Go up at 70% thrust") await drone.offboard.set_attitude(Attitude(0.0, 0.0, 0.0, 0.9)) await asyncio.sleep(5) print("-- Roll 30 at 60% thrust") await drone.offboard.set_attitude(Attitude(10.0, 0.0, 0.0, 0.6)) await asyncio.sleep(5) print("-- Roll -30 at 60% thrust") await drone.offboard.set_attitude(Attitude(-10.0, 0.0, 0.0, 0.6)) await asyncio.sleep(5) print("-- Hover at 60% thrust") await drone.offboard.set_attitude(Attitude(0.0, 0.0, 0.0, 0.5)) await asyncio.sleep(5) print("-- Stopping offboard") try: await drone.offboard.stop() except OffboardError as error: print(f"Stopping offboard mode failed with error code: \ {error._result.result}") await drone.action.land()
async def arm(self, coordinate: List[float] = None, attitude: List[float] = None): async for arm in self.conn.telemetry.armed(): if arm is False: try: print(f"{self.name}: arming") await self.conn.action.arm() print(f"{self.name}: Setting initial setpoint") if coordinate is not None: await self.conn.offboard.set_position_ned( PositionNedYaw(*coordinate, 0.0)) if attitude is not None: await self.conn.offboard.set_attitude( Attitude(*attitude, 0.0)) except Exception as bla: # add exception later print(bla) break else: break
async def set_att_thrust(self, att, att_type, thrust, freq): """ Offboard method that sends attitude and thrust references to the PX4 autopilot of the the vehicle. Converts the thrust from newtons to a normalized value between 0 and 1 through mathematical expression of the thrust curve of the vehicle. Parameters ---------- att : np.array of floats with shape (3,1) or with shape (4,1) Desired attitude for the vehicle, expressed in euler angles or in a quaternion. att_type : str Must be equal to either 'euler' or 'quaternion'. Specifies the format of the desired attitude. thrust : float Desired thrust value in newtons. freq : float Topic publishing frequency, in Hz. """ msg = Attitude( math.degrees(att[0][0]), math.degrees(att[1][0]), math.degrees(att[2][0]), normalize_thrust(thrust, self.info.thrust_curve, norm(self.ekf.vel))) await self.system.offboard.set_attitude(msg) await asyncio.sleep(1 / freq)
async def set_attitude_zero(): await rover.offboard.set_attitude(Attitude(0.0, 0.0, 0.0, 0.0)) await asyncio.sleep(0.5)
async def move_rover(yaw, thrust): print("Moving...") await rover.offboard.set_attitude(Attitude(0.0, 0.0, yaw, thrust)) await asyncio.sleep(1.0) await rover.offboard.set_attitude(Attitude(0.0, 0.0, yaw, 0.0))
async def __call__(self, drone): await drone.arm(coordinate=[0.0, 0.0, 0.0], attitude=[0.0, 0.0, 0.0]) await drone.start_offboard() print("-- Starting Minimum Snap Test") # initialize the vehicle state loop = 1 init_arr = np.zeros(6) async for pos_vel in drone.conn.telemetry.position_velocity_ned(): init_arr[0] = pos_vel.position.north_m init_arr[1] = -pos_vel.position.east_m init_arr[2] = -pos_vel.position.down_m break async for quat in drone.conn.telemetry.attitude_quaternion(): # loop = 2 init_arr[3] = quat.x init_arr[4] = quat.y init_arr[5] = quat.z break self._state = self.set_initial_state(init_arr) self._desired_state = self.set_desired_state([0.0, 0.0, 5.0]) self._goal = np.array(self._desired_state) # main loop; queries sensors, runs min snap trajectory generation and position controller s = np.zeros(13) start_time = time.time() cnt = 0 while(1): # print("1") async for pos_vel in drone.conn.telemetry.position_velocity_ned(): s[0] = pos_vel.position.north_m s[1] = -pos_vel.position.east_m s[2] = -pos_vel.position.down_m s[3] = pos_vel.velocity.north_m_s s[4] = -pos_vel.velocity.east_m_s s[5] = -pos_vel.velocity.down_m_s # TODO: should this be negative? velocity encodes direction... break asyncio.sleep(0.01) # # # print("loop 2") async for quat in drone.conn.telemetry.attitude_quaternion(): s[6] = quat.w s[7] = quat.x s[8] = quat.y s[9] = quat.z break asyncio.sleep(0.01) # # print("loop 3") # async for rate in drone.conn.telemetry.attitude_angular_velocity_body(): # # loop = 1 # s[10] = rate.roll_rad_s # s[11] = rate.pitch_rad_s # s[12] = rate.yaw_rad_s # # print("loop 3") # break asyncio.sleep(0.01) # print(s) self._state = s assert str(np.shape(self._state)) == "(13,)", "Incorrect state vector size in simulation_step: {}".format(np.shape(self._state)) assert str(np.shape(self._desired_state)) == "(11,)", "Incorrect desired state vector size in simulation_step: {}".format(np.shape(self._desired_state)) assert str(np.shape(self._goal)) == "(11,)", "Incorrect goal vector size in simulation_step: {}".format(np.shape(self._goal)) t = time.time() - start_time new_des_state = self.minimun_snap_trajectory(self._state[0:3], t) # minimum snap trajecotry with default path # new_des_state = self.simple_line_trajectory(self._state[0:3], self._goal[0:3], 10, t) # straight line self._desired_state[0:9] = new_des_state thrust, moment = self.pid_controller() # print("state: ", self._state) # print("desired state: ", self._desired_state) # print("pre-clip thrust: ", thrust) # print("pre-clip moment: ", moment) thrust = thrust / 60.0 # normalize the thrust value; what is the max? tried: 24 moment = moment * (180/np.pi) print("clipped thrust: ", thrust) print("desired att: ", moment) asyncio.sleep(0.01) await drone.conn.offboard.set_attitude(Attitude(moment[0], moment[1], moment[2], thrust)) # await drone.conn.offboard.set_attitude_rate(AttitudeRate(0.0, 0.0, 0.0, 0.5)) asyncio.sleep(0.01)