I have written the following code to orbit around a specific coordinate and point(yaw) at the coordinate as it obits. I have calculated all the waypoints(latitude, longitude, absolute angle of yaw at that lat long). The following information is passed as a mavlink mission item to the drone in simulation. For some reason it does not change its yaw. It simply points in the direction of next way point.
I am not sure if this is a simulation error or am I missing out on something?
from pymavlink import mavutil
import pymavlink.dialects.v20.all as dialect
import time
from geopy import distance
from geopy.distance import distance, Distance
from geopy.distance import geodesic
def config_1e7(n):
return int(n*(10**7))/(10**7)
def plan_mission( drone, lat, long, alt, angle):
"""
Displaces from point A(curr) to point B(given). All params in SI units
"""
drone.wait_heartbeat()
mission = dialect.MAVLink_mission_item_int_message(target_system=drone.target_system,
target_component=drone.target_component,
seq=0,
frame=dialect.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
command=dialect.MAV_CMD_NAV_WAYPOINT,
current=2,
autocontinue=1,
param1=0,
param2=0,
param3=0,
param4=angle,
x=int(lat * 1e7),
y=int(long * 1e7),
z=alt
)
drone.mav.send(mission)
drone=mavutil.mavlink_connection("udpin:127.0.0.1:14550")
drone.wait_heartbeat()
mm=drone.mode_mapping()["AUTO"]
drone.set_mode(mm)
tt=[[0],[90],[180],[270],[0]]
radius = 0.05 #in km
lat = -35.363
long = 149.16523
alt = 50
resolution = 10**7
for i in range(len(tt)):
x, y, _ = distance(kilometers=1).destination((lat, long), bearing = tt[i][0], distance = Distance( radius ))
GP=None
while i:
GP = drone.recv_match(type='GLOBAL_POSITION_INT')
if GP==None:
continue
if(geodesic((GP.lat/resolution,GP.lon/resolution),(x,y)).kilometers<0.005):
break
plan_mission(drone,x,y,alt,tt[i][0])