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 
34 #include "rosflight.h"
35 
36 namespace rosflight_firmware
37 {
38 StateManager::StateManager(ROSflight &parent) : RF_(parent), fsm_state_(FSM_STATE_INIT)
39 {
40  state_.armed = false;
41  state_.error = false;
42  state_.failsafe = false;
43  state_.error_codes = 0x00;
44 }
45 
47 {
49 
52 
53  // Initialize LEDs
55 }
56 
58 {
59  // I'm putting this here for the case where we've switched states with existing errors,
60  // but those errors might not have been processed yet for the new state. We could replace
61  // this with a recursive call to process_errors() if the state changed in update_fsm()?
62  process_errors(); // check for any error events
63  update_leds();
64 }
65 
67 {
68  // Set the error code
70 
71  // Tell the FSM that we have had an error change
73 }
74 
76 {
77  // If this error code was set,
78  if (state_.error_codes & error)
79  {
80  // Clear the error code
81  state_.error_codes &= ~(error);
82 
83  // If there are no errors, tell the FSM
85 
86  // Send a status update (for logging)
88  }
89 }
90 
92 {
93  FsmState start_state = fsm_state_;
94  uint16_t start_errors = state_.error_codes;
95  switch (fsm_state_)
96  {
97  case FSM_STATE_INIT:
98  if (event == EVENT_INITIALIZED)
99  {
101  }
102  break;
103 
104  case FSM_STATE_PREFLIGHT:
105  switch (event)
106  {
107  case EVENT_RC_FOUND:
109  state_.failsafe = false;
110  break;
111  case EVENT_RC_LOST:
113  break;
114  case EVENT_ERROR:
115  state_.error = true;
117  break;
118  case EVENT_REQUEST_ARM:
119  // require low RC throttle to arm
120  if (RF_.rc_.stick(RC::Stick::STICK_F) < RF_.params_.get_param_float(PARAM_ARM_THRESHOLD))
121  {
122  // require either min throttle to be enabled or throttle override switch to be on
124  || RF_.rc_.switch_on(RC::Switch::SWITCH_THROTTLE_OVERRIDE))
125  {
127  {
130  }
131  else
132  {
133  state_.armed = true;
136  }
137  }
138  else
139  {
141  "RC throttle override must be active to arm");
142  }
143  }
144  else
145  {
146  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Cannot arm with RC throttle high");
147  }
148  break;
149  default:
150  break;
151  }
152  break;
153 
154  case FSM_STATE_ERROR:
155  switch (event)
156  {
157  case EVENT_RC_LOST:
158  set_error(ERROR_RC_LOST); // sometimes redundant, but reports RC lost error if another error
159  // got reported first
160  break;
161  case EVENT_RC_FOUND:
163  state_.failsafe = false;
164  break;
165  case EVENT_NO_ERROR:
166  state_.error = false;
168  break;
169  case EVENT_REQUEST_ARM:
171  {
173  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: Invalid mixer");
175  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: IMU not responding");
177  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: RC signal lost");
179  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: Unhealthy estimator");
181  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: Time going backwards");
183  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: IMU not calibrated");
184 
185  next_arming_error_msg_ms_ = RF_.board_.clock_millis() + 1000; // throttle messages to 1 Hz
186  }
187  break;
188  default:
189  break;
190  }
191  break;
192 
194  switch (event)
195  {
197  state_.armed = true;
199  break;
202  break;
203  case EVENT_RC_LOST:
205  break;
206  case EVENT_ERROR:
208  state_.error = true;
209  break;
210  case EVENT_NO_ERROR:
211  state_.error = false;
212  break;
213  default:
214  break;
215  }
216  break;
217 
218  case FSM_STATE_ARMED:
219  switch (event)
220  {
221  case EVENT_RC_LOST:
222  state_.failsafe = true;
226  break;
228  state_.armed = false;
229  if (state_.error)
231  else
233  break;
234  case EVENT_ERROR:
235  state_.error = true;
236  break;
237  case EVENT_NO_ERROR:
238  state_.error = false;
239  break;
240  default:
241  break;
242  }
243  break;
244 
245  case FSM_STATE_FAILSAFE:
246  switch (event)
247  {
248  case EVENT_ERROR:
249  state_.error = true;
250  break;
252  state_.armed = false;
254  break;
255  case EVENT_RC_FOUND:
256  state_.failsafe = false;
259  break;
260  default:
261  break;
262  }
263  break;
264  default:
265  break;
266  }
267 
268  // If there has been a change, then report it to the user
269  if (start_state != fsm_state_ || state_.error_codes != start_errors)
271 }
272 
273 void StateManager::write_backup_data(const BackupData::DebugInfo &debug)
274 {
275  BackupData data;
276  data.reset_count = hardfault_count_ + 1;
277  data.error_code = state_.error_codes;
278  data.arm_flag = state_.armed ? BackupData::ARM_MAGIC : 0;
279  data.debug = debug;
280 
281  data.finalize();
282  RF_.board_.backup_memory_write(reinterpret_cast<const void *>(&data), sizeof(data));
283 }
284 
286 {
287  // reinitialize to make sure backup memory is in a good state
289 
290  // check for hardfault recovery data in backup memory
291  BackupData data;
292  if (RF_.board_.backup_memory_read(reinterpret_cast<void *>(&data), sizeof(data)))
293  {
294  if (data.valid_checksum())
295  {
297 
298  if (data.arm_flag == BackupData::ARM_MAGIC)
299  {
300  // do emergency rearm if in a good state
302  {
303  state_.armed = true;
305  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_CRITICAL, "Rearming after hardfault!!!");
306  }
307  else
308  {
309  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_CRITICAL, "Failed to rearm after hardfault!!!");
310  }
311  }
312 
313  // queue sending backup data over comm link
315  RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_CRITICAL, "Recovered from hardfault!!!");
316  }
317 
318  RF_.board_.backup_memory_clear(sizeof(data));
319  }
320 }
321 
323 {
324  if (state_.error_codes)
326  else
328 }
329 
331 {
332  // blink fast if in failsafe
333  if (state_.failsafe)
334  {
336  {
339  }
340  }
341  // blink slowly if in error
342  else if (state_.error)
343  {
345  {
348  }
349  }
350  // off if disarmed, on if armed
351  else if (!state_.armed)
352  RF_.board_.led1_off();
353  else
354  RF_.board_.led1_on();
355 }
356 
357 } // namespace rosflight_firmware
void write_backup_data(const BackupData::DebugInfo &debug)
Write recovery data to backup memory in the case of a hard fault.
static volatile bool error
Definition: drv_i2c.c:92
void log(CommLinkInterface::LogSeverity severity, const char *fmt,...)
virtual void backup_memory_write(const void *src, size_t len)=0
bool switch_on(Switch channel)
Definition: rc.cpp:89
float stick(Stick channel)
Definition: rc.cpp:84
virtual void backup_memory_init()=0
float get_param_float(uint16_t id) const
Get the value of a floating point parameter by id.
Definition: param.h:326
virtual void backup_memory_clear(size_t len)=0
bool start_gyro_calibration(void)
Definition: sensors.cpp:264
void set_error(uint16_t error)
void clear_error(uint16_t error)
void send_backup_data(const StateManager::BackupData &backup_data)
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:319
virtual void led1_off()=0
virtual bool backup_memory_read(void *dest, size_t len)=0


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