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;
}
