px4-configuration 0.2.0__tar.gz → 0.2.2__tar.gz
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/.gitignore +2 -1
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/PKG-INFO +4 -4
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/README.md +1 -1
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/px4_configuration/api/__init__.py +2 -2
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/px4_configuration/api/config.py +12 -2
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/px4_configuration/api/main.py +3 -3
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/px4_configuration/api/models.py +66 -3
- px4_configuration-0.2.2/px4_configuration/api/services/calibration_service.py +236 -0
- px4_configuration-0.2.2/px4_configuration/api/services/health_service.py +215 -0
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/px4_configuration/api/services/param_service.py +26 -7
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/px4_configuration/api/services/sdcard_service.py +26 -7
- px4_configuration-0.2.2/px4_configuration/api/services/shell_service.py +91 -0
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/px4_configuration/api/utils/calibration_parser.py +62 -21
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/pyproject.toml +3 -3
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/requirements.txt +1 -1
- px4_configuration-0.2.0/px4_configuration/api/services/calibration_service.py +0 -136
- px4_configuration-0.2.0/px4_configuration/api/services/health_service.py +0 -131
- px4_configuration-0.2.0/px4_configuration/api/services/shell_service.py +0 -65
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/configs/default-rc.params +0 -0
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/configs/generic-quad-airframe.params +0 -0
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/configs/mrs-generic.params +0 -0
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/configs/platforms/x500.params +0 -0
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/configs/sd-card/extras.txt +0 -0
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/px4_configuration/__init__.py +0 -0
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/px4_configuration/api/services/__init__.py +0 -0
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/px4_configuration/api/utils/__init__.py +0 -0
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/scripts/calibrate.py +0 -0
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/scripts/px_uploader.py +0 -0
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/scripts/set-params.py +0 -0
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/scripts/shell.py +0 -0
- {px4_configuration-0.2.0 → px4_configuration-0.2.2}/scripts/upload-sdcard-config.py +0 -0
|
@@ -1,12 +1,12 @@
|
|
|
1
1
|
Metadata-Version: 2.4
|
|
2
2
|
Name: px4-configuration
|
|
3
|
-
Version: 0.2.
|
|
3
|
+
Version: 0.2.2
|
|
4
4
|
Summary: Tools and REST API for configuring PX4 from a companion computer
|
|
5
5
|
Author-email: Alex <bonnefond@fly4future.com>
|
|
6
6
|
License: BSD-3-Clause
|
|
7
|
-
Requires-Python: >=3.
|
|
7
|
+
Requires-Python: >=3.10
|
|
8
8
|
Requires-Dist: fastapi>=0.104.0
|
|
9
|
-
Requires-Dist: mavsdk==3.
|
|
9
|
+
Requires-Dist: mavsdk==3.10.2
|
|
10
10
|
Requires-Dist: python-multipart>=0.0.6
|
|
11
11
|
Requires-Dist: sse-starlette>=1.6.5
|
|
12
12
|
Requires-Dist: tqdm==4.67.1
|
|
@@ -60,7 +60,7 @@ pip install dist/px4_configuration-0.1.0-py3-none-any.whl
|
|
|
60
60
|
|
|
61
61
|
## Requirements
|
|
62
62
|
|
|
63
|
-
- Python 3.
|
|
63
|
+
- Python 3.10+
|
|
64
64
|
- PX4 flight controller connected over serial (e.g. `/dev/pixhawk`)
|
|
65
65
|
- User added to the `dialout` group for serial access (`sudo usermod -aG dialout $USER`)
|
|
66
66
|
|
|
@@ -45,7 +45,7 @@ pip install dist/px4_configuration-0.1.0-py3-none-any.whl
|
|
|
45
45
|
|
|
46
46
|
## Requirements
|
|
47
47
|
|
|
48
|
-
- Python 3.
|
|
48
|
+
- Python 3.10+
|
|
49
49
|
- PX4 flight controller connected over serial (e.g. `/dev/pixhawk`)
|
|
50
50
|
- User added to the `dialout` group for serial access (`sudo usermod -aG dialout $USER`)
|
|
51
51
|
|
|
@@ -3,6 +3,16 @@
|
|
|
3
3
|
import os
|
|
4
4
|
|
|
5
5
|
|
|
6
|
+
def _get_package_version() -> str:
|
|
7
|
+
"""Read version from package metadata (pyproject.toml). Single source of truth."""
|
|
8
|
+
try:
|
|
9
|
+
from importlib.metadata import PackageNotFoundError, version
|
|
10
|
+
|
|
11
|
+
return version("px4-configuration")
|
|
12
|
+
except Exception:
|
|
13
|
+
return "0.0.0"
|
|
14
|
+
|
|
15
|
+
|
|
6
16
|
class Settings:
|
|
7
17
|
"""Application settings with defaults."""
|
|
8
18
|
|
|
@@ -10,9 +20,9 @@ class Settings:
|
|
|
10
20
|
default_port: str = os.getenv("PX4_DEFAULT_PORT", "/dev/pixhawk")
|
|
11
21
|
default_baudrate: int = int(os.getenv("PX4_DEFAULT_BAUDRATE", "2000000"))
|
|
12
22
|
|
|
13
|
-
# API settings
|
|
23
|
+
# API settings (version from pyproject.toml via package metadata)
|
|
14
24
|
api_title: str = "PX4 Configuration API"
|
|
15
|
-
api_version: str =
|
|
25
|
+
api_version: str = _get_package_version()
|
|
16
26
|
api_description: str = (
|
|
17
27
|
"REST API for configuring PX4 flight controller from companion computer"
|
|
18
28
|
)
|
|
@@ -70,11 +70,11 @@ async def start_calibration(request: CalibrationRequest):
|
|
|
70
70
|
calibration_type=request.calibration_type, # <-- enum here
|
|
71
71
|
):
|
|
72
72
|
# Convert dict -> SSE frame
|
|
73
|
-
yield
|
|
73
|
+
yield json.dumps(payload)
|
|
74
74
|
except Exception as exc:
|
|
75
|
-
yield
|
|
75
|
+
yield json.dumps({'type': 'error', 'message': str(exc)})
|
|
76
76
|
finally:
|
|
77
|
-
yield
|
|
77
|
+
yield json.dumps({'type': 'complete'})
|
|
78
78
|
|
|
79
79
|
return EventSourceResponse(event_generator())
|
|
80
80
|
|
|
@@ -1,12 +1,12 @@
|
|
|
1
1
|
from __future__ import annotations
|
|
2
2
|
|
|
3
|
-
from enum import
|
|
4
|
-
from typing import Literal, Optional, Dict, Any
|
|
3
|
+
from enum import Enum
|
|
4
|
+
from typing import Literal, Optional, Dict, Any, List
|
|
5
5
|
|
|
6
6
|
from pydantic import BaseModel, Field, ConfigDict
|
|
7
7
|
|
|
8
8
|
|
|
9
|
-
class CalibrationType(
|
|
9
|
+
class CalibrationType(Enum):
|
|
10
10
|
"""Supported calibration types."""
|
|
11
11
|
GYROSCOPE = "gyroscope"
|
|
12
12
|
ACCELEROMETER = "accelerometer"
|
|
@@ -59,6 +59,61 @@ class CalibrationHealth(BaseModel):
|
|
|
59
59
|
"""Whether the calibration is required."""
|
|
60
60
|
return not (self.gyroscope_ok and self.accelerometer_ok and self.magnetometer_ok)
|
|
61
61
|
|
|
62
|
+
|
|
63
|
+
class HealthAndArmingCheckProblem(BaseModel):
|
|
64
|
+
"""A single health/arming check problem from PX4 Events."""
|
|
65
|
+
|
|
66
|
+
message: str = Field(..., description="Short, single-line message.")
|
|
67
|
+
description: str = Field(default="", description="Detailed description (optional).")
|
|
68
|
+
health_component: str = Field(default="", description="Associated component, e.g. 'gps'.")
|
|
69
|
+
|
|
70
|
+
|
|
71
|
+
class HealthAndArmingCheckMode(BaseModel):
|
|
72
|
+
"""Arming checks for a specific flight mode (from PX4 Events)."""
|
|
73
|
+
|
|
74
|
+
mode_name: str = Field(..., description="Mode name, e.g. 'Position'.")
|
|
75
|
+
can_arm_or_run: bool = Field(
|
|
76
|
+
...,
|
|
77
|
+
description="If disarmed: whether arming is possible. If armed: whether the mode can be selected.",
|
|
78
|
+
)
|
|
79
|
+
problems: List[HealthAndArmingCheckProblem] = Field(
|
|
80
|
+
default_factory=list,
|
|
81
|
+
description="List of reported problems for this mode.",
|
|
82
|
+
)
|
|
83
|
+
|
|
84
|
+
|
|
85
|
+
class HealthComponentReport(BaseModel):
|
|
86
|
+
"""Health component report (e.g. GPS, Accelerometer) from PX4 Events."""
|
|
87
|
+
|
|
88
|
+
name: str = Field(..., description="Unique component name, e.g. 'gps'.")
|
|
89
|
+
label: str = Field(..., description="Human-readable label, e.g. 'GPS'.")
|
|
90
|
+
is_present: bool = Field(..., description="Whether the component is present.")
|
|
91
|
+
has_error: bool = Field(..., description="Whether the component has errors.")
|
|
92
|
+
has_warning: bool = Field(..., description="Whether the component has warnings.")
|
|
93
|
+
|
|
94
|
+
|
|
95
|
+
class HealthAndArmingCheckReport(BaseModel):
|
|
96
|
+
"""
|
|
97
|
+
Health and arming checks report from MAVSDK Events plugin.
|
|
98
|
+
|
|
99
|
+
Provides detailed readiness-to-fly information: current mode arming status,
|
|
100
|
+
per-component health, and all reported problems.
|
|
101
|
+
"""
|
|
102
|
+
|
|
103
|
+
current_mode_intention: Optional[HealthAndArmingCheckMode] = Field(
|
|
104
|
+
default=None,
|
|
105
|
+
description="Report for the currently intended flight mode (can_arm_or_run, problems).",
|
|
106
|
+
)
|
|
107
|
+
health_components: List[HealthComponentReport] = Field(
|
|
108
|
+
default_factory=list,
|
|
109
|
+
description="Health components (e.g. gps, accelerometer) with present/error/warning flags.",
|
|
110
|
+
)
|
|
111
|
+
all_problems: List[HealthAndArmingCheckProblem] = Field(
|
|
112
|
+
default_factory=list,
|
|
113
|
+
description="Complete list of all reported problems.",
|
|
114
|
+
)
|
|
115
|
+
|
|
116
|
+
|
|
62
117
|
class Px4HealthResponse(BaseModel):
|
|
63
118
|
"""
|
|
64
119
|
High-level PX4 health snapshot.
|
|
@@ -111,6 +166,14 @@ class Px4HealthResponse(BaseModel):
|
|
|
111
166
|
description="True if PX4 considers the vehicle armable.",
|
|
112
167
|
)
|
|
113
168
|
|
|
169
|
+
arming_checks_report: Optional[HealthAndArmingCheckReport] = Field(
|
|
170
|
+
default=None,
|
|
171
|
+
description=(
|
|
172
|
+
"Detailed health and arming checks from MAVSDK Events plugin "
|
|
173
|
+
"(mode intention, health components, all problems). None if Events plugin unavailable."
|
|
174
|
+
),
|
|
175
|
+
)
|
|
176
|
+
|
|
114
177
|
raw_health: Optional[Dict[str, Any]] = Field(
|
|
115
178
|
default=None,
|
|
116
179
|
description=(
|
|
@@ -0,0 +1,236 @@
|
|
|
1
|
+
"""Service for PX4 sensor calibration operations."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
import asyncio
|
|
6
|
+
from typing import AsyncGenerator, Dict, Any, Optional
|
|
7
|
+
|
|
8
|
+
from mavsdk import System
|
|
9
|
+
|
|
10
|
+
try:
|
|
11
|
+
from grpc import RpcError
|
|
12
|
+
except ImportError:
|
|
13
|
+
# Fallback if grpc is not directly available (shouldn't happen with mavsdk)
|
|
14
|
+
RpcError = Exception
|
|
15
|
+
|
|
16
|
+
from ..models import CalibrationType
|
|
17
|
+
from ..utils.calibration_parser import (
|
|
18
|
+
parse_accelerometer_progress,
|
|
19
|
+
parse_gyroscope_progress,
|
|
20
|
+
parse_magnetometer_progress,
|
|
21
|
+
parse_horizon_progress,
|
|
22
|
+
)
|
|
23
|
+
|
|
24
|
+
|
|
25
|
+
def _format_grpc_error(exc: Exception) -> str:
|
|
26
|
+
"""Format gRPC errors into user-friendly messages."""
|
|
27
|
+
error_msg = str(exc)
|
|
28
|
+
|
|
29
|
+
if isinstance(exc, RpcError):
|
|
30
|
+
# Check for COMMAND_DENIED (PX4 rejected the command)
|
|
31
|
+
if "COMMAND_DENIED" in error_msg:
|
|
32
|
+
return (
|
|
33
|
+
"PX4 rejected the calibration command. "
|
|
34
|
+
"This may occur if: (1) a calibration is already in progress, "
|
|
35
|
+
"(2) the vehicle is armed or in flight, (3) the vehicle is not in the correct state. "
|
|
36
|
+
"Please ensure the vehicle is disarmed, on the ground, and no other calibration is running."
|
|
37
|
+
)
|
|
38
|
+
# Check for common gRPC error patterns
|
|
39
|
+
if "Connection reset by peer" in error_msg or "UNAVAILABLE" in error_msg:
|
|
40
|
+
return (
|
|
41
|
+
"Connection to PX4 device was lost. "
|
|
42
|
+
"This may occur if the device disconnected, rebooted, or the connection timed out. "
|
|
43
|
+
"Please check the device connection and try again."
|
|
44
|
+
)
|
|
45
|
+
elif "DEADLINE_EXCEEDED" in error_msg or "timeout" in error_msg.lower():
|
|
46
|
+
return (
|
|
47
|
+
"Connection timeout. The device may be unresponsive. "
|
|
48
|
+
"Please check the device connection and try again."
|
|
49
|
+
)
|
|
50
|
+
elif "PERMISSION_DENIED" in error_msg:
|
|
51
|
+
return "Permission denied. Please check device permissions."
|
|
52
|
+
else:
|
|
53
|
+
return f"gRPC error: {error_msg}"
|
|
54
|
+
|
|
55
|
+
# For non-gRPC exceptions, return the error message as-is
|
|
56
|
+
return error_msg
|
|
57
|
+
|
|
58
|
+
|
|
59
|
+
async def calibrate_sensors(
|
|
60
|
+
port: str,
|
|
61
|
+
baudrate: int,
|
|
62
|
+
calibration_type: CalibrationType,
|
|
63
|
+
) -> AsyncGenerator[Dict[str, Any], None]:
|
|
64
|
+
"""
|
|
65
|
+
Calibrate PX4 sensors and stream progress as structured dict payloads.
|
|
66
|
+
|
|
67
|
+
This function is transport-agnostic: it does NOT produce SSE-formatted strings.
|
|
68
|
+
It yields plain Python dicts suitable for JSON serialization. The API layer
|
|
69
|
+
(FastAPI endpoint) is responsible for converting these dicts into SSE frames.
|
|
70
|
+
|
|
71
|
+
The function properly handles connection cleanup and gRPC errors.
|
|
72
|
+
"""
|
|
73
|
+
|
|
74
|
+
serial = f"serial://{port}:{baudrate}"
|
|
75
|
+
drone: Optional[System] = None
|
|
76
|
+
|
|
77
|
+
try:
|
|
78
|
+
# Initial status messages
|
|
79
|
+
yield {
|
|
80
|
+
"type": "status",
|
|
81
|
+
"message": f"Connecting to {port}...",
|
|
82
|
+
}
|
|
83
|
+
|
|
84
|
+
# Create and connect to the PX4 system
|
|
85
|
+
drone = System()
|
|
86
|
+
await drone.connect(system_address=serial)
|
|
87
|
+
|
|
88
|
+
# Wait for connection to be established
|
|
89
|
+
async for state in drone.core.connection_state():
|
|
90
|
+
if state.is_connected:
|
|
91
|
+
break
|
|
92
|
+
|
|
93
|
+
yield {
|
|
94
|
+
"type": "status",
|
|
95
|
+
"message": "Connected to device",
|
|
96
|
+
}
|
|
97
|
+
|
|
98
|
+
# Small delay to ensure connection is stable before starting calibration
|
|
99
|
+
# This helps avoid COMMAND_DENIED errors from rapid-fire requests
|
|
100
|
+
await asyncio.sleep(0.1)
|
|
101
|
+
|
|
102
|
+
# Dispatch based on calibration type
|
|
103
|
+
if calibration_type is CalibrationType.GYROSCOPE:
|
|
104
|
+
# Gyroscope calibration
|
|
105
|
+
yield {
|
|
106
|
+
"type": "status",
|
|
107
|
+
"message": "Starting gyroscope calibration...",
|
|
108
|
+
}
|
|
109
|
+
|
|
110
|
+
prev: Dict[str, Any] | None = None
|
|
111
|
+
try:
|
|
112
|
+
async for progress_data in drone.calibration.calibrate_gyro():
|
|
113
|
+
structured_data = parse_gyroscope_progress(progress_data, prev=prev)
|
|
114
|
+
prev = structured_data
|
|
115
|
+
yield structured_data
|
|
116
|
+
except (RpcError, Exception) as exc:
|
|
117
|
+
yield {
|
|
118
|
+
"type": "error",
|
|
119
|
+
"message": f"Gyroscope calibration failed: {_format_grpc_error(exc)}",
|
|
120
|
+
}
|
|
121
|
+
raise
|
|
122
|
+
|
|
123
|
+
yield {
|
|
124
|
+
"type": "gyroscope_complete",
|
|
125
|
+
"message": "Gyroscope calibration finished",
|
|
126
|
+
"is_complete": True,
|
|
127
|
+
}
|
|
128
|
+
|
|
129
|
+
elif calibration_type is CalibrationType.ACCELEROMETER:
|
|
130
|
+
# Accelerometer calibration
|
|
131
|
+
yield {
|
|
132
|
+
"type": "status",
|
|
133
|
+
"message": "Starting accelerometer calibration...",
|
|
134
|
+
}
|
|
135
|
+
|
|
136
|
+
prev = None
|
|
137
|
+
try:
|
|
138
|
+
async for progress_data in drone.calibration.calibrate_accelerometer():
|
|
139
|
+
structured_data = parse_accelerometer_progress(progress_data, prev=prev)
|
|
140
|
+
prev = structured_data
|
|
141
|
+
yield structured_data
|
|
142
|
+
except (RpcError, Exception) as exc:
|
|
143
|
+
yield {
|
|
144
|
+
"type": "error",
|
|
145
|
+
"message": f"Accelerometer calibration failed: {_format_grpc_error(exc)}",
|
|
146
|
+
}
|
|
147
|
+
raise
|
|
148
|
+
|
|
149
|
+
yield {
|
|
150
|
+
"type": "accelerometer_complete",
|
|
151
|
+
"message": "Accelerometer calibration finished",
|
|
152
|
+
"is_complete": True,
|
|
153
|
+
}
|
|
154
|
+
|
|
155
|
+
elif calibration_type is CalibrationType.MAGNETOMETER:
|
|
156
|
+
# Magnetometer calibration
|
|
157
|
+
yield {
|
|
158
|
+
"type": "status",
|
|
159
|
+
"message": "Starting magnetometer calibration...",
|
|
160
|
+
}
|
|
161
|
+
|
|
162
|
+
prev = None
|
|
163
|
+
try:
|
|
164
|
+
async for progress_data in drone.calibration.calibrate_magnetometer():
|
|
165
|
+
structured_data = parse_magnetometer_progress(progress_data, prev=prev)
|
|
166
|
+
prev = structured_data
|
|
167
|
+
yield structured_data
|
|
168
|
+
except (RpcError, Exception) as exc:
|
|
169
|
+
yield {
|
|
170
|
+
"type": "error",
|
|
171
|
+
"message": f"Magnetometer calibration failed: {_format_grpc_error(exc)}",
|
|
172
|
+
}
|
|
173
|
+
raise
|
|
174
|
+
|
|
175
|
+
yield {
|
|
176
|
+
"type": "magnetometer_complete",
|
|
177
|
+
"message": "Magnetometer calibration finished",
|
|
178
|
+
"is_complete": True,
|
|
179
|
+
}
|
|
180
|
+
|
|
181
|
+
elif calibration_type is CalibrationType.HORIZON:
|
|
182
|
+
# Level horizon calibration
|
|
183
|
+
yield {
|
|
184
|
+
"type": "status",
|
|
185
|
+
"message": "Starting board level horizon calibration...",
|
|
186
|
+
}
|
|
187
|
+
|
|
188
|
+
prev = None
|
|
189
|
+
try:
|
|
190
|
+
async for progress_data in drone.calibration.calibrate_level_horizon():
|
|
191
|
+
structured_data = parse_horizon_progress(progress_data, prev=prev)
|
|
192
|
+
prev = structured_data
|
|
193
|
+
yield structured_data
|
|
194
|
+
except (RpcError, Exception) as exc:
|
|
195
|
+
yield {
|
|
196
|
+
"type": "error",
|
|
197
|
+
"message": f"Horizon calibration failed: {_format_grpc_error(exc)}",
|
|
198
|
+
}
|
|
199
|
+
raise
|
|
200
|
+
|
|
201
|
+
yield {
|
|
202
|
+
"type": "horizon_complete",
|
|
203
|
+
"message": "Level horizon calibration finished",
|
|
204
|
+
"is_complete": True,
|
|
205
|
+
}
|
|
206
|
+
|
|
207
|
+
elif calibration_type is CalibrationType.ESC:
|
|
208
|
+
# ESC calibration not implemented yet
|
|
209
|
+
yield {
|
|
210
|
+
"type": "status",
|
|
211
|
+
"message": "Starting ESC calibration...",
|
|
212
|
+
}
|
|
213
|
+
yield {
|
|
214
|
+
"type": "warning",
|
|
215
|
+
"message": "ESC calibration not yet implemented",
|
|
216
|
+
}
|
|
217
|
+
yield {
|
|
218
|
+
"type": "status",
|
|
219
|
+
"message": "ESC calibration finished",
|
|
220
|
+
}
|
|
221
|
+
|
|
222
|
+
except (RpcError, Exception) as exc:
|
|
223
|
+
# Handle connection and gRPC errors
|
|
224
|
+
error_message = _format_grpc_error(exc)
|
|
225
|
+
yield {
|
|
226
|
+
"type": "error",
|
|
227
|
+
"message": f"Calibration failed: {error_message}",
|
|
228
|
+
}
|
|
229
|
+
raise
|
|
230
|
+
|
|
231
|
+
finally:
|
|
232
|
+
# Cleanup: Ensure the System object is properly released
|
|
233
|
+
# MAVSDK System objects are cleaned up when they go out of scope,
|
|
234
|
+
# but explicitly setting to None helps with resource management
|
|
235
|
+
if drone is not None:
|
|
236
|
+
drone = None
|
|
@@ -0,0 +1,215 @@
|
|
|
1
|
+
"""Service for PX4 health and calibration snapshot."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
import asyncio
|
|
6
|
+
from typing import Any, Dict, Optional
|
|
7
|
+
|
|
8
|
+
from mavsdk import System
|
|
9
|
+
|
|
10
|
+
try:
|
|
11
|
+
from grpc import RpcError
|
|
12
|
+
except ImportError:
|
|
13
|
+
# Fallback if grpc is not directly available (shouldn't happen with mavsdk)
|
|
14
|
+
RpcError = Exception
|
|
15
|
+
|
|
16
|
+
from ..config import settings
|
|
17
|
+
from ..models import (
|
|
18
|
+
Px4HealthResponse,
|
|
19
|
+
CalibrationHealth,
|
|
20
|
+
HealthAndArmingCheckReport,
|
|
21
|
+
HealthAndArmingCheckMode,
|
|
22
|
+
HealthAndArmingCheckProblem,
|
|
23
|
+
HealthComponentReport,
|
|
24
|
+
)
|
|
25
|
+
|
|
26
|
+
|
|
27
|
+
async def _get_stable_health(drone: System, duration_s: float = 2.0) -> Optional[Any]:
|
|
28
|
+
"""
|
|
29
|
+
Stream health messages for the specified duration and return the latest.
|
|
30
|
+
This skips initial stale messages (all False) and gets accurate values.
|
|
31
|
+
"""
|
|
32
|
+
latest_health = None
|
|
33
|
+
start_time = asyncio.get_event_loop().time()
|
|
34
|
+
|
|
35
|
+
try:
|
|
36
|
+
async for health in drone.telemetry.health():
|
|
37
|
+
latest_health = health
|
|
38
|
+
elapsed = asyncio.get_event_loop().time() - start_time
|
|
39
|
+
if elapsed >= duration_s:
|
|
40
|
+
break
|
|
41
|
+
except Exception:
|
|
42
|
+
pass
|
|
43
|
+
|
|
44
|
+
return latest_health
|
|
45
|
+
|
|
46
|
+
|
|
47
|
+
def _convert_arming_checks_report(report: Any) -> HealthAndArmingCheckReport:
|
|
48
|
+
"""Convert MAVSDK HealthAndArmingCheckReport to our Pydantic model."""
|
|
49
|
+
current = getattr(report, "current_mode_intention", None)
|
|
50
|
+
current_mode = None
|
|
51
|
+
if current is not None:
|
|
52
|
+
problems = [
|
|
53
|
+
HealthAndArmingCheckProblem(
|
|
54
|
+
message=getattr(p, "message", ""),
|
|
55
|
+
description=getattr(p, "description", ""),
|
|
56
|
+
health_component=getattr(p, "health_component", ""),
|
|
57
|
+
)
|
|
58
|
+
for p in (getattr(current, "problems", None) or [])
|
|
59
|
+
]
|
|
60
|
+
current_mode = HealthAndArmingCheckMode(
|
|
61
|
+
mode_name=getattr(current, "mode_name", ""),
|
|
62
|
+
can_arm_or_run=getattr(current, "can_arm_or_run", False),
|
|
63
|
+
problems=problems,
|
|
64
|
+
)
|
|
65
|
+
|
|
66
|
+
health_components = [
|
|
67
|
+
HealthComponentReport(
|
|
68
|
+
name=getattr(c, "name", ""),
|
|
69
|
+
label=getattr(c, "label", ""),
|
|
70
|
+
is_present=getattr(c, "is_present", False),
|
|
71
|
+
has_error=getattr(c, "has_error", False),
|
|
72
|
+
has_warning=getattr(c, "has_warning", False),
|
|
73
|
+
)
|
|
74
|
+
for c in (getattr(report, "health_components", None) or [])
|
|
75
|
+
]
|
|
76
|
+
|
|
77
|
+
all_problems = [
|
|
78
|
+
HealthAndArmingCheckProblem(
|
|
79
|
+
message=getattr(p, "message", ""),
|
|
80
|
+
description=getattr(p, "description", ""),
|
|
81
|
+
health_component=getattr(p, "health_component", ""),
|
|
82
|
+
)
|
|
83
|
+
for p in (getattr(report, "all_problems", None) or [])
|
|
84
|
+
]
|
|
85
|
+
|
|
86
|
+
return HealthAndArmingCheckReport(
|
|
87
|
+
current_mode_intention=current_mode,
|
|
88
|
+
health_components=health_components,
|
|
89
|
+
all_problems=all_problems,
|
|
90
|
+
)
|
|
91
|
+
|
|
92
|
+
|
|
93
|
+
async def _get_arming_checks_report(drone: System) -> Optional[HealthAndArmingCheckReport]:
|
|
94
|
+
"""
|
|
95
|
+
Fetch health and arming checks report from MAVSDK Events plugin.
|
|
96
|
+
Returns None if Events plugin is not available (e.g. older PX4).
|
|
97
|
+
"""
|
|
98
|
+
try:
|
|
99
|
+
report = await drone.events.get_health_and_arming_checks_report()
|
|
100
|
+
if report is None:
|
|
101
|
+
return None
|
|
102
|
+
return _convert_arming_checks_report(report)
|
|
103
|
+
except Exception:
|
|
104
|
+
return None
|
|
105
|
+
|
|
106
|
+
|
|
107
|
+
async def read_px4_health(
|
|
108
|
+
port: Optional[str] = None,
|
|
109
|
+
baudrate: Optional[int] = None,
|
|
110
|
+
timeout_s: float = 10.0,
|
|
111
|
+
) -> Px4HealthResponse:
|
|
112
|
+
"""Connect to PX4 via MAVSDK and return a health snapshot using telemetry health flags."""
|
|
113
|
+
|
|
114
|
+
port = port or settings.default_port
|
|
115
|
+
baudrate = baudrate or settings.default_baudrate
|
|
116
|
+
serial = f"serial://{port}:{baudrate}"
|
|
117
|
+
|
|
118
|
+
drone: Optional[System] = None
|
|
119
|
+
|
|
120
|
+
try:
|
|
121
|
+
drone = System()
|
|
122
|
+
await drone.connect(system_address=serial)
|
|
123
|
+
|
|
124
|
+
# Wait for connection with timeout
|
|
125
|
+
connection_established = False
|
|
126
|
+
try:
|
|
127
|
+
async for state in drone.core.connection_state():
|
|
128
|
+
if state.is_connected:
|
|
129
|
+
connection_established = True
|
|
130
|
+
break
|
|
131
|
+
# Timeout if not connected within reasonable time
|
|
132
|
+
await asyncio.sleep(0.1)
|
|
133
|
+
except (RpcError, Exception):
|
|
134
|
+
connection_established = False
|
|
135
|
+
|
|
136
|
+
if not connection_established:
|
|
137
|
+
return Px4HealthResponse(
|
|
138
|
+
status="disconnected",
|
|
139
|
+
connection_port=port,
|
|
140
|
+
connection_baudrate=baudrate,
|
|
141
|
+
calibration=None,
|
|
142
|
+
gps_fix_type=None,
|
|
143
|
+
satellites_used=None,
|
|
144
|
+
armable=False,
|
|
145
|
+
arming_checks_report=None,
|
|
146
|
+
raw_health=None,
|
|
147
|
+
)
|
|
148
|
+
|
|
149
|
+
# Stream health for 2 seconds to get accurate values (skip initial stale messages)
|
|
150
|
+
health = await asyncio.wait_for(_get_stable_health(drone, duration_s=2.0), timeout_s)
|
|
151
|
+
|
|
152
|
+
if health is None:
|
|
153
|
+
return Px4HealthResponse(
|
|
154
|
+
status="disconnected",
|
|
155
|
+
connection_port=port,
|
|
156
|
+
connection_baudrate=baudrate,
|
|
157
|
+
calibration=None,
|
|
158
|
+
gps_fix_type=None,
|
|
159
|
+
satellites_used=None,
|
|
160
|
+
armable=False,
|
|
161
|
+
arming_checks_report=None,
|
|
162
|
+
raw_health=None,
|
|
163
|
+
)
|
|
164
|
+
|
|
165
|
+
# Use telemetry health flags directly
|
|
166
|
+
calibration = CalibrationHealth(
|
|
167
|
+
gyroscope_ok=health.is_gyrometer_calibration_ok,
|
|
168
|
+
accelerometer_ok=health.is_accelerometer_calibration_ok,
|
|
169
|
+
magnetometer_ok=health.is_magnetometer_calibration_ok,
|
|
170
|
+
)
|
|
171
|
+
|
|
172
|
+
# Determine status
|
|
173
|
+
status = "ok" if (health.is_armable and not calibration.requires_calibration) else "degraded"
|
|
174
|
+
|
|
175
|
+
# Minimal raw health for debugging
|
|
176
|
+
raw_health: Dict[str, Any] = {
|
|
177
|
+
"raw_health_message": str(health),
|
|
178
|
+
"is_gyrometer_calibration_ok": health.is_gyrometer_calibration_ok,
|
|
179
|
+
"is_accelerometer_calibration_ok": health.is_accelerometer_calibration_ok,
|
|
180
|
+
"is_magnetometer_calibration_ok": health.is_magnetometer_calibration_ok,
|
|
181
|
+
"is_armable": health.is_armable,
|
|
182
|
+
}
|
|
183
|
+
|
|
184
|
+
# Fetch detailed health and arming checks from Events plugin (optional)
|
|
185
|
+
arming_checks_report = await _get_arming_checks_report(drone)
|
|
186
|
+
|
|
187
|
+
return Px4HealthResponse(
|
|
188
|
+
status=status,
|
|
189
|
+
connection_port=port,
|
|
190
|
+
connection_baudrate=baudrate,
|
|
191
|
+
calibration=calibration,
|
|
192
|
+
gps_fix_type=None,
|
|
193
|
+
satellites_used=None,
|
|
194
|
+
armable=health.is_armable,
|
|
195
|
+
arming_checks_report=arming_checks_report,
|
|
196
|
+
raw_health=raw_health,
|
|
197
|
+
)
|
|
198
|
+
|
|
199
|
+
except (RpcError, Exception):
|
|
200
|
+
# Return disconnected status on any error
|
|
201
|
+
return Px4HealthResponse(
|
|
202
|
+
status="disconnected",
|
|
203
|
+
connection_port=port,
|
|
204
|
+
connection_baudrate=baudrate,
|
|
205
|
+
calibration=None,
|
|
206
|
+
gps_fix_type=None,
|
|
207
|
+
satellites_used=None,
|
|
208
|
+
armable=False,
|
|
209
|
+
arming_checks_report=None,
|
|
210
|
+
raw_health=None,
|
|
211
|
+
)
|
|
212
|
+
finally:
|
|
213
|
+
# Cleanup: Ensure the System object is properly released
|
|
214
|
+
if drone is not None:
|
|
215
|
+
drone = None
|