failsafe.cpp 15 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495
  1. #include "Sub.h"
  2. /*
  3. * failsafe.cpp
  4. * Failsafe checks and actions
  5. */
  6. static bool failsafe_enabled = false;
  7. static uint16_t failsafe_last_ticks;
  8. static uint32_t failsafe_last_timestamp;
  9. static bool in_failsafe;
  10. // Enable mainloop lockup failsafe
  11. void Sub::mainloop_failsafe_enable()
  12. {
  13. failsafe_enabled = true;
  14. failsafe_last_timestamp = AP_HAL::micros();
  15. }
  16. // Disable mainloop lockup failsafe
  17. // Used when we know we are going to delay the mainloop significantly.
  18. void Sub::mainloop_failsafe_disable()
  19. {
  20. failsafe_enabled = false;
  21. }
  22. // This function is called from the core timer interrupt at 1kHz.
  23. // This checks that the mainloop is running, and has not locked up.
  24. void Sub::mainloop_failsafe_check()
  25. {
  26. uint32_t tnow = AP_HAL::micros();
  27. const uint16_t ticks = scheduler.ticks();
  28. if (ticks != failsafe_last_ticks) {
  29. // the main loop is running, all is OK
  30. failsafe_last_ticks = ticks;
  31. failsafe_last_timestamp = tnow;
  32. if (in_failsafe) {
  33. in_failsafe = false;
  34. AP::logger().Write_Error(LogErrorSubsystem::CPU,LogErrorCode::FAILSAFE_RESOLVED);
  35. }
  36. return;
  37. }
  38. if (!in_failsafe && failsafe_enabled && tnow - failsafe_last_timestamp > 2000000) {
  39. // motors are running but we have gone 2 second since the
  40. // main loop ran. That means we're in trouble and should
  41. // disarm the motors.
  42. in_failsafe = true;
  43. // reduce motors to minimum (we do not immediately disarm because we want to log the failure)
  44. if (motors.armed()) {
  45. motors.output_min();
  46. }
  47. AP::logger().Write_Error(LogErrorSubsystem::CPU,LogErrorCode::FAILSAFE_OCCURRED);
  48. }
  49. if (failsafe_enabled && in_failsafe && tnow - failsafe_last_timestamp > 1000000) {
  50. // disarm motors every second
  51. failsafe_last_timestamp = tnow;
  52. if (motors.armed()) {
  53. arming.disarm();
  54. motors.output();
  55. }
  56. }
  57. }
  58. void Sub::failsafe_sensors_check()
  59. {
  60. if (!ap.depth_sensor_present) {
  61. return;
  62. }
  63. // We need a depth sensor to do any sort of auto z control
  64. if (sensor_health.depth) {
  65. if (failsafe.sensor_health) {
  66. AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_SENSORS, LogErrorCode::ERROR_RESOLVED);
  67. failsafe.sensor_health = false;
  68. }
  69. return;
  70. }
  71. // only report once
  72. if (failsafe.sensor_health) {
  73. return;
  74. }
  75. failsafe.sensor_health = true;
  76. gcs().send_text(MAV_SEVERITY_CRITICAL, "Depth sensor error!");
  77. AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_SENSORS, LogErrorCode::BAD_DEPTH);
  78. if (control_mode == ALT_HOLD || control_mode == SURFACE || mode_requires_GPS(control_mode)) {
  79. // This should always succeed
  80. if (!set_mode(MANUAL, MODE_REASON_BAD_DEPTH)) {
  81. // We should never get here
  82. arming.disarm();
  83. }
  84. }
  85. }
  86. void Sub::failsafe_ekf_check()
  87. {
  88. static uint32_t last_ekf_good_ms = 0;
  89. if (g.fs_ekf_action == FS_EKF_ACTION_DISABLED) {
  90. last_ekf_good_ms = AP_HAL::millis();
  91. failsafe.ekf = false;
  92. AP_Notify::flags.ekf_bad = false;
  93. return;
  94. }
  95. float posVar, hgtVar, tasVar;
  96. Vector3f magVar;
  97. Vector2f offset;
  98. float compass_variance;
  99. float vel_variance;
  100. ahrs.get_variances(vel_variance, posVar, hgtVar, magVar, tasVar, offset);
  101. compass_variance = magVar.length();
  102. if (compass_variance < g.fs_ekf_thresh && vel_variance < g.fs_ekf_thresh) {
  103. last_ekf_good_ms = AP_HAL::millis();
  104. failsafe.ekf = false;
  105. AP_Notify::flags.ekf_bad = false;
  106. return;;
  107. }
  108. // Bad EKF for 2 solid seconds triggers failsafe
  109. if (AP_HAL::millis() < last_ekf_good_ms + 2000) {
  110. failsafe.ekf = false;
  111. AP_Notify::flags.ekf_bad = false;
  112. return;
  113. }
  114. // Only trigger failsafe once
  115. if (failsafe.ekf) {
  116. return;
  117. }
  118. failsafe.ekf = true;
  119. AP_Notify::flags.ekf_bad = true;
  120. AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE);
  121. if (AP_HAL::millis() > failsafe.last_ekf_warn_ms + 20000) {
  122. failsafe.last_ekf_warn_ms = AP_HAL::millis();
  123. gcs().send_text(MAV_SEVERITY_WARNING, "EKF bad");
  124. }
  125. if (g.fs_ekf_action == FS_EKF_ACTION_DISARM) {
  126. arming.disarm();
  127. }
  128. }
  129. // Battery failsafe handler
  130. void Sub::handle_battery_failsafe(const char* type_str, const int8_t action)
  131. {
  132. AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_BATT, LogErrorCode::FAILSAFE_OCCURRED);
  133. switch((Failsafe_Action)action) {
  134. case Failsafe_Action_Surface:
  135. set_mode(SURFACE, MODE_REASON_BATTERY_FAILSAFE);
  136. break;
  137. case Failsafe_Action_Disarm:
  138. arming.disarm();
  139. break;
  140. case Failsafe_Action_Warn:
  141. case Failsafe_Action_None:
  142. break;
  143. }
  144. }
  145. // Make sure that we are receiving pilot input at an appropriate interval
  146. void Sub::failsafe_pilot_input_check()
  147. {
  148. #if CONFIG_HAL_BOARD != HAL_BOARD_SITL
  149. if (g.failsafe_pilot_input == FS_PILOT_INPUT_DISABLED) {
  150. failsafe.pilot_input = false;
  151. return;
  152. }
  153. if (AP_HAL::millis() < failsafe.last_pilot_input_ms + g.failsafe_pilot_input_timeout * 1000.0f) {
  154. failsafe.pilot_input = false; // We've received an update from the pilot within the timeout period
  155. return;
  156. }
  157. if (failsafe.pilot_input) {
  158. return; // only act once
  159. }
  160. failsafe.pilot_input = true;
  161. AP::logger().Write_Error(LogErrorSubsystem::PILOT_INPUT, LogErrorCode::FAILSAFE_OCCURRED);
  162. gcs().send_text(MAV_SEVERITY_CRITICAL, "Lost manual control");
  163. set_neutral_controls();
  164. if(g.failsafe_pilot_input == FS_PILOT_INPUT_DISARM) {
  165. arming.disarm();
  166. }
  167. #endif
  168. }
  169. // Internal pressure failsafe check
  170. // Check if the internal pressure of the watertight electronics enclosure
  171. // has exceeded the maximum specified by the FS_PRESS_MAX parameter
  172. void Sub::failsafe_internal_pressure_check()
  173. {
  174. if (g.failsafe_pressure == FS_PRESS_DISABLED) {
  175. return; // Nothing to do
  176. }
  177. uint32_t tnow = AP_HAL::millis();
  178. static uint32_t last_pressure_warn_ms;
  179. static uint32_t last_pressure_good_ms;
  180. if (barometer.get_pressure(0) < g.failsafe_pressure_max) {
  181. last_pressure_good_ms = tnow;
  182. last_pressure_warn_ms = tnow;
  183. failsafe.internal_pressure = false;
  184. return;
  185. }
  186. // 2 seconds with no readings below threshold triggers failsafe
  187. if (tnow > last_pressure_good_ms + 2000) {
  188. failsafe.internal_pressure = true;
  189. }
  190. // Warn every 30 seconds
  191. if (failsafe.internal_pressure && tnow > last_pressure_warn_ms + 30000) {
  192. last_pressure_warn_ms = tnow;
  193. gcs().send_text(MAV_SEVERITY_WARNING, "Internal pressure critical!");
  194. }
  195. }
  196. // Internal temperature failsafe check
  197. // Check if the internal temperature of the watertight electronics enclosure
  198. // has exceeded the maximum specified by the FS_TEMP_MAX parameter
  199. void Sub::failsafe_internal_temperature_check()
  200. {
  201. if (g.failsafe_temperature == FS_TEMP_DISABLED) {
  202. return; // Nothing to do
  203. }
  204. uint32_t tnow = AP_HAL::millis();
  205. static uint32_t last_temperature_warn_ms;
  206. static uint32_t last_temperature_good_ms;
  207. if (barometer.get_temperature(0) < g.failsafe_temperature_max) {
  208. last_temperature_good_ms = tnow;
  209. last_temperature_warn_ms = tnow;
  210. failsafe.internal_temperature = false;
  211. return;
  212. }
  213. // 2 seconds with no readings below threshold triggers failsafe
  214. if (tnow > last_temperature_good_ms + 2000) {
  215. failsafe.internal_temperature = true;
  216. }
  217. // Warn every 30 seconds
  218. if (failsafe.internal_temperature && tnow > last_temperature_warn_ms + 30000) {
  219. last_temperature_warn_ms = tnow;
  220. gcs().send_text(MAV_SEVERITY_WARNING, "Internal temperature critical!");
  221. }
  222. }
  223. // Check if we are leaking and perform appropriate action
  224. void Sub::failsafe_leak_check()
  225. {
  226. bool status = leak_detector.get_status();
  227. // Do nothing if we are dry, or if leak failsafe action is disabled
  228. if (status == false || g.failsafe_leak == FS_LEAK_DISABLED) {
  229. if (failsafe.leak) {
  230. AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_LEAK, LogErrorCode::FAILSAFE_RESOLVED);
  231. }
  232. AP_Notify::flags.leak_detected = false;
  233. failsafe.leak = false;
  234. return;
  235. }
  236. AP_Notify::flags.leak_detected = status;
  237. uint32_t tnow = AP_HAL::millis();
  238. // We have a leak
  239. // Always send a warning every 20 seconds
  240. if (tnow > failsafe.last_leak_warn_ms + 20000) {
  241. failsafe.last_leak_warn_ms = tnow;
  242. gcs().send_text(MAV_SEVERITY_CRITICAL, "Leak Detected");
  243. }
  244. // Do nothing if we have already triggered the failsafe action, or if the motors are disarmed
  245. if (failsafe.leak) {
  246. return;
  247. }
  248. failsafe.leak = true;
  249. AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_LEAK, LogErrorCode::FAILSAFE_OCCURRED);
  250. // Handle failsafe action
  251. if (failsafe.leak && g.failsafe_leak == FS_LEAK_SURFACE && motors.armed()) {
  252. set_mode(SURFACE, MODE_REASON_LEAK_FAILSAFE);
  253. }
  254. }
  255. // failsafe_gcs_check - check for ground station failsafe
  256. void Sub::failsafe_gcs_check()
  257. {
  258. // return immediately if we have never had contact with a gcs, or if gcs failsafe action is disabled
  259. // this also checks to see if we have a GCS failsafe active, if we do, then must continue to process the logic for recovery from this state.
  260. if (failsafe.last_heartbeat_ms == 0 || (!g.failsafe_gcs && g.failsafe_gcs == FS_GCS_DISABLED)) {
  261. return;
  262. }
  263. uint32_t tnow = AP_HAL::millis();
  264. // Check if we have gotten a GCS heartbeat recently (GCS sysid must match SYSID_MYGCS parameter)
  265. if (tnow - failsafe.last_heartbeat_ms < FS_GCS_TIMEOUT_MS) {
  266. // Log event if we are recovering from previous gcs failsafe
  267. if (failsafe.gcs) {
  268. AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED);
  269. }
  270. failsafe.gcs = false;
  271. return;
  272. }
  273. //////////////////////////////
  274. // GCS heartbeat has timed out
  275. //////////////////////////////
  276. // Send a warning every 30 seconds
  277. if (tnow - failsafe.last_gcs_warn_ms > 30000) {
  278. failsafe.last_gcs_warn_ms = tnow;
  279. gcs().send_text(MAV_SEVERITY_WARNING, "MYGCS: %u, heartbeat lost", g.sysid_my_gcs.get());
  280. }
  281. // do nothing if we have already triggered the failsafe action, or if the motors are disarmed
  282. if (failsafe.gcs || !motors.armed()) {
  283. return;
  284. }
  285. failsafe.gcs = true;
  286. AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_OCCURRED);
  287. // handle failsafe action
  288. if (g.failsafe_gcs == FS_GCS_DISARM) {
  289. arming.disarm();
  290. } else if (g.failsafe_gcs == FS_GCS_HOLD && motors.armed()) {
  291. if (!set_mode(ALT_HOLD, MODE_REASON_GCS_FAILSAFE)) {
  292. arming.disarm();
  293. }
  294. } else if (g.failsafe_gcs == FS_GCS_SURFACE && motors.armed()) {
  295. if (!set_mode(SURFACE, MODE_REASON_GCS_FAILSAFE)) {
  296. arming.disarm();
  297. }
  298. }
  299. }
  300. #define CRASH_CHECK_TRIGGER_MS 2000 // 2 seconds inverted indicates a crash
  301. #define CRASH_CHECK_ANGLE_DEVIATION_DEG 30.0f // 30 degrees beyond angle max is signal we are inverted
  302. // Check for a crash
  303. // The vehicle is considered crashed if the angle error exceeds a specified limit for more than 2 seconds
  304. void Sub::failsafe_crash_check()
  305. {
  306. static uint32_t last_crash_check_pass_ms;
  307. uint32_t tnow = AP_HAL::millis();
  308. // return immediately if disarmed, or crash checking disabled
  309. if (!motors.armed() || g.fs_crash_check == FS_CRASH_DISABLED) {
  310. last_crash_check_pass_ms = tnow;
  311. failsafe.crash = false;
  312. return;
  313. }
  314. // return immediately if we are not in an angle stabilized flight mode
  315. if (control_mode == ACRO || control_mode == MANUAL) {
  316. last_crash_check_pass_ms = tnow;
  317. failsafe.crash = false;
  318. return;
  319. }
  320. // check for angle error over 30 degrees
  321. const float angle_error = attitude_control.get_att_error_angle_deg();
  322. if (angle_error <= CRASH_CHECK_ANGLE_DEVIATION_DEG) {
  323. last_crash_check_pass_ms = tnow;
  324. failsafe.crash = false;
  325. return;
  326. }
  327. if (tnow < last_crash_check_pass_ms + CRASH_CHECK_TRIGGER_MS) {
  328. return;
  329. }
  330. // Conditions met, we are in failsafe
  331. // Send warning to GCS
  332. if (tnow > failsafe.last_crash_warn_ms + 20000) {
  333. failsafe.last_crash_warn_ms = tnow;
  334. gcs().send_text(MAV_SEVERITY_WARNING,"Crash detected");
  335. }
  336. // Only perform failsafe action once
  337. if (failsafe.crash) {
  338. return;
  339. }
  340. failsafe.crash = true;
  341. AP::logger().Write_Error(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_CRASH);
  342. // disarm motors
  343. if (g.fs_crash_check == FS_CRASH_DISARM) {
  344. arming.disarm();
  345. }
  346. }
  347. // executes terrain failsafe if data is missing for longer than a few seconds
  348. // missing_data should be set to true if the vehicle failed to navigate because of missing data, false if navigation is proceeding successfully
  349. void Sub::failsafe_terrain_check()
  350. {
  351. // trigger with 5 seconds of failures while in AUTO mode
  352. bool valid_mode = (control_mode == AUTO || control_mode == GUIDED);
  353. bool timeout = (failsafe.terrain_last_failure_ms - failsafe.terrain_first_failure_ms) > FS_TERRAIN_TIMEOUT_MS;
  354. bool trigger_event = valid_mode && timeout;
  355. // check for clearing of event
  356. if (trigger_event != failsafe.terrain) {
  357. if (trigger_event) {
  358. gcs().send_text(MAV_SEVERITY_CRITICAL,"Failsafe terrain triggered");
  359. failsafe_terrain_on_event();
  360. } else {
  361. AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::ERROR_RESOLVED);
  362. failsafe.terrain = false;
  363. }
  364. }
  365. }
  366. // This gets called if mission items are in ALT_ABOVE_TERRAIN frame
  367. // Terrain failure occurs when terrain data is not found, or rangefinder is not enabled or healthy
  368. // set terrain data status (found or not found)
  369. void Sub::failsafe_terrain_set_status(bool data_ok)
  370. {
  371. uint32_t now = AP_HAL::millis();
  372. // record time of first and latest failures (i.e. duration of failures)
  373. if (!data_ok) {
  374. failsafe.terrain_last_failure_ms = now;
  375. if (failsafe.terrain_first_failure_ms == 0) {
  376. failsafe.terrain_first_failure_ms = now;
  377. }
  378. } else {
  379. // failures cleared after 0.1 seconds of persistent successes
  380. if (now - failsafe.terrain_last_failure_ms > 100) {
  381. failsafe.terrain_last_failure_ms = 0;
  382. failsafe.terrain_first_failure_ms = 0;
  383. }
  384. }
  385. }
  386. // terrain failsafe action
  387. void Sub::failsafe_terrain_on_event()
  388. {
  389. failsafe.terrain = true;
  390. AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::FAILSAFE_OCCURRED);
  391. // If rangefinder is enabled, we can recover from this failsafe
  392. if (!rangefinder_state.enabled || !auto_terrain_recover_start()) {
  393. failsafe_terrain_act();
  394. }
  395. }
  396. // Recovery failed, take action
  397. void Sub::failsafe_terrain_act()
  398. {
  399. switch (g.failsafe_terrain) {
  400. case FS_TERRAIN_HOLD:
  401. if (!set_mode(POSHOLD, MODE_REASON_TERRAIN_FAILSAFE)) {
  402. set_mode(ALT_HOLD, MODE_REASON_TERRAIN_FAILSAFE);
  403. }
  404. AP_Notify::events.failsafe_mode_change = 1;
  405. break;
  406. case FS_TERRAIN_SURFACE:
  407. set_mode(SURFACE, MODE_REASON_TERRAIN_FAILSAFE);
  408. AP_Notify::events.failsafe_mode_change = 1;
  409. break;
  410. case FS_TERRAIN_DISARM:
  411. default:
  412. arming.disarm();
  413. }
  414. }