aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Sub: Sub changes to match newest Copter changes

Browse Source
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

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

7
ArduSub/GCS_Mavlink.cpp
View File

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

36
ArduSub/Log.cpp
View File

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

1
ArduSub/Sub.h
View File

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

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