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

AP_Camera: use timestamp from trigger time 

this avoids time inaccuracy from loop times in timestamps
This commit is contained in:
Andrew Tridgell 2018-05-15 11:03:49 +10:00
parent 19ba2322ac
commit c1516da203
2 changed files with 46 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

116
libraries/AP_Camera/AP_Camera.cpp
View File

@ -3,14 +3,6 @@
#include <AP_Math/AP_Math.h>
#include <RC_Channel/RC_Channel.h>
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#include <drivers/drv_input_capture.h>
#include <drivers/drv_pwm_output.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#endif
// ------------------------------
#define CAM_DEBUG DISABLED
@ -80,8 +72,8 @@ const AP_Param::GroupInfo AP_Camera::var_info[] = {
// @Param: FEEDBACK_PIN
// @DisplayName: Camera feedback pin
// @Description: pin number to use for save accurate camera feedback messages. If set to -1 then don't use a pin flag for this, otherwise this is a pin number which if held high after a picture trigger order, will save camera messages when camera really takes a picture. A universal camera hot shoe is needed. The pin should be held high for at least 2 milliseconds for reliable trigger detection. See also the CAM_FEEDBACK_POL option. If using AUX4 pin on a Pixhawk then a fast capture method is used that allows for the trigger time to be as short as one microsecond.
// @Values: -1:Disabled,50:PX4 AUX1,51:PX4 AUX2,52:PX4 AUX3,53:PX4 AUX4(fast capture),54:PX4 AUX5,55:PX4 AUX6
// @Description: pin number to use for save accurate camera feedback messages. If set to -1 then don't use a pin flag for this, otherwise this is a pin number which if held high after a picture trigger order, will save camera messages when camera really takes a picture. A universal camera hot shoe is needed. The pin should be held high for at least 2 milliseconds for reliable trigger detection. See also the CAM_FEEDBACK_POL option.
// @Values: -1:Disabled,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO("FEEDBACK_PIN", 8, AP_Camera, _feedback_pin, AP_CAMERA_FEEDBACK_DEFAULT_FEEDBACK_PIN),
@ -112,11 +104,6 @@ const AP_Param::GroupInfo AP_Camera::var_info[] = {
extern const AP_HAL::HAL& hal;
/*
static trigger var for PX4 callback
*/
volatile bool AP_Camera::_camera_triggered;
/// Servo operated camera
void
AP_Camera::servo_pic()
@ -298,7 +285,7 @@ void AP_Camera::send_feedback(mavlink_channel_t chan)
current_loc.lat, current_loc.lng,
altitude*1e-2f, altitude_rel*1e-2f,
ahrs.roll_sensor*1e-2f, ahrs.pitch_sensor*1e-2f, ahrs.yaw_sensor*1e-2f,
0.0f, CAMERA_FEEDBACK_PHOTO, _feedback_events);
0.0f, CAMERA_FEEDBACK_PHOTO, _camera_trigger_logged);
}
@ -347,7 +334,17 @@ void AP_Camera::update()
}
/*
check if feedback pin is high
interrupt handler for interrupt based feedback trigger
*/
void AP_Camera::feedback_pin_isr(uint8_t pin, bool high, uint32_t timestamp_us)
{
_feedback_timestamp_us = timestamp_us;
_camera_trigger_count++;
}
/*
check if feedback pin is high for timer based feedback trigger, when
attach_interrupt fails
*/
void AP_Camera::feedback_pin_timer(void)
{
@ -355,77 +352,39 @@ void AP_Camera::feedback_pin_timer(void)
uint8_t trigger_polarity = _feedback_polarity==0?0:1;
if (pin_state == trigger_polarity &&
_last_pin_state != trigger_polarity) {
_camera_triggered = true;
_feedback_timestamp_us = AP_HAL::micros();
_camera_trigger_count++;
}
_last_pin_state = pin_state;
}
/*
check if camera has triggered
*/
bool AP_Camera::check_feedback_pin(void)
{
if (_camera_triggered) {
_camera_triggered = false;
return true;
}
return false;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
/*
callback for timer capture on PX4
*/
void AP_Camera::capture_callback(void *context, uint32_t chan_index,
hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
{
_camera_triggered = true;
}
#endif
/*
setup a callback for a feedback pin. When on PX4 with the right FMU
mode we can use the microsecond timer.
*/
void AP_Camera::setup_feedback_callback(void)
{
if (_feedback_pin <= 0 || _timer_installed) {
if (_feedback_pin <= 0 || _timer_installed || _isr_installed) {
// invalid or already installed
return;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
/*
special case for pin 53 on PX4. We can use the fast timer support
*/
if (_feedback_pin == 53) {
int fd = open("/dev/px4fmu", 0);
if (fd != -1) {
if (ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_MODE_3PWM1CAP) != 0) {
gcs().send_text(MAV_SEVERITY_WARNING, "Camera: unable to setup 3PWM1CAP");
close(fd);
goto failed;
}
if (up_input_capture_set(3, _feedback_polarity==1?Rising:Falling, 0, capture_callback, this) != 0) {
gcs().send_text(MAV_SEVERITY_WARNING, "Camera: unable to setup timer capture");
close(fd);
goto failed;
}
close(fd);
_timer_installed = true;
gcs().send_text(MAV_SEVERITY_WARNING, "Camera: setup fast trigger capture");
}
// ensure we are in input mode
hal.gpio->pinMode(_feedback_pin, HAL_GPIO_INPUT);
// enable pullup/pulldown
uint8_t trigger_polarity = _feedback_polarity==0?0:1;
hal.gpio->write(_feedback_pin, !trigger_polarity);
if (hal.gpio->attach_interrupt(_feedback_pin, FUNCTOR_BIND_MEMBER(&AP_Camera::feedback_pin_isr, void, uint8_t, bool, uint32_t),
trigger_polarity?AP_HAL::GPIO::INTERRUPT_RISING:AP_HAL::GPIO::INTERRUPT_FALLING)) {
_isr_installed = true;
} else {
// install a 1kHz timer to check feedback pin
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Camera::feedback_pin_timer, void));
_timer_installed = true;
}
failed:
#endif // CONFIG_HAL_BOARD
hal.gpio->pinMode(_feedback_pin, HAL_GPIO_INPUT); // ensure we are in input mode
hal.gpio->write(_feedback_pin, 1); // enable pullup
// install a 1kHz timer to check feedback pin
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Camera::feedback_pin_timer, void));
_timer_installed = true;
}
// log_picture - log picture taken and send feedback to GCS
@ -472,13 +431,18 @@ void AP_Camera::take_picture()
void AP_Camera::update_trigger()
{
trigger_pic_cleanup();
if (check_feedback_pin()) {
_feedback_events++;
if (_camera_trigger_logged != _camera_trigger_count) {
uint32_t timestamp32 = _feedback_timestamp_us;
_camera_trigger_logged = _camera_trigger_count;
gcs().send_message(MSG_CAMERA_FEEDBACK);
DataFlash_Class *df = DataFlash_Class::instance();
if (df != nullptr) {
if (df->should_log(log_camera_bit)) {
df->Log_Write_Camera(ahrs, current_loc);
uint32_t tdiff = AP_HAL::micros() - timestamp32;
uint64_t timestamp = AP_HAL::micros64();
df->Log_Write_Camera(ahrs, current_loc, timestamp - tdiff);
}
}
}

20
libraries/AP_Camera/AP_Camera.h
View File

@ -10,9 +10,6 @@
#include <AP_GPS/AP_GPS.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Mission/AP_Mission.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#include <drivers/drv_hrt.h>
#endif
#define AP_CAMERA_TRIGGER_TYPE_SERVO 0
#define AP_CAMERA_TRIGGER_TYPE_RELAY 1
@ -106,11 +103,8 @@ private:
void servo_pic(); // Servo operated camera
void relay_pic(); // basic relay activation
void feedback_pin_timer();
void feedback_pin_isr(uint8_t, bool, uint32_t);
void setup_feedback_callback(void);
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
static void capture_callback(void *context, uint32_t chan_index,
hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);
#endif
AP_Float _trigg_dist; // distance between trigger points (meters)
AP_Int16 _min_interval; // Minimum time between shots required by camera
@ -118,15 +112,16 @@ private:
uint32_t _last_photo_time; // last time a photo was taken
struct Location _last_location;
uint16_t _image_index; // number of pictures taken since boot
uint16_t _feedback_events; // number of feedback events since boot
// pin number for accurate camera feedback messages
AP_Int8 _feedback_pin;
AP_Int8 _feedback_polarity;
// this is set to 1 when camera trigger pin has fired
static volatile bool _camera_triggered;
bool _timer_installed:1;
uint32_t _camera_trigger_count;
uint32_t _camera_trigger_logged;
uint32_t _feedback_timestamp_us;
bool _timer_installed;
bool _isr_installed;
uint8_t _last_pin_state;
void log_picture();
@ -142,9 +137,6 @@ private:
// should be called at 50hz from main program
void trigger_pic_cleanup();
// check if feedback pin has fired
bool check_feedback_pin(void);
// return true if we are using a feedback pin
bool using_feedback_pin(void) const
{