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


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