diff --git a/libraries/AP_Notify/AP_BoardLED.cpp b/libraries/AP_Notify/AP_BoardLED.cpp index 798c50fbeb..7d021322b3 100644 --- a/libraries/AP_Notify/AP_BoardLED.cpp +++ b/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; } diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index 88b4084675..964a85fae7 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/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 notify; -/// Constructor -//AP_Notify::AP_Notify() -//{ -//} +struct AP_Notify::notify_type AP_Notify::flags; + +AP_Notify::update_fn_t AP_Notify::_update_fn; \ No newline at end of file diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index 92dc9853d4..4408bd1621 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/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,11 +20,16 @@ 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; + }; + + static struct notify_type flags; + + /// 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(); } - /// Constructor - //Notify(); - /// To-Do: potential notifications to add /// flight_mode @@ -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__ diff --git a/libraries/AP_Notify/ToshibaLED.cpp b/libraries/AP_Notify/ToshibaLED.cpp index be06f6c251..cbd30975a7 100644 --- a/libraries/AP_Notify/ToshibaLED.cpp +++ b/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); @@ -58,7 +49,8 @@ void ToshibaLED::init() // register update with scheduler if (_healthy) { - hal.scheduler->register_timer_process( ToshibaLED::_scheduled_update ); + 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,167 +162,167 @@ 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 - if (_counter & 0x3F) { + // slow rate from 1khz to 10hz + counter++; + if (counter < 100) { return; } - // counter2 used to drop frequency down to 16hz - uint8_t counter2 = _counter >> 6; + // reset counter + counter = 0; + + // move forward one step + step++; + if (step>=10) { + step = 0; + } // initialising pattern - static uint8_t init_counter = 0; - init_counter++; - if (notify.flags.initialising) { - // blink LED red and blue alternatively - if (init_counter == 1) { - // turn on red + if (AP_Notify::flags.initialising) { + if (step & 1) { + // odd steps display red light _red_des = TOSHIBA_LED_DIM; - _blue_des = 0; - _green_des = 0; + _blue_des = TOSHIBA_LED_OFF; + _green_des = TOSHIBA_LED_OFF; }else{ - // turn on blue - _red_des = 0; + // even display blue light + _red_des = TOSHIBA_LED_OFF; _blue_des = TOSHIBA_LED_DIM; - _green_des = 0; - init_counter = 0; + _green_des = TOSHIBA_LED_OFF; } + + // exit so no other status modify this pattern return; } - // save trim pattern - if (notify.flags.save_trim) { - static uint8_t save_trim_counter = 0; - if ((counter2 & 0x2) == 0) { - save_trim_counter++; - } - switch(save_trim_counter) { + // save trim and esc calibration pattern + if (AP_Notify::flags.save_trim || AP_Notify::flags.esc_calibration) { + switch(step) { 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: - save_trim_counter = -1; + case 9: + _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; } - // 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; - } - - // displaying arming state - if (arm_or_gps == 0) { - static uint8_t arm_counter = 0; - if (notify.flags.armed) { - // red led solid - _red_des = TOSHIBA_LED_DIM; + // solid green or flashing green if armed + if (AP_Notify::flags.armed) { + // solid green if armed with 3d lock + if (AP_Notify::flags.gps_status == 3) { + _red_des = TOSHIBA_LED_OFF; + _blue_des = TOSHIBA_LED_OFF; + _green_des = TOSHIBA_LED_DIM; }else{ - if ((counter2 & 0x2) == 0) { - arm_counter++; + // flash green if armed with no gps lock + switch(step) { + case 0: + case 1: + case 2: + case 3: + case 4: + _red_des = TOSHIBA_LED_OFF; + _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; } - 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; - default: - // reset counter to restart the sequence - arm_counter = -1; - break; - } + } + return; + }else{ + // flash yellow if failing pre-arm checks + if (!AP_Notify::flags.pre_arm_check) { + // flashing blue if no gps lock + switch(step) { + case 0: + case 1: + case 2: + case 3: + case 4: + _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{ - // failed pre-arm checks so double flash - switch(arm_counter) { + // flashing blue if no gps lock + switch(step) { 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; + _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; - break; - default: - arm_counter = -1; + _blue_des = TOSHIBA_LED_OFF; + _green_des = TOSHIBA_LED_OFF; break; } } } - }else{ - // gps light - switch (notify.flags.gps_status) { - 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 - _blue_des = TOSHIBA_LED_DIM; - 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); } \ No newline at end of file diff --git a/libraries/AP_Notify/ToshibaLED.h b/libraries/AP_Notify/ToshibaLED.h index 1733bf1ce7..e623772e5f 100644 --- a/libraries/AP_Notify/ToshibaLED.h +++ b/libraries/AP_Notify/ToshibaLED.h @@ -14,7 +14,6 @@ #define __TOSHIBA_LED_H__ #include -//#include "AP_HAL_AVR_Namespace.h" #include #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_MEDIUM 0x0A // medium brightness -#define TOSHIBA_LED_DIM 0x01 // dim // was 0x05 -#define TOSHIBA_LED_OFF 0x00 // dim // was 0x05 +#define TOSHIBA_LED_BRIGHT 0xFF // full brightness +#define TOSHIBA_LED_MEDIUM 0x80 // medium brightness +#define TOSHIBA_LED_DIM 0x11 // dim +#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__ diff --git a/libraries/AP_Notify/examples/AP_Notify_test/AP_Notify_test.pde b/libraries/AP_Notify/examples/AP_Notify_test/AP_Notify_test.pde index 4e7bbda985..0248b98c96 100644 --- a/libraries/AP_Notify/examples/AP_Notify_test/AP_Notify_test.pde +++ b/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; - notify.flags.gps_status = 1; - notify.flags.armed = 1; - notify.flags.pre_arm_check = 1; + AP_Notify::flags.initialising = true; + AP_Notify::flags.gps_status = 1; + AP_Notify::flags.armed = 1; + AP_Notify::flags.pre_arm_check = 1; } void loop() diff --git a/libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.pde b/libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.pde index 4373a3be18..1b02ec6dc2 100644 --- a/libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.pde +++ b/libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.pde @@ -29,11 +29,11 @@ void setup(void) } // turn on initialising notification - notify.flags.initialising = false; - notify.flags.save_trim = true; - notify.flags.gps_status = 1; - notify.flags.armed = 1; - notify.flags.pre_arm_check = 1; + AP_Notify::flags.initialising = false; + AP_Notify::flags.save_trim = true; + AP_Notify::flags.gps_status = 1; + AP_Notify::flags.armed = 1; + AP_Notify::flags.pre_arm_check = 1; } void loop(void)