Commander: run preflight checks on a regular period if the vehicle is…

… unarmed
PX4 · Sep 19, 2019 · fb9cfdc · fb9cfdc
1 parent 555586c
commit fb9cfdc
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 0 deletions.
diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
@@ -1740,6 +1740,8 @@ Commander::run()
 
 		battery_status_check();
 
+		bool force_preflight_checks = false;
+
 		/* update subsystem info which arrives from outside of commander*/
 		do {
 			if (subsys_sub.updated()) {
@@ -1763,9 +1765,18 @@ Commander::run()
 				}
 
 				status_changed = true;
+				force_preflight_checks = true;
 			}
 		} while (updated);
 
+		// if not armed run pre flight checks every 2s or when the IMU health message is received
+		if (!armed.armed && (hrt_elapsed_time(&_last_preflight_check_time) > 2_s || force_preflight_checks)) {
+			preflight_check(false);
+			status_changed = true;
+			_last_preflight_check_time = hrt_absolute_time();
+		}
+
+
 		/* If in INIT state, try to proceed to STANDBY state */
 		if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
 

diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp
@@ -198,6 +198,8 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 					    subsystem_info_s::SUBSYSTEM_TYPE_MAG | subsystem_info_s::SUBSYSTEM_TYPE_MAG2 |
 					    subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE;
 
+	uint64_t _last_preflight_check_time; /**< timestamp of the last preflight check initiated on the regular interval */
+
 	bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd, actuator_armed_s *armed,
 			    uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub, bool *changed);