diff --git a/base.c b/base.c index 6edabad..463c052 100644 --- a/base.c +++ b/base.c @@ -5,54 +5,68 @@ #include #include #include +#include #include "base.h" -// Base64 decoding table -const unsigned char dtable[256] = { - ['A'] = 0, ['B'] = 1, ['C'] = 2, ['D'] = 3, - ['E'] = 4, ['F'] = 5, ['G'] = 6, ['H'] = 7, - ['I'] = 8, ['J'] = 9, ['K'] = 10, ['L'] = 11, - ['M'] = 12, ['N'] = 13, ['O'] = 14, ['P'] = 15, - ['Q'] = 16, ['R'] = 17, ['S'] = 18, ['T'] = 19, - ['U'] = 20, ['V'] = 21, ['W'] = 22, ['X'] = 23, - ['Y'] = 24, ['Z'] = 25, - ['a'] = 26, ['b'] = 27, ['c'] = 28, ['d'] = 29, - ['e'] = 30, ['f'] = 31, ['g'] = 32, ['h'] = 33, - ['i'] = 34, ['j'] = 35, ['k'] = 36, ['l'] = 37, - ['m'] = 38, ['n'] = 39, ['o'] = 40, ['p'] = 41, - ['q'] = 42, ['r'] = 43, ['s'] = 44, ['t'] = 45, - ['u'] = 46, ['v'] = 47, ['w'] = 48, ['x'] = 49, - ['y'] = 50, ['z'] = 51, - ['0'] = 52, ['1'] = 53, ['2'] = 54, ['3'] = 55, - ['4'] = 56, ['5'] = 57, ['6'] = 58, ['7'] = 59, - ['8'] = 60, ['9'] = 61, ['+'] = 62, ['/'] = 63, -}; +static char encoding_table[] = {'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', + 'I', 'J', 'K', 'L', 'M', 'N', 'O', 'P', + 'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X', + 'Y', 'Z', 'a', 'b', 'c', 'd', 'e', 'f', + 'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', + 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', + 'w', 'x', 'y', 'z', '0', '1', '2', '3', + '4', '5', '6', '7', '8', '9', '+', '/'}; +static char *decoding_table = NULL; +void build_decoding_table() { -int base64_decode(const char *in, unsigned char *out, size_t *out_len) { - size_t len = strlen(in); - size_t i, j; - unsigned char a, b, c, d; + decoding_table = malloc(256); - if (len % 4 != 0) return -1; - - for (i = 0, j = 0; i < len; i += 4) { - a = dtable[(unsigned char)in[i]]; - b = dtable[(unsigned char)in[i+1]]; - c = dtable[(unsigned char)in[i+2]]; - d = dtable[(unsigned char)in[i+3]]; - - out[j++] = (a << 2) | (b >> 4); - if (in[i+2] != '=') { - out[j++] = (b << 4) | (c >> 2); - if (in[i+3] != '=') - out[j++] = (c << 6) | d; - } - } - *out_len = j; - return 0; + for (int i = 0; i < 64; i++) + decoding_table[(unsigned char) encoding_table[i]] = i; } + +unsigned char *base64_decode(const char *data, size_t input_length, size_t *output_length) { + + if (decoding_table == NULL) build_decoding_table(); + + if (input_length % 4 != 0) { + printf("babbaba %zu\n", input_length); + return NULL; + } + + *output_length = input_length / 4 * 3; + if (data[input_length - 1] == '=') (*output_length)--; + if (data[input_length - 2] == '=') (*output_length)--; + + unsigned char *decoded_data = malloc(*output_length); + if (decoded_data == NULL) { + printf("fefefefeg\n"); + return NULL; + } + + for (int i = 0, j = 0; i < input_length;) { + + uint32_t sextet_a = data[i] == '=' ? 0 & i++ : decoding_table[data[i++]]; + uint32_t sextet_b = data[i] == '=' ? 0 & i++ : decoding_table[data[i++]]; + uint32_t sextet_c = data[i] == '=' ? 0 & i++ : decoding_table[data[i++]]; + uint32_t sextet_d = data[i] == '=' ? 0 & i++ : decoding_table[data[i++]]; + + uint32_t triple = (sextet_a << 3 * 6) + + (sextet_b << 2 * 6) + + (sextet_c << 1 * 6) + + (sextet_d << 0 * 6); + + if (j < *output_length) decoded_data[j++] = (triple >> 2 * 8) & 0xFF; + if (j < *output_length) decoded_data[j++] = (triple >> 1 * 8) & 0xFF; + if (j < *output_length) decoded_data[j++] = (triple >> 0 * 8) & 0xFF; + } + + return decoded_data; +} + + void hex_dump(const unsigned char *data, size_t len) { for (size_t i = 0; i < len; i++) { printf("%02X ", data[i]); diff --git a/base.h b/base.h index f231f99..72a7749 100644 --- a/base.h +++ b/base.h @@ -8,7 +8,7 @@ #include void hex_dump(const unsigned char *data, size_t len); -int base64_decode(const char *in, unsigned char *out, size_t *out_len); +unsigned char *base64_decode(const char *data, size_t input_length, size_t *output_length); extern const unsigned char dtable[256]; void init_crc32_table(void); uint32_t crc32_le(uint32_t crc, const uint8_t *buf, size_t len); diff --git a/main.c b/main.c index a04c26d..9946fe3 100644 --- a/main.c +++ b/main.c @@ -7,9 +7,63 @@ #include #include #include +#include #include "packets.h" #include "base.h" +FILE *csvFile; + +void write_grayscale_bmp(FILE *f, const uint8_t *pixels, int width, int height) { + if (!f || !pixels || width <= 0 || height <= 0) return; + + int rowSize = (width + 3) & ~3; // Align to 4 bytes + int pixelDataSize = rowSize * height; + int paletteSize = 256 * 4; // 256 entries, 4 bytes each (BGRA) + int offset = 14 + 40 + paletteSize; // BMP header + DIB header + palette + int fileSize = offset + pixelDataSize; + + // --- BMP Header (14 bytes) --- + uint8_t bmpHeader[14] = { + 'B', 'M', + fileSize & 0xFF, (fileSize >> 8) & 0xFF, (fileSize >> 16) & 0xFF, (fileSize >> 24) & 0xFF, + 0, 0, 0, 0, + offset & 0xFF, (offset >> 8) & 0xFF, (offset >> 16) & 0xFF, (offset >> 24) & 0xFF + }; + fwrite(bmpHeader, 1, 14, f); + + // --- DIB Header (40 bytes - BITMAPINFOHEADER) --- + uint8_t dibHeader[40] = { + 40, 0, 0, 0, // Header size + width & 0xFF, (width >> 8) & 0xFF, (width >> 16) & 0xFF, (width >> 24) & 0xFF, + height & 0xFF, (height >> 8) & 0xFF, (height >> 16) & 0xFF, (height >> 24) & 0xFF, + 1, 0, // Planes + 8, 0, // Bits per pixel (8-bit indexed) + 0, 0, 0, 0, // Compression (0 = BI_RGB) + pixelDataSize & 0xFF, (pixelDataSize >> 8) & 0xFF, (pixelDataSize >> 16) & 0xFF, + (pixelDataSize >> 24) & 0xFF, + 0x13, 0x0B, 0, 0, // X pixels per meter (2835 = 72 DPI) + 0x13, 0x0B, 0, 0, // Y pixels per meter + 256, 0, 0, 0, // Number of colors in palette + 0, 0, 0, 0 // Important colors + }; + fwrite(dibHeader, 1, 40, f); + + // --- Grayscale Palette (BGRA format) --- + for (int i = 0; i < 256; i++) { + uint8_t entry[4] = {i, i, i, 0}; // Blue, Green, Red, Reserved + fwrite(entry, 1, 4, f); + } + + // --- Pixel Data (bottom-up, padded rows) --- + uint8_t padding[3] = {0}; // Max padding = 3 bytes + for (int y = height - 1; y >= 0; y--) { + const uint8_t *row = &pixels[y * width]; + fwrite(row, 1, width, f); + fwrite(padding, 1, rowSize - width, f); + } +} + + #define MAG_SCALE (4912.0f / 32760.0f) void printTelemetryPacketNcurses(TelemetryPacket *packet, DownBoundPacket *down) { @@ -19,7 +73,7 @@ void printTelemetryPacketNcurses(TelemetryPacket *packet, DownBoundPacket *down) // Time attroff(COLOR_PAIR(6)); - mvprintw(row++, 0, "Timestamp: %u", down->missionTimer); + mvprintw(row++, 0, "Timestamp: %lu", down->missionTimer); // MPU9250 if (packet->presentDevices & MPU9250_PRESENT_BIT) { @@ -150,10 +204,196 @@ void printTelemetryPacketNcurses(TelemetryPacket *packet, DownBoundPacket *down) refresh(); } +uint16_t x = 0; TelemetryPacket telemetryPacket; DownBoundPacket down; +typedef enum { + PIXFORMAT_RGB565, // 2BPP/RGB565 + PIXFORMAT_YUV422, // 2BPP/YUV422 + PIXFORMAT_YUV420, // 1.5BPP/YUV420 + PIXFORMAT_GRAYSCALE, // 1BPP/GRAYSCALE + PIXFORMAT_JPEG, // JPEG/COMPRESSED + PIXFORMAT_RGB888, // 3BPP/RGB888 + PIXFORMAT_RAW, // RAW + PIXFORMAT_RGB444, // 3BP2P/RGB444 + PIXFORMAT_RGB555, // 3BP2P/RGB555 +} pixformat_t; + +typedef struct { + bool isVisible; + uint32_t len; + uint16_t width; + uint16_t height; + pixformat_t format; + int64_t timeStamp; +} PreData; + + +PreData preData; + +uint16_t imageIndex = 0; + +FILE *imageFile = NULL; +FILE *imageFileMetadata = NULL; + +uint8_t *fileBuffer = NULL; + +void handle_frame(uint8_t *buf, size_t rxLen) { + char fname[40]; + snprintf(fname, sizeof(fname), "rawdatn0%ldpckt%d.bin", time(NULL), x++); + FILE *file = fopen(fname, "wb"); + if (file) { + fwrite(buf + 71, rxLen - 73, 1, file); + fclose(file); + file = NULL; + } else { + fprintf(stderr, "Failed to open file %s\n", fname); + return; + } + + size_t currentByteIndex = 72; + + if (rxLen < currentByteIndex + 1 + 2 + 2 + 6) { + fprintf(stderr, "Frame too short (header)\n"); + return; + } + + uint8_t type = buf[currentByteIndex++]; + uint16_t seq = (buf[currentByteIndex++] << 8) | buf[currentByteIndex++]; + uint16_t total_chunks = (buf[currentByteIndex++] << 8) | buf[currentByteIndex++]; + + + if (!(buf[currentByteIndex++] == 'P' && + buf[currentByteIndex++] == 'l' && + buf[currentByteIndex++] == 'e' && + buf[currentByteIndex++] == 'c' && + buf[currentByteIndex++] == 'y' && + buf[currentByteIndex++] == 'C')) { + fprintf(stderr, "Magic string 'PlecyC' not found\n"); + return; + } + + if (seq == 0) { + preData.isVisible = buf[currentByteIndex++]; + preData.len = buf[currentByteIndex++] << 24; + preData.len |= buf[currentByteIndex++] << 16; + preData.len |= buf[currentByteIndex++] << 8; + preData.len |= buf[currentByteIndex++]; + preData.width = buf[currentByteIndex++] << 8; + preData.width |= buf[currentByteIndex++]; + preData.height = buf[currentByteIndex++] << 8; + preData.height |= buf[currentByteIndex++]; + preData.format = buf[currentByteIndex++]; + preData.timeStamp = ((int64_t) buf[currentByteIndex++]) << 56; + preData.timeStamp |= ((int64_t) buf[currentByteIndex++]) << 48; + preData.timeStamp |= ((int64_t) buf[currentByteIndex++]) << 40; + preData.timeStamp |= ((int64_t) buf[currentByteIndex++]) << 32; + preData.timeStamp |= buf[currentByteIndex++] << 24; + preData.timeStamp |= buf[currentByteIndex++] << 16; + preData.timeStamp |= buf[currentByteIndex++] << 8; + preData.timeStamp |= buf[currentByteIndex++]; + + if (imageFileMetadata != NULL) { + fclose(imageFileMetadata); + imageFileMetadata = NULL; + } + if (imageFile != NULL && fileBuffer != NULL) { + if (preData.isVisible) { + fwrite(fileBuffer, 1, preData.len, imageFile); + } else { + write_grayscale_bmp(imageFile, fileBuffer, preData.width, preData.height); + } + fclose(imageFile); + imageFile = NULL; + } + + if (fileBuffer != NULL) { + free(fileBuffer); + fileBuffer = NULL; + } + fileBuffer = malloc(preData.len); + + + char fnameImg[40]; + char fnameMeta[40]; + snprintf(fnameMeta, sizeof(fnameMeta), "imgMeta%sn%ldp%d.txt", preData.isVisible ? "OV" : "Lep", time(NULL), + x++); + snprintf(fnameImg, sizeof(fnameImg), "img%sn%ldp%d.%s", preData.isVisible ? "OV" : "Lep", time(NULL), x++, + preData.isVisible ? "jpg" : "bmp"); + imageFile = fopen(fnameImg, "wb"); + imageFileMetadata = fopen(fnameMeta, "wb"); + if (rxLen < currentByteIndex) { + fprintf(stderr, "Frame too short\n"); + return; + } + + fprintf(imageFileMetadata, " Camera: %s\n", preData.isVisible ? "VIS" : "INVIS"); + fprintf(imageFileMetadata, " width: %hu\n", preData.width); + fprintf(imageFileMetadata, " height: %hu\n", preData.height); + fprintf(imageFileMetadata, " len: %u\n", preData.len); + fprintf(imageFileMetadata, " format: %u\n", preData.format); + } + + if (fileBuffer && imageFileMetadata && imageFile) { + fprintf(imageFileMetadata, "Type: %u\n", type); + + fprintf(imageFileMetadata, "Sequence number: %u\n", seq); + fprintf(imageFileMetadata, "Total chunks: %u\n", total_chunks); + fprintf(imageFileMetadata, "PreData:\n"); + //fseek(imageFile, (seq * 1459) - 18, SEEK_SET); + size_t offset = (seq * 1458) - 18; + if (seq == 0) { + offset = 0; + } + // Debug: print key values + printf("[DEBUG] offset: %zu, currentByteIndex: %d, rxLen: %d, seq: %d, total_chunks: %d\n", + offset, currentByteIndex, rxLen, seq, total_chunks); + +// Calculate length safely + size_t length = rxLen - currentByteIndex - ((seq == total_chunks - 1) ? 1 : 2); + + + +// Sanity checks + + if (offset + length > preData.len) { + length = preData.len - offset; + } + fprintf(imageFileMetadata, "RxLen: %zu, length: %zu\n", rxLen, length); + + assert(fileBuffer != NULL); + assert(buf != NULL); + assert(length >= 0); + assert(offset + length <= preData.len); // assuming fileBufferSize is known + assert(currentByteIndex >= 0 && currentByteIndex < rxLen); + assert(seq >= 0 && seq < total_chunks); + +// Do the copy + memcpy(fileBuffer + offset, buf + currentByteIndex, length); + + buf[rxLen - 2] = buf[rxLen - 1]; + //fwrite(buf + currentByteIndex, 1, rxLen - currentByteIndex - (seq == total_chunks - 1 ? 1 : 2), imageFile); + } + if (seq == total_chunks - 1) { + if (imageFileMetadata != NULL) { + fclose(imageFileMetadata); + imageFileMetadata = NULL; + } + if (imageFile != NULL && fileBuffer != NULL) { + if (preData.isVisible) { + fwrite(fileBuffer, 1, preData.len, imageFile); + } else { + write_grayscale_bmp(imageFile, fileBuffer, preData.width, preData.height); + } + fclose(imageFile); + imageFile = NULL; + } + } +} + + void handle_downlink_packet(uint8_t *buf, uint8_t rxLen) { if (rxLen < sizeof(DownBoundPacket)) { printf("Received packet too small to be valid."); @@ -187,7 +427,68 @@ void handle_downlink_packet(uint8_t *buf, uint8_t rxLen) { printf("Telemetry packet received!"); printTelemetryPacketNcurses(&telemetryPacket, &down); - // Here you would actually do something with the telemetry, like save it or display it + fprintf(csvFile, + "%lu,%u,%u,%u," // missionTimer, packetIndex, telemetryIndex, packetType + "%d,%d,%d,%d,%d,%d,%d,%d,%d,%d," // MPU + "%u,%u,%d,%u," // CCS + "%u,%u,%u,%u,%u,%u,%u," // INA, BME basic + "%s,%s,%u,%u," // bools and gas index/range + "%.2f,%.2f,%.2f,%.2f," // Bosch env data + "%u,%.2f,%.2f,%.2f," // IAQ + scores + "%u,%d,%d,%d,%u,%u,%u,%u," // GPS data + "%d,%d,%d," // Predicted GPS + "%d,%d,%d,%d," // ADC + "%d,%d,%d,%d," // Servo + "%u\n", // presentDevices + + down.missionTimer, + down.packetIndex, + telemetryPacket.telemetryIndex, + down.packetType, + + telemetryPacket.accelerationX, telemetryPacket.accelerationY, telemetryPacket.accelerationZ, + telemetryPacket.gyroX, telemetryPacket.gyroY, telemetryPacket.gyroZ, + telemetryPacket.magnetX, telemetryPacket.magnetY, telemetryPacket.magnetZ, + telemetryPacket.accelerometer_temperature, + + telemetryPacket.eCO2, telemetryPacket.tvoc, telemetryPacket.currentCCS, + telemetryPacket.rawCCSData, + + telemetryPacket.volts, telemetryPacket.current, telemetryPacket.power, + telemetryPacket.temperature, telemetryPacket.humidity, telemetryPacket.pressure, + telemetryPacket.gas, + + telemetryPacket.gas_valid ? "Yes" : "No", + telemetryPacket.heater_stable ? "Yes" : "No", + telemetryPacket.gas_range, telemetryPacket.gas_index, + + telemetryPacket.air_temperature, telemetryPacket.relative_humidity, + telemetryPacket.barometric_pressure, telemetryPacket.gas_resistance, + + telemetryPacket.iaq_score, + telemetryPacket.temperature_score, telemetryPacket.humidity_score, telemetryPacket.gas_score, + + telemetryPacket.time_seconds, + telemetryPacket.latitude_centi_degrees, + telemetryPacket.longitude_centi_degrees, + telemetryPacket.altitude_centi_meters, + telemetryPacket.fix_quality, + telemetryPacket.num_satellites, + telemetryPacket.date_yyddmm, + telemetryPacket.speed_centi_knots, + + telemetryPacket.predicted_latitude_centi_degrees, + telemetryPacket.predicted_longitude_centi_degrees, + telemetryPacket.predicted_altitude_centi_meters, + + telemetryPacket.NH3, telemetryPacket.CO, telemetryPacket.NO2, telemetryPacket.UVC, + + telemetryPacket.currentServoA, telemetryPacket.targetServoA, + telemetryPacket.currentServoB, telemetryPacket.targetServoB, + + telemetryPacket.presentDevices); + fflush(csvFile); + fsync(fileno(csvFile)); // Critical: this ensures actual write to disk } else { printf("Telemetry packet too small (%u bytes)", payloadSize); } @@ -221,8 +522,8 @@ int open_serial(const char *device) { return -1; } - cfsetospeed(&tty, B115200); - cfsetispeed(&tty, B115200); + cfsetospeed(&tty, B921600); + cfsetispeed(&tty, B921600); tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8-bit chars tty.c_iflag &= ~IGNBRK; // disable break processing @@ -249,32 +550,32 @@ int open_serial(const char *device) { int subMain() { - int fd = open_serial("/dev/ttyUSB0"); + char *csvName[40]; + + snprintf((char *) csvName, sizeof(csvName), "sens%ld", time(NULL)); + + csvFile = fopen((const char *) csvName, "w"); + + int fd = open_serial("/dev/ttyUSB2"); if (fd < 0) return 1; - char buf[4096]; + char buf[8192]; size_t buf_pos = 0; memset(&telemetryPacket, 0, sizeof(TelemetryPacket)); memset(&down, 0, sizeof(DownBoundPacket)); - unsigned char decoded[256]; + unsigned char *decoded; size_t decoded_len; -// if (base64_decode("UGxlY2hEb2xlADsAAAAADuo+Ai9is1eY/vw/dP4AAAAAAAAAAAAAAABgD8sBCAAB/wPGGEYARQCgHQgAoksEKgUAnwIBAQQAe5APQtL3pEGycXlEqDLaSDcAAAAAAJqZeUAAAFBCAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA/zw=", decoded, &decoded_len) == 0) { -// printf("Decoded data (%zu bytes):\n", decoded_len); -// handle_downlink_packet(decoded, decoded_len); -// } else { -// printf("Base64 decode failed\n"); -// } printTelemetryPacketNcurses(&telemetryPacket, &down); while (1) { char ch; size_t n = read(fd, &ch, 1); if (n > 0) { + //printf("%c", ch); if (ch == '\n') { buf[buf_pos] = '\0'; - printf("%s\n", buf); if (strncmp(buf, "Got DownLink data", 17) == 0) { char *colon = strchr(buf, ':'); @@ -283,12 +584,38 @@ int subMain() { while (*colon == ' ') colon++; // skip spaces printf("Detected base64 payload: %s\n", colon); - if (base64_decode(colon, decoded, &decoded_len) == 0) { + decoded = base64_decode(colon, strlen(colon) - 1, &decoded_len); + if (decoded != NULL) { printf("Decoded data (%zu bytes):\n", decoded_len); handle_downlink_packet(decoded, decoded_len); } else { printf("Base64 decode failed\n"); } + if (decoded != NULL) { + free(decoded); + } + } + } else if (strncmp(buf, "Got 2400mhZ frame", 17) == 0) { + char *colon = strchr(buf, ':'); + if (colon) { + colon++; + while (*colon == ' ') colon++; // skip spaces + printf("Detected base64 payload: %s\n", colon); + + if (colon[strlen(colon) - 1] == '\r') { + colon[strlen(colon) - 1] = 0; + } + + decoded = base64_decode(colon, strlen(colon), &decoded_len); + if (decoded != NULL) { + printf("Decoded data (%zu bytes):\n", decoded_len); + handle_frame(decoded, decoded_len); + } else { + printf("Base64 decode failed\n"); + } + if (decoded != NULL) { + free(decoded); + } } } buf_pos = 0; @@ -310,16 +637,16 @@ int subMain() { int main(int argc, char *argv[]) { init_crc32_table(); - initscr(); // Start ncurses mode - start_color(); // Enable color - use_default_colors(); - init_pair(1, COLOR_CYAN, -1); - init_pair(2, COLOR_GREEN, -1); - init_pair(3, COLOR_YELLOW, -1); - init_pair(4, COLOR_RED, -1); - init_pair(5, COLOR_CYAN, -1); - init_pair(6, COLOR_MAGENTA, -1); - curs_set(0); // Hide cursor +// initscr(); // Start ncurses mode +// start_color(); // Enable color +// use_default_colors(); +// init_pair(1, COLOR_CYAN, -1); +// init_pair(2, COLOR_GREEN, -1); +// init_pair(3, COLOR_YELLOW, -1); +// init_pair(4, COLOR_RED, -1); +// init_pair(5, COLOR_CYAN, -1); +// init_pair(6, COLOR_MAGENTA, -1); +// curs_set(0); // Hide cursor int x = subMain(); endwin(); // End ncurses mode return x; diff --git a/packets.h b/packets.h index 1c9b551..cc265c8 100644 --- a/packets.h +++ b/packets.h @@ -24,7 +24,7 @@ typedef struct __attribute__((packed)) char syncPhrase[10]; uint32_t packetIndex; uint8_t packetType; - uint32_t missionTimer; + uint64_t missionTimer; uint32_t CRCCheck; } DownBoundPacket;