Better transparency in IO mode display, fixes to commander arming, minimum publishing rate for arming

1 files changed, 46 insertions, 5 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 30906034e..12543800e 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -273,6 +273,43 @@ void usage(const char *reason)
 
 void print_status() {
 	warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
+
+	/* read all relevant states */
+	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+	struct vehicle_status_s state;
+	orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+
+	const char* armed_str;
+
+	switch (state.arming_state) {
+		case ARMING_STATE_INIT:
+			armed_str = "INIT";
+			break;
+		case ARMING_STATE_STANDBY:
+			armed_str = "STANDBY";
+			break;
+		case ARMING_STATE_ARMED:
+			armed_str = "ARMED";
+			break;
+		case ARMING_STATE_ARMED_ERROR:
+			armed_str = "ARMED_ERROR";
+			break;
+		case ARMING_STATE_STANDBY_ERROR:
+			armed_str = "STANDBY_ERROR";
+			break;
+		case ARMING_STATE_REBOOT:
+			armed_str = "REBOOT";
+			break;
+		case ARMING_STATE_IN_AIR_RESTORE:
+			armed_str = "IN_AIR_RESTORE";
+			break;
+		default:
+			armed_str = "ERR: UNKNOWN STATE";
+			break;
+	}
+
+
+	warnx("arming: %s", armed_str);
 }
 
 void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
@@ -945,9 +982,9 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* store current state to reason later about a state change */
 		// bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok;
-		bool global_pos_valid = status.condition_global_position_valid;
-		bool local_pos_valid = status.condition_local_position_valid;
-		bool airspeed_valid = status.condition_airspeed_valid;
+		// bool global_pos_valid = status.condition_global_position_valid;
+		// bool local_pos_valid = status.condition_local_position_valid;
+		// bool airspeed_valid = status.condition_airspeed_valid;
 
 
 		/* check for global or local position updates, set a timeout of 2s */
@@ -1274,11 +1311,15 @@ int commander_thread_main(int argc, char *argv[])
 			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
 		}
 
-		/* publish vehicle status at least with 1 Hz */
-		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
+		/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
+		if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
 			status.counter++;
 			status.timestamp = t;
 			orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+			control_mode.timestamp = t;
+			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
+			armed.timestamp = t;
+			orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
 		}
 
 		/* play arming and battery warning tunes */