Skip to content

Commit f821070

Browse files
authored
Attempt to set parameters 3 times by default (#1037)
* Attempt to set parameters 3 times by default * Address copilot review comments * Update param type
1 parent 778849f commit f821070

2 files changed

Lines changed: 16 additions & 8 deletions

File tree

radio/app/controllers/flightModesController.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -110,7 +110,7 @@ def setFlightMode(self, mode_number: int, flight_mode: int) -> Response:
110110
}
111111
mode_name = mavutil.mavlink.enums["COPTER_MODE"][flight_mode].name
112112

113-
param_type = 2
113+
param_type = mavutil.mavlink.MAV_PARAM_TYPE_UINT8
114114
param_set_success = self.drone.paramsController.setParam(
115115
f"FLTMODE{mode_number}", flight_mode, param_type
116116
)
@@ -145,7 +145,7 @@ def setFlightModeChannel(self, channel: int) -> Response:
145145
"message": f"Invalid flight mode channel, must be between 1 and 16 inclusive, got {channel}.",
146146
}
147147

148-
param_type = 2
148+
param_type = mavutil.mavlink.MAV_PARAM_TYPE_UINT8
149149
param_set_success = self.drone.paramsController.setParam(
150150
"FLTMODE_CH", channel, param_type
151151
)

radio/app/controllers/paramsController.py

Lines changed: 14 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -320,10 +320,16 @@ def setParam(
320320
self.drone.logger.error("Could not reserve PARAM_VALUE messages")
321321
return False
322322

323+
# Ensure we try at least once
324+
if retries <= 0:
325+
retries = 1
326+
327+
retries_remaining = retries
328+
323329
try:
324330
# Keep trying to set the parameter until we get an ack or run out of retries or timeout
325-
while retries > 0 and not got_ack:
326-
retries -= 1
331+
while retries_remaining > 0 and not got_ack:
332+
retries_remaining -= 1
327333
self.drone.master.param_set_send(
328334
param_name.upper(), vfloat, parm_type=param_type
329335
)
@@ -347,13 +353,13 @@ def setParam(
347353
elif abs(ack.param_value - param_value) > 0.0001:
348354
# Use a small tolerance for float comparison
349355
self.drone.logger.warning(
350-
f"Could not set {param_name} to {param_value}, keeping value as {ack.param_value} instead"
356+
f"Could not set {param_name} to {param_value}, keeping value as {ack.param_value} instead. Trying again {retries_remaining} times."
351357
)
352358
self.drone.logger.debug(
353359
f"Ack: {ack.to_dict()}, param_name: {param_name}, param_value: {param_value}"
354360
)
355-
saved_param = False
356-
break
361+
got_ack = False
362+
continue
357363
else:
358364
self.drone.logger.debug(
359365
f"Got parameter saving ack for {param_name} for value {param_value}"
@@ -363,7 +369,9 @@ def setParam(
363369
break
364370

365371
if not got_ack:
366-
self.drone.logger.error(f"timeout setting {param_name} to {vfloat}")
372+
self.drone.logger.error(
373+
f"Timeout setting {param_name} to {vfloat}, attempted {retries} times."
374+
)
367375

368376
return saved_param
369377
except serial.serialutil.SerialException:

0 commit comments

Comments
 (0)