diff options
| -rw-r--r-- | src/ufodecode.c | 255 |
1 files changed, 153 insertions, 102 deletions
diff --git a/src/ufodecode.c b/src/ufodecode.c index 68b0da7..cc25a8c 100644 --- a/src/ufodecode.c +++ b/src/ufodecode.c @@ -17,6 +17,13 @@ #define IPECAMERA_PIXELS_PER_CHANNEL 128 /**< Number of pixels per channel */ #define IPECAMERA_WIDTH (IPECAMERA_NUM_CHANNELS * IPECAMERA_PIXELS_PER_CHANNEL) /**< Total pixel width of row */ +#define IPECAMERA_MODE_16_CHAN_IO 0 +#define IPECAMERA_MODE_4_CHAN_IO 2 + +#define IPECAMERA_MODE_12_BIT_ADC 2 +#define IPECAMERA_MODE_11_BIT_ADC 1 +#define IPECAMERA_MODE_10_BIT_ADC 0 + typedef struct { unsigned int pixel_number : 8; unsigned int row_number : 12; @@ -113,10 +120,10 @@ ufo_decoder_set_raw_data(UfoDecoder *decoder, uint32_t *raw, size_t num_bytes) } static int -ufo_decode_frame_channels_v0(UfoDecoder *decoder, - uint16_t *pixel_buffer, - uint32_t *raw, - size_t num_words, +ufo_decode_frame_channels_v0(UfoDecoder *decoder, + uint16_t *pixel_buffer, + uint32_t *raw, + size_t num_words, size_t *offset) { static int channel_order[IPECAMERA_NUM_CHANNELS] = { 15, 13, 14, 12, 10, 8, 11, 7, 9, 6, 5, 2, 4, 3, 0, 1 }; @@ -171,7 +178,7 @@ ufo_decode_frame_channels_v0(UfoDecoder *decoder, /* base++; */ } #ifdef CHECKS - else + else CHECK_FLAG("number of pixels, %i is expected", pixels == IPECAMERA_PIXELS_PER_CHANNEL, pixels, IPECAMERA_PIXELS_PER_CHANNEL); #endif @@ -218,9 +225,9 @@ ufo_decode_frame_channels_v0(UfoDecoder *decoder, for (int i = 1 ; i < bytes; i++) { data = raw[i]; #ifdef DEBUG - header = (data >> 30) & 0x03; + header = (data >> 30) & 0x03; CHECK_FLAG("raw data magic", header == 3, header); - if (err) + if (err) return err; #endif pixel_buffer[base++] = (data >> 20) & 0x3FF; @@ -234,7 +241,7 @@ ufo_decode_frame_channels_v0(UfoDecoder *decoder, header = (data >> 30) & 0x03; CHECK_FLAG("raw data magic", header == 3, header); CHECK_FLAG("raw footer magic", (data & 0x3FF) == 0x55, (data & 0x3FF)); - if (err) + if (err) return err; #endif @@ -252,10 +259,10 @@ ufo_decode_frame_channels_v0(UfoDecoder *decoder, static int ufo_decode_frame_channels_v4(UfoDecoder *decoder, - uint16_t *pixel_buffer, - uint32_t *raw, - size_t num_words, - size_t num_rows, + uint16_t *pixel_buffer, + uint32_t *raw, + size_t num_words, + size_t num_rows, size_t *offset) { static const int channel_order[IPECAMERA_NUM_CHANNELS] = { 15, 13, 14, 12, 10, 8, 11, 7, 9, 6, 5, 2, 4, 3, 0, 1 }; @@ -355,9 +362,9 @@ ufo_decode_frame_channels_v4(UfoDecoder *decoder, for (int i = 1 ; i < bytes; i++) { data = raw[i]; #ifdef DEBUG - header = (data >> 30) & 0x03; + header = (data >> 30) & 0x03; CHECK_FLAG("raw data magic", header == 3, header); - if (err) + if (err) return err; #endif pixel_buffer[base++] = (data >> 20) & 0x3FF; @@ -371,7 +378,7 @@ ufo_decode_frame_channels_v4(UfoDecoder *decoder, header = (data >> 30) & 0x03; CHECK_FLAG("raw data magic", header == 3, header); CHECK_FLAG("raw footer magic", (data & 0x3FF) == 0x55, (data & 0x3FF)); - if (err) + if (err) return err; #endif @@ -388,12 +395,13 @@ ufo_decode_frame_channels_v4(UfoDecoder *decoder, } static int -ufo_decode_frame_channels_v5(UfoDecoder *decoder, - uint16_t *pixel_buffer, - uint32_t *raw, - size_t num_words, - size_t num_rows, - size_t *offset) +ufo_decode_frame_channels_v5(UfoDecoder *decoder, + uint16_t *pixel_buffer, + uint32_t *raw, + size_t num_words, + size_t num_rows, + size_t *offset, + uint8_t output_mode) { payload_header_v5 *header; size_t base = 0, index = 0; @@ -401,7 +409,7 @@ ufo_decode_frame_channels_v5(UfoDecoder *decoder, header = (payload_header_v5 *) &raw[base]; - if (header->pixel_size == 12) { + if (output_mode == IPECAMERA_MODE_4_CHAN_IO) { while (raw[base] != 0xAAAAAAA) { header = (payload_header_v5 *) &raw[base]; index = header->row_number * IPECAMERA_WIDTH + header->pixel_number; @@ -416,7 +424,7 @@ ufo_decode_frame_channels_v5(UfoDecoder *decoder, pixel_buffer[index + (12+off)*IPECAMERA_PIXELS_PER_CHANNEL] = 0xfff & (raw[base+1] >> 16); } else { - off++; + off++; if (header->magic == 0xc0) off = 0; @@ -425,34 +433,66 @@ ufo_decode_frame_channels_v5(UfoDecoder *decoder, base += 6; } } - else if (header->pixel_size == 10) { + else { /*if (output_mode == IPECAMERA_MODE_16_CHAN_IO)*/ while (raw[base] != 0xAAAAAAA) { header = (payload_header_v5 *) &raw[base]; index = header->row_number * IPECAMERA_WIDTH + header->pixel_number; /* Skip header + two zero-filled words */ - base += 3; - - pixel_buffer[index + 15*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base] >> 22); - pixel_buffer[index + 13*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base] >> 12); - pixel_buffer[index + 14*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base] >> 2); - pixel_buffer[index + 12*IPECAMERA_PIXELS_PER_CHANNEL] = ((0x3 & raw[base]) << 8) | (0x3ff & (raw[base+1] >> 24)); - pixel_buffer[index + 10*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+1] >> 14); - pixel_buffer[index + 8*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+1] >> 4); - pixel_buffer[index + 11*IPECAMERA_PIXELS_PER_CHANNEL] = ((0xf & raw[base+1]) << 6) | (0x3ff & (raw[base+2] >> 26)); - pixel_buffer[index + 7*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+2] >> 16); - pixel_buffer[index + 9*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+2] >> 6); - pixel_buffer[index + 6*IPECAMERA_PIXELS_PER_CHANNEL] = ((0x3f & raw[base+2]) << 4) | (0x3ff & (raw[base+3] >> 28)); - pixel_buffer[index + 5*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+3] >> 18); - pixel_buffer[index + 2*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+3] >> 8); - pixel_buffer[index + 4*IPECAMERA_PIXELS_PER_CHANNEL] = ((0xff & raw[base+3]) << 2) | (0x3ff & (raw[base+4] >> 30)); - pixel_buffer[index + 3*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+4] >> 20); - pixel_buffer[index + 0*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+4] >> 10); - pixel_buffer[index + 1*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & raw[base+4]; - base += 5; + /* + base += 3; + pixel_buffer[index + 15*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base] >> 22); + pixel_buffer[index + 13*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base] >> 12); + pixel_buffer[index + 14*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base] >> 2); + pixel_buffer[index + 12*IPECAMERA_PIXELS_PER_CHANNEL] = ((0x3 & raw[base]) << 8) | (0x3ff & (raw[base+1] >> 24)); + pixel_buffer[index + 10*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+1] >> 14); + pixel_buffer[index + 8*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+1] >> 4); + pixel_buffer[index + 11*IPECAMERA_PIXELS_PER_CHANNEL] = ((0xf & raw[base+1]) << 6) | (0x3ff & (raw[base+2] >> 26)); + pixel_buffer[index + 7*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+2] >> 16); + pixel_buffer[index + 9*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+2] >> 6); + pixel_buffer[index + 6*IPECAMERA_PIXELS_PER_CHANNEL] = ((0x3f & raw[base+2]) << 4) | (0x3ff & (raw[base+3] >> 28)); + pixel_buffer[index + 5*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+3] >> 18); + pixel_buffer[index + 2*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+3] >> 8); + pixel_buffer[index + 4*IPECAMERA_PIXELS_PER_CHANNEL] = ((0xff & raw[base+3]) << 2) | (0x3ff & (raw[base+4] >> 30)); + pixel_buffer[index + 3*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+4] >> 20); + pixel_buffer[index + 0*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+4] >> 10); + pixel_buffer[index + 1*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & raw[base+4]; + base += 5; + */ + + base += 2; + + if (header->magic != 0xc0) { + pixel_buffer[index + 15*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base] >> 20); + pixel_buffer[index + 13*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base] >> 8); + pixel_buffer[index + 14*IPECAMERA_PIXELS_PER_CHANNEL] = (0xff & raw[base]) << 4 | (raw[base+1] >> 28); + pixel_buffer[index + 12*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+1] >> 16); + pixel_buffer[index + 10*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+1] >> 4); + pixel_buffer[index + 8*IPECAMERA_PIXELS_PER_CHANNEL] = ((0xf & raw[base+1]) << 8) | (raw[base+2] >> 24); + pixel_buffer[index + 11*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+2] >> 12); + pixel_buffer[index + 7*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & raw[base+2]; + pixel_buffer[index + 9*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+3] >> 20); + pixel_buffer[index + 6*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+3] >> 8); + pixel_buffer[index + 5*IPECAMERA_PIXELS_PER_CHANNEL] = (0xff & raw[base+3]) << 4 | (raw[base+4] >> 28); + pixel_buffer[index + 2*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+4] >> 16); + pixel_buffer[index + 4*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+4] >> 4); + pixel_buffer[index + 3*IPECAMERA_PIXELS_PER_CHANNEL] = ((0xf & raw[base+4]) << 8) | (raw[base+5] >> 24); + pixel_buffer[index + 0*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & (raw[base+5] >> 12); + pixel_buffer[index + 1*IPECAMERA_PIXELS_PER_CHANNEL] = 0x3ff & raw[base+5]; + } + else { + off++; + + if (header->magic == 0xc0) + off = 0; + } + + base += 6; + } } + *offset = base; return 0; } @@ -473,7 +513,7 @@ void ufo_deinterlace_interpolate(const uint16_t *in, uint16_t *out, int width, i memcpy(out, in + row*width, row_size_bytes); out += width; - /* Interpolate between source row and row+1 */ + /* Interpolate between source row and row+1 */ for (int x = 0; x < width; x++) out[x] = (int) (0.5 * in[row*width + x] + 0.5 * in[(row+1)*width + x]); @@ -497,9 +537,9 @@ void ufo_deinterlace_weave(const uint16_t *in1, const uint16_t *in2, uint16_t *o { const size_t row_size_bytes = width * sizeof(uint16_t); for (int row = 0; row < height; row++) { - memcpy(out, in1 + row*width, row_size_bytes); + memcpy(out, in1 + row*width, row_size_bytes); out += width; - memcpy(out, in2 + row*width, row_size_bytes); + memcpy(out, in2 + row*width, row_size_bytes); out += width; } } @@ -520,9 +560,9 @@ void ufo_deinterlace_weave(const uint16_t *in1, const uint16_t *in2, uint16_t *o * \return number of decoded bytes or 0 in case of error */ size_t ufo_decoder_decode_frame(UfoDecoder *decoder, - uint32_t *raw, - size_t num_bytes, - uint16_t *pixels, + uint32_t *raw, + size_t num_bytes, + uint16_t *pixels, UfoDecoderMeta *meta) { int err = 0; @@ -544,67 +584,78 @@ size_t ufo_decoder_decode_frame(UfoDecoder *decoder, CHECK_VALUE(raw[pos++], 0x55555555); switch (version) { - case 0: - CHECK_VALUE(raw[pos++], 0x56666666); - CHECK_VALUE(raw[pos] >> 28, 0x5); - meta->frame_number = raw[pos++] & 0xFFFFFFF; - CHECK_VALUE(raw[pos] >> 28, 0x5); - meta->time_stamp = raw[pos++] & 0xFFFFFFF; - break; - - case 4: - case 5: - CHECK_VALUE(raw[pos] >> 28, 0x5); - meta->cmosis_start_address = (raw[pos] >> 21) & 0x1FF; - meta->n_skipped_rows = (raw[pos] >> 15) & 0x3F; - meta->n_rows = rows_per_frame = raw[pos] & 0x7FF; - pos++; + case 0: + CHECK_VALUE(raw[pos++], 0x56666666); + CHECK_VALUE(raw[pos] >> 28, 0x5); + meta->frame_number = raw[pos++] & 0xFFFFFFF; + CHECK_VALUE(raw[pos] >> 28, 0x5); + meta->time_stamp = raw[pos++] & 0xFFFFFFF; + break; + + case 4: + case 5: + CHECK_VALUE(raw[pos] >> 28, 0x5); + meta->cmosis_start_address = (raw[pos] >> 21) & 0x1FF; + meta->n_skipped_rows = (raw[pos] >> 15) & 0x3F; + meta->n_rows = rows_per_frame = raw[pos] & 0x7FF; + pos++; - meta->frame_number = raw[pos++] & 0x1FFFFFF; - CHECK_VALUE(raw[pos] >> 28, 0x5); - meta->time_stamp = raw[pos++] & 0xFFFFFF; - meta->output_mode = (raw[pos] >> 24) & 0x3; - meta->adc_resolution = (raw[pos] >> 26) & 0x3 - break; + meta->frame_number = raw[pos++] & 0x1FFFFFF; + CHECK_VALUE(raw[pos] >> 28, 0x5); + meta->time_stamp = raw[pos] & 0xFFFFFF; + meta->output_mode = (raw[pos] >> 24) & 0x3; + meta->adc_resolution = (raw[pos] >> 26) & 0x3; + pos++; + + if ((meta->output_mode != IPECAMERA_MODE_4_CHAN_IO)&&(meta->output_mode != IPECAMERA_MODE_4_CHAN_IO)) { +#ifdef DEBUG + fprintf(stderr, "Output mode 0x%lx is not supported\n", meta->output_mode); +#endif + return EILSEQ; + } + break; - default: - fprintf(stderr, "Unsupported data format detected\n"); - return 0; + default: + fprintf(stderr, "Unsupported data format detected\n"); + return 0; } - if (err) + if (err) return 0; #else switch (version) { - case 0: - meta->frame_number = raw[pos + 6] & 0xFFFFFFF; - meta->time_stamp = raw[pos + 7] & 0xFFFFFFF; - break; - case 4: - case 5: - meta->frame_number = raw[pos + 6] & 0x1FFFFFF; - meta->time_stamp = raw[pos + 7] & 0xFFFFFF; - break; - default: - fprintf(stderr, "Unsupported data format detected\n"); - return 0; + case 0: + meta->frame_number = raw[pos + 6] & 0xFFFFFFF; + meta->time_stamp = raw[pos + 7] & 0xFFFFFFF; + break; + case 4: + case 5: + meta->frame_number = raw[pos + 6] & 0x1FFFFFF; + meta->time_stamp = raw[pos + 7] & 0xFFFFFF; + meta->output_mode = (raw[pos + 7] >> 24) & 0x3; + meta->adc_resolution = (raw[pos + 7] >> 26) & 0x3; + + break; + default: + fprintf(stderr, "Unsupported data format detected\n"); + return 0; } pos += 8; #endif switch (version) { - case 0: - err = ufo_decode_frame_channels_v0(decoder, pixels, raw + pos, num_words - pos - 8, &advance); - break; - case 4: - err = ufo_decode_frame_channels_v4(decoder, pixels, raw + pos, num_words - pos - 8, rows_per_frame, &advance); - break; - case 5: - err = ufo_decode_frame_channels_v5(decoder, pixels, raw + pos, num_words - pos - 8, rows_per_frame, &advance); - break; - default: - break; + case 0: + err = ufo_decode_frame_channels_v0(decoder, pixels, raw + pos, num_words - pos - 8, &advance); + break; + case 4: + err = ufo_decode_frame_channels_v4(decoder, pixels, raw + pos, num_words - pos - 8, rows_per_frame, &advance); + break; + case 5: + err = ufo_decode_frame_channels_v5(decoder, pixels, raw + pos, num_words - pos - 8, rows_per_frame, &advance, meta->output_mode); + break; + default: + break; } if (err) @@ -623,7 +674,7 @@ size_t ufo_decoder_decode_frame(UfoDecoder *decoder, CHECK_VALUE(raw[pos++], 0x00000000); CHECK_VALUE(raw[pos++], 0x01111111); - if (err) + if (err) return 0; #else pos += 8; @@ -636,7 +687,7 @@ size_t ufo_decoder_decode_frame(UfoDecoder *decoder, * \brief Iterate and decode next frame * * This function tries to decode the next frame in the currently set raw data - * stream. + * stream. * * \param decoder An UfoDecoder instance * \param pixels If pointer with NULL content is passed, a new buffer is @@ -649,8 +700,8 @@ size_t ufo_decoder_decode_frame(UfoDecoder *decoder, * NULL was passed but no memory could be allocated, EILSEQ if data stream is * corrupt and EFAULT if pixels is a NULL-pointer. */ -int ufo_decoder_get_next_frame(UfoDecoder *decoder, - uint16_t **pixels, +int ufo_decoder_get_next_frame(UfoDecoder *decoder, + uint16_t **pixels, UfoDecoderMeta *meta) { uint32_t *raw = decoder->raw; @@ -662,7 +713,7 @@ int ufo_decoder_get_next_frame(UfoDecoder *decoder, return 0; if (pos >= num_words) - return EIO; + return EIO; if (num_words < 16) return EILSEQ; |