Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
def test_guided_local_velocity_target(self, vx, vy, vz_up, timeout=3):
" Check local target velocity being recieved by vehicle "
self.progress("Setting local NED velocity target: (%f, %f, %f)" %(vx, vy, -vz_up))
self.progress("Setting POSITION_TARGET_LOCAL_NED message rate to 10Hz")
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, 10)
# Drain old messages and ignore the ramp-up to the required target velocity
tstart = self.get_sim_time()
while self.get_sim_time_cached() - tstart < timeout:
# send velocity-control command
self.mav.mav.set_position_target_local_ned_send(
0, # timestamp
1, # target system_id
1, # target component id
mavutil.mavlink.MAV_FRAME_LOCAL_NED,
0b1111111111000111, # mask specifying use only vx,vy,vz
0, # x
0, # y
0, # z
vx, # vx
vy, # vy
-vz_up, # vz
0, # afx
0, # afy
0, # afz
0, # yaw
0, # yawrate
)
m = self.mav.recv_match(type='POSITION_TARGET_LOCAL_NED', blocking=True, timeout=1)
if m is None:
def takeoff(self, n, e, d):
# for mavlink to PX4 need to specify the NED location for landing
# since connection doesn't keep track of this info, have drone send it
# abstract away that part in the drone class
time_boot_ms = 0 # this does not need to be set to a specific time
mask = MASK_IS_TAKEOFF
mask |= (MASK_IGNORE_YAW_RATE | MASK_IGNORE_YAW | MASK_IGNORE_ACCELERATION | MASK_IGNORE_VELOCITY)
self._master.mav.set_position_target_local_ned_send(
time_boot_ms, self._target_system, self._target_component, mavutil.mavlink.MAV_FRAME_LOCAL_NED, mask, n, e,
d, 0, 0, 0, 0, 0, 0, 0, 0
)
def local_acceleration_target(self, an, ae, ad, t=0):
time_boot_ms = t
mask = 0b1111111000111111
msg = self._master.mav.position_target_local_ned_encode(time_boot_ms, mavutil.mavlink.MAV_FRAME_LOCAL_NED, mask,
0, 0, 0, 0, 0, 0, an, ae, ad, 0, 0)
self.send_message(msg)
def goto_position_target_local_ned(north, east, down):
"""
Send SET_POSITION_TARGET_LOCAL_NED command to request the vehicle fly to a specified
location in the North, East, Down frame.
"""
msg = vehicle.message_factory.set_position_target_local_ned_encode(
0, # time_boot_ms (not used)
0, 0, # target system, target component
mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame
0b0000111111111000, # type_mask (only positions enabled)
north, east, down,
0, 0, 0, # x, y, z velocity in m/s (not used)
0, 0, 0, # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink)
0, 0) # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink)
# send command to vehicle
vehicle.send_mavlink(msg)
def send_nav_velocity(self, velocity_x, velocity_y, velocity_z):
# create the SET_POSITION_TARGET_LOCAL_NED command
msg = self.vehicle.message_factory.set_position_target_local_ned_encode(
0, # time_boot_ms (not used)
0, 0, # target system, target component
mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame
0b0000111111000111, # type_mask (only speeds enabled)
0, 0, 0, # x, y, z positions (not used)
velocity_x, velocity_y, velocity_z, # x, y, z velocity in m/s
0, 0, 0, # x, y, z acceleration (not used)
0, 0) # yaw, yaw_rate (not used)
# send command to vehicle
self.vehicle.send_mavlink(msg)
self.vehicle.flush()
def cmd_position(self, n, e, d, heading):
time_boot_ms = 0 # this does not need to be set to a specific time
# mask = PositionMask.MASK_IS_LOITER.value
mask = (PositionMask.MASK_IGNORE_YAW_RATE.value | PositionMask.MASK_IGNORE_ACCELERATION.value |
PositionMask.MASK_IGNORE_VELOCITY.value)
msg = self._mav.set_position_target_local_ned_encode(time_boot_ms, self._target_system, self._target_component,
mavutil.mavlink.MAV_FRAME_LOCAL_NED, mask, n, e, d, 0, 0,
0, 0, 0, 0, heading, 0)
asyncio.ensure_future(self.send_message(msg))
print('SET_POISITION_TARGET_GLOBAL_INT need 11 params')
# Set mask
mask = 0b0000000111111111
for i, value in enumerate(param):
if value is not None:
mask -= 1<
def send_nav_velocity(self, velocity_x, velocity_y, velocity_z, relative = True):
if self.fsController.triggered:
return False
if self.STATE != VehicleState.auto:
print "Err: Velocity control denied with vehicle state %s." % self.STATE
return False
# Decide which frame type to be used
if relative:
frame = mavutil.mavlink.MAV_FRAME_BODY_OFFSET_NED
else:
frame = mavutil.mavlink.MAV_FRAME_LOCAL_NED
# create the SET_POSITION_TARGET_LOCAL_NED command
msg = self.vehicle.message_factory.set_position_target_local_ned_encode(
0, # time_boot_ms (not used)
0, 0, # target system, target component
frame, # frame
0b0000111111000111, # type_mask (only speeds enabled)
0, 0, 0, # x, y, z positions (not used)
velocity_x, velocity_y, velocity_z, # x, y, z velocity in m/s
0, 0, 0, # x, y, z acceleration (not used)
0, 0) # yaw, yaw_rate (not used)
# send command to vehicle
self.vehicle.send_mavlink(msg)
self.vehicle.flush()
return True
This uses the SET_POSITION_TARGET_LOCAL_NED command with a type mask enabling only
velocity components
(http://dev.ardupilot.com/wiki/copter-commands-in-guided-mode/#set_position_target_local_ned).
Note that from AC3.3 the message should be re-sent every second (after about 3 seconds
with no message the velocity will drop back to zero). In AC3.2.1 and earlier the specified
velocity persists until it is canceled. The code below should work on either version
(sending the message multiple times does not cause problems).
See the above link for information on the type_mask (0=enable, 1=ignore).
At time of writing, acceleration and yaw bits are ignored.
"""
msg = vehicle.message_factory.set_position_target_local_ned_encode(
0, # time_boot_ms (not used)
0, 0, # target system, target component
mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame
0b0000111111000111, # type_mask (only speeds enabled)
0, 0, 0, # x, y, z positions (not used)
velocity_x, velocity_y, velocity_z, # x, y, z velocity in m/s
0, 0, 0, # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink)
0, 0) # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink)
# send command to vehicle on 1 Hz cycle
for x in range(0,duration):
vehicle.send_mavlink(msg)
time.sleep(1)
def set_velocity(self, velocity_x, velocity_y, velocity_z):
#only let commands through at 10hz
if(time.time() - self.last_set_velocity) > self.vel_update_rate:
self.last_set_velocity = time.time()
# create the SET_POSITION_TARGET_LOCAL_NED command
msg = self.vehicle.message_factory.set_position_target_local_ned_encode(
0, # time_boot_ms (not used)
0, 0, # target system, target component
mavlink.MAV_FRAME_LOCAL_NED, # frame
0x01C7, # type_mask (ignore pos | ignore acc)
0, 0, 0, # x, y, z positions (not used)
velocity_x, velocity_y, velocity_z, # x, y, z velocity in m/s
0, 0, 0, # x, y, z acceleration (not used)
0, 0) # yaw, yaw_rate (not used)
# send command to vehicle
self.vehicle.send_mavlink(msg)
self.vehicle.flush()
VN_logger.text(VN_logger.AIRCRAFT, 'Sent Vx: {0}, Vy: {1}, Vz: {2}'.format(velocity_x,velocity_y,velocity_z))