int get_serial_adapter(struct sp_port **my_port, struct sp_port_config *my_port_config) { int ret; // Get pointer to port char port_name[] = "/dev/ttyAMA0"; ret = sp_get_port_by_name(port_name, my_port); if (ret != SP_OK) { printf("Failed to get port: %s. Exiting now.\n",port_name); return 0; } // Open port ret = sp_open((*my_port), SP_MODE_READ_WRITE); if (ret == SP_OK) { printf("Successfully opened port!\n"); } else { printf("Failed to open port :(\n"); sp_free_port(*my_port); return 0; } // Initialize port config ret = sp_new_config(&my_port_config); if (ret != SP_OK) { printf("Failed to initialize port config.\n"); sp_free_port(*my_port); return 0; } return 1; }
int serial_close(struct serial_device_t *serial) { if (!serial) { log_error("serial_close: invalid serial port device."); return -DEVICE_CONN_ERROR; } if (!serial->data) { log_error("serial_close: cannot close unopened serial port %s.", serial->port); return -DEVICE_CONN_ERROR; } log_info("serial_close: closing serial port %s.", serial->port); int ret = sp_close(serial->data); char *error; switch (ret) { case SP_ERR_ARG: log_error("serial_close: attempt to close an invalid serial port."); return -DEVICE_CONF_ERROR; case SP_ERR_FAIL: error = sp_last_error_message(); log_error("serial_close: error closing port (%d): %s.", sp_last_error_code(), error); sp_free_error_message(error); return -DEVICE_CONN_ERROR; } sp_free_port(serial->data); serial->data = NULL; return NO_ERROR; }
uart_input::~uart_input() { std::array<std::uint8_t, 31> buffer = { 0 }; sp_blocking_write(port, (void*)buffer.data(), buffer.size(), 0); sp_drain(port); sp_close(port); sp_free_port(port); }
int navilink_free_serial_list(NavilinkSerialPort** list) { NavilinkSerialPort** iter = &list[0]; while (*iter) { sp_free_port((*iter)->serial_port); iter++; } free(list); return 0; }
void Arduino_disconnect() { if(serialPort) { // Close the serial port sp_close(serialPort); sp_free_port(serialPort); serialPort = NULL; } }
int main( int argc, const char** argv ) { ParseOptions(argc, argv); ConvertCrLf = (strcmp(ConvertCrLfStr, "on") == 0); OpenSerialPort(&SerialPort, SP_MODE_READ|SP_MODE_WRITE); InstallSignalHandler(); EventLoop(); sp_close(SerialPort); sp_free_port(SerialPort); return 0; }
int Arduino_connect(const char *serialPortName, int baudrate) { struct sp_port_config *serialPortConfiguration; enum sp_return r; if(serialPort) sp_free_port(serialPort); // Find the specified serial port if(sp_get_port_by_name(serialPortName, &serialPort) != SP_OK) { fprintf(stderr, "Cannot find the serial port\n"); return 0; } // Open the serial port if(sp_open(serialPort, SP_MODE_READ_WRITE) != SP_OK) { fprintf(stderr, "Cannot open the serial port\n"); return 0; } // Configure the serial port sp_new_config(&serialPortConfiguration); sp_set_config_baudrate(serialPortConfiguration, 9600); sp_set_config_parity(serialPortConfiguration, SP_PARITY_NONE); sp_set_config_bits(serialPortConfiguration, 8); sp_set_config_stopbits(serialPortConfiguration, 1); sp_set_config_flowcontrol(serialPortConfiguration, SP_FLOWCONTROL_NONE); if(sp_set_config(serialPort, serialPortConfiguration) != SP_OK) { fprintf(stderr, "Cannot configure the serial port\n"); return 0; } sp_free_config(serialPortConfiguration); return 1; }
void FlotillaDock::disconnect(void){ if (state == Disconnected) return; state = Disconnected; int x; for (x = 0; x < MAX_CHANNELS; x++){ if (module[x].state == ModuleConnected){ module[x].disconnect(); queue_module_event(x); } } sp_flush(port, SP_BUF_OUTPUT); sp_flush(port, SP_BUF_INPUT); sp_close(port); sp_free_port(port); std::ostringstream msg; msg << GetTimestamp() << "Dock Disconnected" << std::endl; // , serial " << serial << std::endl; std::cout << msg.str(); }
/** * Close the specified serial port. * * @param serial Previously initialized serial port structure. * * @retval SR_OK Success. * @retval SR_ERR Failure. * * @private */ SR_PRIV int serial_close(struct sr_serial_dev_inst *serial) { int ret; char *error; if (!serial) { sr_dbg("Invalid serial port."); return SR_ERR; } if (!serial->data) { sr_dbg("Cannot close unopened serial port %s.", serial->port); return SR_ERR; } sr_spew("Closing serial port %s.", serial->port); ret = sp_close(serial->data); switch (ret) { case SP_ERR_ARG: sr_err("Attempt to close an invalid serial port."); return SR_ERR_ARG; case SP_ERR_FAIL: error = sp_last_error_message(); sr_err("Error closing port (%d): %s.", sp_last_error_code(), error); sp_free_error_message(error); return SR_ERR; } sp_free_port(serial->data); serial->data = NULL; return SR_OK; }
int navilink_open_sp_port(struct sp_port* port, NavilinkDevice* device) { enum sp_return result = sp_open(port, SP_MODE_READ_WRITE); if (result != SP_OK) { CATCH_LIBSERIAL_ERROR(device); goto error_cleanup_port; } struct sp_port_config* config = NULL; result = sp_new_config(&config); if (result != SP_OK) { CATCH_LIBSERIAL_ERROR(device); goto error_clean_config; } // Set the config sp_set_baudrate(port, 115200); sp_set_bits(port, 8); sp_set_parity(port, SP_PARITY_NONE); sp_set_stopbits(port, 1); sp_set_flowcontrol(port, SP_FLOWCONTROL_NONE); sp_set_config(port, config); // Allocate events buffer struct sp_event_set* event_set = NULL; sp_new_event_set(&event_set); sp_add_port_events(event_set, port, SP_EVENT_TX_READY); //Wait for the port to be ready result = sp_wait(event_set, 5000); if (result != SP_OK) { CATCH_LIBSERIAL_ERROR(device); goto error_clean_event_set; } device->serial_port = port; device->event_set = event_set; //Check that this is a Navilink device int res = navilink_check_protocol(device); if (res < 0) { goto error_clean_event_set; } // Retrieve the informations about the device res = navilink_query_information(device, &device->informations); if (res < 0) { goto error_clean_event_set; } // Retrieve the firmware version res = navilink_query_firmware_version(device, &device->firmware_version); if (res < 0) { goto error_clean_event_set; } return 0; error_clean_event_set: sp_free_event_set(event_set); error_clean_config: sp_free_config(config); error_cleanup_port: sp_free_port(port); return -1; }
int main() { printf("Started!\n"); /* // Initialize GPIO if (gpioInitialise() < 0) { printf("pigpio initialization failed\n"); } else { printf("pigpio initialization succeeded\n"); } //gpioTerminate(); // Setup GPIO pin 18 and send rest signal #define GPIO_IMU_RST 18 #define GPIO_LEVEL_HIGH 1 #define GPIO_LEVEL_LOW 0 if (gpioSetMode(GPIO_IMU_RST, PI_OUTPUT) < 0) { printf("GPIO 18 setup failed\n"); gpioTerminate(); } else { printf("GPIO 18 setup succeeded\n"); } if (gpioWrite(GPIO_IMU_RST, GPIO_LEVEL_HIGH) < 0) { printf("GPIO RST write high failed\n"); gpioTerminate(); } else { printf("GPIO RST write high succeeded\n"); } // Sleep 650 ms to allow BNO055 to fully reset time_sleep(0.65); */ // - Get serial port adapter and config object int ret; struct sp_port *serial_adapter; struct sp_port_config *serial_config; ret = sp_new_config(&serial_config); ret = get_serial_adapter(&serial_adapter, serial_config); if (ret != 1) { printf("Failed to get serial adapter :(\n"); sp_free_port(serial_adapter); } Point3D measurement_vector; ret = read_linear_acceleration_vector(serial_adapter, &measurement_vector); if (ret == 1) { printf("Successfully read linear acceleration.\n"); printf("Measurement - x: %f - y: %f - z: %f\n",measurement_vector.x,measurement_vector.y,measurement_vector.z); } else { printf("Failed to read linear acceleration.\n"); } // - Measure average sensor read time over N reads int n, N = 50; clock_t start_time = clock(), time_diff; for (n = 0; n<N; n++) { ret = sleep(1); ret = read_linear_acceleration_vector(serial_adapter, &measurement_vector); if (ret != 1) { printf("Failed to read linear acceleration in turn %d.\n",n); } } time_diff = clock() - start_time; float msec = (float)time_diff * 1000 / CLOCKS_PER_SEC; printf("Time per read (averaged over %d reads): %f (ms)\n",N,msec/N); // - Clean up program gpioTerminate(); sp_free_port(serial_adapter); printf("Reached end!\n"); return 0; }
int main(int argc, char **argv) { int opt; int result = 0; sbp_state_t s; if (argc <= 1) { usage(argv[0]); exit(EXIT_FAILURE); } while ((opt = getopt(argc, argv, "p:")) != -1) { switch (opt) { case 'p': serial_port_name = (char *)calloc(strlen(optarg) + 1, sizeof(char)); if (!serial_port_name) { fprintf(stderr, "Cannot allocate memory!\n"); exit(EXIT_FAILURE); } strcpy(serial_port_name, optarg); break; case 'h': usage(argv[0]); exit(EXIT_FAILURE); } } if (!serial_port_name) { fprintf(stderr, "Please supply the serial port path where the Piksi is " \ "connected!\n"); exit(EXIT_FAILURE); } result = sp_get_port_by_name(serial_port_name, &piksi_port); if (result != SP_OK) { fprintf(stderr, "Cannot find provided serial port!\n"); exit(EXIT_FAILURE); } result = sp_open(piksi_port, SP_MODE_READ); if (result != SP_OK) { fprintf(stderr, "Cannot open %s for reading!\n", serial_port_name); exit(EXIT_FAILURE); } setup_port(); sbp_state_init(&s); #if 0 /* Register a node and callback, and associate them with a specific message ID. */ sbp_register_callback(&s, SBP_MSG_HEARTBEAT, &hearbeat_callback, NULL, &heartbeat_callback_node); sbp_register_callback(&s, SBP_MSG_GPS_TIME, &sbp_gps_time_callback, NULL, &gps_time_node); sbp_register_callback(&s, SBP_MSG_POS_LLH, &sbp_pos_llh_callback, NULL, &pos_llh_node); sbp_register_callback(&s, SBP_MSG_BASELINE_NED, &sbp_baseline_ned_callback, NULL, &baseline_ned_node); sbp_register_callback(&s, SBP_MSG_VEL_NED, &sbp_vel_ned_callback, NULL, &vel_ned_node); sbp_register_callback(&s, SBP_MSG_DOPS, &sbp_dops_callback, NULL, &dops_node); /* Use sprintf to right justify floating point prints. */ char rj[30]; /* Only want 1 call to SH_SendString as semihosting is quite slow. * sprintf everything to this array and then print using array. */ char str[1000]; int str_i; #endif while (1) { // sbp_process(&s, &piksi_port_read); sleep(1); fprintf(stdout, "hello\n"); #if 0 str_i = 0; memset(str, 0, sizeof(str)); str_i += sprintf(str + str_i, "\n\n\n\n"); /* Print GPS time. */ str_i += sprintf(str + str_i, "GPS Time:\n"); str_i += sprintf(str + str_i, "\tWeek\t\t: %6d\n", (int)gps_time.wn); sprintf(rj, "%6.2f", ((float)gps_time.tow) / 1e3); str_i += sprintf(str + str_i, "\tSeconds\t: %9s\n", rj); str_i += sprintf(str + str_i, "\n"); /* Print absolute position. */ str_i += sprintf(str + str_i, "Absolute Position:\n"); sprintf(rj, "%4.10lf", pos_llh.lat); str_i += sprintf(str + str_i, "\tLatitude\t: %17s\n", rj); sprintf(rj, "%4.10lf", pos_llh.lon); str_i += sprintf(str + str_i, "\tLongitude\t: %17s\n", rj); sprintf(rj, "%4.10lf", pos_llh.height); str_i += sprintf(str + str_i, "\tHeight\t: %17s\n", rj); str_i += sprintf(str + str_i, "\tSatellites\t: %02d\n", pos_llh.n_sats); str_i += sprintf(str + str_i, "\n"); /* Print NED (North/East/Down) baseline (position vector from base to rover). */ str_i += sprintf(str + str_i, "Baseline (mm):\n"); str_i += sprintf(str + str_i, "\tNorth\t\t: %6d\n", (int)baseline_ned.n); str_i += sprintf(str + str_i, "\tEast\t\t: %6d\n", (int)baseline_ned.e); str_i += sprintf(str + str_i, "\tDown\t\t: %6d\n", (int)baseline_ned.d); str_i += sprintf(str + str_i, "\n"); /* Print NED velocity. */ str_i += sprintf(str + str_i, "Velocity (mm/s):\n"); str_i += sprintf(str + str_i, "\tNorth\t\t: %6d\n", (int)vel_ned.n); str_i += sprintf(str + str_i, "\tEast\t\t: %6d\n", (int)vel_ned.e); str_i += sprintf(str + str_i, "\tDown\t\t: %6d\n", (int)vel_ned.d); str_i += sprintf(str + str_i, "\n"); /* Print Dilution of Precision metrics. */ str_i += sprintf(str + str_i, "Dilution of Precision:\n"); sprintf(rj, "%4.2f", ((float)dops.gdop / 100)); str_i += sprintf(str + str_i, "\tGDOP\t\t: %7s\n", rj); sprintf(rj, "%4.2f", ((float)dops.hdop / 100)); str_i += sprintf(str + str_i, "\tHDOP\t\t: %7s\n", rj); sprintf(rj, "%4.2f", ((float)dops.pdop / 100)); str_i += sprintf(str + str_i, "\tPDOP\t\t: %7s\n", rj); sprintf(rj, "%4.2f", ((float)dops.tdop / 100)); str_i += sprintf(str + str_i, "\tTDOP\t\t: %7s\n", rj); sprintf(rj, "%4.2f", ((float)dops.vdop / 100)); str_i += sprintf(str + str_i, "\tVDOP\t\t: %7s\n", rj); str_i += sprintf(str + str_i, "\n"); #endif } result = sp_close(piksi_port); if (result != SP_OK) { fprintf(stderr, "Cannot close %s properly!\n", serial_port_name); } sp_free_port(piksi_port); free(serial_port_name); return 0; }
int main(int argc, char *argv[]) { /* select部份 */ int opt = TRUE; int master_socket, addrlen, new_socket, client_socket[MAXCLIENTS], max_clients = MAXCLIENTS, activity, i, valread, sd; user_content_t *my_contents[MAXCLIENTS + 1]; int max_sd; struct timeval tv; /* select超时 */ //set of socket descriptors fd_set readfds; /* IP部份 */ struct addrinfo hints, *res; /* 连接到target的用到的 */ struct sockaddr_in address; /* 每个客户端的缓冲区和索引 */ char buffer[MAXCLIENTS][MAXLEN]; char *buffer_p[MAXCLIENTS]; //data buffer of 1K int buffer_data_size[MAXCLIENTS]; char itoa_buffer[8]; /* ip地址从网络顺序转成char数组 */ char *header = NULL; /* 将转换好的ip地址:封包成header */ /* 串口部份 固定变量 */ static char com_devicename[] = "/dev/ttyUSB0"; /* 固定的linux串口设备文件 */ user_content_t *my_com_conf = my_malloc(sizeof(user_content_t));/* 串口配置 */ /* 串口部份 动态变量 */ #ifdef MODULE_SERIAL char buffer_com[MAXLEN]; /* 串口缓冲区 */ char *buffer_com_p=buffer_com; int buffer_com_data_size=0; #endif /* 蓝牙部份 */ #ifdef MODULE_BLUETOOTH struct sockaddr_rc blue_rem_addr = {0}; char blue_buffer[MAXLEN],blue_sender_MAC[18]; int blue_fd,blue_fd_client,blue_bytes_read; socklen_t blue_opt=sizeof(blue_rem_addr); user_content_t *blue_user_content; #endif /* args参数 */ char *PORT; if (argc != 2) { fprintf(stderr, "usage: %s listen-port\n", argv[0]); return 1; } PORT = argv[1]; #ifdef MODULE_SERIAL /* 打开串口,须root权限 */ if(NULL==(my_com_conf=open_com(com_devicename))) { printf("error open com!\n"); return 1; } #endif #ifdef MODULE_BLUETOOTH /* 打开蓝牙 */ blue_fd=create_bluetooth_socket(); if(blue_fd<0) { printf("error bluetooth fd is -1\n"); return -1; } listen(blue_fd, 1); #endif //initialise all client_socket[] to 0 so not checked for (i = 0; i < max_clients; i++) { client_socket[i] = 0; } if ((master_socket = create_server_socket("0.0.0.0", PORT)) < 0) { printf("error create socket fd\n"); return 1; } //set master socket to allow multiple connections , this is just a good habit, it will work without this if (setsockopt(master_socket, SOL_SOCKET, SO_REUSEADDR, (char *) &opt, sizeof(opt)) < 0) { perror("setsockopt"); exit(EXIT_FAILURE); } //try to specify maximum of 3 pending connections for the master socket if (listen(master_socket, 3) < 0) { perror("listen"); exit(EXIT_FAILURE); } printf("Listening on port %s \n", PORT); //accept the incoming connection addrlen = sizeof(address); puts("Waiting for connections ..."); while (TRUE) { //clear the socket set FD_ZERO(&readfds); //add master socket to set FD_SET(master_socket, &readfds); max_sd = master_socket; #ifdef MODULE_SERIAL FD_SET(my_com_conf->fd,&readfds); max_sd = max_sd>my_com_conf->fd?max_sd:my_com_conf->fd; #endif #ifdef MODULE_BLUETOOTH FD_SET(blue_fd,&readfds); max_sd = max_sd>blue_fd?max_sd:blue_fd; #endif //add child sockets to set for (i = 0; i < max_clients; i++) { //socket descriptor sd = client_socket[i]; //if valid socket descriptor then add to read list if (sd > 0) FD_SET(sd, &readfds); //highest file descriptor number, need it for the select function if (sd > max_sd) max_sd = sd; } //wait for an activity on one of the sockets , timeout is NULL , so wait indefinitely activity = select(max_sd + 1, &readfds, NULL, NULL, NULL); if ((activity < 0) && (errno != EINTR)) { printf("select error"); } //If something happened on the master socket , then its an incoming connection if (FD_ISSET(master_socket, &readfds)) { if ((new_socket = accept(master_socket, (struct sockaddr *) &address, (socklen_t*) &addrlen)) < 0) { perror("accept"); exit(EXIT_FAILURE); } //add new socket to array of sockets for (i = 0; i < max_clients; i++) { //if position is empty, create new one if (client_socket[i] == 0) { client_socket[i] = new_socket; // 初始化buffer buffer_p[i] = buffer[i]; memset(buffer_p[i], 0, MAXLEN); buffer_data_size[i] = 0; printf("accepted #%d client\n", i); break; } } } #ifdef MODULE_SERIAL // 串口读 if (FD_ISSET(my_com_conf->fd, &readfds)) { /* 非阻塞读取 */ valread=sp_nonblocking_read(my_com_conf->com_port,buffer_com_p+buffer_com_data_size,MAXLEN); if(valread<0) { printf("read data from com error: %d\n",valread); return 1; buffer_com_data_size=0; buffer_com_p=buffer_com; } else { buffer_com_data_size+=valread; /* 读完所有数据,串口数据包必须以\r\n结尾 */ if(buffer_com[buffer_com_data_size-2]==13 && buffer_com[buffer_com_data_size-1]==10) { printf("- - - - - - - - - -\nread from COM ok\n"); buffer_com_p[buffer_com_data_size]=0; my_contents[MAXCLIENTS]=new_user_content_from_str(buffer_com,com_devicename,get_direction(buffer_com)); if(!my_contents[MAXCLIENTS]) { printf("invalid packet!\n"); } else { printf(" %s",com_devicename); redirect_from_user_content(my_contents[MAXCLIENTS]); } my_free(my_contents[MAXCLIENTS]); /* reset buffer offset */ buffer_com_data_size=0; buffer_com_p=buffer_com; } } } #endif #ifdef MODULE_BLUETOOTH // 蓝牙读 if(FD_ISSET(blue_fd,&readfds)) { // accept one connection blue_fd_client = accept(blue_fd, (struct sockaddr *)&blue_rem_addr, &blue_opt); ba2str( &blue_rem_addr.rc_bdaddr, blue_sender_MAC ); // read data from the client blue_bytes_read = read(blue_fd_client, blue_buffer, sizeof(blue_buffer)); if( blue_bytes_read > 0 ) { printf("- - - - - - - - - -\nread from bluetooth ok\n"); blue_buffer[blue_bytes_read]=0; blue_user_content=new_user_content_from_str(blue_buffer,blue_sender_MAC,get_direction(blue_buffer)); if(!blue_user_content) { printf("invalid packet!\n"); } else { printf(" %s",blue_sender_MAC); redirect_from_user_content(blue_user_content); } my_free(blue_user_content); } else { printf("bluetooth recv data error!\n"); } // close connection close(blue_fd_client); } #endif // 局域网ip读 for (i = 0; i < max_clients; i++) { sd = client_socket[i]; if (FD_ISSET(sd, &readfds)) { //Check if it was for closing , and also read the incoming message if ((valread = read(sd, buffer_p[i] + buffer_data_size[i], MAXLEN)) == 0) { //Somebody disconnected , get his details and print buffer[i][buffer_data_size[i]] = 0; getpeername(sd, (struct sockaddr*) &address, (socklen_t*) &addrlen); printf( "- - - - - - - - - -\nread %d bytes from LAN client\n", buffer_data_size[i]); //Close the socket and mark as 0 in list for reuse close(sd); client_socket[i] = 0; /* convert port(interger) to char* */ sprintf(itoa_buffer, "%d", ntohs(address.sin_port)); header = get_header_ipv4(inet_ntoa(address.sin_addr), itoa_buffer); /* create relay struct: from LAN ip, to serial */ my_contents[i] = new_user_content_from_str(buffer[i], header, get_direction(buffer[i])); if (!my_contents[i]) { printf("invalid packet!\n"); } else { printf(" %s", header); redirect_from_user_content(my_contents[i]); } my_free(header); my_free(my_contents[i]); } else { /* 累加数据 */ buffer_data_size[i] += valread; } } } } #ifdef MODULE_SERIAL sp_close(my_com_conf->com_port); sp_free_port(my_com_conf->com_port); sp_free_config(my_com_conf->com_conf); my_free(my_com_conf); #endif #ifdef MODULE_BLUETOOTH close(blue_fd); #endif close(master_socket); printf("exit..\n"); return 0; }