mixer.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab
3  *
4  * All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions are met:
8  *
9  * * Redistributions of source code must retain the above copyright notice, this
10  * list of conditions and the following disclaimer.
11  *
12  * * Redistributions in binary form must reproduce the above copyright notice,
13  * this list of conditions and the following disclaimer in the documentation
14  * and/or other materials provided with the distribution.
15  *
16  * * Neither the name of the copyright holder nor the names of its
17  * contributors may be used to endorse or promote products derived from
18  * this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30  */
31 
32 #include "mixer.h"
33 
34 #include "rosflight.h"
35 
36 #include <cstdint>
37 
38 namespace rosflight_firmware
39 {
40 Mixer::Mixer(ROSflight &_rf) : RF_(_rf)
41 {
42  mixer_to_use_ = nullptr;
43 }
44 
46 {
47  init_mixing();
48 }
49 
50 void Mixer::param_change_callback(uint16_t param_id)
51 {
52  switch (param_id)
53  {
54  case PARAM_MIXER:
55  init_mixing();
56  break;
58  case PARAM_RC_TYPE:
59  init_PWM();
60  break;
61  default:
62  // do nothing
63  break;
64  }
65 }
66 
68 {
69  // clear the invalid mixer error
71 
72  uint8_t mixer_choice = RF_.params_.get_param_int(PARAM_MIXER);
73 
74  if (mixer_choice >= NUM_MIXERS)
75  {
77 
78  // set the invalid mixer flag
80  mixer_to_use_ = nullptr;
81  }
82  else
83  {
84  mixer_to_use_ = array_of_mixers_[mixer_choice];
85  }
86 
87  init_PWM();
88 
89  for (int8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++)
90  {
91  raw_outputs_[i] = 0.0f;
92  outputs_[i] = 0.0f;
93  }
94 }
95 
97 {
98  uint32_t refresh_rate = RF_.params_.get_param_int(PARAM_MOTOR_PWM_SEND_RATE);
99  if (refresh_rate == 0 && mixer_to_use_ != nullptr)
100  {
101  refresh_rate = mixer_to_use_->default_pwm_rate;
102  }
103  int16_t off_pwm = 1000;
104 
105  if (mixer_to_use_ == nullptr || refresh_rate == 0)
106  RF_.board_.pwm_init(50, 0);
107  else
108  RF_.board_.pwm_init(refresh_rate, off_pwm);
109 }
110 
111 void Mixer::write_motor(uint8_t index, float value)
112 {
114  {
115  if (value > 1.0)
116  {
117  value = 1.0;
118  }
121  {
123  }
124  else if (value < 0.0)
125  {
126  value = 0.0;
127  }
128  }
129  else
130  {
131  value = 0.0;
132  }
133  raw_outputs_[index] = value;
134  RF_.board_.pwm_write(index, raw_outputs_[index]);
135 }
136 
137 void Mixer::write_servo(uint8_t index, float value)
138 {
139  if (value > 1.0)
140  {
141  value = 1.0;
142  }
143  else if (value < -1.0)
144  {
145  value = -1.0;
146  }
147  raw_outputs_[index] = value;
148  RF_.board_.pwm_write(index, raw_outputs_[index] * 0.5 + 0.5);
149 }
150 
152 {
153  for (uint8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++)
154  {
155  aux_command_.channel[i].type = new_aux_command.channel[i].type;
156  aux_command_.channel[i].value = new_aux_command.channel[i].value;
157  }
158 }
159 
161 {
163  float max_output = 1.0f;
164 
165  // Reverse fixed-wing channels just before mixing if we need to
167  {
168  commands.x *= RF_.params_.get_param_int(PARAM_AILERON_REVERSE) ? -1 : 1;
169  commands.y *= RF_.params_.get_param_int(PARAM_ELEVATOR_REVERSE) ? -1 : 1;
170  commands.z *= RF_.params_.get_param_int(PARAM_RUDDER_REVERSE) ? -1 : 1;
171  }
173  {
174  // For multirotors, disregard yaw commands if throttle is low to prevent motor spin-up while
175  // arming/disarming
176  commands.z = 0.0;
177  }
178 
179  if (mixer_to_use_ == nullptr)
180  return;
181 
182  for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++)
183  {
184  if (mixer_to_use_->output_type[i] != NONE)
185  {
186  // Matrix multiply to mix outputs
187  outputs_[i] = (commands.F * mixer_to_use_->F[i] + commands.x * mixer_to_use_->x[i]
188  + commands.y * mixer_to_use_->y[i] + commands.z * mixer_to_use_->z[i]);
189 
190  // Save off the largest control output if it is greater than 1.0 for future scaling
191  if (outputs_[i] > max_output)
192  {
193  max_output = outputs_[i];
194  }
195  }
196  }
197 
198  // saturate outputs to maintain controllability even during aggressive maneuvers
199  float scale_factor = 1.0;
200  if (max_output > 1.0)
201  {
202  scale_factor = 1.0 / max_output;
203  }
204 
205  // Perform Motor Output Scaling
206  for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++)
207  {
208  // scale all motor outputs by scale factor (this is usually 1.0, unless we saturated)
209  outputs_[i] *= scale_factor;
210  }
211 
212  // Insert AUX Commands, and assemble combined_output_types array (Does not override mixer values)
213 
214  // For the first NUM_MIXER_OUTPUTS channels, only write aux_command to channels the mixer is not
215  // using
216  for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++)
217  {
218  if (mixer_to_use_->output_type[i] == NONE)
219  {
222  }
223  else
224  {
226  }
227  }
228 
229  // The other channels are never used by the mixer
230  for (uint8_t i = NUM_MIXER_OUTPUTS; i < NUM_TOTAL_OUTPUTS; i++)
231  {
234  }
235 
236  // Write to outputs
237  for (uint8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++)
238  {
239  if (combined_output_type_[i] == S)
240  {
241  write_servo(i, outputs_[i]);
242  }
243  else if (combined_output_type_[i] == M)
244  {
245  write_motor(i, outputs_[i]);
246  }
247  }
248 }
249 
250 } // namespace rosflight_firmware
virtual void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm)=0
Mixer(ROSflight &_rf)
Definition: mixer.cpp:40
float raw_outputs_[NUM_TOTAL_OUTPUTS]
Definition: mixer.h:100
void log(CommLinkInterface::LogSeverity severity, const char *fmt,...)
ROSflight & RF_
Definition: mixer.h:98
void set_new_aux_command(aux_command_t new_aux_command)
Definition: mixer.cpp:151
void param_change_callback(uint16_t param_id) override
Definition: mixer.cpp:50
float get_param_float(uint16_t id) const
Get the value of a floating point parameter by id.
Definition: param.h:326
void write_servo(uint8_t index, float value)
Definition: mixer.cpp:137
virtual void pwm_write(uint8_t channel, float value)=0
const State & state() const
float y[NUM_MIXER_OUTPUTS]
Definition: mixer.h:81
void set_error(uint16_t error)
output_type_t output_type[NUM_MIXER_OUTPUTS]
Definition: mixer.h:78
void clear_error(uint16_t error)
static constexpr uint8_t NUM_TOTAL_OUTPUTS
Definition: mixer.h:47
aux_channel_t channel[NUM_TOTAL_OUTPUTS]
Definition: mixer.h:94
float x[NUM_MIXER_OUTPUTS]
Definition: mixer.h:80
const Output & output() const
Definition: controller.h:62
float outputs_[NUM_TOTAL_OUTPUTS]
Definition: mixer.h:101
int get_param_int(uint16_t id) const
Get the value of an integer parameter by id.
Definition: param.h:319
static constexpr uint8_t NUM_MIXER_OUTPUTS
Definition: mixer.h:48
const mixer_t * array_of_mixers_[NUM_MIXERS]
Definition: mixer.h:206
const mixer_t * mixer_to_use_
Definition: mixer.h:203
void write_motor(uint8_t index, float value)
Definition: mixer.cpp:111
output_type_t combined_output_type_[NUM_TOTAL_OUTPUTS]
Definition: mixer.h:103
float F[NUM_MIXER_OUTPUTS]
Definition: mixer.h:79
float z[NUM_MIXER_OUTPUTS]
Definition: mixer.h:82
aux_command_t aux_command_
Definition: mixer.h:102


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Fri Oct 9 2020 03:17:15