|
|
|
@ -602,11 +602,15 @@ void AP_Camera::send_camera_zr_status(mavlink_channel_t chan)
@@ -602,11 +602,15 @@ void AP_Camera::send_camera_zr_status(mavlink_channel_t chan)
|
|
|
|
|
delt_pic_num += abs(num - camera_state.num[i]); // 判断各个相机照片数是否相等
|
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
if(delt_pic_num == 0){
|
|
|
|
|
delt_cnt = 0; |
|
|
|
|
last_delt = 0; |
|
|
|
|
} |
|
|
|
|
if(last_delt != delt_pic_num){ |
|
|
|
|
delt_cnt+=1; |
|
|
|
|
if(delt_cnt > 5){ // Mavlink一次可能会发两三个chan,取5确保是两次计算差值满足条件
|
|
|
|
|
// gcs().send_text(MAV_SEVERITY_INFO, "照片数差异(%d):%d,%d,%d,%d,%d",delt_pic_num,camera_state.num[0],camera_state.num[1],camera_state.num[2],camera_state.num[3],camera_state.num[4]);
|
|
|
|
|
if(delt_pic_num > 3){ |
|
|
|
|
if(delt_pic_num > 5){ |
|
|
|
|
gcs().send_text(MAV_SEVERITY_CRITICAL, "%d:照片数差异大,总差值:%d",camera_state.num[0],delt_pic_num); |
|
|
|
|
feedback_gimbal = 20; |
|
|
|
|
}else |
|
|
|
@ -616,7 +620,6 @@ void AP_Camera::send_camera_zr_status(mavlink_channel_t chan)
@@ -616,7 +620,6 @@ void AP_Camera::send_camera_zr_status(mavlink_channel_t chan)
|
|
|
|
|
last_delt = delt_pic_num; |
|
|
|
|
delt_cnt = 0; |
|
|
|
|
} |
|
|
|
|
}else{
|
|
|
|
|
} |
|
|
|
|
mavlink_msg_camera_zr_status_send( |
|
|
|
|
chan, |
|
|
|
|