mirror of
https://github.com/okalachev/flix.git
synced 2025-07-27 01:29:33 +00:00
Updates in pyflix
Rename mav_to to system_id to match firmware naming. Readme updates.
This commit is contained in:
parent
018a6d4fce
commit
32f417efae
@ -71,7 +71,7 @@ The `value` argument specifies a condition for filtering events. It can be eithe
|
|||||||
```python
|
```python
|
||||||
flix.wait('armed', value=True) # wait until armed
|
flix.wait('armed', value=True) # wait until armed
|
||||||
flix.wait('armed', value=False) # wait until disarmed
|
flix.wait('armed', value=False) # wait until disarmed
|
||||||
flix.wait('motors', value=lambda motors: all(m == 0 for m in motors)) # wait until all motors stop
|
flix.wait('motors', value=lambda motors: not any(motors)) # wait until all motors stop
|
||||||
flix.wait('attitude_euler', value=lambda att: att[0] > 0) # wait until roll angle is positive
|
flix.wait('attitude_euler', value=lambda att: att[0] > 0) # wait until roll angle is positive
|
||||||
```
|
```
|
||||||
|
|
||||||
@ -106,7 +106,7 @@ Full list of events:
|
|||||||
Get and set firmware parameters using `get_param` and `set_param` methods:
|
Get and set firmware parameters using `get_param` and `set_param` methods:
|
||||||
|
|
||||||
```python
|
```python
|
||||||
print(flix.get_param('PITCH_P')) # get parameter value
|
pitch_p = flix.get_param('PITCH_P') # get parameter value
|
||||||
flix.set_param('PITCH_P', 5) # set parameter value
|
flix.set_param('PITCH_P', 5) # set parameter value
|
||||||
```
|
```
|
||||||
|
|
||||||
|
@ -34,18 +34,18 @@ class Flix:
|
|||||||
acc: List[float]
|
acc: List[float]
|
||||||
gyro: List[float]
|
gyro: List[float]
|
||||||
|
|
||||||
mav_id: int
|
system_id: int
|
||||||
messages: Dict[str, Dict[str, Any]] # MAVLink messages storage
|
messages: Dict[str, Dict[str, Any]] # MAVLink messages storage
|
||||||
values: Dict[Union[str, int], Union[float, List[float]]] = {} # named values
|
values: Dict[Union[str, int], Union[float, List[float]]] = {} # named values
|
||||||
|
|
||||||
_connection_timeout = 3
|
_connection_timeout = 3
|
||||||
_print_buffer: str = ''
|
_print_buffer: str = ''
|
||||||
|
|
||||||
def __init__(self, mav_id: int=1, wait_connection: bool=True):
|
def __init__(self, system_id: int=1, wait_connection: bool=True):
|
||||||
if not (0 <= mav_id < 256):
|
if not (0 <= system_id < 256):
|
||||||
raise ValueError('mav_id must be in range [0, 255]')
|
raise ValueError('system_id must be in range [0, 255]')
|
||||||
self._setup_mavlink()
|
self._setup_mavlink()
|
||||||
self.mav_id = mav_id
|
self.system_id = system_id
|
||||||
self._init_state()
|
self._init_state()
|
||||||
try:
|
try:
|
||||||
# Direct connection
|
# Direct connection
|
||||||
@ -57,7 +57,7 @@ class Flix:
|
|||||||
# Port busy - using proxy
|
# Port busy - using proxy
|
||||||
logger.debug('Listening on port 14560 (proxy)')
|
logger.debug('Listening on port 14560 (proxy)')
|
||||||
self.connection: mavutil.mavfile = mavutil.mavlink_connection('udpin:0.0.0.0:14555', source_system=254) # type: ignore
|
self.connection: mavutil.mavfile = mavutil.mavlink_connection('udpin:0.0.0.0:14555', source_system=254) # type: ignore
|
||||||
self.connection.target_system = mav_id
|
self.connection.target_system = system_id
|
||||||
self.mavlink: mavlink.MAVLink = self.connection.mav
|
self.mavlink: mavlink.MAVLink = self.connection.mav
|
||||||
self._event_listeners: Dict[str, List[Callable[..., Any]]] = {}
|
self._event_listeners: Dict[str, List[Callable[..., Any]]] = {}
|
||||||
self.messages = {}
|
self.messages = {}
|
||||||
@ -253,7 +253,7 @@ class Flix:
|
|||||||
for attempt in range(3):
|
for attempt in range(3):
|
||||||
try:
|
try:
|
||||||
logger.debug(f'Get param {name} (attempt #{attempt + 1})')
|
logger.debug(f'Get param {name} (attempt #{attempt + 1})')
|
||||||
self.mavlink.param_request_read_send(self.mav_id, 0, name.encode('ascii'), -1)
|
self.mavlink.param_request_read_send(self.system_id, 0, name.encode('ascii'), -1)
|
||||||
msg: mavlink.MAVLink_param_value_message = \
|
msg: mavlink.MAVLink_param_value_message = \
|
||||||
self.wait('mavlink.PARAM_VALUE', value=lambda msg: msg.param_id == name, timeout=0.1)
|
self.wait('mavlink.PARAM_VALUE', value=lambda msg: msg.param_id == name, timeout=0.1)
|
||||||
return msg.param_value
|
return msg.param_value
|
||||||
@ -267,7 +267,7 @@ class Flix:
|
|||||||
for attempt in range(3):
|
for attempt in range(3):
|
||||||
try:
|
try:
|
||||||
logger.debug(f'Set param {name} to {value} (attempt #{attempt + 1})')
|
logger.debug(f'Set param {name} to {value} (attempt #{attempt + 1})')
|
||||||
self.mavlink.param_set_send(self.mav_id, 0, name.encode('ascii'), value, mavlink.MAV_PARAM_TYPE_REAL32)
|
self.mavlink.param_set_send(self.system_id, 0, name.encode('ascii'), value, mavlink.MAV_PARAM_TYPE_REAL32)
|
||||||
self.wait('mavlink.PARAM_VALUE', value=lambda msg: msg.param_id == name, timeout=0.1)
|
self.wait('mavlink.PARAM_VALUE', value=lambda msg: msg.param_id == name, timeout=0.1)
|
||||||
return
|
return
|
||||||
except TimeoutError:
|
except TimeoutError:
|
||||||
@ -297,7 +297,7 @@ class Flix:
|
|||||||
raise ValueError('roll, pitch, yaw must be in range [-1, 1]')
|
raise ValueError('roll, pitch, yaw must be in range [-1, 1]')
|
||||||
if not 0 <= throttle <= 1:
|
if not 0 <= throttle <= 1:
|
||||||
raise ValueError('throttle must be in range [0, 1]')
|
raise ValueError('throttle must be in range [0, 1]')
|
||||||
self.mavlink.manual_control_send(self.mav_id, roll * 1000, pitch * 1000, yaw * 1000, throttle * 1000, 0) # type: ignore
|
self.mavlink.manual_control_send(self.system_id, roll * 1000, pitch * 1000, yaw * 1000, throttle * 1000, 0) # type: ignore
|
||||||
|
|
||||||
def set_mode(self, mode: Literal['MANUAL', 'ACRO', 'STAB', 'USER']):
|
def set_mode(self, mode: Literal['MANUAL', 'ACRO', 'STAB', 'USER']):
|
||||||
raise NotImplementedError('Setting mode is not implemented yet')
|
raise NotImplementedError('Setting mode is not implemented yet')
|
||||||
|
Loading…
x
Reference in New Issue
Block a user