Hi! I want to use GUIDED mode to do some development, but I found that I couldn’t switch to this mode. Is there any other requirement to switch this mode? I have sent the location via GPS_INPUT message to the rov and the rov has also received the message.
Hi @chang-M -
Guided mode indeed requires the autopilot to know its position. What messages are you generating with location, and how? How are they being sent to the autopilot?
Your motion sensors also need to be calibrated (accelerometer & compass) to be in this mode as well…
@tony-white Thank you for your reply, My message code is as follows:
def send_gps_input(master, lat, lon, hdop, fix_quality, numsats):
try:
time_usec = int(time.time() * 1e6)
# 1e7
lat_int = int(lat * 1e7)
lon_int = int(lon * 1e7)
# GPS_INPUT_IGNORE_FLAGS
ignore_flags = (mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_VEL_HORIZ |
mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_VEL_VERT |
mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_SPEED_ACCURACY)
master.mav.gps_input_send(
time_usec, # Timestamp (micros since boot or Unix epoch)
0, # ID of the GPS for multiple GPS inputs
# Flags indicating which fields to ignore (see GPS_INPUT_IGNORE_FLAGS enum).
# All other fields must be provided.
ignore_flags,
0, # GPS time (milliseconds from start of GPS week)
0, # GPS week number
fix_quality, # 0-1: no fix, 2: 2D fix, 3: 3D fix. 4: 3D with DGPS. 5: 3D with RTK
lat_int, # Latitude (WGS84), in degrees * 1E7
lon_int, # Longitude (WGS84), in degrees * 1E7
0, # Altitude (AMSL, not WGS84), in m (positive for up)
int(hdop * 100), # GPS HDOP horizontal dilution of position in m
1, # GPS VDOP vertical dilution of position in m
0, # GPS velocity in m/s in NORTH direction in earth-fixed NED frame
0, # GPS velocity in m/s in EAST direction in earth-fixed NED frame
0, # GPS velocity in m/s in DOWN direction in earth-fixed NED frame
0, # GPS speed accuracy in m/s
0, # GPS horizontal accuracy in m
0, # GPS vertical accuracy in m
numsats # Number of satellites visible.
)
print(f"GPS_INPUT sent: lat={lat}, lon={lon}, hdop={hdop}, numsats={numsats}")
except Exception as e:
print(f"Failed to send GPS_INPUT: {e}")
After executing the code, I can see the correct position in the GLOBAL_POSITION_INT message, and also display the position correctly in QGC, but I can’t switch to GUIDED mode. And there is another problem, which is when I use SET_POSITION_TARGET_GLOBAL_INT to set the rov position in ALT_HOLD mode, the lat and lon of the message are invalid, and the robot is just moving up and down to the target alt. Is this also the two reasons you mentioned above?
This is my code for setting the location:
def set_global_position(self):
type_mask = (
# 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
)
# MAV_FRAME_GLOBAL
# MAV_FRAME_GLOBAL_RELATIVE_ALT
# MAV_FRAME_GLOBAL_TERRAIN_ALT
lat,lon = get_target_location()
if lat is None or lon is None:
LOG.error('The target location cannot be obtained, please check the UGPS')
return
self.master.mav.set_position_target_global_int_send(
int(1e3 * (time.time() - self.boot_time)),
self.master.target_system, self.master.target_component,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
type_mask,
int(lat * 1e7),
int(lon * 1e7),
-0.5, # alt
0, 0, 0, # speed
0, 0, 0, # acc
0, 0 # yaw, yaw_rate
)
Hi @chang-M -
This python script shows how to send position and heading to the vehicle. To use it, copy the text and save it as fake_gps_yaw.py:
- In BlueOS, navigate to terminal, take red-pill, and nano a new file named fake_gps_yaw.py, save (cntrl o) and exit (cntrl x).
- chmod +X fake_gps_yaw.py
- Edit the file (nano fake_gps_yaw.py) and on line 96, change the IP address of the vehicle to an interface it has a connection on - typically this would be 192.168.2.2 as this is the static IP on the Pi ethernet adapter by default, the example is what I used for my unit that is only connected via WiFi and received that dhcp address.
response = requests.post(‘http://192.168.1.23:6040/mavlink’, json=payload) - Optionally edit the error message on line 101 to use the same IP address.
- In autopilot parameters, verify that
EK3_SRC1_POSXY,EK3_SRC1_YVELXY,EK3_SRC1_YAW, are all set to GPS. Also verify thatGPS_TYPEis set to MAV. - Run the script. The vehicle will jump to the coordinates and heading described in the file, and update as the script runs.
import requests
import json
import time
from datetime import datetime, timezone
import math
def get_gps_week_and_ms():
# GPS epoch started January 6, 1980
gps_epoch = datetime(1980, 1, 6, tzinfo=timezone.utc)
current = datetime.now(timezone.utc)
# Calculate total seconds since GPS epoch
total_seconds = (current - gps_epoch).total_seconds()
# Calculate GPS week number (7 days = 604800 seconds)
week = math.floor(total_seconds / 604800)
# Calculate milliseconds into the week
ms = int((total_seconds % 604800) * 1000)
return week, ms
def calculate_circle_position(center_lat, center_lon, radius, angle_degrees):
"""
Calculate position and yaw for a point on a circle
radius in meters, angle in degrees
Returns lat, lon (in 1e7 format) and yaw in degrees
"""
# Convert center coordinates from 1e7 format to radians
lat1 = center_lat / 1e7 * math.pi / 180
lon1 = center_lon / 1e7 * math.pi / 180
# Earth's radius in meters
R = 6378137.0
# Convert angle to radians
angle_rad = math.radians(angle_degrees)
# Calculate offset position
dx = radius * math.cos(angle_rad)
dy = radius * math.sin(angle_rad)
# Calculate new position
new_lat = lat1 + (dy / R)
new_lon = lon1 + (dx / R) / math.cos(lat1)
# Convert back to degrees * 1e7
new_lat_e7 = int(new_lat * 180 / math.pi * 1e7)
new_lon_e7 = int(new_lon * 180 / math.pi * 1e7)
# Calculate yaw (90 degrees offset from angle as yaw is clockwise from north)
yaw = (angle_degrees + 90) % 360
return new_lat_e7, new_lon_e7, yaw
def send_gps_data(lat, lon, alt, yaw):
# Get current GPS week and milliseconds
week, week_ms = get_gps_week_and_ms()
# Current time in microseconds
time_usec = int(time.time() * 1e6)
payload = {
"header": {
"system_id": 255,
"component_id": 0,
"sequence": 0
},
"message": {
"type": "GPS_INPUT",
"time_usec": time_usec,
"time_week_ms": week_ms,
"lat": int(lat), # Latitude in degrees * 1e7
"lon": int(lon), # Longitude in degrees * 1e7
"alt": int(alt), # Altitude in meters * 1e3 (millimeters)
"hdop": 100, # Horizontal dilution of precision (100 = 1.0)
"vdop": 100, # Vertical dilution of precision (100 = 1.0)
"vn": 0, # North velocity in m/s
"ve": 0, # East velocity in m/s
"vd": 0, # Down velocity in m/s
"speed_accuracy": 100, # Speed accuracy in mm/s
"horiz_accuracy": 100, # Horizontal accuracy in mm
"vert_accuracy": 200, # Vertical accuracy in mm
"ignore_flags": {
"bits": 0 # 0 means use all fields
},
"time_week": week,
"gps_id": 0,
"fix_type": 3, # 3D fix
"satellites_visible": 10,
"yaw": int(yaw * 100) # Updated yaw value
}
}
try:
response = requests.post('http://192.168.15.10:6040/mavlink', json=payload)
if response.status_code != 200:
print(f"Failed to send GPS data. Status code: {response.status_code}")
print(f"Response content: {response.text}")
except requests.exceptions.ConnectionError as e:
print(f"Connection Error: Could not connect to mavlink2rest server at http://192.168.15.10:6040")
print(f"Detailed error: {str(e)}")
except requests.exceptions.RequestException as e:
print(f"Error sending GPS data: {type(e).__name__}")
print(f"Detailed error: {str(e)}")
def main():
# Center coordinates (San Francisco, CA)
center_lat = 377777778 # 37.7777778 degrees
center_lon = -1224167778 # -122.4167778 degrees
alt = 10000 # 10 meters (in millimeters)
radius = 10 # 10 meters radius
angle = 0.0
while True:
# Calculate new position and yaw
lat, lon, yaw = calculate_circle_position(center_lat, center_lon, radius, angle)
print(f"Sending GPS data: Lat={lat/1e7}, Lon={lon/1e7}, Alt={alt/1e3}m, Yaw={yaw}")
send_gps_data(lat, lon, alt, yaw)
# Increment angle (1 degree per update)
angle = (angle + 1) % 360
time.sleep(0.1) # Send GPS data every second
if __name__ == "__main__":
main()
Thank you for your explanation ! @EliotBR
I have another question, that is, when I switch to GUIDED mode in QGC, there will be an error in MAV_COM(176). I have sent location information to my rov through the GPS_INPUT message, and my robot also displays location in the QGC and GLOBAL_POSITION_INT message. EK3_SRC1_POSXY, EK3_SRC1_YVELXY has been set to GPS. Do I need to set other parameters? Or is it because my location information is missing some key factors？
Here is the sending location code:
def send_gps_input(master, lat, lon, hdop, fix_quality, numsats):
try:
time_usec = int(time.time() * 1e6)
lat_int = int(lat * 1e7)
lon_int = int(lon * 1e7)
# GPS_INPUT_IGNORE_FLAGS
ignore_flags = (mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_VEL_HORIZ |
mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_VEL_VERT |
mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_SPEED_ACCURACY)
master.mav.gps_input_send(
time_usec, # Timestamp (micros since boot or Unix epoch)
0, # ID of the GPS for multiple GPS inputs
# Flags indicating which fields to ignore (see GPS_INPUT_IGNORE_FLAGS enum).
# All other fields must be provided.
ignore_flags,
0, # GPS time (milliseconds from start of GPS week)
0, # GPS week number
fix_quality, # 0-1: no fix, 2: 2D fix, 3: 3D fix. 4: 3D with DGPS. 5: 3D with RTK
lat_int, # Latitude (WGS84), in degrees * 1E7
lon_int, # Longitude (WGS84), in degrees * 1E7
0, # Altitude (AMSL, not WGS84), in m (positive for up)
int(hdop * 100), # GPS HDOP horizontal dilution of position in m
1, # GPS VDOP vertical dilution of position in m
0, # GPS velocity in m/s in NORTH direction in earth-fixed NED frame
0, # GPS velocity in m/s in EAST direction in earth-fixed NED frame
0, # GPS velocity in m/s in DOWN direction in earth-fixed NED frame
0, # GPS speed accuracy in m/s
0, # GPS horizontal accuracy in m
0, # GPS vertical accuracy in m
numsats # Number of satellites visible.
)
print(f"GPS_INPUT sent: lat={lat}, lon={lon}, hdop={hdop}, numsats={numsats}")
except Exception as e:
print(f"Failed to send GPS_INPUT: {e}")
Hi @chang-M,
I’ve moved your comment here, because it’s on the same topic.
That’s the mode switch message. The command failure/rejection should be accompanied by some kind of error message, or at least an error type should be findable through the corresponding MAV_RESULT that the autopilot replies with.
As in @tony-white’s response above, you should
Yes, my GPS_TYPE is also set to MAV, and I execute the command to switch to GUIDED mode. The result is MAV_RESULT_FAILED. Is it because the ardusub system has any checks? I also tried all the options to turn off GPS_CHECK and still returned this result.
The underwater GPS I use is the SBL of waterlink. Can I use this code from four years ago (change the addresses of MAV2RESET and other interfaces in it) to perform underwater positioning? Is there a built-in underwater positioning device processing program in blueos? (just like ping360 sonar and p30)
@EliotBR @tony-white Thank you for your help 
. I am now able to switch to GUIDED mode, but I still can’t switch to AUTO mode. When switching to AUTO mode, I will not return any MAV_RESULT messages when I keep waiting. Why is this? Are there any conditions required to use AUTO mode? I couldn’t switch to AUTO mode during the system trial in the upper right corner of the BlueOS document website. Is it because the firmware itself does not support AUTO mode?
Hi @chang-M -
Have you uploaded a valid mission file to the autopilot? This is required for auto mode to have something to follow…
That code is from our old Companion software, which is no longer maintained or supported.
WaterLinked UGPS support is not built in to BlueOS, but is instead available through a dedicated Extension, which you can install through the Extensions manager 