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)