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 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);
}
SIMPLE_THREAD_END
int scl_elevmap_init(void)
{
THROW_BEGIN();
scl_socket = scl_get_socket("elev");
THROW_IF(scl_socket == NULL, -ENODEV);
tsfloat_init(&elevation, 0.0f);
simple_thread_start(&thread, thread_func, "elevmap_reader", THREAD_PRIORITY, NULL);
THROW_END();
}
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;
}
SIMPLE_THREAD_END
int scl_power_init(void)
{
THROW_BEGIN();
scl_socket = scl_get_socket("power");
THROW_IF(scl_socket == NULL, -ENODEV);
pthread_mutexattr_init(&mutexattr);
pthread_mutexattr_setprotocol(&mutexattr, PTHREAD_PRIO_INHERIT);
pthread_mutex_init(&mutex, &mutexattr);
simple_thread_start(&thread, thread_func, "power_reader", THREAD_PRIORITY, NULL);
THROW_END();
}
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();
}
SIMPLE_THREAD_END
int scl_mag_decl_init(void)
{
decl_socket = scl_get_socket("decl");
if (decl_socket == NULL)
{
return -1;
}
pthread_mutexattr_init(&mutexattr);
pthread_mutexattr_setprotocol(&mutexattr, PTHREAD_PRIO_INHERIT);
pthread_mutex_init(&mutex, &mutexattr);
simple_thread_start(&thread, thread_func, "geomag", THREAD_PRIORITY, NULL);
return 0;
}
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);
}
int _main(void)
{
THROW_BEGIN()
/* init scl and get sockets:: */
THROW_ON_ERR(scl_init("arduino"));
void *rc_socket = scl_get_socket("rc_raw");
THROW_IF(rc_socket == NULL, -ENODEV);
void *power_socket = scl_get_socket("power");
THROW_IF(power_socket == NULL, -ENODEV);
/* allocate msgpack buffers: */
msgpack_sbuffer *msgpack_buf = msgpack_sbuffer_new();
THROW_IF(msgpack_buf == NULL, -ENOMEM);
msgpack_packer *pk = msgpack_packer_new(msgpack_buf, msgpack_sbuffer_write);
THROW_IF(pk == NULL, -ENOMEM);
/* fetch parameters: */
char *dev_path;
tsint_t dev_speed;
opcd_params_init("exynos_quad.arduino_serial.", 0);
opcd_param_t params[] =
{
{"path", &dev_path},
{"speed", &dev_speed},
OPCD_PARAMS_END
};
opcd_params_apply("", params);
/* opern serial port: */
serialport_t port;
THROW_ON_ERR(serial_open(&port, dev_path, tsint_get(&dev_speed), O_RDONLY));
uint16_t channels_raw[PPM_CHAN_MAX];
uint32_t voltage_raw, current_raw;
float channels[PPM_CHAN_MAX];
while (running)
{
int c = serial_read_char(&port);
if (c < 0)
msleep(1);
/* parse channels: */
int status = ppm_parse_frame(channels_raw, (uint8_t)(c));
if (status)
{
int sig_valid = 0;
int invalid_count = 0;
FOR_N(i, PPM_CHAN_MAX)
{
uint16_t chan_in = channels_raw[i];
if (chan_in >= PPM_VALUE_INVALID)
invalid_count++;
if (chan_in > PPM_VALUE_MAX)
chan_in = PPM_VALUE_MAX;
if (chan_in < PPM_VALUE_MIN)
chan_in = PPM_VALUE_MIN;
channels[i] = (float)(chan_in - PPM_VALUE_MIN) / (PPM_VALUE_MAX - PPM_VALUE_MIN) * 2.f - 1.f;
}
sig_valid = (invalid_count < (PPM_CHAN_MAX / 3));
/* send channels: */
msgpack_sbuffer_clear(msgpack_buf);
msgpack_pack_array(pk, 1 + PPM_CHAN_MAX);
PACKI(sig_valid); /* index 0: valid */
PACKFV(channels, PPM_CHAN_MAX); /* index 1, .. : channels */
scl_copy_send_dynamic(rc_socket, msgpack_buf->data, msgpack_buf->size);
}
/* parse adc voltage/current: */
status = power_parse_frame(&voltage_raw, ¤t_raw, (uint8_t)(c));
if (status)
{
float voltage = (float)(voltage_raw) / 1000.0;
float current = (float)(current_raw) / 1000.0;
/* send voltage / current: */
msgpack_sbuffer_clear(msgpack_buf);
msgpack_pack_array(pk, 2);
PACKF(voltage); /* index 0 */
PACKF(current); /* index 1 */
scl_copy_send_dynamic(power_socket, msgpack_buf->data, msgpack_buf->size);
}
}
int main_i2c(void)
{
THROW_BEGIN();
void *gps_socket = scl_get_socket("gps");
THROW_IF(gps_socket == NULL, -EIO);
int64_t hwm = 1;
zmq_setsockopt(gps_socket, ZMQ_SNDHWM, &hwm, sizeof(hwm));
void *sats_socket = scl_get_socket("sats");
THROW_IF(sats_socket == NULL, -EIO);
i2c_bus_t bus;
i2c_dev_t device;
uint8_t data_w[128];
uint8_t data_r[128];
if (i2c_bus_open(&bus, "/dev/i2c-1"))
{
syslog(LOG_CRIT, "could not open i2c device");
exit(EXIT_FAILURE);
}
i2c_dev_init(&device, &bus, I2C_GPS_ADDRESS);
msgpack_sbuffer *msgpack_buf = msgpack_sbuffer_new();
THROW_IF(msgpack_buf == NULL, -ENOMEM);
msgpack_packer *pk = msgpack_packer_new(msgpack_buf, msgpack_sbuffer_write);
THROW_IF(pk == NULL, -ENOMEM);
while (1)
{
msleep(200);
data_w[0] = I2C_GPS_STATUS;
i2c_xfer(&device, 1, data_w, 1, data_r);
msgpack_sbuffer_clear(msgpack_buf);
int status = data_r[0];
int fix = 0;
if (status & I2C_GPS_STATUS_2DFIX)
fix = 2;
if (status & I2C_GPS_STATUS_3DFIX)
fix = 3;
if (fix == 2)
msgpack_pack_array(pk, 7); /* 2d fix */
else if (fix == 3)
msgpack_pack_array(pk, 9); /* 3d fix */
else
msgpack_pack_array(pk, 1); /* no fix */
data_w[0] = I2C_GPS_TIME;
i2c_xfer(&device, 1, data_w, 4, data_r);
long time = (((long) data_r[3]) << 24)
| (((long) data_r[2]) << 16)
| (((long) data_r[1]) << 8)
| (data_r[0]);
time /= 1000;
char *time_str = ctime(&time);
size_t len = strlen(time_str);
msgpack_pack_raw(pk, len);
msgpack_pack_raw_body(pk, time_str, len); /* gps array index 0 */
if (fix == 2 || fix == 3)
{
data_w[0] = I2C_GPS_LOCATION;
i2c_xfer(&device, 1, data_w, 8, data_r);
PACKD(( (((long) data_r[3]) << 24)
| (((long) data_r[2]) << 16)
| (((long) data_r[1]) << 8)
| (data_r[0])) / 10000000.0); /* latitude, gps array index 1 */
PACKD(( (((long) data_r[7]) << 24)
| (((long) data_r[6]) << 16)
| (((long) data_r[5]) << 8)
| (data_r[4])) / 10000000.0); /* logitude, gps array index 2 */
PACKI((status & I2C_GPS_STATUS_NUMSATS) >> 4); /* gps array index 3 */
data_w[0] = I2C_GPS_GROUND_SPEED;
i2c_xfer(&device, 1, data_w, 2, data_r);
PACKF(((float)((data_r[1] << 8) | data_r[0])) / 100.0 * 1.94384); /* gps array index 4 */
data_w[0] = I2C_GPS_GROUND_SPEED;
i2c_xfer(&device, 1, data_w, 2, data_r);
PACKF((data_r[1] << 8) | data_r[0]); /* gps array index 5 */
PACKF(0 /* HDOP */); /* gps array index 6 */
}
if (fix == 3)
{
data_w[0] = I2C_GPS_ALTITUDE;
i2c_xfer(&device, 1, data_w, 2, data_r);
PACKF(((data_r[1]) << 8) | data_r[0]); /* gps array index 7 */
PACKF(0 /* VDOP */); /* gps array index 8 */
}
scl_copy_send_dynamic(gps_socket, msgpack_buf->data, msgpack_buf->size);
}
void _main(int argc, char *argv[])
{
(void)argc;
(void)argv;
if (scl_init("gpsp") != 0)
{
syslog(LOG_CRIT, "could not init scl module");
exit(EXIT_FAILURE);
}
gps_socket = scl_get_socket("data");
if (gps_socket == NULL)
{
syslog(LOG_CRIT, "could not get scl gate");
exit(EXIT_FAILURE);
}
int64_t hwm = 1;
zmq_setsockopt(gps_socket, ZMQ_SNDHWM, &hwm, sizeof(hwm));
opcd_param_t params[] =
{
{"serial_path", &serial_path},
{"serial_speed", &serial_speed},
OPCD_PARAMS_END
};
opcd_params_init("sensors.gps.", 0);
opcd_params_apply("", params);
int status = serial_open(&port, serial_path, tsint_get(&serial_speed), 0, 0, 0);
if (status < 0)
{
syslog(LOG_CRIT, "could not open serial port: %s", serial_path);
exit(EXIT_FAILURE);
}
nmeaPARSER parser;
nmea_parser_init(&parser);
nmeaINFO info;
nmea_zero_INFO(&info);
int time_set = 0;
int smask = 0; /* global smask collects all sentences and is never reset,
in contrast to info.smask */
while (running)
{
int c = serial_read_char(&port);
if (c < 0)
continue;
char b = c;
/* parse NMEA frame: */
if (nmea_parse(&parser, &b, 1, &info) == 1)
{
smask |= info.smask;
if ( (info.smask & GPGGA) /* check for new position update */
&& (smask & (GPGSA | GPRMC))) /* go sure that we collect all sentences for first output*/
{
GpsData gps_data = GPS_DATA__INIT;
/* set general data: */
char time_str[TIME_STR_LEN];
generate_time_str(time_str, &info.utc);
gps_data.fix = 0;
gps_data.time = time_str;
/* set system time to gps time once: */
if (!time_set && info.fix >= 2)
{
char shell_date_cmd[TIME_STR_LEN + 8];
linux_sys_set_timezone(convert(info.lat), convert(info.lon));
sprintf(shell_date_cmd, "date -s \"%s\"", time_str);
time_set = system(shell_date_cmd) == 0;
}
/* set position data if a minimum of satellites is seen: */
if (info.fix >= 2)
{
gps_data.fix = 2;
PB_SET(gps_data, hdop, info.HDOP);
PB_SET(gps_data, lat, convert(info.lat));
PB_SET(gps_data, lon, convert(info.lon));
PB_SET(gps_data, sats, info.satinfo.inuse);
PB_SET(gps_data, course, info.track);
PB_SET(gps_data, speed, info.speed);
}
/* set data for 3d fix: */
if (info.fix == 3)
{
gps_data.fix = 3;
PB_SET(gps_data, vdop, info.VDOP);
PB_SET(gps_data, alt, info.elv);
}
/* add satellit info: */
unsigned int i;
gps_data.n_satinfo = info.satinfo.inview;
SatInfo **satinfo = malloc(gps_data.n_satinfo * sizeof(SatInfo *));
for (i = 0; i < gps_data.n_satinfo; i++)
{
/* fill SatInfo structure: */
nmeaSATELLITE *nmea_satinfo = &info.satinfo.sat[i];
satinfo[i] = malloc(gps_data.n_satinfo * sizeof(SatInfo));
sat_info__init(satinfo[i]);
satinfo[i]->id = nmea_satinfo->id;
satinfo[i]->in_use = info.satinfo.in_use[i] ? 1 : 0;
satinfo[i]->elv = nmea_satinfo->elv;
satinfo[i]->azimuth = nmea_satinfo->azimuth;
satinfo[i]->sig = nmea_satinfo->sig;
}
gps_data.satinfo = satinfo;
/* send the data: */
SCL_PACK_AND_SEND_DYNAMIC(gps_socket, gps_data, gps_data);
/* free allocated memory: */
for (i = 0; i < gps_data.n_satinfo; i++)
{
free(satinfo[i]);
}
free(satinfo);
}
nmea_zero_INFO(&info);
}
}
}
