Momentum-Apps/firmware/firmware.ino

#include <FS.h>
#include <esp_camera.h>
#include <stdint.h>

// #include <SD_MMC.h>

// Define Pin numbers used by the camera.
#define FLASH_GPIO_NUM 4
#define HREF_GPIO_NUM 23
#define PCLK_GPIO_NUM 22
#define PWDN_GPIO_NUM 32
#define RESET_GPIO_NUM -1
#define SIOC_GPIO_NUM 27
#define SIOD_GPIO_NUM 26
#define VSYNC_GPIO_NUM 25
#define XCLK_GPIO_NUM 0
#define Y2_GPIO_NUM 5
#define Y3_GPIO_NUM 18
#define Y4_GPIO_NUM 19
#define Y5_GPIO_NUM 21
#define Y6_GPIO_NUM 36
#define Y7_GPIO_NUM 39
#define Y8_GPIO_NUM 34
#define Y9_GPIO_NUM 35

/** Initialize the camera model. */
void initialize_camera();

/** Dither the image using the selected algorithm. */
void dither_image(camera_fb_t* frame_buffer);

/** Process and send grayscale images back to the Flipper Zero. */
void process_image(camera_fb_t* frame_buffer);

/** Handle the serial input commands coming from the Flipper Zero. */
void process_serial_commands();

/**
 * Save the current picture to the onboard SD card.
 * @todo - Future feature.
 */
// void save_picture_to_sd();

/** Turn the flash off. */
void toggle_flash_off();

/** Turn the flash on. */
void toggle_flash_on();

/** The dithering algorithms available. */
typedef enum {
    FLOYD_STEINBERG,
    JARVIS_JUDICE_NINKE,
    STUCKI,
} DitheringAlgorithm;

/** Flag to enable or disable dithering. */
bool isDitheringEnabled = true;
/** Flag to represent the flash state when saving pictures to the Flipper. */
bool isFlashEnabled = false;
/** Flag to invert pixel colors. */
bool isInvertEnabled = false;
/** Flag to stop or start the stream. */
bool isStreamEnabled = true;
/** Holds the current camera configuration. */
camera_config_t camera_config;
/** Holds the currently selected dithering algorithm. */
DitheringAlgorithm ditherAlgorithm = FLOYD_STEINBERG;

/** Entry point of the program. */
void setup() {
    // Begin serial communication.
    Serial.begin(230400); // 115200

    // Initialize the camera.
    initialize_camera();
}

/** Main loop of the program. */
void loop() {
    if (isStreamEnabled) {
        camera_fb_t* frame_buffer = esp_camera_fb_get();
        if (frame_buffer) {
            process_image(frame_buffer);
            // Return the frame buffer back to the camera driver.
            esp_camera_fb_return(frame_buffer);
        }
        delay(50);
    }
    process_serial_commands();
}

void initialize_camera() {
    // Set initial configurations.
    camera_config.ledc_channel = LEDC_CHANNEL_0;
    camera_config.ledc_timer = LEDC_TIMER_0;
    camera_config.pin_d0 = Y2_GPIO_NUM;
    camera_config.pin_d1 = Y3_GPIO_NUM;
    camera_config.pin_d2 = Y4_GPIO_NUM;
    camera_config.pin_d3 = Y5_GPIO_NUM;
    camera_config.pin_d4 = Y6_GPIO_NUM;
    camera_config.pin_d5 = Y7_GPIO_NUM;
    camera_config.pin_d6 = Y8_GPIO_NUM;
    camera_config.pin_d7 = Y9_GPIO_NUM;
    camera_config.pin_xclk = XCLK_GPIO_NUM;
    camera_config.pin_pclk = PCLK_GPIO_NUM;
    camera_config.pin_vsync = VSYNC_GPIO_NUM;
    camera_config.pin_href = HREF_GPIO_NUM;
    camera_config.pin_sscb_sda = SIOD_GPIO_NUM;
    camera_config.pin_sscb_scl = SIOC_GPIO_NUM;
    camera_config.pin_pwdn = PWDN_GPIO_NUM;
    camera_config.pin_reset = RESET_GPIO_NUM;
    camera_config.xclk_freq_hz = 20000000;
    camera_config.pixel_format = PIXFORMAT_GRAYSCALE;
    camera_config.frame_size = FRAMESIZE_QQVGA;
    camera_config.fb_count = 1;

    esp_err_t err = esp_camera_init(&camera_config);
    if (err != ESP_OK) {
        return;
    }

    // Check if the flash is already on, if it is turn it off.
    if (isFlashEnabled) {
        toggle_flash_off();
    }

    // Get the camera sensor reference.
    sensor_t* cam = esp_camera_sensor_get();

    cam->set_contrast(cam, 0); // Set initial contrast.
    cam->set_vflip(cam, true); // Set initial rotation (vertical).

    // cam->set_brightness(cam, 0); // Set initial brightness.
    // cam->set_hmirror(cam, true); // Set initial rotation (horizontal).
    // cam->set_saturation(cam, 0); // Set initial saturation.
    // cam->set_sharpness(cam, 0);  // Set initial sharpness.
}

void dither_image(camera_fb_t* frame_buffer) {
    for (uint8_t y = 0; y < frame_buffer->height; ++y) {
        for (uint8_t x = 0; x < frame_buffer->width; ++x) {
            size_t current = (y * frame_buffer->width) + x;
            uint8_t oldpixel = frame_buffer->buf[current];
            uint8_t newpixel = oldpixel >= 128 ? 255 : 0;
            frame_buffer->buf[current] = newpixel;
            int8_t quant_error = oldpixel - newpixel;

            // Apply error diffusion based on the selected algorithm.
            switch (ditherAlgorithm) {
            case JARVIS_JUDICE_NINKE:
                frame_buffer->buf[(y * frame_buffer->width) + x + 1] +=
                    quant_error * 7 / 48;
                frame_buffer->buf[(y * frame_buffer->width) + x + 2] +=
                    quant_error * 5 / 48;
                frame_buffer->buf[(y + 1) * frame_buffer->width + x - 2] +=
                    quant_error * 3 / 48;
                frame_buffer->buf[(y + 1) * frame_buffer->width + x - 1] +=
                    quant_error * 5 / 48;
                frame_buffer->buf[(y + 1) * frame_buffer->width + x] +=
                    quant_error * 7 / 48;
                frame_buffer->buf[(y + 1) * frame_buffer->width + x + 1] +=
                    quant_error * 5 / 48;
                frame_buffer->buf[(y + 1) * frame_buffer->width + x + 2] +=
                    quant_error * 3 / 48;
                frame_buffer->buf[(y + 2) * frame_buffer->width + x - 2] +=
                    quant_error * 1 / 48;
                frame_buffer->buf[(y + 2) * frame_buffer->width + x - 1] +=
                    quant_error * 3 / 48;
                frame_buffer->buf[(y + 2) * frame_buffer->width + x] +=
                    quant_error * 5 / 48;
                frame_buffer->buf[(y + 2) * frame_buffer->width + x + 1] +=
                    quant_error * 3 / 48;
                frame_buffer->buf[(y + 2) * frame_buffer->width + x + 2] +=
                    quant_error * 1 / 48;
                break;
            case STUCKI:
                frame_buffer->buf[(y * frame_buffer->width) + x + 1] +=
                    quant_error * 8 / 42;
                frame_buffer->buf[(y * frame_buffer->width) + x + 2] +=
                    quant_error * 4 / 42;
                frame_buffer->buf[(y + 1) * frame_buffer->width + x - 2] +=
                    quant_error * 2 / 42;
                frame_buffer->buf[(y + 1) * frame_buffer->width + x - 1] +=
                    quant_error * 4 / 42;
                frame_buffer->buf[(y + 1) * frame_buffer->width + x] +=
                    quant_error * 8 / 42;
                frame_buffer->buf[(y + 1) * frame_buffer->width + x + 1] +=
                    quant_error * 4 / 42;
                frame_buffer->buf[(y + 1) * frame_buffer->width + x + 2] +=
                    quant_error * 2 / 42;
                frame_buffer->buf[(y + 2) * frame_buffer->width + x - 2] +=
                    quant_error * 1 / 42;
                frame_buffer->buf[(y + 2) * frame_buffer->width + x - 1] +=
                    quant_error * 2 / 42;
                frame_buffer->buf[(y + 2) * frame_buffer->width + x] +=
                    quant_error * 4 / 42;
                frame_buffer->buf[(y + 2) * frame_buffer->width + x + 1] +=
                    quant_error * 2 / 42;
                frame_buffer->buf[(y + 2) * frame_buffer->width + x + 2] +=
                    quant_error * 1 / 42;
                break;
            case FLOYD_STEINBERG:
            default:
                frame_buffer->buf[(y * frame_buffer->width) + x + 1] +=
                    quant_error * 7 / 16;
                frame_buffer->buf[(y + 1) * frame_buffer->width + x - 1] +=
                    quant_error * 3 / 16;
                frame_buffer->buf[(y + 1) * frame_buffer->width + x] +=
                    quant_error * 5 / 16;
                frame_buffer->buf[(y + 1) * frame_buffer->width + x + 1] +=
                    quant_error * 1 / 16;
                break;
            }
        }
    }
}

void process_image(camera_fb_t* frame_buffer) {
    // If dithering is not disabled, perform dithering on the image. Dithering
    // is the process of approximating the look of a high-resolution grayscale
    // image in a lower resolution by binary values (black & white), thereby
    // representing different shades of gray.
    if (isDitheringEnabled) {
        // Invokes the dithering process on the frame buffer.
        dither_image(frame_buffer);
    }

    uint8_t flipper_y = 0;

    // Iterating over specific rows of the frame buffer.
    for (uint8_t y = 28; y < 92; ++y) {
        Serial.print("Y:");      // Print "Y:" for every new row.
        Serial.write(flipper_y); // Send the row identifier as a byte.

        // Calculate the actual y index in the frame buffer 1D array by
        // multiplying the y value with the width of the frame buffer. This
        // gives the starting index of the row in the 1D array.
        size_t true_y = y * frame_buffer->width;

        // Iterating over specific columns of each row in the frame buffer.
        for (uint8_t x = 16; x < 144;
             x += 8) { // step by 8 as we're packing 8 pixels per byte.
            uint8_t packed_pixels = 0;
            // Packing 8 pixel values into one byte.
            for (uint8_t bit = 0; bit < 8; ++bit) {
                // Check the invert flag and pack the pixels accordingly.
                if (isInvertEnabled) {
                    // If invert is true, consider pixel as 1 if it's more than
                    // 127.
                    if (frame_buffer->buf[true_y + x + bit] > 127) {
                        packed_pixels |= (1 << (7 - bit));
                    }
                } else {
                    // If invert is false, consider pixel as 1 if it's less than
                    // 127.
                    if (frame_buffer->buf[true_y + x + bit] < 127) {
                        packed_pixels |= (1 << (7 - bit));
                    }
                }
            }
            Serial.write(packed_pixels); // Sending packed pixel byte.
        }
        // Move to the next row.
        ++flipper_y;
        // Ensure all data in the Serial buffer is sent before moving to the
        // next iteration.
        Serial.flush();
    }
}

void process_serial_commands() {
    if (Serial.available() > 0) {
        char input = Serial.read();
        sensor_t* cam = esp_camera_sensor_get();

        switch (input) {
        case '>': // Toggle dithering.
            isDitheringEnabled = !isDitheringEnabled;
            break;
        case '<': // Toggle invert.
            isInvertEnabled = !isInvertEnabled;
            break;
        case 'b': // Remove brightness.
            cam->set_contrast(cam, cam->status.brightness - 1);
            break;
        case 'B': // Add brightness.
            cam->set_contrast(cam, cam->status.brightness + 1);
            break;
        case 'c': // Remove contrast.
            cam->set_contrast(cam, cam->status.contrast - 1);
            break;
        case 'C': // Add contrast.
            cam->set_contrast(cam, cam->status.contrast + 1);
            break;
        case 'f': // Turn the flash off.
            toggle_flash_off();
            break;
        case 'F': // Turn the flash on.
            toggle_flash_on();
            break;
        case 'P': // Save image to the onboard SD card.
            // @todo - Future feature.
            // save_picture_to_sd();
            break;
        case 'M': // Toggle Mirror.
            cam->set_hmirror(cam, !cam->status.hmirror);
            break;
        case 's': // Stop stream.
            isStreamEnabled = false;
            break;
        case 'S': // Start stream.
            isStreamEnabled = true;
            break;
        case '0': // Use Floyd Steinberg dithering.
            ditherAlgorithm = FLOYD_STEINBERG;
            break;
        case '1': // Use Jarvis Judice dithering.
            ditherAlgorithm = JARVIS_JUDICE_NINKE;
            break;
        case '2': // Use Stucki dithering.
            ditherAlgorithm = STUCKI;
            break;
        default:
            // Do nothing.
            break;
        }
    }
}

/**
void save_picture_to_sd() {
    sensor_t* cam = esp_camera_sensor_get();

    // Check if the sensor is valid.
    if (!cam) {
        Serial.println("Failed to acquire camera sensor");
        return;
    }

    // Set pixel format to JPEG for saving picture.
    cam->set_pixformat(cam, PIXFORMAT_JPEG);

    // Set frame size based on available PSRAM.
    if (psramFound()) {
        cam->set_framesize(cam, FRAMESIZE_UXGA);
    } else {
        cam->set_framesize(cam, FRAMESIZE_SVGA);
    }

    // Get a frame buffer from camera.
    camera_fb_t* frame_buffer = esp_camera_fb_get();
    if (!frame_buffer) {
        // Camera capture failed
        return;
    }

    if (!SD_MMC.begin()) {
        // SD Card Mount Failed.
        esp_camera_fb_return(frame_buffer);
        return;
    }

    // Generate a unique filename.
    String path = "/picture";
    path += String(millis());
    path += ".jpg";

    fs::FS& fs = SD_MMC;
    File file = fs.open(path.c_str(), FILE_WRITE);

    if (!file) {
        // Failed to open file in writing mode
    } else {
        if (file.write(frame_buffer->buf, frame_buffer->len) !=
            frame_buffer->len) {
            // Failed to write the image to the file
        }
        file.close(); // Close the file in any case.
    }

    // Update framesize back to the default.
    cam->set_framesize(cam, FRAMESIZE_QQVGA);

    // Return the frame buffer back to the camera driver.
    esp_camera_fb_return(frame_buffer);
}
*/

void toggle_flash_off() {
    pinMode(FLASH_GPIO_NUM, OUTPUT);
    digitalWrite(FLASH_GPIO_NUM, LOW);
    isFlashEnabled = false;
}

void toggle_flash_on() {
    pinMode(FLASH_GPIO_NUM, OUTPUT);
    digitalWrite(FLASH_GPIO_NUM, HIGH);
    isFlashEnabled = true;
}