From c67d85945ba26a1c45e92110bb52450b6e76cbed Mon Sep 17 00:00:00 2001
From: Inhwan Wee <10ezenfinani@gmail.com>
Date: Mon, 15 Mar 2021 22:09:43 +0900
Subject: [PATCH] [commander] more detailed print_status

---
 src/modules/commander/Commander.cpp           |  1 +
 .../commander/state_machine_helper.cpp        | 26 +++++++++++++++++++
 src/modules/commander/state_machine_helper.h  |  1 +
 3 files changed, 28 insertions(+)

diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index 94e696d592..d667d162a7 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -384,6 +384,7 @@ int Commander::custom_command(int argc, char *argv[])
 int Commander::print_status()
 {
 	PX4_INFO("arming: %s", arming_state_names[_status.arming_state]);
+	PX4_INFO("navigation: %s", nav_state_names[_status.nav_state]);
 	return 0;
 }
 
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 595426b5f0..eca44ec13e 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -85,6 +85,32 @@ const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = {
 	"IN_AIR_RESTORE",
 };
 
+// You can index into the array with an navigation_state_t in order to get its textual representation
+const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
+	"MANUAL",
+	"ALTCTL",
+	"POSCTL",
+	"AUTO_MISSION",
+	"AUTO_LOITER",
+	"AUTO_RTL",
+	"6: unallocated",
+	"7: unallocated",
+	"AUTO_LANDENGFAIL",
+	"AUTO_LANDGPSFAIL",
+	"ACRO",
+	"11: UNUSED",
+	"DESCEND",
+	"TERMINATION",
+	"OFFBOARD",
+	"STAB",
+	"16: UNUSED2",
+	"AUTO_TAKEOFF",
+	"AUTO_LAND",
+	"AUTO_FOLLOW_TARGET",
+	"AUTO_PRECLAND",
+	"ORBIT"
+};
+
 static hrt_abstime last_preflight_check = 0;	///< initialize so it gets checked immediately
 
 void set_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed,
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index fc28a54de1..b415a7a9dc 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -96,6 +96,7 @@ enum class position_nav_loss_actions_t {
 };
 
 extern const char *const arming_state_names[];
+extern const char *const nav_state_names[];
 
 enum class arm_disarm_reason_t {
 	TRANSITION_TO_STANDBY = 0,