int serial_read(serial *s, char *buf, unsigned int len) { int i = 0; while (i < len-1) { int j = serial_read_char(s, &buf[i++]); if (j != 0){ buf[i-1] = '\0'; return -1; } } buf[i] = '\0'; return 1; }
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); } }
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); } } }