// this is supported from 0.31.04 forward DFhackCExport int SDL_NumJoysticks(void) { if(errorstate) return -1; if(!inited) { SHM_Init(); return -2; } SHM_Act(); return -3; }
static void Test(void) { uint8_t line; SHM_Init(); sharpmem_buffer[0] = 0b11000111; /* bit 0: black, bit 1: white */ SHM_ClearDisplay(); for(line=0;line<SHARPMEM_LCDHEIGHT;line++) { SHM_Line(line, 0xF0); } SHM_ClearDisplay(); SHM_Refresh(); }
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; }