/* [timestamp] */ static void *store_to_log_thread_func(void * arg) { int stat = 0; api_pos_data_t g_origin_pos; //原点在羽毛球场起飞点 g_origin_pos.longti = ORIGIN_IN_HENGSHENG_LONGTI; g_origin_pos.lati = ORIGIN_IN_HENGSHENG_LATI; g_origin_pos.alti = ORIGIN_IN_HENGSHENG_ALTI; XYZ g_origin_XYZ, cur_XYZ; api_quaternion_data_t cur_quaternion; struct timeval tv; struct tm *tmlocal; double dji_time=0; char _wbuf[500] = {0}; Queue* pq=create(4); if(log_fd == -1) { printf("Bad log fd.\n"); pthread_exit(NULL); } char header[300]; sprintf(header, "linux time;dji time;" "body_roll;body_pitch;body_yaw;" "agx;agy;agz;" "vgx;vgy;vgz;" "wx;wy;wz;" "mx;my;mz;" "longti;lati;alti;height;health;" "cur_xyz;" "no_gps_z;" "pitch,roll,yaw,thr;" "ultra,ultra_calc;" "of_x;of_y;of_z;" "of_ad_x;of_ad_y;of_ad_z;\n"); //printf("%s\n", header); int ret = write(log_fd, header, strlen((const char *) header)); if(ret == -1) { printf("Header bad write.\n"); } while(1) { if(if_log_on() ) { DJI_Pro_Get_Quaternion(&cur_quaternion); QUA2ANGLE(cur_quaternion,&body_angle) ; DJI_Pro_Get_GroundAcc(&cur_acc); DJI_Pro_Get_GroundVo(&cur_vo); DJI_Pro_Get_Broadcast_Data(&cur_broadcast_data); stat |= 0x01; DJI_Pro_Get_Pos(&_log_pos); geo2XYZ(_log_pos, &cur_XYZ); XYZ2xyz(g_origin_pos, cur_XYZ, &cur_xyz); stat |= (0x02 | 0x04); _log_no_gps_z = get_no_gps_z_data(); stat |= 0x08; if( XY_Get_Ctrl_Data(&_log_user_ctrl_data) == 0 ) { stat |= 0x10; } if(XY_Get_Ultra_Data(&_log_ultra_data, ULTRA_GET_ID_A) == 0) { if(!ultra_queue_full(pq)) { push_queue(pq,_log_ultra_data); //printf("_log_ultra_data is %.4f\n",_log_ultra_data); } if(ultra_queue_full(pq)) { ultra_calc(pq); Get_calced_Ultra(pq,&_calc_ultra_data); queue_pop(pq); //printf("_calc_ultra_data is %.4f\n",_calc_ultra_data); } stat |= 0x20; } /* sd code should use OFFSET_GET_ID_B */ if( XY_Get_Offset_Data(&_log_offset, OFFSET_GET_ID_B) == 0) { stat |= 0x40; get_log_offset_adjust(&_log_offset_adjust); stat |= 0x80; } if( gettimeofday(&tv, NULL) != 0) { printf("ERROR get timestamp for log.\n"); } tmlocal = localtime(&tv.tv_sec); dji_time=((float)cur_broadcast_data.time_stamp/600); sprintf(_wbuf, "%d-%d %d:%d:%d:%6ld;%6lf;", 1 + tmlocal->tm_mon, tmlocal->tm_mday, tmlocal->tm_hour, tmlocal->tm_min, tmlocal->tm_sec, tv.tv_usec, dji_time); int ret = write(log_fd, _wbuf, strlen((const char *)_wbuf)); memset(_wbuf, 0, strlen((const char *)_wbuf)); if(ret == -1) { perror("log write"); } check_stat_store(stat, 0x01, _wbuf); check_stat_store(stat, 0x02, _wbuf); check_stat_store(stat, 0x04, _wbuf); check_stat_store(stat, 0x08, _wbuf); check_stat_store(stat, 0x10, _wbuf); check_stat_store(stat, 0x20, _wbuf); check_stat_store(stat, 0x40, _wbuf); check_stat_store(stat, 0x80, _wbuf); add_crlf(); stat = 0; usleep(100000); //100ms } else { printf("log fd will be closed.\n"); close(log_fd); break; //wait_log_on(); } } pthread_exit(NULL); }
//---------------------------------------------------------- // timer spin_function 50Hz //---------------------------------------------------------- void DJISDKNode::broadcast_callback() { sdk_std_msg_t recv_sdk_std_msgs; unsigned short msg_flags; DJI_Pro_Get_Broadcast_Data(&recv_sdk_std_msgs, &msg_flags); static int frame_id = 0; frame_id ++; auto current_time = ros::Time::now(); //update attitude msg if ((msg_flags & ENABLE_MSG_Q) && (msg_flags & ENABLE_MSG_W)) { attitude_quaternion.header.frame_id = "/world"; attitude_quaternion.header.stamp = current_time; attitude_quaternion.q0 = recv_sdk_std_msgs.q.q0; attitude_quaternion.q1 = recv_sdk_std_msgs.q.q1; attitude_quaternion.q2 = recv_sdk_std_msgs.q.q2; attitude_quaternion.q3 = recv_sdk_std_msgs.q.q3; attitude_quaternion.wx = recv_sdk_std_msgs.w.x; attitude_quaternion.wy = recv_sdk_std_msgs.w.y; attitude_quaternion.wz = recv_sdk_std_msgs.w.z; attitude_quaternion.ts = recv_sdk_std_msgs.time_stamp; attitude_quaternion_publisher.publish(attitude_quaternion); } //update global_position msg if ((msg_flags & ENABLE_MSG_POS)) { global_position.header.frame_id = "/world"; global_position.header.stamp = current_time; global_position.ts = recv_sdk_std_msgs.time_stamp; global_position.latitude = recv_sdk_std_msgs.pos.lati * 180.0 / C_PI; global_position.longitude = recv_sdk_std_msgs.pos.longti * 180.0 / C_PI; global_position.height = recv_sdk_std_msgs.pos.height; global_position.altitude = recv_sdk_std_msgs.pos.alti; global_position.health = recv_sdk_std_msgs.pos.health_flag; global_position_publisher.publish(global_position); //TODO: // FIX BUG about flying at lat = 0 if (global_position.ts != 0 && global_position_ref_seted == 0 && global_position.latitude != 0) { global_position_ref = global_position; global_position_ref_seted = 1; } //update local_position msg local_position.header.frame_id = "/world"; local_position.header.stamp = current_time; gps_convert_ned( local_position.x, local_position.y, global_position.longitude, global_position.latitude, global_position_ref.longitude, global_position_ref.latitude ); local_position.z = global_position.height; local_position.ts = global_position.ts; local_position_ref = local_position; local_position_publisher.publish(local_position); } //update velocity msg if ((msg_flags & ENABLE_MSG_V)) { velocity.header.frame_id = "/world"; velocity.header.stamp = current_time; velocity.ts = recv_sdk_std_msgs.time_stamp; velocity.vx = recv_sdk_std_msgs.v.x; velocity.vy = recv_sdk_std_msgs.v.y; velocity.vz = recv_sdk_std_msgs.v.z; velocity_publisher.publish(velocity); } //update acceleration msg if ((msg_flags & ENABLE_MSG_A)) { acceleration.header.frame_id = "/world"; acceleration.header.stamp = current_time; acceleration.ts = recv_sdk_std_msgs.time_stamp; acceleration.ax = recv_sdk_std_msgs.a.x; acceleration.ay = recv_sdk_std_msgs.a.y; acceleration.az = recv_sdk_std_msgs.a.z; acceleration_publisher.publish(acceleration); } //update gimbal msg if ((msg_flags & ENABLE_MSG_GIMBAL)) { gimbal.header.frame_id = "/gimbal"; gimbal.header.stamp= current_time; gimbal.ts = recv_sdk_std_msgs.time_stamp; gimbal.roll = recv_sdk_std_msgs.gimbal.x; gimbal.pitch = recv_sdk_std_msgs.gimbal.y; gimbal.yaw = recv_sdk_std_msgs.gimbal.z; gimbal_publisher.publish(gimbal); } //update odom msg if ((msg_flags & ENABLE_MSG_POS) && (msg_flags & ENABLE_MSG_Q) && (msg_flags & ENABLE_MSG_W) && (msg_flags & ENABLE_MSG_V)) { odometry.header.frame_id = "/world"; odometry.header.stamp = current_time; odometry.pose.pose.position.x = local_position.x; odometry.pose.pose.position.y = local_position.y; odometry.pose.pose.position.z = local_position.z; odometry.pose.pose.orientation.w = attitude_quaternion.q0; odometry.pose.pose.orientation.x = attitude_quaternion.q1; odometry.pose.pose.orientation.y = attitude_quaternion.q2; odometry.pose.pose.orientation.z = attitude_quaternion.q3; odometry.twist.twist.angular.x = attitude_quaternion.wx; odometry.twist.twist.angular.y = attitude_quaternion.wy; odometry.twist.twist.angular.z = attitude_quaternion.wz; odometry.twist.twist.linear.x = velocity.vx; odometry.twist.twist.linear.y = velocity.vy; odometry.twist.twist.linear.z = velocity.vz; odometry_publisher.publish(odometry); } //update rc_channel msg if ((msg_flags & ENABLE_MSG_RC)) { rc_channels.header.frame_id = "/rc"; rc_channels.header.stamp = current_time; rc_channels.ts = recv_sdk_std_msgs.time_stamp; rc_channels.pitch = recv_sdk_std_msgs.rc.pitch; rc_channels.roll = recv_sdk_std_msgs.rc.roll; rc_channels.mode = recv_sdk_std_msgs.rc.mode; rc_channels.gear = recv_sdk_std_msgs.rc.gear; rc_channels.throttle = recv_sdk_std_msgs.rc.throttle; rc_channels.yaw = recv_sdk_std_msgs.rc.yaw; rc_channels_publisher.publish(rc_channels); } //update compass msg if ((msg_flags & ENABLE_MSG_MAG)) { compass.header.frame_id = "/world"; compass.header.stamp = current_time; compass.ts = recv_sdk_std_msgs.time_stamp; compass.x = recv_sdk_std_msgs.mag.x; compass.y = recv_sdk_std_msgs.mag.y; compass.z = recv_sdk_std_msgs.mag.z; compass_publisher.publish(compass); } //update flight_status if ((msg_flags & ENABLE_MSG_STATUS)) { std_msgs::UInt8 msg; flight_status = recv_sdk_std_msgs.status; msg.data = flight_status; flight_status_publisher.publish(msg); } //update battery msg if ((msg_flags & ENABLE_MSG_BATTERY)) { power_status.percentage = recv_sdk_std_msgs.battery_remaining_capacity; power_status_publisher.publish(power_status); } //update flight control info if ((msg_flags & ENABLE_MSG_DEVICE)) { flight_control_info.cur_ctrl_dev_in_navi_mode = recv_sdk_std_msgs.ctrl_info.cur_ctrl_dev_in_navi_mode; flight_control_info.serial_req_status = recv_sdk_std_msgs.ctrl_info.serial_req_status; flight_control_info_publisher.publish(flight_control_info); } //update obtaincontrol msg if ((msg_flags & ENABLE_MSG_TIME)) { std_msgs::UInt8 msg; sdk_permission_opened = recv_sdk_std_msgs.obtained_control; msg.data = recv_sdk_std_msgs.obtained_control; sdk_permission_publisher.publish(msg); //update activation msg activated = recv_sdk_std_msgs.activation; msg.data = recv_sdk_std_msgs.activation; activation_publisher.publish(msg); } }