/* [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);
}
}
