AP_Notify: tidy and remove code duplications

Saves 60 bytes, too

libraries/AP_Notify/RGBLed.cpp

@@ -62,9 +62,7 @@ void RGBLed::set_rgb(uint8_t red, uint8_t green, uint8_t blue)
     _set_rgb(red, green, blue);
 }
 
-
+uint8_t RGBLed::get_brightness(void) const
-// _scheduled_update - updates _red, _green, _blue according to notify flags
-void RGBLed::update_colours(void)
 {
     uint8_t brightness = _led_bright;
 
@@ -83,228 +81,88 @@ void RGBLed::update_colours(void)
         break;
     }
 
-    const uint8_t step = (AP_HAL::millis()/100) % 10;
-
     // use dim light when connected through USB
     if (hal.gpio->usb_connected() && brightness > _led_dim) {
         brightness = _led_dim;
     }
+    return brightness;
+}
 
+// _scheduled_update - updates _red, _green, _blue according to notify flags
+uint32_t RGBLed::get_colour_sequence(void) const
+{
     // initialising pattern
     if (AP_Notify::flags.initialising) {
-        if (step & 1) {
+        return sequence_initialising;
-            // odd steps display red light
-            _red_des = brightness;
-            _blue_des = _led_off;
-            _green_des = _led_off;
-        } else {
-            // even display blue light
-            _red_des = _led_off;
-            _blue_des = brightness;
-            _green_des = _led_off;
-        }
-
-        // exit so no other status modify this pattern
-        return;
     }
-    
+
     // save trim and esc calibration pattern
     if (AP_Notify::flags.save_trim || AP_Notify::flags.esc_calibration) {
-        switch(step) {
+        return sequence_trim_or_esc;
-            case 0:
-            case 3:
-            case 6:
-                // red on
-                _red_des = brightness;
-                _blue_des = _led_off;
-                _green_des = _led_off;
-                break;
-
-            case 1:
-            case 4:
-            case 7:
-                // blue on
-                _red_des = _led_off;
-                _blue_des = brightness;
-                _green_des = _led_off;
-                break;
-
-            case 2:
-            case 5:
-            case 8:
-                // green on
-                _red_des = _led_off;
-                _blue_des = _led_off;
-                _green_des = brightness;
-                break;
-
-            case 9:
-                // all off
-                _red_des = _led_off;
-                _blue_des = _led_off;
-                _green_des = _led_off;
-                break;
-        }
-        // exit so no other status modify this pattern
-        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 ||
+    if (AP_Notify::flags.failsafe_radio ||
-            AP_Notify::flags.ekf_bad || AP_Notify::flags.gps_glitching || AP_Notify::flags.leak_detected) {
+        AP_Notify::flags.failsafe_battery ||
-        switch(step) {
+        AP_Notify::flags.ekf_bad ||
-            case 0:
+        AP_Notify::flags.gps_glitching ||
-            case 1:
+        AP_Notify::flags.leak_detected) {
-            case 2:
+
-            case 3:
+        if (AP_Notify::flags.leak_detected) {
-            case 4:
+            // purple if leak detected
-                // yellow on
+            return sequence_failsafe_leak;
-                _red_des = brightness;
+        } else if (AP_Notify::flags.ekf_bad) {
-                _blue_des = _led_off;
+            // red on if ekf bad
-                _green_des = brightness;
+            return sequence_failsafe_ekf;
-                break;
+        } else if (AP_Notify::flags.gps_glitching) {
-            case 5:
+            // blue on gps glitch
-            case 6:
+            return sequence_failsafe_gps_glitching;
-            case 7:
-            case 8:
-            case 9:
-                if (AP_Notify::flags.leak_detected) {
-                    // purple if leak detected
-                    _red_des = brightness;
-                    _blue_des = brightness;
-                    _green_des = brightness;
-                } else if (AP_Notify::flags.ekf_bad) {
-                    // red on if ekf bad
-                    _red_des = brightness;
-                    _blue_des = _led_off;
-                    _green_des = _led_off;
-                } else if (AP_Notify::flags.gps_glitching) {
-                    // blue on gps glitch
-                    _red_des = _led_off;
-                    _blue_des = brightness;
-                    _green_des = _led_off;
-                }else{
-                    // all off for radio or battery failsafe
-                    _red_des = _led_off;
-                    _blue_des = _led_off;
-                    _green_des = _led_off;
-                }
-                break;
         }
-        // exit so no other status modify this pattern
+        // all off for radio or battery failsafe
-        return;
+        return sequence_failsafe_radio_or_battery;
     }
 
     // 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) {
-            _red_des = _led_off;
+            return sequence_armed;
-            _blue_des = _led_off;
-            _green_des = brightness;
-        }else{
-            // solid blue if armed with no GPS lock
-            _red_des = _led_off;
-            _blue_des = brightness;
-            _green_des = _led_off;
-        }
-        return;
-    }else{
-        // double flash yellow if failing pre-arm checks
-        if (!AP_Notify::flags.pre_arm_check) {
-            switch(step) {
-                case 0:
-                case 1:
-                case 4:
-                case 5:
-                    // yellow on
-                    _red_des = brightness;
-                    _blue_des = _led_off;
-                    _green_des = brightness;
-                    break;
-                case 2:
-                case 3:
-                case 6:
-                case 7:
-                case 8:
-                case 9:
-                    // all off
-                    _red_des = _led_off;
-                    _blue_des = _led_off;
-                    _green_des = _led_off;
-                    break;
-            }
-        }else{
-            // fast flashing green if disarmed with GPS 3D lock and DGPS
-            // slow flashing green if disarmed with GPS 3d lock (and no DGPS)
-            // flashing blue if disarmed with no gps lock or gps pre_arm checks have failed
-            bool fast_green = AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS && AP_Notify::flags.pre_arm_gps_check;
-            switch(step) {
-                case 0:
-                    if (fast_green) {
-                        _green_des = brightness;
-                    }
-                    break;
-                case 1:
-                    if (fast_green) {
-                        _green_des = _led_off;
-                    }
-                    break;
-                case 2:
-                    if (fast_green) {
-                        _green_des = brightness;
-                    }
-                    break;
-                case 3:
-                    if (fast_green) {
-                        _green_des = _led_off;
-                    }
-                    break;
-                case 4:
-                    _red_des = _led_off;
-                    if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D && AP_Notify::flags.pre_arm_gps_check) {
-                        // flashing green if disarmed with GPS 3d lock
-                        _blue_des = _led_off;
-                        _green_des = brightness;
-                    }else{
-                        // flashing blue if disarmed with no gps lock
-                        _blue_des = brightness;
-                        _green_des = _led_off;
-                    }
-                    break;
-                case 5:
-                    if (fast_green) {
-                        _green_des = _led_off;
-                    }
-                    break;
-
-                case 6:
-                    if (fast_green) {
-                        _green_des = brightness;
-                    }
-                    break;
-
-                case 7:
-                    if (fast_green) {
-                        _green_des = _led_off;
-                    }
-                    break;
-                case 8:
-                    if (fast_green) {
-                        _green_des = brightness;
-                    }
-                    break;
-                case 9:
-                    // all off
-                    _red_des = _led_off;
-                    _blue_des = _led_off;
-                    _green_des = _led_off;
-                    break;
-            }
         }
+        // solid blue if armed with no GPS lock
+        return sequence_armed_nogps;
+    }
+
+    // double flash yellow if failing pre-arm checks
+    if (!AP_Notify::flags.pre_arm_check) {
+        return sequence_prearm_failing;
     }
+    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;
+    }
+
+    if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D && AP_Notify::flags.pre_arm_gps_check) {
+        return sequence_disarmed_good_gps;
+    }
+
+    return sequence_disarmed_bad_gps;
+}
+
+// _scheduled_update - updates _red, _green, _blue according to notify flags
+void RGBLed::update_colours(void)
+{
+    const uint8_t brightness = get_brightness();
+
+    const uint32_t current_colour_sequence = get_colour_sequence();
+
+    const uint8_t step = (AP_HAL::millis()/100) % 10;
+
+    const uint8_t colour = current_colour_sequence >> (step*3);
+
+    _red_des = (colour & RED) ? brightness : 0;
+    _green_des = (colour & GREEN) ? brightness : 0;
+    _blue_des = (colour & BLUE) ? brightness : 0;
 }
 
 // update - updates led according to timed_updated.  Should be called

libraries/AP_Notify/RGBLed.h

@@ -64,5 +64,42 @@ protected:
     } _led_override;
     
 private:
-    virtual void update_colours();
+    void update_colours();
+    uint32_t get_colour_sequence() const;
+
+    uint8_t get_brightness(void) const;
+
+#define DEFINE_COLOUR_SEQUENCE(S0, S1, S2, S3, S4, S5, S6, S7, S8, S9)  \
+    ((S0) << (0*3) | (S1) << (1*3) | (S2) << (2*3) | (S3) << (3*3) | (S4) << (4*3) | (S5) << (5*3) | (S6) << (6*3) | (S7) << (7*3) | (S8) << (8*3) | (S9) << (9*3))
+
+#define DEFINE_COLOUR_SEQUENCE_SLOW(colour)                       \
+    DEFINE_COLOUR_SEQUENCE(colour,colour,colour,colour,colour,OFF,OFF,OFF,OFF,OFF)
+#define DEFINE_COLOUR_SEQUENCE_FAILSAFE(colour) \
+    DEFINE_COLOUR_SEQUENCE(YELLOW,YELLOW,YELLOW,YELLOW,YELLOW,colour,colour,colour,colour,colour)
+#define DEFINE_COLOUR_SEQUENCE_SOLID(colour) \
+    DEFINE_COLOUR_SEQUENCE(colour,colour,colour,colour,colour,colour,colour,colour,colour,colour)
+#define DEFINE_COLOUR_SEQUENCE_ALTERNATE(colour1, colour2)                      \
+    DEFINE_COLOUR_SEQUENCE(colour1,colour2,colour1,colour2,colour1,colour2,colour1,colour2,colour1,colour2)
+
+#define OFF    0
+#define BLUE   1
+#define GREEN  2
+#define RED    4
+#define YELLOW (RED|GREEN)
+#define WHITE (RED|GREEN|BLUE)
+
+    const uint32_t sequence_initialising = DEFINE_COLOUR_SEQUENCE_ALTERNATE(RED,BLUE);
+    const uint32_t sequence_trim_or_esc = DEFINE_COLOUR_SEQUENCE(RED,BLUE,GREEN,RED,BLUE,GREEN,RED,BLUE,GREEN,OFF);
+    const uint32_t sequence_failsafe_leak = DEFINE_COLOUR_SEQUENCE_FAILSAFE(WHITE);
+    const uint32_t sequence_failsafe_ekf = DEFINE_COLOUR_SEQUENCE_FAILSAFE(RED);
+    const uint32_t sequence_failsafe_gps_glitching = DEFINE_COLOUR_SEQUENCE_FAILSAFE(BLUE);
+    const uint32_t sequence_failsafe_radio_or_battery = DEFINE_COLOUR_SEQUENCE_FAILSAFE(OFF);
+
+    const uint32_t sequence_armed = DEFINE_COLOUR_SEQUENCE_SOLID(GREEN);
+    const uint32_t sequence_armed_nogps = DEFINE_COLOUR_SEQUENCE_SOLID(BLUE);
+    const uint32_t sequence_prearm_failing = DEFINE_COLOUR_SEQUENCE(YELLOW,YELLOW,OFF,OFF,YELLOW,YELLOW,OFF,OFF,OFF,OFF);
+    const uint32_t sequence_disarmed_good_dgps = DEFINE_COLOUR_SEQUENCE_ALTERNATE(GREEN,OFF);
+    const uint32_t sequence_disarmed_good_gps = DEFINE_COLOUR_SEQUENCE_SLOW(GREEN);
+    const uint32_t sequence_disarmed_bad_gps = DEFINE_COLOUR_SEQUENCE_SLOW(BLUE);
+
 };