update
This commit is contained in:
92
base.c
92
base.c
@@ -5,54 +5,68 @@
|
|||||||
#include <string.h>
|
#include <string.h>
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include <stdint-gcc.h>
|
#include <stdint-gcc.h>
|
||||||
|
#include <malloc.h>
|
||||||
#include "base.h"
|
#include "base.h"
|
||||||
|
|
||||||
// Base64 decoding table
|
static char encoding_table[] = {'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H',
|
||||||
const unsigned char dtable[256] = {
|
'I', 'J', 'K', 'L', 'M', 'N', 'O', 'P',
|
||||||
['A'] = 0, ['B'] = 1, ['C'] = 2, ['D'] = 3,
|
'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X',
|
||||||
['E'] = 4, ['F'] = 5, ['G'] = 6, ['H'] = 7,
|
'Y', 'Z', 'a', 'b', 'c', 'd', 'e', 'f',
|
||||||
['I'] = 8, ['J'] = 9, ['K'] = 10, ['L'] = 11,
|
'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n',
|
||||||
['M'] = 12, ['N'] = 13, ['O'] = 14, ['P'] = 15,
|
'o', 'p', 'q', 'r', 's', 't', 'u', 'v',
|
||||||
['Q'] = 16, ['R'] = 17, ['S'] = 18, ['T'] = 19,
|
'w', 'x', 'y', 'z', '0', '1', '2', '3',
|
||||||
['U'] = 20, ['V'] = 21, ['W'] = 22, ['X'] = 23,
|
'4', '5', '6', '7', '8', '9', '+', '/'};
|
||||||
['Y'] = 24, ['Z'] = 25,
|
static char *decoding_table = NULL;
|
||||||
['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,
|
|
||||||
};
|
|
||||||
|
|
||||||
|
void build_decoding_table() {
|
||||||
|
|
||||||
int base64_decode(const char *in, unsigned char *out, size_t *out_len) {
|
decoding_table = malloc(256);
|
||||||
size_t len = strlen(in);
|
|
||||||
size_t i, j;
|
|
||||||
unsigned char a, b, c, d;
|
|
||||||
|
|
||||||
if (len % 4 != 0) return -1;
|
for (int i = 0; i < 64; i++)
|
||||||
|
decoding_table[(unsigned char) encoding_table[i]] = i;
|
||||||
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;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
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;
|
||||||
}
|
}
|
||||||
*out_len = j;
|
|
||||||
return 0;
|
*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) {
|
void hex_dump(const unsigned char *data, size_t len) {
|
||||||
for (size_t i = 0; i < len; i++) {
|
for (size_t i = 0; i < len; i++) {
|
||||||
printf("%02X ", data[i]);
|
printf("%02X ", data[i]);
|
||||||
|
2
base.h
2
base.h
@@ -8,7 +8,7 @@
|
|||||||
#include <stddef.h>
|
#include <stddef.h>
|
||||||
|
|
||||||
void hex_dump(const unsigned char *data, size_t len);
|
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];
|
extern const unsigned char dtable[256];
|
||||||
void init_crc32_table(void);
|
void init_crc32_table(void);
|
||||||
uint32_t crc32_le(uint32_t crc, const uint8_t *buf, size_t len);
|
uint32_t crc32_le(uint32_t crc, const uint8_t *buf, size_t len);
|
||||||
|
377
main.c
377
main.c
@@ -7,9 +7,63 @@
|
|||||||
#include <stdint-gcc.h>
|
#include <stdint-gcc.h>
|
||||||
#include <ncurses.h>
|
#include <ncurses.h>
|
||||||
#include <time.h>
|
#include <time.h>
|
||||||
|
#include <assert.h>
|
||||||
#include "packets.h"
|
#include "packets.h"
|
||||||
#include "base.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)
|
#define MAG_SCALE (4912.0f / 32760.0f)
|
||||||
|
|
||||||
void printTelemetryPacketNcurses(TelemetryPacket *packet, DownBoundPacket *down) {
|
void printTelemetryPacketNcurses(TelemetryPacket *packet, DownBoundPacket *down) {
|
||||||
@@ -19,7 +73,7 @@ void printTelemetryPacketNcurses(TelemetryPacket *packet, DownBoundPacket *down)
|
|||||||
|
|
||||||
// Time
|
// Time
|
||||||
attroff(COLOR_PAIR(6));
|
attroff(COLOR_PAIR(6));
|
||||||
mvprintw(row++, 0, "Timestamp: %u", down->missionTimer);
|
mvprintw(row++, 0, "Timestamp: %lu", down->missionTimer);
|
||||||
|
|
||||||
// MPU9250
|
// MPU9250
|
||||||
if (packet->presentDevices & MPU9250_PRESENT_BIT) {
|
if (packet->presentDevices & MPU9250_PRESENT_BIT) {
|
||||||
@@ -150,10 +204,196 @@ void printTelemetryPacketNcurses(TelemetryPacket *packet, DownBoundPacket *down)
|
|||||||
refresh();
|
refresh();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
uint16_t x = 0;
|
||||||
|
|
||||||
TelemetryPacket telemetryPacket;
|
TelemetryPacket telemetryPacket;
|
||||||
DownBoundPacket down;
|
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) {
|
void handle_downlink_packet(uint8_t *buf, uint8_t rxLen) {
|
||||||
if (rxLen < sizeof(DownBoundPacket)) {
|
if (rxLen < sizeof(DownBoundPacket)) {
|
||||||
printf("Received packet too small to be valid.");
|
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!");
|
printf("Telemetry packet received!");
|
||||||
|
|
||||||
printTelemetryPacketNcurses(&telemetryPacket, &down);
|
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 {
|
} else {
|
||||||
printf("Telemetry packet too small (%u bytes)", payloadSize);
|
printf("Telemetry packet too small (%u bytes)", payloadSize);
|
||||||
}
|
}
|
||||||
@@ -221,8 +522,8 @@ int open_serial(const char *device) {
|
|||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
cfsetospeed(&tty, B115200);
|
cfsetospeed(&tty, B921600);
|
||||||
cfsetispeed(&tty, B115200);
|
cfsetispeed(&tty, B921600);
|
||||||
|
|
||||||
tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8-bit chars
|
tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8-bit chars
|
||||||
tty.c_iflag &= ~IGNBRK; // disable break processing
|
tty.c_iflag &= ~IGNBRK; // disable break processing
|
||||||
@@ -249,32 +550,32 @@ int open_serial(const char *device) {
|
|||||||
|
|
||||||
int subMain() {
|
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;
|
if (fd < 0) return 1;
|
||||||
|
|
||||||
char buf[4096];
|
char buf[8192];
|
||||||
size_t buf_pos = 0;
|
size_t buf_pos = 0;
|
||||||
|
|
||||||
memset(&telemetryPacket, 0, sizeof(TelemetryPacket));
|
memset(&telemetryPacket, 0, sizeof(TelemetryPacket));
|
||||||
memset(&down, 0, sizeof(DownBoundPacket));
|
memset(&down, 0, sizeof(DownBoundPacket));
|
||||||
|
|
||||||
unsigned char decoded[256];
|
unsigned char *decoded;
|
||||||
size_t decoded_len;
|
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);
|
printTelemetryPacketNcurses(&telemetryPacket, &down);
|
||||||
while (1) {
|
while (1) {
|
||||||
char ch;
|
char ch;
|
||||||
size_t n = read(fd, &ch, 1);
|
size_t n = read(fd, &ch, 1);
|
||||||
if (n > 0) {
|
if (n > 0) {
|
||||||
|
//printf("%c", ch);
|
||||||
if (ch == '\n') {
|
if (ch == '\n') {
|
||||||
buf[buf_pos] = '\0';
|
buf[buf_pos] = '\0';
|
||||||
printf("%s\n", buf);
|
|
||||||
|
|
||||||
if (strncmp(buf, "Got DownLink data", 17) == 0) {
|
if (strncmp(buf, "Got DownLink data", 17) == 0) {
|
||||||
char *colon = strchr(buf, ':');
|
char *colon = strchr(buf, ':');
|
||||||
@@ -283,12 +584,38 @@ int subMain() {
|
|||||||
while (*colon == ' ') colon++; // skip spaces
|
while (*colon == ' ') colon++; // skip spaces
|
||||||
printf("Detected base64 payload: %s\n", colon);
|
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);
|
printf("Decoded data (%zu bytes):\n", decoded_len);
|
||||||
handle_downlink_packet(decoded, decoded_len);
|
handle_downlink_packet(decoded, decoded_len);
|
||||||
} else {
|
} else {
|
||||||
printf("Base64 decode failed\n");
|
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;
|
buf_pos = 0;
|
||||||
@@ -310,16 +637,16 @@ int subMain() {
|
|||||||
int main(int argc, char *argv[]) {
|
int main(int argc, char *argv[]) {
|
||||||
|
|
||||||
init_crc32_table();
|
init_crc32_table();
|
||||||
initscr(); // Start ncurses mode
|
// initscr(); // Start ncurses mode
|
||||||
start_color(); // Enable color
|
// start_color(); // Enable color
|
||||||
use_default_colors();
|
// use_default_colors();
|
||||||
init_pair(1, COLOR_CYAN, -1);
|
// init_pair(1, COLOR_CYAN, -1);
|
||||||
init_pair(2, COLOR_GREEN, -1);
|
// init_pair(2, COLOR_GREEN, -1);
|
||||||
init_pair(3, COLOR_YELLOW, -1);
|
// init_pair(3, COLOR_YELLOW, -1);
|
||||||
init_pair(4, COLOR_RED, -1);
|
// init_pair(4, COLOR_RED, -1);
|
||||||
init_pair(5, COLOR_CYAN, -1);
|
// init_pair(5, COLOR_CYAN, -1);
|
||||||
init_pair(6, COLOR_MAGENTA, -1);
|
// init_pair(6, COLOR_MAGENTA, -1);
|
||||||
curs_set(0); // Hide cursor
|
// curs_set(0); // Hide cursor
|
||||||
int x = subMain();
|
int x = subMain();
|
||||||
endwin(); // End ncurses mode
|
endwin(); // End ncurses mode
|
||||||
return x;
|
return x;
|
||||||
|
@@ -24,7 +24,7 @@ typedef struct __attribute__((packed))
|
|||||||
char syncPhrase[10];
|
char syncPhrase[10];
|
||||||
uint32_t packetIndex;
|
uint32_t packetIndex;
|
||||||
uint8_t packetType;
|
uint8_t packetType;
|
||||||
uint32_t missionTimer;
|
uint64_t missionTimer;
|
||||||
uint32_t CRCCheck;
|
uint32_t CRCCheck;
|
||||||
} DownBoundPacket;
|
} DownBoundPacket;
|
||||||
|
|
||||||
|
Reference in New Issue
Block a user