Line data Source code
1 : /* 2 : This program is free software: you can redistribute it and/or modify 3 : it under the terms of the GNU General Public License as published by 4 : the Free Software Foundation, either version 3 of the License, or 5 : (at your option) any later version. 6 : 7 : This program is distributed in the hope that it will be useful, 8 : but WITHOUT ANY WARRANTY; without even the implied warranty of 9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 10 : GNU General Public License for more details. 11 : 12 : You should have received a copy of the GNU General Public License 13 : along with this program. If not, see <http://www.gnu.org/licenses/>. 14 : */ 15 : 16 : #include "ProfiLED.h" 17 : 18 : #include "AP_Notify/AP_Notify.h" 19 : 20 : #include "SRV_Channel/SRV_Channel.h" 21 : #include <utility> 22 : #include <GCS_MAVLink/GCS.h> 23 : 24 : // This limit is from the dshot driver rc out groups limit, we need at least one channel for clock 25 : #define AP_NOTIFY_ProfiLED_MAX_INSTANCES 3 26 : 27 : #define ProfiLED_LOW 0x33 28 : #define ProfiLED_MEDIUM 0x7F 29 : #define ProfiLED_HIGH 0xFF 30 : #define ProfiLED_OFF 0x00 31 : 32 : extern const AP_HAL::HAL& hal; 33 : 34 : #if AP_NOTIFY_PROFILED_ENABLED 35 : 36 0 : ProfiLED::ProfiLED() : 37 0 : SerialLED(ProfiLED_OFF, ProfiLED_HIGH, ProfiLED_MEDIUM, ProfiLED_LOW) 38 : { 39 0 : } 40 : 41 0 : uint16_t ProfiLED::init_ports() 42 : { 43 0 : uint16_t mask = 0; 44 0 : for (uint16_t i=0; i<AP_NOTIFY_ProfiLED_MAX_INSTANCES; i++) { 45 0 : const SRV_Channel::Function fn = (SRV_Channel::Function)((uint8_t)SRV_Channel::k_ProfiLED_1 + i); 46 0 : if (!SRV_Channels::function_assigned(fn)) { 47 0 : continue; 48 : } 49 0 : mask |= SRV_Channels::get_output_channel_mask(fn); 50 : } 51 : 52 0 : if (mask == 0) { 53 0 : return 0; 54 : } 55 : 56 0 : AP_SerialLED *led = AP_SerialLED::get_singleton(); 57 0 : if (led == nullptr) { 58 0 : return 0; 59 : } 60 : 61 0 : for (uint16_t chan=0; chan<16; chan++) { 62 0 : if ((1U<<chan) & mask) { 63 0 : led->set_num_profiled(chan+1, (pNotify->get_led_len())); 64 : } 65 : } 66 : 67 0 : return mask; 68 : } 69 : #endif // AP_NOTIFY_PROFILED_ENABLED 70 : 71 : #if AP_NOTIFY_PROFILED_SPI_ENABLED 72 : ProfiLED_SPI::ProfiLED_SPI() : 73 : RGBLed(ProfiLED_OFF, ProfiLED_HIGH, ProfiLED_MEDIUM, ProfiLED_LOW) {} 74 : 75 : bool ProfiLED_SPI::init() 76 : { 77 : num_leds = pNotify->get_led_len() + 1; // for some reason we have to send an additional LED data 78 : rgb = NEW_NOTHROW ProfiLED_SPI::RGB[num_leds]; 79 : if (!rgb) { 80 : return false; 81 : } 82 : memset(rgb, 0, num_leds * sizeof(ProfiLED_SPI::RGB)); 83 : _dev = std::move(hal.spi->get_device("profiled")); 84 : if (!_dev) { 85 : return false; 86 : } 87 : WITH_SEMAPHORE(_dev->get_semaphore()); 88 : _dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&ProfiLED_SPI::_timer, void)); 89 : _need_update = true; 90 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Initialised ProfiLED over SPIs"); 91 : return true; 92 : } 93 : 94 : void ProfiLED_SPI::rgb_set_id(uint8_t red, uint8_t green, uint8_t blue, uint8_t id) 95 : { 96 : if (id >= num_leds) { 97 : return; 98 : } 99 : rgb[id] = {blue, red, green}; 100 : _need_update = true; 101 : } 102 : 103 : bool ProfiLED_SPI::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) 104 : { 105 : for (uint16_t i = 0; i < num_leds; i++) { 106 : rgb[i] = {blue, red, green}; 107 : } 108 : _need_update = true; 109 : return true; 110 : } 111 : 112 : void ProfiLED_SPI::_timer(void) 113 : { 114 : _need_update = false; 115 : update_led_strip(); 116 : } 117 : 118 : void ProfiLED_SPI::update_led_strip() { 119 : const uint32_t min_bits = num_leds*25+50; 120 : const uint8_t num_leading_zeros = 8-min_bits%8 + 50; 121 : const uint32_t output_stream_byte_length = (min_bits+7)/8; 122 : 123 : WITH_SEMAPHORE(_dev->get_semaphore()); 124 : 125 : uint32_t curr_led_idx = 0; 126 : 127 : uint8_t send_buf[32]; 128 : uint8_t send_buf_len = 0; 129 : 130 : union { 131 : ProfiLED_SPI::RGB struct_val; 132 : uint8_t bytes_val[3]; 133 : } curr_led_color; 134 : 135 : curr_led_color.struct_val = rgb[curr_led_idx]; 136 : for (uint32_t i=0; i<output_stream_byte_length; i++) { 137 : uint8_t byte = 0; 138 : for (uint8_t bit = 0; bit < 8; bit++) { 139 : uint32_t out_bit_idx = i*8+bit; 140 : uint8_t bit_val; 141 : if (out_bit_idx < num_leading_zeros) { 142 : bit_val = 0; 143 : } else if ((out_bit_idx-num_leading_zeros) % 25 == 0) { 144 : bit_val = 1; 145 : } else { 146 : uint32_t in_bit_idx = out_bit_idx - num_leading_zeros - (out_bit_idx - num_leading_zeros)/25; 147 : uint32_t in_led_idx = in_bit_idx/24; 148 : 149 : if (curr_led_idx != in_led_idx) { 150 : curr_led_idx = in_led_idx; 151 : curr_led_color.struct_val = rgb[curr_led_idx]; 152 : } 153 : 154 : bit_val = (curr_led_color.bytes_val[(in_bit_idx%24)/8] >> (8-in_bit_idx%8)) & 1; 155 : } 156 : 157 : byte |= bit_val << (7-bit); 158 : } 159 : send_buf[send_buf_len++] = byte; 160 : if (send_buf_len >= 32) { 161 : _dev->transfer(send_buf, send_buf_len, NULL, 0); 162 : send_buf_len = 0; 163 : } 164 : } 165 : if (send_buf_len > 0) { 166 : _dev->transfer(send_buf, send_buf_len, NULL, 0); 167 : send_buf_len = 0; 168 : } 169 : } 170 : 171 : #endif // AP_NOTIFY_PROFILED_SPI_ENABLED
