PX4 Firmware
PX4 Autopilot Software http://px4.io
SimpleMixer.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2012-2015 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 mixer_simple.cpp
36  *
37  * Simple summing mixer.
38  */
39 
40 #include "SimpleMixer.hpp"
41 
42 #include <stdio.h>
43 #include <stdlib.h>
44 
45 #define debug(fmt, args...) do { } while(0)
46 //#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
47 
48 SimpleMixer::SimpleMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_simple_s *mixinfo) :
49  Mixer(control_cb, cb_handle),
50  _pinfo(mixinfo)
51 {
52 }
53 
55 {
56  if (_pinfo != nullptr) {
57  free(_pinfo);
58  }
59 }
60 
61 unsigned SimpleMixer::set_trim(float trim)
62 {
63  _pinfo->output_scaler.offset = trim;
64  return 1;
65 }
66 
67 unsigned SimpleMixer::get_trim(float *trim)
68 {
69  *trim = _pinfo->output_scaler.offset;
70  return 1;
71 }
72 
73 int
74 SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler)
75 {
76  int ret;
77  int s[5];
78  int n = -1;
79 
80  buf = findtag(buf, buflen, 'O');
81 
82  if ((buf == nullptr) || (buflen < 12)) {
83  debug("output parser failed finding tag, ret: '%s'", buf);
84  return -1;
85  }
86 
87  if ((ret = sscanf(buf, "O: %d %d %d %d %d %n",
88  &s[0], &s[1], &s[2], &s[3], &s[4], &n)) != 5) {
89  debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n);
90  return -1;
91  }
92 
93  buf = skipline(buf, buflen);
94 
95  if (buf == nullptr) {
96  debug("no line ending, line is incomplete");
97  return -1;
98  }
99 
100  scaler.negative_scale = s[0] / 10000.0f;
101  scaler.positive_scale = s[1] / 10000.0f;
102  scaler.offset = s[2] / 10000.0f;
103  scaler.min_output = s[3] / 10000.0f;
104  scaler.max_output = s[4] / 10000.0f;
105 
106  return 0;
107 }
108 
109 int
110 SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group,
111  uint8_t &control_index)
112 {
113  unsigned u[2];
114  int s[5];
115 
116  buf = findtag(buf, buflen, 'S');
117 
118  if ((buf == nullptr) || (buflen < 16)) {
119  debug("control parser failed finding tag, ret: '%s'", buf);
120  return -1;
121  }
122 
123  if (sscanf(buf, "S: %u %u %d %d %d %d %d",
124  &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) {
125  debug("control parse failed on '%s'", buf);
126  return -1;
127  }
128 
129  buf = skipline(buf, buflen);
130 
131  if (buf == nullptr) {
132  debug("no line ending, line is incomplete");
133  return -1;
134  }
135 
136  control_group = u[0];
137  control_index = u[1];
138  scaler.negative_scale = s[0] / 10000.0f;
139  scaler.positive_scale = s[1] / 10000.0f;
140  scaler.offset = s[2] / 10000.0f;
141  scaler.min_output = s[3] / 10000.0f;
142  scaler.max_output = s[4] / 10000.0f;
143 
144  return 0;
145 }
146 
147 SimpleMixer *
148 SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
149 {
150  SimpleMixer *sm = nullptr;
151  mixer_simple_s *mixinfo = nullptr;
152  unsigned inputs;
153  int used;
154  const char *end = buf + buflen;
155  char next_tag;
156 
157  /* enforce that the mixer ends with a new line */
158  if (!string_well_formed(buf, buflen)) {
159  return nullptr;
160  }
161 
162  /* get the base info for the mixer */
163  if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) {
164  debug("simple parse failed on '%s'", buf);
165  goto out;
166  }
167 
168  /* at least 1 input is required */
169  if (inputs == 0) {
170  debug("simple parse got 0 inputs");
171  goto out;
172  }
173 
174  buf = skipline(buf, buflen);
175 
176  if (buf == nullptr) {
177  debug("no line ending, line is incomplete");
178  goto out;
179  }
180 
181  mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs));
182 
183  if (mixinfo == nullptr) {
184  debug("could not allocate memory for mixer info");
185  goto out;
186  }
187 
188  mixinfo->control_count = inputs;
189 
190  /* find the next tag */
191  next_tag = findnexttag(end - buflen, buflen);
192 
193  if (next_tag == 'S') {
194  /* No output scalers specified. Use default values.
195  * Corresponds to:
196  * O: 10000 10000 0 -10000 10000
197  */
198  mixinfo->output_scaler.negative_scale = 1.0f;
199  mixinfo->output_scaler.positive_scale = 1.0f;
200  mixinfo->output_scaler.offset = 0.f;
201  mixinfo->output_scaler.min_output = -1.0f;
202  mixinfo->output_scaler.max_output = 1.0f;
203 
204  } else {
205  if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler)) {
206  debug("simple mixer parser failed parsing out scaler tag, ret: '%s'", buf);
207  goto out;
208  }
209  }
210 
211  for (unsigned i = 0; i < inputs; i++) {
212  if (parse_control_scaler(end - buflen, buflen,
213  mixinfo->controls[i].scaler,
214  mixinfo->controls[i].control_group,
215  mixinfo->controls[i].control_index)) {
216  debug("simple mixer parser failed parsing ctrl scaler tag, ret: '%s'", buf);
217  goto out;
218  }
219  }
220 
221  sm = new SimpleMixer(control_cb, cb_handle, mixinfo);
222 
223  if (sm != nullptr) {
224  mixinfo = nullptr;
225  debug("loaded mixer with %d input(s)", inputs);
226 
227  } else {
228  debug("could not allocate memory for mixer");
229  }
230 
231 out:
232 
233  if (mixinfo != nullptr) {
234  free(mixinfo);
235  }
236 
237  return sm;
238 }
239 
240 unsigned
241 SimpleMixer::mix(float *outputs, unsigned space)
242 {
243  float sum = 0.0f;
244 
245  if (_pinfo == nullptr) {
246  return 0;
247  }
248 
249  if (space < 1) {
250  return 0;
251  }
252 
253  for (unsigned i = 0; i < _pinfo->control_count; i++) {
254  float input = 0.0f;
255 
259  input);
260 
261  sum += scale(_pinfo->controls[i].scaler, input);
262  }
263 
264  *outputs = scale(_pinfo->output_scaler, sum);
265  return 1;
266 }
267 
268 void
270 {
271  for (unsigned i = 0; i < _pinfo->control_count; i++) {
272  groups |= 1 << _pinfo->controls[i].control_group;
273  }
274 }
275 
276 int
278 {
279  float junk;
280 
281  /* sanity that presumes that a mixer includes a control no more than once */
282  /* max of 32 groups due to groups_required API */
283  if (_pinfo->control_count > 32) {
284  return -2;
285  }
286 
287  /* validate the output scaler */
288  int ret = scale_check(_pinfo->output_scaler);
289 
290  if (ret != 0) {
291  return ret;
292  }
293 
294  /* validate input scalers */
295  for (unsigned i = 0; i < _pinfo->control_count; i++) {
296 
297  /* verify that we can fetch the control */
301  junk) != 0) {
302  return -3;
303  }
304 
305  /* validate the scaler */
306  ret = scale_check(_pinfo->controls[i].scaler);
307 
308  if (ret != 0) {
309  return (10 * i + ret);
310  }
311  }
312 
313  return 0;
314 }
315 
316 float
317 SimpleMixer::scale(const mixer_scaler_s &scaler, float input)
318 {
319  float output;
320 
321  if (input < 0.0f) {
322  output = (input * scaler.negative_scale) + scaler.offset;
323 
324  } else {
325  output = (input * scaler.positive_scale) + scaler.offset;
326  }
327 
328  return math::constrain(output, scaler.min_output, scaler.max_output);
329 }
330 
331 int
333 {
334  if (scaler.offset > 1.001f) {
335  return 1;
336  }
337 
338  if (scaler.offset < -1.001f) {
339  return 2;
340  }
341 
342  if (scaler.min_output > scaler.max_output) {
343  return 3;
344  }
345 
346  if (scaler.min_output < -1.001f) {
347  return 4;
348  }
349 
350  if (scaler.max_output > 1.001f) {
351  return 5;
352  }
353 
354  return 0;
355 }
unsigned set_trim(float trim) override
Set trim offset for this mixer.
Definition: SimpleMixer.cpp:61
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Definition: Limits.hpp:66
mixer_scaler_s output_scaler
scaling for the output
Definition: SimpleMixer.hpp:59
static char findnexttag(const char *buf, unsigned buflen)
Find next tag and return it (0 is returned if no tag is found)
Definition: Mixer.cpp:75
static SimpleMixer * from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
Factory method with full external configuration.
simple mixer
Definition: SimpleMixer.hpp:57
int check()
Check that the mixer configuration as loaded is sensible.
virtual ~SimpleMixer()
Definition: SimpleMixer.cpp:54
static const char * skipline(const char *buf, unsigned &buflen)
Skip a line.
Definition: Mixer.cpp:90
ControlCallback _control_cb
client-supplied callback used when fetching control values
Definition: Mixer.hpp:235
SimpleMixer(ControlCallback control_cb, uintptr_t cb_handle, mixer_simple_s *mixinfo)
Constructor.
Definition: SimpleMixer.cpp:48
int(* ControlCallback)(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control)
Fetch a control value.
Definition: Mixer.hpp:154
static float scale(const mixer_scaler_s &scaler, float input)
Perform simpler linear scaling.
static int scale_check(struct mixer_scaler_s &scaler)
Validate a scaler.
unsigned get_trim(float *trim) override
Get trim offset for this mixer.
Definition: SimpleMixer.cpp:67
float positive_scale
Definition: SimpleMixer.hpp:41
static bool string_well_formed(const char *buf, unsigned &buflen)
Check wether the string is well formed and suitable for parsing.
Definition: Mixer.cpp:105
#define MIXER_SIMPLE_SIZE(_icount)
Definition: SimpleMixer.hpp:54
void groups_required(uint32_t &groups) override
Analyses the mix configuration and updates a bitmask of groups that are required. ...
uintptr_t _cb_handle
Definition: Mixer.hpp:236
static int parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index)
static int parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler)
Definition: SimpleMixer.cpp:74
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
unsigned mix(float *outputs, unsigned space) override
Perform the mixing function.
mixer_simple_s * _pinfo
simple channel scaler
Definition: SimpleMixer.hpp:39
mixer_control_s controls[]
actual size of the array is set by control_count
Definition: SimpleMixer.hpp:60
float negative_scale
Definition: SimpleMixer.hpp:40
uint8_t control_count
number of inputs
Definition: SimpleMixer.hpp:58
static const char * findtag(const char *buf, unsigned &buflen, char tag)
Find a tag.
Definition: Mixer.cpp:60
uint8_t control_index
index within the control group
Definition: SimpleMixer.hpp:50
Simple summing mixer.
Definition: SimpleMixer.hpp:68
uint8_t control_group
group from which the input reads
Definition: SimpleMixer.hpp:49
#define debug(fmt, args...)
Definition: SimpleMixer.cpp:45
mixer_scaler_s scaler
scaling applied to the input before use
Definition: SimpleMixer.hpp:51
Abstract class defining a mixer mixing zero or more inputs to one or more outputs.
Definition: Mixer.hpp:136