zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Notify: make flags static variable

master
Randy Mackay 12 years ago committed by Andrew Tridgell
parent
496962f037
commit
a52b1831ca
7 changed files with 160 additions and 176 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 20
      libraries/AP_Notify/AP_BoardLED.cpp
  2. 8
      libraries/AP_Notify/AP_Notify.cpp
  3. 19
      libraries/AP_Notify/AP_Notify.h
  4. 218
      libraries/AP_Notify/ToshibaLED.cpp
  5. 13
      libraries/AP_Notify/ToshibaLED.h
  6. 8
      libraries/AP_Notify/examples/AP_Notify_test/AP_Notify_test.pde
  7. 10
      libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.pde

20
libraries/AP_Notify/AP_BoardLED.cpp
Unescape View File

@ -31,7 +31,7 @@ void AP_BoardLED::_update(uint32_t now) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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;
}

8
libraries/AP_Notify/AP_Notify.cpp
Unescape View File

@ -1,8 +1,6 @@ @@ -1,8 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_Notify.h>
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;

19
libraries/AP_Notify/AP_Notify.h
Unescape View File

@ -9,6 +9,9 @@ class AP_Notify @@ -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: @@ -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;
};
static struct notify_type flags;
/// Constructor
//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: @@ -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__

218
libraries/AP_Notify/ToshibaLED.cpp
Unescape View File

@ -16,7 +16,6 @@ @@ -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 @@ -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() @@ -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() @@ -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) @@ -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) @@ -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) @@ -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
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
// 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_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;
_green_des = TOSHIBA_LED_DIM;
}else{
if ((counter2 & 0x2) == 0) {
arm_counter++;
}
if (notify.flags.pre_arm_check) {
// passed pre-arm checks so slower single flash
switch(arm_counter) {
// flash green if armed with no gps lock
switch(step) {
case 0:
case 1:
case 2:
_red_des = TOSHIBA_LED_DIM;
break;
case 3:
case 4:
case 5:
_red_des = TOSHIBA_LED_OFF;
_blue_des = TOSHIBA_LED_OFF;
_green_des = TOSHIBA_LED_DIM;
break;
default:
// reset counter to restart the sequence
arm_counter = -1;
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;
}
}
return;
}else{
// failed pre-arm checks so double flash
switch(arm_counter) {
// 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:
_red_des = TOSHIBA_LED_DIM;
break;
case 2:
_red_des = TOSHIBA_LED_OFF;
break;
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;
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) {
// 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{
_blue_des = TOSHIBA_LED_OFF;
}
}
break;
// flashing blue if no gps lock
switch(step) {
case 0:
case 1:
case 2:
// GPS attached but 2D lock, blink more slowly (around 2Hz)
if ((counter2 & 0x7) == 0) {
// toggle blue
if (_blue_des == TOSHIBA_LED_OFF) {
case 3:
case 4:
_red_des = TOSHIBA_LED_OFF;
_blue_des = TOSHIBA_LED_DIM;
}else{
_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;
}
}
break;
case 3:
// 3D lock so become solid
_blue_des = TOSHIBA_LED_DIM;
break;
}
}
}
@ -342,5 +334,5 @@ void ToshibaLED::update() @@ -342,5 +334,5 @@ void ToshibaLED::update()
return;
}
set_rgb(_red_des,_blue_des,_green_des);
set_rgb(_red_des,_green_des,_blue_des);
}

13
libraries/AP_Notify/ToshibaLED.h
Unescape View File

@ -14,7 +14,6 @@ @@ -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 @@ @@ -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: @@ -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__

8
libraries/AP_Notify/examples/AP_Notify_test/AP_Notify_test.pde
Unescape View File

@ -25,10 +25,10 @@ void setup() @@ -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()

10
libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.pde
Unescape View File

@ -29,11 +29,11 @@ void setup(void) @@ -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)

Write Preview
Loading…
Cancel
Save