zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

WS2812 OK

master
z 5 years ago
parent
056d98ed63
commit
42bd7f3b1f
7 changed files with 166 additions and 5 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 10
      libraries/AP_Notify/AP_Notify.cpp
  2. 2
      libraries/AP_Notify/AP_Notify.h
  3. 2
      libraries/AP_Notify/NotifyDevice.h
  4. 131
      libraries/AP_Notify/RGBLed.cpp
  5. 5
      libraries/AP_Notify/RGBLed.h
  6. 19
      libraries/AP_Notify/ToshibaLED_I2C.cpp
  7. 2
      libraries/AP_Notify/ToshibaLED_I2C.h

10
libraries/AP_Notify/AP_Notify.cpp
Unescape View File

@ -352,6 +352,16 @@ void AP_Notify::handle_led_control(const mavlink_message_t &msg) @@ -352,6 +352,16 @@ void AP_Notify::handle_led_control(const mavlink_message_t &msg)
}
}
void AP_Notify::set_rgb_mode(uint8_t mode)
{
for (uint8_t i = 0; i < _num_devices; i++) {
if (_devices[i] != nullptr) {
_devices[i]->set_rgb_mode(mode);
}
}
}
// handle a PLAY_TUNE message
void AP_Notify::handle_play_tune(const mavlink_message_t &msg)
{

2
libraries/AP_Notify/AP_Notify.h
Unescape View File

@ -160,6 +160,8 @@ public: @@ -160,6 +160,8 @@ public:
uint8_t get_buzz_level() const { return _buzzer_level; }
uint8_t get_buzz_volume() const { return _buzzer_volume; }
static void set_rgb_mode(uint8_t mode); // @Brown
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
HAL_Semaphore sf_window_mutex;
#endif

2
libraries/AP_Notify/NotifyDevice.h
Unescape View File

@ -23,6 +23,8 @@ public: @@ -23,6 +23,8 @@ public:
// play a MML tune
virtual void play_tune(const char *tune) {}
virtual void set_rgb_mode(uint8_t mode) {} // @Brown
// this pointer is used to read the parameters relative to devices
const AP_Notify *pNotify;
};

131
libraries/AP_Notify/RGBLed.cpp
Unescape View File

@ -21,6 +21,9 @@ @@ -21,6 +21,9 @@
#include "RGBLed.h"
#include "AP_Notify.h"
#ifndef USE_WS2812
#define USE_WS2812 1
#endif
extern const AP_HAL::HAL& hal;
RGBLed::RGBLed(uint8_t led_off, uint8_t led_bright, uint8_t led_medium, uint8_t led_dim):
@ -163,6 +166,106 @@ uint32_t RGBLed::get_colour_sequence(void) const @@ -163,6 +166,106 @@ uint32_t RGBLed::get_colour_sequence(void) const
return sequence_disarmed_bad_gps;
}
void RGBLed::get_colour_ws2812(void)
{
// initialising pattern
if (AP_Notify::flags.initialising) {
// return sequence_initialising;
_red_ws = _led_medium;
_blue_ws = _led_off;
_green_ws = _led_medium;
set_rgb_mode(1);
return;
}
// save trim and esc calibration pattern
if (AP_Notify::flags.save_trim || AP_Notify::flags.esc_calibration) {
// return sequence_trim_or_esc;
_red_ws = _led_medium;
_blue_ws = _led_medium;
_green_ws = _led_medium;
set_rgb_mode(1); // flow
return;
}
// radio and battery failsafe patter: flash yellow
// gps failsafe pattern : flashing yellow and blue
// ekf_bad pattern : flashing yellow and red
if (AP_Notify::flags.failsafe_radio ||
AP_Notify::flags.failsafe_battery ||
AP_Notify::flags.ekf_bad ||
AP_Notify::flags.gps_glitching ||
AP_Notify::flags.leak_detected) {
if (AP_Notify::flags.leak_detected) {
// purple if leak detected
// return sequence_failsafe_leak;
} else if (AP_Notify::flags.ekf_bad) {
// red on if ekf bad
// return sequence_failsafe_ekf;
} else if (AP_Notify::flags.gps_glitching) {
// blue on gps glitch
// return sequence_failsafe_gps_glitching;
}
// all off for radio or battery failsafe
// return sequence_failsafe_radio_or_battery;
_red_ws = _led_medium;
_blue_ws = _led_medium;
_green_ws = _led_off;
set_rgb_mode(1);
return;
}
// solid green or blue if armed
if (AP_Notify::flags.armed) {
// solid green if armed with GPS 3d lock
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D) {
// return sequence_armed;
_red_ws = _led_off;
_blue_ws = _led_off;
_green_ws = _led_medium;
}
// solid blue if armed with no GPS lock
// return sequence_armed_nogps;
_red_ws = _led_off;
_blue_ws = _led_medium;
_green_ws = _led_medium;
set_rgb_mode(0);
return;
}
// double flash yellow if failing pre-arm checks
if (!AP_Notify::flags.pre_arm_check) {
// return sequence_prearm_failing;
_red_ws = _led_medium;
_blue_ws = _led_off;
_green_ws = _led_medium;
set_rgb_mode(1);
return;
}
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS && AP_Notify::flags.pre_arm_gps_check) {
// return sequence_disarmed_good_dgps;
_red_ws = _led_off;
_blue_ws = _led_off;
_green_ws = _led_medium;
set_rgb_mode(2); // green breath
}else if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D && AP_Notify::flags.pre_arm_gps_check) {
// return sequence_disarmed_good_gps;
_red_ws = _led_off;
_blue_ws = _led_off;
_green_ws = _led_medium;
set_rgb_mode(2); // blue green breath
}else{
_red_ws = _led_off;
_blue_ws = _led_medium;
_green_ws = _led_medium;
set_rgb_mode(2); // blue breath
}
// return sequence_disarmed_bad_gps;
}
uint32_t RGBLed::get_colour_sequence_traffic_light(void) const
{
if (AP_Notify::flags.initialising) {
@ -206,6 +309,10 @@ void RGBLed::update() @@ -206,6 +309,10 @@ void RGBLed::update()
case traffic_light:
current_colour_sequence = get_colour_sequence_traffic_light();
break;
case use_ws2812:
get_colour_ws2812();
break;
}
const uint8_t brightness = get_brightness();
@ -220,9 +327,19 @@ void RGBLed::update() @@ -220,9 +327,19 @@ void RGBLed::update()
const uint8_t colour = (current_colour_sequence >> (step*3)) & 7;
_red_des = (colour & RED) ? brightness : 0;
_green_des = (colour & GREEN) ? brightness : 0;
_blue_des = (colour & BLUE) ? brightness : 0;
// _red_des = (colour & RED) ? brightness : 0;
// _green_des = (colour & GREEN) ? brightness : 0;
// _blue_des = (colour & BLUE) ? brightness : 0;
if(rgb_source() == use_ws2812){
_red_des = (_red_ws != _led_off) ? brightness : 0;
_green_des = (_green_ws != _led_off) ? brightness : 0;
_blue_des = (_blue_ws != _led_off) ? brightness : 0;
}else{
_red_des = (colour & RED) ? brightness : 0;
_green_des = (colour & GREEN) ? brightness : 0;
_blue_des = (colour & BLUE) ? brightness : 0;
}
set_rgb(_red_des, _green_des, _blue_des);
}
@ -262,6 +379,14 @@ void RGBLed::handle_led_control(const mavlink_message_t &msg) @@ -262,6 +379,14 @@ void RGBLed::handle_led_control(const mavlink_message_t &msg)
}
}
void RGBLed::set_rgb_mode(uint8_t mode)
{
if (!pNotify->_rgb_led_override) {
// ignore LED_CONTROL commands if not in LED_OVERRIDE mode
return;
}
}
/*
update LED when in override mode
*/

5
libraries/AP_Notify/RGBLed.h
Unescape View File

@ -38,6 +38,8 @@ public: @@ -38,6 +38,8 @@ public:
// handle LED control, only used when LED_OVERRIDE=1
virtual void handle_led_control(const mavlink_message_t &msg) override;
virtual void set_rgb_mode(uint8_t mode) override; // @Brown
protected:
// methods implemented in hardware specific classes
@ -51,6 +53,7 @@ protected: @@ -51,6 +53,7 @@ protected:
// meta-data common to all hw devices
uint8_t _red_des, _green_des, _blue_des; // color requested by timed update
uint8_t _red_ws, _green_ws, _blue_ws; // color requested by timed update, @Brown
uint8_t _red_curr, _green_curr, _blue_curr; // current colours displayed by the led
uint8_t _led_off;
uint8_t _led_bright;
@ -66,6 +69,7 @@ protected: @@ -66,6 +69,7 @@ protected:
private:
void update_colours();
uint32_t get_colour_sequence() const;
void get_colour_ws2812();
uint32_t get_colour_sequence_obc() const;
uint32_t get_colour_sequence_traffic_light() const;
@ -110,6 +114,7 @@ private: @@ -110,6 +114,7 @@ private:
mavlink = 1,
obc = 2,
traffic_light = 3,
use_ws2812 = 4,
};
rgb_source_t rgb_source() const;

19
libraries/AP_Notify/ToshibaLED_I2C.cpp
Unescape View File

@ -31,7 +31,8 @@ extern const AP_HAL::HAL& hal; @@ -31,7 +31,8 @@ extern const AP_HAL::HAL& hal;
#define TOSHIBA_LED_DIM 0x11 // dim
#define TOSHIBA_LED_OFF 0x00 // off
#define TOSHIBA_LED_I2C_ADDR 0x55 // default I2C bus address
// #define TOSHIBA_LED_I2C_ADDR 0x55 // default I2C bus address
#define TOSHIBA_LED_I2C_ADDR 0x56 // default I2C bus address
#define TOSHIBA_LED_PWM0 0x01 // pwm0 register
#define TOSHIBA_LED_PWM1 0x02 // pwm1 register
@ -47,7 +48,12 @@ ToshibaLED_I2C::ToshibaLED_I2C(uint8_t bus) @@ -47,7 +48,12 @@ ToshibaLED_I2C::ToshibaLED_I2C(uint8_t bus)
bool ToshibaLED_I2C::hw_init(void)
{
// first look for led on external bus
_dev = std::move(hal.i2c_mgr->get_device(_bus, TOSHIBA_LED_I2C_ADDR));
// _dev = std::move(hal.i2c_mgr->get_device(_bus, TOSHIBA_LED_I2C_ADDR));
FOREACH_I2C(i) {
_dev = std::move(hal.i2c_mgr->get_device(i, TOSHIBA_LED_I2C_ADDR));
}
if (!_dev) {
return false;
}
@ -82,6 +88,11 @@ bool ToshibaLED_I2C::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) @@ -82,6 +88,11 @@ bool ToshibaLED_I2C::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
return true;
}
void ToshibaLED_I2C::set_rgb_mode(uint8_t mode)
{
rgb_mode = mode;
}
void ToshibaLED_I2C::_timer(void)
{
if (!_need_update) {
@ -93,5 +104,9 @@ void ToshibaLED_I2C::_timer(void) @@ -93,5 +104,9 @@ void ToshibaLED_I2C::_timer(void)
uint8_t val[4] = { TOSHIBA_LED_PWM0, (uint8_t)(rgb.b >> 4),
(uint8_t)(rgb.g / 16), (uint8_t)(rgb.r / 16) };
val[0] = rgb_mode;
val[1] = rgb.r;
val[2] = rgb.g;
val[3] = rgb.b;
_dev->transfer(val, sizeof(val), nullptr, 0);
}

2
libraries/AP_Notify/ToshibaLED_I2C.h
Unescape View File

@ -26,6 +26,7 @@ public: @@ -26,6 +26,7 @@ public:
protected:
bool hw_init(void) override;
bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b) override;
void set_rgb_mode(uint8_t mode) override;
private:
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
@ -35,4 +36,5 @@ private: @@ -35,4 +36,5 @@ private:
uint8_t r, g, b;
} rgb;
uint8_t _bus;
uint8_t rgb_mode;
};

Write Preview
Loading…
Cancel
Save