mirror of https://github.com/ArduPilot/ardupilot
Sub: camera is responsible for taking distance-based-images and logging
This commit is contained in:
parent
2fb46a67dd
commit
b299772a75
|
@ -238,13 +238,7 @@ void Sub::update_mount()
|
|||
// update camera trigger
|
||||
void Sub::update_trigger(void)
|
||||
{
|
||||
camera.trigger_pic_cleanup();
|
||||
if (camera.check_trigger_pin()) {
|
||||
gcs().send_message(MSG_CAMERA_FEEDBACK);
|
||||
if (should_log(MASK_LOG_CAMERA)) {
|
||||
DataFlash.Log_Write_Camera(ahrs, gps, current_loc);
|
||||
}
|
||||
}
|
||||
camera.update_trigger();
|
||||
}
|
||||
#endif
|
||||
|
||||
|
@ -408,17 +402,11 @@ void Sub::update_GPS(void)
|
|||
// set system time if necessary
|
||||
set_system_time_from_GPS();
|
||||
|
||||
// checks to initialise home and take location based pictures
|
||||
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
|
||||
|
||||
#if CAMERA == ENABLED
|
||||
if (camera.update_location(current_loc, sub.ahrs) == true) {
|
||||
do_take_picture();
|
||||
}
|
||||
camera.update();
|
||||
#endif
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Sub::read_AHRS(void)
|
||||
{
|
||||
|
|
|
@ -578,7 +578,7 @@ bool GCS_MAVLINK_Sub::try_send_message(enum ap_message id)
|
|||
case MSG_CAMERA_FEEDBACK:
|
||||
#if CAMERA == ENABLED
|
||||
CHECK_PAYLOAD_SIZE(CAMERA_FEEDBACK);
|
||||
sub.camera.send_feedback(chan, sub.gps, sub.ahrs, sub.current_loc);
|
||||
sub.camera.send_feedback(chan);
|
||||
#endif
|
||||
break;
|
||||
|
||||
|
@ -1147,14 +1147,12 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
|
|||
break;
|
||||
|
||||
case MAV_CMD_DO_DIGICAM_CONTROL:
|
||||
if (sub.camera.control(packet.param1,
|
||||
sub.camera.control(packet.param1,
|
||||
packet.param2,
|
||||
packet.param3,
|
||||
packet.param4,
|
||||
packet.param5,
|
||||
packet.param6)) {
|
||||
sub.log_picture();
|
||||
}
|
||||
packet.param6);
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
break;
|
||||
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
|
||||
|
@ -1566,7 +1564,6 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
|
|||
//deprecated. Use MAV_CMD_DO_DIGICAM_CONTROL
|
||||
case MAVLINK_MSG_ID_DIGICAM_CONTROL:
|
||||
sub.camera.control_msg(msg);
|
||||
sub.log_picture();
|
||||
break;
|
||||
#endif // CAMERA == ENABLED
|
||||
|
||||
|
|
|
@ -62,7 +62,7 @@ Sub::Sub(void) :
|
|||
mainLoop_count(0),
|
||||
ServoRelayEvents(relay),
|
||||
#if CAMERA == ENABLED
|
||||
camera(&relay),
|
||||
camera(&relay, MASK_LOG_CAMERA, current_loc, gps, ahrs),
|
||||
#endif
|
||||
#if MOUNT == ENABLED
|
||||
camera_mount(ahrs, current_loc),
|
||||
|
|
|
@ -689,8 +689,6 @@ private:
|
|||
#if CAMERA == ENABLED
|
||||
void do_digicam_configure(const AP_Mission::Mission_Command& cmd);
|
||||
void do_digicam_control(const AP_Mission::Mission_Command& cmd);
|
||||
void do_take_picture();
|
||||
void log_picture();
|
||||
void update_trigger();
|
||||
#endif
|
||||
|
||||
|
|
|
@ -873,37 +873,14 @@ void Sub::do_digicam_configure(const AP_Mission::Mission_Command& cmd)
|
|||
// do_digicam_control Send Digicam Control message with the camera library
|
||||
void Sub::do_digicam_control(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
if (camera.control(cmd.content.digicam_control.session,
|
||||
camera.control(cmd.content.digicam_control.session,
|
||||
cmd.content.digicam_control.zoom_pos,
|
||||
cmd.content.digicam_control.zoom_step,
|
||||
cmd.content.digicam_control.focus_lock,
|
||||
cmd.content.digicam_control.shooting_cmd,
|
||||
cmd.content.digicam_control.cmd_id)) {
|
||||
log_picture();
|
||||
}
|
||||
cmd.content.digicam_control.cmd_id);
|
||||
}
|
||||
|
||||
// do_take_picture - take a picture with the camera library
|
||||
void Sub::do_take_picture()
|
||||
{
|
||||
camera.trigger_pic(true);
|
||||
log_picture();
|
||||
}
|
||||
|
||||
// log_picture - log picture taken and send feedback to GCS
|
||||
void Sub::log_picture()
|
||||
{
|
||||
if (!camera.using_feedback_pin()) {
|
||||
gcs().send_message(MSG_CAMERA_FEEDBACK);
|
||||
if (should_log(MASK_LOG_CAMERA)) {
|
||||
DataFlash.Log_Write_Camera(ahrs, gps, current_loc);
|
||||
}
|
||||
} else {
|
||||
if (should_log(MASK_LOG_CAMERA)) {
|
||||
DataFlash.Log_Write_Trigger(ahrs, gps, current_loc);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
// point the camera to a specified angle
|
||||
|
|
Loading…
Reference in New Issue