mirror of https://github.com/ArduPilot/ardupilot
AP_OSD: use software blink
This should fix blink issue with some hardware
This commit is contained in:
parent
f083b80700
commit
785cf293cd
|
@ -25,12 +25,16 @@ extern const AP_HAL::HAL& hal;
|
|||
|
||||
void AP_OSD_Backend::write(uint8_t x, uint8_t y, bool blink, const char *fmt, ...)
|
||||
{
|
||||
if (blink && (blink_phase < 2)) {
|
||||
return;
|
||||
}
|
||||
char buff[32];
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
int res = hal.util->vsnprintf(buff, sizeof(buff), fmt, ap);
|
||||
if (res > 0 && _osd.options.get() & AP_OSD::OPTION_DECIMAL_PACK) {
|
||||
if (res > 0 && check_option(AP_OSD::OPTION_DECIMAL_PACK)) {
|
||||
// automatically use packed decimal characters
|
||||
// based on fiam idea implemented in inav osd
|
||||
char *p = strchr(&buff[1],'.');
|
||||
if (p && isdigit(p[1]) && isdigit(p[-1])) {
|
||||
p[-1] += SYM_DIG_OFS_1;
|
||||
|
@ -40,7 +44,7 @@ void AP_OSD_Backend::write(uint8_t x, uint8_t y, bool blink, const char *fmt, ..
|
|||
}
|
||||
}
|
||||
if (res < int(sizeof(buff))) {
|
||||
write(x, y, buff, blink? AP_OSD_Backend::BLINK : 0);
|
||||
write(x, y, buff);
|
||||
}
|
||||
va_end(ap);
|
||||
}
|
||||
|
|
|
@ -30,7 +30,7 @@ public:
|
|||
virtual ~AP_OSD_Backend(void) {}
|
||||
|
||||
//draw given text to framebuffer
|
||||
virtual void write(uint8_t x, uint8_t y, const char* text, uint8_t char_attr = 0) = 0;
|
||||
virtual void write(uint8_t x, uint8_t y, const char* text) = 0;
|
||||
|
||||
//draw formatted text to framebuffer
|
||||
virtual void write(uint8_t x, uint8_t y, bool blink, const char *fmt, ...);
|
||||
|
@ -42,11 +42,10 @@ public:
|
|||
virtual void flush() = 0;
|
||||
|
||||
//clear screen
|
||||
virtual void clear() = 0;
|
||||
|
||||
enum character_attribute_t {
|
||||
BLINK = (1 << 4),
|
||||
INVERT = (1 << 3),
|
||||
//should match hw blink
|
||||
virtual void clear()
|
||||
{
|
||||
blink_phase = (blink_phase+1)%4;
|
||||
};
|
||||
|
||||
AP_OSD * get_osd()
|
||||
|
@ -58,8 +57,18 @@ protected:
|
|||
AP_OSD& _osd;
|
||||
|
||||
// get font choice
|
||||
uint8_t get_font_num(void) const { return (uint8_t)_osd.font_num.get(); }
|
||||
uint8_t get_font_num(void) const
|
||||
{
|
||||
return (uint8_t)_osd.font_num.get();
|
||||
}
|
||||
|
||||
//check option
|
||||
bool check_option(uint32_t option)
|
||||
{
|
||||
return (_osd.options & option) != 0;
|
||||
}
|
||||
|
||||
int8_t blink_phase;
|
||||
};
|
||||
|
||||
|
||||
|
|
|
@ -347,7 +347,6 @@ void AP_OSD_MAX7456::reinit()
|
|||
|
||||
// force redrawing all screen
|
||||
memset(shadow_frame, 0xFF, sizeof(shadow_frame));
|
||||
memset(shadow_attr, 0xFF, sizeof(shadow_attr));
|
||||
|
||||
initialized = true;
|
||||
}
|
||||
|
@ -380,8 +379,8 @@ void AP_OSD_MAX7456::flush()
|
|||
|
||||
void AP_OSD_MAX7456::transfer_frame()
|
||||
{
|
||||
uint8_t last_attribute = 0xFF;
|
||||
uint16_t previous_pos = UINT16_MAX - 1;
|
||||
bool autoincrement = false;
|
||||
if (!initialized) {
|
||||
return;
|
||||
}
|
||||
|
@ -389,8 +388,7 @@ void AP_OSD_MAX7456::transfer_frame()
|
|||
buffer_offset = 0;
|
||||
for (uint8_t y=0; y<video_lines; y++) {
|
||||
for (uint8_t x=0; x<video_columns; x++) {
|
||||
|
||||
if (frame[y][x] == shadow_frame[y][x] && attr[y][x] == shadow_attr[y][x]) {
|
||||
if (!is_dirty(x, y)) {
|
||||
continue;
|
||||
}
|
||||
//ensure space for 1 char and escape sequence
|
||||
|
@ -398,58 +396,67 @@ void AP_OSD_MAX7456::transfer_frame()
|
|||
break;
|
||||
}
|
||||
shadow_frame[y][x] = frame[y][x];
|
||||
shadow_attr[y][x] = attr[y][x];
|
||||
uint8_t attribute = attr[y][x] & (DMM_BLINK | DMM_INVERT_PIXEL_COLOR);
|
||||
uint8_t chr = frame[y][x];
|
||||
uint16_t pos = y * video_columns + x;
|
||||
bool attribute_changed = (attribute != last_attribute);
|
||||
bool position_changed = ((previous_pos + 1) != pos);
|
||||
|
||||
if (position_changed) {
|
||||
if (position_changed && autoincrement) {
|
||||
//it is impossible to write to MAX7456ADD_DMAH/MAX7456ADD_DMAL in autoincrement mode
|
||||
//so, exit autoincrement mode
|
||||
if (buffer_offset != 0) {
|
||||
buffer_add_cmd(MAX7456ADD_DMDI, 0xFF);
|
||||
buffer_add_cmd(MAX7456ADD_DMM, 0);
|
||||
autoincrement = false;
|
||||
}
|
||||
|
||||
if (!autoincrement) {
|
||||
buffer_add_cmd(MAX7456ADD_DMAH, pos >> 8);
|
||||
buffer_add_cmd(MAX7456ADD_DMAL, pos & 0xFF);
|
||||
}
|
||||
|
||||
//update character attributes and (re)enter autoincrement mode
|
||||
if (position_changed || attribute_changed) {
|
||||
buffer_add_cmd(MAX7456ADD_DMM, attribute | DMM_AUTOINCREMENT);
|
||||
last_attribute = attribute;
|
||||
if (!autoincrement && is_dirty(x+1, y)) {
|
||||
//(re)enter autoincrement mode
|
||||
buffer_add_cmd(MAX7456ADD_DMM, DMM_AUTOINCREMENT);
|
||||
autoincrement = true;
|
||||
}
|
||||
|
||||
buffer_add_cmd(MAX7456ADD_DMDI, chr);
|
||||
previous_pos = pos;
|
||||
}
|
||||
}
|
||||
|
||||
if (buffer_offset > 0) {
|
||||
if (autoincrement) {
|
||||
buffer_add_cmd(MAX7456ADD_DMDI, 0xFF);
|
||||
buffer_add_cmd(MAX7456ADD_DMM, 0);
|
||||
autoincrement = false;
|
||||
}
|
||||
|
||||
if (buffer_offset > 0) {
|
||||
_dev->get_semaphore()->take_blocking();
|
||||
_dev->transfer(buffer, buffer_offset, nullptr, 0);
|
||||
_dev->get_semaphore()->give();
|
||||
}
|
||||
}
|
||||
|
||||
void AP_OSD_MAX7456::clear()
|
||||
bool AP_OSD_MAX7456::is_dirty(uint8_t x, uint8_t y)
|
||||
{
|
||||
memset(frame, ' ', sizeof(frame));
|
||||
memset(attr, 0, sizeof(attr));
|
||||
if (y>=video_lines || x>=video_columns) {
|
||||
return false;
|
||||
}
|
||||
return frame[y][x] != shadow_frame[y][x];
|
||||
}
|
||||
|
||||
void AP_OSD_MAX7456::write(uint8_t x, uint8_t y, const char* text, uint8_t char_attr)
|
||||
void AP_OSD_MAX7456::clear()
|
||||
{
|
||||
AP_OSD_Backend::clear();
|
||||
memset(frame, ' ', sizeof(frame));
|
||||
}
|
||||
|
||||
void AP_OSD_MAX7456::write(uint8_t x, uint8_t y, const char* text)
|
||||
{
|
||||
if (y >= video_lines_pal || text == nullptr) {
|
||||
return;
|
||||
}
|
||||
while ((x < VIDEO_COLUMNS) && (*text != 0)) {
|
||||
frame[y][x] = *text;
|
||||
attr[y][x] = char_attr;
|
||||
++text;
|
||||
++x;
|
||||
}
|
||||
|
|
|
@ -25,7 +25,7 @@ public:
|
|||
static AP_OSD_Backend *probe(AP_OSD &osd, AP_HAL::OwnPtr<AP_HAL::Device> dev);
|
||||
|
||||
//draw given text to framebuffer
|
||||
void write(uint8_t x, uint8_t y, const char* text, uint8_t char_attr) override;
|
||||
void write(uint8_t x, uint8_t y, const char* text) override;
|
||||
|
||||
//initilize display port and underlying hardware
|
||||
bool init() override;
|
||||
|
@ -55,6 +55,8 @@ private:
|
|||
|
||||
void transfer_frame();
|
||||
|
||||
bool is_dirty(uint8_t x, uint8_t y);
|
||||
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
|
||||
|
||||
uint8_t video_signal_reg;
|
||||
|
@ -74,10 +76,6 @@ private:
|
|||
//used to optimize number of characters updated
|
||||
uint8_t shadow_frame[video_lines_pal][video_columns];
|
||||
|
||||
// this assumes at most 32 columns
|
||||
uint8_t attr[video_lines_pal][video_columns];
|
||||
uint8_t shadow_attr[video_lines_pal][video_columns];
|
||||
|
||||
uint8_t buffer[spi_buffer_size];
|
||||
int buffer_offset;
|
||||
|
||||
|
|
|
@ -98,7 +98,7 @@ void AP_OSD_SITL::load_font(void)
|
|||
free(font_data);
|
||||
}
|
||||
|
||||
void AP_OSD_SITL::write(uint8_t x, uint8_t y, const char* text, uint8_t char_attr)
|
||||
void AP_OSD_SITL::write(uint8_t x, uint8_t y, const char* text)
|
||||
{
|
||||
if (y >= video_lines || text == nullptr) {
|
||||
return;
|
||||
|
@ -106,7 +106,6 @@ void AP_OSD_SITL::write(uint8_t x, uint8_t y, const char* text, uint8_t char_att
|
|||
mutex->take_blocking();
|
||||
while ((x < video_cols) && (*text != 0)) {
|
||||
buffer[y][x] = *text;
|
||||
attr[y][x] = char_attr;
|
||||
++text;
|
||||
++x;
|
||||
}
|
||||
|
@ -115,6 +114,7 @@ void AP_OSD_SITL::write(uint8_t x, uint8_t y, const char* text, uint8_t char_att
|
|||
|
||||
void AP_OSD_SITL::clear(void)
|
||||
{
|
||||
AP_OSD_Backend::clear();
|
||||
mutex->take_blocking();
|
||||
memset(buffer, 0, sizeof(buffer));
|
||||
mutex->give();
|
||||
|
@ -136,7 +136,6 @@ void AP_OSD_SITL::update_thread(void)
|
|||
AP_HAL::panic("Unable to create OSD window");
|
||||
}
|
||||
|
||||
uint8_t blink = 0;
|
||||
while (w->isOpen()) {
|
||||
sf::Event event;
|
||||
while (w->pollEvent(event)) {
|
||||
|
@ -151,10 +150,8 @@ void AP_OSD_SITL::update_thread(void)
|
|||
last_counter = counter;
|
||||
|
||||
uint8_t buffer2[video_lines][video_cols];
|
||||
uint8_t attr2[video_lines][video_cols];
|
||||
mutex->take_blocking();
|
||||
memcpy(buffer2, buffer, sizeof(buffer2));
|
||||
memcpy(attr2, attr, sizeof(attr2));
|
||||
mutex->give();
|
||||
w->clear();
|
||||
|
||||
|
@ -164,11 +161,6 @@ void AP_OSD_SITL::update_thread(void)
|
|||
uint16_t py = y * (char_height+char_spacing) * char_scale;
|
||||
sf::Sprite s;
|
||||
uint8_t c = buffer2[y][x];
|
||||
if (attr2[y][x] & BLINK) {
|
||||
if (blink >= 2) {
|
||||
c = ' ';
|
||||
}
|
||||
}
|
||||
s.setTexture(font[c]);
|
||||
s.setPosition(sf::Vector2f(px, py));
|
||||
s.scale(sf::Vector2f(char_scale,char_scale));
|
||||
|
@ -176,7 +168,6 @@ void AP_OSD_SITL::update_thread(void)
|
|||
}
|
||||
}
|
||||
|
||||
blink = (blink+1) % 4;
|
||||
w->display();
|
||||
if (last_font != get_font_num()) {
|
||||
load_font();
|
||||
|
|
|
@ -31,7 +31,7 @@ public:
|
|||
static AP_OSD_Backend *probe(AP_OSD &osd);
|
||||
|
||||
//draw given text to framebuffer
|
||||
void write(uint8_t x, uint8_t y, const char* text, uint8_t char_attr) override;
|
||||
void write(uint8_t x, uint8_t y, const char* text) override;
|
||||
|
||||
//initilize display port and underlying hardware
|
||||
bool init() override;
|
||||
|
@ -62,7 +62,6 @@ private:
|
|||
static const uint8_t char_scale = 2;
|
||||
|
||||
uint8_t buffer[video_lines][video_cols];
|
||||
uint8_t attr[video_lines][video_cols];
|
||||
|
||||
void update_thread();
|
||||
static void *update_thread_start(void *obj);
|
||||
|
|
Loading…
Reference in New Issue