PX4 Firmware
PX4 Autopilot Software http://px4.io
MixerGroup.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2012-2019 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_group.cpp
36  *
37  * Mixer collection.
38  */
39 
40 #include "MixerGroup.hpp"
41 
44 #include "NullMixer/NullMixer.hpp"
46 
47 #define debug(fmt, args...) do { } while(0)
48 //#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
49 //#include <debug.h>
50 //#define debug(fmt, args...) syslog(fmt "\n", ##args)
51 
52 unsigned
53 MixerGroup::mix(float *outputs, unsigned space)
54 {
55  unsigned index = 0;
56 
57  for (auto mixer : _mixers) {
58  index += mixer->mix(outputs + index, space - index);
59 
60  if (index >= space) {
61  break;
62  }
63  }
64 
65  return index;
66 }
67 
68 /*
69  * set_trims() has no effect except for the SimpleMixer implementation for which set_trim()
70  * always returns the value one.
71  * The only other existing implementation is MultirotorMixer, which ignores the trim value
72  * and returns _rotor_count.
73  */
74 unsigned
75 MixerGroup::set_trims(int16_t *values, unsigned n)
76 {
77  unsigned index = 0;
78 
79  for (auto mixer : _mixers) {
80  // convert from integer to float
81  // to be safe, clamp offset to range of [-500, 500] usec
82  float offset = math::constrain((float)values[index] / 10000, -1.0f, 1.0f);
83 
84  debug("set trim: %d, offset: %5.3f", values[index], (double)offset);
85  index += mixer->set_trim(offset);
86 
87  if (index >= n) {
88  break;
89  }
90  }
91 
92  return index;
93 }
94 
95 /*
96  * get_trims() has no effect except for the SimpleMixer implementation for which get_trim()
97  * always returns the value one and sets the trim value.
98  * The only other existing implementation is MultirotorMixer, which ignores the trim value
99  * and returns _rotor_count.
100  */
101 unsigned
102 MixerGroup::get_trims(int16_t *values)
103 {
104  unsigned index_mixer = 0;
105  unsigned index = 0;
106 
107  for (auto mixer : _mixers) {
108  float trim = 0;
109  index_mixer += mixer->get_trim(&trim);
110 
111  // MultirotorMixer returns the number of motors so we
112  // loop through index_mixer and set the same trim value for all motors
113  while (index < index_mixer) {
114  values[index] = trim * 10000;
115  index++;
116  }
117  }
118 
119  return index;
120 }
121 
122 void
124 {
125  for (auto mixer : _mixers) {
126  mixer->set_thrust_factor(val);
127  }
128 }
129 
130 void
132 {
133  for (auto mixer : _mixers) {
134  mixer->set_airmode(airmode);
135  }
136 }
137 
138 unsigned
140 {
141  for (auto mixer : _mixers) {
142  unsigned rotor_count = mixer->get_multirotor_count();
143 
144  if (rotor_count > 0) {
145  return rotor_count;
146  }
147  }
148 
149  return 0;
150 }
151 
152 uint16_t
154 {
155  uint16_t sat = 0;
156 
157  for (auto mixer : _mixers) {
158  sat |= mixer->get_saturation_status();
159  }
160 
161  return sat;
162 }
163 
164 void
166 {
167  for (auto mixer : _mixers) {
168  mixer->groups_required(groups);
169  }
170 }
171 
172 int
173 MixerGroup::load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
174 {
175  int ret = -1;
176  const char *end = buf + buflen;
177 
178  /*
179  * Loop until either we have emptied the buffer, or we have failed to
180  * allocate something when we expected to.
181  */
182  while (buflen > 0) {
183  Mixer *m = nullptr;
184  const char *p = end - buflen;
185  unsigned resid = buflen;
186 
187  /*
188  * Use the next character as a hint to decide which mixer class to construct.
189  */
190  switch (*p) {
191  case 'Z':
192  m = NullMixer::from_text(p, resid);
193  break;
194 
195  case 'M':
196  m = SimpleMixer::from_text(control_cb, cb_handle, p, resid);
197  break;
198 
199  case 'R':
200  m = MultirotorMixer::from_text(control_cb, cb_handle, p, resid);
201  break;
202 
203  case 'H':
204  m = HelicopterMixer::from_text(control_cb, cb_handle, p, resid);
205  break;
206 
207  default:
208  /* it's probably junk or whitespace, skip a byte and retry */
209  buflen--;
210  continue;
211  }
212 
213  /*
214  * If we constructed something, add it to the group.
215  */
216  if (m != nullptr) {
217  add_mixer(m);
218 
219  /* we constructed something */
220  ret = 0;
221 
222  /* only adjust buflen if parsing was successful */
223  buflen = resid;
224  debug("SUCCESS - buflen: %d", buflen);
225 
226  } else {
227 
228  /*
229  * There is data in the buffer that we expected to parse, but it didn't,
230  * so give up for now.
231  */
232  break;
233  }
234  }
235 
236  /* nothing more in the buffer for us now */
237  return ret;
238 }
239 
240 void MixerGroup::set_max_delta_out_once(float delta_out_max)
241 {
242  for (auto mixer : _mixers) {
243  mixer->set_max_delta_out_once(delta_out_max);
244  }
245 }
int load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
Adds mixers to the group based on a text description in a buffer.
Definition: MixerGroup.cpp:173
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Definition: Limits.hpp:66
static SimpleMixer * from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
Factory method with full external configuration.
void set_thrust_factor(float val) override
Sets the thrust factor used to calculate mapping from desired thrust to motor control signal output...
uint16_t get_saturation_status()
Definition: MixerGroup.cpp:153
unsigned get_multirotor_count()
Definition: MixerGroup.cpp:139
void add_mixer(Mixer *mixer)
Add a mixer to the group.
Definition: MixerGroup.hpp:69
static MultirotorMixer * from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
Factory method.
uint16_t get_saturation_status() override
Get the saturation status.
int(* ControlCallback)(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control)
Fetch a control value.
Definition: Mixer.hpp:154
unsigned set_trims(int16_t *v, unsigned n)
Definition: MixerGroup.cpp:75
void set_max_delta_out_once(float delta_out_max) override
Update slew rate parameter.
static MultirotorMixer * mixer
mixer initialization
void set_airmode(Airmode airmode) override
Set airmode.
void set_max_delta_out_once(float delta_out_max)
Update slew rate parameter.
Definition: MixerGroup.cpp:240
static HelicopterMixer * from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
Factory method.
void set_thrust_factor(float val)
Sets the thrust factor used to calculate mapping from desired thrust to motor control signal output...
Definition: MixerGroup.cpp:123
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
Airmode
Definition: Mixer.hpp:139
unsigned get_multirotor_count() override
#define debug(fmt, args...)
Definition: MixerGroup.cpp:47
void groups_required(uint32_t &groups)
Definition: MixerGroup.cpp:165
unsigned mix(float *outputs, unsigned space) override
Perform the mixing function.
void set_airmode(Mixer::Airmode airmode)
Definition: MixerGroup.cpp:131
void groups_required(uint32_t &groups) override
Analyses the mix configuration and updates a bitmask of groups that are required. ...
static NullMixer * from_text(const char *buf, unsigned &buflen)
Factory method.
Definition: NullMixer.cpp:52
unsigned mix(float *outputs, unsigned space)
Definition: MixerGroup.cpp:53
unsigned get_trim(float *trim) override
Get trim offset for this mixer.
unsigned set_trim(float trim) override
Set trim offset for this mixer.
Abstract class defining a mixer mixing zero or more inputs to one or more outputs.
Definition: Mixer.hpp:136
unsigned get_trims(int16_t *values)
Definition: MixerGroup.cpp:102
List< Mixer * > _mixers
linked list of mixers
Definition: MixerGroup.hpp:169