state_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 "state_manager.h"
33 #include "rosflight.h"
34 
35 namespace rosflight_firmware
36 {
37 
39  RF_(parent), fsm_state_(FSM_STATE_INIT)
40 {
41  state_.armed = false;
42  state_.error = false;
43  state_.failsafe = false;
44  state_.error_codes = 0x00;
45 }
46 
48 {
51 
52  // Initialize LEDs
55  {
57  this->state_=error_data.state;
58  //Be very sure that arming is correct
60  this->state_.armed=false;
61  }
62 }
63 
65 {
66  // I'm putting this here for the case where we've switched states with existing errors,
67  // but those errors might not have been processed yet for the new state. We could replace
68  // this with a recursive call to process_errors() if the state changed in update_fsm()?
69  process_errors(); // check for any error events
70  update_leds();
71 }
72 
74 {
75  // Set the error code
77 
78  // Tell the FSM that we have had an error change
80 }
81 
83 {
84  // If this error code was set,
85  if (state_.error_codes & error)
86  {
87  // Clear the error code
88  state_.error_codes &= ~(error);
89 
90  // If there are no errors, tell the FSM
92 
93  // Send a status update (for logging)
95  }
96 }
97 
99 {
100  FsmState start_state = fsm_state_;
101  uint16_t start_errors = state_.error_codes;
102  switch (fsm_state_)
103  {
104  case FSM_STATE_INIT:
105  if (event == EVENT_INITIALIZED)
106  {
108  }
109  break;
110 
111  case FSM_STATE_PREFLIGHT:
112  switch (event)
113  {
114  case EVENT_RC_FOUND:
116  state_.failsafe = false;
117  break;
118  case EVENT_RC_LOST:
120  break;
121  case EVENT_ERROR:
122  state_.error = true;
124  break;
125  case EVENT_REQUEST_ARM:
126  // require low RC throttle to arm
127  if (RF_.rc_.stick(RC::Stick::STICK_F) < RF_.params_.get_param_float(PARAM_ARM_THRESHOLD))
128  {
129  // require either min throttle to be enabled or throttle override switch to be on
131  || RF_.rc_.switch_on(RC::Switch::SWITCH_THROTTLE_OVERRIDE))
132  {
134  {
137  }
138  else
139  {
140  state_.armed = true;
143  }
144  }
145  else
146  {
147  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "RC throttle override must be active to arm");
148  }
149  }
150  else
151  {
152  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Cannot arm with RC throttle high");
153  }
154  break;
155  default:
156  break;
157  }
158  break;
159 
160  case FSM_STATE_ERROR:
161  switch (event)
162  {
163  case EVENT_RC_LOST:
164  set_error(ERROR_RC_LOST); // sometimes redundant, but reports RC lost error if another error got reported first
165  break;
166  case EVENT_RC_FOUND:
168  state_.failsafe = false;
169  break;
170  case EVENT_NO_ERROR:
171  state_.error = false;
173  break;
174  case EVENT_REQUEST_ARM:
176  {
178  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: Invalid mixer");
180  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: IMU not responding");
182  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: RC signal lost");
184  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: Unhealthy estimator");
186  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: Time going backwards");
188  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: IMU not calibrated");
189 
190  next_arming_error_msg_ms_ = RF_.board_.clock_millis() + 1000; // throttle messages to 1 Hz
191  }
192  break;
193  default:
194  break;
195  }
196  break;
197 
199  switch (event)
200  {
202  state_.armed = true;
204  break;
207  break;
208  case EVENT_RC_LOST:
210  break;
211  case EVENT_ERROR:
213  state_.error = true;
214  break;
215  case EVENT_NO_ERROR:
216  state_.error = false;
217  break;
218  default:
219  break;
220  }
221  break;
222 
223  case FSM_STATE_ARMED:
224  switch (event)
225  {
226  case EVENT_RC_LOST:
227  state_.failsafe = true;
231  break;
233  state_.armed = false;
234  if (state_.error)
236  else
238  break;
239  case EVENT_ERROR:
240  state_.error = true;
241  break;
242  case EVENT_NO_ERROR:
243  state_.error = false;
244  break;
245  default:
246  break;
247  }
248  break;
249 
250  case FSM_STATE_FAILSAFE:
251  switch (event)
252  {
253  case EVENT_ERROR:
254  state_.error = true;
255  break;
257  state_.armed = false;
259  break;
260  case EVENT_RC_FOUND:
261  state_.failsafe = false;
264  break;
265  default:
266  break;
267  }
268  break;
269  default:
270  break;
271  }
272 
273  // If there has been a change, then report it to the user
274  if (start_state != fsm_state_ || state_.error_codes != start_errors)
276 }
277 
279 {
280  if (state_.error_codes)
282  else
284 }
285 
287 {
288  // blink fast if in failsafe
289  if (state_.failsafe)
290  {
292  {
295  }
296  }
297  // blink slowly if in error
298  else if (state_.error)
299  {
301  {
304  }
305  }
306  // off if disarmed, on if armed
307  else if (!state_.armed)
308  RF_.board_.led1_off();
309  else
310  RF_.board_.led1_on();
311 }
312 
313 } //namespace rosflight_firmware
static volatile bool error
Definition: drv_i2c.c:92
void log(CommLinkInterface::LogSeverity severity, const char *fmt,...)
bool switch_on(Switch channel)
Definition: rc.cpp:92
float stick(Stick channel)
Definition: rc.cpp:87
float get_param_float(uint16_t id) const
Get the value of a floating point parameter by id.
Definition: param.h:322
bool start_gyro_calibration(void)
Definition: sensors.cpp:247
void set_error(uint16_t error)
void clear_error(uint16_t error)
StateManager::State state
Definition: board.h:64
virtual uint32_t clock_millis()=0
virtual void led1_on()=0
virtual void led1_toggle()=0
int get_param_int(uint16_t id) const
Get the value of an integer parameter by id.
Definition: param.h:312
virtual bool has_backup_data()=0
virtual void led1_off()=0
const uint32_t ARM_MAGIC
Definition: board.h:69
virtual BackupData get_backup_data()=0


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