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 <stdbool.h>
33 #include <stdlib.h>
34 
35 #include "command_manager.h"
36 #include "rosflight.h"
37 
38 namespace rosflight_firmware
39 {
40 
41 typedef enum
42 {
45 } att_mode_t;
46 
47 typedef struct
48 {
52 
54 {
55  { RC::STICK_X, 0 },
56  { RC::STICK_Y, 0 },
57  { RC::STICK_Z, 0 }
58 };
59 
60 typedef struct
61 {
65 } mux_t;
66 
68  RF_(_rf),
69  failsafe_command_(multirotor_failsafe_command_)
70 {}
71 
73 {
74  init_failsafe();
75 }
76 
78 {
79  switch (param_id)
80  {
81  case PARAM_FIXED_WING:
83  init_failsafe();
84  break;
85  default:
86  // do nothing
87  break;
88  }
89 }
90 
92 {
94 
97  else
99 }
100 
102 {
103  // get initial, unscaled RC values
108 
109  // determine control mode for each channel and scale command values accordingly
111  {
116  }
117  else
118  {
119  // roll and pitch
120  control_type_t roll_pitch_type;
122  {
123  roll_pitch_type = RF_.rc_.switch_on(RC::SWITCH_ATT_TYPE) ? ANGLE : RATE;
124  }
125  else
126  {
128  }
129 
130  rc_command_.x.type = roll_pitch_type;
131  rc_command_.y.type = roll_pitch_type;
132 
133  // Scale command to appropriate units
134  switch (roll_pitch_type)
135  {
136  case RATE:
139  break;
140  case ANGLE:
143  default:
144  break;
145  }
146 
147  // yaw
150 
151  // throttle
153  }
154 }
155 
157 {
158  uint32_t now = RF_.board_.clock_millis();
159 
160  // if we are still in the lag time, return true
161  if (now < rc_stick_override_[channel].last_override_time + RF_.params_.get_param_int(PARAM_OVERRIDE_LAG_TIME))
162  {
163  return true;
164  }
165  else
166  {
167  if (fabsf(RF_.rc_.stick(rc_stick_override_[channel].rc_channel))
169  {
170  rc_stick_override_[channel].last_override_time = now;
171  return true;
172  }
173  return false;
174  }
175 }
176 
178 {
179  bool override_this_channel = false;
180  //Check if the override switch exists and is triggered, or if the sticks have deviated enough to trigger an override
182  || stick_deviated(channel))
183  {
184  override_this_channel = true;
185  }
186  else // Otherwise only have RC override if the offboard channel is inactive
187  {
188  if (muxes[channel].onboard->active)
189  {
190  override_this_channel = false;
191  }
192  else
193  {
194  override_this_channel = true;
195  }
196  }
197  // set the combined channel output depending on whether RC is overriding for this channel or not
198  *muxes[channel].combined = override_this_channel ? *muxes[channel].rc : *muxes[channel].onboard;
199  return override_this_channel;
200 }
201 
203 {
204  bool override_this_channel = false;
205  // Check if the override switch exists and is triggered
207  {
208  override_this_channel = true;
209  }
210  else // Otherwise check if the offboard throttle channel is active, if it isn't, have RC override
211  {
212  if (muxes[MUX_F].onboard->active)
213  {
214  // Check if the parameter flag is set to have us always take the smaller throttle
216  {
217  override_this_channel = (muxes[MUX_F].rc->value < muxes[MUX_F].onboard->value);
218  }
219  else
220  {
221  override_this_channel = false;
222  }
223  }
224  else
225  {
226  override_this_channel = true;
227  }
228  }
229 
230  // Set the combined channel output depending on whether RC is overriding for this channel or not
231  *muxes[MUX_F].combined = override_this_channel ? *muxes[MUX_F].rc : *muxes[MUX_F].onboard;
232  return override_this_channel;
233 }
234 
236 {
237  return rc_override_;
238 }
239 
241 {
242  for (int i = 0; i < 4; i++)
243  {
244  if (muxes[i].onboard->active)
245  return true;
246  }
247  return false;
248 }
249 
251 {
252  new_command_ = true;
253  offboard_command_ = new_offboard_command;
254 }
255 
257 {
258  new_command_ = true;
259  rc_command_ = new_rc_command;
260 }
261 
263 {
264  new_command_ = true;
266 }
267 
268 
270 {
271  bool last_rc_override = rc_override_;
272 
273  // Check for and apply failsafe command
275  {
277  }
278  else if (RF_.rc_.new_command())
279  {
280  // Read RC
281  interpret_rc();
282 
283  // Check for offboard control timeout (100 ms)
285  {
286  // If it has been longer than 100 ms, then disable the offboard control
287  offboard_command_.F.active = false;
288  offboard_command_.x.active = false;
289  offboard_command_.y.active = false;
290  offboard_command_.z.active = false;
291  }
292 
293  // Perform muxing
298 
299  // Light to indicate override
300  if (rc_override_)
301  {
302  RF_.board_.led0_on();
303  }
304  else
305  {
306  RF_.board_.led0_off();
307  }
308  }
309 
310  // There was a change in rc_override state
311  if (last_rc_override != rc_override_)
312  {
314  }
315  return true;
316 }
317 
318 
319 }
rc_stick_override_t rc_stick_override_[3]
bool switch_mapped(Switch channel)
Definition: rc.cpp:97
virtual void led0_off()=0
bool switch_on(Switch channel)
Definition: rc.cpp:92
virtual void led0_on()=0
control_channel_t * combined
float stick(Stick channel)
Definition: rc.cpp:87
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:322
rc_stick_override_t rc_stick_override[]
void param_change_callback(uint16_t param_id) override
const State & state() const
Definition: state_manager.h:82
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:312
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:326


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