AP_Notify: make flags static variable

libraries/AP_Notify/AP_BoardLED.cpp

@@ -31,7 +31,7 @@ void AP_BoardLED::_update(uint32_t now)
     uint8_t counter2 = _counter >> 6;
 
     // initialising
-    if (notify.flags.initialising) {
+    if (AP_Notify::flags.initialising) {
         // blink LEDs A and C at 8Hz (full cycle) during initialisation
         if (counter2 & 1) {
             hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
@@ -44,7 +44,7 @@ void AP_BoardLED::_update(uint32_t now)
 	}
 
     // save trim
-    if (notify.flags.save_trim) {
+    if (AP_Notify::flags.save_trim) {
         static uint8_t save_trim_counter = 0;
         if ((counter2 & 0x2) == 0) {
             save_trim_counter++;
@@ -73,14 +73,14 @@ void AP_BoardLED::_update(uint32_t now)
 
     // arming light
     static uint8_t arm_counter = 0;
-	if (notify.flags.armed) {
+	if (AP_Notify::flags.armed) {
         // red led solid
         hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
     }else{
         if ((counter2 & 0x2) == 0) {
             arm_counter++;
         }
-        if (notify.flags.pre_arm_check) {
+        if (AP_Notify::flags.pre_arm_check) {
             // passed pre-arm checks so slower single flash
             switch(arm_counter) {
                 case 0:
@@ -98,15 +98,7 @@ void AP_BoardLED::_update(uint32_t now)
                     arm_counter = -1;
                     break;
             }
-            // disarmed and passing pre-arm checks, blink at about 2hz
-            //if ((counter2 & 0x7) == 0) {
-            //    hal.gpio->toggle(HAL_GPIO_A_LED_PIN);
-            //}
         }else{
-            // disarmed and failing pre-arm checks, double blink
-            //if (counter2 & 0x4) {
-            //    hal.gpio->toggle(HAL_GPIO_A_LED_PIN);
-            //}
             // failed pre-arm checks so double flash
             switch(arm_counter) {
                 case 0:
@@ -132,7 +124,7 @@ void AP_BoardLED::_update(uint32_t now)
     }
 
     // gps light
-    switch (notify.flags.gps_status) {
+    switch (AP_Notify::flags.gps_status) {
         case 0:
             // no GPS attached
             hal.gpio->write(HAL_GPIO_C_LED_PIN, HAL_GPIO_LED_OFF);
@@ -153,7 +145,7 @@ void AP_BoardLED::_update(uint32_t now)
             break;
 
         case 3:
-            // GPS attached but 2D lock, blink more slowly (around 2Hz)
+            // solid blue on gps lock
             hal.gpio->write(HAL_GPIO_C_LED_PIN, HAL_GPIO_LED_ON);
             break;        
     }

libraries/AP_Notify/AP_Notify.cpp

@@ -1,8 +1,6 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 #include <AP_Notify.h>
 
-AP_Notify notify;
+struct AP_Notify::notify_type AP_Notify::flags;
-/// Constructor
+
-//AP_Notify::AP_Notify()
+AP_Notify::update_fn_t AP_Notify::_update_fn;
-//{
-//}

libraries/AP_Notify/AP_Notify.h

@@ -9,6 +9,9 @@ class AP_Notify
 {
 public:
 
+    /// definition of callback function
+    typedef void (*update_fn_t)(void);
+
     /// notify_type - bitmask of notification types
     struct notify_type {
         uint16_t initialising   : 1;    // 1 if initialising and copter should not be moved
@@ -17,10 +20,15 @@ public:
         uint16_t pre_arm_check  : 1;    // 0 = failing checks, 1 = passed
         uint16_t save_trim      : 1;    // 1 if gathering trim data
         uint16_t esc_calibration: 1;    // 1 if calibrating escs
-    } flags;
+    };
 
-    /// Constructor
+    static struct notify_type flags;
-    //Notify();
+
+    /// register_callback - allows registering functions to be called with AP_Notify::update() is called from main loop
+    static void register_update_function(update_fn_t fn) {_update_fn = fn;}
+
+    /// update - allow updates of leds that cannot be updated during a timed interrupt
+    static void update() { if (AP_Notify::_update_fn != NULL) AP_Notify::_update_fn(); }
 
     /// To-Do: potential notifications to add
 
@@ -46,10 +54,9 @@ public:
     /// copter leds
     /// blinkm
     /// buzzer
+//private:
+    static update_fn_t _update_fn;
 
 };
 
-// declare a static instance so that it can be accessed easily from anywhere
-extern AP_Notify notify;
-
 #endif	// __AP_NOTIFY_H__

libraries/AP_Notify/ToshibaLED.cpp

@@ -16,7 +16,6 @@
 extern const AP_HAL::HAL& hal;
 
 // static private variable instantiation
-uint16_t ToshibaLED::_counter;
 bool ToshibaLED::_enabled;          // true if the led was initialised successfully
 bool ToshibaLED::_healthy;          // true if the led's latest update was successful
 uint8_t ToshibaLED::_red_des;       // desired redness of led
@@ -26,11 +25,6 @@ uint8_t ToshibaLED::_red_curr;      // current redness of led
 uint8_t ToshibaLED::_green_curr;    // current greenness of led
 uint8_t ToshibaLED::_blue_curr;     // current blueness of led
 
-// constructor
-ToshibaLED::ToshibaLED()
-{
-}
-
 void ToshibaLED::init()
 {
     // default LED to healthy
@@ -45,9 +39,6 @@ void ToshibaLED::init()
         return;
     }
 
-    // set i2c bus to low speed - it seems to work at high speed even though the datasheet doesn't say this
-    //hal.i2c->setHighSpeed(false);
-
     // enable the led
     _healthy = (hal.i2c->writeRegister(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_ENABLE, 0x03) == 0);
 
@@ -59,6 +50,7 @@ void ToshibaLED::init()
     // register update with scheduler
     if (_healthy) {
         hal.scheduler->register_timer_process( ToshibaLED::_scheduled_update );
+        AP_Notify::register_update_function(ToshibaLED::update);
         _enabled = true;
     }
 
@@ -133,9 +125,18 @@ void ToshibaLED::set_rgb(uint8_t red, uint8_t green, uint8_t blue)
         return;
     }
 
+    // update the red value
+    if (red != _red_curr) {
+        if (hal.i2c->writeRegister(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_PWM2, red>>4) == 0) {
+            _red_curr = red;
+        }else{
+            success = false;
+        }
+    }
+
     // update the green value
     if (green != _green_curr) {
-        if (hal.i2c->writeRegister(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_PWM0, green) == 0) {
+        if (hal.i2c->writeRegister(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_PWM1, green>>4) == 0) {
             _green_curr = green;
         }else{
             success = false;
@@ -144,22 +145,13 @@ void ToshibaLED::set_rgb(uint8_t red, uint8_t green, uint8_t blue)
 
     // update the blue value
     if (blue != _blue_curr) {
-        if (hal.i2c->writeRegister(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_PWM1, blue) == 0) {
+        if (hal.i2c->writeRegister(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_PWM0, blue>>4) == 0) {
             _blue_curr = blue;
         }else{
             success = false;
         }
     }
 
-    // update the red value
-    if (red != _red_curr) {
-        if (hal.i2c->writeRegister(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_PWM2, red) == 0) {
-            _red_curr = red;
-        }else{
-            success = false;
-        }
-    }       
-
     // set healthy status
     _healthy = success;
 
@@ -170,166 +162,166 @@ void ToshibaLED::set_rgb(uint8_t red, uint8_t green, uint8_t blue)
 // _scheduled_update - updates _red, _green, _blue according to notify flags
 void ToshibaLED::_scheduled_update(uint32_t now)
 {
-    _counter++;
+    static uint8_t counter;     // to reduce 1khz to 10hz
+    static uint8_t step;        // holds step of 10hz filter
 
-    // we never want to update LEDs at a higher than 16Hz rate
+    // slow rate from 1khz to 10hz
-    if (_counter & 0x3F) {
+    counter++;
+    if (counter < 100) {
         return;
     }
 
-    // counter2 used to drop frequency down to 16hz
+    // reset counter
-    uint8_t counter2 = _counter >> 6;
+    counter = 0;
+
+    // move forward one step
+    step++;
+    if (step>=10) {
+        step = 0;
+    }
 
     // initialising pattern
-    static uint8_t init_counter = 0;
+    if (AP_Notify::flags.initialising) {
-    init_counter++;
+        if (step & 1) {
-    if (notify.flags.initialising) {
+            // odd steps display red light
-        // blink LED red and blue alternatively
-        if (init_counter == 1) {
-            // turn  on red
             _red_des = TOSHIBA_LED_DIM;
-            _blue_des = 0;
+            _blue_des = TOSHIBA_LED_OFF;
-            _green_des = 0;
+            _green_des = TOSHIBA_LED_OFF;
         }else{
-            // turn on blue
+            // even display blue light
-            _red_des = 0;
+            _red_des = TOSHIBA_LED_OFF;
             _blue_des = TOSHIBA_LED_DIM;
-            _green_des = 0;
+            _green_des = TOSHIBA_LED_OFF;
-            init_counter = 0;
         }
+
+        // exit so no other status modify this pattern
         return;
 	}
 
-    // save trim pattern
+    // save trim and esc calibration pattern
-    if (notify.flags.save_trim) {
+    if (AP_Notify::flags.save_trim || AP_Notify::flags.esc_calibration) {
-        static uint8_t save_trim_counter = 0;
+        switch(step) {
-        if ((counter2 & 0x2) == 0) {
-            save_trim_counter++;
-        }
-        switch(save_trim_counter) {
             case 0:
+            case 3:
+            case 6:
                 _red_des = TOSHIBA_LED_DIM;
                 _blue_des = TOSHIBA_LED_OFF;
                 _green_des = TOSHIBA_LED_OFF;
                 break;
 
             case 1:
+            case 4:
+            case 7:
                 _red_des = TOSHIBA_LED_OFF;
                 _blue_des = TOSHIBA_LED_DIM;
                 _green_des = TOSHIBA_LED_OFF;
                 break;
 
             case 2:
+            case 5:
+            case 8:
                 _red_des = TOSHIBA_LED_OFF;
                 _blue_des = TOSHIBA_LED_OFF;
                 _green_des = TOSHIBA_LED_DIM;
                 break;
 
-            default:
+            case 9:
-                save_trim_counter = -1;
-        }
-        return;
-    }
-
-    // armed and gps
-    static uint8_t arm_or_gps = 0;  // 0 = displaying arming state, 1 = displaying gps state
-    // switch between showing arming and gps state every second
-    if (counter2 == 0) {
-        arm_or_gps = !arm_or_gps;
-        // turn off both red and blue
                 _red_des = TOSHIBA_LED_OFF;
                 _blue_des = TOSHIBA_LED_OFF;
                 _green_des = TOSHIBA_LED_OFF;
+                break;
+        }
+        // exit so no other status modify this pattern
+        return;
     }
 
-    // displaying arming state
+    // solid green or flashing green if armed
-    if (arm_or_gps == 0) {
+    if (AP_Notify::flags.armed) {
-        static uint8_t arm_counter = 0;
+        // solid green if armed with 3d lock
-        if (notify.flags.armed) {
+        if (AP_Notify::flags.gps_status == 3) {
-            // red led solid
+            _red_des = TOSHIBA_LED_OFF;
-            _red_des = TOSHIBA_LED_DIM;
+            _blue_des = TOSHIBA_LED_OFF;
+            _green_des = TOSHIBA_LED_DIM;
         }else{
-            if ((counter2 & 0x2) == 0) {
+            // flash green if armed with no gps lock
-                arm_counter++;
+            switch(step) {
-            }
-            if (notify.flags.pre_arm_check) {
-                // passed pre-arm checks so slower single flash
-                switch(arm_counter) {
                 case 0:
                 case 1:
                 case 2:
-                        _red_des = TOSHIBA_LED_DIM;
-                        break;
                 case 3:
                 case 4:
-                    case 5:
                     _red_des = TOSHIBA_LED_OFF;
-                        break;
+                    _blue_des = TOSHIBA_LED_OFF;
-                    default:
+                    _green_des = TOSHIBA_LED_DIM;
-                        // reset counter to restart the sequence
-                        arm_counter = -1;
-                        break;
-                }
-            }else{
-                // failed pre-arm checks so double flash
-                switch(arm_counter) {
-                    case 0:
-                    case 1:
-                        _red_des = TOSHIBA_LED_DIM;
-                        break;
-                    case 2:
-                        _red_des = TOSHIBA_LED_OFF;
-                        break;
-                    case 3:
-                    case 4:
-                        _red_des = TOSHIBA_LED_DIM;
                     break;
                 case 5:
                 case 6:
+                case 7:
+                case 8:
+                case 9:
+                    // even display blue light
                     _red_des = TOSHIBA_LED_OFF;
-                        break;
+                    _blue_des = TOSHIBA_LED_OFF;
-                    default:
+                    _green_des = TOSHIBA_LED_OFF;
-                        arm_counter = -1;
                     break;
             }
         }
-        }
+        return;
     }else{
-        // gps light
+        // flash yellow if failing pre-arm checks
-        switch (notify.flags.gps_status) {
+        if (!AP_Notify::flags.pre_arm_check) {
+            // flashing blue if no gps lock
+            switch(step) {
                 case 0:
-                // no GPS attached
-                _blue_des = TOSHIBA_LED_OFF;
-                break;
-
                 case 1:
-                // GPS attached but no lock, blink at 4Hz
-                if ((counter2 & 0x3) == 0) {
-                    // toggle blue
-                    if (_blue_des == TOSHIBA_LED_OFF) {
-                        _blue_des = TOSHIBA_LED_DIM;
-                    }else{
-                        _blue_des = TOSHIBA_LED_OFF;
-                    }
-                }
-                break;
-
                 case 2:
-                // GPS attached but 2D lock, blink more slowly (around 2Hz)
-                if ((counter2 & 0x7) == 0) {
-                    // toggle blue
-                    if (_blue_des == TOSHIBA_LED_OFF) {
-                        _blue_des = TOSHIBA_LED_DIM;
-                    }else{
-                        _blue_des = TOSHIBA_LED_OFF;
-                    }
-                }
-                break;
-
                 case 3:
-                // 3D lock so become solid
+                case 4:
-                _blue_des = TOSHIBA_LED_DIM;
+                    _red_des = TOSHIBA_LED_DIM;
+                    _blue_des = TOSHIBA_LED_OFF;
+                    _green_des = TOSHIBA_LED_DIM;
                     break;
+                case 5:
+                case 6:
+                case 7:
+                case 8:
+                case 9:
+                    // even display blue light
+                    _red_des = TOSHIBA_LED_OFF;
+                    _blue_des = TOSHIBA_LED_OFF;
+                    _green_des = TOSHIBA_LED_OFF;
+                    break;
+            }
+        }else{
+            // solid blue if gps 3d lock
+            if (AP_Notify::flags.gps_status == 3) {
+                _red_des = TOSHIBA_LED_OFF;
+                _blue_des = TOSHIBA_LED_DIM;
+                _green_des = TOSHIBA_LED_OFF;
+            }else{
+                // flashing blue if no gps lock
+                switch(step) {
+                    case 0:
+                    case 1:
+                    case 2:
+                    case 3:
+                    case 4:
+                        _red_des = TOSHIBA_LED_OFF;
+                        _blue_des = TOSHIBA_LED_DIM;
+                        _green_des = TOSHIBA_LED_OFF;
+                        break;
+                    case 5:
+                    case 6:
+                    case 7:
+                    case 8:
+                    case 9:
+                        // even display blue light
+                        _red_des = TOSHIBA_LED_OFF;
+                        _blue_des = TOSHIBA_LED_OFF;
+                        _green_des = TOSHIBA_LED_OFF;
+                        break;
+                }
+            }
         }
     }
 }
@@ -342,5 +334,5 @@ void ToshibaLED::update()
         return;
     }
 
-    set_rgb(_red_des,_blue_des,_green_des);
+    set_rgb(_red_des,_green_des,_blue_des);
 }

libraries/AP_Notify/ToshibaLED.h

@@ -14,7 +14,6 @@
 #define __TOSHIBA_LED_H__
 
 #include <AP_HAL.h>
-//#include "AP_HAL_AVR_Namespace.h"
 #include <AP_Notify.h>
 
 #define TOSHIBA_LED_ADDRESS 0x55    // default I2C bus address
@@ -23,17 +22,14 @@
 #define TOSHIBA_LED_PWM2    0x03    // pwm2 register
 #define TOSHIBA_LED_ENABLE  0x04    // enable register
 
-#define TOSHIBA_LED_BRIGHT  0x0F    // full brightness
+#define TOSHIBA_LED_BRIGHT  0xFF    // full brightness
-#define TOSHIBA_LED_MEDIUM  0x0A    // medium brightness
+#define TOSHIBA_LED_MEDIUM  0x80    // medium brightness
-#define TOSHIBA_LED_DIM     0x01    // dim      // was 0x05
+#define TOSHIBA_LED_DIM     0x11    // dim
-#define TOSHIBA_LED_OFF     0x00    // dim      // was 0x05
+#define TOSHIBA_LED_OFF     0x00    // off
 
 class ToshibaLED {
 public:
 
-    // constructor
-    ToshibaLED();
-
     // init - initialised the LED
     static void init(void);
 
@@ -62,7 +58,6 @@ private:
     static bool _healthy;                               // true if the LED is operating properly
     static uint8_t _red_des, _green_des, _blue_des;     // color requested by timed update
     static uint8_t _red_curr, _green_curr, _blue_curr;  // current colours displayed by the led
-    static uint16_t _counter;                           // used to slow the update rate
 };
 
 #endif // __TOSHIBA_LED_H__

libraries/AP_Notify/examples/AP_Notify_test/AP_Notify_test.pde

@@ -25,10 +25,10 @@ void setup()
     board_led.init();
 
     // turn on initialising notification
-    //notify.flags.initialising = true;
+    AP_Notify::flags.initialising = true;
-    notify.flags.gps_status = 1;
+    AP_Notify::flags.gps_status = 1;
-    notify.flags.armed = 1;
+    AP_Notify::flags.armed = 1;
-    notify.flags.pre_arm_check = 1;
+    AP_Notify::flags.pre_arm_check = 1;
 }
 
 void loop()

libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.pde

@@ -29,11 +29,11 @@ void setup(void)
     }
 
     // turn on initialising notification
-    notify.flags.initialising = false;
+    AP_Notify::flags.initialising = false;
-    notify.flags.save_trim = true;
+    AP_Notify::flags.save_trim = true;
-    notify.flags.gps_status = 1;
+    AP_Notify::flags.gps_status = 1;
-    notify.flags.armed = 1;
+    AP_Notify::flags.armed = 1;
-    notify.flags.pre_arm_check = 1;
+    AP_Notify::flags.pre_arm_check = 1;
 }
 
 void loop(void)