From 2e02ad7c4e6714484d35e2797ee9e1e95ae852e0 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 3 Aug 2021 10:26:43 -0400 Subject: [PATCH] atl_mantis-edu front and rear status LEDS --- boards/atl/mantis-edu/init/rc.board_defaults | 2 + boards/atl/mantis-edu/src/board_config.h | 4 + src/modules/events/set_leds.cpp | 83 +++++++++++++++++++- src/modules/events/status_display.cpp | 3 +- src/modules/events/status_display.h | 26 +++--- 5 files changed, 99 insertions(+), 19 deletions(-) diff --git a/boards/atl/mantis-edu/init/rc.board_defaults b/boards/atl/mantis-edu/init/rc.board_defaults index ac2f76c02b..da11507457 100644 --- a/boards/atl/mantis-edu/init/rc.board_defaults +++ b/boards/atl/mantis-edu/init/rc.board_defaults @@ -14,6 +14,8 @@ param set-default EKF2_MULTI_MAG 1 param set-default SENS_IMU_MODE 0 param set-default SENS_MAG_MODE 0 +param set-default EV_TSK_STAT_DIS 1 + set LOGGER_ARGS "-m mavlink" # Start esc diff --git a/boards/atl/mantis-edu/src/board_config.h b/boards/atl/mantis-edu/src/board_config.h index 883a60056b..684884594a 100644 --- a/boards/atl/mantis-edu/src/board_config.h +++ b/boards/atl/mantis-edu/src/board_config.h @@ -118,6 +118,10 @@ #define BOARD_TAP_ESC_MODE 2 // select closed-loop control mode for the esc // #define BOARD_USE_ESC_CURRENT_REPORT +// LED mapping +#define BOARD_FRONT_LED_MASK (1 << 0) | (1 << 3) +#define BOARD_REAR_LED_MASK (1 << 1) | (1 << 2) + /* HEATER */ #define GPIO_HEATER_OUTPUT /* PA7 T14CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) diff --git a/src/modules/events/set_leds.cpp b/src/modules/events/set_leds.cpp index ed441f5c1d..4e79025b0d 100644 --- a/src/modules/events/set_leds.cpp +++ b/src/modules/events/set_leds.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,6 +40,13 @@ #include "status_display.h" +#include +#include +#include +#include + +using namespace time_literals; + namespace events { namespace status @@ -47,7 +54,79 @@ namespace status void StatusDisplay::set_leds() { - // Put your LED handling here + bool gps_lock_valid = _vehicle_status_flags_sub.get().condition_global_position_valid; + bool home_position_valid = _vehicle_status_flags_sub.get().condition_home_position_valid; + int nav_state = _vehicle_status_sub.get().nav_state; + +#if defined(BOARD_FRONT_LED_MASK) + + // try to publish the static LED for the first 10s + // this avoid the problem if a LED driver did not subscribe to the topic yet + if (hrt_absolute_time() < 10_s) { + + // set the base color for front LED + _led_control.led_mask = BOARD_FRONT_LED_MASK; + _led_control.color = led_control_s::COLOR_WHITE; + _led_control.mode = led_control_s::MODE_ON; + + publish(); + } + +#endif // BOARD_FRONT_LED_MASK + +#if defined(BOARD_REAR_LED_MASK) + // set the led mask for the status led which are the back LED + _led_control.led_mask = BOARD_REAR_LED_MASK; + + if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL + || nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) { + _led_control.color = led_control_s::COLOR_PURPLE; + + } else if (nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL) { + _led_control.color = led_control_s::COLOR_BLUE; + + } else if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) { + _led_control.color = led_control_s::COLOR_GREEN; + + } else { + _led_control.color = led_control_s::COLOR_YELLOW; // TODO fix yellow and purple error + } + + // blink if no GPS and home are set + if (gps_lock_valid && home_position_valid) { + _led_control.mode = led_control_s::MODE_ON; + + } else { + _led_control.mode = led_control_s::MODE_BLINK_NORMAL; + } + + // handle battery warnings, once a state is reached it can not be reset + if (_battery_status_sub.get().warning == battery_status_s::BATTERY_WARNING_CRITICAL || _critical_battery) { + _led_control.color = led_control_s::COLOR_RED; + _led_control.mode = led_control_s::MODE_BLINK_FAST; + _critical_battery = true; + + } else if (_battery_status_sub.get().warning == battery_status_s::BATTERY_WARNING_LOW || _low_battery) { + _led_control.color = led_control_s::COLOR_RED; + _led_control.mode = led_control_s::MODE_FLASH; + _low_battery = true; + } + + if (nav_state != _old_nav_state + || gps_lock_valid != _old_gps_lock_valid + || home_position_valid != _old_home_position_valid + || _battery_status_sub.get().warning != _old_battery_status_warning) { + + publish(); + } + +#endif // BOARD_REAR_LED_MASK + + // copy actual state + _old_nav_state = nav_state; + _old_gps_lock_valid = gps_lock_valid; + _old_home_position_valid = home_position_valid; + _old_battery_status_warning = _battery_status_sub.get().warning; } } /* namespace status */ diff --git a/src/modules/events/status_display.cpp b/src/modules/events/status_display.cpp index b4fcc6b17d..e2f127314f 100644 --- a/src/modules/events/status_display.cpp +++ b/src/modules/events/status_display.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -96,7 +96,6 @@ void StatusDisplay::process() void StatusDisplay::publish() { _led_control.timestamp = hrt_absolute_time(); - _led_control_pub.publish(_led_control); } diff --git a/src/modules/events/status_display.h b/src/modules/events/status_display.h index f70ce1006d..259b422900 100644 --- a/src/modules/events/status_display.h +++ b/src/modules/events/status_display.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -47,7 +47,6 @@ #include #include #include -#include #include #include @@ -80,25 +79,22 @@ protected: /** publish LED control */ void publish(); - // TODO: review if there is a better variant that allocates this in the memory - uORB::SubscriptionData _battery_status_sub{ORB_ID(battery_status)}; - uORB::SubscriptionData _cpu_load_sub{ORB_ID(cpuload)}; - uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::SubscriptionData _battery_status_sub{ORB_ID(battery_status)}; + uORB::SubscriptionData _cpu_load_sub{ORB_ID(cpuload)}; + uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::SubscriptionData _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)}; - uORB::SubscriptionData _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; - struct led_control_s _led_control = {}; + led_control_s _led_control{}; private: - bool _old_gps_lock_valid = false; - bool _old_home_position_valid = false; - bool _low_battery = false; - bool _critical_battery = false; - int _old_nav_state = -1; - int _old_battery_status_warning = -1; - uORB::Publication _led_control_pub{ORB_ID(led_control)}; + bool _old_gps_lock_valid{false}; + bool _old_home_position_valid{false}; + bool _low_battery{false}; + bool _critical_battery{false}; + int _old_nav_state{-1}; + int _old_battery_status_warning{-1}; }; } /* namespace status */