diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 95adb595b6..e84c804714 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -164,6 +164,10 @@ void AP_Camera::trigger_pic() case AP_CAMERA_TRIGGER_TYPE_UAVCAN: uavcan_pic(); break; + case AP_CAMERA_TRIGGER_TYPE_UAVCAN_ATT: + uavcan_pic_att(); + break; + } log_picture(); @@ -285,6 +289,18 @@ void AP_Camera::control(float session, float zoom_pos, float zoom_step, float fo GCS_MAVLINK::send_to_components(MAVLINK_MSG_ID_COMMAND_LONG, (char*)&mav_cmd_long, sizeof(mav_cmd_long)); } +void AP_Camera::control_att(float pitch,float yaw,float shooting_cmd ) +{ + // take picture + + _camera_pitch = pitch; + _camera_pitch = yaw; + if (is_equal(shooting_cmd,1.0f)) { + trigger_pic(); + } + gcs().send_text(MAV_SEVERITY_INFO, "camera p:%.2f,y:%.2f,trg:%d",_camera_pitch,_camera_yaw,shooting_cmd); +} + /* Send camera feedback to the GCS */ @@ -497,6 +513,25 @@ void AP_Camera::uavcan_pic() #endif } +void AP_Camera::uavcan_pic_att() +{ + + gcs().send_text(MAV_SEVERITY_INFO, "camera p:%.2f,y:%.2f",_camera_pitch,_camera_yaw); + //TODO: 增加发送云台角度控制 +#if HAL_WITH_UAVCAN + uint8_t mode = 12; + uint8_t can_num_drivers = AP::can().get_num_drivers(); + for (uint8_t i = 0; i < can_num_drivers; i++) + { + AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i); + if (uavcan != nullptr) + { + uavcan->set_zr_cam_cmd(camera_state.id, mode, 1, _image_index&0xff, _image_index>>8, 0); + } + } + _trigger_counter = constrain_int16(_trigger_duration * 5, 0, 255); +#endif +} void AP_Camera::send_uavcan_camera_zr_control(const mavlink_camera_zr_control_t &packet){ #if HAL_WITH_UAVCAN diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 08ae26fcf0..dcec943a18 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -5,10 +5,6 @@ #include #include -#define AP_CAMERA_TRIGGER_TYPE_SERVO 0 -#define AP_CAMERA_TRIGGER_TYPE_RELAY 1 -#define AP_CAMERA_TRIGGER_TYPE_UAVCAN 2 - #define AP_CAMERA_TRIGGER_DEFAULT_TRIGGER_TYPE AP_CAMERA_TRIGGER_TYPE_RELAY // default is to use servo to trigger camera #define AP_CAMERA_TRIGGER_DEFAULT_DURATION 3 // default duration servo or relay is held open in 10ths of a second (i.e. 10 = 1 second) @@ -58,6 +54,8 @@ public: void take_picture(); void uavcan_pic(); + void uavcan_pic_att(); + void control_att(float pitch,float yaw,float shooting_cmd ); uint8_t send_msg_step; // Update - to be called periodically @at least 10Hz void update(); @@ -94,6 +92,14 @@ public: CAMERA_TYPE_BMMCC }; + enum trigger_types { + AP_CAMERA_TRIGGER_TYPE_SERVO, + AP_CAMERA_TRIGGER_TYPE_RELAY, + AP_CAMERA_TRIGGER_TYPE_UAVCAN, + AP_CAMERA_TRIGGER_TYPE_UAVCAN_ATT + }; + + void get_last_trigger_pos(int32_t &lat,int32_t &lng){ lat=last_pos_lat;lng=last_pos_lng; } // @Brown,for get camera pos // modivy by @Brown @@ -205,6 +211,8 @@ private: uint8_t hv_minor; uint8_t hv_patch; mavlink_zr_cam_sum_t zr_cam_sum; + float _camera_pitch; // 航线传入相机姿态pitch + float _camera_yaw; // 航线传入相机姿态yaw // void send_can_msg(void); }; namespace AP { diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index ae180ce345..d90edeb569 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -289,6 +289,7 @@ bool AP_Mission::verify_command(const Mission_Command& cmd) case MAV_CMD_DO_DIGICAM_CONTROL: case MAV_CMD_DO_SET_CAM_TRIGG_DIST: case MAV_CMD_DO_PARACHUTE: + case MAV_CMD_DO_GIMBAL_CONTROL: return true; default: return _cmd_verify_fn(cmd); @@ -310,6 +311,7 @@ bool AP_Mission::start_command(const Mission_Command& cmd) case MAV_CMD_DO_DIGICAM_CONFIGURE: case MAV_CMD_DO_DIGICAM_CONTROL: case MAV_CMD_DO_SET_CAM_TRIGG_DIST: + case MAV_CMD_DO_GIMBAL_CONTROL: return start_command_camera(cmd); case MAV_CMD_DO_PARACHUTE: return start_command_parachute(cmd); @@ -927,6 +929,13 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ cmd.content.mount_control.yaw = packet.param3; break; + case MAV_CMD_DO_GIMBAL_CONTROL: // MAV ID: 225 + cmd.content.gimbal_control.pitch = packet.param1; + cmd.content.gimbal_control.roll = packet.param2; + cmd.content.gimbal_control.yaw = packet.param3; + cmd.content.gimbal_control.shooting_cmd = packet.param4; + break; + case MAV_CMD_DO_SET_CAM_TRIGG_DIST: // MAV ID: 206 cmd.content.cam_trigg_dist.meters = packet.param1; // distance between camera shots in meters break; @@ -1363,6 +1372,13 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c packet.param3 = cmd.content.mount_control.yaw; break; + case MAV_CMD_DO_GIMBAL_CONTROL: // MAV ID: 225 + packet.param1 = cmd.content.gimbal_control.pitch; + packet.param2 = cmd.content.gimbal_control.roll; + packet.param3 = cmd.content.gimbal_control.yaw; + packet.param4 = cmd.content.gimbal_control.shooting_cmd; + break; + case MAV_CMD_DO_SET_CAM_TRIGG_DIST: // MAV ID: 206 packet.param1 = cmd.content.cam_trigg_dist.meters; // distance between camera shots in meters break; @@ -1936,6 +1952,8 @@ const char *AP_Mission::Mission_Command::type() const { return "DigiCamCfg"; case MAV_CMD_DO_DIGICAM_CONTROL: return "DigiCamCtrl"; + case MAV_CMD_DO_GIMBAL_CONTROL: + return "GimbalCamCtrl"; case MAV_CMD_DO_SET_CAM_TRIGG_DIST: return "SetCamTrigDst"; case MAV_CMD_DO_SET_ROI: diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index 378b3cba55..241e8bcdee 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -108,7 +108,12 @@ public: float roll; // roll angle in degrees float yaw; // yaw angle (relative to vehicle heading) in degrees }; - + struct PACKED Gimbal_Control { + float pitch; // pitch angle in degrees + float roll; // roll angle in degrees + float yaw; // yaw angle (relative to vehicle heading) in degrees + uint8_t shooting_cmd; // + }; // digicam control command structure struct PACKED Digicam_Configure { uint8_t shooting_mode; // ProgramAuto = 1, AV = 2, TV = 3, Man=4, IntelligentAuto=5, SuperiorAuto=6 @@ -255,6 +260,7 @@ public: // do-winch Winch_Command winch; + Gimbal_Control gimbal_control; // location Location location{}; // Waypoint location }; diff --git a/libraries/AP_Mission/AP_Mission_Commands.cpp b/libraries/AP_Mission/AP_Mission_Commands.cpp index 0564869911..52de9b3d70 100644 --- a/libraries/AP_Mission/AP_Mission_Commands.cpp +++ b/libraries/AP_Mission/AP_Mission_Commands.cpp @@ -100,6 +100,12 @@ bool AP_Mission::start_command_camera(const AP_Mission::Mission_Command& cmd) cmd.content.digicam_control.cmd_id); return true; + case MAV_CMD_DO_GIMBAL_CONTROL: // Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| + camera->control_att(cmd.content.gimbal_control.pitch, + cmd.content.gimbal_control.yaw, + cmd.content.gimbal_control.shooting_cmd); + return true; + case MAV_CMD_DO_SET_CAM_TRIGG_DIST: camera->set_trigger_distance(cmd.content.cam_trigg_dist.meters); return true; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index cf80c21df1..0b3cd50ebf 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -2777,6 +2777,14 @@ MAV_RESULT GCS_MAVLINK::handle_command_camera(const mavlink_command_long_t &pack camera->set_trigger_distance(packet.param1); result = MAV_RESULT_ACCEPTED; break; + + case MAV_CMD_DO_GIMBAL_CONTROL: + camera->control_att(packet.param1, + packet.param3, + packet.param4); + camera->cameras_take_enable = 0; + result = MAV_RESULT_ACCEPTED; + break; default: result = MAV_RESULT_UNSUPPORTED; break; @@ -3721,6 +3729,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t case MAV_CMD_DO_DIGICAM_CONFIGURE: case MAV_CMD_DO_DIGICAM_CONTROL: case MAV_CMD_DO_SET_CAM_TRIGG_DIST: + case MAV_CMD_DO_GIMBAL_CONTROL: result = handle_command_camera(packet); break;