int main(){
int usbErr = 0;
int iface_num[NUM_IFACES];
iface_num[0]= 1; // bulk interface
libusb_context *imu_host = NULL;
libusb_device_handle *imu_handle = NULL;
libusbSource * usb_source = NULL;
struct libusb_transfer * bulkIO[2]; //todo: have something like endpoint[numendpoints] and place each transfer at the ep they correspond to?
struct libusb_transfer * bulk_in = NULL;//bulkIO[0];
struct libusb_transfer * bulk_out = NULL;//bulkIO[1];
unsigned char * bulk_in_buffer = NULL;
unsigned char * bulk_out_buffer = NULL;
GMainContext * edfc_context = NULL;
GIOChannel * g_stdin = NULL;
GSource * gs_stdin = NULL;
sfd = open_port();
usbErr = libusb_init(&imu_host);
if(usbErr){
print_libusb_error(usbErr, "libusb_init");
exit(EXIT_FAILURE);
}
libusb_set_debug(imu_host, 3);
imu_handle = open_usb_device_handle(imu_host, is_imu_device, iface_num, NUM_IFACES);
if(!imu_handle){
printf("**imu_handle acquisition error\n");
libusb_exit(imu_host);
exit(EXIT_FAILURE);
}
usb_source = libusb_source_new(imu_host);
// g_source_set_callback((GSource*) usb_source, (GSourceFunc)libusb_mainloop_error_cb, &edfc_main, NULL);
bulk_in = libusb_alloc_transfer(0);
bulk_out = libusb_alloc_transfer(0);
bulk_in_buffer = calloc(MAX_PACKET_SIZE, sizeof(unsigned char)); //todo: slice allocate?
bulk_out_buffer = calloc(MAX_PACKET_SIZE, sizeof(unsigned char)); //todo: slice allocate?
libusb_fill_bulk_transfer(bulk_in,
imu_handle,
BULK_IN_EP,
bulk_in_buffer,
MAX_PACKET_SIZE,
bulk_in_cb,
NULL,
0);
libusb_fill_bulk_transfer(bulk_out,
imu_handle,
BULK_OUT_EP,
bulk_out_buffer,
MAX_PACKET_SIZE,
bulk_out_cb,
NULL,
0);
bulkIO[0] = bulk_in;
bulkIO[1] = bulk_out;
edfc_context = g_main_context_new(); //edfc == event driven flight computer
edfc_main = g_main_loop_new(edfc_context, FALSE);
g_stdin = g_io_channel_unix_new(fileno(stdin));
if(!g_stdin){
printf("error creating g_stdin\n");
}
g_source_attach((GSource*) usb_source, edfc_context);
gs_stdin = g_io_create_watch(g_stdin, G_IO_IN | G_IO_ERR | G_IO_HUP);
g_source_set_callback(gs_stdin, (GSourceFunc)read_g_stdin, bulkIO, NULL);
g_source_attach(gs_stdin, edfc_context);
printf("beginning main loop\n");
g_main_loop_run(edfc_main);
printf("main loop finished\n");
//cleanup
g_source_destroy(gs_stdin);
g_io_channel_shutdown(g_stdin, TRUE, NULL); //todo: fix null
free(bulk_in_buffer);
free(bulk_out_buffer);
libusb_free_transfer(bulk_in);
libusb_free_transfer(bulk_out);
g_source_destroy((GSource*) usb_source);
g_main_loop_unref(edfc_main);
g_main_context_unref(edfc_context);
usbErr = libusb_release_interface(imu_handle, iface_num[0]);
if(usbErr) print_libusb_error(usbErr, "exit libusb_release_interface");
usbErr = libusb_attach_kernel_driver(imu_handle, iface_num[0]);
if(usbErr) print_libusb_error(usbErr, "exit libusb_attach_kernel_driver");
libusb_close(imu_handle);
libusb_exit(imu_host);
exit(EXIT_SUCCESS);
}
int main(int argc, char *argv[]) {
fprintf(stderr,"Proxy Start yuruiz\n");
int http_port;
char *log_file;
int http_listen_socket, http_client_sock;
struct sockaddr_in http_addr, cli_addr;
socklen_t conn_size;
pool conn_pool;
if (argc < 7 || argc > 8) {
printf(USAGE, argv[0]);
return EXIT_FAILURE;
}
log_file = argv[1];
alpha = atof(argv[2]);
http_port = atoi(argv[3]);
fake_ip = argv[4];
proxy.dns_ip = argv[5];
proxy.dns_port = argv[6];
if (argc == 8) {
www_ip = argv[7];
}
loginit(log_file);
fprintf(stderr,"-------------------Server Start------------------\n");
if ((http_listen_socket = open_port(http_port, &http_addr)) == -1) {
fprintf(stderr,"Open port failed\n");
return EXIT_FAILURE;
}
// parse fake-ip
bzero(&proxy.myaddr, sizeof(struct sockaddr_in));
proxy.myaddr.sin_family = AF_INET;
inet_aton(fake_ip, &proxy.myaddr.sin_addr);
proxy.myaddr.sin_port = htons(0);
init_pool(http_listen_socket, &conn_pool);
do {
// printf("Pool start\n");
conn_pool.ready_set = conn_pool.read_set;
conn_pool.nconn = select(conn_pool.maxfd + 1, &conn_pool.ready_set, NULL, NULL, NULL);
conn_size = sizeof(cli_addr);
if (FD_ISSET(http_listen_socket, &conn_pool.ready_set)) {
fprintf(stderr,"Adding new http connection\n");
if ((http_client_sock = accept(http_listen_socket, (struct sockaddr *) &cli_addr, &conn_size)) == -1) {
fprintf(stderr,"Error accepting http connection.\n");
continue;
}
add_conn(http_client_sock, &conn_pool, &cli_addr);
}
conn_handle(&conn_pool);
} while (1);
close_socket(http_listen_socket);
return EXIT_SUCCESS;
}
/*!
\brief main() is the typical main function in a C program, it performs
all core initialization, loading of all main parameters, initializing handlers
and entering gtk_main to process events until program close
\param argc (gint) count of command line arguments
\param argv (char **) array of command line args
\returns TRUE
*/
gint main(gint argc, gchar ** argv)
{
gint port_fd = 0;
gint file_fd = 0;
verify_args(argc, argv);
output(g_strdup_printf("MegaTunix msloader %s\n",VERSION),TRUE);
/* If we got this far, all is good argument wise */
if (!lock_port(argv[2]))
{
output((gchar *)"Could NOT LOCK Serial Port\nCheck for already running serial apps using the port\n",FALSE);
exit(-1);
}
port_fd = open_port(argv[2]);
if (port_fd > 0)
output((gchar *)"Port successfully opened\n",FALSE);
else
{
output((gchar *)"Could NOT open Port check permissions\n",FALSE);
unlock_port();
exit(-1);
}
#ifdef __WIN32__
file_fd = open(argv[3], O_RDWR | O_BINARY );
#else
file_fd = g_open(argv[3],O_RDONLY,S_IRUSR);
#endif
if (file_fd > 0 )
output((gchar *)"Firmware file successfully opened\n",FALSE);
else
{
output((gchar *)"Could NOT open firmware file, check permissions/paths\n",FALSE);
close_port(port_fd);
unlock_port();
exit(-1);
}
if (type == MS1)
{
setup_port(port_fd, 9600);
do_ms1_load(port_fd,file_fd);
}
else if (type == MS2)
{
setup_port(port_fd,115200);
do_ms2_load(port_fd,file_fd);
}
else if (type == LIBREEMS)
{
setup_port(port_fd,115200);
do_libreems_load(port_fd,file_fd);
}
flush_serial(port_fd,BOTH);
close_port(port_fd);
unlock_port();
return (0) ;
}
int main(int argc, char* argv[])
{
int n=pow(2,GC_FRAMES);
int *codes=new int[n];
struct termios options;
pthread_t get_result_thread;
void * thread_ret;
if(argc<3){
printf("usage:\n\t%s <dev_file> <baud rate>\n",argv[0]);
return -1;
}
printf("Selected baud rate is %ld\n",atol(argv[2]));
int fd=open_port(argv[1]);
if(fd==-1){
return -1;
}
tcgetattr(fd, &options);
//Set the baud rate
switch(atol(argv[2])){
case 9600:
cfsetispeed(&options, B9600);
cfsetospeed(&options, B9600);
break;
case 19200:
cfsetispeed(&options, B19200);
cfsetospeed(&options, B19200);
break;
case 38400:
cfsetispeed(&options, B38400);
cfsetospeed(&options, B38400);
break;
case 57600:
cfsetispeed(&options, B57600);
cfsetospeed(&options, B57600);
break;
case 115200:
cfsetispeed(&options, B115200);
cfsetospeed(&options, B115200);
break;
case 230400:
cfsetispeed(&options, B230400);
cfsetospeed(&options, B230400);
break;
default:
printf("Selected baud rate %ld is not support now!\n", atol(argv[2]));
close(fd);
return -1;
}
//Enable the receiver and set local mode...
options.c_cflag |= (CLOCAL | CREAD);
options.c_cflag &= ~CSIZE; /* Mask the character size bits */
options.c_cflag |= CS8; /* Select 8 data bits */
//No parity
options.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB;
//Set the new options for the port...
tcsetattr(fd, TCSANOW, &options);
GCTHREAD_FUNC gc_func=get_result;
pthread_create( &get_result_thread, NULL, gc_func, (void*)&fd);
//------------------------------------------------------------------------
generate_graycode(GC_FRAMES,codes);
cv::Scalar color;
cv::namedWindow(window_name);
// 移动到第二个屏幕上（第一个屏幕的大小是2560x1440)
cv::moveWindow(window_name,DESKTOP_WIDTH,0);
// 全屏显示窗口。事实上未必会全屏，但至少可以去掉标题栏部分
cv::setWindowProperty(window_name, cv::WND_PROP_FULLSCREEN, cv::WINDOW_FULLSCREEN);
// show an image
cv::Mat mengnalisha=cv::imread("mengnalisha.png");
cv::imshow(window_name, mengnalisha);
cv::waitKey(0);
for(int i=0; i<GC_FRAMES; i++){
gcImgH[i].create(PROJ_HEIGHT,PROJ_WIDTH,CV_8UC3);
for(int c=0; c<n; c++){
int delta_c=PROJ_WIDTH/n;
int b=(codes[c]>>(GC_FRAMES-1-i)) & 0x01;
if(!b) color=cv::Scalar(0,0,0);
else color=cv::Scalar(255,255,255);
cv::rectangle(gcImgH[i], cv::Point(c*delta_c,0), cv::Point((c+1)*delta_c,PROJ_HEIGHT-1), color, cv::FILLED);
}
gcImgV[i].create(PROJ_HEIGHT,PROJ_WIDTH,CV_8UC3);
for(int r=0; r<n; r++){
int delta_r=PROJ_HEIGHT/n;
int b=(codes[r]>>(GC_FRAMES-1-i)) & 0x01;
if(!b) color=cv::Scalar(0,0,0);
else color=cv::Scalar(255,255,255);
cv::rectangle(gcImgV[i], cv::Point(0,r*delta_r), cv::Point(PROJ_WIDTH-1, (r+1)*delta_r), color, cv::FILLED);
}
// 保存两种格雷码结构图
/*
char fname[32];
snprintf(fname,32,"gcImgV%02d.png", i);
std::cout<<fname<<std::endl;
cv::imwrite(fname, gcImgV[i]);
snprintf(fname,32,"gcImgH%02d.png", i);
std::cout<<fname<<std::endl;
cv::imwrite(fname, gcImgH[i]);
*/
}
cv::Mat black(cv::Size(PROJ_WIDTH,PROJ_HEIGHT), CV_8UC3, cv::Scalar(0,0,0));
cv::Mat white(cv::Size(PROJ_WIDTH,PROJ_HEIGHT), CV_8UC3, cv::Scalar(255,255,255));
// 标定亮暗水平
std::cout<<"Prepare step: begin to detect light level, wait for some seconds..."<<std::endl;
char wbuf[1];
wbuf[0]=CALIB_L;
n=write(fd,wbuf, 1);
if(n!=1){
std::cout<<"Write ttyUSB error!"<<std::endl;
}
cv::imshow(window_name, white);
cv::waitKey(500);
std::cout<<"Prepare step: begin to detect dark level, wait for some seconds..."<<std::endl;
wbuf[0]=CALIB_D;
n=write(fd,wbuf, 1);
if(n!=1){
std::cout<<"Write ttyUSB error!"<<std::endl;
}
cv::imshow(window_name, black);
cv::waitKey(500);
if((!board1_exist) && !(board2_exist)){
std::cout<<"No projecting board found!"<<std::endl;
return -1;
}
if(board1_exist){
while(x_current_pass[0]<COORDS_MULTIPASS){
calib_X(fd);
}
while(y_current_pass[0]<COORDS_MULTIPASS){
calib_Y(fd);
}
}
if(board2_exist){
while(x_current_pass[1]<COORDS_MULTIPASS){
calib_X(fd);
}
while(y_current_pass[1]<COORDS_MULTIPASS){
calib_Y(fd);
}
}
cv::Mat warp1(cv::Size(PROJ_WIDTH,PROJ_HEIGHT), CV_8UC3, cv::Scalar(0,0,0));
cv::Mat warp2(cv::Size(PROJ_WIDTH,PROJ_HEIGHT), CV_8UC3, cv::Scalar(0,0,0));
if(board1_exist){
std::cout<<"X & Y coordinates are stable, now to calibrate the projector..."<<std::endl;
// 输出坐标值
for(int i=0;i<SENSOR_NUMBER;i++){
std::cout<<"("<<x_coords[0][0][i]<<","<<y_coords[0][0][i]<<")"<<std::endl;
}
warp1=generate_max_display(1);
}
if(board2_exist){
std::cout<<"X & Y coordinates are stable, now to calibrate the projector..."<<std::endl;
// 输出坐标值
for(int i=0;i<SENSOR_NUMBER;i++){
std::cout<<"("<<x_coords[1][0][i]<<","<<y_coords[1][0][i]<<")"<<std::endl;
}
warp2=generate_max_display(2);
}
output_perspective_images(warp1,warp2);
cv::waitKey(10000);
// 等待接收线程结束
pthread_join(get_result_thread,&thread_ret);
}
int main ( int argc, char** argv)
{
int s = sizeof(local_uav);
gtk_init(&argc, &argv);
if (argc < 3)
{
printf("Use: ivy2serial ac_id serial_device\n");
printf("or\n");
printf("Use: ivy2serial ac_id serial_device xbee_power_level [0-4] to configure the xbee as broadcast, no retries\n");
return -1;
}
if (argc == 4)
{
printf("Programming XBee Modem\n");
power_level = (int) (argv[3][0]) - (int) '0';
if (power_level < 0)
power_level = 0;
else if (power_level > 4)
power_level = 4;
printf("Set Power Level To: '%d'\n", power_level);
}
else
{
power_level = -1;
}
local_uav.ac_id = atoi(argv[1]);
sprintf(status_str, "Listening to AC=%d, Serial Data Size = %d",local_uav.ac_id, s);
sprintf(status_ivy_str, "---");
sprintf(status_serial_str, "---");
printf("%s\n",status_str);
// Open Serial or Die
port = argv[2];
open_port(port);
// Init UAV
remote_uav.ac_id = 6;
remote_uav.phi = 1000;
remote_uav.theta = 200;
remote_uav.psi = -3140;
// Start IVY
IvyInit ("IVY <-> Serial", "IVY <-> Serial READY", NULL, NULL, NULL, NULL);
IvyBindMsg(on_Desired, NULL, "^%d DESIRED (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)",local_uav.ac_id);
IvyBindMsg(on_Estimator, NULL, "^%d ESTIMATOR (\\S*) (\\S*)",local_uav.ac_id);
IvyBindMsg(on_Navigation, NULL, "^%d NAVIGATION (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)",local_uav.ac_id);
IvyBindMsg(on_Attitude, NULL, "^%d ATTITUDE (\\S*) (\\S*) (\\S*)", local_uav.ac_id);
IvyBindMsg(on_Gps, NULL, "^%d GPS (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)",local_uav.ac_id);
IvyStart("127.255.255.255");
// Add Timer
gtk_timeout_add(delay / 4, timeout_callback, NULL);
// GTK Window
GtkWidget *window = gtk_window_new (GTK_WINDOW_TOPLEVEL);
gtk_window_set_title (GTK_WINDOW (window), "IVY_Serial_Bridge");
gtk_signal_connect (GTK_OBJECT (window), "delete_event",
GTK_SIGNAL_FUNC (delete_event), NULL);
GtkWidget *box = gtk_vbox_new(TRUE, 1);
gtk_container_add (GTK_CONTAINER (window), box);
GtkWidget *hbox = gtk_hbox_new(FALSE, 1);
gtk_container_add (GTK_CONTAINER (box), hbox);
status = gtk_label_new( "Status:" );
gtk_box_pack_start(GTK_BOX(hbox), status, FALSE, FALSE, 1);
gtk_label_set_justify( (GtkLabel*) status, GTK_JUSTIFY_LEFT );
status = gtk_label_new( status_str );
gtk_box_pack_start(GTK_BOX(hbox), status, FALSE, FALSE, 1);
gtk_label_set_justify( (GtkLabel*) status, GTK_JUSTIFY_LEFT );
hbox = gtk_hbox_new(FALSE, 1);
gtk_container_add (GTK_CONTAINER (box), hbox);
status_ivy = gtk_label_new( "IVY->SERIAL:" );
gtk_box_pack_start(GTK_BOX(hbox), status_ivy, FALSE, FALSE, 1);
gtk_label_set_justify( (GtkLabel*) status_ivy, GTK_JUSTIFY_LEFT );
status_ivy = gtk_label_new( status_ivy_str );
gtk_box_pack_start(GTK_BOX(hbox), status_ivy, FALSE, FALSE, 1);
gtk_label_set_justify( (GtkLabel*) status_ivy, GTK_JUSTIFY_LEFT );
hbox = gtk_hbox_new(FALSE, 1);
gtk_container_add (GTK_CONTAINER (box), hbox);
status_serial = gtk_label_new( "SERIAL->IVY:" );
gtk_box_pack_start(GTK_BOX(hbox), status_serial, FALSE, FALSE, 1);
gtk_label_set_justify( (GtkLabel*) status_serial, GTK_JUSTIFY_LEFT );
status_serial = gtk_label_new( status_serial_str );
gtk_label_set_justify( GTK_LABEL(status_serial), GTK_JUSTIFY_LEFT );
gtk_box_pack_start(GTK_BOX(hbox), status_serial, FALSE, FALSE, 1);
gtk_widget_show_all(window);
gtk_main();
// Clean up
fprintf(stderr,"Stopping\n");
return 0;
}
/*======================================
*Function: uart_com_thread
*description: this is communication thread with MP5
*Author: zorro
*Created Date :6/18/2015
*======================================*/
int main(void)
{
int serial_fd, recv_fd, maxfd;
fd_set inset, tmp_inset;
struct timeval tv;
unsigned loop = 1;
int real_read, i, real_write, data_len, rsp_counter, write_len = 0;;
int cmd_len;
int rsp_cmd;
static int step = 0;
unsigned char rcv_buf[BUFFER_SIZE];
unsigned char snd_buf[BUFFER_SIZE];
unsigned char data_t;
unsigned char errvalue;
int rcv_trans = 0;
/*The serial port data written to this file*/
if ((recv_fd = open(RECV_FILE_NAME, O_CREAT|O_WRONLY, 0644)) < 0){
perror("open");
printf("\n\n\nzorro, can't open recv_file: %s\n", strerror(errno));
return 1;
}
printf("the recv_file fd = %d,\n", recv_fd);
//fds[0] = STDIN_FILENO; /*The standard input*/
if ((serial_fd = open_port(TARGET_COM_PORT)) < 0){
perror("open_port");
printf("\n\n\nzorro, can't open target com port: %s\n", strerror(errno));
return 1;
}
printf("the serial_file fd = %d,\n", serial_fd);
/*config com*/
if (set_com_config(serial_fd, 115200, 8, 'N', 1) < 0){
perror("set_com_config");
printf("\n\n\nzorro, can't set com fonfig: %s\n", strerror(errno));
return 1;
}
printf("Input some words(enter 'quit' to exit):\n");
while (1)
{
if (!rcv_trans){ /*rcv uart data */
/*read frame head from serial port*/
real_read = read(serial_fd, &data_t, 1);
if ((real_read <= 0) && (errno != EAGAIN)){
printf("\nzorro, ERROR:READ FRAME HEAD error1 real_read = %d, \n", real_read);
}
switch (step){
case 0x00: //DA_DECODE_SYN_HEAD
if (data_t == 0xAA){
rcv_buf[0] = data_t;
step=1;
}
break;
case 0x01: //DA_DECODE_GET_SYN_COUNTER
if (data_t == 0xAA)
step = 0;
else{
rcv_buf[1] = data_t;
step = 2;
}
break;
case 0x02: //DA_DECODE_GET_DATA_LENGTH
if ((data_t < 2) ||(data_t > 17))
step = 0;
else{
data_len = rcv_buf[2] = data_t;
step = 3;
i = 0;
}
break;
case 0x03: //DA_DECODE_GET_DATA and DA_DECODE_CHECK
rcv_buf[3+i++] = data_t;
if (i == data_len){
step = 0; //reset the unpack step
int check;
check = check_frame_sum(rcv_buf, (data_len+3));
if (!check){
if (msg_unpack(rcv_buf,&rsp_cmd, &errvalue) < 0){
printf("zorro, ERROR:cmd unpack err\n");
}
else{
rcv_trans = 1;
}
rcv_buf[data_len+3] = '\0';
/*write the data to commen file*/
write(recv_fd, rcv_buf, data_len+3);
}
else if (check < 0){
printf("\nzorro, ERROR:check sum error \n");
}
}
break;
}
}
else{ /*transmit uart data */
if (msg_pack(snd_buf,rsp_cmd, rsp_counter, errvalue, &write_len) == 0){
rcv_trans = 0;
}
/*read frame head from serial port*/
real_write = write(serial_fd, &snd_buf, write_len);
if ((real_write <= 0) && (errno != EAGAIN)){
printf("\nzorro, ERROR:write cmd error1 real_write = %d, \n", real_write);
}
if ((write_len - real_write) != 0)
printf("error!\n");
else
rsp_counter ++;
}
}
}
int unibo_filtertest_thread_main(int argc, char *argv[])
{
warnx("main thread started");
thread_running = true;
int unibo_ref_pub_fd;
int unibo_param_pub_fd;
int unibo_opti_pub_fd;
/* default values for arguments */
// use (ttyS2) for UART5 in px4fum_v1
char *uart_name = (char*)"/dev/ttyS6"; //(ttyS2)--> UART5,
int baudrate = 115200;
const char *commandline_usage = "\tusage: %s -d <devicename> -b <baudrate> [-v/--verbose] [--debug]\n\t\tdefault: -d %s -b %i\n";
/* read program arguments */
int i;
for (i = 1; i < argc; i++) { /* argv[0] is "start/stop/status" */
if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) {
warnx(commandline_usage, argv[0], uart_name, baudrate);
thread_should_exit = true;
return 0;
}
/* UART device ID */
if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) {
if (argc > i + 1) {
uart_name = argv[i + 1];
} else {
warnx(commandline_usage, argv[0], uart_name, baudrate);
thread_should_exit = true;
return 0;
}
}
/* baud rate */
if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--baud") == 0) {
if (argc > i + 1) {
baudrate = atoi(argv[i + 1]);
} else {
warnx(commandline_usage, argv[0], uart_name, baudrate);
thread_should_exit = true;
return 0;
}
}
}
// SETUP SERIAL PORT
// Exit if opening port failed
// Open the serial port.
if (!silent) warnx("Trying to connect to %s.. ", uart_name);
fflush(stdout);
fd = open_port(uart_name);
if (fd == -1)
{
warnx("failure, could not open port.\n");
exit(EXIT_FAILURE);
}
else
{
warnx("success.\n");
}
warnx("Trying to configure %s.. ", uart_name);
bool setup = setup_port(fd, baudrate, 8, 1, false, false);
if (!setup)
{
warnx("failure, could not configure port.\n");
//exit(EXIT_FAILURE);
}
else
{
warnx("success.\n");
}
int noErrors = 0;
if (fd == -1 || fd == 0)
{
fprintf(stderr, "Connection attempt to port %s with %d baud, 8N1 failed, exiting.\n", uart_name, baudrate);
//exit(EXIT_FAILURE);
}
else
{
fprintf(stderr, "\nConnected to %s with %d baud, 8 data bits, no parity, 1 stop bit (8N1)\n", uart_name, baudrate);
}
if(fd < 0)
{
//exit(noErrors);
}
// Round Buffer for REF packet
static char round_buffer_PACK[LENGTH*4];
static char packet_PACK[LENGTH];
int pos_PACK=0;
int start_PACK=0;
int lastSidx_PACK = -1;
bool PACK_ready = false;
bool updated;
//Topics advertise
// subscribe to sensor_combined topic
int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
orb_set_interval(sensor_sub_fd, 10);
int gps_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
orb_set_interval(gps_sub_fd, 200);
int att_sub_fd = orb_subscribe(ORB_ID(vehicle_attitude));
orb_set_interval(att_sub_fd, 10);
// Run indefinitely while the serial loop handles data
warnx("\nREADY, waiting for serial data.\n");
/* Main loop*/
while (!thread_should_exit) {
orb_check(sensor_sub_fd, &updated);
if (updated){
struct sensor_combined_s sens;
orb_copy(ORB_ID(sensor_combined),sensor_sub_fd,&sens);
struct vehicle_attitude_s att;
orb_copy(ORB_ID(vehicle_attitude),att_sub_fd,&att);
struct vehicle_gps_position_s gps;
orb_copy(ORB_ID(vehicle_gps_position),gps_sub_fd,&gps);
serial_write(fd, sens,att,gps);
}
//usleep(20000);
}
close_port(fd);
thread_running = false;
return 0;
}
int main(void){
int ret = -1;
pthread_t thread_uartRec;
int on = 1;
//Initialize socket
if((sockfd = socket(AF_INET, SOCK_STREAM,0))==-1){
perror("socket");
exit(1);
};
bzero(&servaddr, sizeof(servaddr));
servaddr.sin_family = AF_INET;
servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
servaddr.sin_port = htons(SERV_PORT);
if (setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on)) < 0){
perror("sockopt error");
exit(1);
}
if(bind(sockfd, (struct sockaddr *)&servaddr,sizeof(servaddr)) == -1){
perror("bind error");
exit(1);
}
if(listen(sockfd,10)<0){
perror("listen");
exit(1);
}
//Initialize UART2
if((uartfd=open_port(uartfd,4)) < 0)//打开串口 3
{
perror("open_port error1\n");
exit(1);
}
if((ret=set_opt(uartfd,9600,8,'N',1)) < 0)//设置串口 9600 8 N 1
{
perror("set_opt error1\n");
exit(1);
}
/*Initialize list*/
clientListHead = initList();
/*Intialize semaphore*/
ret = sem_init(&uart_sem, 0, 1);
if(ret == -1){
perror("semaphore create fail!");
exit(EXIT_FAILURE);
}
ret = pthread_create (&thread_uartRec, NULL, (void*)(&pthread_UART2WiFi), NULL);
if (ret != 0)
{
perror ("pthread_create error!");
}
while(1){
pthread_t thread_id = 0;
size_t len = sizeof(struct sockaddr);
clifd = accept(sockfd, (struct sockaddr *)&cliaddr, &len);
if(clifd < 0){
perror("error accept\n");
}
appendList(clifd);
/*Create Thread to send CMD from uart to wifi*/
ret = pthread_create (&thread_id, NULL, (void*)(&pthread_wifiUart), (void*)(&clifd));
if (ret != 0)
{
perror ("pthread_create error!");
}
}
ret = shutdown(sockfd,SHUT_WR);
assert(ret != -1);
return 0;
}
int main(int argc, char *argv[])
{
int ret = ERROR_OK;
int err_count = 0;
int rcv_err_count = 0;
bool msg_received = false;
char* device;
/* Para parsear el los mensajes se recorre el arreglo con un puntero
* a enteros de dos bytes.
*/
int16_t *ch_buff;
//char str[128]; // dbg
// check input arguments
if(argc<2)
{
err_log(HOW_TO);
return -1;
}
else device = argv[1];
struct timeval tv_in;
struct timeval tv_end;
struct timeval tv_diff;
#if DEBUG_TIMING_SBUSD
struct timeval tv_last;
#endif //#if DEBUG_TIMING_SBUSD
#if !PC_TEST
fd = open_port(device);
if (fd == -1)
{
return -1;
}
configure_port(fd);
ret = custom_baud(fd);
if (ret < 0)
{
err_log_stderr("custom_baud() failed!");
return ret;
}
#endif
/**
* Inherit priority from main.c for correct IPC.
*/
if(setpriority(PRIO_PROCESS, 0, -18) == -1) //requires being superuser
{
err_log_num("setpriority() failed!",errno);
return -1;
}
// Catch signals
prctl(PR_SET_PDEATHSIG, SIGHUP);
signal(SIGHUP, uquad_sig_handler);
signal(SIGINT, uquad_sig_handler);
signal(SIGQUIT, uquad_sig_handler);
#if PC_TEST
futaba_sbus_begin(); //para tiempo de start
#endif //PC_TEST
// Lleva a cero todos los canales y el mensaje sbus
futaba_sbus_set_channel(ROLL_CHANNEL, 1500); //init roll en cero
futaba_sbus_set_channel(PITCH_CHANNEL, 1500); //init pitch en cero
futaba_sbus_set_channel(YAW_CHANNEL, 1500); //init yaw en cero
futaba_sbus_set_channel(THROTTLE_CHANNEL, 950); //init throttle en minimo
futaba_sbus_set_channel(FLIGHTMODE_CHANNEL, 1500); //inint flight mode 2
futaba_sbus_update_msg();
sleep_ms(500); //Para ponerme a tiro con main
bool main_ready = false;
// -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
// Loop
// -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
for(;;)
{
gettimeofday(&tv_in,NULL);
//printf("errores: %d\n",err_count);//dbg
#if !PC_TEST
if (err_count > MAX_ERR_SBUSD)
{
err_log("error count exceded");
//err_count = 0;
quit();
}
#endif
ret = uquad_read(&rbuf);
if(ret == ERROR_OK)
{
if(!main_ready) main_ready = true;
//err_log("read ok!");
msg_received = true;
// Parse message. 2 bytes per channel.
ch_buff = (int16_t *)rbuf.mtext;
// send ack
ret = uquad_send_ack();
if(ret != ERROR_OK)
{
err_log("Failed to send ack!");
}
if (rcv_err_count > 0)
rcv_err_count = 0;
} else {
//err_log("Failed to read msg!");
msg_received = false;
rcv_err_count++;
if (main_ready && rcv_err_count > 3) {
err_count++;
rcv_err_count = 0;
}
}
if(msg_received)
{
futaba_sbus_set_channel(ROLL_CHANNEL, ch_buff[ROLL_CH_INDEX]);
futaba_sbus_set_channel(PITCH_CHANNEL, ch_buff[PITCH_CH_INDEX]);
futaba_sbus_set_channel(YAW_CHANNEL, ch_buff[YAW_CH_INDEX]);
futaba_sbus_set_channel(THROTTLE_CHANNEL, ch_buff[THROTTLE_CH_INDEX]);
//futaba_sbus_set_channel(7, ch_buff[FLIGHTMODE_CH_INDEX]); // flight mode no se modifica
// Comando para activar failsafe
if ( (ch_buff[FAILSAFE_CH_INDEX] == ACTIVATE_FAILSAFE) &&
(futaba_sbus_get_failsafe() == SBUS_SIGNAL_OK) )
futaba_sbus_set_failsafe(SBUS_SIGNAL_FAILSAFE);
// Comando para desactivar failsafe
if ( (ch_buff[FAILSAFE_CH_INDEX] == DEACTIVATE_FAILSAFE) &&
(futaba_sbus_get_failsafe() == SBUS_SIGNAL_FAILSAFE) )
futaba_sbus_set_failsafe(SBUS_SIGNAL_OK);
futaba_sbus_update_msg();
msg_received = false;
//print_sbus_data(); // dbg
}
#if !PC_TEST
ret = futaba_sbus_write_msg(fd);
if (ret < 0)
{
err_count++; // si fallo al enviar el mensaje se apagan los motores!!
}
else
{
/// This loop was fine
if(err_count > 0)
err_count--;
}
sleep_ms(5); //TODO revisar si hay que hacerlo siempre!!
#else
sleep_ms(5); //TODO revisar si hay que hacerlo siempre!!
#endif
// Escribe el mensaje a stdout - dbg
//convert_sbus_data(str);
//printf("%s",str);
/// Control de tiempo
gettimeofday(&tv_end,NULL);
ret = uquad_timeval_substract(&tv_diff, tv_end, tv_in);
if(ret > 0)
{
if(tv_diff.tv_usec < LOOP_T_US)
usleep(LOOP_T_US - (unsigned long)tv_diff.tv_usec);
#if DEBUG_TIMING_SBUSD
gettimeofday(&tv_end,NULL);
uquad_timeval_substract(&tv_diff, tv_end, tv_in);
printf("duracion loop sbusd (14ms): %lu\n",(unsigned long)tv_diff.tv_usec);
#endif
} else {
err_log("WARN: Absurd timing!");
err_count++; // si no cumplo el tiempo de loop falla la comunicacion sbus
}
//printf("rcv_err_count: %d\n", rcv_err_count);
//printf("err_count: %d\n", err_count);
} //for(;;)
return 0; //never gets here
}
int send_cmd_with_delay(char *portname, uint8_t *write_bytes, int writelen, uint8_t *read_bytes, int delay) {
// initialize read_bytes to 0
read_bytes[0] = '\0';
char errmsg[255];
char bytecheck[8];
snprintf(errmsg, 255, "Port: %s", portname);
rblog(RBLOG_DEBUG, errmsg);
int fd = open_port(portname);
uint8_t tmpbuf[64];
size_t bufout_idx = 1; /* first element will be length, data starts 2nd element */
//Needed to use select
fd_set input;
int max_fd;
int s_ret;
FD_ZERO(&input);
FD_SET(fd, &input);
struct timeval timeout;
max_fd = fd + 1;
timeout.tv_sec = 1;
timeout.tv_usec = 0;
// Give port some time to open
// Might be able to be removed
usleep(delay);
//debug
errmsg[0] = '\0';
bytecheck[0] = '\0';
if (rbloggable(RBLOG_DEBUG)) {
snprintf(errmsg, 255, "Sending bytes: ");
size_t idx2 = 0;
while (idx2 < writelen)
{
snprintf(bytecheck, 8, "%02x", (int)write_bytes[idx2]);
strncat(errmsg, bytecheck, 8);
idx2++;
}
rblog(RBLOG_DEBUG, errmsg);
}
// end debug
/* writing writelen write_bytes to the serial port */
int bytes_written = write(fd, write_bytes, writelen);
snprintf(errmsg, 255, "Wrote %d bytes to port", bytes_written);
rblog(RBLOG_DEBUG, errmsg);
// Don't need delay with select
s_ret = select(max_fd, &input, NULL, NULL, &timeout);
if (s_ret < 0) {
snprintf(errmsg, 255, "Selecting port failed");
rblog(RBLOG_ERROR, errmsg);
return -4;
}
int exit_code;
if (FD_ISSET(fd, &input)) {
snprintf(errmsg, 255, "Input on port");
rblog(RBLOG_DEBUG, errmsg);
// -- Process input
size_t len = read(fd, tmpbuf, sizeof(tmpbuf));
size_t retry_count = 0;
size_t eagain_max_retry = 3;
while ((EAGAIN == errno) && (retry_count < eagain_max_retry))
{
snprintf(errmsg, 255, "Recieved: '%s (%d)', retrying(%zu)", strerror(errno), errno, retry_count);
rblog(RBLOG_WARN, errmsg);
errno = 0;
len = read(fd, tmpbuf, sizeof(tmpbuf));
retry_count++;
}
if (len == -1) {
snprintf(errmsg, 255, "Error reading from port: %s", strerror(errno));
rblog(RBLOG_ERROR, errmsg);
exit_code = -1;
} else if (len == 0) {
snprintf(errmsg, 255, "No data from port");
rblog(RBLOG_ERROR, errmsg);
exit_code = -2;
} else if ((len > 0) && (len <= MAX_READ_LEN)) {
/* Success Condition */
errmsg[0] = '\0';
bytecheck[0] = '\0';
snprintf(errmsg, 255, "Bytes from serial port:");
read_bytes[0] = len;
while (bufout_idx < len + 1)
{
snprintf(bytecheck, 8, "%02x", (int)tmpbuf[bufout_idx - 1]);
strncat(errmsg, bytecheck, 8);
read_bytes[bufout_idx] = tmpbuf[bufout_idx - 1];
bufout_idx++;
}
rblog(RBLOG_DEBUG, errmsg);
exit_code = 0;
} else {
snprintf(errmsg, 255, "Error in data length");
rblog(RBLOG_ERROR, errmsg);
exit_code = -3;
}
} else {
// attemptint to process input but no input available
snprintf(errmsg, 255, "Input on port");
rblog(RBLOG_DEBUG, errmsg);
exit_code = -6;
}
snprintf(errmsg, 255, "closing port: %d", fd);
rblog(RBLOG_DEBUG, errmsg);
close(fd);
snprintf(errmsg, 255, "closed port: %d", fd);
rblog(RBLOG_DEBUG, errmsg);
snprintf(errmsg, 255, "exit code: %d", exit_code);
rblog(RBLOG_DEBUG, errmsg);
return exit_code;
}
return (fd);
}
int mainfd=0;
int num, n; /* File descriptor */
char chout[16];
char serialBuffer[16];
char preBuffer[16];
char scannerInput[12];
char scaleInput[9];
char scaleBuffer[9] = "000000000";
struct termios options;
char serialInput;
mainfd = open_port();
fcntl(mainfd, F_SETFL, FNDELAY); /* Configure port reading */
/* Get the current options for the port */
tcgetattr(mainfd, &options);
cfsetispeed(&options, B9600); /* Set the baud rates to 9600 */
cfsetospeed(&options, B9600);
/* Enable the receiver and set local mode */
options.c_cflag |= (CLOCAL | CREAD);
/* 7 bits, odd parity */
options.c_cflag |= PARENB;
options.c_cflag |= PARODD; /* odd parity */
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS7;
int main(int args, char *argv[])
{
int server;
int receive_length, line_length;
char ip_addr[BUFSIZE];
char *line = NULL;
char buf[BUFSIZE];
FILE *fp = NULL;
SSL_CTX *ctx;
// Thread
pthread_t* threads[2];
if (pthread_mutex_init(&mutex, NULL) != 0)
{
printf("mutex init failed\n");
exit(-1);
}
my_ip_addr = get_ip_addr();
printf("My ip addr is: %s\n", my_ip_addr);
/* READ INPUT FILE */
fp = fopen("config_file", "r");
if(fp == NULL){
fprintf(stderr, "Error opening config file with name 'config_file'. Exiting.\n");
exit(1);
}
fp_output = fopen("Lab4-E-2.txt", "w");
if(fp_output == NULL){
fprintf(stderr, "Error opening config file with name 'Lab4-E-2.txt'. Exiting.\n");
exit(1);
}
printf("Reading input file...\n");
while(getline(&line, &line_length, fp) > 0){
if(strstr(line, "host_ip") != NULL){
sscanf(line, "host_ip: %s\n", ip_addr);
}
else if(strstr(line, "port") != NULL){
sscanf(line, "port: %d\n", &port);
}
else if(strstr(line, "range") != NULL){
sscanf(line, "range: %d\n", &range);
}
else if(strstr(line, "rate") != NULL){
sscanf(line, "rate: %d\n", &rate);
}
else{
fprintf(stderr, "Unrecognized line found: %s. Ignoring.\n", line);
}
}
fclose(fp);
/* FINISH READING INPUT FILE */
printf("Connecting to: %s:%d\n", ip_addr, port);
fprintf(fp_output,"Connecting to: %s:%d\n", ip_addr, port);
/* SET UP TLS COMMUNICATION */
SSL_library_init();
ctx = initialize_client_CTX();
server = open_port(ip_addr, port);
ssl = SSL_new(ctx);
SSL_set_fd(ssl, server);
/* FINISH SETUP OF TLS COMMUNICATION */
/* SEND HEART RATE TO SERVER */
if (SSL_connect(ssl) == -1){ //make sure connection is valid
fprintf(stderr, "Error. TLS connection failure. Aborting.\n");
ERR_print_errors_fp(stderr);
exit(1);
}
else {
printf("Client-Server connection complete with %s encryption\n", SSL_get_cipher(ssl));
fprintf(fp_output, "Client-Server connection complete with %s encryption\n", SSL_get_cipher(ssl));
fflush(fp_output);
display_server_certificate(ssl);
}
// Thread
pthread_create(&threads[0],NULL,worker_read,NULL);
while(1)
{
printf("Current settings: rate: %d, range: %d\n", rate, range);
heart_rate =
generate_random_number(AVERAGE_HEART_RATE-(double)range, AVERAGE_HEART_RATE+(double)range);
memset(buf,0,sizeof(buf)); //clear out the buffer
//populate the buffer with information about the ip address of the client and the heart rate
sprintf(buf, "Heart rate of patient %s is %4.2f", my_ip_addr, heart_rate);
printf("Sending message '%s' to server...\n", buf);
fprintf(fp_output,"Sending message '%s' to server...\n", buf);
fflush(fp_output);
SSL_write(ssl, buf, strlen(buf));
sleep(rate);
}
// Thread
pthread_join(threads[0],NULL);
/* FINISH HEART RATE TO SERVER */
//clean up operations
SSL_free(ssl);
close(server);
SSL_CTX_free(ctx);
return 0;
}
error_code cellAudioPortOpen(vm::ptr<CellAudioPortParam> audioParam, vm::ptr<u32> portNum)
{
cellAudio.warning("cellAudioPortOpen(audioParam=*0x%x, portNum=*0x%x)", audioParam, portNum);
const auto g_audio = g_idm->lock<named_thread<audio_thread>>(0);
if (!g_audio)
{
return CELL_AUDIO_ERROR_NOT_INIT;
}
if (!audioParam || !portNum)
{
return CELL_AUDIO_ERROR_PARAM;
}
const u64 channel = audioParam->nChannel;
const u64 block = audioParam->nBlock;
const u64 attr = audioParam->attr;
// check attributes
if (channel != CELL_AUDIO_PORT_2CH &&
channel != CELL_AUDIO_PORT_8CH &&
channel)
{
return CELL_AUDIO_ERROR_PARAM;
}
if (block != CELL_AUDIO_BLOCK_8 &&
block != CELL_AUDIO_BLOCK_16 &&
block != 2 &&
block != 4 &&
block != 32)
{
return CELL_AUDIO_ERROR_PARAM;
}
// list unsupported flags
if (attr & CELL_AUDIO_PORTATTR_BGM)
{
cellAudio.todo("cellAudioPortOpen(): CELL_AUDIO_PORTATTR_BGM");
}
if (attr & CELL_AUDIO_PORTATTR_OUT_SECONDARY)
{
cellAudio.todo("cellAudioPortOpen(): CELL_AUDIO_PORTATTR_OUT_SECONDARY");
}
if (attr & CELL_AUDIO_PORTATTR_OUT_PERSONAL_0)
{
cellAudio.todo("cellAudioPortOpen(): CELL_AUDIO_PORTATTR_OUT_PERSONAL_0");
}
if (attr & CELL_AUDIO_PORTATTR_OUT_PERSONAL_1)
{
cellAudio.todo("cellAudioPortOpen(): CELL_AUDIO_PORTATTR_OUT_PERSONAL_1");
}
if (attr & CELL_AUDIO_PORTATTR_OUT_PERSONAL_2)
{
cellAudio.todo("cellAudioPortOpen(): CELL_AUDIO_PORTATTR_OUT_PERSONAL_2");
}
if (attr & CELL_AUDIO_PORTATTR_OUT_PERSONAL_3)
{
cellAudio.todo("cellAudioPortOpen(): CELL_AUDIO_PORTATTR_OUT_PERSONAL_3");
}
if (attr & CELL_AUDIO_PORTATTR_OUT_NO_ROUTE)
{
cellAudio.todo("cellAudioPortOpen(): CELL_AUDIO_PORTATTR_OUT_NO_ROUTE");
}
if (attr & 0xFFFFFFFFF0EFEFEEULL)
{
cellAudio.todo("cellAudioPortOpen(): unknown attributes (0x%llx)", attr);
}
// Open audio port
const auto port = g_audio->open_port();
if (!port)
{
return CELL_AUDIO_ERROR_PORT_FULL;
}
port->channel = ::narrow<u32>(channel);
port->block = ::narrow<u32>(block);
port->attr = attr;
port->size = ::narrow<u32>(channel * block * AUDIO_SAMPLES * sizeof(f32));
port->tag = 0;
if (attr & CELL_AUDIO_PORTATTR_INITLEVEL)
{
port->level = audioParam->level * g_cfg.audio.volume / 100.0f;
}
else
{
port->level = g_cfg.audio.volume / 100.0f;
}
port->level_set.store({ port->level, 0.0f });
*portNum = port->number;
return CELL_OK;
}
Xbee::Xbee(std::string path, int DATASIZE)
{
open_port(path, DATASIZE);
}
void test_ipc_6_receiver(PROCESS self, PARAM param)
{
PORT port1;
PORT port2;
PORT port3;
PROCESS sender_1;
PROCESS sender_2;
PROCESS sender_3;
int second_sender;
int *data1;
int *data2;
int *data3;
check_sum += 1;
port1 = self->first_port;
kprintf("%s: creating port 3 and port 2...\n", self->name);
port3 = create_port();
port2 = create_port();
kprintf("%s: closing port 3 and port 2...\n", self->name);
close_port(port2);
close_port(port3);
check_port(port1, self->name, TRUE);
check_port(port2, self->name, FALSE);
check_port(port3, self->name, FALSE);
if (test_result != 0)
test_failed(test_result);
create_process(test_ipc_6_sender_process_3, 5, (PARAM) port3, "Sender 3");
create_process(test_ipc_6_sender_process_2, 5, (PARAM) port2, "Sender 2");
create_process(test_ipc_6_sender_process_1, 4, (PARAM) port1, "Sender 1");
/*
* receive first message
*/
kprintf("%s: receiving first message...\n", self->name);
data1 = (int*) receive(&sender_1);
kprintf("\n%s: received a message from %s, parameter = %d.\n",
self->name, sender_1->name, * data1);
if (string_compare(sender_1->name, "Sender 2") == 1)
test_failed(60);
if (string_compare(sender_1->name, "Sender 3") == 1)
test_failed(60);
if (string_compare(sender_1->name, "Sender 1") != 1)
test_failed(58);
check_process("Sender 3", STATE_MESSAGE_BLOCKED, FALSE);
check_process("Sender 2", STATE_MESSAGE_BLOCKED, FALSE);
check_process("Sender 1", STATE_REPLY_BLOCKED, FALSE);
if (test_result != 0) {
print_all_processes(kernel_window);
test_failed(test_result);
}
/*
* receive second message
*/
kprintf("%s: opening port 2 and port 3...\n", self->name);
open_port(port2);
open_port(port3);
check_sum += 2; // to check that first message is receiverd.
kprintf("%s: receiving second message...\n", self->name);
data2 = (int*) receive (&sender_2);
kprintf("%s: received a message from %s, parameter = %d.\n",
self->name, sender_2->name, * data2);
// second message can be from either sender 2 or sender 3, depending
// on implementation of create_port().
second_sender = 0;
if (string_compare(sender_2->name, "Sender 2") == 1) {
check_process("Sender 2", STATE_READY, TRUE);
check_process("Sender 3", STATE_MESSAGE_BLOCKED, FALSE);
second_sender = 2;
} else if (string_compare(sender_2->name, "Sender 3") == 1) {
check_process("Sender 3", STATE_READY, TRUE);
check_process("Sender 2", STATE_MESSAGE_BLOCKED, FALSE);
second_sender = 3;
} else
test_failed(44);
check_sum += 4; // to check that second message is received.
/*
* receive third message
*/
kprintf("%s: receiving third message...\n", self->name);
data3 = (int*) receive(&sender_3);
kprintf("%s: received a message from %s, parameter = %d.\n",
self->name, sender_3->name, * data3);
if (string_compare(sender_3->name, "Sender 1") == 1)
test_failed(44);
if (second_sender == 2) {
if (string_compare(sender_3->name, "Sender 2") == 1)
test_failed(44);
if (string_compare(sender_3->name, "Sender 3") != 1)
test_failed(58);
} else {
if (string_compare(sender_3->name, "Sender 3") == 1)
test_failed(44);
if (string_compare(sender_3->name, "Sender 2") != 1)
test_failed(58);
}
check_process("Sender 3", STATE_READY, TRUE);
check_process("Sender 2", STATE_READY, TRUE);
check_process("Sender 1", STATE_REPLY_BLOCKED, FALSE);
if (test_result != 0) {
print_all_processes(kernel_window);
test_failed(test_result);
}
check_sum += 8; // to check that third message is received.
return_to_boot();
}
int gps_main(int argc, char *argv[])
{
// print text
printf("Hello, %s!\n",APPNAME);
usleep(100000);
/* initialize shared data structures */
global_data_init(&global_data_gps.access_conf);
global_data_init(&global_data_sys_status.access_conf);
/* default values */
char * commandline_usage = "\tusage: gps -d devicename -m mode\n";
char * device = "/dev/ttyS3";
char * mode = "nmea";
/* read arguments */
int i;
for (i = 1; i < argc; i++) //argv[0] is "mavlink"
{
if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) //device set
{
if(argc > i+1)
{
device = argv[i+1];
}
else
{
printf(commandline_usage);
return 0;
}
}
if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--mode") == 0) //device set
{
if(argc > i+1)
{
mode = argv[i+1];
}
else
{
printf(commandline_usage);
return 0;
}
}
}
/* open port (baud rate is set in defconfig file) */
int fd = open_port(device);
if(fd != -1)
{
printf("\t%s: Port opened: %s\n", APPNAME, device);
}
else
{
printf("\t%s: Could not open port, exiting gps app!\n", APPNAME);
sleep(1);
return 0;
}
//TODO: add mode if here (reads from arguments and configuration, watchdog will look for changes in mode configuration while running (?))
/* Init mutex for datasharing between ubx gps reading thrad (ubx_thread) and ubx_watchdog thread*/
pthread_mutex_init(&ubx_mutex, NULL);
/* Initialize ubx state */
ubx_state = malloc(sizeof(gps_bin_ubx_state_t));
ubx_decode_init();
/* create pthreads */
pthread_create (&ubx_thread, NULL, ubx_loop, (void *)&fd);
/* wait before starting watchdog */
sleep(5);
pthread_create (&ubx_watchdog_thread, NULL, ubx_watchdog_loop, (void *)&fd);
/* wait for threads to complete */
pthread_join(ubx_thread, NULL);
pthread_join(ubx_watchdog_thread, (void *)&fd);
return 0;
}
BIF_RETTYPE erts_internal_open_port_2(BIF_ALIST_2)
{
BIF_RETTYPE ret;
Port *port;
Eterm res;
char *str;
int err_type, err_num;
ErtsLinkData *ldp;
ErtsLink *lnk;
port = open_port(BIF_P, BIF_ARG_1, BIF_ARG_2, &err_type, &err_num);
if (!port) {
if (err_type == -3) {
ASSERT(err_num == BADARG || err_num == SYSTEM_LIMIT);
if (err_num == BADARG)
res = am_badarg;
else if (err_num == SYSTEM_LIMIT)
res = am_system_limit;
else
/* this is only here to silence gcc, it should not happen */
BIF_ERROR(BIF_P, EXC_INTERNAL_ERROR);
} else if (err_type == -2) {
str = erl_errno_id(err_num);
res = erts_atom_put((byte *) str, sys_strlen(str), ERTS_ATOM_ENC_LATIN1, 1);
} else {
res = am_einval;
}
BIF_RET(res);
}
ldp = erts_link_create(ERTS_LNK_TYPE_PORT, BIF_P->common.id, port->common.id);
ASSERT(ldp->a.other.item == port->common.id);
ASSERT(ldp->b.other.item == BIF_P->common.id);
/*
* This link should not already be present, but can potentially
* due to id wrapping...
*/
lnk = erts_link_tree_lookup_insert(&ERTS_P_LINKS(BIF_P), &ldp->a);
erts_link_tree_insert(&ERTS_P_LINKS(port), &ldp->b);
if (port->drv_ptr->flags & ERL_DRV_FLAG_USE_INIT_ACK) {
/* Copied from erl_port_task.c */
port->async_open_port = erts_alloc(ERTS_ALC_T_PRTSD,
sizeof(*port->async_open_port));
erts_make_ref_in_array(port->async_open_port->ref);
port->async_open_port->to = BIF_P->common.id;
/*
* We unconditionaly *must* do a receive on a message
* containing the reference after this...
*/
ERTS_RECV_MARK_SAVE(BIF_P);
ERTS_RECV_MARK_SET(BIF_P);
res = erts_proc_store_ref(BIF_P, port->async_open_port->ref);
} else {
res = port->common.id;
}
if (IS_TRACED_FL(BIF_P, F_TRACE_PROCS))
trace_proc(BIF_P, ERTS_PROC_LOCK_MAIN, BIF_P,
am_link, port->common.id);
ERTS_BIF_PREP_RET(ret, res);
erts_port_release(port);
if (lnk)
erts_link_release(lnk);
return ret;
}
/// Main function
int main(int argc, char **argv) {
// default values for options
const char
*defaultbus = "127.255.255.255:2010",
*bus = defaultbus,
*defaultdevice = "/dev/rfcomm0";
device = defaultdevice;
long delay = 1000;
// parse options
char c;
while ((c = getopt (argc, argv, "hab:d:i:s:")) != EOF) {
switch (c) {
case 'h':
print_usage(argc, argv);
exit(EXIT_SUCCESS);
break;
case 'a':
want_alive_msg = TRUE;
break;
case 'b':
bus = optarg;
break;
case 'd':
device = optarg;
break;
case 'i':
ac_id = atoi(optarg);
break;
case 's':
delay = atoi(optarg)*1000;
break;
case '?':
if (optopt == 'a' || optopt == 'b' || optopt == 'd' || optopt == 's')
fprintf (stderr, "Option -%c requires an argument.\n", optopt);
else if (isprint (optopt))
fprintf (stderr, "Unknown option `-%c'.\n", optopt);
else
fprintf (stderr, "Unknown option character `\\x%x'.\n", optopt);
print_usage(argc, argv);
exit(EXIT_FAILURE);
default:
abort ();
}
}
// make Ctrl-C stop the main loop and clean up properly
signal(SIGINT, sigint_handler);
bzero (packet, PACKET_LENGTH);
open_port(device);
// setup Ivy communication
IvyInit("kestrel2ivy", "READY", 0, 0, 0, 0);
IvyStart(bus);
// create timer (Ivy)
tid = TimerRepeatAfter (0, delay, handle_timer, 0);
/* main loop */
#if IVYMINOR_VERSION == 8
IvyMainLoop (NULL,NULL);
#else
IvyMainLoop ();
#endif
return 0;
}
