Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Camera: more accuate timestamps in CAM messages #8397

Merged
merged 2 commits into from
Nov 29, 2018
Merged

Camera: more accuate timestamps in CAM messages #8397

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
d0377b1
DataFlash: added timestamp to CAM messages
tridge May 15, 2018
a6c1edb
AP_Camera: use timestamp from trigger time
tridge May 15, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
112 changes: 38 additions & 74 deletions libraries/AP_Camera/AP_Camera.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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),
Expand Down Expand Up @@ -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()
Expand Down Expand Up @@ -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);
}


Expand Down Expand Up @@ -347,85 +334,57 @@ 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)
{
uint8_t pin_state = hal.gpio->read(_feedback_pin);
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
Expand Down Expand Up @@ -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);
}
}
}
Expand Down
20 changes: 6 additions & 14 deletions libraries/AP_Camera/AP_Camera.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -106,27 +103,25 @@ class AP_Camera {
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
AP_Int16 _max_roll; // Maximum acceptable roll angle when trigging camera
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();
Expand All @@ -142,9 +137,6 @@ class AP_Camera {
// 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
{
Expand Down
4 changes: 2 additions & 2 deletions libraries/DataFlash/DataFlash.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -125,8 +125,8 @@ class DataFlash_Class
void Log_Write_Radio(const mavlink_radio_t &packet);
void Log_Write_Message(const char *message);
void Log_Write_MessageF(const char *fmt, ...);
void Log_Write_CameraInfo(enum LogMessages msg, const AP_AHRS &ahrs, const Location &current_loc);
void Log_Write_Camera(const AP_AHRS &ahrs, const Location &current_loc);
void Log_Write_CameraInfo(enum LogMessages msg, const AP_AHRS &ahrs, const Location &current_loc, uint64_t timestamp_us=0);
void Log_Write_Camera(const AP_AHRS &ahrs, const Location &current_loc, uint64_t timestamp_us=0);
void Log_Write_Trigger(const AP_AHRS &ahrs, const Location &current_loc);
void Log_Write_ESC(void);
void Log_Write_Airspeed(AP_Airspeed &airspeed);
Expand Down
10 changes: 5 additions & 5 deletions libraries/DataFlash/LogFile.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1314,7 +1314,7 @@ void DataFlash_Class::Log_Write_Radio(const mavlink_radio_t &packet)
}

// Write a Camera packet
void DataFlash_Class::Log_Write_CameraInfo(enum LogMessages msg, const AP_AHRS &ahrs, const Location &current_loc)
void DataFlash_Class::Log_Write_CameraInfo(enum LogMessages msg, const AP_AHRS &ahrs, const Location &current_loc, uint64_t timestamp_us)
{
int32_t altitude, altitude_rel, altitude_gps;
if (current_loc.flags.relative_alt) {
Expand All @@ -1333,7 +1333,7 @@ void DataFlash_Class::Log_Write_CameraInfo(enum LogMessages msg, const AP_AHRS &

struct log_Camera pkt = {
LOG_PACKET_HEADER_INIT(static_cast<uint8_t>(msg)),
time_us : AP_HAL::micros64(),
time_us : timestamp_us?timestamp_us:AP_HAL::micros64(),
gps_time : gps.time_week_ms(),
gps_week : gps.time_week(),
latitude : current_loc.lat,
Expand All @@ -1349,15 +1349,15 @@ void DataFlash_Class::Log_Write_CameraInfo(enum LogMessages msg, const AP_AHRS &
}

// Write a Camera packet
void DataFlash_Class::Log_Write_Camera(const AP_AHRS &ahrs, const Location &current_loc)
void DataFlash_Class::Log_Write_Camera(const AP_AHRS &ahrs, const Location &current_loc, uint64_t timestamp_us)
{
Log_Write_CameraInfo(LOG_CAMERA_MSG, ahrs, current_loc);
Log_Write_CameraInfo(LOG_CAMERA_MSG, ahrs, current_loc, timestamp_us);
}

// Write a Trigger packet
void DataFlash_Class::Log_Write_Trigger(const AP_AHRS &ahrs, const Location &current_loc)
{
Log_Write_CameraInfo(LOG_TRIGGER_MSG, ahrs, current_loc);
Log_Write_CameraInfo(LOG_TRIGGER_MSG, ahrs, current_loc, 0);
}

// Write an attitude packet
Expand Down