fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

atl_mantis-edu front and rear status LEDS

Browse Source
This commit is contained in:
Daniel Agar 2021-08-03 10:26:43 -04:00
parent 89b920333b
commit 2e02ad7c4e
5 changed files with 99 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
boards/atl/mantis-edu/init/rc.board_defaults
View File

@ -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

4
boards/atl/mantis-edu/src/board_config.h
View File

@ -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)

83
src/modules/events/set_leds.cpp
View File

@ -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 <board_config.h>
#include <px4_log.h>
#include <matrix/math.hpp>
#include <drivers/drv_led.h>
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 */

3
src/modules/events/status_display.cpp
View File

@ -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);
}

26
src/modules/events/status_display.h
View File

@ -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 @@ protected:
/** 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 */