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);
}
int fc_init(void)
{
if (!is_initialized)
{
is_initialized = 1;
char *serial_port;
opcd_param_t params[] =
{
{"serial_port", &serial_port},
{"setting", &setting},
OPCD_PARAMS_END
};
opcd_params_apply("actuators.mk_fc.", params);
int ret;
/* perform initialization once here: */
if ((ret = serial_open(&port, serial_port, 57600, ICRNL, ICANON, 0)) != 0)
{
return ret;
}
else
{
const struct timespec dreq_period = {0, DREQ_THREAD_TIMEOUT_MS * NSEC_PER_MSEC};
frame_t frame;
char data = 1;
LOG(LL_INFO, "setting up mikrokopter interface");
build_frame(frame, OUT_NC_REDIRECT_UART, (const plchar_t *)&data, 0);
serial_write_line(&port, frame);
rpm = malloc(sizeof(float) * platform_motors());
for (int i = 0; i < platform_motors(); i++)
{
rpm[i] = 0.0f;
}
periodic_thread_start(&dreq_thread, dreq_thread_func, DREQ_THREAD_NAME,
DREQ_THREAD_PRIORITY, dreq_period, NULL);
simple_thread_start(&sread_thread, sread_thread_func, SREAD_THREAD_NAME,
SREAD_THREAD_PRIORITY, NULL);
LOG(LL_INFO, "mikrokopter interface up and running");
}
}
else
{
LOG(LL_DEBUG, "mikrokopter interface already running");
}
return 0;
}
