Plane: camera is responsible for taking distance-based-images and logging

This commit is contained in:
Peter Barker 2017-07-26 13:48:21 +10:00 committed by Francisco Ferreira
parent cdf9ebcb39
commit 2fb46a67dd
4 changed files with 17 additions and 58 deletions

View File

@ -223,13 +223,7 @@ void Plane::update_mount(void)
void Plane::update_trigger(void) void Plane::update_trigger(void)
{ {
#if CAMERA == ENABLED #if CAMERA == ENABLED
camera.trigger_pic_cleanup(); camera.update_trigger();
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);
}
}
#endif #endif
} }
@ -514,10 +508,8 @@ void Plane::update_GPS_10Hz(void)
geofence_check(false); geofence_check(false);
#if CAMERA == ENABLED #if CAMERA == ENABLED
if (camera.update_location(current_loc, plane.ahrs ) == true) { camera.update();
do_take_picture(); #endif
}
#endif
// update wind estimate // update wind estimate
ahrs.estimate_wind(); ahrs.estimate_wind();

View File

@ -601,7 +601,7 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
case MSG_CAMERA_FEEDBACK: case MSG_CAMERA_FEEDBACK:
#if CAMERA == ENABLED #if CAMERA == ENABLED
CHECK_PAYLOAD_SIZE(CAMERA_FEEDBACK); CHECK_PAYLOAD_SIZE(CAMERA_FEEDBACK);
plane.camera.send_feedback(chan, plane.gps, plane.ahrs, plane.current_loc); plane.camera.send_feedback(chan);
#endif #endif
break; break;
@ -1132,14 +1132,12 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
break; break;
case MAV_CMD_DO_DIGICAM_CONTROL: case MAV_CMD_DO_DIGICAM_CONTROL:
if (plane.camera.control(packet.param1, plane.camera.control(packet.param1,
packet.param2, packet.param2,
packet.param3, packet.param3,
packet.param4, packet.param4,
packet.param5, packet.param5,
packet.param6)) { packet.param6);
plane.log_picture();
}
result = MAV_RESULT_ACCEPTED; result = MAV_RESULT_ACCEPTED;
break; break;
@ -1678,7 +1676,6 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_DIGICAM_CONTROL: case MAVLINK_MSG_ID_DIGICAM_CONTROL:
{ {
plane.camera.control_msg(msg); plane.camera.control_msg(msg);
plane.log_picture();
break; break;
} }
#endif // CAMERA == ENABLED #endif // CAMERA == ENABLED

View File

@ -273,7 +273,7 @@ private:
// Camera // Camera
#if CAMERA == ENABLED #if CAMERA == ENABLED
AP_Camera camera {&relay}; AP_Camera camera{&relay, MASK_LOG_CAMERA, current_loc, gps, ahrs};
#endif #endif
#if OPTFLOW == ENABLED #if OPTFLOW == ENABLED
@ -891,9 +891,7 @@ private:
bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd); bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd);
bool verify_vtol_land(const AP_Mission::Mission_Command &cmd); bool verify_vtol_land(const AP_Mission::Mission_Command &cmd);
void do_loiter_at_location(); void do_loiter_at_location();
void do_take_picture();
bool verify_loiter_heading(bool init); bool verify_loiter_heading(bool init);
void log_picture();
void exit_mission_callback(); void exit_mission_callback();
void mavlink_delay(uint32_t ms); void mavlink_delay(uint32_t ms);
void read_control_switch(); void read_control_switch();

View File

@ -927,23 +927,12 @@ void Plane::do_digicam_configure(const AP_Mission::Mission_Command& cmd)
void Plane::do_digicam_control(const AP_Mission::Mission_Command& cmd) void Plane::do_digicam_control(const AP_Mission::Mission_Command& cmd)
{ {
#if CAMERA == ENABLED #if CAMERA == ENABLED
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_pos,
cmd.content.digicam_control.zoom_step, cmd.content.digicam_control.zoom_step,
cmd.content.digicam_control.focus_lock, cmd.content.digicam_control.focus_lock,
cmd.content.digicam_control.shooting_cmd, cmd.content.digicam_control.shooting_cmd,
cmd.content.digicam_control.cmd_id)) { cmd.content.digicam_control.cmd_id);
log_picture();
}
#endif
}
// do_take_picture - take a picture with the camera library
void Plane::do_take_picture()
{
#if CAMERA == ENABLED
camera.trigger_pic(true);
log_picture();
#endif #endif
} }
@ -968,23 +957,6 @@ void Plane::do_parachute(const AP_Mission::Mission_Command& cmd)
} }
#endif #endif
// log_picture - log picture taken and send feedback to GCS
void Plane::log_picture()
{
#if CAMERA == ENABLED
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
}
// start_command_callback - callback function called from ap-mission when it begins a new mission command // start_command_callback - callback function called from ap-mission when it begins a new mission command
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode // we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
bool Plane::start_command_callback(const AP_Mission::Mission_Command &cmd) bool Plane::start_command_callback(const AP_Mission::Mission_Command &cmd)