PX4 Firmware
PX4 Autopilot Software http://px4.io
parameters.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2016 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file parameters.cpp
36  *
37  * @author Beat Kueng <beat-kueng@gmx.net>
38  */
39 
40 #include "parameters.h"
41 
42 namespace RCUpdate
43 {
44 
45 void initialize_parameter_handles(ParameterHandles &parameter_handles)
46 {
47  /* basic r/c parameters */
48  for (unsigned i = 0; i < RC_MAX_CHAN_COUNT; i++) {
49  char nbuf[16];
50 
51  /* min values */
52  sprintf(nbuf, "RC%d_MIN", i + 1);
53  parameter_handles.min[i] = param_find(nbuf);
54 
55  /* trim values */
56  sprintf(nbuf, "RC%d_TRIM", i + 1);
57  parameter_handles.trim[i] = param_find(nbuf);
58 
59  /* max values */
60  sprintf(nbuf, "RC%d_MAX", i + 1);
61  parameter_handles.max[i] = param_find(nbuf);
62 
63  /* channel reverse */
64  sprintf(nbuf, "RC%d_REV", i + 1);
65  parameter_handles.rev[i] = param_find(nbuf);
66 
67  /* channel deadzone */
68  sprintf(nbuf, "RC%d_DZ", i + 1);
69  parameter_handles.dz[i] = param_find(nbuf);
70 
71  }
72 
73  /* mandatory input switched, mapped to channels 1-4 per default */
74  parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL");
75  parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH");
76  parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW");
77  parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE");
78  parameter_handles.rc_map_failsafe = param_find("RC_MAP_FAILSAFE");
79 
80  /* mandatory mode switches, mapped to channel 5 and 6 per default */
81  parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW");
82  parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW");
83 
84  parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");
85 
86  /* optional mode switches, not mapped per default */
87  parameter_handles.rc_map_rattitude_sw = param_find("RC_MAP_RATT_SW");
88  parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW");
89  parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW");
90  parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW");
91  parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW");
92  parameter_handles.rc_map_kill_sw = param_find("RC_MAP_KILL_SW");
93  parameter_handles.rc_map_arm_sw = param_find("RC_MAP_ARM_SW");
94  parameter_handles.rc_map_trans_sw = param_find("RC_MAP_TRANS_SW");
95  parameter_handles.rc_map_gear_sw = param_find("RC_MAP_GEAR_SW");
96  parameter_handles.rc_map_stab_sw = param_find("RC_MAP_STAB_SW");
97  parameter_handles.rc_map_man_sw = param_find("RC_MAP_MAN_SW");
98 
99  parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1");
100  parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
101  parameter_handles.rc_map_aux3 = param_find("RC_MAP_AUX3");
102  parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4");
103  parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5");
104  parameter_handles.rc_map_aux6 = param_find("RC_MAP_AUX6");
105 
106  /* RC to parameter mapping for changing parameters with RC */
107  for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
108  char name[rc_parameter_map_s::PARAM_ID_LEN];
109  snprintf(name, rc_parameter_map_s::PARAM_ID_LEN, "RC_MAP_PARAM%d",
110  i + 1); // shifted by 1 because param name starts at 1
111  parameter_handles.rc_map_param[i] = param_find(name);
112  }
113 
114  parameter_handles.rc_map_flightmode = param_find("RC_MAP_FLTMODE");
115 
116  /* RC thresholds */
117  parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR");
118  parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH");
119  parameter_handles.rc_auto_th = param_find("RC_AUTO_TH");
120  parameter_handles.rc_rattitude_th = param_find("RC_RATT_TH");
121  parameter_handles.rc_posctl_th = param_find("RC_POSCTL_TH");
122  parameter_handles.rc_return_th = param_find("RC_RETURN_TH");
123  parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH");
124  parameter_handles.rc_acro_th = param_find("RC_ACRO_TH");
125  parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH");
126  parameter_handles.rc_killswitch_th = param_find("RC_KILLSWITCH_TH");
127  parameter_handles.rc_armswitch_th = param_find("RC_ARMSWITCH_TH");
128  parameter_handles.rc_trans_th = param_find("RC_TRANS_TH");
129  parameter_handles.rc_gear_th = param_find("RC_GEAR_TH");
130  parameter_handles.rc_stab_th = param_find("RC_STAB_TH");
131  parameter_handles.rc_man_th = param_find("RC_MAN_TH");
132 
133  /* RC low pass filter configuration */
134  parameter_handles.rc_flt_smp_rate = param_find("RC_FLT_SMP_RATE");
135  parameter_handles.rc_flt_cutoff = param_find("RC_FLT_CUTOFF");
136 
137  // These are parameters for which QGroundControl always expects to be returned in a list request.
138  // We do a param_find here to force them into the list.
139  (void)param_find("RC_CHAN_CNT");
140 }
141 
142 int update_parameters(const ParameterHandles &parameter_handles, Parameters &parameters)
143 {
144  bool rc_valid = true;
145  float tmpScaleFactor = 0.0f;
146  float tmpRevFactor = 0.0f;
147  int ret = PX4_OK;
148 
149  /* rc values */
150  for (unsigned int i = 0; i < RC_MAX_CHAN_COUNT; i++) {
151 
152  param_get(parameter_handles.min[i], &(parameters.min[i]));
153  param_get(parameter_handles.trim[i], &(parameters.trim[i]));
154  param_get(parameter_handles.max[i], &(parameters.max[i]));
155  param_get(parameter_handles.rev[i], &(parameters.rev[i]));
156  param_get(parameter_handles.dz[i], &(parameters.dz[i]));
157 
158  tmpScaleFactor = (1.0f / ((parameters.max[i] - parameters.min[i]) / 2.0f) * parameters.rev[i]);
159  tmpRevFactor = tmpScaleFactor * parameters.rev[i];
160 
161  /* handle blowup in the scaling factor calculation */
162  if (!PX4_ISFINITE(tmpScaleFactor) ||
163  (tmpRevFactor < 0.000001f) ||
164  (tmpRevFactor > 0.2f)) {
165  PX4_WARN("RC chan %u not sane, scaling: %8.6f, rev: %d", i, (double)tmpScaleFactor, (int)(parameters.rev[i]));
166  /* scaling factors do not make sense, lock them down */
167  parameters.scaling_factor[i] = 0.0f;
168  rc_valid = false;
169 
170  } else {
171  parameters.scaling_factor[i] = tmpScaleFactor;
172  }
173  }
174 
175  /* handle wrong values */
176  if (!rc_valid) {
177  PX4_ERR("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n");
178  }
179 
180  const char *paramerr = "FAIL PARM LOAD";
181 
182  /* channel mapping */
183  if (param_get(parameter_handles.rc_map_roll, &(parameters.rc_map_roll)) != OK) {
184  PX4_WARN("%s", paramerr);
185  }
186 
187  if (param_get(parameter_handles.rc_map_pitch, &(parameters.rc_map_pitch)) != OK) {
188  PX4_WARN("%s", paramerr);
189  }
190 
191  if (param_get(parameter_handles.rc_map_yaw, &(parameters.rc_map_yaw)) != OK) {
192  PX4_WARN("%s", paramerr);
193  }
194 
195  if (param_get(parameter_handles.rc_map_throttle, &(parameters.rc_map_throttle)) != OK) {
196  PX4_WARN("%s", paramerr);
197  }
198 
199  if (param_get(parameter_handles.rc_map_failsafe, &(parameters.rc_map_failsafe)) != OK) {
200  PX4_WARN("%s", paramerr);
201  }
202 
203  if (param_get(parameter_handles.rc_map_mode_sw, &(parameters.rc_map_mode_sw)) != OK) {
204  PX4_WARN("%s", paramerr);
205  }
206 
207  if (param_get(parameter_handles.rc_map_return_sw, &(parameters.rc_map_return_sw)) != OK) {
208  PX4_WARN("%s", paramerr);
209  }
210 
211  if (param_get(parameter_handles.rc_map_rattitude_sw, &(parameters.rc_map_rattitude_sw)) != OK) {
212  PX4_WARN("%s", paramerr);
213  }
214 
215  if (param_get(parameter_handles.rc_map_posctl_sw, &(parameters.rc_map_posctl_sw)) != OK) {
216  PX4_WARN("%s", paramerr);
217  }
218 
219  if (param_get(parameter_handles.rc_map_loiter_sw, &(parameters.rc_map_loiter_sw)) != OK) {
220  PX4_WARN("%s", paramerr);
221  }
222 
223  if (param_get(parameter_handles.rc_map_acro_sw, &(parameters.rc_map_acro_sw)) != OK) {
224  PX4_WARN("%s", paramerr);
225  }
226 
227  if (param_get(parameter_handles.rc_map_offboard_sw, &(parameters.rc_map_offboard_sw)) != OK) {
228  PX4_WARN("%s", paramerr);
229  }
230 
231  if (param_get(parameter_handles.rc_map_kill_sw, &(parameters.rc_map_kill_sw)) != OK) {
232  PX4_WARN("%s", paramerr);
233  }
234 
235  if (param_get(parameter_handles.rc_map_arm_sw, &(parameters.rc_map_arm_sw)) != OK) {
236  PX4_WARN("%s", paramerr);
237  }
238 
239  if (param_get(parameter_handles.rc_map_trans_sw, &(parameters.rc_map_trans_sw)) != OK) {
240  PX4_WARN("%s", paramerr);
241  }
242 
243  if (param_get(parameter_handles.rc_map_flaps, &(parameters.rc_map_flaps)) != OK) {
244  PX4_WARN("%s", paramerr);
245  }
246 
247  if (param_get(parameter_handles.rc_map_gear_sw, &(parameters.rc_map_gear_sw)) != OK) {
248  PX4_WARN("%s", paramerr);
249  }
250 
251  if (param_get(parameter_handles.rc_map_stab_sw, &(parameters.rc_map_stab_sw)) != OK) {
252  PX4_WARN("%s", paramerr);
253  }
254 
255  if (param_get(parameter_handles.rc_map_man_sw, &(parameters.rc_map_man_sw)) != OK) {
256  PX4_WARN("%s", paramerr);
257  }
258 
259  param_get(parameter_handles.rc_map_aux1, &(parameters.rc_map_aux1));
260  param_get(parameter_handles.rc_map_aux2, &(parameters.rc_map_aux2));
261  param_get(parameter_handles.rc_map_aux3, &(parameters.rc_map_aux3));
262  param_get(parameter_handles.rc_map_aux4, &(parameters.rc_map_aux4));
263  param_get(parameter_handles.rc_map_aux5, &(parameters.rc_map_aux5));
264  param_get(parameter_handles.rc_map_aux6, &(parameters.rc_map_aux6));
265 
266  for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
267  param_get(parameter_handles.rc_map_param[i], &(parameters.rc_map_param[i]));
268  }
269 
270  param_get(parameter_handles.rc_map_flightmode, &(parameters.rc_map_flightmode));
271 
272  param_get(parameter_handles.rc_fails_thr, &(parameters.rc_fails_thr));
273  param_get(parameter_handles.rc_assist_th, &(parameters.rc_assist_th));
274  parameters.rc_assist_inv = (parameters.rc_assist_th < 0);
275  parameters.rc_assist_th = fabsf(parameters.rc_assist_th);
276  param_get(parameter_handles.rc_auto_th, &(parameters.rc_auto_th));
277  parameters.rc_auto_inv = (parameters.rc_auto_th < 0);
278  parameters.rc_auto_th = fabsf(parameters.rc_auto_th);
279  param_get(parameter_handles.rc_rattitude_th, &(parameters.rc_rattitude_th));
280  parameters.rc_rattitude_inv = (parameters.rc_rattitude_th < 0);
281  parameters.rc_rattitude_th = fabsf(parameters.rc_rattitude_th);
282  param_get(parameter_handles.rc_posctl_th, &(parameters.rc_posctl_th));
283  parameters.rc_posctl_inv = (parameters.rc_posctl_th < 0);
284  parameters.rc_posctl_th = fabsf(parameters.rc_posctl_th);
285  param_get(parameter_handles.rc_return_th, &(parameters.rc_return_th));
286  parameters.rc_return_inv = (parameters.rc_return_th < 0);
287  parameters.rc_return_th = fabsf(parameters.rc_return_th);
288  param_get(parameter_handles.rc_loiter_th, &(parameters.rc_loiter_th));
289  parameters.rc_loiter_inv = (parameters.rc_loiter_th < 0);
290  parameters.rc_loiter_th = fabsf(parameters.rc_loiter_th);
291  param_get(parameter_handles.rc_acro_th, &(parameters.rc_acro_th));
292  parameters.rc_acro_inv = (parameters.rc_acro_th < 0);
293  parameters.rc_acro_th = fabsf(parameters.rc_acro_th);
294  param_get(parameter_handles.rc_offboard_th, &(parameters.rc_offboard_th));
295  parameters.rc_offboard_inv = (parameters.rc_offboard_th < 0);
296  parameters.rc_offboard_th = fabsf(parameters.rc_offboard_th);
297  param_get(parameter_handles.rc_killswitch_th, &(parameters.rc_killswitch_th));
298  parameters.rc_killswitch_inv = (parameters.rc_killswitch_th < 0);
299  parameters.rc_killswitch_th = fabsf(parameters.rc_killswitch_th);
300  param_get(parameter_handles.rc_armswitch_th, &(parameters.rc_armswitch_th));
301  parameters.rc_armswitch_inv = (parameters.rc_armswitch_th < 0);
302  parameters.rc_armswitch_th = fabsf(parameters.rc_armswitch_th);
303  param_get(parameter_handles.rc_trans_th, &(parameters.rc_trans_th));
304  parameters.rc_trans_inv = (parameters.rc_trans_th < 0);
305  parameters.rc_trans_th = fabsf(parameters.rc_trans_th);
306  param_get(parameter_handles.rc_gear_th, &(parameters.rc_gear_th));
307  parameters.rc_gear_inv = (parameters.rc_gear_th < 0);
308  parameters.rc_gear_th = fabsf(parameters.rc_gear_th);
309  param_get(parameter_handles.rc_stab_th, &(parameters.rc_stab_th));
310  parameters.rc_stab_inv = (parameters.rc_stab_th < 0);
311  parameters.rc_stab_th = fabsf(parameters.rc_stab_th);
312  param_get(parameter_handles.rc_man_th, &(parameters.rc_man_th));
313  parameters.rc_man_inv = (parameters.rc_man_th < 0);
314  parameters.rc_man_th = fabsf(parameters.rc_man_th);
315 
316  param_get(parameter_handles.rc_flt_smp_rate, &(parameters.rc_flt_smp_rate));
317  parameters.rc_flt_smp_rate = math::max(1.0f, parameters.rc_flt_smp_rate);
318  param_get(parameter_handles.rc_flt_cutoff, &(parameters.rc_flt_cutoff));
319  /* make sure the filter is in its stable region -> fc < fs/2 */
320  parameters.rc_flt_cutoff = math::min(parameters.rc_flt_cutoff, (parameters.rc_flt_smp_rate / 2) - 1.f);
321 
322  return ret;
323 }
324 
325 } /* namespace RCUpdate */
RCUpdate::RC_MAX_CHAN_COUNT
static constexpr unsigned RC_MAX_CHAN_COUNT
maximum number of r/c channels we handle
Definition: parameters.h:54
RCUpdate::Parameters::max
float max[RC_MAX_CHAN_COUNT]
Definition: parameters.h:60
RCUpdate::Parameters::min
float min[RC_MAX_CHAN_COUNT]
Definition: parameters.h:58
RCUpdate::ParameterHandles::dz
param_t dz[RC_MAX_CHAN_COUNT]
Definition: parameters.h:137
RCUpdate::ParameterHandles::max
param_t max[RC_MAX_CHAN_COUNT]
Definition: parameters.h:135
RCUpdate::Parameters::dz
float dz[RC_MAX_CHAN_COUNT]
Definition: parameters.h:62
param_get
int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition: parameters.cpp:589
RCUpdate::Parameters::trim
float trim[RC_MAX_CHAN_COUNT]
Definition: parameters.h:59
RCUpdate::Parameters::rc_map_posctl_sw
int32_t rc_map_posctl_sw
Definition: parameters.h:74
RCUpdate::Parameters::rc_map_return_sw
int32_t rc_map_return_sw
Definition: parameters.h:72
RCUpdate::Parameters::rc_map_mode_sw
int32_t rc_map_mode_sw
Definition: parameters.h:71
RCUpdate::Parameters::rc_map_param
int32_t rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]
Definition: parameters.h:93
parameters.h
RCUpdate::ParameterHandles::rev
param_t rev[RC_MAX_CHAN_COUNT]
Definition: parameters.h:136
RCUpdate::Parameters::rc_map_rattitude_sw
int32_t rc_map_rattitude_sw
Definition: parameters.h:73
RCUpdate::update_parameters
int update_parameters(const ParameterHandles &parameter_handles, Parameters &parameters)
Read out the parameters using the handles into the parameters struct.
Definition: parameters.cpp:142
f
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
param_find
param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
RCUpdate::Parameters::rc_map_failsafe
int32_t rc_map_failsafe
Definition: parameters.h:69
RCUpdate::Parameters::rev
float rev[RC_MAX_CHAN_COUNT]
Definition: parameters.h:61
RCUpdate::ParameterHandles::rc_map_param
param_t rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]
Definition: parameters.h:167
RCUpdate::Parameters::rc_map_flightmode
int32_t rc_map_flightmode
Definition: parameters.h:95
RCUpdate::Parameters::rc_map_offboard_sw
int32_t rc_map_offboard_sw
Definition: parameters.h:77
math::min
constexpr _Tp min(_Tp a, _Tp b)
Definition: Limits.hpp:54
name
const char * name
Definition: tests_main.c:58
RCUpdate::initialize_parameter_handles
void initialize_parameter_handles(ParameterHandles &parameter_handles)
initialize ParameterHandles struct
Definition: parameters.cpp:45
RCUpdate::ParameterHandles::trim
param_t trim[RC_MAX_CHAN_COUNT]
Definition: parameters.h:134
RCUpdate::Parameters::rc_map_stab_sw
int32_t rc_map_stab_sw
Definition: parameters.h:82
RCUpdate::Parameters::rc_map_acro_sw
int32_t rc_map_acro_sw
Definition: parameters.h:76
math::max
constexpr _Tp max(_Tp a, _Tp b)
Definition: Limits.hpp:60
RCUpdate::Parameters::rc_map_throttle
int32_t rc_map_throttle
Definition: parameters.h:68
RCUpdate::Parameters::rc_map_loiter_sw
int32_t rc_map_loiter_sw
Definition: parameters.h:75
OK
#define OK
Definition: uavcan_main.cpp:71
RCUpdate::Parameters::scaling_factor
float scaling_factor[RC_MAX_CHAN_COUNT]
Definition: parameters.h:63
RCUpdate::Parameters::rc_map_trans_sw
int32_t rc_map_trans_sw
Definition: parameters.h:80
RCUpdate::ParameterHandles::min
param_t min[RC_MAX_CHAN_COUNT]
Definition: parameters.h:133
RCUpdate::Parameters::rc_map_gear_sw
int32_t rc_map_gear_sw
Definition: parameters.h:81
RCUpdate::Parameters::rc_map_kill_sw
int32_t rc_map_kill_sw
Definition: parameters.h:78