static void ParseOptions(Datum options, Reader **rd, Writer **wt, time_t tm) { List *defs; List *rest_defs = NIL; ListCell *cell; DefElem *opt; char *keyword; char *value; char *type = NULL; char *writer = NULL; bool multi_process = false; Assert(*rd == NULL); Assert(*wt == NULL); /* parse for each option */ defs = untransformRelOptions(options); foreach (cell, defs) { opt = lfirst(cell); if (opt->arg == NULL) ereport(ERROR, (errcode(ERRCODE_INVALID_PARAMETER_VALUE), errmsg("option \"%s\" has no value", opt->defname))); keyword = opt->defname; value = strVal(opt->arg); if (CompareKeyword(keyword, "TYPE")) { ASSERT_ONCE(type == NULL); type = value; } else if (CompareKeyword(keyword, "WRITER") || CompareKeyword(keyword, "LOADER")) { ASSERT_ONCE(writer == NULL); writer = value; } else if (CompareKeyword(keyword, "MULTI_PROCESS")) { multi_process = ParseBoolean(value); } else { rest_defs = lappend(rest_defs, opt); continue; } }
void acc_mag_cal_init(void) { ASSERT_ONCE(); /* load calibration: */ opcd_param_t params[] = { /* acc bias: */ {"acc_bias_x", &acc_bias[0]}, {"acc_bias_y", &acc_bias[1]}, {"acc_bias_z", &acc_bias[2]}, /* acc scale: */ {"acc_scale_x", &acc_scale[0]}, {"acc_scale_y", &acc_scale[1]}, {"acc_scale_z", &acc_scale[2]}, /* mag bias: */ {"mag_bias_x", &mag_bias[0]}, {"mag_bias_y", &mag_bias[1]}, {"mag_bias_z", &mag_bias[2]}, /* mag scale: */ {"mag_scale_x", &mag_scale[0]}, {"mag_scale_y", &mag_scale[1]}, {"mag_scale_z", &mag_scale[2]}, OPCD_PARAMS_END }; opcd_params_apply("cal.", params); }
void att_ctrl_init(void) { ASSERT_ONCE(); /* load parameters: */ opcd_param_t params[] = { {"p", &angle_p.value}, {"i", &angle_i.value}, {"i_max", &angle_i_max.value}, {"d", &angle_d.value}, {"filt_fg", &filt_fg}, {"pitch_bias", &biases[0]}, {"roll_bias", &biases[1]}, OPCD_PARAMS_END }; opcd_params_apply("controllers.attitude.", params); /* initialize filter: */ filter1_lp_init(&filter, tsfloat_get(&filt_fg), 0.0033333, 2); /* initialize controllers: */ FOR_EACH(i, controllers) { pid_init(&controllers[i], &angle_p, &angle_i, &angle_d, &angle_i_max); }
PERIODIC_THREAD_END void mon_init(void) { ASSERT_ONCE(); /* open monitoring socket: */ mon_socket = scl_get_socket("ap_mon"); ASSERT_NOT_NULL(mon_socket); int64_t hwm = 1; zmq_setsockopt(mon_socket, ZMQ_SNDHWM, &hwm, sizeof(hwm)); /* create monitoring connection: */ const struct timespec period = {0, 20 * NSEC_PER_MSEC}; pthread_mutexattr_init(&mutexattr); pthread_mutexattr_setprotocol(&mutexattr, PTHREAD_PRIO_INHERIT); pthread_mutex_init(&mutex, &mutexattr); /* init msgpack buffer: */ ASSERT_NULL(msgpack_buf); msgpack_buf = msgpack_sbuffer_new(); ASSERT_NOT_NULL(msgpack_buf); ASSERT_NULL(pk); pk = msgpack_packer_new(msgpack_buf, msgpack_sbuffer_write); periodic_thread_start(&emitter_thread, mon_emitter, "mon_thread", THREAD_PRIORITY, period, NULL); }
void pos_init(void) { ASSERT_ONCE(); /* read configuration and initialize scl gates: */ opcd_param_t params[] = { {"process_noise", &process_noise}, {"ultra_noise", &ultra_noise}, {"baro_noise", &baro_noise}, {"gps_noise", &gps_noise}, {"use_gps_speed", &use_gps_speed}, OPCD_PARAMS_END }; opcd_params_apply("kalman_pos.", params); LOG(LL_DEBUG, "process noise: %f, ultra noise: %f, baro noise: %f, gps noise: %f", tsfloat_get(&process_noise), tsfloat_get(&ultra_noise), tsfloat_get(&baro_noise), tsfloat_get(&gps_noise)); /* set-up kalman filters: */ kalman_init(&e_kalman, &process_noise, &gps_noise, 0, 0, tsint_get(&use_gps_speed)); kalman_init(&n_kalman, &process_noise, &gps_noise, 0, 0, tsint_get(&use_gps_speed)); kalman_init(&baro_u_kalman, &process_noise, &baro_noise, 0, 0, false); kalman_init(&ultra_u_kalman, &process_noise, &ultra_noise, 0, 0, false); }
static void main_realtime_init(void) { ASSERT_ONCE(); LOG(LL_INFO, "setting up real-time scheduling"); sp.sched_priority = sched_get_priority_max(SCHED_FIFO); sched_setscheduler(getpid(), SCHED_FIFO, &sp); if (nice(-20) == -1) { LOG(LL_ERROR, "could not renice process"); die(); } thread->sched_param.sched_priority = 97; pthread_setschedparam(pthread_self(), SCHED_FIFO, &thread->sched_param); /*if (mlockall(MCL_CURRENT | MCL_FUTURE) != 0) { LOG(LL_ERROR, "mlockall() failed"); die(); }*/ thread->name = "main_loop"; thread->running = 1; thread->periodic_data.period.tv_sec = 0; thread->periodic_data.period.tv_nsec = NSEC_PER_SEC * REALTIME_PERIOD; }
void motors_state_init(void) { ASSERT_ONCE(); ASSERT_NULL(spinning_socket); spinning_socket = scl_get_socket("motors_spinning"); scl_send_static(spinning_socket, "false", 5); ASSERT_NOT_NULL(spinning_socket); etimer_init(&timer, 1.5); }
static bool BufferedWriterParam(BufferedWriter *self, const char *keyword, char *value) { if (CompareKeyword(keyword, "TABLE") || CompareKeyword(keyword, "OUTPUT")) { ASSERT_ONCE(self->base.output == NULL); self->base.relid = RangeVarGetRelid(makeRangeVarFromNameList( stringToQualifiedNameList(value)), NoLock, false); self->base.output = get_relation_name(self->base.relid); } else if (CompareKeyword(keyword, "DUPLICATE_BADFILE")) { ASSERT_ONCE(self->base.dup_badfile == NULL); self->base.dup_badfile = pstrdup(value); } else if (CompareKeyword(keyword, "DUPLICATE_ERRORS")) { ASSERT_ONCE(self->base.max_dup_errors < -1); self->base.max_dup_errors = ParseInt64(value, -1); if (self->base.max_dup_errors == -1) self->base.max_dup_errors = INT64_MAX; } else if (CompareKeyword(keyword, "ON_DUPLICATE_KEEP")) { const ON_DUPLICATE values[] = { ON_DUPLICATE_KEEP_NEW, ON_DUPLICATE_KEEP_OLD }; self->base.on_duplicate = values[choice(keyword, value, ON_DUPLICATE_NAMES, lengthof(values))]; } else if (CompareKeyword(keyword, "TRUNCATE")) { self->base.truncate = ParseBoolean(value); } else return false; /* unknown parameter */ return true; }
/** * @brief Parse a line in control file. */ bool ReaderParam(Reader *rd, const char *keyword, char *target) { /* * result */ if (CompareKeyword(keyword, "INFILE") || CompareKeyword(keyword, "INPUT")) { ASSERT_ONCE(rd->infile == NULL); rd->infile = pstrdup(target); } else if (CompareKeyword(keyword, "LOGFILE")) { ASSERT_ONCE(rd->logfile == NULL); rd->logfile = pstrdup(target); } else if (CompareKeyword(keyword, "PARSE_BADFILE")) { ASSERT_ONCE(rd->parse_badfile == NULL); rd->parse_badfile = pstrdup(target); } else if (CompareKeyword(keyword, "PARSE_ERRORS") || CompareKeyword(keyword, "MAX_ERR_CNT")) { ASSERT_ONCE(rd->max_parse_errors < -1); rd->max_parse_errors = ParseInt64(target, -1); if (rd->max_parse_errors == -1) rd->max_parse_errors = INT64_MAX; } else if (CompareKeyword(keyword, "LOAD") || CompareKeyword(keyword, "LIMIT")) { ASSERT_ONCE(rd->limit == INT64_MAX); rd->limit = ParseInt64(target, 0); } else if (CompareKeyword(keyword, "CHECK_CONSTRAINTS")) { rd->checker.check_constraints = ParseBoolean(target); } else if (CompareKeyword(keyword, "ENCODING")) { ASSERT_ONCE(rd->checker.encoding < 0); rd->checker.encoding = pg_valid_client_encoding(target); if (rd->checker.encoding < 0) ereport(ERROR, (errcode(ERRCODE_INVALID_PARAMETER_VALUE), errmsg("invalid encoding for parameter \"ENCODING\": \"%s\"", target))); } else if (rd->parser == NULL || !ParserParam(rd->parser, keyword, target)) return false; return true; }
void flight_state_init(size_t window, size_t hysteresis, float treshold) { ASSERT_ONCE(); hyst = hysteresis; tresh = treshold; wnd = window; dim = 3; vars = malloc(dim * sizeof(sliding_var_t)); ASSERT_NOT_NULL(vars); FOR_N(i, dim) sliding_var_init(&vars[i], window, 0.0f); }
void cmc_init(void) { ASSERT_ONCE(); opcd_param_t params[] = { {"scale_x", &scale[0]}, {"scale_y", &scale[1]}, {"scale_z", &scale[2]}, {"bias", &bias}, OPCD_PARAMS_END }; opcd_params_apply("cmc.", params); }
SIMPLE_THREAD_END int cmd_init(void) { ASSERT_ONCE(); cmd_socket = scl_get_socket("ap_ctrl"); if (cmd_socket == NULL) { return -1; } simple_thread_start(&thread, thread_func, THREAD_NAME, THREAD_PRIORITY, NULL); return 0; }
static bool BinaryParserParam(BinaryParser *self, const char *keyword, char *value) { if (CompareKeyword(keyword, "COL")) { BinaryParam(&self->fields, &self->nfield, value, self->preserve_blanks, false); if (self->fields[self->nfield - 1].character) self->fields[self->nfield - 1].str = palloc(self->fields[self->nfield - 1].len * MAX_CONVERSION_GROWTH + 1); } else if (CompareKeyword(keyword, "PRESERVE_BLANKS")) { self->preserve_blanks = ParseBoolean(value); } else if (CompareKeyword(keyword, "STRIDE")) { ASSERT_ONCE(self->rec_len == 0); self->rec_len = ParseInt32(value, 1); } else if (CompareKeyword(keyword, "SKIP") || CompareKeyword(keyword, "OFFSET")) { ASSERT_ONCE(self->offset < 0); self->offset = ParseInt64(value, 0); } else if (CompareKeyword(keyword, "FILTER")) { ASSERT_ONCE(!self->filter.funcstr); self->filter.funcstr = pstrdup(value); } else return false; /* unknown parameter */ return true; }
void u_speed_init(void) { ASSERT_ONCE(); opcd_param_t params[] = { {"p", &speed_p}, {"i", &speed_i}, {"imax", &speed_imax}, OPCD_PARAMS_END }; opcd_params_apply("controllers.u_speed.", params); pid_init(&ctrl, &speed_p, &speed_i, NULL, &speed_imax); }
SIMPLE_THREAD_END int scl_gps_init(void) { ASSERT_ONCE(); THROW_BEGIN(); scl_socket = scl_get_socket("gps"); THROW_IF(scl_socket == NULL, -ENODEV); pthread_mutexattr_init(&mutexattr); pthread_mutexattr_setprotocol(&mutexattr, PTHREAD_PRIO_INHERIT); pthread_mutex_init(&mutex, &mutexattr); interval_init(&interval); simple_thread_start(&thread, thread_func, "gps_reader", THREAD_PRIORITY, NULL); THROW_END(); }
int generic_platform_init(platform_t *plat) { ASSERT_ONCE(); THROW_BEGIN(); LOG(LL_INFO, "initializing power reader"); THROW_ON_ERR(scl_power_init()); plat->read_power = scl_power_read; LOG(LL_INFO, "initializing remote control reader"); THROW_ON_ERR(scl_rc_init()); plat->read_rc = scl_rc_read; LOG(LL_INFO, "initializing GPS reader"); THROW_ON_ERR(scl_gps_init()); plat->read_gps = scl_gps_read; THROW_END(); }
void u_ctrl_init(const float dt) { ASSERT_ONCE(); opcd_param_t params[] = { {"p", &pos_p}, {"d", &pos_d}, {"i", &pos_i}, {"imax", &pos_imax}, {"lpfg", &lpfg}, OPCD_PARAMS_END }; opcd_params_apply("controllers.u_pos.", params); pid_init(&ctrl, &pos_p, &pos_i, &pos_d, &pos_imax); filter1_lp_init(&filter, tsfloat_get(&lpfg), dt, 1); }
int channels_init(void) { ASSERT_ONCE(); opcd_param_t params[] = { {"pitch.index", &pitch_index}, {"pitch.max", &pitch_max}, {"pitch.min", &pitch_min}, {"roll.index", &roll_index}, {"roll.max", &roll_max}, {"roll.min", &roll_min}, {"yaw.index", &yaw_index}, {"yaw.max", &yaw_max}, {"yaw.min", &yaw_min}, {"gas.index", &gas_index}, {"gas.max", &gas_max}, {"gas.min", &gas_min}, {"two_state.index", &two_state_index}, {"two_state.max", &two_state_max}, {"two_state.min", &two_state_min}, {"three_state.index", &three_state_index}, {"three_state.max", &three_state_max}, {"three_state.min", &three_state_min}, OPCD_PARAMS_END }; opcd_params_apply(".", params); /* check if channels are configured: */ if (tsint_get(&pitch_index) == -1) { LOG(LL_INFO, "channels not configured; use pp_rc_cal and start the service again"); return -1; } linfunc_init_points(&pitch_linfunc, tsfloat_get(&pitch_min), -1.0f, tsfloat_get(&pitch_max), 1.0f); linfunc_init_points(&roll_linfunc, tsfloat_get(&roll_min), -1.0f, tsfloat_get(&roll_max), 1.0f); linfunc_init_points(&yaw_linfunc, tsfloat_get(&yaw_min), -1.0f, tsfloat_get(&yaw_max), 1.0f); linfunc_init_points(&gas_linfunc, tsfloat_get(&gas_min), -1.0f, tsfloat_get(&gas_max), 1.0f); linfunc_init_points(&two_state_linfunc, tsfloat_get(&two_state_min), 0.0f, tsfloat_get(&two_state_max), 1.0f); linfunc_init_points(&three_state_linfunc, tsfloat_get(&three_state_min), 0.0f, tsfloat_get(&three_state_max), 1.0f); return 0; }
void flight_logic_init(void) { ASSERT_ONCE(); sticks_init(); switch (flight_mode) { case MODE_MANUAL: man_logic_init(); break; case MODE_SAFE_AUTO: auto_logic_init(); break; case MODE_FULL_AUTO: auto_logic_init(); break; } }
void blackbox_init(void) { ASSERT_ONCE(); ASSERT_NULL(blackbox_socket); /* get scl socket */ blackbox_socket = scl_get_socket("blackbox"); ASSERT_NOT_NULL(blackbox_socket); /* send blackbox header: */ ASSERT_NULL(msgpack_buf); msgpack_buf = msgpack_sbuffer_new(); ASSERT_NOT_NULL(msgpack_buf); ASSERT_NULL(pk); pk = msgpack_packer_new(msgpack_buf, msgpack_sbuffer_write); ASSERT_NOT_NULL(pk); msgpack_pack_array(pk, BLACKBOX_ITEMS); FOR_EACH(i, blackbox_spec) { size_t len = strlen(blackbox_spec[i]); msgpack_pack_raw(pk, len); msgpack_pack_raw_body(pk, blackbox_spec[i], len); }
void yaw_ctrl_init(void) { ASSERT_ONCE(); opcd_param_t params[] = { {"speed_min", &speed_min}, {"speed_std", &speed_std}, {"speed_max", &speed_max}, {"speed_slope", &speed_slope}, {"speed_p", &speed_p.value}, {"speed_i", &speed_i.value}, {"speed_imax", &speed_imax.value}, {"manual", &manual}, OPCD_PARAMS_END }; opcd_params_apply("controllers.yaw.", params); tsfloat_init(&pos, 0.0f); tsfloat_init(&speed, tsfloat_get(&speed_std)); pid_init(&controller, &speed_p, &speed_i, NULL, &speed_imax); }
void ne_speed_ctrl_init(float dt) { ASSERT_ONCE(); /* load parameters: */ opcd_param_t params[] = { {"p", &speed_p.value}, {"i", &speed_i.value}, {"i_max", &speed_i_max.value}, {"d", &speed_d.value}, {"lpfg", &lpfg}, OPCD_PARAMS_END }; opcd_params_apply("controllers.ne_speed.", params); filter1_lp_init(&filter, tsfloat_get(&lpfg), dt, 2); /* initialize controllers: */ FOR_EACH(i, controllers) { pid_init(&controllers[i], &speed_p, &speed_i, &speed_d, &speed_i_max); }
void att_ctrl_init(void) { ASSERT_ONCE(); /* load parameters: */ opcd_param_t params[] = { {"p", &angle_p.value}, {"i", &angle_i.value}, {"i_max", &angle_i_max.value}, {"d", &angle_d.value}, {"pitch_bias", &biases[0]}, {"roll_bias", &biases[1]}, {"angles_max", &angles_max}, OPCD_PARAMS_END }; opcd_params_apply("controllers.attitude.", params); /* initialize controllers: */ FOR_EACH(i, controllers) pid_init(&controllers[i], &angle_p, &angle_i, &angle_d, &angle_i_max); }