jellyfish-powersupply/fw/jellyfish/jf_measure.c

/*
 * SPDX-FileCopyrightText: 2025 Igor Brkic <igor@hyperglitch.com>
 *
 * SPDX-License-Identifier: GPL-3.0-or-later
 */

#include "jf_measure.h"

#include <stdio.h>

#include "jf_qspi.h"
#include "jf_usb.h"

#include "FreeRTOS.h"
#include "jf_common.h"
#include "stream_buffer.h"

#include "main.h"
#include "queue.h"

static uint8_t data_buffer[2][256] = {0};
static int current_buffer_idx = 0;

static bool initialized = false;

uint8_t data_queue_buffer[sizeof(jf_measure_data_t)*JF_MEASURE_BUFFER_SIZE];
static StaticQueue_t data_queue_structure;
QueueHandle_t data_queue;

static int32_t avg_frames_parsed = 0;
static int32_t avg_buffer_full = 0;

static jf_measure_data_t current_data = {0};

static jf_measure_range_t current_range = JF_RANGE_3A;

static const jf_calibration_t *calibration_data = NULL;

static int accum_count = 0;
static jf_measure_data_t accum = {
  .Isense1 = 0,
  .Isense2 = 0,
  .Vsense = 0,
  .ain = 0,
  .dig = {0, 0, 0, 0, 0},
  .range = 0
};

// called from timer interrupt, pulls the data from the FPGA
// and processes it
void jf_measure_fetch_data() {
  if (!initialized) {
    return;
  }

  // send a request to fill one buffer while parsing the other
  uint8_t *data = data_buffer[current_buffer_idx];
  current_buffer_idx = (current_buffer_idx+1)%2;
  uint8_t * proc_data = data_buffer[current_buffer_idx];

  HAL_GPIO_TogglePin(DEBUG_PAD_GPIO_Port, DEBUG_PAD_Pin);

  // clock prescaler  QSPI clock  time for the 256byte request
  //       9             20MHz           15.5us
  //       5             36MHz            9.5us
  jf_qspi_direct_call(
    JF_CMD_ADC_READ,
    0x00,
    data,
    256
  );

  int last_header = -1;
  int frames_found = 0;

  int16_t adc00;
  int16_t adc01;
  int16_t adc10;
  int16_t adc11;
  uint8_t gpios = 0;
  uint8_t range = 0;

  uint8_t *adc00p = (uint8_t *) &adc00;
  uint8_t *adc01p = (uint8_t *) &adc01;
  uint8_t *adc10p = (uint8_t *) &adc10;
  uint8_t *adc11p = (uint8_t *) &adc11;

  // parsing the whole packet here slows everything down a lot
  // here, we'll parse just the first packet and do some averaging
  // every 100 or so packets
  for (int i = 0; i < 256-1; i++) {
    if (proc_data[i] == 0x4a && proc_data[i + 1] == 0x00) {
      if (last_header != -1 && i - last_header == 12) {
        frames_found++;

        //memcpy(data_to_send+data_to_send_idx, &proc_data[last_header], 12);
        //data_to_send_idx += 12;

        adc00p[0] = proc_data[last_header + 3];
        adc00p[1] = proc_data[last_header + 2];
        accum.Isense1 += (float)adc00;

        adc01p[0] = proc_data[last_header + 5];
        adc01p[1] = proc_data[last_header + 4];
        accum.Isense2 += (float)adc01;

        adc10p[0] = proc_data[last_header + 7];
        adc10p[1] = proc_data[last_header + 6];
        accum.Vsense += (float)adc10;

        adc11p[0] = proc_data[last_header + 9];
        adc11p[1] = proc_data[last_header + 8];
        accum.ain += (float)adc11;

        gpios = proc_data[last_header + 11] & 0x1f;
        range = proc_data[last_header + 11] >> 5;

        accum.range = range;
        accum.dig[0] = gpios;
        accum_count++;
        break;
      }
      last_header = i;
    }
  }

  if (accum_count>100) {
    // update the "current data" with the last received record
    // (this doesn't have to be done in each cycle...)

    current_data.Isense2 = accum.Isense2/(float)accum_count * calibration_data->isense2[0] + calibration_data->isense2[1];
    if (range==0) {
      current_data.Isense1 = current_data.Isense2;
    }
    else {
      current_data.Isense1 = accum.Isense1/(float)accum_count * calibration_data->isense1[range*2] + calibration_data->isense2[range*2+1];
    }
    current_data.Vsense = (accum.Vsense/(float)accum_count * calibration_data->voltage[0] + calibration_data->voltage[1])/1000.0;
    current_data.ain = accum.ain/(float)accum_count * calibration_data->ain[0] + calibration_data->ain[1];
    gpios = accum.dig[0];
    for (int i=0; i<5; i++) {
      current_data.dig[i] = (gpios&(1<<i)) == (1<<i);
    }
    accum.Isense1 = 0.0;
    accum.Isense2 = 0.0;
    accum.Vsense = 0.0;
    accum.ain = 0.0;
    accum_count = 0;
  }

  jf_usb_add_to_buffer_from_isr(proc_data, 256);
}

int32_t jf_measure_avg_frames_parsed() {
  return avg_frames_parsed;
}

int32_t jf_measure_avg_buffer_full() {
  return avg_buffer_full;
}

jf_measure_data_t jf_measure_get_current_values() {
  return current_data;
}

uint16_t jf_measure_get_recent_data(jf_measure_data_t *data, uint16_t size, bool from_isr) {
  BaseType_t ret;
  for (int i = 0; i < size; i++) {
    if (from_isr) {
      ret = xQueueReceiveFromISR(data_queue, &data[i], NULL);

    }
    else {
      ret = xQueueReceive(data_queue, &data[i], portMAX_DELAY);
    }
    if (ret!=pdPASS) {
      return i;
    }
  }
  return size;
  /*
  for (int i = 0; i < size; i++) {
    data_out[i] = data_buffer[current_buffer_idx][i];
  }
  return size;
  */

  /*
  if (ch >= JF_MEAS_DIG) return 0 ;

  int start_idx = (buffer_idx + JF_MEASURE_BUFFER_SIZE - size) % JF_MEASURE_BUFFER_SIZE;
  for (int i = 0; i < size; i++) {
    data_out[i] = buffer_adc[ch][start_idx];
    start_idx = start_idx + 1;
    if (start_idx >= JF_MEASURE_BUFFER_SIZE) start_idx = 0;
  }
  return (int16_t)size;
  */
}

jf_measure_range_t jf_measure_get_range() {
  return current_range;
}

void jf_measure_set_range(jf_measure_range_t range, bool from_isr) {
  if (range<JF_RANGE_3A || range>JF_RANGE_OFF) {
    return;
  }
  current_range = range;
  if (from_isr) {
    jf_qspi_add_to_queue_from_isr(
      JF_CMD_SET_RANGE,
      range,
      NULL,
      0
      );
  }
  else {
    jf_qspi_add_to_queue(
      JF_CMD_SET_RANGE,
      range,
      NULL,
      0,
      false
      );
  }
}

const jf_calibration_t * jf_measure_get_calibration() {
  return calibration_data;
}

void jf_measure_init() {
  // load the calibration
  int i=0;
  uint32_t uid[3];
  jf_dev_get_uid(uid);
  while (true) {
    if (JF_CALIBRATION_DATA[i].uid[0]==0 && JF_CALIBRATION_DATA[i].uid[1]==0 && JF_CALIBRATION_DATA[i].uid[2]==0) {
      break;
    }

    if (JF_CALIBRATION_DATA[i].uid[0]==uid[0] && JF_CALIBRATION_DATA[i].uid[1]==uid[1] && JF_CALIBRATION_DATA[i].uid[2]==uid[2]) {
      calibration_data = &JF_CALIBRATION_DATA[i];
      printf("Loaded calibration data for device %s\r\n", JF_CALIBRATION_DATA[i].description);
      break;
    }
    i++;
  }

  if (!calibration_data) {
    printf("ERROR: No calibration data available for this device.\r\n");
    calibration_data = &JF_CALIBRATION_EMPTY;
  }

  data_queue = xQueueCreateStatic(
    JF_MEASURE_BUFFER_SIZE,
    sizeof(jf_measure_data_t),
    data_queue_buffer,
    &data_queue_structure
    );
  if (data_queue==NULL) {
    printf("failed to create a data queue\r\n");
    Error_Handler();
  }

  initialized = true;
}