diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 8a5488e510..5354a27bf4 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -338,17 +338,27 @@ void AP_Camera::send_feedback(mavlink_channel_t chan) altitude_rel = current_loc.alt - ahrs.get_home().alt; } + ant_ned = camera_pos_rotation(ant_offset); mavlink_msg_camera_feedback_send( chan, AP::gps().time_epoch_usec(), 0, 0, _image_index, current_loc.lat, current_loc.lng, altitude*1e-2f, altitude_rel*1e-2f, - ahrs.roll_sensor*1e-2f, ahrs.pitch_sensor*1e-2f, ahrs.yaw_sensor*1e-2f, + ant_ned.x, ant_ned.y, ant_ned.z, 0.0f, CAMERA_FEEDBACK_PHOTO, _camera_trigger_logged); } +Vector3f AP_Camera::camera_pos_rotation(Vector3f offset) +{ + Matrix3f Tbn; // dcm rotation matrix to rotate body frame to north + Vector3f cam_pos_ned; + const AP_AHRS &ahrs = AP::ahrs(); + Tbn = ahrs.get_rotation_body_to_ned(); + cam_pos_ned = Tbn * (offset); + return cam_pos_ned; +} /* update; triggers by distance moved */ void AP_Camera::update() @@ -390,6 +400,7 @@ void AP_Camera::update() take_picture(); _last_location = current_loc; + ant_ned = camera_pos_rotation(ant_offset); _last_photo_time = tnow; } @@ -402,6 +413,7 @@ void AP_Camera::feedback_pin_isr(uint8_t pin, bool high, uint32_t timestamp_us) if(timestamp_us - last_timestamp_us > _min_time_interval * 1000){ last_timestamp_us = timestamp_us; _feedback_timestamp_us = timestamp_us; + ant_ned = camera_pos_rotation(ant_offset); _camera_trigger_count++; } } @@ -416,6 +428,7 @@ void AP_Camera::feedback_pin_timer(void) if (pin_state == trigger_polarity && _last_pin_state != trigger_polarity) { _feedback_timestamp_us = AP_HAL::micros(); + ant_ned = camera_pos_rotation(ant_offset); _camera_trigger_count++; } _last_pin_state = pin_state; @@ -460,14 +473,14 @@ void AP_Camera::log_picture() if (!using_feedback_pin()) { gcs().send_message(MSG_CAMERA_FEEDBACK); if (logger->should_log(log_camera_bit)) { - logger->Write_Camera(current_loc); + logger->Write_Camera(current_loc,ant_ned); last_pos_lat = current_loc.lat; last_pos_lng = current_loc.lng; _image_index_log++; } } else { if (logger->should_log(log_camera_bit)) { - logger->Write_Trigger(current_loc); + logger->Write_Trigger(current_loc,ant_ned); last_pos_lat = current_loc.lat; last_pos_lng = current_loc.lng; _image_index_log++; @@ -507,7 +520,7 @@ void AP_Camera::update_trigger() if (logger->should_log(log_camera_bit)) { uint32_t tdiff = AP_HAL::micros() - timestamp32; uint64_t timestamp = AP_HAL::micros64(); - logger->Write_Camera(current_loc, timestamp - tdiff); + logger->Write_Camera(current_loc,ant_ned, timestamp - tdiff); } } } diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 8823818156..ededda0ace 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -138,6 +138,7 @@ public: float yaw; uint8_t mode; }; + Vector3f camera_pos_rotation(Vector3f offset); struct Camera_State &get_camera_state(){ return camera_state; } @@ -215,6 +216,8 @@ private: float _camera_yaw; // 航线传入相机姿态yaw // void send_can_msg(void); bool new_att; + Vector3f ant_offset = {0.02,0.225,0.17}; + Vector3f ant_ned; }; namespace AP { AP_Camera *camera(); diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 57802df083..58f80d40d8 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -257,9 +257,9 @@ public: void Write_Radio(const mavlink_radio_t &packet); void Write_Message(const char *message); void Write_MessageF(const char *fmt, ...); - void Write_CameraInfo(enum LogMessages msg, const Location ¤t_loc, uint64_t timestamp_us=0); - void Write_Camera(const Location ¤t_loc, uint64_t timestamp_us=0); - void Write_Trigger(const Location ¤t_loc); + void Write_CameraInfo(enum LogMessages msg, const Location ¤t_loc,Vector3f offset, uint64_t timestamp_us=0); + void Write_Camera(const Location ¤t_loc, Vector3f offset, uint64_t timestamp_us=0); + void Write_Trigger(const Location ¤t_loc, Vector3f offset); void Write_ESC(uint8_t id, uint64_t time_us, int32_t rpm, uint16_t voltage, uint16_t current, int16_t temperature, uint16_t current_tot); void Write_Attitude(const Vector3f &targets); void Write_AttitudeView(AP_AHRS_View &ahrs, const Vector3f &targets); diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index bd86a9aa41..76fc0df964 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -554,7 +554,7 @@ void AP_Logger::Write_Radio(const mavlink_radio_t &packet) } // Write a Camera packet -void AP_Logger::Write_CameraInfo(enum LogMessages msg, const Location ¤t_loc, uint64_t timestamp_us) +void AP_Logger::Write_CameraInfo(enum LogMessages msg, const Location ¤t_loc, Vector3f offset, uint64_t timestamp_us) { const AP_AHRS &ahrs = AP::ahrs(); @@ -583,23 +583,26 @@ void AP_Logger::Write_CameraInfo(enum LogMessages msg, const Location ¤t_l altitude : altitude, altitude_rel: altitude_rel, altitude_gps: altitude_gps, - roll : (int16_t)ahrs.roll_sensor, - pitch : (int16_t)ahrs.pitch_sensor, - yaw : (uint16_t)ahrs.yaw_sensor + // roll : (int16_t)ahrs.roll_sensor, + // pitch : (int16_t)ahrs.pitch_sensor, + // yaw : (uint16_t)ahrs.yaw_sensor + roll : (int16_t)(offset.x * 100), + pitch : (int16_t)(offset.y * 100), + yaw : (uint16_t)(offset.z * 100) }; WriteCriticalBlock(&pkt, sizeof(pkt)); } // Write a Camera packet -void AP_Logger::Write_Camera(const Location ¤t_loc, uint64_t timestamp_us) +void AP_Logger::Write_Camera(const Location ¤t_loc, Vector3f offset, uint64_t timestamp_us) { - Write_CameraInfo(LOG_CAMERA_MSG, current_loc, timestamp_us); + Write_CameraInfo(LOG_CAMERA_MSG, current_loc, offset, timestamp_us); } // Write a Trigger packet -void AP_Logger::Write_Trigger(const Location ¤t_loc) +void AP_Logger::Write_Trigger(const Location ¤t_loc, Vector3f offset) { - Write_CameraInfo(LOG_TRIGGER_MSG, current_loc, 0); + Write_CameraInfo(LOG_TRIGGER_MSG, current_loc, offset, 0); } // Write an attitude packet diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 261f9d9013..eb41608479 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -2062,14 +2062,14 @@ void AP_Mission::auto_resume_mission(uint8_t f_mode, bool is_arm,int32_t r_lat,i if(f_mode == 3 && need_change_cmd == 0) { need_change_cmd = 1; - // gcs_send_text_fmt(MAV_SEVERITY_DEBUG, "enter resume dec"); + gcs().send_text(MAV_SEVERITY_DEBUG, "enter resume dec"); }else if(need_change_cmd == 1 && _auto_resume && 6 == f_mode) { cmd_rtl_index = _nav_cmd.index; cmd_rtl_index -= 1; - // gcs_send_text_fmt(MAV_SEVERITY_DEBUG, "resume %d",cmd_rtl_index); + gcs().send_text(MAV_SEVERITY_DEBUG, "resume %d",cmd_rtl_index); if(cmd_rtl_index < 5 || num_commands() - cmd_rtl_index < 4)// 飛行的航點小於6則直接推出，否則讀取當前航點；飞完所有航点，也直接退出 { need_change_cmd = 0; @@ -2083,7 +2083,7 @@ void AP_Mission::auto_resume_mission(uint8_t f_mode, bool is_arm,int32_t r_lat,i { read_cmd_from_storage(i,cmd_rtl); // 读取原本航点 - // gcs_send_text_fmt(MAV_SEVERITY_DEBUG, "read[%d]: %d",i,cmd_rtl.id); + gcs().send_text(MAV_SEVERITY_DEBUG, "read[%d]: %d",i,cmd_rtl.id); if (is_nav_cmd(cmd_rtl)) { // 是否是Waypoint break; } @@ -2106,14 +2106,15 @@ void AP_Mission::auto_resume_mission(uint8_t f_mode, bool is_arm,int32_t r_lat,i // 飞机上锁，则开始改写航点 - if (!is_arm && need_change_cmd == 2) { + // if (!is_arm && need_change_cmd == 2) { + if (f_mode == 1 && need_change_cmd == 2) { // 测试用,模式ACRO重写航点 cmd_total = num_commands(); // gcs_send_text_fmt(MAV_SEVERITY_DEBUG, "[Resume] --- Star replace wp ,total:%d ---",cmd_total); for(cmd_i = 1; cmd_i < cmd_total; cmd_i++) { read_cmd_from_storage(cmd_i,cmd_temp); - // gcs_send_text_fmt(MAV_SEVERITY_DEBUG, "[%d] step:%d, id:%d ",cmd_i,cmd_step ,cmd_temp.id); + gcs().send_text(MAV_SEVERITY_DEBUG, "[%d] step:%d, id:%d ",cmd_i,cmd_step ,cmd_temp.id); switch (cmd_step) { // gcs_send_text_fmt(MAV_SEVERITY_INFO, "[d]t:%d,i:%d \n ",cmd_step,cmd_rtl_i); @@ -2144,7 +2145,7 @@ void AP_Mission::auto_resume_mission(uint8_t f_mode, bool is_arm,int32_t r_lat,i } break; case 2: - if(cmd_temp.id == MAV_CMD_DO_SET_CAM_TRIGG_DIST) + if(cmd_temp.id == MAV_CMD_DO_SET_CAM_TRIGG_DIST || cmd_temp.id == MAV_CMD_DO_GIMBAL_CONTROL) { // cmd_rtl_i = cmd_i; cmd_step = 3; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp index 421b4fd288..c3797b3221 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp @@ -186,7 +186,7 @@ void NavEKF2_core::calcGpsGoodToAlign(void) } // fail if satellite geometry is poor - bool hdopFail = (gps.get_hdop() > 250) && (frontend->_gpsCheck & MASK_GPS_HDOP); + bool hdopFail = (gps.get_hdop() > 800) && (frontend->_gpsCheck & MASK_GPS_HDOP); // Report check result as a text string and bitmask if (hdopFail) {