Skip to content

Commit 445e39e

Browse files
Update submodule, adding distance sensor to telemetry
1 parent 5e3ce79 commit 445e39e

File tree

10 files changed

+1327
-234
lines changed

10 files changed

+1327
-234
lines changed

mavsdk/action.py

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -368,6 +368,28 @@ async def shutdown(self):
368368
raise ActionError(result, "shutdown()")
369369

370370

371+
async def terminate(self):
372+
"""
373+
Send command to terminate the drone.
374+
375+
This will run the terminate routine as configured on the drone (e.g. disarm and open the parachute).
376+
377+
Raises
378+
------
379+
ActionError
380+
If the request fails. The error contains the reason for the failure.
381+
"""
382+
383+
request = action_pb2.TerminateRequest()
384+
response = await self._stub.Terminate(request)
385+
386+
387+
result = self._extract_result(response)
388+
389+
if result.result is not ActionResult.Result.SUCCESS:
390+
raise ActionError(result, "terminate()")
391+
392+
371393
async def kill(self):
372394
"""
373395
Send command to kill the drone.

mavsdk/action_pb2.py

Lines changed: 146 additions & 62 deletions
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

mavsdk/action_pb2_grpc.py

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,11 @@ def __init__(self, channel):
4545
request_serializer=action_dot_action__pb2.ShutdownRequest.SerializeToString,
4646
response_deserializer=action_dot_action__pb2.ShutdownResponse.FromString,
4747
)
48+
self.Terminate = channel.unary_unary(
49+
'/mavsdk.rpc.action.ActionService/Terminate',
50+
request_serializer=action_dot_action__pb2.TerminateRequest.SerializeToString,
51+
response_deserializer=action_dot_action__pb2.TerminateResponse.FromString,
52+
)
4853
self.Kill = channel.unary_unary(
4954
'/mavsdk.rpc.action.ActionService/Kill',
5055
request_serializer=action_dot_action__pb2.KillRequest.SerializeToString,
@@ -173,6 +178,16 @@ def Shutdown(self, request, context):
173178
context.set_details('Method not implemented!')
174179
raise NotImplementedError('Method not implemented!')
175180

181+
def Terminate(self, request, context):
182+
"""
183+
Send command to terminate the drone.
184+
185+
This will run the terminate routine as configured on the drone (e.g. disarm and open the parachute).
186+
"""
187+
context.set_code(grpc.StatusCode.UNIMPLEMENTED)
188+
context.set_details('Method not implemented!')
189+
raise NotImplementedError('Method not implemented!')
190+
176191
def Kill(self, request, context):
177192
"""
178193
Send command to kill the drone.
@@ -314,6 +329,11 @@ def add_ActionServiceServicer_to_server(servicer, server):
314329
request_deserializer=action_dot_action__pb2.ShutdownRequest.FromString,
315330
response_serializer=action_dot_action__pb2.ShutdownResponse.SerializeToString,
316331
),
332+
'Terminate': grpc.unary_unary_rpc_method_handler(
333+
servicer.Terminate,
334+
request_deserializer=action_dot_action__pb2.TerminateRequest.FromString,
335+
response_serializer=action_dot_action__pb2.TerminateResponse.SerializeToString,
336+
),
317337
'Kill': grpc.unary_unary_rpc_method_handler(
318338
servicer.Kill,
319339
request_deserializer=action_dot_action__pb2.KillRequest.FromString,
@@ -476,6 +496,22 @@ def Shutdown(request,
476496
options, channel_credentials,
477497
call_credentials, compression, wait_for_ready, timeout, metadata)
478498

499+
@staticmethod
500+
def Terminate(request,
501+
target,
502+
options=(),
503+
channel_credentials=None,
504+
call_credentials=None,
505+
compression=None,
506+
wait_for_ready=None,
507+
timeout=None,
508+
metadata=None):
509+
return grpc.experimental.unary_unary(request, target, '/mavsdk.rpc.action.ActionService/Terminate',
510+
action_dot_action__pb2.TerminateRequest.SerializeToString,
511+
action_dot_action__pb2.TerminateResponse.FromString,
512+
options, channel_credentials,
513+
call_credentials, compression, wait_for_ready, timeout, metadata)
514+
479515
@staticmethod
480516
def Kill(request,
481517
target,

mavsdk/param.py

Lines changed: 253 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,238 @@
66
from enum import Enum
77

88

9+
class IntParam:
10+
"""
11+
Type for integer parameters.
12+
13+
Parameters
14+
----------
15+
name : std::string
16+
Name of the parameter
17+
18+
value : int32_t
19+
Value of the parameter
20+
21+
"""
22+
23+
24+
25+
def __init__(
26+
self,
27+
name,
28+
value):
29+
""" Initializes the IntParam object """
30+
self.name = name
31+
self.value = value
32+
33+
def __equals__(self, to_compare):
34+
""" Checks if two IntParam are the same """
35+
try:
36+
# Try to compare - this likely fails when it is compared to a non
37+
# IntParam object
38+
return \
39+
(self.name == to_compare.name) and \
40+
(self.value == to_compare.value)
41+
42+
except AttributeError:
43+
return False
44+
45+
def __str__(self):
46+
""" IntParam in string representation """
47+
struct_repr = ", ".join([
48+
"name: " + str(self.name),
49+
"value: " + str(self.value)
50+
])
51+
52+
return f"IntParam: [{struct_repr}]"
53+
54+
@staticmethod
55+
def translate_from_rpc(rpcIntParam):
56+
""" Translates a gRPC struct to the SDK equivalent """
57+
return IntParam(
58+
59+
rpcIntParam.name,
60+
61+
62+
rpcIntParam.value
63+
)
64+
65+
def translate_to_rpc(self, rpcIntParam):
66+
""" Translates this SDK object into its gRPC equivalent """
67+
68+
69+
70+
71+
rpcIntParam.name = self.name
72+
73+
74+
75+
76+
77+
rpcIntParam.value = self.value
78+
79+
80+
81+
82+
83+
class FloatParam:
84+
"""
85+
Type for float paramters.
86+
87+
Parameters
88+
----------
89+
name : std::string
90+
Name of the parameter
91+
92+
value : float
93+
Value of the parameter
94+
95+
"""
96+
97+
98+
99+
def __init__(
100+
self,
101+
name,
102+
value):
103+
""" Initializes the FloatParam object """
104+
self.name = name
105+
self.value = value
106+
107+
def __equals__(self, to_compare):
108+
""" Checks if two FloatParam are the same """
109+
try:
110+
# Try to compare - this likely fails when it is compared to a non
111+
# FloatParam object
112+
return \
113+
(self.name == to_compare.name) and \
114+
(self.value == to_compare.value)
115+
116+
except AttributeError:
117+
return False
118+
119+
def __str__(self):
120+
""" FloatParam in string representation """
121+
struct_repr = ", ".join([
122+
"name: " + str(self.name),
123+
"value: " + str(self.value)
124+
])
125+
126+
return f"FloatParam: [{struct_repr}]"
127+
128+
@staticmethod
129+
def translate_from_rpc(rpcFloatParam):
130+
""" Translates a gRPC struct to the SDK equivalent """
131+
return FloatParam(
132+
133+
rpcFloatParam.name,
134+
135+
136+
rpcFloatParam.value
137+
)
138+
139+
def translate_to_rpc(self, rpcFloatParam):
140+
""" Translates this SDK object into its gRPC equivalent """
141+
142+
143+
144+
145+
rpcFloatParam.name = self.name
146+
147+
148+
149+
150+
151+
rpcFloatParam.value = self.value
152+
153+
154+
155+
156+
157+
class AllParams:
158+
"""
159+
Type collecting all integer and float parameters.
160+
161+
Parameters
162+
----------
163+
int_params : [IntParam]
164+
Collection of all parameter names and values of type int
165+
166+
float_params : [FloatParam]
167+
Collection of all parameter names and values of type float
168+
169+
"""
170+
171+
172+
173+
def __init__(
174+
self,
175+
int_params,
176+
float_params):
177+
""" Initializes the AllParams object """
178+
self.int_params = int_params
179+
self.float_params = float_params
180+
181+
def __equals__(self, to_compare):
182+
""" Checks if two AllParams are the same """
183+
try:
184+
# Try to compare - this likely fails when it is compared to a non
185+
# AllParams object
186+
return \
187+
(self.int_params == to_compare.int_params) and \
188+
(self.float_params == to_compare.float_params)
189+
190+
except AttributeError:
191+
return False
192+
193+
def __str__(self):
194+
""" AllParams in string representation """
195+
struct_repr = ", ".join([
196+
"int_params: " + str(self.int_params),
197+
"float_params: " + str(self.float_params)
198+
])
199+
200+
return f"AllParams: [{struct_repr}]"
201+
202+
@staticmethod
203+
def translate_from_rpc(rpcAllParams):
204+
""" Translates a gRPC struct to the SDK equivalent """
205+
return AllParams(
206+
207+
map(lambda elem: IntParam.translate_from_rpc(elem), rpcAllParams.int_params),
208+
209+
210+
map(lambda elem: FloatParam.translate_from_rpc(elem), rpcAllParams.float_params)
211+
)
212+
213+
def translate_to_rpc(self, rpcAllParams):
214+
""" Translates this SDK object into its gRPC equivalent """
215+
216+
217+
218+
219+
rpc_elems_list = []
220+
for elem in self.int_params:
221+
rpc_elem = param_pb2.IntParam()
222+
elem.translate_to_rpc(rpc_elem)
223+
rpc_elems_list.append(rpc_elem)
224+
rpcAllParams.int_params.extend(rpc_elems_list)
225+
226+
227+
228+
229+
230+
rpc_elems_list = []
231+
for elem in self.float_params:
232+
rpc_elem = param_pb2.FloatParam()
233+
elem.translate_to_rpc(rpc_elem)
234+
rpc_elems_list.append(rpc_elem)
235+
rpcAllParams.float_params.extend(rpc_elems_list)
236+
237+
238+
239+
240+
9241
class ParamResult:
10242
"""
11243
Result type.
@@ -321,4 +553,24 @@ async def set_param_float(self, name, value):
321553

322554
if result.result is not ParamResult.Result.SUCCESS:
323555
raise ParamError(result, "set_param_float()", name, value)
324-
556+
557+
558+
async def get_all_params(self):
559+
"""
560+
Get all parameters.
561+
562+
Returns
563+
-------
564+
params : AllParams
565+
Collection of all parameters
566+
567+
568+
"""
569+
570+
request = param_pb2.GetAllParamsRequest()
571+
response = await self._stub.GetAllParams(request)
572+
573+
574+
575+
return AllParams.translate_from_rpc(response.params)
576+

0 commit comments

Comments
 (0)