Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: Sub changes to match newest Copter changes

This commit is contained in:
Rustom Jehangir 2016-02-03 14:56:39 -08:00 committed by Andrew Tridgell
parent c31ccd242f
commit c15d7299fa
5 changed files with 54 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
ArduSub/ArduSub.cpp
View File

@ -127,6 +127,7 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = {
SCHED_TASK(gcs_send_deferred, 50, 550),
SCHED_TASK(gcs_data_stream_send, 50, 550),
SCHED_TASK(update_mount, 50, 75),
SCHED_TASK(update_trigger, 50, 75),
SCHED_TASK(ten_hz_logging_loop, 10, 350),
SCHED_TASK(fifty_hz_logging_loop, 50, 110),
SCHED_TASK(full_rate_logging_loop,400, 100),
@ -343,9 +344,20 @@ void Sub::update_mount()
// update camera mount's position
camera_mount.update();
#endif
}
#if CAMERA == ENABLED
camera.trigger_pic_cleanup();
// update camera trigger
void Sub::update_trigger(void)
{
#if CAMERA == ENABLED
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);
}
}
#endif
}

7
ArduSub/GCS_Mavlink.cpp
View File

@ -1301,13 +1301,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_DO_DIGICAM_CONTROL:
sub.camera.control(packet.param1,
if (sub.camera.control(packet.param1,
packet.param2,
packet.param3,
packet.param4,
packet.param5,
packet.param6);
packet.param6)) {
sub.log_picture();
}
result = MAV_RESULT_ACCEPTED;
break;
#endif // CAMERA == ENABLED

36
ArduSub/Log.cpp
View File

@ -30,21 +30,27 @@ bool Sub::print_log_menu(void)
if (0 == g.log_bitmask) {
cliSerial->printf("none");
}else{
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) cliSerial->printf(" ATTITUDE_FAST");
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) cliSerial->printf(" ATTITUDE_MED");
if (g.log_bitmask & MASK_LOG_GPS) cliSerial->printf(" GPS");
if (g.log_bitmask & MASK_LOG_PM) cliSerial->printf(" PM");
if (g.log_bitmask & MASK_LOG_CTUN) cliSerial->printf(" CTUN");
if (g.log_bitmask & MASK_LOG_NTUN) cliSerial->printf(" NTUN");
if (g.log_bitmask & MASK_LOG_RCIN) cliSerial->printf(" RCIN");
if (g.log_bitmask & MASK_LOG_IMU) cliSerial->printf(" IMU");
if (g.log_bitmask & MASK_LOG_CMD) cliSerial->printf(" CMD");
if (g.log_bitmask & MASK_LOG_CURRENT) cliSerial->printf(" CURRENT");
if (g.log_bitmask & MASK_LOG_RCOUT) cliSerial->printf(" RCOUT");
if (g.log_bitmask & MASK_LOG_OPTFLOW) cliSerial->printf(" OPTFLOW");
if (g.log_bitmask & MASK_LOG_COMPASS) cliSerial->printf(" COMPASS");
if (g.log_bitmask & MASK_LOG_CAMERA) cliSerial->printf(" CAMERA");
if (g.log_bitmask & MASK_LOG_PID) cliSerial->printf(" PID");
// Macro to make the following code a bit easier on the eye.
// Pass it the capitalised name of the log option, as defined
// in defines.h but without the LOG_ prefix. It will check for
// the bit being set and print the name of the log option to suit.
#define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) cliSerial->printf(" %s", # _s)
PLOG(ATTITUDE_FAST);
PLOG(ATTITUDE_MED);
PLOG(GPS);
PLOG(PM);
PLOG(CTUN);
PLOG(NTUN);
PLOG(RCIN);
PLOG(IMU);
PLOG(CMD);
PLOG(CURRENT);
PLOG(RCOUT);
PLOG(OPTFLOW);
PLOG(COMPASS);
PLOG(CAMERA);
PLOG(PID);
#undef PLOG
}
cliSerial->println();

1
ArduSub/Sub.h
View File

@ -572,6 +572,7 @@ private:
void rc_loop();
void throttle_loop();
void update_mount();
void update_trigger();
void update_batt_compass(void);
void ten_hz_logging_loop();
void fifty_hz_logging_loop();

21
ArduSub/commands_logic.cpp
View File

@ -873,13 +873,14 @@ void Sub::do_digicam_configure(const AP_Mission::Mission_Command& cmd)
void Sub::do_digicam_control(const AP_Mission::Mission_Command& cmd)
{
#if CAMERA == ENABLED
camera.control(cmd.content.digicam_control.session,
if (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)) {
log_picture();
}
#endif
}
@ -895,10 +896,16 @@ void Sub::do_take_picture()
// log_picture - log picture taken and send feedback to GCS
void Sub::log_picture()
{
gcs_send_message(MSG_CAMERA_FEEDBACK);
if (should_log(MASK_LOG_CAMERA)) {
DataFlash.Log_Write_Camera(ahrs, gps, current_loc);
}
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);
}
}
}
// point the camera to a specified angle