int read_config_int_tab(char * name, char * table, char * tag, int * value)
{
char buffer[500];
//KK
erret(read_config_string(name, tag, buffer, sizeof(buffer)));
erret(read_config_int(table, buffer, value));
return(0);
}
int main(int argc, char *argv[])
{
int policy;
struct sched_param param;
pthread_attr_t attr;
char buffer[1024];
int err;
dprintf("init\n");
if(argc != 2) {
perr(AR_ERR_BAD_INIT_PARAM);
printf("Useage: %s INSTANCE\n", AR_MODULE_NAME);
return(AR_ERR_BAD_INIT_PARAM);
}
memset(&config, 0, sizeof(config));
erret(init_config(AR_MODULE_NAME, atoi(argv[1]), &config.base));
erret(read_config_int(config.base.name, "PRIORITY", &config.priority));
erret(read_config_int(config.base.name, "PRIORITY_DGPS_CORR_DATA", &config.priority_dgps_corr_data));
erret(read_config_string(config.base.name, "DEVICE", config.ser_device, sizeof(config.ser_device)));
err = read_config_int_tab(config.base.name, "QUEUE_INDEX", "QUEUE_IN_GPS_CORR", &config.queue_corr_data);
if(err != 0) {
erret(read_config_string(config.base.name, "DEVICE_CORR_DATA_READ", config.ser_device_corr_data_read, sizeof(config.ser_device_corr_data_read)));
// fprintf(stderr, "DGPS correction data from ser. device <%s>\n", config.ser_device_corr_data_read);
} else {
// fprintf(stderr, "DGPS correction data from queue <%d>\n", config.queue_corr_data);
}
memset(buffer, 0, sizeof(buffer));
config.corr_mode = -1;
erret(read_config_string(config.base.name, "CORRECTION_MODE", buffer, sizeof(buffer)));
if(strnicmp(buffer, "RTCA", strlen("RTCA")) == 0) {
config.corr_mode = OEM4_RTCA_MODE;
} else if(strnicmp(buffer, "RTCM", strlen("RTCM")) == 0) {
config.corr_mode = OEM4_RTCM_MODE;
} else {
perrs(AR_ERR_BAD_INIT_PARAM, "Correction data mode not specified (RTCA | RTCM).");
// return(AR_ERR_BAD_INIT_PARAM);
}
erret(read_config_int_tab(config.base.name, "SHM_INDEX", "SHM_OUT_GPS_POS", &config.ishm_pos_out));
erret(read_config_int_tab(config.base.name, "SHM_INDEX", "SHM_OUT_GPS_VEL", &config.ishm_vel_out));
if(read_config_int(config.base.name, "UTC_OFFSET", &config.utc_offset)) {
config.utc_offset = 15;
}
erret(SHM_Init());
erret(SHM_InitSlot(config.ishm_pos_out, sizeof(gps_pos_novatel_t)));
erret(SHM_InitSlot(config.ishm_vel_out, sizeof(gps_vel_novatel_t)));
if(config.queue_corr_data != 0) {
erret(Q_Init());
erret(Q_InitSlot(config.queue_corr_data, sizeof(gps_corr_t), MAX_GPS_CORRECTION_Q_ITEMS_NUMBER, 1));
} else if (strlen(config.ser_device_corr_data_read)>0){
erret(open_comport(&fd_corr_data_read, config.ser_device_corr_data_read, O_RDWR, 115200));
}
erret(open_comport(&fd, config.ser_device, O_RDWR, 57600));
if(0){
struct termios termios_p;
if(tcgetattr(fd, &termios_p)==0) {
termios_p.c_cc[VTIME] = 1; // 2*0.1s = 0.2s
termios_p.c_cc[VMIN] = 0;
tcsetattr(fd, TCSADRAIN, &termios_p);
} else fprintf(stderr, "failed to set read timeout\n");
}
//create thread for reading and writing of the correction data:
ifret(pthread_getschedparam(pthread_self(), &policy, ¶m), AR_ERR_SYS_RESOURCES);
param.sched_priority = config.priority_dgps_corr_data;
pthread_attr_init( &attr );
ifret(pthread_attr_setschedparam(&attr, ¶m), AR_ERR_SYS_RESOURCES);
ifret(pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED), AR_ERR_SYS_RESOURCES);
ifret(pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED), AR_ERR_SYS_RESOURCES);
if(config.corr_mode == OEM4_RTCA_MODE) {
ifret(write(fd, OEM4_INIT_ROVER_RTCA, strlen(OEM4_INIT_ROVER_RTCA))==-1, AR_ERR_IO_ERROR);
ifret(pthread_create(&pthreads[0], &attr, &corr_data_thread_func, NULL), AR_ERR_SYS_RESOURCES);
} else if(config.corr_mode == OEM4_RTCM_MODE) {
ifret(write(fd, OEM4_INIT_ROVER_RTCM, strlen(OEM4_INIT_ROVER_RTCM))==-1, AR_ERR_IO_ERROR);
ifret(pthread_create(&pthreads[0], &attr, &corr_data_thread_func, NULL), AR_ERR_SYS_RESOURCES);
} else
{
ifret(write(fd, OEM4_INIT_ROVER_COM1, strlen(OEM4_INIT_ROVER_COM1))==-1, AR_ERR_IO_ERROR);
printf("running without differential data\n");
}
ifret(pthread_getschedparam(pthread_self(), &policy, ¶m), AR_ERR_SYS_RESOURCES);
param.sched_priority = config.priority;
ifret(pthread_setschedparam(pthread_self(), policy, ¶m), AR_ERR_SYS_RESOURCES);
while(1) {
errp(read_data());
}
return 0;
}
// This callback is called for initialising the Simulink model by the RTW code
// It should contain all definitions of callbacks and configurations
// Its arguments argc and argv are passed on by the calling RTW main function
// The argc argument contains the number of strings contained in the argv pointer array
// The argv argument contains an array of pointers to strings
// The pointer argv[0] contains the program's name
// The pointers argv[1...] contain the arguments with which the process was started
int AR_MatlabRTW_Init_CB(int argc, const char *argv[])
{
// Define call back functions for the main loop
// The functions on the right are the generated by RTW
// The handles on the left are used by the model's loop
// See ar_matlab_rtw_int.c for more info
ar_model_initialize_cb = airdata_sfl_initialize;
ar_model_step_cb = (void (*)(int_T))airdata_sfl_step;
ar_model_terminate_cb = airdata_sfl_terminate;
// Define call back functions for updating of input/output signals
// The function on the right will be defined in this module
// The handles on the left are used by the model's loop
// See ar_matlab_rtw_int.c for more info
AR_UpdateInputs_CB = Airdata_SFL_UpdateInputs;
AR_UpdateOutputs_CB = Airdata_SFL_UpdateOutputs;
// Define time constants
// The makros on the right are defined in the process' header file
// The variables on the left are used by the model's loop
// See ar_matlab_rtw_int.c for more info
AR_CtrlPeriod = AR_AIRDATA_SFL_PERIOD;
AR_MaxCommDelay = AR_AIRDATA_SFL_MAX_COMM_DELAY;
// Check for number of arguments to the function:
// argv[0] should be program name
// argv[1] should be instance number
// There should be no more arguments...
if(argc != 2)
{
// perr sends a message to the system logger and outputs an error to the screen
perr(AR_ERR_BAD_INIT_PARAM);
printf("Useage: %s INSTANCE\n", argv[0]);
return(AR_ERR_BAD_INIT_PARAM);
}
// init_config checks, that the config file is readable
// and sets the name of the process to filename_instance
// and sets the instance number of the process
// It accepts a pointer to the filename char, an instance integer and a pointer to the base configuration
erret(init_config((char*)"ar_airdata_sfl", atoi(argv[1]), &cfg.base));
// Read the process' priority into the base configuration structure
// read_config_int accepts:
// - the section name [some_process_INSTANCE]
// - the tag name SOME_TAG_NAME = 12345
// - a pointer to the variable the data will be stored in
erret(read_config_int( cfg.base.name, "PRIORITY_DRIVER", (int*)&cfg.base.priority ));
// Read the RTW priorities and ports into the appropriate global variables
// The external mode port port must be unique within the system
// See ar_matlab_rtw for more info
erret(read_config_int( cfg.base.name, "PRIORITY_RTW_CALC", &AR_CalcThreadPriority ));
erret(read_config_int( cfg.base.name, "PRIORITY_RTW_COMM", &AR_CommThreadPriority ));
erret(read_config_int( cfg.base.name, "EXT_MODE_PORT", &AR_ExtModePort ));
// Read the device name of the airdata connection
// read_config_string accepts the maximum length of the destination variable as an additional argument
erret(read_config_string( cfg.base.name, "DEVICE", cfg.ser_device, sizeof(cfg.ser_device) ));
// Read the shared memory slots into the configuration structure
// read_config_int_tab also accepts the section name for a table lookup.
// The string value found for the actual tag will then be replaced with its value from the table section
erret(read_config_int_tab( cfg.base.name, "SHM_INDEX", "SHM_OUT_AIRDATA", &cfg.shm_out_airdata ));
erret(read_config_int_tab( cfg.base.name, "SHM_INDEX", "SHM_OUT_TAS_ALPHA_BETA", &cfg.shm_out_tas_alpha_beta ));
erret(read_config_int_tab(cfg.base.name, "SHM_INDEX", "SHM_IN_CMD_NAV_FLAGS",&cfg.shm_in_cmd_nav_flags));
// Initialize the shared memory module
// SHM_Init initializes the internal slot table to null pointers
erret(SHM_Init());
// Initialize the modules shared memory slots
// SHM_InitSlot initializes a shared memory slot and allocates the pointer in the slots table
// Inputs: Slot number, data size
erret(SHM_InitSlot(cfg.shm_out_airdata, sizeof(airdata_t)));
erret(SHM_InitSlot(cfg.shm_out_tas_alpha_beta, sizeof(v3_t)));
erret(SHM_InitSlot(cfg.shm_in_cmd_nav_flags, sizeof(ar_cmd_nav_flags_t)));
// The private slots are used for inter-thread communication in this single process
erret(SHM_Priv_Init());
erret(SHM_Priv_InitSlot(AR_SHM_PRIV_SLOT_AIRDATA_SFL, sizeof(airdata_sfl_data_raw_t)));
// This section starts the driver thread to be independent of the RTW thread
{
static pthread_attr_t attr; // All attributes of a thread. Including scheduling policy and parameters
static pthread_t driver_thread; // ID of the driver thread
int policy; // Scheduling policy identifier (when to use how much resources for which thread)
struct sched_param param; // Scheduling parameters of a thread (to feed policy)
// Read current thread's scheduling parameters as a starter config and set custom priority of the new thread
ifret(pthread_getschedparam(pthread_self(), &policy, ¶m), AR_ERR_SYS_RESOURCES);
param.sched_priority = cfg.base.priority;
// Initialize an attributes structure and order the custom scheduling
pthread_attr_init( &attr );
ifret( pthread_attr_setschedparam( &attr, ¶m) ,AR_ERR_SYS_RESOURCES); // Set scheduling parameters
ifret( pthread_attr_setinheritsched( &attr, PTHREAD_EXPLICIT_SCHED) ,AR_ERR_SYS_RESOURCES); // Force custom scheduling
// As we will never want to wait for the new thread to finish, we start it in a detached state
ifret( pthread_attr_setdetachstate( &attr, PTHREAD_CREATE_DETACHED) ,AR_ERR_SYS_RESOURCES);
// Start the driver thread:
// Receive driver_thread as the new thread's ID
// Use attr as thread attributes
// Use start_driver as the starting function for the thread
// Use null arguments, as the start_driver function does not require any
ifret( pthread_create(&driver_thread, &attr, &start_driver, NULL) ,AR_ERR_SYS_RESOURCES);
}
return 0;
}
int main(int argc, char* argv[])
{
int policy;
struct sched_param param;
char buffer[1024];
dprintf("init\n");
if(argc != 2)
{
perr(AR_ERR_BAD_INIT_PARAM);
printf("Useage: %s INSTANCE\n", AR_MODULE_NAME);
return(AR_ERR_BAD_INIT_PARAM);
}
erret(init_config(AR_MODULE_NAME, atoi(argv[1]), &config.base));
erret(read_config_int(config.base.name, "PRIORITY", &config.priority));
erret(read_config_string(config.base.name, "DEVICE", config.ser_device, sizeof(config.ser_device)));
memset(buffer, 0, sizeof(buffer));
erret(read_config_string(config.base.name, "CORRECTION_MODE", buffer, sizeof(buffer)));
if(strnicmp(buffer, "RTCA", strlen("RTCA")) == 0)
config.corr_mode = OEMX_RTCA_MODE;
else if(strnicmp(buffer, "RTCM", strlen("RTCM")) == 0)
config.corr_mode = OEMX_RTCM_MODE;
else {
perrs(AR_ERR_BAD_INIT_PARAM, "Correction data mode not specified (RTCA | RTCM).");
return(AR_ERR_BAD_INIT_PARAM);
}
erret(open_comport(&fd, config.ser_device, O_RDWR, 115200));
memset(buffer, 0, sizeof(buffer));
erret(read_config_string(config.base.name, "GPS_MODE", buffer, sizeof(buffer)));
if(strnicmp(buffer, "OEM4", strlen("OEM4")) == 0)
config.gps_mode = OEM4_GPS;
else if(strnicmp(buffer, "OEM5", strlen("OEM5")) == 0)
config.gps_mode = OEM4_GPS;
else {
perrs(AR_ERR_BAD_INIT_PARAM, "GPS_MODE must be OEM4 or OEM5.");
return(AR_ERR_BAD_INIT_PARAM);
}
//ifret(write(fd, OEM4_INIT_ROVER_COM1, strlen(OEM4_INIT_ROVER_COM1))==-1, AR_ERR_IO_ERROR);
/*
if(config.gps_mode == OEM4_GPS && config.corr_mode == OEMX_RTCA_MODE) {
ifret(write(fd, OEM4_INIT_RTCA, strlen(OEM4_INIT_RTCA))==-1, AR_ERR_IO_ERROR);
printf("INIT RTCA\n");
} else if(config.gps_mode == OEM4_GPS && config.corr_mode == OEMX_RTCM_MODE) {
struct termios termios_p;
int rlen=0;
if(tcgetattr(fd, &termios_p)==0) {
termios_p.c_cc[VTIME] = 10; // 2*0.1s = 0.2s
termios_p.c_cc[VMIN] = 0;
tcsetattr(fd, TCSANOW, &termios_p);
}
sprintf(buffer, "unlogall com2\n\r");
sleep(1);
write(fd,buffer, strlen(buffer));
rlen=read(fd,buffer,sizeof(buffer)-1);
buffer[rlen]=0;
printf("%s\n---\n",buffer);
ifret(write(fd, OEM4_INIT_RTCM, strlen(OEM4_INIT_RTCM))==-1, AR_ERR_IO_ERROR);
sleep(1);
write(fd,buffer, strlen(buffer));
rlen=read(fd,buffer,sizeof(buffer)-1);
buffer[rlen]=0;
printf("%s\n---\n",buffer);
printf("INIT RTCM\n");
} else {
perrs(AR_ERR_BAD_INIT_PARAM, "can not initialized mode for corr. data\n");
return(AR_ERR_BAD_INIT_PARAM);
}
*/
config.queue_corr_data_1 = 0;
config.queue_corr_data_2 = 0;
config.queue_corr_data_3 = 0;
config.queue_corr_data_4 = 0;
config.queue_corr_data_5 = 0;
config.queue_corr_data_6 = 0;
erret(read_config_int_tab(config.base.name, "QUEUE_INDEX", "QUEUE_OUT_GPS_CORRCTION_1", &config.queue_corr_data_1));
erret(Q_Init());
erret(Q_InitSlot(config.queue_corr_data_1, sizeof(gps_corr_t), MAX_GPS_CORRECTION_Q_ITEMS_NUMBER, 0));
if(read_config_int_tab(config.base.name, "QUEUE_INDEX", "QUEUE_OUT_GPS_CORRCTION_2", &config.queue_corr_data_2)==0) {
erret(Q_InitSlot(config.queue_corr_data_2, sizeof(gps_corr_t), MAX_GPS_CORRECTION_Q_ITEMS_NUMBER, 0));
}
if(read_config_int_tab(config.base.name, "QUEUE_INDEX", "QUEUE_OUT_GPS_CORRCTION_3", &config.queue_corr_data_3)==0) {
erret(Q_InitSlot(config.queue_corr_data_3, sizeof(gps_corr_t), MAX_GPS_CORRECTION_Q_ITEMS_NUMBER, 0));
}
if(read_config_int_tab(config.base.name, "QUEUE_INDEX", "QUEUE_OUT_GPS_CORRCTION_4", &config.queue_corr_data_4)==0) {
erret(Q_InitSlot(config.queue_corr_data_4, sizeof(gps_corr_t), MAX_GPS_CORRECTION_Q_ITEMS_NUMBER, 0));
}
if(read_config_int_tab(config.base.name, "QUEUE_INDEX", "QUEUE_OUT_GPS_CORRCTION_5", &config.queue_corr_data_5)==0) {
erret(Q_InitSlot(config.queue_corr_data_5, sizeof(gps_corr_t), MAX_GPS_CORRECTION_Q_ITEMS_NUMBER, 0));
}
if(read_config_int_tab(config.base.name, "QUEUE_INDEX", "QUEUE_OUT_GPS_CORRCTION_6", &config.queue_corr_data_6)==0) {
erret(Q_InitSlot(config.queue_corr_data_6, sizeof(gps_corr_t), MAX_GPS_CORRECTION_Q_ITEMS_NUMBER, 0));
}
//both queues are initialized in non-blocking mode; if one of communicators is blocked,
//the second one continues to transmit the data
ifret(pthread_getschedparam(pthread_self(), &policy, ¶m), AR_ERR_SYS_RESOURCES);
param.sched_priority = config.priority;
ifret(pthread_setschedparam(pthread_self(), policy, ¶m), AR_ERR_SYS_RESOURCES);
read_corr_data_loop();
return(0);
}
// Alsa can convert about any format/channels/rate to any other rate
// However, since we added some code in the daemon to convert, why not do it ourselves!!!
// Moreover some player like aplay won't play a wav file if the device that do not natively support the requested format
// If you want alsa to do the conversion, just remove the value you want to see converted
static int a2dp_constraint(snd_pcm_a2dp_t * a2dp)
{
snd_pcm_ioplug_t *io = &a2dp->io;
#define ARRAY_SIZE(x) (sizeof(x)/sizeof((x)[0]))
snd_pcm_access_t access_list[] = { SND_PCM_ACCESS_RW_INTERLEAVED };
unsigned int formats[] = { SND_PCM_FORMAT_U8, SND_PCM_FORMAT_S8, SND_PCM_FORMAT_S16_LE };
unsigned int channels[] = { 1, 2 };
unsigned int rates[] = { 8000, 11025, 22050, 32000, 44100, 48000 };
int formats_nb = ARRAY_SIZE(formats);
int channels_nb = ARRAY_SIZE(channels);
int rates_nb = ARRAY_SIZE(rates);
int rate_daemon = 0;
int rate_prefered = 0;
char srcfilename[512];
int err;
get_config_filename(srcfilename, sizeof(srcfilename));
// Default is same as the daemon
rate_daemon = read_config_int(srcfilename, "a2dpd", "rate", A2DPD_FRAME_RATE);
// If a value is specified, use it
rate_prefered = read_config_int(srcfilename, "a2dpd", "plugin-rate", rate_daemon);
// If this value is not 0, alsa will convert to plugin-rate
if(rate_prefered != 0) {
// use defaults settings the rate specified + 16 bits stereo
rates[0] = rate_prefered;
rates_nb = 1;
formats[0] = SND_PCM_FORMAT_S16_LE;
formats_nb = 1;
channels[0] = 2;
channels_nb = 1;
} else {
// If this value is 0, the daemon will do most conversions
}
syslog(LOG_INFO, "[build %s %s] a2dp %p", __DATE__, __TIME__, a2dp);
err = snd_pcm_ioplug_set_param_list(io, SND_PCM_IOPLUG_HW_ACCESS, ARRAY_SIZE(access_list), access_list);
if (err < 0)
return err;
err = snd_pcm_ioplug_set_param_list(io, SND_PCM_IOPLUG_HW_FORMAT, formats_nb, formats);
if (err < 0)
return err;
err = snd_pcm_ioplug_set_param_list(io, SND_PCM_IOPLUG_HW_CHANNELS, channels_nb, channels);
if (err < 0)
return err;
err = snd_pcm_ioplug_set_param_list(io, SND_PCM_IOPLUG_HW_RATE, rates_nb, rates);
if (err < 0)
return err;
err = snd_pcm_ioplug_set_param_minmax(io, SND_PCM_IOPLUG_HW_PERIOD_BYTES, 8192, 8192);
if (err < 0)
return err;
err = snd_pcm_ioplug_set_param_minmax(io, SND_PCM_IOPLUG_HW_PERIODS, 2, 2);
if (err < 0)
return err;
return 0;
}
