293 lines
7.5 KiB
C
293 lines
7.5 KiB
C
#include "bl0939.h"
|
||
#include "string.h"
|
||
#include "utils.h"
|
||
// #include "ModbusS.h"
|
||
#include "config.h"
|
||
|
||
#include <stdio.h>
|
||
#include <stdlib.h>
|
||
#include <string.h>
|
||
#include "freertos/FreeRTOS.h"
|
||
#include "freertos/task.h"
|
||
#include "esp_system.h"
|
||
#include "driver/spi_master.h"
|
||
#include "driver/gpio.h"
|
||
#include "esp_log.h"
|
||
|
||
#define TAG "BL0939"
|
||
|
||
#define BL0939_HOST SPI3_HOST
|
||
|
||
#define SPI1_PIN_NUM_MISO 37
|
||
#define SPI1_PIN_NUM_MOSI 36
|
||
#define SPI1_PIN_NUM_CLK 35
|
||
#define SPI1_PIN_NUM_CS 40
|
||
|
||
|
||
extern move_t *pMoveCtx;
|
||
// To speed up transfers, every SPI transfer sends a bunch of lines. This define specifies how many. More means more memory use,
|
||
// but less overhead for setting up / finishing transfers. Make sure 240 is dividable by this.
|
||
#define PARALLEL_LINES 16
|
||
static spi_device_handle_t spi1;
|
||
|
||
static void SPI1_Init(void)
|
||
{
|
||
esp_err_t ret;
|
||
|
||
spi_bus_config_t buscfg1 = {
|
||
.miso_io_num = SPI1_PIN_NUM_MISO,
|
||
.mosi_io_num = SPI1_PIN_NUM_MOSI,
|
||
.sclk_io_num = SPI1_PIN_NUM_CLK,
|
||
.quadwp_io_num = -1,
|
||
.quadhd_io_num = -1,
|
||
.max_transfer_sz = PARALLEL_LINES * 320 * 2 + 8};
|
||
|
||
spi_device_interface_config_t devcfg1 = {
|
||
#ifdef CONFIG_LCD_OVERCLOCK
|
||
.clock_speed_hz = 26 * 1000 * 1000, // Clock out at 26 MHz
|
||
#else
|
||
.clock_speed_hz = 8 * 100 * 1000, // Clock out at 800kHz
|
||
#endif
|
||
|
||
.mode = 1, // SPI mode 1
|
||
.spics_io_num = SPI1_PIN_NUM_CS, // CS pin
|
||
.queue_size = 7, // We want to be able to queue 7 transactions at a time
|
||
// .command_bits = 8,
|
||
// .pre_cb = lcd_spi_pre_transfer_callback, // Specify pre-transfer callback to handle D/C line
|
||
};
|
||
// Initialize the SPI bus
|
||
ret = spi_bus_initialize(BL0939_HOST, &buscfg1, SPI_DMA_CH_AUTO);
|
||
ESP_ERROR_CHECK(ret);
|
||
// Attach the ADS1220 to the SPI bus
|
||
ret = spi_bus_add_device(BL0939_HOST, &devcfg1, &spi1);
|
||
ESP_ERROR_CHECK(ret);
|
||
}
|
||
|
||
void bl0939_spi_reset(void);
|
||
|
||
static void BL0939_SPI_TransmitReceive(uint8_t *tbuff, uint8_t *rbuff,
|
||
uint16_t len)
|
||
{
|
||
esp_err_t ret;
|
||
spi_transaction_t t;
|
||
memset(&t, 0, sizeof(t)); // Zero out the transaction
|
||
t.tx_buffer = tbuff;
|
||
t.rx_buffer = rbuff;
|
||
t.length = len * 8; // Command is 16 bits
|
||
ret = spi_device_polling_transmit(spi1, &t); // Transmit!
|
||
assert(ret == ESP_OK);
|
||
}
|
||
|
||
static uint32_t bl0939_read_reg(uint8_t reg)
|
||
{
|
||
uint8_t tx_buf[6] = {0x55, reg, 0, 0, 0, 0};
|
||
uint8_t rx_buf[6] = {0};
|
||
// bl0939_spi_reset();
|
||
BL0939_SPI_TransmitReceive(tx_buf, rx_buf, 6);
|
||
if (rx_buf[5] == (uint8_t) ~(0x55 + reg + rx_buf[2] + rx_buf[3] + rx_buf[4]))
|
||
{
|
||
return (uint32_t)rx_buf[2] << 16 |
|
||
(uint32_t)rx_buf[3] << 8 |
|
||
(uint32_t)rx_buf[4] << 0;
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
uint32_t r_temp = 0;
|
||
static int bl0939_write_reg(uint8_t reg, uint32_t val, int check)
|
||
{
|
||
int i = 5;
|
||
uint8_t h = val >> 16;
|
||
uint8_t m = val >> 8;
|
||
uint8_t l = val >> 0;
|
||
do
|
||
{
|
||
i--;
|
||
vTaskDelay(5);
|
||
// bl0939_spi_reset();
|
||
uint8_t tx_buf[6] = {0xA5, reg, h, m, l, ~(0XA5 + reg + h + m + l)};
|
||
uint8_t rx_buf[6] = {0};
|
||
BL0939_SPI_TransmitReceive(tx_buf, rx_buf, 6);
|
||
vTaskDelay(10);
|
||
if (0 == check)
|
||
return 0;
|
||
r_temp = bl0939_read_reg(reg);
|
||
if (r_temp == val)
|
||
return 0;
|
||
} while (i > 0);
|
||
return 1;
|
||
}
|
||
|
||
void bl0939_spi_reset(void)
|
||
{
|
||
uint8_t tx_buf[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||
uint8_t rx_buf[6] = {0};
|
||
BL0939_SPI_TransmitReceive(tx_buf, rx_buf, 6);
|
||
}
|
||
|
||
uint32_t comp_threshold(float mA)
|
||
{
|
||
return (uint32_t)(mA * 1 * 324004 * 0.00140625f * 0.95f / (600 * 1.218f));
|
||
}
|
||
|
||
void bl0939_reset(void)
|
||
{
|
||
bl0939_spi_reset();
|
||
bl0939_write_reg(0x19, 0x005a5a5a, 0); // <20><>λ<EFBFBD>û<EFBFBD><C3BB>Ĵ<EFBFBD><C4B4><EFBFBD>
|
||
bl0939_write_reg(0x1a, 0x00000055, 1); // <20><><EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD>
|
||
bl0939_write_reg(0x10, 0xffff, 0); // Threshold A
|
||
bl0939_write_reg(0x1E, 0xffff, 1); // Threshold B
|
||
bl0939_write_reg(0x18, 0x00002000, 1); // cf
|
||
bl0939_write_reg(0x1B, 0x000047ff, 0); // cf
|
||
bl0939_write_reg(0x1a, 0x00000000, 1); // д<><D0B4><EFBFBD><EFBFBD>
|
||
}
|
||
|
||
// T = 40ms
|
||
int bl0939_get_current_A()
|
||
{
|
||
bl0939_spi_reset();
|
||
uint32_t Ia = bl0939_read_reg(0x00);
|
||
// return Ia * 1.218f / (float) ( 324004 * 1 );
|
||
return Ia;
|
||
}
|
||
// T = 40ms
|
||
int bl0939_get_current_B()
|
||
{
|
||
bl0939_spi_reset();
|
||
uint32_t Ib = bl0939_read_reg(0x07);
|
||
// return Ib * 1.218f / (float) ( 324004 * 1 );
|
||
return Ib;
|
||
}
|
||
// T = 400ms
|
||
int bl0939_get_voltage()
|
||
{
|
||
bl0939_spi_reset();
|
||
uint32_t v = bl0939_read_reg(0x06);
|
||
// return v * 1.218f * ( 2 + 2000 ) / (float) ( 79931 * 2 * 1000 );
|
||
return v;
|
||
}
|
||
|
||
/// @brief
|
||
uint8_t bl0939_cmd[36] =
|
||
{
|
||
0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
|
||
0x55, 0x00, 0, 0, 0, 0,
|
||
0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
|
||
0x55, 0x07, 0, 0, 0, 0,
|
||
0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
|
||
0x55, 0x06, 0, 0, 0, 0};
|
||
|
||
int check_sum_ch(uint8_t *buf, int offset)
|
||
{
|
||
uint8_t sum = ~(0x55 + bl0939_cmd[offset - 1] + buf[offset] + buf[offset + 1] + buf[offset + 2]);
|
||
if (buf[offset + 3] == sum)
|
||
{
|
||
return 1;
|
||
}
|
||
return 0;
|
||
}
|
||
uint32_t readUint24LE(uint8_t *buf, int offset)
|
||
{
|
||
return buf[offset] | buf[offset + 1] << 8 | buf[offset + 2] << 16;
|
||
}
|
||
uint32_t readUint24BE(uint8_t *buf, int offset)
|
||
{
|
||
return buf[offset] << 16 | buf[offset + 1] << 8 | buf[offset + 2];
|
||
}
|
||
|
||
// __IO uint8_t bl0939_Transfer_done = 0;
|
||
uint8_t bl0939_Transfer_done = 0;
|
||
|
||
void bl0939_init(void)
|
||
{
|
||
bl0939_reset();
|
||
}
|
||
|
||
float ac_current_coef[3] = {
|
||
1.218f / (324004.f * 0.225 * 1000 / 2000) * 100.f,
|
||
1.218f / (324004.f * 0.23 * 1000 / 2000) * 100.f,
|
||
1.218f / (79931.f * 0.002 * 500) * 100.f};
|
||
uint8_t bl0939_rxbuf[2][36];
|
||
int32_t ac_ad_values[3] = {0};
|
||
int move_time = 0;
|
||
int move_req = 0;
|
||
extern uint16_t gWordVar[];
|
||
int move_status = 0;
|
||
|
||
void BL0939_task()
|
||
{
|
||
int index = 0;
|
||
while (1)
|
||
{
|
||
vTaskDelay(10);
|
||
// ESP_LOGI(TAG, "BL0939_SPI_TransmitReceive before\n ");
|
||
bl0939_Transfer_done = 1;
|
||
|
||
BL0939_SPI_TransmitReceive(bl0939_cmd, bl0939_rxbuf[index & 1], 36);
|
||
if (!memcmp(bl0939_rxbuf[0], bl0939_rxbuf[1], sizeof(bl0939_rxbuf[0])))
|
||
{
|
||
index++;
|
||
bl0939_Transfer_done = 0;
|
||
// continue;
|
||
}
|
||
// ESP_LOGI(TAG, "BL0939_SPI_TransmitReceive after\n ");
|
||
if (bl0939_Transfer_done)
|
||
{
|
||
int ch;
|
||
// ESP_LOGI(TAG, "bl0939_Transfer_done \n ");
|
||
bl0939_Transfer_done = 0;
|
||
for (ch = 0; ch < 3; ch++)
|
||
{
|
||
if (check_sum_ch(bl0939_rxbuf[index & 1], 8 + ch * 12))
|
||
{
|
||
ac_ad_values[ch] = readUint24BE(bl0939_rxbuf[index & 1], 8 + ch * 12);
|
||
// ESP_LOGI(TAG, "channel : %d ac_ad_values: %d\n ", ch, ac_ad_values[ch]);
|
||
gWordVar[AC_CURRENT_REG_ADDR + ch] = ac_ad_values[ch] * ac_current_coef[ch];
|
||
}
|
||
// ac_ad_values[ch] = readUint24BE(bl0939_rxbuf,8+ch*12);
|
||
// ESP_LOGI(TAG, "channel : %d ac_ad_values: %d\n ", ch, ac_ad_values[ch]);
|
||
}
|
||
if (pMoveCtx->status == 0)
|
||
{ // 停止状态，等待启动
|
||
int move_current_channel = depth_config->move_current_channel;
|
||
if (move_current_channel > 2 || move_current_channel < 0)
|
||
{
|
||
move_current_channel = 2;
|
||
}
|
||
if (gWordVar[AC_CURRENT_REG_ADDR + move_current_channel] > depth_config->current_on_threshold)
|
||
{
|
||
if (++pMoveCtx->time_count > depth_config->move_on_duratino)
|
||
{
|
||
pMoveCtx->status = 1;
|
||
pMoveCtx->time_count = 0;
|
||
pMoveCtx->pile_inc_req = 1;
|
||
}
|
||
}
|
||
}
|
||
else if (pMoveCtx->status == 1)
|
||
{
|
||
int move_current_channel = depth_config->move_current_channel;
|
||
if (move_current_channel > 2 || move_current_channel < 0)
|
||
{
|
||
move_current_channel = 2;
|
||
}
|
||
if (gWordVar[AC_CURRENT_REG_ADDR + 2] < depth_config->current_off_threshold)
|
||
{
|
||
if (++pMoveCtx->time_count > depth_config->move_off_duration)
|
||
{
|
||
pMoveCtx->status = 0;
|
||
pMoveCtx->time_count = 0;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
void BL0939_init(void)
|
||
{
|
||
SPI1_Init();
|
||
bl0939_init();
|
||
xTaskCreate(BL0939_task, "BL0939_task", 4096, NULL, 10, NULL);
|
||
}
|