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