rc.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 "rc.h"
33 
34 #include "rosflight.h"
35 
36 #include <cstring>
37 
38 namespace rosflight_firmware
39 {
40 RC::RC(ROSflight &_rf) : RF_(_rf) {}
41 
42 void RC::init()
43 {
44  init_rc();
45  new_command_ = false;
46 }
47 
49 {
50  RF_.board_.rc_init(static_cast<Board::rc_type_t>(RF_.params_.get_param_int(PARAM_RC_TYPE)));
51  init_sticks();
52  init_switches();
53 }
54 
55 void RC::param_change_callback(uint16_t param_id)
56 {
57  switch (param_id)
58  {
59  case PARAM_RC_TYPE:
60  RF_.board_.rc_init(static_cast<Board::rc_type_t>(RF_.params_.get_param_int(PARAM_RC_TYPE)));
61  break;
62  case PARAM_RC_X_CHANNEL:
63  case PARAM_RC_Y_CHANNEL:
64  case PARAM_RC_Z_CHANNEL:
65  case PARAM_RC_F_CHANNEL:
66  init_sticks();
67  break;
76  init_switches();
77  break;
78  default:
79  // do nothing
80  break;
81  }
82 }
83 
84 float RC::stick(Stick channel)
85 {
86  return stick_values[channel];
87 }
88 
89 bool RC::switch_on(Switch channel)
90 {
91  return switch_values[channel];
92 }
93 
95 {
96  return switches[channel].mapped;
97 }
98 
99 void RC::init_sticks(void)
100 {
102  sticks[STICK_X].one_sided = false;
103 
105  sticks[STICK_Y].one_sided = false;
106 
108  sticks[STICK_Z].one_sided = false;
109 
111  sticks[STICK_F].one_sided = true;
112 }
113 
115 {
116  for (uint8_t chan = 0; chan < static_cast<uint8_t>(SWITCHES_COUNT); chan++)
117  {
118  char channel_name[18];
119  switch (chan)
120  {
121  case SWITCH_ARM:
122  strcpy(channel_name, "ARM");
124  break;
125  case SWITCH_ATT_OVERRIDE:
126  strcpy(channel_name, "ATTITUDE OVERRIDE");
128  break;
130  strcpy(channel_name, "THROTTLE OVERRIDE");
132  break;
133  case SWITCH_ATT_TYPE:
134  strcpy(channel_name, "ATTITUDE TYPE");
136  break;
137  default:
138  strcpy(channel_name, "INVALID");
139  switches[chan].channel = 255;
140  break;
141  }
142 
143  switches[chan].mapped =
145 
146  switch (switches[chan].channel)
147  {
148  case 4:
150  break;
151  case 5:
153  break;
154  case 6:
156  break;
157  case 7:
159  break;
160  default:
161  switches[chan].direction = 1;
162  break;
163  }
164 
165  if (switches[chan].mapped)
166  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "%s switch mapped to RC channel %d", channel_name,
167  switches[chan].channel);
168  else
169  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "%s switch not mapped", channel_name);
170  }
171 }
172 
174 {
175  bool failsafe = false;
176 
177  // If the board reports that we have lost RC, tell the state manager
178  if (RF_.board_.rc_lost())
179  {
180  failsafe = true;
181  }
182  else
183  {
184  // go into failsafe if we get an invalid RC command for any channel
185  for (int8_t i = 0; i < RF_.params_.get_param_int(PARAM_RC_NUM_CHANNELS); i++)
186  {
187  float pwm = RF_.board_.rc_read(i);
188  if (pwm < -0.25 || pwm > 1.25)
189  {
190  failsafe = true;
191  }
192  }
193  }
194 
195  if (failsafe)
196  // set the RC Lost error flag
198  else
199  // Clear the RC Lost Error
201 
202  return failsafe;
203 }
204 
206 {
207  uint32_t now_ms = RF_.board_.clock_millis();
208  uint32_t dt = now_ms - prev_time_ms;
209  prev_time_ms = now_ms;
210  // check for arming switch
212  {
213  if (!RF_.state_manager_.state().armed) // we are DISARMED
214  {
215  // if left stick is down and to the right
218  {
220  }
221  else
222  {
224  }
226  {
228  }
229  }
230  else // we are ARMED
231  {
232  // if left stick is down and to the left
235  {
237  }
238  else
239  {
241  }
243  {
246  }
247  }
248  }
249  else // ARMING WITH SWITCH
250  {
252  {
253  if (!RF_.state_manager_.state().armed)
255  ;
256  }
257  else
258  {
260  }
261  }
262 }
263 
264 bool RC::run()
265 {
266  uint32_t now = RF_.board_.clock_millis();
267 
268  // if it has been more than 20ms then look for new RC values and parse them
269  if (now - last_rc_receive_time < 20)
270  {
271  return false;
272  }
273  last_rc_receive_time = now;
274 
275  // Check for rc lost
276  if (check_rc_lost())
277  return false;
278 
279  // read and normalize stick values
280  for (uint8_t channel = 0; channel < static_cast<uint8_t>(STICKS_COUNT); channel++)
281  {
282  float pwm = RF_.board_.rc_read(sticks[channel].channel);
283  if (sticks[channel].one_sided) // generally only F is one_sided
284  {
285  stick_values[channel] = pwm;
286  }
287  else
288  {
289  stick_values[channel] = 2.0 * (pwm - 0.5);
290  }
291  }
292 
293  // read and interpret switch values
294  for (uint8_t channel = 0; channel < static_cast<uint8_t>(SWITCHES_COUNT); channel++)
295  {
296  if (switches[channel].mapped)
297  {
298  if (switches[channel].direction < 0)
299  {
300  switch_values[channel] = RF_.board_.rc_read(switches[channel].channel) < 0.2;
301  }
302  else
303  {
304  switch_values[channel] = RF_.board_.rc_read(switches[channel].channel) >= 0.8;
305  }
306  }
307  else
308  {
309  switch_values[channel] = false;
310  }
311  }
312 
313  // Look for arming and disarming signals
315 
316  // Signal to the mux that we need to compute a new combined command
317  new_command_ = true;
318  return true;
319 }
320 
322 {
323  if (new_command_)
324  {
325  new_command_ = false;
326  return true;
327  }
328  else
329  return false;
330  ;
331 }
332 
333 } // namespace rosflight_firmware
bool switch_mapped(Switch channel)
Definition: rc.cpp:94
virtual float rc_read(uint8_t channel)=0
virtual bool rc_lost()=0
bool new_command_
Definition: rc.h:91
rc_switch_config_t switches[SWITCHES_COUNT]
Definition: rc.h:99
void init_switches()
Definition: rc.cpp:114
void log(CommLinkInterface::LogSeverity severity, const char *fmt,...)
rc_stick_config_t sticks[STICKS_COUNT]
Definition: rc.h:98
bool switch_on(Switch channel)
Definition: rc.cpp:89
ROSflight & RF_
Definition: rc.h:76
float stick(Stick channel)
Definition: rc.cpp:84
void init_sticks()
Definition: rc.cpp:99
RC(ROSflight &_rf)
Definition: rc.cpp:40
uint32_t time_sticks_have_been_in_arming_position_ms
Definition: rc.h:94
float get_param_float(uint16_t id) const
Get the value of a floating point parameter by id.
Definition: param.h:326
void param_change_callback(uint16_t param_id) override
Definition: rc.cpp:55
const State & state() const
uint32_t prev_time_ms
Definition: rc.h:95
volatile float stick_values[STICKS_COUNT]
Definition: rc.h:102
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
volatile bool switch_values[SWITCHES_COUNT]
Definition: rc.h:101
uint32_t last_rc_receive_time
Definition: rc.h:96
virtual void rc_init(rc_type_t rc_type)=0
void look_for_arm_disarm_signal()
Definition: rc.cpp:205
bool new_command()
Definition: rc.cpp:321
bool check_rc_lost()
Definition: rc.cpp:173


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Sat May 9 2020 03:16:53