#include <stdio.h> | |
#include <stdlib.h> | |
#include <string.h> | |
#include <unistd.h> | |
#include <stdint.h> | |
#include <ctype.h> | |
#include <time.h> | |
#include <wiringPi.h> | |
#include <math.h> | |
#include <bcm_host.h> | |
#include <pthread.h> | |
// Pin 9 - WPi 9 | |
// Pin 11 - WPi 7 | |
// Pin 13 - WPi 15 | |
// Pin 15 - WPi 16 | |
#define PIN_VS (9) | |
#define PIN_HS (7) | |
#define PIN_VCLK (15) | |
#define PIN_VID (16) | |
#define SCR_WIDTH (320) | |
#define SCR_HEIGHT (256) | |
#define SCR_STRIDE (SCR_WIDTH / 8) | |
unsigned char framebufA[SCR_STRIDE * SCR_HEIGHT]; | |
unsigned char framebufB[SCR_STRIDE * SCR_HEIGHT]; | |
unsigned char *framebuffers[] = {framebufA, framebufB}; | |
unsigned char *frontbuffer; | |
sem_t update_wait_for_image; | |
sem_t image_wait_for_update; | |
pthread_mutex_t fps_lock; | |
int fps_counter; | |
static void delay_ns(unsigned int ns) | |
{ | |
struct timespec sleeper, dummy; | |
sleeper.tv_sec = 0; | |
sleeper.tv_nsec = ns ; | |
nanosleep (&sleeper, &dummy) ; | |
} | |
void frame(void) { | |
uint8_t *rdptr = frontbuffer; | |
digitalWrite(PIN_VCLK, 1); | |
for (int y = 0; y < SCR_HEIGHT + 1; y++) { | |
digitalWrite(PIN_HS, 0); | |
for (int x = 0; x < 4; x++) { | |
digitalWrite(PIN_VCLK, 0); | |
digitalWrite(PIN_VCLK, 0); | |
digitalWrite(PIN_VCLK, 1); | |
digitalWrite(PIN_VCLK, 1); | |
} | |
digitalWrite(PIN_HS, 1); | |
for (int x = 0; x < SCR_STRIDE; x++) { | |
uint8_t d = *rdptr++; | |
digitalWrite(PIN_VS, (y == 0) ? (((x > 10) && (x < 20)) ? 0 : 1) : 1); | |
for (int b = 0; b < 8; b++) { | |
digitalWrite(PIN_VID, d & 0x01); | |
d >>= 1; | |
digitalWrite(PIN_VCLK, 0); | |
digitalWrite(PIN_VCLK, 0); | |
digitalWrite(PIN_VCLK, 1); | |
} | |
} | |
} | |
digitalWrite(PIN_VS, 0); | |
digitalWrite(PIN_HS, 0); | |
digitalWrite(PIN_VCLK, 0); | |
} | |
void putpixel(unsigned char *buf, int x, int y, int c) { | |
if (c) | |
buf[SCR_STRIDE * y + x / 8] |= 1 << (x % 8); | |
else | |
buf[SCR_STRIDE * y + x / 8] &= ~(1 << (x % 8)); | |
} | |
static int add_clamp(int a, int b) { | |
int sum = a + b; | |
if (sum < 0) return 0; | |
if (sum > 255) return 255; | |
return sum; | |
} | |
static int add_clamp_pixel(uint8_t *buf, int x, int y, int v) { | |
buf[y * SCR_WIDTH + x] = add_clamp(buf[y * SCR_WIDTH + x], v); | |
} | |
void *image_thread_entrance(void *args) { | |
DISPMANX_DISPLAY_HANDLE_T display; | |
DISPMANX_MODEINFO_T display_info; | |
DISPMANX_RESOURCE_HANDLE_T screen_resource; | |
VC_IMAGE_TRANSFORM_T transform; | |
uint32_t image_prt; | |
VC_RECT_T rect1; | |
int ret; | |
char *fbp = malloc(SCR_WIDTH * SCR_HEIGHT * 2); | |
char *fbg = malloc(SCR_WIDTH * SCR_HEIGHT); | |
bcm_host_init(); | |
display = vc_dispmanx_display_open(0); | |
if (!display) { | |
fprintf(stderr, "Unable to open primary display"); | |
return NULL; | |
} | |
ret = vc_dispmanx_display_get_info(display, &display_info); | |
if (ret) { | |
fprintf(stderr, "Unable to get primary display information"); | |
return NULL; | |
} | |
fprintf(stdout, "Primary display is %d x %d", display_info.width, display_info.height); | |
screen_resource = vc_dispmanx_resource_create(VC_IMAGE_RGB565, SCR_WIDTH, SCR_HEIGHT, &image_prt); | |
if (!screen_resource) { | |
fprintf(stderr, "Unable to create screen buffer"); | |
vc_dispmanx_display_close(display); | |
return NULL; | |
} | |
vc_dispmanx_rect_set(&rect1, 0, 0, SCR_WIDTH, SCR_HEIGHT); | |
int backbufID = 0; | |
while (1) { | |
clock_t t = clock(); | |
ret = vc_dispmanx_snapshot(display, screen_resource, 0); | |
vc_dispmanx_resource_read_data(screen_resource, &rect1, fbp, SCR_WIDTH * 2); | |
uint16_t *rdptr = (uint16_t *)fbp; | |
uint8_t *wrptr = (uint8_t *)fbg; | |
sem_wait(&image_wait_for_update); | |
for (int y = 0; y < SCR_HEIGHT; y++) { | |
for (int x = 0; x < SCR_WIDTH; x++) { | |
uint16_t rgb = *rdptr++; | |
//putpixel(framebuffers[backbufID], x, y, (rgb > 0x8000)); | |
uint8_t red = ((rgb & 0xF800) >> 8); | |
uint8_t green = ((rgb & 0x07E0) >> 3); | |
uint8_t blue = ((rgb & 0x001F) << 3); | |
uint8_t grayscale = (0.2126 * red) + (0.7152 * green) + (0.0722 * blue); | |
*wrptr++ = grayscale; | |
} | |
} | |
for (int y = 0; y < SCR_HEIGHT; y++) { | |
for (int x = 0; x < SCR_WIDTH; x++) { | |
uint8_t old = fbg[y * SCR_WIDTH + x]; | |
uint8_t new = (old & 0x80) ? 0xff : 0x00; | |
int quantError = (int)old - (int)new; | |
fbg[y * SCR_WIDTH + x] = new; | |
add_clamp_pixel(fbg, x + 1, y , quantError * 7 / 16); | |
if (x != 0) | |
add_clamp_pixel(fbg, x - 1, y + 1, quantError * 3 / 16); | |
if (y != (SCR_HEIGHT - 1)) { | |
add_clamp_pixel(fbg, x , y + 1, quantError * 5 / 16); | |
add_clamp_pixel(fbg, x + 1, y + 1, quantError * 1 / 16); | |
} | |
} | |
} | |
for (int y = 0; y < SCR_HEIGHT; y++) { | |
for (int x = 0; x < SCR_WIDTH; x++) { | |
putpixel(framebuffers[backbufID], x, y, fbg[y * SCR_WIDTH + x]); | |
} | |
} | |
frontbuffer = framebuffers[backbufID]; | |
backbufID = !backbufID; | |
sem_post(&update_wait_for_image); | |
pthread_mutex_lock(&fps_lock); | |
fps_counter++; | |
pthread_mutex_unlock(&fps_lock); | |
// FPS limit | |
t = clock() - t; | |
int time_to_sleep = CLOCKS_PER_SEC / 61 - t; | |
if (time_to_sleep > 0) { | |
int ns_to_sleep = time_to_sleep * (1000000000 / CLOCKS_PER_SEC); | |
delay_ns(ns_to_sleep); | |
} | |
} | |
ret = vc_dispmanx_resource_delete(screen_resource); | |
vc_dispmanx_display_close(display); | |
free(fbp); | |
free(fbg); | |
return NULL; | |
} | |
void *update_thread_entrance(void *args) { | |
while (1) { | |
sem_wait(&update_wait_for_image); | |
frame(); | |
sem_post(&image_wait_for_update); | |
} | |
return NULL; | |
} | |
int main(int argc, char *argv[]) { | |
pthread_t update_thread; | |
pthread_t image_thread; | |
fps_counter = 0; | |
wiringPiSetup(); | |
pinMode(PIN_VS, OUTPUT); | |
pinMode(PIN_HS, OUTPUT); | |
pinMode(PIN_VCLK, OUTPUT); | |
pinMode(PIN_VID, OUTPUT); | |
digitalWrite(PIN_VS, 0); | |
digitalWrite(PIN_HS, 0); | |
digitalWrite(PIN_VCLK, 0); | |
digitalWrite(PIN_VID, 0); | |
sem_init(&update_wait_for_image, 0, 0); | |
sem_init(&image_wait_for_update, 0, 1); | |
pthread_mutex_init(&fps_lock, NULL); | |
pthread_create(&update_thread, NULL, update_thread_entrance, NULL); | |
pthread_create(&image_thread, NULL, image_thread_entrance, NULL); | |
printf("CLOCKS_PER_SEC = %d\n", CLOCKS_PER_SEC); | |
while(1) { | |
sleep(1); | |
pthread_mutex_lock(&fps_lock); | |
printf("FPS: %d\n", fps_counter); | |
fps_counter = 0; | |
pthread_mutex_unlock(&fps_lock); | |
} | |
pthread_join(update_thread, NULL); | |
pthread_join(image_thread, NULL); | |
sem_destroy(&update_wait_for_image); | |
sem_destroy(&image_wait_for_update); | |
return 0; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment