251 lines
7.6 KiB
C
251 lines
7.6 KiB
C
/**
|
|
* @file main.c
|
|
* @brief Excuting commands, resample data, triggle camera, and send data to valve
|
|
* @author miaow (3703781@qq.com)
|
|
* @version 1.1
|
|
* @date 2023/02/08
|
|
*
|
|
* @copyright Copyright (c) 2022 miaow
|
|
*
|
|
* @par Changelog:
|
|
* <table>
|
|
* <tr><th>Date <th>Version <th>Author <th>Description
|
|
* <tr><td>2022/01/16 <td>1.0 <td>miaow <td>Write this file
|
|
* <tr><td>2023/02/08 <td>1.1 <td>miaow <td>Add debug option for interval of camera triggle
|
|
* </table>
|
|
*/
|
|
#include <valve.h>
|
|
#include <sys/socket.h>
|
|
#include <arpa/inet.h>
|
|
#include <sys/time.h>
|
|
#include <math.h>
|
|
#include <main.h>
|
|
#include <queue_uint64.h>
|
|
#include <camera_trigger.h>
|
|
#include <encoder.h>
|
|
#include <host_computer.h>
|
|
|
|
/**
|
|
* @brief Value of state machine
|
|
*/
|
|
typedef enum
|
|
{
|
|
SLEEPING = 0,
|
|
RUNNING = 1,
|
|
} status_enum_t;
|
|
|
|
valvedata_t valvedata = {0};
|
|
queue_uint64_msg_t data_queue = {0};
|
|
queue_uint64_msg_t cmd_queue = {0};
|
|
|
|
static int count_valve = 1, count_camera = 0, count_valve_should_be = 2;
|
|
static uint64_t count_continues = 0UL, count_valve_continues = 0UL, count_camera_continues = 0UL;
|
|
static status_enum_t status = SLEEPING;
|
|
static int camera_trigger_pulse_count = 500;
|
|
static int valve_should_trigger_pulse_count = 1;
|
|
static int valve_trigger_pulse_count = 10;
|
|
static int camera_to_valve_pulse_count = 3015;
|
|
#if defined(DEBUG_CAMERA_TRIG_PERIOD)
|
|
static struct timeval tv;
|
|
static uint64_t camera_period_interval_last_us = 0UL;
|
|
static uint64_t camera_period_interval_us = 0UL;
|
|
#endif
|
|
|
|
#define ROTATE_UINT64_RIGHT(x, n) ((x) >> (n)) | ((x) << ((64) - (n)))
|
|
#define ROTATE_UINT64_LEFT(x, n) ((x) << (n)) | ((x) >> ((64) - (n)))
|
|
|
|
void on_encoder(void);
|
|
void valve_test(float ms_for_each_channel);
|
|
void valve_test2(float ms_for_each_channel, int which_channel);
|
|
void valve_test3(float ms_for_each_channel);
|
|
void process_cmd(uint64_t *cmd);
|
|
|
|
int main(int argc, char *argv[])
|
|
{
|
|
queue_uint64_init(&data_queue, 99999);
|
|
queue_uint64_init(&cmd_queue, 99999);
|
|
|
|
// valve_init();
|
|
// printf("testing valve.....");
|
|
// fflush(stdout);
|
|
// valve_test3(100.0f);
|
|
// valve_test2(200.0f, 0);
|
|
// for (int i = 0; i < 999; i++)
|
|
// {
|
|
// valve_test(200.0f);
|
|
// }
|
|
// printf("OK\r\n");
|
|
// valve_deinit();
|
|
|
|
hostcomputer_init(&data_queue, &cmd_queue);
|
|
printf("\r\n>>>>>\r\nstatus==SLEEPING\r\n<<<<<\r\n\r\n");
|
|
uint64_t cmd;
|
|
int TRUE = 1;
|
|
while (TRUE)
|
|
{
|
|
if (queue_uint64_get(&cmd_queue, &cmd) == 0)
|
|
{
|
|
process_cmd(&cmd);
|
|
}
|
|
usleep(100000);
|
|
}
|
|
hostcomputer_deinit();
|
|
queue_uint64_deinit(&data_queue);
|
|
queue_uint64_deinit(&cmd_queue);
|
|
return 0;
|
|
}
|
|
|
|
void process_cmd(uint64_t *cmd)
|
|
{
|
|
int tmp_cmd = (int)*cmd;
|
|
int tmp_data = (int)(*cmd >> 32);
|
|
if (status == SLEEPING)
|
|
{
|
|
if (tmp_cmd == HOSTCOMPUTER_CMD_START)
|
|
{
|
|
queue_uint64_clear(&data_queue);
|
|
valve_should_trigger_pulse_count = camera_trigger_pulse_count / HOST_COMPUTER_PICTURE_ROW_NUM;
|
|
for (int i = 0; i < camera_to_valve_pulse_count * HOST_COMPUTER_PICTURE_ROW_NUM / camera_trigger_pulse_count; i++)
|
|
queue_uint64_put(&data_queue, 0L);
|
|
|
|
valve_init();
|
|
cameratrigger_init();
|
|
encoder_init(on_encoder);
|
|
printf("\r\n>>>>>\r\nstatus==RUNNING\r\ncamera_trigger_pulse_count=%d\r\nvalve_trigger_pulse_count=%d\r\ncamera_to_valve_pulse_count=%d\r\n<<<<<\r\n\r\n", camera_trigger_pulse_count, valve_trigger_pulse_count, camera_to_valve_pulse_count);
|
|
status = RUNNING;
|
|
}
|
|
else if (tmp_cmd == HOSTCOMPUTER_CMD_TEST)
|
|
{
|
|
valve_init();
|
|
valve_test(500.0f);
|
|
valve_deinit();
|
|
}
|
|
else if (tmp_cmd == HOSTCOMPUTER_CMD_SETCAMERATRIGPULSECOUNT)
|
|
{
|
|
camera_trigger_pulse_count = tmp_data;
|
|
}
|
|
else if (tmp_cmd == HOSTCOMPUTER_CMD_SETVALVETRIGPULSECOUNT)
|
|
{
|
|
valve_trigger_pulse_count = tmp_data;
|
|
}
|
|
else if (tmp_cmd == HOSTCOMPUTER_CMD_SETCAMERATOVALVEPULSECOUNT)
|
|
{
|
|
camera_to_valve_pulse_count = tmp_data;
|
|
}
|
|
}
|
|
else if (status == RUNNING)
|
|
{
|
|
if (tmp_cmd == HOSTCOMPUTER_CMD_STOP)
|
|
{
|
|
encoder_deinit();
|
|
cameratrigger_deinit();
|
|
valve_deinit();
|
|
queue_uint64_clear(&data_queue);
|
|
#if defined(DEBUG_CAMERA_TRIG_PERIOD)
|
|
printf("\r\n>>>>>\r\nstatus==SLEEPING\r\ncamera_period_us=%.2f\r\n<<<<<\r\n\r\n", (float)camera_period_interval_us / (float)(count_camera_continues - 1));
|
|
camera_period_interval_us = 0UL;
|
|
camera_period_interval_last_us = 0UL;
|
|
#else
|
|
printf("\r\n>>>>>\r\nstatus==SLEEPING\r\n<<<<<\r\n\r\n");
|
|
#endif
|
|
count_continues = 0UL;
|
|
count_valve_continues = 0UL;
|
|
count_camera_continues = 0UL;
|
|
count_camera = 0;
|
|
count_valve = 1;
|
|
count_valve_should_be = 2;
|
|
|
|
status = SLEEPING;
|
|
}
|
|
}
|
|
}
|
|
|
|
void valve_test(float ms_for_each_channel)
|
|
{
|
|
uint64_t valve_data = 1ul;
|
|
|
|
for (int i = 0; i < HOST_COMPUTER_PICTURE_COLUMN_NUM; i++)
|
|
{
|
|
usleep((useconds_t)(ms_for_each_channel * 500.0f));
|
|
valvedata.valvedata_1 = valve_data << i;
|
|
valve_sendmsg(&valvedata);
|
|
usleep((useconds_t)(ms_for_each_channel * 500.0f));
|
|
valvedata.valvedata_1 = 0;
|
|
valve_sendmsg(&valvedata);
|
|
}
|
|
}
|
|
|
|
void valve_test2(float ms_for_each_channel, int which_channel)
|
|
{
|
|
uint64_t valve_data = 1ul;
|
|
for (int i = 0; i < 10; i++)
|
|
{
|
|
usleep((useconds_t)(ms_for_each_channel * 500.0f));
|
|
valvedata.valvedata_1 = valve_data << which_channel;
|
|
valve_sendmsg(&valvedata);
|
|
usleep((useconds_t)(ms_for_each_channel * 500.0f));
|
|
valvedata.valvedata_1 = 0;
|
|
valve_sendmsg(&valvedata);
|
|
}
|
|
}
|
|
|
|
void valve_test3(float ms_for_each_channel)
|
|
{
|
|
valvedata.valvedata_1 = 0x5555555555555555ul;
|
|
for (int i = 0; i < 9999; i++)
|
|
{
|
|
usleep((useconds_t)(ms_for_each_channel * 250.0f));
|
|
valvedata.valvedata_1 = 0x5555555555555555ul;
|
|
valve_sendmsg(&valvedata);
|
|
usleep((useconds_t)(ms_for_each_channel * 250.0f));
|
|
|
|
valvedata.valvedata_1 = 0;
|
|
valve_sendmsg(&valvedata);
|
|
usleep((useconds_t)(ms_for_each_channel * 250.0f));
|
|
|
|
valvedata.valvedata_1 = 0xaaaaaaaaaaaaaaaaul;
|
|
valve_sendmsg(&valvedata);
|
|
usleep((useconds_t)(ms_for_each_channel * 250.0f));
|
|
|
|
valvedata.valvedata_1 = 0;
|
|
valve_sendmsg(&valvedata);
|
|
usleep((useconds_t)(ms_for_each_channel * 250.0f));
|
|
}
|
|
}
|
|
|
|
void on_encoder()
|
|
{
|
|
count_continues++;
|
|
|
|
// send resampled data to valve, the resample cycle is valve_trigger_pulse_count
|
|
if (++count_valve == valve_trigger_pulse_count + 1)
|
|
{
|
|
count_valve = 1;
|
|
count_valve_continues++;
|
|
valve_sendmsg(&valvedata);
|
|
}
|
|
|
|
// load valve data to valvedata structure
|
|
if (++count_valve_should_be == valve_should_trigger_pulse_count + 2)
|
|
{
|
|
count_valve_should_be = 2;
|
|
valvedata.valvedata_1 = 0;
|
|
queue_uint64_get(&data_queue, &(valvedata.valvedata_1));
|
|
}
|
|
|
|
// triggle camera in a cycle of camera_trigger_pulse_count
|
|
if (++count_camera == camera_trigger_pulse_count)
|
|
{
|
|
count_camera = 0;
|
|
count_camera_continues++;
|
|
#if defined(DEBUG_CAMERA_TRIG_PERIOD)
|
|
gettimeofday(&tv, NULL);
|
|
if (camera_period_interval_last_us != 0UL)
|
|
camera_period_interval_us += ((uint64_t)tv.tv_sec * 1000000 + (uint64_t)tv.tv_usec) - camera_period_interval_last_us;
|
|
camera_period_interval_last_us = ((uint64_t)tv.tv_sec * 1000000 + (uint64_t)tv.tv_usec);
|
|
#endif
|
|
|
|
cameratrigger_trig();
|
|
}
|
|
}
|