Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Notify: make flags static variable

Browse Source
This commit is contained in:
Randy Mackay 2013-08-14 11:50:52 +09:00 committed by Andrew Tridgell
parent 496962f037
commit a52b1831ca
7 changed files with 160 additions and 176 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_Notify/AP_BoardLED.cpp
View File

@ -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;
}

8
libraries/AP_Notify/AP_Notify.cpp
View File

@ -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
View File

@ -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
//Notify();
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(); }
/// 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__

242
libraries/AP_Notify/ToshibaLED.cpp
View File

@ -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
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;
}
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
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;
}
// 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++;
}
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;
break;
default:
// 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;
_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;
}
}
}
return;
}else{
// gps light
switch (notify.flags.gps_status) {
// flash yellow if failing pre-arm checks
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
_blue_des = TOSHIBA_LED_DIM;
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{
// 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);
}

13
libraries/AP_Notify/ToshibaLED.h
View File

@ -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_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__

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

@ -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
View File

@ -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)