|
|
|
@ -251,8 +251,9 @@ void AP_Camera::send_feedback(mavlink_channel_t chan)
@@ -251,8 +251,9 @@ void AP_Camera::send_feedback(mavlink_channel_t chan)
|
|
|
|
|
altitude_rel = current_loc.alt - ahrs.get_home().alt; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
mavlink_msg_camera_feedback_send(chan,
|
|
|
|
|
gps.time_epoch_usec(), |
|
|
|
|
mavlink_msg_camera_feedback_send( |
|
|
|
|
chan, |
|
|
|
|
AP::gps().time_epoch_usec(), |
|
|
|
|
0, 0, _image_index, |
|
|
|
|
current_loc.lat, current_loc.lng, |
|
|
|
|
altitude*1e-2, altitude_rel*1e-2, |
|
|
|
@ -265,7 +266,7 @@ void AP_Camera::send_feedback(mavlink_channel_t chan)
@@ -265,7 +266,7 @@ void AP_Camera::send_feedback(mavlink_channel_t chan)
|
|
|
|
|
*/ |
|
|
|
|
void AP_Camera::update() |
|
|
|
|
{ |
|
|
|
|
if (gps.status() < AP_GPS::GPS_OK_FIX_3D) { |
|
|
|
|
if (AP::gps().status() < AP_GPS::GPS_OK_FIX_3D) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -405,11 +406,11 @@ void AP_Camera::log_picture()
@@ -405,11 +406,11 @@ void AP_Camera::log_picture()
|
|
|
|
|
if (!using_feedback_pin()) { |
|
|
|
|
gcs().send_message(MSG_CAMERA_FEEDBACK); |
|
|
|
|
if (df->should_log(log_camera_bit)) { |
|
|
|
|
df->Log_Write_Camera(ahrs, gps, current_loc); |
|
|
|
|
df->Log_Write_Camera(ahrs, current_loc); |
|
|
|
|
} |
|
|
|
|
} else { |
|
|
|
|
if (df->should_log(log_camera_bit)) { |
|
|
|
|
df->Log_Write_Trigger(ahrs, gps, current_loc); |
|
|
|
|
df->Log_Write_Trigger(ahrs, current_loc); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
@ -444,7 +445,7 @@ void AP_Camera::update_trigger()
@@ -444,7 +445,7 @@ void AP_Camera::update_trigger()
|
|
|
|
|
DataFlash_Class *df = DataFlash_Class::instance(); |
|
|
|
|
if (df != nullptr) { |
|
|
|
|
if (df->should_log(log_camera_bit)) { |
|
|
|
|
df->Log_Write_Camera(ahrs, gps, current_loc); |
|
|
|
|
df->Log_Write_Camera(ahrs, current_loc); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|