Line data Source code
1 : /*
2 : Generic RGBLed driver
3 : */
4 :
5 : /*
6 : This program is free software: you can redistribute it and/or modify
7 : it under the terms of the GNU General Public License as published by
8 : the Free Software Foundation, either version 3 of the License, or
9 : (at your option) any later version.
10 :
11 : This program is distributed in the hope that it will be useful,
12 : but WITHOUT ANY WARRANTY; without even the implied warranty of
13 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 : GNU General Public License for more details.
15 :
16 : */
17 :
18 :
19 : #include <AP_HAL/AP_HAL.h>
20 : #include <AP_GPS/AP_GPS.h>
21 : #include "RGBLed.h"
22 : #include "AP_Notify.h"
23 : #include <AP_AHRS/AP_AHRS.h>
24 :
25 : extern const AP_HAL::HAL& hal;
26 :
27 35598 : RGBLed::RGBLed(uint8_t led_off, uint8_t led_bright, uint8_t led_medium, uint8_t led_dim):
28 35598 : _led_off(led_off),
29 35598 : _led_bright(led_bright),
30 35598 : _led_medium(led_medium),
31 35598 : _led_dim(led_dim)
32 : {
33 :
34 35598 : }
35 :
36 : // set_rgb - set color as a combination of red, green and blue values
37 23245344 : void RGBLed::_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
38 : {
39 23245344 : if (red != _red_curr ||
40 22819773 : green != _green_curr ||
41 22186020 : blue != _blue_curr) {
42 : // call the hardware update routine
43 1091997 : if (hw_set_rgb(red, green, blue)) {
44 1091997 : _red_curr = red;
45 1091997 : _green_curr = green;
46 1091997 : _blue_curr = blue;
47 : }
48 : }
49 23245344 : }
50 :
51 46490688 : RGBLed::Source RGBLed::rgb_source() const
52 : {
53 46490688 : return Source(pNotify->_rgb_led_override.get());
54 : }
55 :
56 : // set_rgb - set color as a combination of red, green and blue values
57 23245344 : void RGBLed::set_rgb(uint8_t red, uint8_t green, uint8_t blue)
58 : {
59 23245344 : if (rgb_source() == Source::mavlink) {
60 : // don't set if in override mode
61 0 : return;
62 : }
63 23245344 : _set_rgb(red, green, blue);
64 : }
65 :
66 23245344 : uint8_t RGBLed::get_brightness(void) const
67 : {
68 23245344 : uint8_t brightness = _led_bright;
69 :
70 23245344 : switch (pNotify->_rgb_led_brightness) {
71 0 : case RGB_LED_OFF:
72 0 : brightness = _led_off;
73 0 : break;
74 0 : case RGB_LED_LOW:
75 0 : brightness = _led_dim;
76 0 : break;
77 0 : case RGB_LED_MEDIUM:
78 0 : brightness = _led_medium;
79 0 : break;
80 23245344 : case RGB_LED_HIGH:
81 23245344 : brightness = _led_bright;
82 23245344 : break;
83 : }
84 :
85 : // use dim light when connected through USB
86 23245344 : if (hal.gpio->usb_connected() && brightness > _led_dim) {
87 0 : brightness = _led_dim;
88 : }
89 23245344 : return brightness;
90 : }
91 :
92 0 : uint32_t RGBLed::get_colour_sequence_obc(void) const
93 : {
94 0 : if (AP_Notify::flags.armed) {
95 0 : return DEFINE_COLOUR_SEQUENCE_SOLID(RED);
96 : }
97 0 : return DEFINE_COLOUR_SEQUENCE_SOLID(GREEN);
98 : }
99 :
100 : // _scheduled_update - updates _red, _green, _blue according to notify flags
101 23245344 : uint32_t RGBLed::get_colour_sequence(void) const
102 : {
103 : // initialising pattern
104 23245344 : if (AP_Notify::flags.initialising) {
105 100344 : return sequence_initialising;
106 : }
107 :
108 : // save trim or any calibration pattern
109 23145000 : if (AP_Notify::flags.save_trim ||
110 23139465 : AP_Notify::flags.esc_calibration ||
111 23127267 : AP_Notify::flags.compass_cal_running ||
112 22908039 : AP_Notify::flags.temp_cal_running) {
113 268491 : return sequence_trim_or_esc;
114 : }
115 :
116 : // radio and battery failsafe patter: flash yellow
117 : // gps failsafe pattern : flashing yellow and blue
118 : // ekf_bad pattern : flashing yellow and red
119 22876509 : if (AP_Notify::flags.failsafe_radio ||
120 22735404 : AP_Notify::flags.failsafe_gcs ||
121 22632243 : AP_Notify::flags.failsafe_battery ||
122 22531593 : AP_Notify::flags.ekf_bad ||
123 22491414 : AP_Notify::flags.gps_glitching ||
124 22453017 : AP_Notify::flags.leak_detected) {
125 :
126 434928 : if (AP_Notify::flags.leak_detected) {
127 : // purple if leak detected
128 11436 : return sequence_failsafe_leak;
129 423492 : } else if (AP_Notify::flags.ekf_bad) {
130 : // red on if ekf bad
131 65397 : return sequence_failsafe_ekf;
132 358095 : } else if (AP_Notify::flags.gps_glitching) {
133 : // blue on gps glitch
134 38397 : return sequence_failsafe_gps_glitching;
135 : }
136 : // all off for radio or battery failsafe
137 319698 : return sequence_failsafe_radio_or_battery;
138 : }
139 :
140 : #if AP_GPS_ENABLED
141 22441581 : Location loc;
142 : #if AP_AHRS_ENABLED
143 : // the AHRS can return "true" for get_location and still not be
144 : // happy enough with the location to set its origin from that
145 : // location:
146 22441581 : const bool good_ahrs_location = AP::ahrs().get_location(loc) && AP::ahrs().get_origin(loc);
147 : #else
148 : const bool good_ahrs_location = true;
149 : #endif
150 :
151 : // solid green or blue if armed
152 22441581 : if (AP_Notify::flags.armed) {
153 : #if AP_GPS_ENABLED
154 : // solid green if armed with GPS 3d lock and good location
155 14454138 : if (AP_Notify::flags.gps_status >= AP_GPS_FixType::FIX_3D &&
156 : good_ahrs_location) {
157 14319360 : return sequence_armed;
158 : }
159 : #endif
160 : // solid blue if armed with no GPS lock
161 134778 : return sequence_armed_no_gps_or_no_location;
162 : }
163 :
164 : // double flash yellow if failing pre-arm checks
165 7987443 : if (!AP_Notify::flags.pre_arm_check) {
166 4272429 : return sequence_prearm_failing;
167 : }
168 3715014 : if (AP_Notify::flags.pre_arm_gps_check && good_ahrs_location) {
169 3046815 : if (AP_Notify::flags.gps_status >= AP_GPS_FixType::DGPS) {
170 3007242 : return sequence_disarmed_good_dgps_and_location;
171 : }
172 39573 : if (AP_Notify::flags.gps_status >= AP_GPS_FixType::FIX_3D) {
173 13773 : return sequence_disarmed_good_gps_and_location;
174 : }
175 : }
176 : #endif
177 :
178 693999 : return sequence_disarmed_bad_gps_or_no_location;
179 : }
180 :
181 0 : uint32_t RGBLed::get_colour_sequence_traffic_light(void) const
182 : {
183 0 : if (AP_Notify::flags.initialising) {
184 0 : return DEFINE_COLOUR_SEQUENCE(RED,GREEN,BLUE,RED,GREEN,BLUE,RED,GREEN,BLUE,BLACK);
185 : }
186 :
187 0 : if (AP_Notify::flags.armed) {
188 0 : return DEFINE_COLOUR_SEQUENCE_SLOW(RED);
189 : }
190 :
191 0 : if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
192 0 : if (!AP_Notify::flags.pre_arm_check) {
193 0 : return DEFINE_COLOUR_SEQUENCE_ALTERNATE(YELLOW, BLACK);
194 : } else {
195 0 : return DEFINE_COLOUR_SEQUENCE_SLOW(YELLOW);
196 : }
197 : }
198 :
199 0 : if (!AP_Notify::flags.pre_arm_check) {
200 0 : return DEFINE_COLOUR_SEQUENCE_ALTERNATE(GREEN, BLACK);
201 : }
202 0 : return DEFINE_COLOUR_SEQUENCE_SLOW(GREEN);
203 : }
204 :
205 : // update - updates led according to timed_updated. Should be called
206 : // at 50Hz
207 23245344 : void RGBLed::update()
208 : {
209 23245344 : uint32_t current_colour_sequence = 0;
210 :
211 23245344 : switch (rgb_source()) {
212 0 : case Source::mavlink:
213 0 : update_override();
214 0 : return; // note this is a return not a break!
215 23245344 : case Source::standard:
216 23245344 : current_colour_sequence = get_colour_sequence();
217 23245344 : break;
218 0 : case Source::obc:
219 0 : current_colour_sequence = get_colour_sequence_obc();
220 0 : break;
221 0 : case Source::traffic_light:
222 0 : current_colour_sequence = get_colour_sequence_traffic_light();
223 0 : break;
224 : }
225 :
226 23245344 : const uint8_t brightness = get_brightness();
227 :
228 23245344 : uint8_t step = (AP_HAL::millis()/100) % 10;
229 :
230 : // ensure we can't skip a step even with awful timing
231 23245344 : if (step != last_step) {
232 4662762 : step = (last_step+1) % 10;
233 4662762 : last_step = step;
234 : }
235 :
236 23245344 : const uint8_t colour = (current_colour_sequence >> (step*3)) & 7;
237 :
238 23245344 : uint8_t red_des = (colour & RED) ? brightness : _led_off;
239 23245344 : uint8_t green_des = (colour & GREEN) ? brightness : _led_off;
240 23245344 : uint8_t blue_des = (colour & BLUE) ? brightness : _led_off;
241 :
242 23245344 : set_rgb(red_des, green_des, blue_des);
243 : }
244 :
245 : #if AP_NOTIFY_MAVLINK_LED_CONTROL_SUPPORT_ENABLED
246 : /*
247 : handle LED control, only used when LED_OVERRIDE=1
248 : */
249 0 : void RGBLed::handle_led_control(const mavlink_message_t &msg)
250 : {
251 0 : if (rgb_source() != Source::mavlink) {
252 : // ignore LED_CONTROL commands if not in LED_OVERRIDE mode
253 0 : return;
254 : }
255 :
256 : // decode mavlink message
257 : mavlink_led_control_t packet;
258 0 : mavlink_msg_led_control_decode(&msg, &packet);
259 :
260 0 : _led_override.start_ms = AP_HAL::millis();
261 :
262 0 : uint8_t rate_hz = 0;
263 0 : switch (packet.custom_len) {
264 0 : case 4:
265 0 : rate_hz = packet.custom_bytes[3];
266 : FALLTHROUGH;
267 0 : case 3:
268 0 : rgb_control(packet.custom_bytes[0], packet.custom_bytes[1], packet.custom_bytes[2], rate_hz);
269 0 : break;
270 : }
271 : }
272 : #endif
273 :
274 : /*
275 : update LED when in override mode
276 : */
277 0 : void RGBLed::update_override(void)
278 : {
279 0 : if (_led_override.rate_hz == 0) {
280 : // solid colour
281 0 : _set_rgb(_led_override.r, _led_override.g, _led_override.b);
282 0 : return;
283 : }
284 : // blinking
285 0 : uint32_t ms_per_cycle = 1000 / _led_override.rate_hz;
286 0 : uint32_t cycle = (AP_HAL::millis() - _led_override.start_ms) % ms_per_cycle;
287 0 : if (cycle > ms_per_cycle / 2) {
288 : // on
289 0 : _set_rgb(_led_override.r, _led_override.g, _led_override.b);
290 : } else {
291 0 : _set_rgb(0, 0, 0);
292 : }
293 : }
294 :
295 : /*
296 : RGB control
297 : give RGB and flash rate, used with scripting
298 : */
299 0 : void RGBLed::rgb_control(uint8_t r, uint8_t g, uint8_t b, uint8_t rate_hz)
300 : {
301 0 : _led_override.rate_hz = rate_hz;
302 0 : _led_override.r = r;
303 0 : _led_override.g = g;
304 0 : _led_override.b = b;
305 0 : }
|
