gboolean on_transmit_goals(gpointer _user)
{
RendererGoal *self = (RendererGoal*) _user;
pthread_mutex_lock(&self->mutex);
lcmtypes_goal_list_t glist;
glist.num_goals = self->my_goals->len;
glist.goals = (lcmtypes_goal_t *) self->my_goals->data;
glist.utime = timestamp_now();
glist.sender_id = self->sender_id;
lcmtypes_goal_list_t_publish(self->lc, "GOALS", &glist);
pthread_mutex_unlock(&self->mutex);
if (glist.num_goals == 0)
self->empty_count++;
else
self->empty_count = 0;
/* If goal list is empty, stop transmitting shortly afterwards */
if (self->empty_count > 20) {
self->transmitter = 0;
return FALSE;
}
return TRUE;
}
overlay::overlay(client_service_sptr clientService, int topologyID)
: basic_engine_object(clientService->get_io_service())
, basic_client_object(clientService->get_client_param_sptr())
, client_service_(clientService)
, topology_id_(topologyID)
, neighbors_conn_((topologyID == STREAM_TOPOLOGY) ? STREAM_NEIGHTBOR_PEER_CNT : HUB_NEIGHTBOR_PEER_CNT)
{
set_obj_desc("overlay");
timestamp_t now = timestamp_now();
last_exchange_neighbor_time_ = now;
last_member_request_time_ = now;
if (topology_id_ == STREAM_TOPOLOGY)
{
max_neighbor_cnt_ = get_client_param_sptr()->stream_neighbor_peer_cnt;
neighbor_ping_interval_ = STREAM_PEER_PEER_PING_INTERVAL;
}
else if (topology_id_ == HUB_TOPOLOGY)
{
max_neighbor_cnt_ = get_client_param_sptr()->hub_neighbor_peer_cnt;
neighbor_ping_interval_ = HUB_PEER_PEER_PING_INTERVAL;
}
else if (topology_id_ == CACHE_TOPOLOGY)
{
max_neighbor_cnt_ = get_client_param_sptr()->stream_neighbor_peer_cnt;
neighbor_ping_interval_ = STREAM_PEER_PEER_PING_INTERVAL;
}
else
{
BOOST_ASSERT(0 && "必须明确指定拓扑的max_neighbor_cnt_、neighbor_ping_interval_等");
}
}
int main(void){
cc3d_init();
printf("SystemCoreClock: %d\n", (int)SystemCoreClock);
// test config read/write (to eeprom)
const char str[] = "Hello World!";
uint8_t buf[13] = {0};
printf("Writing string to config: %s\n", str);
cc3d_write_config((const uint8_t*)str, sizeof(str));
printf("Reading string from config: ");
cc3d_read_config(buf, sizeof(str));
printf("%s\n", buf);
uint8_t led_state = 0;
timestamp_t ts = timestamp_from_now_us(500000);
serial_dev_t flexiport = cc3d_get_flexiport_serial_interface();
unsigned int loop = 0;
while(1){
printf("RC1: %d ", (int)cc3d_read_pwm(CC3D_IN_PWM1));
printf("RC2: %d ", (int)cc3d_read_pwm(CC3D_IN_PWM2));
printf("RC3: %d ", (int)cc3d_read_pwm(CC3D_IN_PWM3));
printf("RC4: %d ", (int)cc3d_read_pwm(CC3D_IN_PWM4));
printf("RC5: %d ", (int)cc3d_read_pwm(CC3D_IN_PWM5));
printf("RC6: %d ", (int)cc3d_read_pwm(CC3D_IN_PWM6));
cc3d_write_pwm(CC3D_OUT_PWM1, 1000 + loop % 1000);
cc3d_write_pwm(CC3D_OUT_PWM2, 1000 + loop % 1000);
cc3d_write_pwm(CC3D_OUT_PWM3, 1000 + loop % 1000);
cc3d_write_pwm(CC3D_OUT_PWM4, 1000 + loop % 1000);
cc3d_write_pwm(CC3D_OUT_PWM5, 1000 + loop % 1000);
cc3d_write_pwm(CC3D_OUT_PWM6, 1000 + loop % 1000);
float ax, ay, az, gx, gy, gz;
cc3d_read_acceleration_g(&ax, &ay, &az);
cc3d_read_angular_velocity_dps(&gx, &gy, &gz);
printf("Time: %ld ", (long int)timestamp_now());
printf("Gyro: %5d %5d %5d, Acc: %5d %5d %5d\n",
(int)(ax * 1000), (int)(ay * 1000), (int)(az * 1000),
(int)(gx * 1000), (int)(gy * 1000), (int)(gz * 1000));
serial_printf(flexiport, "Hello World!\n");
if(timestamp_expired(ts)){
led_state = ~led_state;
if(led_state) cc3d_led_on();
else cc3d_led_off();
ts = timestamp_from_now_us(500000);
loop += 100;
}
}
}
static int
lcm_tcpq_handle(lcm_tcpq_t * self)
{
if(self->socket < 0 && 0 != _connect_to_server(self)) {
return -1;
}
// read, ignore message type
uint32_t msg_type;
if(_recv_uint32(self->socket, &msg_type))
goto disconnected;
// read channel length, channel
uint32_t channel_len;
if(_recv_uint32(self->socket, &channel_len))
goto disconnected;
if(_ensure_buf_capacity((void**)&self->recv_channel_buf,
&self->recv_channel_buf_len, channel_len+1)) {
fprintf(stderr, "Memory allocation error\n");
return -1;
}
if(channel_len != _recv_fully(self->socket, self->recv_channel_buf,
channel_len))
goto disconnected;
self->recv_channel_buf[channel_len] = 0;
// read payload size, payload
uint32_t data_len;
if(_recv_uint32(self->socket, &data_len))
goto disconnected;
if(_ensure_buf_capacity(&self->data_buf, &self->data_buf_len, data_len)) {
fprintf(stderr, "Memory allocation error\n");
return -1;
}
if(data_len != _recv_fully(self->socket, self->data_buf, data_len))
goto disconnected;
lcm_recv_buf_t rbuf;
rbuf.data = self->data_buf;
rbuf.data_size = data_len;
rbuf.recv_utime = timestamp_now();
rbuf.lcm = self->lcm;
if(lcm_try_enqueue_message(self->lcm, self->recv_channel_buf))
lcm_dispatch_handlers(self->lcm, &rbuf, self->recv_channel_buf);
return 0;
disconnected:
_close_socket(self->socket);
self->socket = -1;
return -1;
}
static gboolean
on_timer(void *user_data)
{
state_t *app = (state_t*) user_data;
int64_t now = timestamp_now();
if(now > app->next_report_utime) {
// int nhosts = g_hash_table_size(app->hosts);
// printf("%d host%s transmitting\n", nhosts, nhosts == 1 ? "" : "s");
app->next_report_utime = now + app->report_interval_usec;
}
return TRUE;
}
void *gps_read_proc(void *v)
{
gps_t *g = (gps_t*) v;
uint8_t buf[GPS_MESSAGE_MAXLEN + 1];
buf[GPS_MESSAGE_MAXLEN] = 0;
while (1) {
int res = gps_readline(g, buf, GPS_MESSAGE_MAXLEN);
if (!res && g->readline_callback != NULL) {
int64_t now = timestamp_now();
g->readline_callback(g->readline_callback_context, now, (const char*) buf);
}
}
return NULL;
}
int main(int argc, char **argv)
{
state_t *app = calloc(1, sizeof(state_t));
app->lcmurl = strdup(get_default_lcm_url());
if (0 != lcm_parse_url(app->lcmurl, &app->mc_addr, &app->mc_port)) {
fprintf (stderr, "invalid URL [%s]\n", app->lcmurl);
return 1;
}
struct in_addr ia;
ia = app->mc_addr;
printf("MC group: %s port: %d\n", inet_ntoa(ia), app->mc_port);
if(0 != setup_socket(app)) {
return 1;
}
app->hosts = g_hash_table_new_full(g_int_hash, g_int_equal, NULL,
(GDestroyNotify)host_destroy);
app->mainloop = g_main_loop_new(NULL, FALSE);
app->ioc = g_io_channel_unix_new(app->recvfd);
app->sid = g_io_add_watch(app->ioc, G_IO_IN, (GIOFunc)on_message_ready,
app);
g_timeout_add(100, on_timer, app);
app->report_interval_usec = DEFAULT_REPORT_INTERVAL_SECONDS * 1000000;
app->next_report_utime = timestamp_now() + 1000000;
bot_signal_pipe_glib_quit_on_kill(app->mainloop);
g_main_loop_run(app->mainloop);
g_io_channel_unref(app->ioc);
g_source_remove(app->sid);
shutdown(app->recvfd, SHUT_RDWR);
g_hash_table_destroy(app->hosts);
free(app->lcmurl);
free(app);
return 0;
}
void writeDataFromPipe(int in, int out) {
/* reads data from `in`, formats it, writes it to `out`.
* mode 0 - read from pipe
*/
unsigned char buffer[INBUFFSIZE];
char outbuf[OUTBUFFSIZE];
int n;
char tstamp[30];
int tstamp_len;
if ((n = read(in, buffer, INBUFFSIZE)) < 0) { /* read nothing */
perror(RPIPEFAIL);
}
else { /* successfully read something from somewhere */
if (n >=8) { /* need at least 8 bytes for a meter packet */
/* read from pipe */
/* print timestamp if necessary */
if (tty_data.tstamp) {
timestamp_now( timeStamp );
tstamp_len = timestamp_sprint( timeStamp, tstamp, 30 );
write(out, tstamp, tstamp_len);
write(out, "\t", 1);
}
/* format data */
fmtData(buffer, outbuf, n);
/* print data */
write(out, outbuf, strlen(outbuf));
/* count (decoded packet) writes */
tty_data.numSamples++;
/* print total number of bytes if necessary */
if (tty_data.dspbytes) {
buffer[0] = 0;
sprintf(buffer, "\t%s %i\n", TOTALBYTES, n);
write (out, buffer, strlen(buffer));
}
} /* end if n >= 8 */
} /* end else there were characters to read */
} /* end writeData() */
static int
on_message_ready(GIOChannel *source, GIOCondition cond, void *user_data)
{
state_t *app = (state_t*) user_data;
char buf[65536];
struct sockaddr_in from;
socklen_t fromlen = sizeof(struct sockaddr);
int sz = recvfrom(app->recvfd, buf, 65536, 0,
(struct sockaddr*) &from, &fromlen);
if (sz < 0) {
perror("receiving message");
return TRUE;
}
int64_t recv_utime = timestamp_now();
time_t recv_t = recv_utime / 1000000;
struct tm *recv_tm = localtime(&recv_t);
char recv_tm_buf[200];
strftime(recv_tm_buf, sizeof(recv_tm_buf), "%b %d %H:%M:%S", recv_tm);
int *key = (int*) &from.sin_addr.s_addr;
host_t * host = g_hash_table_lookup(app->hosts, key);
if(!host) {
host = host_new(from.sin_addr);
g_hash_table_insert(app->hosts, &host->addr.s_addr, host);
printf("%s - new host detected! %s\n", recv_tm_buf, host->addr_str);
}
sender_t *sender = host_get_sender(host, ntohs(from.sin_port));
if(!sender) {
sender = host_add_new_sender(host, ntohs(from.sin_port));
printf("%s - new sender detected! %s\n", recv_tm_buf, sender->id_str);
}
sender->last_recvtime = recv_utime;
host->last_recvtime = recv_utime;
return TRUE;
}
static int lcm_memq_publish(lcm_memq_t *self, const char *channel, const void *data,
unsigned int datalen)
{
if (!lcm_has_handlers(self->lcm, channel)) {
dbg(DBG_LCM, "Publishing [%s] size [%d] - dropping (no subscribers)\n", channel, datalen);
return 0;
}
dbg(DBG_LCM, "Publishing to [%s] message size [%d]\n", channel, datalen);
memq_msg_t *msg = memq_msg_new(self->lcm, channel, data, datalen, timestamp_now());
g_mutex_lock(self->mutex);
int was_empty = g_queue_is_empty(self->queue);
g_queue_push_tail(self->queue, msg);
if (was_empty) {
if (lcm_internal_pipe_write(self->notify_pipe[1], "+", 1) < 0) {
perror(__FILE__ " - write to notify pipe (lcm_memq_publish)");
}
}
g_mutex_unlock(self->mutex);
return 0;
}
void absent_packet_list::erase(seqno_t seqno)
{
BOOST_AUTO(itr, not_requesting_packets_.find(seqno));
if (itr != not_requesting_packets_.end())
{
erase(itr, false);
}
else if ((itr = requesting_packets_.find(seqno)) != requesting_packets_.end())
{
erase(itr, true);
}
else
{
BOOST_AUTO(&pktInfoPtr, get_slot(media_packet_info_vector_, seqno));
if (pktInfoPtr&&pktInfoPtr->is_this(seqno, timestamp_now()))
{
__delete(seqno);
}
}
BOOST_ASSERT(!find(seqno));
}
static void *
timer_thread (void * user)
{
lcm_logprov_t * lr = (lcm_logprov_t *) user;
int64_t abstime;
struct timeval sleep_tv;
while (lcm_internal_pipe_read(lr->timer_pipe[0], &abstime, 8) == 8) {
if (abstime < 0) return NULL;
int64_t now = timestamp_now();
if (abstime > now) {
int64_t sleep_utime = abstime - now;
sleep_tv.tv_sec = sleep_utime / 1000000;
sleep_tv.tv_usec = sleep_utime % 1000000;
// sleep until the next timed message, or until an abort message
fd_set fds;
FD_ZERO (&fds);
FD_SET (lr->timer_pipe[0], &fds);
int status = select (lr->timer_pipe[0] + 1, &fds, NULL, NULL,
&sleep_tv);
if (0 == status) {
// select timed out
if(lcm_internal_pipe_write(lr->notify_pipe[1], "+", 1) < 0) {
perror(__FILE__ " - write (timer select)");
}
}
} else {
if(lcm_internal_pipe_write(lr->notify_pipe[1], "+", 1) < 0) {
perror(__FILE__ " - write (timer)");
}
}
}
perror ("timer_thread read failed");
return NULL;
}
/* output to telemetry
* whatever 'tag' means is up to the user, and can just be set to 0, but
* was designed to be used for tracking specific objects as they head around
* the place
*/
void telemetry_vprintf(uint32_t tag, const char *format, va_list vargs) {
char telem_str[TELEMETRY_MAX_LINE_LENGTH];
char annotated_telem_str[TELEMETRY_MAX_LINE_LENGTH];
if (! telemetry_running)
return;
/* Build the string for the supplied stuff */
vsnprintf(telem_str, TELEMETRY_MAX_LINE_LENGTH-1, format, vargs);
telem_str[TELEMETRY_MAX_LINE_LENGTH-1] = 0;
/* Add our own stuff to give context */
snprintf(annotated_telem_str, TELEMETRY_MAX_LINE_LENGTH-1,
"%s %lu %08x %s", telemetry_client_name, timestamp_now(), tag, telem_str);
annotated_telem_str[TELEMETRY_MAX_LINE_LENGTH-1] = 0;
/* Output */
if (telemetry_output_file != NULL)
fprintf((FILE *)telemetry_output_file, "%s\n", annotated_telem_str);
if (telemetry_context != NULL)
telemetry_zmq_send(annotated_telem_str, strlen(annotated_telem_str));
}
static void*
write_thread(void *user_data)
{
logger_t *logger = (logger_t*) user_data;
GTimeVal start_time;
g_get_current_time(&start_time);
int num_splits = 0;
int64_t next_split_sec = start_time.tv_sec + logger->auto_split_hours * SECONDS_PER_HOUR;
while(1) {
void *msg = g_async_queue_pop(logger->write_queue);
// Is it time to start a new logfile?
int split_log = 0;
if(logger->auto_split_hours) {
GTimeVal now;
g_get_current_time(&now);
split_log = (now.tv_sec > next_split_sec);
} else if(logger->auto_split_mb) {
double logsize_mb = (double)logger->logsize / (1 << 20);
split_log = (logsize_mb > logger->auto_split_mb);
}
if(_reset_logfile) {
split_log = 1;
_reset_logfile = 0;
}
if(split_log) {
// Yes. open up a new log file
lcm_eventlog_destroy(logger->log);
if(0 != open_logfile(logger))
exit(1);
num_splits++;
int64_t log_duration_sec = logger->auto_split_hours * SECONDS_PER_HOUR;
next_split_sec = start_time.tv_sec + (num_splits + 1) * log_duration_sec;
logger->logsize = 0;
logger->last_report_logsize = 0;
}
// Should the write thread exit?
g_mutex_lock(logger->mutex);
if(logger->write_thread_exit_flag) {
g_mutex_unlock(logger->mutex);
return NULL;
}
// nope. write the event to disk
lcm_eventlog_event_t *le = (lcm_eventlog_event_t*) msg;
int64_t sz = sizeof(lcm_eventlog_event_t) + le->channellen + 1 + le->datalen;
logger->write_queue_size -= sz;
g_mutex_unlock(logger->mutex);
if(0 != lcm_eventlog_write_event(logger->log, le)) {
static int64_t last_spew_utime = 0;
char *reason = strdup(strerror(errno));
int64_t now = timestamp_now();
if(now - last_spew_utime > 500000) {
fprintf(stderr, "lcm_eventlog_write_event: %s\n", reason);
last_spew_utime = now;
}
free(reason);
free(le);
if(errno == ENOSPC) {
exit(1);
} else {
continue;
}
}
if (logger->fflush_interval_ms >= 0 &&
(le->timestamp - logger->last_fflush_time) > logger->fflush_interval_ms*1000) {
fflush(logger->log->f);
logger->last_fflush_time = le->timestamp;
}
// bookkeeping, cleanup
int64_t offset_utime = le->timestamp - logger->time0;
logger->nevents++;
logger->events_since_last_report ++;
logger->logsize += 4 + 8 + 8 + 4 + le->channellen + 4 + le->datalen;
free(le);
if (offset_utime - logger->last_report_time > 1000000) {
double dt = (offset_utime - logger->last_report_time)/1000000.0;
double tps = logger->events_since_last_report / dt;
double kbps = (logger->logsize - logger->last_report_logsize) / dt / 1024.0;
printf("Summary: %s ti:%4"PRIi64"sec Events: %-9"PRIi64" ( %4"PRIi64" MB ) TPS: %8.2f KB/s: %8.2f\n",
logger->fname,
timestamp_seconds(offset_utime),
logger->nevents, logger->logsize/1048576,
tps, kbps);
logger->last_report_time = offset_utime;
logger->events_since_last_report = 0;
logger->last_report_logsize = logger->logsize;
}
}
}
int main(int argc, char *argv[])
{
auto options = std::unique_ptr<getopt_t, void(*)(getopt_t*)> (getopt_create(), getopt_destroy);
// getopt_t *options = getopt_create();
getopt_add_bool(options.get(), 'h', "help", 0, "Show this help");
getopt_add_bool(options.get(), 'd', "debug", 0, "Enable debugging output (slow)");
getopt_add_bool(options.get(), 'w', "window", 1, "Show the detected tags in a window");
getopt_add_bool(options.get(), 'q', "quiet", 0, "Reduce output");
getopt_add_int(options.get(), '\0', "border", "1", "Set tag family border size");
getopt_add_int(options.get(), 't', "threads", "4", "Use this many CPU threads");
getopt_add_double(options.get(), 'x', "decimate", "1.0", "Decimate input image by this factor");
getopt_add_double(options.get(), 'b', "blur", "0.0", "Apply low-pass blur to input");
getopt_add_bool(options.get(), '0', "refine-edges", 1, "Spend more time trying to align edges of tags");
getopt_add_bool(options.get(), '1', "refine-decode", 0, "Spend more time trying to decode tags");
getopt_add_bool(options.get(), '2', "refine-pose", 0, "Spend more time trying to precisely localize tags");
getopt_add_double(options.get(), 's', "size", "0.04047", "Physical side-length of the tag (meters)");
getopt_add_int(options.get(), 'c', "camera", "0", "Camera ID");
getopt_add_int(options.get(), 'i', "tag_id", "-1", "Tag ID (-1 for all tags in family)");
if (!getopt_parse(options.get(), argc, argv, 1) || getopt_get_bool(options.get(), "help")) {
printf("Usage: %s [options]\n", argv[0]);
getopt_do_usage(options.get());
exit(0);
}
AprilTagDetector tag_detector(options);
auto lcm = std::make_shared<lcm::LCM>();
Eigen::Matrix3d camera_matrix = Eigen::Matrix3d::Identity();
// camera_matrix(0,0) = bot_camtrans_get_focal_length_x(mCamTransLeft);
// camera_matrix(1,1) = bot_camtrans_get_focal_length_y(mCamTransLeft);
// camera_matrix(0,2) = bot_camtrans_get_principal_x(mCamTransLeft);
// camera_matrix(1,2) = bot_camtrans_get_principal_y(mCamTransLeft);
camera_matrix(0,0) = 535.04778754;
camera_matrix(1,1) = 533.37100256;
camera_matrix(0,2) = 302.83654976;
camera_matrix(1,2) = 237.69023961;
Eigen::Vector4d distortion_coefficients(-7.74010810e-02, -1.97835565e-01, -4.47956948e-03, -5.42361499e-04);
// camera matrix:
// [[ 535.04778754 0. 302.83654976]
// [ 0. 533.37100256 237.69023961]
// [ 0. 0. 1. ]]
// distortion coefficients: [ -7.74010810e-02 -1.97835565e-01 -4.47956948e-03 -5.42361499e-04
// 9.30985112e-01]
cv::VideoCapture capture(getopt_get_int(options.get(), "camera"));
if (!capture.isOpened()) {
std::cout << "Cannot open the video cam" << std::endl;
return -1;
}
cv::Mat frame;
Eigen::Isometry3d tag_to_camera = Eigen::Isometry3d::Identity();
crazyflie_t::webcam_pos_t tag_to_camera_msg;
while (capture.read(frame)) {
std::vector<TagMatch> tags = tag_detector.detectTags(frame);
if (tags.size() > 0) {
tag_to_camera = getRelativeTransform(tags[0], camera_matrix, distortion_coefficients, tag_detector.getTagSize());
tag_to_camera_msg = encodeWebcamPos(tag_to_camera);
tag_to_camera_msg.frame_id = 1;
} else {
tag_to_camera_msg = encodeWebcamPos(tag_to_camera);
tag_to_camera_msg.frame_id = -1;
}
tag_to_camera_msg.timestamp = timestamp_now();
lcm->publish("WEBCAM_POS", &tag_to_camera_msg);
}
return 0;
}
/*! \brief Thread routine I/O
*
* @param ptr pointer to Ports type with input and output ip address and port.
*/
void *datap_io_thread (void* ptr) {
Ports* port_info;
SensorID sensor_listen_id;
port_info = (Ports*) ptr;
int i = 0;
int status;
int retval;
int clientsocket_fd;
int numbytes;
socklen_t addr_len;
char timestring[TIMEBUFLEN];
char ipstr[INET6_ADDRSTRLEN];
char recvbuf[MAX_RECV_BUFLEN];
char s[INET6_ADDRSTRLEN];
pthread_t my_id;
FILE *fp_mpl, *fp_mpu, *fp_adis;
struct addrinfo hints, *res, *p, *ai_client;
struct sockaddr_storage client_addr;
socklen_t client_addr_len;
client_addr_len = sizeof(struct sockaddr_storage);
memset(&hints, 0, sizeof hints);
hints.ai_family = AF_UNSPEC; // AF_INET or AF_INET6 to force version
hints.ai_socktype = SOCK_DGRAM; // UDP
hints.ai_flags = AI_PASSIVE; // use local host address.
fprintf(stderr, "%s: listen port %s\n", __func__, port_info->host_listen_port);
if(ports_equal(port_info->client_port, IMU_A_TX_PORT_ADIS)) {
sensor_listen_id = ADIS_LISTENER;
} else if (ports_equal(port_info->client_port, IMU_A_TX_PORT_MPU)) {
sensor_listen_id = MPU_LISTENER;
} else if (ports_equal(port_info->client_port, IMU_A_TX_PORT_MPL)) {
sensor_listen_id = MPL_LISTENER;
} else {
sensor_listen_id = UNKNOWN_SENSOR;
}
if(sensor_listen_id == ADIS_LISTENER) {
fp_adis = fopen("adis16405_log.txt", "w");
get_current_time(timestring) ;
fprintf(fp_adis, "# adis16405 IMU data started at: %s\n", timestring);
fprintf(fp_adis, "# adis16405 IMU raw data\n");
fprintf(fp_adis, "# timestamp,ax,ay,az,gx,gy,gz,mx,my,mz,C\n");
} else if (sensor_listen_id == MPU_LISTENER) {
fp_mpu = fopen("mpu9150_log.txt", "w");
get_current_time(timestring) ;
fprintf(fp_mpu, "# mpu9150 IMU data started at: %s\n", timestring);
fprintf(fp_mpu, "# mpu9150 IMU raw data\n");
fprintf(fp_mpu, "# timestamp,ax,ay,az,gx,gy,gz,C\n");
} else if (sensor_listen_id == MPL_LISTENER) {
fp_mpl = fopen("mpl3115a2_log.txt", "w");
get_current_time(timestring) ;
fprintf(fp_mpl, "# mpl3115a2 Pressure Sensor data started at: %s\n", timestring);
fprintf(fp_mpl, "# mpl3115a2 Pressure sensor raw data\n");
fprintf(fp_mpl, "# timestamp,P,T\n");
} else {
;
}
memset(&hints, 0, sizeof hints);
hints.ai_family = AF_UNSPEC;
hints.ai_socktype = SOCK_DGRAM;
if ((retval = getaddrinfo(port_info->client_addr, port_info->client_port, &hints, &res)) != 0) {
fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(retval));
die_nice("client get address");
}
/* Create a socket for the client */
for(ai_client = res; ai_client != NULL; ai_client = ai_client->ai_next) {
if ((clientsocket_fd = socket(ai_client->ai_family, ai_client->ai_socktype,
ai_client->ai_protocol)) == -1) {
perror("clientsocket");
continue;
}
break;
}
if (ai_client == NULL) {
die_nice("failed to bind client socket\n");
}
//for(i=0; i<NPACK; ++i) {
uint32_t datacount = 0;
while(!user_exit_requested) {
char countmsg[MAX_USER_STRBUF];
struct sockaddr *sa;
socklen_t len;
char hbuf[NI_MAXHOST], sbuf[NI_MAXSERV];
addr_len = sizeof client_addr;
if ((numbytes = recvfrom(hostsocket_fd, recvbuf, MAX_RECV_BUFLEN-1 , 0,
(struct sockaddr *)&client_addr, &addr_len)) == -1) {
die_nice("recvfrom");
}
if (getnameinfo((struct sockaddr *)&client_addr, client_addr_len, hbuf, sizeof(hbuf), sbuf,
sizeof(sbuf), NI_NUMERICHOST | NI_NUMERICSERV) == 0) {
if(ports_equal(sbuf, IMU_A_TX_PORT_MPU) && (sensor_listen_id == MPU_LISTENER)) {
if(numbytes != sizeof(MPU_packet) ){
die_nice("wrong numbytes mpu");
}
memcpy (&mpu9150_udp_data, recvbuf, sizeof(MPU_packet));
mpu9150_imu_data = (MPU9150_read_data) mpu9150_udp_data.data;
if(enable_logging) {
fprintf(fp_mpu, "%c%c%c%c,%f,%d,%d,%d,%d,%d,%d,%d\n",
mpu9150_udp_data.ID[0], mpu9150_udp_data.ID[1],mpu9150_udp_data.ID[2],mpu9150_udp_data.ID[3],
timestamp_now(),
mpu9150_imu_data.accel_xyz.x,
mpu9150_imu_data.accel_xyz.y,
mpu9150_imu_data.accel_xyz.z,
mpu9150_imu_data.gyro_xyz.x,
mpu9150_imu_data.gyro_xyz.y,
mpu9150_imu_data.gyro_xyz.z,
mpu9150_imu_data.celsius);
++datacount;
snprintf(countmsg, MAX_USER_STRBUF , " %d MPU entries.", datacount );
if(datacount %COUNT_INTERVAL == 0) {
log_msg(countmsg);
}
}
fflush(fp_mpu);
#if DEBUG_MPU_NET
printf("\r\nraw_temp: %3.2f C\r\n", mpu9150_temp_to_dC(mpu9150_udp_data.celsius));
printf("ACCL: x: %d\ty: %d\tz: %d\r\n", mpu9150_udp_data.accel_xyz.x, mpu9150_udp_data.accel_xyz.y, mpu9150_udp_data.accel_xyz.z);
printf("GRYO: x: 0x%x\ty: 0x%x\tz: 0x%x\r\n", mpu9150_udp_data.gyro_xyz.x, mpu9150_udp_data.gyro_xyz.y, mpu9150_udp_data.gyro_xyz.z);
if ((numbytes = sendto(clientsocket_fd, recvbuf, strlen(recvbuf), 0,
ai_client->ai_addr, ai_client->ai_addrlen)) == -1) {
die_nice("client sendto");
}
#endif
} else if (ports_equal(sbuf, IMU_A_TX_PORT_MPL) && (sensor_listen_id == MPL_LISTENER)) {
if(numbytes != sizeof(MPL_packet) ){
die_nice("wrong numbytes mpl");
}
memcpy (&mpl3115a2_udp_data, recvbuf, sizeof(MPL_packet));
mpl3115a2_pt_data = mpl3115a2_udp_data.data;
if(enable_logging) {
fprintf(fp_mpl, "%c%c%c%c,%f,%d,%d\n",
mpl3115a2_udp_data.ID[0], mpl3115a2_udp_data.ID[1],mpl3115a2_udp_data.ID[2],mpl3115a2_udp_data.ID[3],
timestamp_now(),
mpl3115a2_pt_data.mpu_pressure,
mpl3115a2_pt_data.mpu_temperature
);
++datacount;
snprintf(countmsg, MAX_USER_STRBUF , " %d MPL entries.", datacount );
if(datacount % COUNT_INTERVAL == 0) {
log_msg(countmsg);
}
}
fflush(fp_mpl);
} else if (ports_equal(sbuf, IMU_A_TX_PORT_ADIS) && (sensor_listen_id == ADIS_LISTENER)) {
double adis_temp_C = 0.0;
bool adis_temp_neg = false;
if(numbytes != sizeof(ADIS_packet) ){
die_nice("wrong numbytes adis");
}
memcpy (&adis16405_udp_data, recvbuf, sizeof(ADIS_packet));
adis16405_imu_data = adis16405_udp_data.data;
if(enable_logging) {
adis_temp_neg = adis16405_temp_to_dC(&adis_temp_C, &adis16405_imu_data.adis_temp_out);
fprintf(fp_adis, "%c%c%c%c,%f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
// fprintf(fp_adis, "%f,%d,%d,%d,%d,%d,%d,%0x%x\n",
adis16405_udp_data.ID[0], adis16405_udp_data.ID[1],adis16405_udp_data.ID[2],adis16405_udp_data.ID[3],
timestamp_now(),
adis16405_imu_data.adis_xaccl_out,
adis16405_imu_data.adis_yaccl_out,
adis16405_imu_data.adis_zaccl_out,
adis16405_imu_data.adis_xgyro_out,
adis16405_imu_data.adis_ygyro_out,
adis16405_imu_data.adis_zgyro_out,
adis16405_imu_data.adis_xmagn_out,
adis16405_imu_data.adis_ymagn_out,
adis16405_imu_data.adis_zmagn_out,
adis16405_imu_data.adis_temp_out
);
++datacount;
snprintf(countmsg, MAX_USER_STRBUF , " %d ADIS entries.", datacount );
if(datacount %COUNT_INTERVAL == 0) {
log_msg(countmsg);
}
}
fflush(fp_adis);
} else {
// printf("Unrecognized Packet %s:%s\tListen: %s\tThreadID: %d\n", hbuf, sbuf, listentostr(sensor_listen_id), port_info->thread_id);
// if(ports_equal(sbuf, IMU_A_TX_PORT_ADIS)) printf("ADIS port\n\n");
// if(ports_equal(sbuf, IMU_A_TX_PORT_MPU)) printf("MPU port\n\n");
}
}
}
get_current_time(timestring) ;
if(sensor_listen_id == MPU_LISTENER ) {
fprintf(fp_mpu, "# mpu9150 IMU data closed at: %s\n", timestring);
fclose(fp_mpu);
} else if (sensor_listen_id == MPL_LISTENER) {
fprintf(fp_mpl, "# mpu3115a2 P T data closed at: %s\n", timestring);
fclose(fp_mpl);
}
else if (sensor_listen_id == ADIS_LISTENER) {
fprintf(fp_adis, "# adis16405 IMU data closed at: %s\n", timestring);
fclose(fp_adis);
}
close(clientsocket_fd);
freeaddrinfo(res); // free the linked list
fprintf(stderr, "Leaving thread %d\n", port_info->thread_id);
return 0;
}
int main(int argc, char *argv[])
{
#ifndef WIN32
setlinebuf (stdout);
#endif
char logpath[PATH_MAX];
memset (logpath, 0, sizeof (logpath));
logger_t logger;
memset (&logger, 0, sizeof (logger));
// set some defaults
logger.force_overwrite = 0;
logger.auto_increment = 0;
logger.use_strftime = 0;
char *chan_regex = strdup(".*");
double max_write_queue_size_mb = DEFAULT_MAX_WRITE_QUEUE_SIZE_MB;
logger.invert_channels = 0;
logger.fflush_interval_ms = 100;
char *lcmurl = NULL;
char *optstring = "a:fic:shm:vu:";
int c;
struct option long_opts[] = {
{ "auto-split-hours", required_argument, 0, 'a' },
{ "auto-split-mb", required_argument, 0, 'b' },
{ "channel", required_argument, 0, 'c' },
{ "force", no_argument, 0, 'f' },
{ "increment", required_argument, 0, 'i' },
{ "lcm-url", required_argument, 0, 'l' },
{ "max-unwritten-mb", required_argument, 0, 'm' },
{ "strftime", required_argument, 0, 's' },
{ "invert-channels", no_argument, 0, 'v' },
{ "flush-interval", required_argument, 0,'u'},
{ 0, 0, 0, 0 }
};
while ((c = getopt_long (argc, argv, optstring, long_opts, 0)) >= 0)
{
switch (c) {
case 'a':
logger.auto_split_hours = strtod(optarg, NULL);
logger.auto_increment = 1;
if(logger.auto_split_hours <= 0) {
usage();
return 1;
}
break;
case 'b':
logger.auto_split_mb = strtod(optarg, NULL);
logger.auto_increment = 1;
if(logger.auto_split_mb <= 0) {
usage();
return 1;
}
break;
case 'f':
logger.force_overwrite = 1;
break;
case 'c':
free(chan_regex);
chan_regex = strdup(optarg);
break;
case 'i':
logger.auto_increment = 1;
break;
case 's':
logger.use_strftime = 1;
break;
case 'l':
free(lcmurl);
lcmurl = strdup(optarg);
break;
case 'v':
logger.invert_channels = 1;
break;
case 'm':
max_write_queue_size_mb = strtod(optarg, NULL);
if(max_write_queue_size_mb <= 0) {
usage();
return 1;
}
break;
case 'u':
logger.fflush_interval_ms = atol(optarg);
if(logger.fflush_interval_ms <= 0) {
usage();
return 1;
}
break;
case 'h':
default:
usage();
return 1;
};
}
if (optind == argc) {
strcpy (logger.input_fname, "lcmlog-%Y-%m-%d");
logger.auto_increment = 1;
logger.use_strftime = 1;
} else if (optind == argc - 1) {
strncpy (logger.input_fname, argv[optind], sizeof (logger.input_fname));
} else if (optind < argc-1) {
usage ();
return 1;
}
// initialize GLib threading
g_thread_init(NULL);
logger.time0 = timestamp_now();
logger.max_write_queue_size = (int64_t)(max_write_queue_size_mb * (1 << 20));
if(0 != open_logfile(&logger))
return 1;
// create write thread
logger.write_thread_exit_flag = 0;
logger.mutex = g_mutex_new();
logger.write_queue_size = 0;
logger.write_queue = g_async_queue_new();
logger.write_thread = g_thread_create(write_thread, &logger, TRUE, NULL);
// begin logging
logger.lcm = lcm_create (lcmurl);
free(lcmurl);
if (!logger.lcm) {
fprintf (stderr, "Couldn't initialize LCM!");
return 1;
}
if(logger.invert_channels) {
// if inverting the channels, subscribe to everything and invert on the
// callback
lcm_subscribe(logger.lcm, ".*", message_handler, &logger);
char *regexbuf = g_strdup_printf("^%s$", chan_regex);
#ifdef USE_GREGEX
GError *rerr = NULL;
logger.regex = g_regex_new(regexbuf, (GRegexCompileFlags) 0, (GRegexMatchFlags) 0, &rerr);
if(rerr) {
fprintf(stderr, "%s\n", rerr->message);
g_free(regexbuf);
return 1;
}
#else
if (0 != regcomp (&logger.preg, regexbuf, REG_NOSUB | REG_EXTENDED)) {
fprintf(stderr, "bad regex!\n");
g_free(regexbuf);
return 1;
}
#endif
g_free(regexbuf);
} else {
// otherwise, let LCM handle the regex
lcm_subscribe(logger.lcm, chan_regex, message_handler, &logger);
}
free(chan_regex);
_mainloop = g_main_loop_new (NULL, FALSE);
signal_pipe_glib_quit_on_kill ();
glib_mainloop_attach_lcm (logger.lcm);
#ifdef USE_SIGHUP
signal(SIGHUP, sighup_handler);
#endif
// main loop
g_main_loop_run (_mainloop);
fprintf(stderr, "Logger exiting\n");
// stop the write thread
g_mutex_lock(logger.mutex);
logger.write_thread_exit_flag = 1;
g_mutex_unlock(logger.mutex);
g_async_queue_push(logger.write_queue, &logger.write_thread_exit_flag);
g_thread_join(logger.write_thread);
g_mutex_free(logger.mutex);
// cleanup. This isn't strictly necessary, do it to be pedantic and so that
// leak checkers don't complain
glib_mainloop_detach_lcm (logger.lcm);
lcm_destroy (logger.lcm);
lcm_eventlog_destroy (logger.log);
for(void *msg = g_async_queue_try_pop(logger.write_queue); msg;
msg=g_async_queue_try_pop(logger.write_queue)) {
if(msg == &logger.write_thread_exit_flag)
continue;
free(msg);
}
g_async_queue_unref(logger.write_queue);
if(logger.invert_channels) {
#ifdef USE_GREGEX
g_regex_unref(logger.regex);
#else
regfree(&logger.preg);
#endif
}
return 0;
}
static gboolean
on_gl_expose (GtkWidget *widget, GdkEventExpose *event, void *user_data)
{
Viewer * self = (Viewer *) user_data;
// if not enough time has elapsed since our last redraw, we
// schedule a redraw in the future (if one isn't already pending).
int64_t now = timestamp_now();
double dt = (now - self->last_draw_utime) / 1000000.0;
if (dt < (1.0/MAX_REDRAW_HZ)) {
if (!self->redraw_timer_pending) {
int delay_ms = (now - self->last_draw_utime)/1000 + 1;
g_timeout_add(delay_ms, on_redraw_timer, self);
self->redraw_timer_pending = 1;
}
return TRUE;
}
self->last_draw_utime = now;
// If we're making a movie, don't draw any faster than the
// requested movie FPS rate.
if (self->movie_gzf && !self->movie_draw_pending)
return TRUE;
// set this to 1 in order to cause viewer to exit cleanly after a
// few hundred frames: useful for generating gprof output.
g_draws++;
if (0) {
int thresh = 300;
// for profiling PROFILE
if (g_draws%50 == 0)
printf("draws: %5i / %i\n", g_draws, thresh);
if (g_draws == thresh) {
printf("Profiling is enabled: exiting now\n");
exit(0);
}
}
// we're going to actually draw.
gtku_gl_drawing_area_set_context (self->gl_area);
render_scene (self);
gtku_gl_drawing_area_swap_buffers (self->gl_area);
// write a movie frame?
if (self->movie_draw_pending) {
assert(self->movie_gzf);
glReadPixels (0, 0, self->movie_width, self->movie_height, GL_RGB, GL_UNSIGNED_BYTE, self->movie_buffer);
gzprintf(self->movie_gzf, "P6 %d %d %d\n", self->movie_width, self->movie_height, 255);
for (int h = self->movie_height - 1; h >= 0; h--) {
int offset = self->movie_stride * h;
gzwrite(self->movie_gzf, &self->movie_buffer[offset], self->movie_stride);
}
self->movie_draw_pending = 0;
int64_t now = timestamp_now();
double dt;
if (self->movie_frame_last_utime == 0)
dt = 1.0 / self->movie_desired_fps;
else
dt = (now - self->movie_frame_last_utime)/1000000.0;
double fps = 1.0 / dt;
self->movie_frame_last_utime = now;
double alpha = 0.8; // higher = lower-pass
self->movie_actual_fps = alpha * self->movie_actual_fps + (1 - alpha) * fps;
self->movie_frames++;
printf("%20s %6d (%5.2f fps)\r", self->movie_path, self->movie_frames, self->movie_actual_fps);
fflush(NULL);
}
return TRUE;
}
static int
lcm_logprov_handle (lcm_logprov_t * lr)
{
lcm_recv_buf_t rbuf;
if (!lr->event)
return -1;
char ch;
int status = lcm_internal_pipe_read(lr->notify_pipe[0], &ch, 1);
if (status == 0) {
fprintf (stderr, "Error: lcm_handle read 0 bytes from notify_pipe\n");
return -1;
}
else if (status < 0) {
fprintf (stderr, "Error: lcm_handle read: %s\n", strerror (errno));
return -1;
}
int64_t now = timestamp_now ();
/* Initialize the wall clock if this is the first time through */
if (lr->next_clock_time < 0)
lr->next_clock_time = now;
// rbuf.channel = lr->event->channel,
rbuf.data = (uint8_t*) lr->event->data;
rbuf.data_size = lr->event->datalen;
rbuf.recv_utime = lr->next_clock_time;
rbuf.lcm = lr->lcm;
if(lcm_try_enqueue_message(lr->lcm, lr->event->channel))
lcm_dispatch_handlers (lr->lcm, &rbuf, lr->event->channel);
int64_t prev_log_time = lr->event->timestamp;
if (load_next_event (lr) < 0) {
/* end-of-file reached. This call succeeds, but next call to
* _handle will fail */
lr->event = NULL;
if(lcm_internal_pipe_write(lr->notify_pipe[1], "+", 1) < 0) {
perror(__FILE__ " - write(notify)");
}
return 0;
}
/* Compute the wall time for the next event */
if (lr->speed > 0)
lr->next_clock_time +=
(lr->event->timestamp - prev_log_time) / lr->speed;
else
lr->next_clock_time = now;
if (lr->next_clock_time > now) {
int wstatus = lcm_internal_pipe_write(lr->timer_pipe[1], &lr->next_clock_time, 8);
if(wstatus < 0) {
perror(__FILE__ " - write(timer_pipe)");
}
} else {
int wstatus = lcm_internal_pipe_write(lr->notify_pipe[1], "+", 1);
if(wstatus < 0) {
perror(__FILE__ " - write(notify_pipe)");
}
}
return 0;
}
static void
message_handler (const lcm_recv_buf_t *rbuf, const char *channel, void *u)
{
logger_t *logger = (logger_t*) u;
if(logger->invert_channels) {
#ifdef USE_GREGEX
if(g_regex_match(logger->regex, channel, (GRegexMatchFlags) 0, NULL))
return;
#else
if(0 == regexec(&logger->preg, channel, 0, NULL, 0))
return;
#endif
}
int channellen = strlen(channel);
// check if the backlog of unwritten messages is too big. If so, then
// ignore this event
int64_t mem_sz = sizeof(lcm_eventlog_event_t) + channellen + 1 + rbuf->data_size;
g_mutex_lock(logger->mutex);
int64_t mem_required = mem_sz + logger->write_queue_size;
if(mem_required > logger->max_write_queue_size) {
// can't write to logfile fast enough. drop packet.
g_mutex_unlock(logger->mutex);
// maybe print an informational message to stdout
int64_t now = timestamp_now();
logger->dropped_packets_count ++;
int rc = logger->dropped_packets_count - logger->last_drop_report_count;
if(now - logger->last_drop_report_utime > 1000000 && rc > 0) {
printf("Can't write to log fast enough. Dropped %d packet%s\n",
rc, rc==1?"":"s");
logger->last_drop_report_utime = now;
logger->last_drop_report_count = logger->dropped_packets_count;
}
return;
} else {
logger->write_queue_size = mem_required;
g_mutex_unlock(logger->mutex);
}
// queue up the message for writing to disk by the write thread
lcm_eventlog_event_t *le = (lcm_eventlog_event_t*) malloc(mem_sz);
memset(le, 0, mem_sz);
le->timestamp = rbuf->recv_utime;
le->channellen = channellen;
le->datalen = rbuf->data_size;
// log_write_event will handle le.eventnum.
le->channel = ((char*)le) + sizeof(lcm_eventlog_event_t);
strcpy(le->channel, channel);
le->data = le->channel + channellen + 1;
assert((char*)le->data + rbuf->data_size == (char*)le + mem_sz);
memcpy(le->data, rbuf->data, rbuf->data_size);
g_async_queue_push(logger->write_queue, le);
}
//等待其它节点连接请求
void overlay::start()
{
//先停止
stop();
//启动URDP监听器，监听连接请求
OVERLAY_DBG(
std::cout << topology_id_ << "***********overlay start********************:" << (timestamp_t)timestamp_now() << std::endl;
std::cout << "udp_local_endpoint_:" << udp_local_endpoint_ << std::endl;
std::cout << "tcp_local_endpoint_:" << tcp_local_endpoint_ << std::endl;
);
