314 lines
10 KiB
C
314 lines
10 KiB
C
#include "mini212g2.h"
|
|
#include "debug.h"
|
|
#include "string.h"
|
|
#include "qdx_port.h"
|
|
|
|
#if SENSOR_UART_ENABLE
|
|
|
|
/* ============================================================
|
|
* Protocol constants
|
|
* ============================================================ */
|
|
#define FRAME_HEAD1 0x55
|
|
#define FRAME_HEAD2 0xAA
|
|
#define FRAME_TAIL 0xF0
|
|
|
|
/* ACK response: 55 AA 01 00 01 F0 */
|
|
#define ACK_LEN 6
|
|
static const uint8_t ACK_PATTERN[ACK_LEN] = {0x55, 0xAA, 0x01, 0x00, 0x01, 0xF0};
|
|
|
|
/* ============================================================
|
|
* DVP output configuration commands
|
|
* (from Mini212G2 serial protocol document)
|
|
* ============================================================ */
|
|
|
|
/* Digital port type → CMOS */
|
|
static const uint8_t CMD_DIGITAL_CMOS[] =
|
|
{0x55, 0xAA, 0x07, 0x02, 0x01, 0x02, 0x00, 0x00, 0x00, 0x02, 0x04, 0xF0};
|
|
|
|
/* CMOS interface → CMOS8(MSB) */
|
|
static const uint8_t CMD_CMOS8_MSB[] =
|
|
{0x55, 0xAA, 0x07, 0x02, 0x01, 0x04, 0x00, 0x00, 0x00, 0x01, 0x01, 0xF0};
|
|
|
|
/* CMOS content → Y16 */
|
|
static const uint8_t CMD_CONTENT_Y16[] =
|
|
{0x55, 0xAA, 0x07, 0x02, 0x01, 0x03, 0x00, 0x00, 0x00, 0x02, 0x05, 0xF0};
|
|
|
|
/* Frame rate → 30Hz */
|
|
static const uint8_t CMD_FPS_30HZ[] =
|
|
{0x55, 0xAA, 0x07, 0x02, 0x01, 0x05, 0x00, 0x00, 0x00, 0x00, 0x01, 0xF0};
|
|
|
|
/* Save settings */
|
|
static const uint8_t CMD_SAVE[] =
|
|
{0x55, 0xAA, 0x07, 0x01, 0x00, 0x04, 0x00, 0x00, 0x00, 0x01, 0x03, 0xF0};
|
|
|
|
/* Shutter compensation (NUC) */
|
|
static const uint8_t CMD_SHUTTER[] =
|
|
{0x55, 0xAA, 0x07, 0x02, 0x01, 0x08, 0x00, 0x00, 0x00, 0x01, 0x0D, 0xF0};
|
|
|
|
/* Query: digital video page */
|
|
static const uint8_t CMD_QUERY_DIGITAL[] =
|
|
{0x55, 0xAA, 0x07, 0x02, 0x01, 0x80, 0x00, 0x00, 0x00, 0x00, 0x84, 0xF0};
|
|
|
|
/* Query: status page (simplest query, always works) */
|
|
static const uint8_t CMD_QUERY_STATUS[] =
|
|
{0x55, 0xAA, 0x07, 0x00, 0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x87, 0xF0};
|
|
|
|
/* ============================================================
|
|
* Low-level UART helpers
|
|
* ============================================================ */
|
|
|
|
static void Sensor_UART_Init(void)
|
|
{
|
|
GPIO_InitTypeDef gpio = {0};
|
|
USART_InitTypeDef usart = {0};
|
|
|
|
#if SENSOR_USE_USART3
|
|
/* USART3: PB10=TX PB11=RX
|
|
* NOTE: This reuses the debug printf port. printf is disabled. */
|
|
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);
|
|
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
|
|
|
|
gpio.GPIO_Pin = GPIO_Pin_10;
|
|
gpio.GPIO_Speed = GPIO_Speed_50MHz;
|
|
gpio.GPIO_Mode = GPIO_Mode_AF_PP;
|
|
GPIO_Init(GPIOB, &gpio);
|
|
|
|
gpio.GPIO_Pin = GPIO_Pin_11;
|
|
gpio.GPIO_Mode = GPIO_Mode_IN_FLOATING;
|
|
GPIO_Init(GPIOB, &gpio);
|
|
#else
|
|
/* USART2: PA2=TX PA3=RX */
|
|
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
|
|
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
|
|
|
|
gpio.GPIO_Pin = GPIO_Pin_2;
|
|
gpio.GPIO_Speed = GPIO_Speed_50MHz;
|
|
gpio.GPIO_Mode = GPIO_Mode_AF_PP;
|
|
GPIO_Init(GPIOA, &gpio);
|
|
|
|
gpio.GPIO_Pin = GPIO_Pin_3;
|
|
gpio.GPIO_Mode = GPIO_Mode_IN_FLOATING;
|
|
GPIO_Init(GPIOA, &gpio);
|
|
#endif
|
|
|
|
usart.USART_BaudRate = SENSOR_UART_BAUD;
|
|
usart.USART_WordLength = USART_WordLength_8b;
|
|
usart.USART_StopBits = USART_StopBits_1;
|
|
usart.USART_Parity = USART_Parity_No;
|
|
usart.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
|
|
usart.USART_Mode = USART_Mode_Tx | USART_Mode_Rx;
|
|
|
|
USART_Init(SENSOR_UART, &usart);
|
|
USART_Cmd(SENSOR_UART, ENABLE);
|
|
}
|
|
|
|
static void Sensor_SendBytes(const uint8_t *data, uint8_t len)
|
|
{
|
|
for (uint8_t i = 0; i < len; i++) {
|
|
while (!USART_GetFlagStatus(SENSOR_UART, USART_FLAG_TXE));
|
|
USART_SendData(SENSOR_UART, data[i]);
|
|
}
|
|
while (!USART_GetFlagStatus(SENSOR_UART, USART_FLAG_TC));
|
|
}
|
|
|
|
/**
|
|
* Read response bytes until FRAME_TAIL seen or timeout.
|
|
* timeout_ms ≈ approximate milliseconds (uses Delay_Us polling).
|
|
* Returns number of bytes actually read.
|
|
*/
|
|
static int Sensor_ReadResp(uint8_t *buf, uint8_t max_len, uint16_t timeout_ms)
|
|
{
|
|
uint8_t idx = 0;
|
|
/* Poll with ~0.1 ms granularity */
|
|
for (uint32_t i = 0; i < (uint32_t)timeout_ms * 10; i++) {
|
|
if (USART_GetFlagStatus(SENSOR_UART, USART_FLAG_RXNE)) {
|
|
buf[idx++] = (uint8_t)USART_ReceiveData(SENSOR_UART);
|
|
if (idx >= max_len) return idx;
|
|
if (idx >= 2 && buf[idx - 1] == FRAME_TAIL) return idx;
|
|
} else {
|
|
Delay_Us(100);
|
|
}
|
|
}
|
|
return idx;
|
|
}
|
|
|
|
static void Sensor_FlushRx(void)
|
|
{
|
|
while (USART_GetFlagStatus(SENSOR_UART, USART_FLAG_RXNE))
|
|
(void)USART_ReceiveData(SENSOR_UART);
|
|
}
|
|
|
|
/* ============================================================
|
|
* Public API
|
|
* ============================================================ */
|
|
|
|
/* ============================================================
|
|
* Debug inspection variables (watch in JTAG debugger)
|
|
* ============================================================ */
|
|
volatile uint8_t sensor_init_ok = 0;
|
|
volatile uint8_t sensor_init_fail = 0;
|
|
volatile uint8_t sensor_last_resp[32] = {0};
|
|
volatile uint8_t sensor_last_resp_len = 0;
|
|
volatile uint8_t sensor_comm_test = 0;
|
|
|
|
int Mini212G2_SendCmd(const uint8_t *cmd, uint8_t len)
|
|
{
|
|
uint8_t resp[8];
|
|
Sensor_FlushRx();
|
|
Sensor_SendBytes(cmd, len);
|
|
|
|
int n = Sensor_ReadResp(resp, sizeof(resp), 200);
|
|
/* Save last response for debugger inspection */
|
|
sensor_last_resp_len = (uint8_t)n;
|
|
for (int j = 0; j < n && j < 32; j++)
|
|
sensor_last_resp[j] = resp[j];
|
|
|
|
if (n == ACK_LEN && memcmp(resp, ACK_PATTERN, ACK_LEN) == 0)
|
|
return 0;
|
|
|
|
DBG_INIT("Sensor resp(%d):", n);
|
|
for (int j = 0; j < n; j++) printf(" %02x", resp[j]);
|
|
printf("\r\n");
|
|
return -1;
|
|
}
|
|
|
|
#endif /* SENSOR_UART_ENABLE (UART helpers + SendCmd) */
|
|
|
|
int Mini212G2_Init(void)
|
|
{
|
|
#if !SENSOR_UART_ENABLE
|
|
DBG_INIT("Sensor: UART disabled, pre-configured via USB\r\n");
|
|
DBG_INIT("Sensor: CMOS output, CMOS8(MSB), Y16, 30Hz\r\n");
|
|
return 0;
|
|
#else
|
|
int ok = 0, fail = 0;
|
|
|
|
Sensor_UART_Init();
|
|
DBG_INIT("Sensor: UART%d baud=%d\r\n",
|
|
(int)(SENSOR_UART == USART3 ? 3 : 2), (int)SENSOR_UART_BAUD);
|
|
|
|
/* Sensor needs several seconds to boot after power-on */
|
|
DBG_INIT("Sensor: Waiting 3s for boot...\r\n");
|
|
Delay_Ms(3000);
|
|
|
|
/* === Communication test: send status query === */
|
|
{
|
|
uint8_t test_resp[32];
|
|
Sensor_FlushRx();
|
|
Sensor_SendBytes(CMD_QUERY_STATUS, sizeof(CMD_QUERY_STATUS));
|
|
int tn = Sensor_ReadResp(test_resp, sizeof(test_resp), 500);
|
|
sensor_last_resp_len = (uint8_t)tn;
|
|
for (int j = 0; j < tn && j < 32; j++)
|
|
sensor_last_resp[j] = test_resp[j];
|
|
sensor_comm_test = (tn > 0) ? 1 : 0xFF;
|
|
/* Breakpoint here to check sensor_comm_test and sensor_last_resp */
|
|
}
|
|
Delay_Ms(200);
|
|
|
|
DBG_INIT("Sensor: Configuring CMOS/DVP output...\r\n");
|
|
|
|
/* 1. Shutter compensation */
|
|
if (Mini212G2_SendCmd(CMD_SHUTTER, sizeof(CMD_SHUTTER)) == 0)
|
|
{ ok++; DBG_INIT("Sensor: Shutter OK\r\n"); }
|
|
else
|
|
{ fail++; DBG_ERR("Sensor: Shutter FAIL\r\n"); }
|
|
Delay_Ms(200);
|
|
|
|
/* 2. Digital port → CMOS */
|
|
if (Mini212G2_SendCmd(CMD_DIGITAL_CMOS, sizeof(CMD_DIGITAL_CMOS)) == 0)
|
|
{ ok++; DBG_INIT("Sensor: Digital->CMOS OK\r\n"); }
|
|
else
|
|
{ fail++; DBG_ERR("Sensor: Digital->CMOS FAIL\r\n"); }
|
|
Delay_Ms(200);
|
|
|
|
/* 3. CMOS interface → 8-bit MSB */
|
|
if (Mini212G2_SendCmd(CMD_CMOS8_MSB, sizeof(CMD_CMOS8_MSB)) == 0)
|
|
{ ok++; DBG_INIT("Sensor: CMOS8(MSB) OK\r\n"); }
|
|
else
|
|
{ fail++; DBG_ERR("Sensor: CMOS8(MSB) FAIL\r\n"); }
|
|
Delay_Ms(200);
|
|
|
|
/* 4. CMOS content → Y16 (raw 16-bit thermal) */
|
|
if (Mini212G2_SendCmd(CMD_CONTENT_Y16, sizeof(CMD_CONTENT_Y16)) == 0)
|
|
{ ok++; DBG_INIT("Sensor: Y16 OK\r\n"); }
|
|
else
|
|
{ fail++; DBG_ERR("Sensor: Y16 FAIL\r\n"); }
|
|
Delay_Ms(200);
|
|
|
|
/* 5. Frame rate → 30Hz */
|
|
if (Mini212G2_SendCmd(CMD_FPS_30HZ, sizeof(CMD_FPS_30HZ)) == 0)
|
|
{ ok++; DBG_INIT("Sensor: 30Hz OK\r\n"); }
|
|
else
|
|
{ fail++; DBG_ERR("Sensor: 30Hz FAIL\r\n"); }
|
|
Delay_Ms(200);
|
|
|
|
/* 6. Save settings to flash */
|
|
if (Mini212G2_SendCmd(CMD_SAVE, sizeof(CMD_SAVE)) == 0)
|
|
{ ok++; DBG_INIT("Sensor: Save OK\r\n"); }
|
|
else
|
|
{ fail++; DBG_ERR("Sensor: Save FAIL\r\n"); }
|
|
Delay_Ms(500);
|
|
|
|
DBG_INIT("Sensor: %d ok, %d fail\r\n", ok, fail);
|
|
sensor_init_ok = (uint8_t)ok;
|
|
sensor_init_fail = (uint8_t)fail;
|
|
|
|
/* Print current digital video status for verification */
|
|
Mini212G2_PrintDigitalVideoStatus();
|
|
|
|
return (fail == 0) ? 0 : -1;
|
|
#endif /* SENSOR_UART_ENABLE */
|
|
}
|
|
|
|
#if SENSOR_UART_ENABLE
|
|
void Mini212G2_PrintDigitalVideoStatus(void)
|
|
{
|
|
uint8_t resp[32];
|
|
Sensor_FlushRx();
|
|
Sensor_SendBytes(CMD_QUERY_DIGITAL, sizeof(CMD_QUERY_DIGITAL));
|
|
|
|
int n = Sensor_ReadResp(resp, sizeof(resp), 500);
|
|
if (n < 24) {
|
|
DBG_ERR("Sensor: Query failed (got %d bytes)\r\n", n);
|
|
return;
|
|
}
|
|
|
|
/* Parse per protocol spec (Page 11-12):
|
|
* Byte5 = ext-sync, Byte6 = digital port type, Byte7 = CMOS content,
|
|
* Byte8 = CMOS interface, Byte9 = frame rate, Byte11 = clock phase */
|
|
static const char *port_names[] = {
|
|
"OFF", "USB2.0", "CMOS", "BT1120", "BT656",
|
|
"USB+UART", "LCD", "LVDS", "LCD+DVP", "UVC+CDC"
|
|
};
|
|
static const char *content_names[] = {
|
|
"YUV422", "YUV422+Param", "Y16", "Y16+Param",
|
|
"Y16+YUV422", "Y16+Param+YUV422"
|
|
};
|
|
static const char *iface_names[] = {"CMOS16", "CMOS8(MSB)", "CMOS8(LSB)"};
|
|
static const char *fps_names[] = {"30Hz", "25Hz", "9Hz", "50Hz"};
|
|
|
|
uint8_t port_type = resp[6];
|
|
uint8_t content = resp[7];
|
|
uint8_t iface = resp[8];
|
|
uint8_t fps = resp[9];
|
|
uint8_t clk_phase = resp[11];
|
|
|
|
DBG_INIT("Sensor Digital Video Status:\r\n");
|
|
DBG_INIT(" Port: %s (%d)\r\n",
|
|
(port_type < 10) ? port_names[port_type] : "?", (int)port_type);
|
|
DBG_INIT(" Content: %s (%d)\r\n",
|
|
(content < 6) ? content_names[content] : "?", (int)content);
|
|
DBG_INIT(" Interface: %s (%d)\r\n",
|
|
(iface < 3) ? iface_names[iface] : "?", (int)iface);
|
|
DBG_INIT(" FPS: %s (%d)\r\n",
|
|
(fps < 4) ? fps_names[fps] : "?", (int)fps);
|
|
DBG_INIT(" ClkPhase: %s (%d)\r\n",
|
|
(clk_phase == 0) ? "Rising" : "Falling", (int)clk_phase);
|
|
}
|
|
|
|
int Mini212G2_ShutterCompensation(void)
|
|
{
|
|
return Mini212G2_SendCmd(CMD_SHUTTER, sizeof(CMD_SHUTTER));
|
|
}
|
|
#endif /* SENSOR_UART_ENABLE */
|