diff --git a/src/librf625.cpp b/src/librf625.cpp new file mode 100644 index 0000000000000000000000000000000000000000..55a0b0aaedf0f624bdaeb00b4d021af0d2fa0dd2 --- /dev/null +++ b/src/librf625.cpp @@ -0,0 +1,940 @@ +/* + * File name: librf034.cpp + * + * Description: Interface library for Riftek RF034 + * + * Updated: 2018/01/16 + */ +#include "librf625.h" +#include +#include +#include +/* windows sockets tweaks */ +#ifdef _WIN32 +extern BOOL WinSockInit(); +extern void WinSockDeinit(); +extern BOOL EnumAdapterAddresses(); +extern void FreeAdapterAddresses(); +extern BOOL BindToCompatibleInterface(SOCKET s, in_addr* addr, u_long netMask); +extern u_long GetCompatibleInterface(u_long RemoteIP, u_long SubnetMask); +typedef int socklen_t; +/* !windows sockets tweaks */ +#else +#include +#include +#include +#define SOCKET_ERROR (-1) +#endif +#ifdef TEST +#include +#endif + +#define CMD_GetResult 0x01 +#define CMD_GetImage 0x02 +#define CMD_GetImageBuffer 0x03 +#define CMD_ReadParams 0x04 +#define CMD_WriteParams 0x05 +#define CMD_FlushParams 0x06 +#define CMD_Reboot 0x14 +#define CMD_Shutdown 0x16 +#define CMD_Disconnect 0x19 +#define CMD_Identify 0x21 +#define DEFAULT_UDP_RECVTIMEO 250 +#define DEFAULT_TCP_RECVTIMEO 4000 + +static std::mutex _mx; +static const size_t _user_cfg_area_size = 512; +static const int _tr_delay = 0; +static const size_t _buf_size = 65536; +static int _ninstances = 0; + +extern bool send_cmd_packet(SOCKET s, uint32_t cmd, uint32_t attach_size = 0, uint32_t offset = 0, uint32_t size = 0, bool reply = false); +extern bool recv_data(SOCKET s, char* buf, size_t nbytes, bool accept_incomplete_packet = false, size_t* packet_size = nullptr); +extern bool send_data(SOCKET s, const char* buf, size_t nbytes); +extern bool udp_recv_datagram(SOCKET s, char* buf, size_t max_length, size_t* datagram_size, struct sockaddr_in* from_addr = nullptr); +extern bool set_sock_recvtimeo(SOCKET s, int msec); + +bool rf625::init() +{ + bool res = true; +#ifdef _WIN32 + if (!WinSockInit()) { + res = false; + } + EnumAdapterAddresses(); +#endif + return res; +} + +void rf625::cleanup() +{ +#ifdef _WIN32 + FreeAdapterAddresses(); + WinSockDeinit(); +#endif +} + +rf625_info_list rf625::search(int ntime) +{ + const size_t buf_size = 268; + const int recv_timeo = 250; + const u_short port_number = 6001; + const uint16_t device_type = 625; + SOCKET s; + struct sockaddr_in recv_addr; + struct sockaddr_in from_addr; + socklen_t from_len; + rf625_info_list res; + struct timeb start_t, now_t; + int nret; + register int i; + char* buf = nullptr; + s = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); + if (s == SOCKET_ERROR) { + return res; + } + set_sock_recvtimeo(s, recv_timeo); + recv_addr.sin_family = AF_INET; + recv_addr.sin_port = htons(port_number); + recv_addr.sin_addr.s_addr = htonl(INADDR_ANY); + nret = bind(s, reinterpret_cast(&recv_addr), sizeof(recv_addr)); + if (nret == SOCKET_ERROR) { +#ifdef _WIN32 + nret = WSAGetLastError(); + if (nret == WSAEADDRINUSE) { +#endif + nret = 1; + setsockopt(s, SOL_SOCKET, SO_REUSEADDR, reinterpret_cast(&nret), sizeof(nret)); +#ifdef _WIN32 + } +#endif + nret = bind(s, reinterpret_cast(&recv_addr), sizeof(recv_addr)); + if (nret == SOCKET_ERROR) { + return res; + } + } + buf = new char[buf_size]; + ftime(&start_t); + do + { + from_len = sizeof(from_addr); + nret = recvfrom(s, buf, buf_size, 0, reinterpret_cast(&from_addr), &from_len); + if (nret == buf_size) { + if (!memcmp(&buf[0], &device_type, sizeof(device_type))) { + rf625_info info; + memset(&info, 0, sizeof(info)); + memcpy(&info.serial_number, &buf[12+1], 3); + for (i=0; i<=res.size(); i++) { + if (i(&nret), sizeof(nret)); + sockaddr_in sin; + sin.sin_family = AF_INET; +#ifdef _WIN32 + sin.sin_addr.s_addr = GetCompatibleInterface(info.ip_address, info.subnet_mask); +#else + sin.sin_addr.s_addr = INADDR_ANY; +#endif + sin.sin_port = htons(info.udp_port); + nret = bind(m_usock, reinterpret_cast(&sin), sizeof(sin)); + if (nret == SOCKET_ERROR) { +#ifdef _WIN32 + closesocket(m_usock); +#else + close(m_usock); +#endif + m_usock = INVALID_SOCKET; + } + } +} + +rf625::rf625(const rf625 &other) +{ + m_sock = other.m_sock; + m_usock = other.m_usock; + memcpy(&m_info, &other.m_info, sizeof(m_info)); + m_user_cfg_area = new uint8_t[_user_cfg_area_size]; + memcpy(m_user_cfg_area, other.m_user_cfg_area, _user_cfg_area_size); + m_buf = new uint8_t[_buf_size]; + memset(m_buf, 0, _buf_size); + memcpy(&m_params, &other.m_params, sizeof(m_params)); +} + +rf625::~rf625() +{ + if (is_connected()) { + disconnect(); + } + if (m_usock != INVALID_SOCKET) { +#ifdef _WIN32 + closesocket(m_usock); +#else + close(m_usock); +#endif + } + delete[] m_user_cfg_area; + delete[] m_buf; +} + +bool rf625::connect() +{ + const int recv_timeo = DEFAULT_TCP_RECVTIMEO; + int nret; + if (!m_info.ip_address || !m_info.tcp_port) { + return false; + } + if (is_connected()) { + return false; + } + m_sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); + if (m_sock == SOCKET_ERROR) { + return false; + } + set_sock_recvtimeo(m_sock, recv_timeo); + nret = 1048576; + setsockopt(m_sock, SOL_SOCKET, SO_RCVBUF, reinterpret_cast(&nret), sizeof(nret)); +#ifdef _WIN32 + nret = 1; + setsockopt(m_sock, IPPROTO_TCP, TCP_NODELAY, reinterpret_cast(&nret), sizeof(nret)); +#endif + sockaddr_in sin; + sin.sin_family = AF_INET; + sin.sin_addr.s_addr = m_info.ip_address; + sin.sin_port = htons(m_info.tcp_port); +#ifdef _WIN32 + BindToCompatibleInterface(m_sock, &sin.sin_addr, m_info.subnet_mask); +#endif + nret = ::connect(m_sock, reinterpret_cast(&sin), sizeof(sin)); + if (nret == SOCKET_ERROR) { + return false; + } + _mx.lock(); + if (!send_cmd_packet(m_sock, CMD_Identify, 0, RF625_API_VERSION)) { + _mx.unlock(); + disconnect(); + return false; + } + read_params(m_params); + _mx.unlock(); + return true; +} + +bool rf625::disconnect() +{ + if (!is_connected()) { + return false; + } + _mx.lock(); + if (!send_cmd_packet(m_sock, CMD_Disconnect)) { + _mx.unlock(); + return false; + } + _mx.unlock(); +#ifdef _WIN32 + shutdown(m_sock, 0x02); /* SD_BOTH */ + closesocket(m_sock); +#else + close(m_sock); +#endif + m_sock = INVALID_SOCKET; + return true; +} + +bool rf625::read_params(const rf625_parameters ¶ms) +{ + uint16_t v; + if (is_connected()) { + return false; + } + if (!m_user_cfg_area) { // jic + return false; + } + _mx.lock(); + if (!send_cmd_packet(m_sock, CMD_ReadParams)) { + _mx.unlock(); + return false; + } + if (!recv_data(m_sock, reinterpret_cast(m_user_cfg_area), _user_cfg_area_size)) { + return false; + } + _mx.unlock(); + m_params.laser_enable = (m_user_cfg_area[68] != 0); + m_params.laser_level = m_user_cfg_area[2]; + memcpy(&m_params.exposure_time, &m_user_cfg_area[3], 2); + memcpy(&m_params.roi_top, &m_user_cfg_area[5], 2); + memcpy(&m_params.roi_height, &m_user_cfg_area[7], 2); + memcpy(&v, &m_user_cfg_area[9], 2); + switch (v) { + case 0: + m_params.trigger = max_frequency; break; + case 1: + m_params.trigger = custom_frequency; break; + case 2: + m_params.trigger = external_input; break; + case 3: + m_params.trigger = encoder; break; + case 4: + m_params.trigger = step_dir; break; + default: + break; + } + memcpy(&m_params.ext_sync_divisor, &m_user_cfg_area[11], 2); + m_params.ip_address = static_cast(m_user_cfg_area[16]) + | (static_cast(m_user_cfg_area[15]) << 8) + | (static_cast(m_user_cfg_area[14]) << 16) + | (static_cast(m_user_cfg_area[13]) << 24); + m_params.subnet_mask = static_cast(m_user_cfg_area[20]) + | (static_cast(m_user_cfg_area[19]) << 8) + | (static_cast(m_user_cfg_area[18]) << 16) + | (static_cast(m_user_cfg_area[17]) << 24); + m_params.udp_host_address = static_cast(m_user_cfg_area[24]) + | (static_cast(m_user_cfg_area[23]) << 8) + | (static_cast(m_user_cfg_area[22]) << 16) + | (static_cast(m_user_cfg_area[21]) << 24); + memcpy(&m_params.udp_port, &m_user_cfg_area[25], 2); + memcpy(&m_params.udp_frequency, &m_user_cfg_area[27], 2); + memcpy(&m_params.tcp_port, &m_user_cfg_area[29], 2); + m_params.pixel_brightness_thres = m_user_cfg_area[32]; + m_params.diff_brightness_thres = m_user_cfg_area[33]; + m_params.raw_image = (m_user_cfg_area[34] != 0); + switch (m_user_cfg_area[35]) { + case 0: + m_params.resolution = res_80; break; + case 1: + m_params.resolution = res_160; break; + case 2: + m_params.resolution = res_320; break; + case 3: + m_params.resolution = res_640; break; + case 4: + m_params.resolution = res_1280; break; + default: + break; + } + m_params.dhs_enable = (m_user_cfg_area[36] != 0); + memcpy(&v, &m_user_cfg_area[38], 2); + if ((v & 0x000F) == 0x00) m_params.sync_input_channel = 0; + else if ((v & 0x000F) == 0x01) m_params.sync_input_channel = 1; + else if ((v & 0x000F) == 0x02) m_params.sync_input_channel = 2; + else if ((v & 0x000F) == 0x04) m_params.sync_input_channel = 3; + else if ((v & 0x000F) == 0x08) m_params.sync_input_channel = 4; + m_params.measure_sync_enable = (m_user_cfg_area[40] != 0); + if ((v & 0x0F00) == 0x0000) m_params.measurement_sync_input_channel = 0; + else if ((v & 0x0F00) == 0x0100) m_params.measurement_sync_input_channel = 1; + else if ((v & 0x0F00) == 0x0200) m_params.measurement_sync_input_channel = 2; + else if ((v & 0x0F00) == 0x0400) m_params.measurement_sync_input_channel = 3; + else if ((v & 0x0F00) == 0x0800) m_params.measurement_sync_input_channel = 4; + if ((v & 0x00F0) == 0x0000) m_params.output_channel = 0; + else if ((v & 0x00F0) == 0x0010) m_params.output_channel = 1; + else if ((v & 0x00F0) == 0x0020) m_params.output_channel = 2; + m_params.tcp_autodisconnect_enable = (m_user_cfg_area[46] != 0); + memcpy(&m_params.tcp_autodisconnect_timeout, &m_user_cfg_area[44], 2); + m_params.udp_stream_enable = (m_user_cfg_area[55] != 0); + m_params.encoder_has_zero = (m_user_cfg_area[67] != 0); + m_params.bracketing_enable = (m_user_cfg_area[69] != 0); + memcpy(&m_params.bracketing_exposure_0, &m_user_cfg_area[70], 2); + memcpy(&m_params.bracketing_exposure_1, &m_user_cfg_area[72], 2); + m_params.fixed_frame_enable = (m_user_cfg_area[75] != 0); + if (¶ms != &m_params) { + memcpy(const_cast(¶ms), &m_params, sizeof(params)); + } + return true; +} + +bool rf625::validate_params(const rf625_parameters ¶ms, int *error_index) +{ + if (params.exposure_time > 3824) { + if (error_index) *error_index = 2; return false; + } + if (params.roi_top % 32 || params.roi_top > 448) { + if (error_index) *error_index = 3; return false; + } + if (params.roi_height % 32 || params.roi_height < 32 || params.roi_height > (480-params.roi_top)) { + if (error_index) *error_index = 4; return false; + } + if (params.trigger < max_frequency || params.trigger > step_dir) { + if (error_index) *error_index = 5; return false; + } + //TODO + return true; +} + +bool rf625::write_params(const rf625_parameters ¶ms) +{ + uint16_t v; + if (is_connected()) { + return false; + } + if (!m_user_cfg_area) { // jic + return false; + } + if (¶ms != &m_params) { + memcpy(&m_params, const_cast(¶ms), sizeof(params)); + } + m_user_cfg_area[68] = m_params.laser_enable; + m_user_cfg_area[2] = m_params.laser_level; + memcpy(&m_user_cfg_area[3], &m_params.exposure_time, 2); + memcpy(&m_user_cfg_area[5], &m_params.roi_top, 2); + memcpy(&m_user_cfg_area[7], &m_params.roi_height, 2); + switch (m_params.trigger) { + case max_frequency: + v=0; break; + case custom_frequency: + v=1; break; + case external_input: + v=2; break; + case encoder: + v=3; break; + case step_dir: + v=4; break; + default: + break; + } + memcpy(&m_user_cfg_area[9], &v, 2); + memcpy(&m_user_cfg_area[11], &m_params.ext_sync_divisor, 2); + m_user_cfg_area[16] = m_params.ip_address & 0xFF; + m_user_cfg_area[15] = (m_params.ip_address >> 8) & 0xFF; + m_user_cfg_area[14] = (m_params.ip_address >> 16) & 0xFF; + m_user_cfg_area[13] = (m_params.ip_address >> 24) & 0xFF; + m_user_cfg_area[20] = m_params.subnet_mask & 0xFF; + m_user_cfg_area[19] = (m_params.subnet_mask >> 8) & 0xFF; + m_user_cfg_area[18] = (m_params.subnet_mask >> 16) & 0xFF; + m_user_cfg_area[17] = (m_params.subnet_mask >> 24) & 0xFF; + m_user_cfg_area[24] = m_params.udp_host_address & 0xFF; + m_user_cfg_area[23] = (m_params.udp_host_address >> 8) & 0xFF; + m_user_cfg_area[22] = (m_params.udp_host_address >> 16) & 0xFF; + m_user_cfg_area[21] = (m_params.udp_host_address >> 24) & 0xFF; + memcpy(&m_user_cfg_area[25], &m_params.udp_port, 2); + memcpy(&m_user_cfg_area[27], &m_params.udp_frequency, 2); + memcpy(&m_user_cfg_area[29], &m_params.tcp_port, 2); + m_user_cfg_area[32] = m_params.pixel_brightness_thres; + m_user_cfg_area[33] = m_params.diff_brightness_thres; + m_user_cfg_area[34] = m_params.raw_image; + switch (m_params.resolution) { + case res_80: + m_user_cfg_area[35] = 0; break; + case res_160: + m_user_cfg_area[35] = 1; break; + case res_320: + m_user_cfg_area[35] = 2; break; + case res_640: + m_user_cfg_area[35] = 3; break; + case res_1280: + m_user_cfg_area[35] = 4; break; + default: + break; + } + m_user_cfg_area[36] = m_params.dhs_enable; + memcpy(&v, &m_user_cfg_area[38], 2); + v &= 0xF000; + v |= (0x0001 << m_params.sync_input_channel); + v |= (0x0100 << m_params.measurement_sync_input_channel); + v |= (0x0010 << m_params.output_channel); + memcpy(&m_user_cfg_area[38], &v, 2); + m_user_cfg_area[46] = m_params.tcp_autodisconnect_enable; + memcpy(&m_user_cfg_area[44], &m_params.tcp_autodisconnect_timeout, 2); + m_user_cfg_area[55] = m_params.udp_stream_enable; + m_user_cfg_area[67] = m_params.encoder_has_zero; + m_user_cfg_area[69] = m_params.bracketing_enable; + memcpy(&m_user_cfg_area[70], &m_params.bracketing_exposure_0, 2); + memcpy(&m_user_cfg_area[72], &m_params.bracketing_exposure_1, 2); + m_user_cfg_area[75] = m_params.fixed_frame_enable; + _mx.lock(); + if (!send_cmd_packet(m_sock, CMD_WriteParams, _user_cfg_area_size, 1, 0)) { + _mx.unlock(); + return false; + } + if (!send_data(m_sock, reinterpret_cast(m_user_cfg_area), _user_cfg_area_size)) { + _mx.unlock(); + return false; + } + uint16_t rx; + if (!recv_data(m_sock, reinterpret_cast(&rx), 2)) { + _mx.unlock(); + return false; + } + _mx.unlock(); + return (rx == 0x4B4F); +} + +bool rf625::flush_params() +{ + if (is_connected()) { + return false; + } + _mx.lock(); + if (!send_cmd_packet(m_sock, CMD_FlushParams, 0, 0, 0, true)) { + _mx.unlock(); + return false; + } + _mx.unlock(); + return true; +} + +bool rf625::reset_params() +{ + if (is_connected()) { + return false; + } + _mx.lock(); + if (!send_cmd_packet(m_sock, CMD_FlushParams, 0, 1, 0, true)) { + _mx.unlock(); + return false; + } + _mx.unlock(); + return true; +} + +/*! One function for both TCP and UDP packets + * \return true on valid packets + * \param buf : a ptr to packet's data buffer + * \param buf_size : data buffer size + * \param profile : a ref to output ref625_profile struct + */ +static bool process_result(const uint8_t* buf, size_t buf_size, rf625_info& info, rf625_profile &profile) +{ + uint8_t proto_revision = 0; + uint16_t npts, nextlen, crc; + const uint8_t* pext = nullptr; + register int i; + profile.serial_number = 0; + profile.x_emr = 0; + profile.z_range = 0; + profile.source_ip = 0; + profile.source_port = 0; + profile.destination_port = 0; + profile.points.clear(); + if (!buf || !buf_size) { // jic + return false; + } + if (buf[9] == 0xFF) { + proto_revision = buf[8]; + } + if (proto_revision < 2) { + return false; // we don't support fossil formats anymore + } + memcpy(&profile.measure_cnt, &buf[0], 2); + memcpy(&profile.packet_cnt, &buf[2], 2); + memcpy(&profile.timestamp, &buf[4], 4); + memcpy(&npts, &buf[10], 2); + if (buf_size < 12 + npts*4 + 2) { + return false; + } + memcpy(&nextlen, &buf[12 + npts*4], 2); + if ((nextlen > 512) || (buf_size < 12 + npts*4 + 2 + nextlen + 2)) { + return false; + } + memcpy(&crc, &buf[12 + npts*4 + 2 + nextlen], 2); + if (nextlen > 0) { + pext = &buf[12 + npts*4 + 2]; + switch (pext[0]) { + case 0x01: // UDP extended record + if (nextlen >= 8) { + memcpy(&profile.serial_number, &pext[1], 3); + memcpy(&profile.x_emr, &pext[4], 2); + memcpy(&profile.z_range, &pext[6], 2); + if (nextlen >= 10) { + memcpy(&profile.dir, &pext[8], 2); + } + } + break; + case 0xFF: // No data; no fail with empty result (FIXME?) + return true; + } + } + for (i=0; i(x) * info.x_emr / 16384.0; + } + else { + pt.x = static_cast(x) * profile.x_emr / 16384.0; + } + if (!profile.z_range) { + pt.z = static_cast(z) * info.z_range / 16384.0; + } + else { + pt.z = static_cast(z) * profile.z_range / 16384.0; + } + profile.points.push_back(pt); + } + profile.source_ip = info.ip_address; + profile.source_port = info.tcp_port; + return true; +} + +bool rf625::get_result(rf625_profile &profile) +{ + const size_t max_packet_size = 10768; + size_t nrecv; + if (is_connected()) { + return false; + } + _mx.lock(); + if (!send_cmd_packet(m_sock, CMD_GetResult)) { + _mx.unlock(); + return false; + } + if (!recv_data(m_sock, reinterpret_cast(m_buf), max_packet_size, true, &nrecv)) { + _mx.unlock(); + return false; + } + _mx.unlock(); + if (!process_result(m_buf, nrecv, m_info, profile)) { + return false; + } + return true; +} + +bool rf625::get_image(uint8_t *ppixmap) +{ + const size_t image_size = 640 * 480; + const size_t frame_size = 32768 - 2; + const uint8_t nframes = image_size / frame_size + 1; + uint8_t nframe = 0xFF; + uint32_t offset = 0; + uint32_t chunk_size; + uint8_t nimage; + uint8_t nreq = 0; + if (is_connected()) { + return false; + } + if (!ppixmap) { + return false; + } + memset(ppixmap, 0, image_size); + uint8_t* pframe = new uint8_t[frame_size + 2]; + if (!send_cmd_packet(m_sock, CMD_GetImage)) { + delete[] pframe; + return false; + } + _mx.lock(); + while (offset < image_size) + { + if ((offset + frame_size) > image_size) + chunk_size = image_size - offset; + else + chunk_size = frame_size; + if (!send_cmd_packet(m_sock, CMD_GetImageBuffer, 0, offset, chunk_size)) { + delete[] pframe; + _mx.unlock(); + return false; + } + if (!recv_data(m_sock, reinterpret_cast(pframe), chunk_size + 2)) + { + delete[] pframe; + _mx.unlock(); + return false; + } + if (nframe == 0xFF) { + nimage = pframe[0]; + } + if (nimage == pframe[0]) + { + nframe = pframe[1]; + if (nframe < nframes) + { + memcpy(&ppixmap[static_cast(nframe) * frame_size], &pframe[2], chunk_size); + } + else { + delete[] pframe; + _mx.unlock(); + return false; + } + } + offset += chunk_size; + if (++nreq > (nframes * 2)) { + delete[] pframe; + _mx.unlock(); + return false; + } + } + _mx.unlock(); + delete[] pframe; + return true; +} + +bool rf625::reboot() +{ + if (is_connected()) { + return false; + } + _mx.lock(); + if (!send_cmd_packet(m_sock, CMD_Reboot)) { + _mx.unlock(); + return false; + } + _mx.unlock(); + return true; +} + +bool rf625::power_off() +{ + if (is_connected()) { + return false; + } + _mx.lock(); + if (!send_cmd_packet(m_sock, CMD_Shutdown)) { + _mx.unlock(); + return false; + } + _mx.unlock(); + return true; +} + +bool rf625::udp_set_timeout(int msec) +{ + int nret; + nret = set_sock_recvtimeo(m_usock, msec); + return (nret != SOCKET_ERROR); +} + +bool rf625::udp_get_result(rf625_profile &profile) +{ + struct sockaddr_in from_addr; + const size_t max_packet_size = 10768; + size_t nrecv; + if (!udp_recv_datagram(m_usock, reinterpret_cast(m_buf), max_packet_size, &nrecv, &from_addr)) { + return false; + } + profile.source_ip = ntohl(from_addr.sin_addr.s_addr); + profile.source_port = ntohs(from_addr.sin_port); + profile.destination_port = m_params.udp_port; + if (!process_result(m_buf, nrecv, m_info, profile)) { + return false; + } + return true; +} + +void rf625::udp_proc() +{ + rf625_profile profile; + std::mutex mx; + while(m_callback != nullptr) + { + if (udp_get_result(profile)) { + if (m_callback) + { + mx.lock(); + m_callback(profile); + mx.unlock(); + } + } + } +} + +void rf625::udp_register_callback(rf625_udp_proc proc) +{ + m_callback = proc; + m_thread = new std::thread(&rf625::udp_proc, this); +} + +void rf625::udp_unregister_callback() +{ + std::mutex mx; + if (m_callback) { + mx.lock(); + m_callback = nullptr; + m_thread->join(); + mx.unlock(); + delete m_thread; + m_thread = nullptr; + } +} + +#ifdef TEST +static bool t_rc = 0; +static bool t_assert(bool fn, const char* fn_name) { + if (!fn) { + std::cout << "Failed: " << fn_name << std::endl; + t_rc = 1; + } + else + std::cout << "OK: " << fn_name << std::endl; + return fn; +} + +void udpproc(rf625_profile profile) +{ + std::cout << "proc" << std::endl; + std::cout << "mc " << profile.measure_cnt << " pc " << profile.packet_cnt << " "; + std::cout << profile.points.size() << " points: ["; + for (register int j=0; j(profile.points.size()-3)) + std::cout << "(" << profile.points.at(j).x << "," << profile.points.at(j).z << ")"; + else if (j==2) + std::cout << "..."; + } + std::cout << "]" << std::endl; +} +void udpproc2(rf625_profile profile) +{ + std::cout << "proc2" << std::endl; + struct in_addr in; + in.s_addr = profile.source_ip; + std::cout << "sn " << profile.serial_number << " from " << inet_ntoa(in) << std::endl; +} + +int main(int argc, const char* argv[]) +{ + t_assert(rf625::init(), "init"); + std::cout << "Testing data on port 6003" << std::endl; + rf625_info just_udp_port; + memset(&just_udp_port, 0, sizeof(just_udp_port)); + just_udp_port.udp_port = 6003; + rf625 test_dev(just_udp_port); + rf625_profile profile; + for (register int i=0; i<110; i++) { + if (t_assert(test_dev.udp_get_result(profile), "udp_get_result")) { + std::cout << "sn " << profile.serial_number << " mc " << profile.measure_cnt << " pc " << profile.packet_cnt << " "; + std::cout << profile.points.size() << " points: ["; + for (register int j=0; j(profile.points.size()-3)) + std::cout << "(" << profile.points.at(j).x << "," << profile.points.at(j).z << ")"; + else if (j==2) + std::cout << "..."; + } + std::cout << "]" << std::endl; + } + } + std::cout << "Searching" << std::endl; + rf625_info_list lst = rf625::search(3000); + if (t_assert(lst.size() > 0, "search")) { + std::cout << "Found " << lst.size() << std::endl; + rf625 dev(lst.at(0)); + std::cout << "TCP test" << std::endl; + if (t_assert(dev.connect(), "connect")) { + rf625_parameters params; + if (t_assert(dev.read_params(params), "read_params")) { + std::cout << "Current trigger " << (int)params.trigger << std::endl; +// params.trigger = (params.trigger == max_frequency) ? external_input : max_frequency; +// if (t_assert(dev.write_params(params), "write_params")) { +// std::cout << "New trigger " << (int)params.trigger << std::endl; +// } + if (params.trigger == max_frequency) { + rf625_profile profile; + uint8_t* ppixmap = new uint8_t[640*480]; + for (register int i=0; i<110; i++) { + if (i<100) + { + if (t_assert(dev.get_result(profile), "get_result")) { + std::cout << "mc " << profile.measure_cnt << " pc " << profile.packet_cnt << " "; + std::cout << profile.points.size() << " points: ["; + for (register int j=0; j(profile.points.size()-3)) + std::cout << "(" << profile.points.at(j).x << "," << profile.points.at(j).z << ")"; + else if (j==2) + std::cout << "..."; + } + std::cout << "]" << std::endl; + } + } + else + { + if (t_assert(dev.get_image(ppixmap), "get_image")) { + //std::string file_name = "image_" + std::to_string(i) + ".bmp"; + } + } + } + delete[] ppixmap; + } + } + t_assert(dev.disconnect(), "disconnect"); + } + std::cout << "UDP test" << std::endl; + rf625_profile profile; + for (register int i=0; i<110; i++) { + if (t_assert(dev.udp_get_result(profile), "udp_get_result")) { + std::cout << "mc " << profile.measure_cnt << " pc " << profile.packet_cnt << " "; + std::cout << profile.points.size() << " points: ["; + for (register int j=0; j(profile.points.size()-3)) + std::cout << "(" << profile.points.at(j).x << "," << profile.points.at(j).z << ")"; + else if (j==2) + std::cout << "..."; + } + std::cout << "]" << std::endl; + } + } + std::cout << "Testing thread" << std::endl; + dev.udp_register_callback(udpproc); + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + dev.udp_register_callback(udpproc2); + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + dev.udp_unregister_callback(); + } + rf625::cleanup(); + std::cout << std::endl << "Finished, rc=" << t_rc << std::endl; + return 0; +} +#endif diff --git a/src/librf625.h b/src/librf625.h new file mode 100644 index 0000000000000000000000000000000000000000..7c5455d62bba9ed176b937fd115678bdfb712874 --- /dev/null +++ b/src/librf625.h @@ -0,0 +1,251 @@ +/* + * File name: librf625.h + * + * Description: Interface library for Riftek RF625 2D Laser Scanner + * + * Updated: 2018/01/18 + * + * Change history: + * + * 2018/01/18 + * Initial addition of UDP callbacks + * + */ +#pragma once + +#ifdef _WIN32 +#include +#else +#include +#include +#include +#define INVALID_SOCKET (-1) +typedef int SOCKET; +#endif +#include +#include +#include + +#define RF625_API_VERSION (0x00040000) + +#if (defined _WIN32 && defined RF625_LIBRARY && !defined TEST) +#define RF625_API __declspec(dllexport) +#else +#define RF625_API +#endif + +/*! Structure containing RF625 information + */ +enum rf625_laser_color: uint8_t +{ + red, + blue, + green, + infrared +}; +struct rf625_info +{ + uint8_t hardware_address[6]; //!< MAC Address + uint32_t ip_address; //!< IP Address + uint32_t subnet_mask; //!< Subnet mask + u_short tcp_port; //!< TCP port number + u_short udp_port; //!< UDP port number + bool dhs_capable; //!< Is scanner DHS capable + rf625_laser_color laser_color; //!< Laser beam color + uint32_t serial_number; //!< Serial number + uint16_t z_begin; //!< Begin of Z range, mm + uint16_t z_range; //!< Range of Z, mm + uint16_t x_smr; //!< Range of X (SMR), mm + uint16_t x_emr; //!< Range of X (EMR), mm + uint32_t uimage_version; //!< uImage version + uint32_t core_a_version; //!< Core A version + uint32_t core_b_version; //!< Core B version + uint32_t fpga_version; //!< FPGA version + uint32_t uptime; //!< System uptime, seconds + bool tcp_connection_active; //!< TCP connection is active? +}; +/*! Structure containing RF625 user parameters + */ +enum rf625_trigger +{ + max_frequency, + custom_frequency, + external_input, + encoder, + step_dir +}; +enum rf625_resolution +{ + res_80, + res_160, + res_320, + res_640, + res_1280 +}; +struct rf625_parameters +{ + bool laser_enable; //!< Enable laser trigger + uint8_t laser_level; //!< Laser level [0..255] + uint16_t exposure_time; //!< Exposure time, usec [0..3824] + uint16_t roi_top; //!< Top position of ROI window, px + uint16_t roi_height; //!< Height of ROI window, px + rf625_trigger trigger; //!< Trigger + uint16_t ext_sync_divisor; //!< External sync signal divisor (0 or 1 is off) + uint32_t ip_address; //!< IP address + uint32_t subnet_mask; //!< Subnet mask + uint32_t udp_host_address; //!< UDP host address + u_short udp_port; //!< UDP port number + uint16_t udp_frequency; //!< Frequency, Hz of UDP stream (when Trigger set to Custom freq.) + u_short tcp_port; //!< TCP port number + uint8_t pixel_brightness_thres; //!< Threshold level for pixel brightness filter + uint8_t diff_brightness_thres; //!< Threshold level for differential brightness filter + bool raw_image; //!< Raw (unprocessed) image mode + rf625_resolution resolution; //!< Measured profile resolution in points + bool dhs_enable; //!< Enable DHS mode + uint8_t sync_input_channel; //!< Input channel for external input (0 is off or [1..4]) + bool measure_sync_enable; //!< Enable measurement sync + uint8_t measurement_sync_input_channel; //!< Input channel for external input for measurement sync (0 is off or [1..4]) + uint16_t measurement_sync_delay; //!< Measurement sync delay, usec + uint8_t output_channel; //!< Output channel (0 is off or [1..2]) + bool tcp_autodisconnect_enable; //!< Enable autodisconnection of inactive TCP session + uint16_t tcp_autodisconnect_timeout; //!< Timeout to autodisconnet inactive TCP session, sec (0 is off) + bool udp_stream_enable; //!< Enable UDP stream + bool encoder_has_zero; //!< Encoder has zero? + bool bracketing_enable; //!< Enable bracketing mode (experimental) + uint16_t bracketing_exposure_0; //!< Exposure values for even frames, usec + uint16_t bracketing_exposure_1; //!< Exposure values for odd frames, usec + bool fixed_frame_enable; //!< Enable fixed frame mode +}; +/*! Structure to store a point of profile + */ +struct rf625_point +{ + double x; + double z; +}; +/*! Structure to store a profile + * NOTE: All entries next to 'dir' are used only when receiving result profile over UDP + */ +struct rf625_profile +{ + std::vector points; //!< Points of profile + uint16_t measure_cnt; //!< Measurement counter (or Step value in StepDir trigger mode) + uint16_t packet_cnt; //!< Packet counter + uint32_t timestamp; //!< Timestamp set upon packet send + uint16_t dir; //!< Dir value in StepDir trigger mode + uint32_t serial_number; //!< (UDP) Serial number of scanner + uint32_t source_ip; //!< (UDP) Source IP address + uint16_t source_port; //!< (UDP) Source port number + uint16_t destination_port; //!< (UDP) Destination port number + uint16_t x_emr; //!< (UDP) Range of X (EMR), mm + uint16_t z_range; //!< (UDP) Range of Z, mm +}; +typedef std::vector rf625_info_list; +typedef void (*rf625_udp_proc)(rf625_profile profile); +class RF625_API rf625 +{ +public: + /*! Must be called once before using of library + * \return true on success + */ + static bool init(); + /*! Should be called to free resources allocated by init() + */ + static void cleanup(); + /*! Search for instances of RF625 over network + * \param ntime : search time in milliseconds + * \return list of s_rf625_info structures for each of instances found + */ + static rf625_info_list search(int ntime); + /*! rf625 constructor + * \param info : rf625_info structure + */ + explicit rf625(const rf625_info& info); + rf625(const rf625& other); + ~rf625(); + /*! Start TCP connection to scanner + * \return true on success + */ + bool connect(); + /*! + * Close TCP connection to scanner + * \return + */ + bool disconnect(); + /*! Check connection state + * \return true if connected + */ + inline bool is_connected() const {return (m_sock != INVALID_SOCKET);} + /*! Read user parameters from scanner's memory + * \param params : a reference to rf625_params structure being filled + * \return true on success + */ + bool read_params(const rf625_parameters& params); + /*! Validate rf625_parameters structure + * \param params : a reference to rf625_params structure + * \param error_index: a pointer to int to store first invalid field (optional) + * \return true if all values are valid + */ + bool validate_params(const rf625_parameters& params, int* error_index = nullptr); + /*! Transfer user parameters to scanner's memory + * \param params : a reference to rf625_params structure to transfer + * \return true on success + */ + bool write_params(const rf625_parameters& params); + /*! Write user parameters from scanner's memory to flash + * \return true on success + */ + bool flush_params(); + /*! Reset user parameters to factory + * \return true on success + */ + bool reset_params(); + /*! Get result profile by TCP request + * \param profile : a reference to result rf625_profile structure + * \return true on success + */ + bool get_result(rf625_profile& profile); + /*! + * \param ppixmap : pointer to pixmap array (640 * 480 bytes, 8 bit per pixel) + * \return true on success + */ + bool get_image(uint8_t* ppixmap); + /*! Reboot scanner + * \return true on success + */ + bool reboot(); + /*! Power off scanner + * \return true on success + */ + bool power_off(); + /*! Override UDP read timeout (default is 250) + * \param msec : timeout in milliseconds + * \return + */ + bool udp_set_timeout(int msec); + /*! Get result profile from scanner's UDP stream + * \param profile : a reference to result rf625_profile structure + * \return true on success + */ + bool udp_get_result(rf625_profile& profile); + /*! Register udp callback function and start udp streaming in background + * \param proc : ptr to callback function + * callback function must be defined as void fn(rf625_profile profile) + */ + void udp_register_callback(rf625_udp_proc proc); + /*! Unregister udp callback function and stop background thread + */ + void udp_unregister_callback(); +private: + int m_id; + SOCKET m_sock, + m_usock; + rf625_info m_info; + rf625_parameters m_params; + rf625_profile m_profile; + uint8_t *m_user_cfg_area; + uint8_t* m_buf; + rf625_udp_proc m_callback; + std::thread* m_thread; + void udp_proc(); +}; diff --git a/src/sockfun.cpp b/src/sockfun.cpp new file mode 100644 index 0000000000000000000000000000000000000000..384df70c714f6273f076caafa56d27119dcb5e2d --- /dev/null +++ b/src/sockfun.cpp @@ -0,0 +1,105 @@ +#ifdef _WIN32 +#include +typedef int socklen_t; +#else +#include +#include +#include +#include +#define INVALID_SOCKET (-1) +#define SOCKET_ERROR (-1) +typedef int SOCKET; +#endif +#include + +bool send_cmd_packet(SOCKET s, uint32_t cmd, uint32_t attach_size = 0, uint32_t offset = 0, uint32_t size = 0, bool reply = false); +bool recv_data(SOCKET s, char* buf, size_t nbytes, bool accept_incomplete_packet = false, size_t* packet_size = nullptr); +bool send_data(SOCKET s, const char* buf, size_t nbytes); +bool udp_recv_datagram(SOCKET s, char* buf, size_t max_length, size_t* datagram_size, struct sockaddr_in* from_addr = nullptr); +bool set_sock_recvtimeo(SOCKET s, int msec); + +bool send_cmd_packet(SOCKET s, uint32_t cmd, uint32_t attach_size, uint32_t offset, uint32_t size, bool reply) +{ + int nret; + uint32_t blk[4]; + uint16_t rx; + blk[0] = cmd; blk[1] = attach_size; blk[2] = offset; blk[3] = size; + nret = send(s, reinterpret_cast(&blk), sizeof(blk), 0); + if (nret != sizeof(blk)) { + return false; + } + if (reply) { + nret = recv(s, reinterpret_cast(&rx), sizeof(rx), 0); + return (nret == sizeof(rx) && rx == 0x4B4F); + } + return true; +} + +bool recv_data(SOCKET s, char *buf, size_t nbytes, bool accept_incomplete_packet, size_t *packet_size) +{ + int nret, nrecv = 0; + if (!buf || !nbytes) { + return false; + } + do { + nret = recv(s, &buf[nrecv], nbytes-nrecv, 0); + if (nret > 0) { + nrecv += nret; + } + } while(nret != SOCKET_ERROR && !accept_incomplete_packet && (nrecv < nbytes)); + if (accept_incomplete_packet) { + if (packet_size) + *packet_size = nrecv; + return (nrecv > 0); + } + else { + return (nrecv == nbytes); + } +} + +bool send_data(SOCKET s, const char *buf, size_t nbytes) +{ + int nret; + if (!buf || !nbytes) { + return false; + } + nret = send(s, buf, nbytes, 0); + return (nret == nbytes); +} + +bool udp_recv_datagram(SOCKET s, char *buf, size_t max_length, size_t *datagram_size, sockaddr_in *from_addr) +{ + int nret; + socklen_t from_size; + struct sockaddr_in from_addr_l; + from_size = sizeof(from_addr_l); + if (s == INVALID_SOCKET) { + return false; + } + nret = recvfrom(s, buf, max_length, 0, reinterpret_cast(&from_addr_l), &from_size); + if (nret > 0) + { + if (from_addr) { + memcpy(from_addr, &from_addr_l, sizeof(from_addr_l)); + } + *datagram_size = nret; + return true; + } + else + { + return false; + } +} + +bool set_sock_recvtimeo(SOCKET s, int msec) +{ +#ifdef _WIN32 + DWORD t = msec; +#else + timeval t; + t.tv_sec = msec / 1000; + t.tv_usec = (msec % 1000) * 1000; +#endif + return (setsockopt(s, SOL_SOCKET, SO_RCVTIMEO, reinterpret_cast(&t), sizeof(t))); +} + diff --git a/src/wshlp.cpp b/src/wshlp.cpp new file mode 100644 index 0000000000000000000000000000000000000000..5542024f28888a9baf7852d499a4056159d0f3d7 --- /dev/null +++ b/src/wshlp.cpp @@ -0,0 +1,167 @@ +#ifdef _WIN32 + +#pragma comment(lib,"ws2_32.lib") +#pragma comment(lib,"iphlpapi.lib") + +#include +#include +#include +#include + +/************************************************** + */ +char* _ifs[64]; +unsigned int _ifs_cnt; + +BOOL WinSockInit() +{ + WORD wVersionRequested; + WSADATA wsaData; + wVersionRequested = MAKEWORD( 2, 2 ); + if (WSAStartup( wVersionRequested, &wsaData ) != 0) { + return FALSE; + } + return TRUE; +} + +void WinSockDeinit() +{ + WSACleanup(); +} + +BOOL EnumAdapterAddresses() +{ + PIP_ADAPTER_ADDRESSES adapter_addresses, aa; + PIP_ADAPTER_UNICAST_ADDRESS ua; + DWORD rv, size; + + _ifs_cnt = 0; + memset(_ifs, 0, sizeof(_ifs)); + + rv = GetAdaptersAddresses(AF_UNSPEC, GAA_FLAG_INCLUDE_PREFIX, NULL, NULL, &size); + if (rv != ERROR_BUFFER_OVERFLOW) { + return FALSE; + } + adapter_addresses = (PIP_ADAPTER_ADDRESSES)malloc(size); + + rv = GetAdaptersAddresses(AF_UNSPEC, GAA_FLAG_INCLUDE_PREFIX, NULL, adapter_addresses, &size); + if (rv != ERROR_SUCCESS) { + free(adapter_addresses); + return FALSE; + } + + for (aa = adapter_addresses; aa != NULL; aa = aa->Next) { + if (aa->OperStatus != IfOperStatusUp) continue; + for (ua = aa->FirstUnicastAddress; ua != NULL; ua = ua->Next) { + if (ua->Address.lpSockaddr->sa_family != AF_INET) continue; + _ifs[_ifs_cnt] = (char*)malloc(64); + memset(_ifs[_ifs_cnt], 0, 64); + sprintf(_ifs[_ifs_cnt++], "%u.%u.%u.%u", (BYTE)(ua->Address.lpSockaddr->sa_data[2]), (BYTE)(ua->Address.lpSockaddr->sa_data[3]), (BYTE)(ua->Address.lpSockaddr->sa_data[4]), (BYTE)(ua->Address.lpSockaddr->sa_data[5])); + } + } + + free(adapter_addresses); + return TRUE; +} + +void FreeAdapterAddresses() +{ + for(register size_t i=0; i<_ifs_cnt; i++) { + if (_ifs[i]) { + free(_ifs[i]); + _ifs[i] = NULL; + } + } + _ifs_cnt = 0; +} + +BOOL MatchIP(const char* ip1, const char* ip2, const char* mask) +{ + unsigned long nip1, nip2, nmask; + nip1 = inet_addr(ip1); + nip2 = inet_addr(ip2); + nmask = mask ? inet_addr(mask) : inet_addr("255.255.255.0"); + nip1&=nmask; + nip2&=nmask; + if (nip1 == nip2) + return TRUE; + else + return FALSE; +} + +BOOL MatchIP(u_long ip1, u_long ip2, u_long mask) +{ + ip1&=mask; + ip2&=mask; + if (ip1 == ip2) + return TRUE; + else + return FALSE; +} + +char* GetCompatibleInterface(const char* RemoteIP, const char* SubnetMask) +{ + for (register size_t i=0; i<_ifs_cnt; i++) + { + if (MatchIP(_ifs[i], RemoteIP, SubnetMask)) + return _ifs[i]; + } + return NULL; +} + +u_long GetCompatibleInterface(u_long RemoteIP, u_long SubnetMask) +{ + if (RemoteIP) + { + for (register size_t i=0; i<_ifs_cnt; i++) + { + if (MatchIP(inet_addr(_ifs[i]), RemoteIP, SubnetMask)) + return inet_addr(_ifs[i]); + } + } + return htonl(INADDR_ANY); +} + +BOOL MatchUDP(u_long testIP, u_long hostIP) +{ + u_long mul = testIP | hostIP; + if (mul == hostIP) + return TRUE; + else + return FALSE; +} + +u_long GetUDPCompatibleInterface(u_long HostIP) +{ + for (register size_t i=0; i<_ifs_cnt; i++) + { + if (MatchUDP(inet_addr(_ifs[i]), HostIP)) + return inet_addr(_ifs[i]); + } + return htonl(INADDR_ANY); +} + +void DumpInterfaces() +{ + printf("detected network interfaces(%u):\n", _ifs_cnt); + for (register unsigned int i=0; i<_ifs_cnt; i++) { + puts(_ifs[i]); + } +} + +BOOL BindToCompatibleInterface(SOCKET s, in_addr* addr, u_long netMask) +{ + u_long local_ip = INADDR_ANY; + u_long net_mask = netMask; + sockaddr_in sin_loc; + memset(&sin_loc, 0, sizeof(sockaddr_in)); + sin_loc.sin_family = PF_INET; + local_ip = GetCompatibleInterface(addr->s_addr, net_mask); + sin_loc.sin_addr.s_addr = local_ip; + return (bind(s, reinterpret_cast(&sin_loc), sizeof(sin_loc)) != SOCKET_ERROR); +} + +/************************************************** + */ + +#endif