zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Camera: use timestamp from trigger time 

this avoids time inaccuracy from loop times in timestamps
master
Andrew Tridgell 7 years ago
parent
19ba2322ac
commit
c1516da203
2 changed files with 44 additions and 88 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 112
      libraries/AP_Camera/AP_Camera.cpp
  2. 20
      libraries/AP_Camera/AP_Camera.h

112
libraries/AP_Camera/AP_Camera.cpp
Unescape View File

@ -3,14 +3,6 @@ @@ -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[] = { @@ -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[] = { @@ -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) @@ -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() @@ -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) @@ -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");
}
}
failed:
#endif // CONFIG_HAL_BOARD
// ensure we are in input mode
hal.gpio->pinMode(_feedback_pin, HAL_GPIO_INPUT);
hal.gpio->pinMode(_feedback_pin, HAL_GPIO_INPUT); // ensure we are in input mode
hal.gpio->write(_feedback_pin, 1); // enable pullup
// enable pullup/pulldown
uint8_t trigger_polarity = _feedback_polarity==0?0:1;
hal.gpio->write(_feedback_pin, !trigger_polarity);
// install a 1kHz timer to check feedback pin
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Camera::feedback_pin_timer, void));
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;
_timer_installed = true;
}
}
// log_picture - log picture taken and send feedback to GCS
@ -472,13 +431,18 @@ void AP_Camera::take_picture() @@ -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
Unescape View File

@ -10,9 +10,6 @@ @@ -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: @@ -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: @@ -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: @@ -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
{

Write Preview
Loading…
Cancel
Save