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

3 files changed, 49 insertions, 8 deletions

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 8318238e2..0f90db858 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1519,8 +1519,8 @@ PX4IO::print_status()
 	uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
 	printf("arming 0x%04x%s%s%s%s%s%s\n",
 		arming,
-		((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)          ? " FMU_ARMED" : ""),
-		((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)	    ? " IO_ARM_OK" : ""),
+		((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)          ? " FMU_ARMED" : " FMU_DISARMED"),
+		((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)	    ? " IO_ARM_OK" : " IO_ARM_DENIED"),
 		((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
 		((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)   ? " FAILSAFE_CUSTOM" : ""),
 		((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)   ? " INAIR_RESTART_OK" : ""),
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 */
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 5842a33b1..1e31573d6 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -119,7 +119,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
 			{
 				ret = TRANSITION_CHANGED;
 				armed->armed = true;
-				armed->ready_to_arm = false;
+				armed->ready_to_arm = true;
 			}
 
 			break;