Description
I'm trying to track when offboard commands are completed.
i.e. For set_position_global the way I check for completion is by polling Position & EulerAngle updates via mavsdk.telemetry, here is a quick example of this:
def is_pos_reached(curr_lat: float,
curr_lng: float,
curr_rel_alt: float,
dest_lat: float,
dest_lng: float,
dest_rel_alt: float,
tolerance_alt_m: float = 1.5,
tolerance_pos_m: float = 5.0) -> bool:
coord_tolerance_deg = tolerance_pos_m / 111139
diff_lat = fabs(curr_lat - dest_lat) - coord_tolerance_deg
diff_lon = fabs(curr_lng - dest_lng) - coord_tolerance_deg
diff_rel_alt = fabs(curr_rel_alt - dest_rel_alt) - tolerance_alt_m
if diff_lat <= 0 and diff_lon <= 0 and diff_rel_alt <= 0:
return True
return False
def is_alt_reached(curr_alt: float,
dest_alt: float,
tolerance_alt_m: float = 0.3) -> bool:
diff_rel_alt = curr_alt - dest_alt - tolerance_alt_m
if diff_rel_alt >= 0:
return True
return False
async def check_pos_and_yaw_reached(self,
command_type: EnumCommandType,
dest_lat: float,
dest_lng: float,
dest_rel_alt_m: float,
dest_yaw: float,
command_poll_timeout_s: float = 0.1):
position_reached = False
yaw_reached = False
while not position_reached or not yaw_reached:
# NOTE: self._telemetry.position_sensor & self._telemetry.attitude_euler_sensor are just some wrappers I made for
# Position and EulerAngle updates
curr_pos: PositionState = self._telemetry.position_sensor.value
curr_att: EulerAngleState = self._telemetry.attitude_euler_sensor.value
position_reached = is_pos_reached(curr_lat=curr_pos.latitude_deg,
curr_lng=curr_pos.longitude_deg,
curr_rel_alt=curr_pos.relative_altitude_m,
dest_lat=dest_lat,
dest_lng=dest_lng,
dest_rel_alt=dest_rel_alt_m)
yaw_reached = is_yaw_reached(curr_yaw=curr_att.yaw_deg,
dest_yaw=dest_yaw)
await asyncio.sleep(command_poll_timeout_s)
You'll note:
is_pos_reachedhas tolerance valuestolerance_alt_mandtolerance_pos_mis_alt_reachedhas tolerance valuetolerance_alt_m
Obviously, I need these tolerance values because the aircraft position (latitude, longitude, altitude, etc) are never going to precisely match the desired position (due to interference).
However, this is not ideal as I need to use a tolerance value that I've come up with which results in a mismatch between when my logic determines an offboard command like set_position_global has completed and when the command has actually completed.
To my knowledge this isnt an issue for anything other than offboard commands as for each MAVSDK exposes some way to easily detect and track the status of a command, i.e:
- I can easily detect when a takeoff command via completed via either LandedState or FlightMode
- I can easily detect when a mission has completed via MissionProgress mission item updates
- etc...
Is there currently a way or could something be exposed in MAVSDK that would provide an easy way to detect when offboard commands have completed that wouldn't result in the mismatch I have described?