Skip to content

Commit

Permalink
1.1.6
Browse files Browse the repository at this point in the history
  • Loading branch information
test-bbb committed Aug 27, 2023
1 parent 13a1d4a commit 35ae3f3
Show file tree
Hide file tree
Showing 3 changed files with 16 additions and 15 deletions.
5 changes: 4 additions & 1 deletion CHANGES.txt
Original file line number Diff line number Diff line change
Expand Up @@ -19,4 +19,7 @@
- Docstrings updated

1.1.4
- Add HOME_POSITION to mandatory message types
- Add HOME_POSITION to mandatory message types

1.1.6
- Change way of prompting HOME_POSITION
2 changes: 1 addition & 1 deletion pyproject.toml
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"

[project]
name = "mavcom"
version = "1.1.5"
version = "1.1.6"
description = "A python package that creates a simplified programming interface for controlling Mavlink-capable flight controllers"
readme = "README.md"
authors = [{ name = "Mavscient", email = "[email protected]" }]
Expand Down
24 changes: 11 additions & 13 deletions src/mavcom/mavcontrol.py
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,7 @@ def __init__(self,
self._flight_mode = None
self._motors_armed = False
self.airframe = None
self.data_rate = 120

self.telemetry_thread = threading.Thread(target=self._monitor_mavlink_messages, daemon=has_controller)

Expand All @@ -68,6 +69,8 @@ def start(self) -> None:
so that it exits when the caller object terminates.
"""
print("MAVCOM: Mavcom active")
self.connection.mav.request_data_stream_send(self.connection.target_system, self.connection.target_component,
mavutil.mavlink.MAV_DATA_STREAM_ALL, self.data_rate, 1)
self.telemetry_thread.start()
time.sleep(3)

Expand All @@ -80,7 +83,7 @@ def _get_heartbeat(self):
self.current_values['HEARTBEAT'] = hb.to_dict()

self.airframe = AIRFRAME_TYPES[self.current_values['HEARTBEAT']['type']]
self.flight_mode = MODE_MAP[self.airframe][self.current_values["HEARTBEAT"]["custom_mode"]]
# self.flight_mode = MODE_MAP[self.airframe][self.current_values["HEARTBEAT"]["custom_mode"]] # Removed, don't set the mode

def _monitor_mavlink_messages(self):

Expand All @@ -98,18 +101,13 @@ def get_home_pos(self):
FC will only send home position message a few times during initialisation.
Prompt it to transmit and capture it.
"""
i = 0
while "HOME_POSITION" not in self.current_values:
self.connection.mav.command_long_send(
self.connection.target_system,
self.connection.target_component,
mavutil.mavlink.MAV_CMD_GET_HOME_POSITION,
0,0,0,0,0,0,0,0
)
time.sleep(0.5)
i += 1
if i > 10:
break
self.connection.mav.command_long_send(
self.connection.target_system,
self.connection.target_component,
mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE,
242,0,0,0,0,0,0,0
)
time.sleep(0.5)
home_position = self.current_values['HOME_POSITION']
return home_position

Expand Down

0 comments on commit 35ae3f3

Please sign in to comment.