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 
33 #include <stdint.h>
34 
35 #include "mixer.h"
36 #include "rosflight.h"
37 
38 namespace rosflight_firmware
39 {
40 
42  RF_(_rf)
43 {
44  mixer_to_use_ = nullptr;
45 }
46 
48 {
49  init_mixing();
50 }
51 
52 void Mixer::param_change_callback(uint16_t param_id)
53 {
54  switch (param_id)
55  {
56  case PARAM_MIXER:
57  init_mixing();
58  break;
60  case PARAM_RC_TYPE:
61  init_PWM();
62  break;
63  default:
64  // do nothing
65  break;
66  }
67 }
68 
69 
71 {
72  // clear the invalid mixer error
74 
75  uint8_t mixer_choice = RF_.params_.get_param_int(PARAM_MIXER);
76 
77  if (mixer_choice >= NUM_MIXERS)
78  {
80 
81  // set the invalid mixer flag
83  mixer_to_use_ = nullptr;
84  }
85  else
86  {
87  mixer_to_use_ = array_of_mixers_[mixer_choice];
88  }
89 
90 
91  init_PWM();
92 
93  for (int8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++)
94  {
95  raw_outputs_[i] = 0.0f;
96  outputs_[i] = 0.0f;
97  }
98 }
99 
101 {
102  uint32_t refresh_rate = RF_.params_.get_param_int(PARAM_MOTOR_PWM_SEND_RATE);
103  if (refresh_rate == 0 && mixer_to_use_ != nullptr)
104  {
105  refresh_rate = mixer_to_use_->default_pwm_rate;
106  }
107  int16_t off_pwm = 1000;
108 
109  if (mixer_to_use_ == nullptr || refresh_rate == 0)
110  RF_.board_.pwm_init(50, 0);
111  else
112  RF_.board_.pwm_init(refresh_rate, off_pwm);
113 }
114 
115 
116 void Mixer::write_motor(uint8_t index, float value)
117 {
119  {
120  if (value > 1.0)
121  {
122  value = 1.0;
123  }
126  {
128  }
129  else if (value < 0.0)
130  {
131  value = 0.0;
132  }
133  }
134  else
135  {
136  value = 0.0;
137  }
138  raw_outputs_[index] = value;
139  RF_.board_.pwm_write(index, raw_outputs_[index]);
140 }
141 
142 
143 void Mixer::write_servo(uint8_t index, float value)
144 {
145  if (value > 1.0)
146  {
147  value = 1.0;
148  }
149  else if (value < -1.0)
150  {
151  value = -1.0;
152  }
153  raw_outputs_[index] = value;
154  RF_.board_.pwm_write(index, raw_outputs_[index] * 0.5 + 0.5);
155 }
156 
158 {
159  for (uint8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++)
160  {
161  aux_command_.channel[i].type = new_aux_command.channel[i].type;
162  aux_command_.channel[i].value = new_aux_command.channel[i].value;
163  }
164 }
165 
167 {
169  float max_output = 1.0f;
170 
171  // Reverse fixed-wing channels just before mixing if we need to
173  {
174  commands.x *= RF_.params_.get_param_int(PARAM_AILERON_REVERSE) ? -1 : 1;
175  commands.y *= RF_.params_.get_param_int(PARAM_ELEVATOR_REVERSE) ? -1 : 1;
176  commands.z *= RF_.params_.get_param_int(PARAM_RUDDER_REVERSE) ? -1 : 1;
177  }
179  {
180  // For multirotors, disregard yaw commands if throttle is low to prevent motor spin-up while arming/disarming
181  commands.z = 0.0;
182  }
183 
184  if (mixer_to_use_ == nullptr)
185  return;
186 
187  for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++)
188  {
189  if (mixer_to_use_->output_type[i] != NONE)
190  {
191  // Matrix multiply to mix outputs
192  outputs_[i] = (commands.F*mixer_to_use_->F[i] + commands.x*mixer_to_use_->x[i] +
193  commands.y*mixer_to_use_->y[i] + commands.z*mixer_to_use_->z[i]);
194 
195  // Save off the largest control output if it is greater than 1.0 for future scaling
196  if (outputs_[i] > max_output)
197  {
198  max_output = outputs_[i];
199  }
200  }
201  }
202 
203  // saturate outputs to maintain controllability even during aggressive maneuvers
204  float scale_factor = 1.0;
205  if (max_output > 1.0)
206  {
207  scale_factor = 1.0/max_output;
208  }
209 
210  // Perform Motor Output Scaling
211  for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++)
212  {
213  // scale all motor outputs by scale factor (this is usually 1.0, unless we saturated)
214  outputs_[i] *= scale_factor;
215  }
216 
217  // Insert AUX Commands, and assemble combined_output_types array (Does not override mixer values)
218 
219  // For the first NUM_MIXER_OUTPUTS channels, only write aux_command to channels the mixer is not using
220  for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++)
221  {
222  if (mixer_to_use_->output_type[i] == NONE)
223  {
226  }
227  else
228  {
230  }
231  }
232 
233  // The other channels are never used by the mixer
234  for (uint8_t i = NUM_MIXER_OUTPUTS; i < NUM_TOTAL_OUTPUTS; i++)
235  {
238  }
239 
240  // Write to outputs
241  for (uint8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++)
242  {
243  if (combined_output_type_[i] == S)
244  {
245  write_servo(i, outputs_[i]);
246  }
247  else if (combined_output_type_[i] == M)
248  {
249  write_motor(i, outputs_[i]);
250  }
251  }
252 }
253 
254 }
virtual void pwm_init(uint32_t refresh_rate, uint16_t idle_pwm)=0
Mixer(ROSflight &_rf)
Definition: mixer.cpp:41
float raw_outputs_[NUM_TOTAL_OUTPUTS]
Definition: mixer.h:104
void log(CommLinkInterface::LogSeverity severity, const char *fmt,...)
void set_new_aux_command(aux_command_t new_aux_command)
Definition: mixer.cpp:157
void param_change_callback(uint16_t param_id) override
Definition: mixer.cpp:52
float get_param_float(uint16_t id) const
Get the value of a floating point parameter by id.
Definition: param.h:322
void write_servo(uint8_t index, float value)
Definition: mixer.cpp:143
virtual void pwm_write(uint8_t channel, float value)=0
const State & state() const
Definition: state_manager.h:82
float y[NUM_MIXER_OUTPUTS]
Definition: mixer.h:85
void set_error(uint16_t error)
output_type_t output_type[NUM_MIXER_OUTPUTS]
Definition: mixer.h:82
void clear_error(uint16_t error)
static constexpr uint8_t NUM_TOTAL_OUTPUTS
Definition: mixer.h:51
aux_channel_t channel[NUM_TOTAL_OUTPUTS]
Definition: mixer.h:98
float x[NUM_MIXER_OUTPUTS]
Definition: mixer.h:84
const Output & output() const
Definition: controller.h:63
float outputs_[NUM_TOTAL_OUTPUTS]
Definition: mixer.h:105
int get_param_int(uint16_t id) const
Get the value of an integer parameter by id.
Definition: param.h:312
static constexpr uint8_t NUM_MIXER_OUTPUTS
Definition: mixer.h:52
const mixer_t * array_of_mixers_[NUM_MIXERS]
Definition: mixer.h:245
const mixer_t * mixer_to_use_
Definition: mixer.h:243
void write_motor(uint8_t index, float value)
Definition: mixer.cpp:116
output_type_t combined_output_type_[NUM_TOTAL_OUTPUTS]
Definition: mixer.h:107
float F[NUM_MIXER_OUTPUTS]
Definition: mixer.h:83
float z[NUM_MIXER_OUTPUTS]
Definition: mixer.h:86
aux_command_t aux_command_
Definition: mixer.h:106


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Thu Oct 24 2019 03:17:19