From 42bd7f3b1f9e0959d3171b4d329d0f7b40972d94 Mon Sep 17 00:00:00 2001 From: z Date: Thu, 19 Mar 2020 10:50:51 +0800 Subject: [PATCH] WS2812 OK --- libraries/AP_Notify/AP_Notify.cpp | 10 ++ libraries/AP_Notify/AP_Notify.h | 2 + libraries/AP_Notify/NotifyDevice.h | 2 + libraries/AP_Notify/RGBLed.cpp | 131 ++++++++++++++++++++++++- libraries/AP_Notify/RGBLed.h | 5 + libraries/AP_Notify/ToshibaLED_I2C.cpp | 19 +++- libraries/AP_Notify/ToshibaLED_I2C.h | 2 + 7 files changed, 166 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index 8691922e89..1905d0f90b 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -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) { diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index a1fd98c1b2..8171f16860 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -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 diff --git a/libraries/AP_Notify/NotifyDevice.h b/libraries/AP_Notify/NotifyDevice.h index 090ac759f9..4999063958 100644 --- a/libraries/AP_Notify/NotifyDevice.h +++ b/libraries/AP_Notify/NotifyDevice.h @@ -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; }; diff --git a/libraries/AP_Notify/RGBLed.cpp b/libraries/AP_Notify/RGBLed.cpp index 5707c09951..d520867cf5 100644 --- a/libraries/AP_Notify/RGBLed.cpp +++ b/libraries/AP_Notify/RGBLed.cpp @@ -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 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() 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() 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) } } +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 */ diff --git a/libraries/AP_Notify/RGBLed.h b/libraries/AP_Notify/RGBLed.h index 6b4bb54bc7..5f8364f063 100644 --- a/libraries/AP_Notify/RGBLed.h +++ b/libraries/AP_Notify/RGBLed.h @@ -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: // 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: 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: mavlink = 1, obc = 2, traffic_light = 3, + use_ws2812 = 4, }; rgb_source_t rgb_source() const; diff --git a/libraries/AP_Notify/ToshibaLED_I2C.cpp b/libraries/AP_Notify/ToshibaLED_I2C.cpp index 446837b406..534313d257 100644 --- a/libraries/AP_Notify/ToshibaLED_I2C.cpp +++ b/libraries/AP_Notify/ToshibaLED_I2C.cpp @@ -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) 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) 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) 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); } diff --git a/libraries/AP_Notify/ToshibaLED_I2C.h b/libraries/AP_Notify/ToshibaLED_I2C.h index a6a7bb2117..c5c5125bcf 100644 --- a/libraries/AP_Notify/ToshibaLED_I2C.h +++ b/libraries/AP_Notify/ToshibaLED_I2C.h @@ -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 _dev; @@ -35,4 +36,5 @@ private: uint8_t r, g, b; } rgb; uint8_t _bus; + uint8_t rgb_mode; };