command_manager.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 "command_manager.h"
33 
34 #include "rosflight.h"
35 
36 #include <cstdbool>
37 #include <cstdlib>
38 
39 namespace rosflight_firmware
40 {
41 typedef enum
42 {
45 } att_mode_t;
46 
47 typedef struct
48 {
52 
54 
55 typedef struct
56 {
60 } mux_t;
61 
62 CommandManager::CommandManager(ROSflight &_rf) : RF_(_rf), failsafe_command_(multirotor_failsafe_command_) {}
63 
65 {
66  init_failsafe();
67 }
68 
70 {
71  switch (param_id)
72  {
73  case PARAM_FIXED_WING:
75  init_failsafe();
76  break;
77  default:
78  // do nothing
79  break;
80  }
81 }
82 
84 {
86 
89  else
91 }
92 
94 {
95  // get initial, unscaled RC values
100 
101  // determine control mode for each channel and scale command values accordingly
103  {
108  }
109  else
110  {
111  // roll and pitch
112  control_type_t roll_pitch_type;
114  {
115  roll_pitch_type = RF_.rc_.switch_on(RC::SWITCH_ATT_TYPE) ? ANGLE : RATE;
116  }
117  else
118  {
120  }
121 
122  rc_command_.x.type = roll_pitch_type;
123  rc_command_.y.type = roll_pitch_type;
124 
125  // Scale command to appropriate units
126  switch (roll_pitch_type)
127  {
128  case RATE:
131  break;
132  case ANGLE:
135  default:
136  break;
137  }
138 
139  // yaw
142 
143  // throttle
145  }
146 }
147 
149 {
150  uint32_t now = RF_.board_.clock_millis();
151 
152  // if we are still in the lag time, return true
153  if (now < rc_stick_override_[channel].last_override_time + RF_.params_.get_param_int(PARAM_OVERRIDE_LAG_TIME))
154  {
155  return true;
156  }
157  else
158  {
159  if (fabsf(RF_.rc_.stick(rc_stick_override_[channel].rc_channel))
161  {
162  rc_stick_override_[channel].last_override_time = now;
163  return true;
164  }
165  return false;
166  }
167 }
168 
170 {
171  bool override_this_channel = false;
172  // Check if the override switch exists and is triggered, or if the sticks have deviated enough to
173  // trigger an override
175  || stick_deviated(channel))
176  {
177  override_this_channel = true;
178  }
179  else // Otherwise only have RC override if the offboard channel is inactive
180  {
181  if (muxes[channel].onboard->active)
182  {
183  override_this_channel = false;
184  }
185  else
186  {
187  override_this_channel = true;
188  }
189  }
190  // set the combined channel output depending on whether RC is overriding for this channel or not
191  *muxes[channel].combined = override_this_channel ? *muxes[channel].rc : *muxes[channel].onboard;
192  return override_this_channel;
193 }
194 
196 {
197  bool override_this_channel = false;
198  // Check if the override switch exists and is triggered
200  {
201  override_this_channel = true;
202  }
203  else // Otherwise check if the offboard throttle channel is active, if it isn't, have RC override
204  {
205  if (muxes[MUX_F].onboard->active)
206  {
207  // Check if the parameter flag is set to have us always take the smaller throttle
209  {
210  override_this_channel = (muxes[MUX_F].rc->value < muxes[MUX_F].onboard->value);
211  }
212  else
213  {
214  override_this_channel = false;
215  }
216  }
217  else
218  {
219  override_this_channel = true;
220  }
221  }
222 
223  // Set the combined channel output depending on whether RC is overriding for this channel or not
224  *muxes[MUX_F].combined = override_this_channel ? *muxes[MUX_F].rc : *muxes[MUX_F].onboard;
225  return override_this_channel;
226 }
227 
229 {
230  return rc_override_;
231 }
232 
234 {
235  for (int i = 0; i < 4; i++)
236  {
237  if (muxes[i].onboard->active)
238  return true;
239  }
240  return false;
241 }
242 
244 {
245  new_command_ = true;
246  offboard_command_ = new_offboard_command;
247 }
248 
250 {
251  new_command_ = true;
252  rc_command_ = new_rc_command;
253 }
254 
256 {
257  new_command_ = true;
259 }
260 
262 {
263  bool last_rc_override = rc_override_;
264 
265  // Check for and apply failsafe command
267  {
269  }
270  else if (RF_.rc_.new_command())
271  {
272  // Read RC
273  interpret_rc();
274 
275  // Check for offboard control timeout (100 ms)
277  {
278  // If it has been longer than 100 ms, then disable the offboard control
279  offboard_command_.F.active = false;
280  offboard_command_.x.active = false;
281  offboard_command_.y.active = false;
282  offboard_command_.z.active = false;
283  }
284 
285  // Perform muxing
290 
291  // Light to indicate override
292  if (rc_override_)
293  {
294  RF_.board_.led0_on();
295  }
296  else
297  {
298  RF_.board_.led0_off();
299  }
300  }
301 
302  // There was a change in rc_override state
303  if (last_rc_override != rc_override_)
304  {
306  }
307  return true;
308 }
309 
310 } // namespace rosflight_firmware
rc_stick_override_t rc_stick_override_[3]
bool switch_mapped(Switch channel)
Definition: rc.cpp:94
virtual void led0_off()=0
bool switch_on(Switch channel)
Definition: rc.cpp:89
virtual void led0_on()=0
control_channel_t * combined
float stick(Stick channel)
Definition: rc.cpp:84
control_channel_t * rc
float get_param_float(uint16_t id) const
Get the value of a floating point parameter by id.
Definition: param.h:326
rc_stick_override_t rc_stick_override[]
void param_change_callback(uint16_t param_id) override
const State & state() const
void set_new_offboard_command(control_t new_offboard_command)
virtual uint32_t clock_millis()=0
int get_param_int(uint16_t id) const
Get the value of an integer parameter by id.
Definition: param.h:319
void set_new_rc_command(control_t new_rc_command)
bool do_roll_pitch_yaw_muxing(MuxChannel channel)
bool stick_deviated(MuxChannel channel)
control_channel_t * onboard
bool new_command()
Definition: rc.cpp:321


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