Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
#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