diff --git a/platforms/common/include/px4_platform_common/log.h b/platforms/common/include/px4_platform_common/log.h index 3a781ca411..8e709d86ae 100644 --- a/platforms/common/include/px4_platform_common/log.h +++ b/platforms/common/include/px4_platform_common/log.h @@ -424,3 +424,13 @@ __END_DECLS #define PX4_LOG_NAMED(name, FMT, ...) __px4_log_named_cond(name, true, FMT, ##__VA_ARGS__) #define PX4_LOG_NAMED_COND(name, cond, FMT, ...) __px4_log_named_cond(name, cond, FMT, ##__VA_ARGS__) #endif + +#define PX4_ANSI_COLOR_RED "\x1b[31m" +#define PX4_ANSI_COLOR_GREEN "\x1b[32m" +#define PX4_ANSI_COLOR_YELLOW "\x1b[33m" +#define PX4_ANSI_COLOR_BLUE "\x1b[34m" +#define PX4_ANSI_COLOR_MAGENTA "\x1b[35m" +#define PX4_ANSI_COLOR_CYAN "\x1b[36m" +#define PX4_ANSI_COLOR_GRAY "\x1B[37m" +#define PX4_ANSI_COLOR_RESET "\x1b[0m" + diff --git a/platforms/common/px4_log.cpp b/platforms/common/px4_log.cpp index 0883784fb2..28cc92f3ca 100644 --- a/platforms/common/px4_log.cpp +++ b/platforms/common/px4_log.cpp @@ -54,15 +54,6 @@ static orb_advert_t orb_log_message_pub = nullptr; __EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1] = { "DEBUG", "INFO", "WARN", "ERROR", "PANIC" }; -#define PX4_ANSI_COLOR_RED "\x1b[31m" -#define PX4_ANSI_COLOR_GREEN "\x1b[32m" -#define PX4_ANSI_COLOR_YELLOW "\x1b[33m" -#define PX4_ANSI_COLOR_BLUE "\x1b[34m" -#define PX4_ANSI_COLOR_MAGENTA "\x1b[35m" -#define PX4_ANSI_COLOR_CYAN "\x1b[36m" -#define PX4_ANSI_COLOR_GRAY "\x1B[37m" -#define PX4_ANSI_COLOR_RESET "\x1b[0m" - static constexpr const char *__px4_log_level_color[_PX4_LOG_LEVEL_PANIC + 1] { PX4_ANSI_COLOR_GREEN, // DEBUG PX4_ANSI_COLOR_RESET, // INFO diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 5a6014ac65..e2e5a79fed 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2963,6 +2963,8 @@ Commander::run() _vehicle_status_flags.pre_flight_checks_pass = _health_and_arming_checks.canArm( _vehicle_status.nav_state); perf_end(_preflight_check_perf); + + check_and_inform_ready_for_takeoff(); } // publish actuator_armed first (used by output modules) @@ -3099,6 +3101,23 @@ Commander::get_circuit_breaker_params() CBRK_VTOLARMING_KEY); } +void Commander::check_and_inform_ready_for_takeoff() +{ +#ifdef CONFIG_ARCH_BOARD_PX4_SITL + static bool ready_for_takeoff_printed = false; + + if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || + _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + if (!ready_for_takeoff_printed && + _health_and_arming_checks.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF)) { + PX4_INFO("%sReady for takeoff!%s", PX4_ANSI_COLOR_GREEN, PX4_ANSI_COLOR_RESET); + ready_for_takeoff_printed = true; + } + } + +#endif // CONFIG_ARCH_BOARD_PX4_SITL +} + void Commander::control_status_leds(bool changed, const uint8_t battery_warning) { switch (blink_msg_state()) { diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index d38c5ade7b..5753212165 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -186,6 +186,8 @@ private: void updateParameters(); + void check_and_inform_ready_for_takeoff(); + DEFINE_PARAMETERS( (ParamInt) _param_nav_dll_act,