Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Notify: avoid suspend_timer_procs() by using atomic updates in ToshibaLED_PX4

Browse Source
This commit is contained in:
Andrew Tridgell 2014-08-21 07:31:53 +10:00
parent 7d350735df
commit 7343de2893
2 changed files with 27 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_Notify/ToshibaLED_PX4.cpp
View File

@ -41,8 +41,8 @@ bool ToshibaLED_PX4::hw_init()
return false; return false;
} }
ioctl(_rgbled_fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON); ioctl(_rgbled_fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
last.zero(); last.v = 0;
next.zero(); next.v = 0;
hal.scheduler->register_io_process(AP_HAL_MEMBERPROC(&ToshibaLED_PX4::update_timer)); hal.scheduler->register_io_process(AP_HAL_MEMBERPROC(&ToshibaLED_PX4::update_timer));
return true; return true;
} }
@ -50,28 +50,30 @@ bool ToshibaLED_PX4::hw_init()
// set_rgb - set color as a combination of red, green and blue values // set_rgb - set color as a combination of red, green and blue values
bool ToshibaLED_PX4::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) bool ToshibaLED_PX4::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
{ {
hal.scheduler->suspend_timer_procs(); union rgb_value v;
next[0] = red; v.r = red;
next[1] = green; v.g = green;
next[2] = blue; v.b = blue;
hal.scheduler->resume_timer_procs(); // this does an atomic 32 bit update
next.v = v.v;
return true; return true;
} }
void ToshibaLED_PX4::update_timer(void) void ToshibaLED_PX4::update_timer(void)
{ {
if (last == next) { if (last.v == next.v) {
return; return;
} }
rgbled_rgbset_t v; rgbled_rgbset_t v;
union rgb_value newv;
v.red = next[0]; newv.v = next.v;
v.green = next[1]; v.red = newv.r;
v.blue = next[2]; v.green = newv.g;
v.blue = newv.b;
ioctl(_rgbled_fd, RGBLED_SET_RGB, (unsigned long)&v); ioctl(_rgbled_fd, RGBLED_SET_RGB, (unsigned long)&v);
last = next; last.v = next.v;
} }
#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4 #endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4

13
libraries/AP_Notify/ToshibaLED_PX4.h
View File

@ -31,7 +31,18 @@ private:
int _rgbled_fd; int _rgbled_fd;
void update_timer(void); void update_timer(void);
VectorN<uint8_t,3> last, next; // use a union so that updates can be of a single 32 bit value,
// making it atomic on PX4
union rgb_value {
struct {
uint8_t r;
uint8_t g;
uint8_t b;
};
volatile uint32_t v;
};
union rgb_value last, next;
}; };
#endif // __TOSHIBA_LED_PX4_H__ #endif // __TOSHIBA_LED_PX4_H__