atl_mantis-edu front and rear status LEDS

dagar · dagar · commit 2e02ad7c4e67 · 2021-08-04T17:15:24.000-04:00
diff --git 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
@@ -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
@@ -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,14 +40,93 @@
 
 #include "status_display.h"
 
+#include <board_config.h>
+#include <px4_log.h>
+#include <matrix/math.hpp>
+#include <drivers/drv_led.h>
+
+using namespace time_literals;
+
 namespace events
 {
 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
@@ -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
@@ -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 <uORB/topics/battery_status.h>
 #include <uORB/topics/cpuload.h>
 #include <uORB/topics/led_control.h>
-#include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_status_flags.h>
 
@@ -80,25 +79,22 @@ class StatusDisplay
 	/** publish LED control */
 	void publish();
 
-	// TODO: review if there is a better variant that allocates this in the memory
-	uORB::SubscriptionData<battery_status_s> _battery_status_sub{ORB_ID(battery_status)};
-	uORB::SubscriptionData<cpuload_s> _cpu_load_sub{ORB_ID(cpuload)};
-	uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::SubscriptionData<battery_status_s>       _battery_status_sub{ORB_ID(battery_status)};
+	uORB::SubscriptionData<cpuload_s>              _cpu_load_sub{ORB_ID(cpuload)};
+	uORB::SubscriptionData<vehicle_status_s>       _vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::SubscriptionData<vehicle_status_flags_s> _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)};
-	uORB::SubscriptionData<vehicle_attitude_s> _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_s> _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 */

Original file line number	Diff line number	Diff line change
`@@ -1,6 +1,6 @@`
`1`	`1`	`/****************************************************************************`
`2`	`2`	`*`
`3`		`- * Copyright (c) 2017 PX4 Development Team. All rights reserved.`
	`3`	`+ * Copyright (c) 2017-2021 PX4 Development Team. All rights reserved.`
`4`	`4`	`*`
`5`	`5`	`* Redistribution and use in source and binary forms, with or without`
`6`	`6`	`* modification, are permitted provided that the following conditions`
`@@ -96,7 +96,6 @@ void StatusDisplay::process()`
`96`	`96`	`void StatusDisplay::publish()`
`97`	`97`	`{`
`98`	`98`	`_led_control.timestamp = hrt_absolute_time();`
`99`		`-`
`100`	`99`	`_led_control_pub.publish(_led_control);`
`101`	`100`	`}`
`102`	`101`