I was running Ardusub(Sitl) with Qgroundcontrol in linux.
sim_vehicle.py -j6 -L RATBeach -v ArduSub -f vectored_6dof JSON --out=udpout:0.0.0.0:14550 --map --console
I want to Move the ROV in square shape, where heading values are 0,270,180,90 at velocity 5m/s for Time(seconds).
import time
import math
import pandas as pd
from pymavlink import mavutil
# Define UDP connection parameters
UDP_HOST = '0.0.0.0'
UDP_PORT = 14551
# Connect to the MAVLink
master = mavutil.mavlink_connection(f'udp:{UDP_HOST}:{UDP_PORT}')
boot_time = time.time()
master.wait_heartbeat()
# Function to check the current flight mode
def check_flight_mode():
mode = master.mode_mapping()[master.flightmode]
print(f"Current Flight Mode: {mode}")
# Function to change flight mode
def change_flight_mode(new_mode):
master.set_mode(new_mode)
# Function to check if the vehicle is armed
def check_vehicle_armed():
return master.motors_armed()
# Function to arm the vehicle if it's not already armed
def arm_vehicle():
master.arducopter_arm()
print("Arming the vehicle...")
# Function to disarm the vehicle
def disarm_vehicle():
master.arducopter_disarm()
print("Vehicle is disarmed.")
# Function to set heading
def set_heading(heading_deg):
master.mav.command_long_send(
master.target_system,
master.target_component,
mavutil.mavlink.MAV_CMD_CONDITION_YAW, 0, heading_deg, 45, 0, 0, 0, 0, 0)
# Function to set position
def set_position(x, y, z, vx, vy, vz, heading_deg):
time_boot_ms = int(time.time() * 1000) & 0xFFFFFFFF # Timestamp in milliseconds
heading_rad = math.radians(heading_deg)
master.mav.set_position_target_local_ned_send(
time_boot_ms,
master.target_system,
master.target_component,
mavutil.mavlink.MAV_FRAME_LOCAL_NED,
(
#mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE |
#mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE |
#mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE |
#mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE |
#mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE |
#mavutil.mavlink.POSITION_TARGET_TYPEMASK_FORCE_SET |
#mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE |
mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE
),
x, y, z, # X, Y, Z positions in meters
vx, vy, vz, # VX, VY, VZ
0, 0, 0, # Accelerations
heading_rad, 0) # Yaw, yaw_rate
# Function to wait for command acknowledgment
def wait_for_command_ack(timeout_sec=5):
start_time = time.time()
while time.time() - start_time < timeout_sec:
msg = master.recv_match(type='COMMAND_ACK', blocking=False)
if msg:
ack_result = msg.result
if ack_result == mavutil.mavlink.MAV_RESULT_ACCEPTED:
print("Command accepted.")
return True # Acknowledgment received successfully
else:
print("Command ACK waiting...")
time.sleep(0.1)
print("Timeout waiting for command acknowledgment.")
return False
def set_speed(connection, speed: float):
connection.mav.command_long_send(
connection.target_system,
connection.target_component,
mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED,0,1,speed,0,0,0,0,0,)
#set_speed_ack = connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3)
#print(f"\nSet Speed ACK: {set_speed_ack}\n")
return
def execute_mission_profile(mission_profile):
for _, row in mission_profile.iterrows():
heading_deg = row['Heading(degree)']
velocity = row['Speed(m/s)']
depth_m = row['Depth(meter)']
time_sec = row['Time(sec)']
heading_rad = math.radians(heading_deg)
velocity_x = round(velocity * math.cos(heading_rad), 10)
velocity_y = round(velocity * math.sin(heading_rad), 10)
print(f"Heading: {heading_deg} velocity: {velocity_x}, {velocity_y} depth_m: {depth_m}")
# Start time
#start_time = time.time()
#while time.time() - start_time < time_sec:
# # Calculate elapsed time
# elapsed_time = time.time() - start_time
# Calculate new position based on velocity and time elapsed
new_x = velocity_x * time_sec
new_y = velocity_y * time_sec
# Update position
set_position(new_x, new_y, depth_m, velocity_x, velocity_y, 0, heading_deg)
# Wait for next iteration
time.sleep(time_sec)
def set_ekf_origin(latitude, longitude, altitude):
# Latitude and longitude should be multiplied by 10^7 to match the format
lat_int = int(latitude * 1e7)
lon_int = int(longitude * 1e7)
# Send COMMAND_INT message to set the home location
master.mav.command_int_send(
master.target_system,
master.target_component,
mavutil.mavlink.MAV_FRAME_GLOBAL,
mavutil.mavlink.MAV_CMD_DO_SET_HOME,
0, # Not used
0, # Not used
1, # Use specified location
0, # Not used
lat_int, # Latitude in degrees * 10^7
lon_int, # Longitude in degrees * 10^7
altitude, # Altitude in meters
0, # y parameter
0 # z parameter
)
print("EKF origin set to specified position.")
def main():
# Read mission profile from CSV
mission_profile = pd.read_csv('mission.csv')
# Disarm the vehicle
disarm_vehicle()
time.sleep(2)
# Arm the vehicle
arm_vehicle()
time.sleep(1)
# Check the current flight mode
current_mode = check_flight_mode()
# If the current flight mode is not "guided", change it to "guided"
if current_mode != "guided":
print("Switching to 'guided' mode...")
change_flight_mode("GUIDED")
print("Flight mode changed to 'guided'.")
else:
print("Already in 'guided' mode.")
# Set EKF origin to current position
#set_ekf_origin(0, 0, 0)
while(1):
# Execute mission profile
execute_mission_profile(mission_profile)
# Disarm the vehicle
disarm_vehicle()
# Run the main function
if __name__ == "__main__":
main()
Leg,Heading(degree),Speed(m/s),Depth(meter),Time(sec)
1,0,8,10,30
2,270,8,20,30
3,180,8,30,30
4,90,8,40,30
I read the documentation for pymavlink. According to my understanding MAV_FRAME_LOCAL_NED with guided mode was perfect fit for me. i read from csv file and set the heading and velocity for time(seconds). but it doesn’t move according in square shape.
There were some drift in angle. it was not perfect.
Some case it form square with some tilted angle.
Do anyone have any idea on rootcase of this?