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

AP_InertialSensor: added optional FSYNC external sync bit

Browse Source 
used to synchronise with image sensor on Disco
This commit is contained in:
Andrew Tridgell 2016-08-31 14:56:27 +10:00
parent c62fc336cb
commit 6f9530ebaa
3 changed files with 33 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -159,7 +159,8 @@ void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f
void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
const Vector3f &accel,
uint64_t sample_us)
uint64_t sample_us,
bool fsync_set)
{
float dt;
@ -170,7 +171,7 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
dt = 1.0f / _imu._accel_raw_sample_rates[instance];
// call gyro_sample hook if any
AP_Module::call_hook_accel_sample(instance, dt, accel);
AP_Module::call_hook_accel_sample(instance, dt, accel, fsync_set);
_imu.calc_vibration_and_clipping(instance, accel, dt);

2
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -95,7 +95,7 @@ protected:
// be it published or not
// the sample is raw in the sense that it's not filtered yet, but it must
// be rotated and corrected (_rotate_and_correct_accel)
void _notify_new_accel_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0);
void _notify_new_accel_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0, bool fsync_set=false);
// set accelerometer max absolute offset for calibration
void _set_accel_max_abs_offset(uint8_t instance, float offset);

30
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -20,9 +20,19 @@ extern const AP_HAL::HAL& hal;
#define MPU6000_DRDY_PIN RPI_GPIO_24
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
#define MPU6000_DRDY_PIN MINNOW_GPIO_I2S_CLK
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
#define MPU6000_EXT_SYNC_ENABLE 1
#endif
#endif
/*
EXT_SYNC allows for frame synchronisation with an external device
such as a camera. When enabled the LSB of AccelZ holds the FSYNC bit
*/
#ifndef MPU6000_EXT_SYNC_ENABLE
#define MPU6000_EXT_SYNC_ENABLE 0
#endif
// MPU 6000 registers
#define MPUREG_XG_OFFS_TC 0x00
#define MPUREG_YG_OFFS_TC 0x01
@ -51,6 +61,13 @@ extern const AP_HAL::HAL& hal;
# define MPUREG_SMPLRT_100HZ 0x09
# define MPUREG_SMPLRT_50HZ 0x13
#define MPUREG_CONFIG 0x1A
# define MPUREG_CONFIG_EXT_SYNC_SHIFT 3
# define MPUREG_CONFIG_EXT_SYNC_GX 0x02
# define MPUREG_CONFIG_EXT_SYNC_GY 0x03
# define MPUREG_CONFIG_EXT_SYNC_GZ 0x04
# define MPUREG_CONFIG_EXT_SYNC_AX 0x05
# define MPUREG_CONFIG_EXT_SYNC_AY 0x06
# define MPUREG_CONFIG_EXT_SYNC_AZ 0x07
#define MPUREG_GYRO_CONFIG 0x1B
// bit definitions for MPUREG_GYRO_CONFIG
# define BITS_GYRO_FS_250DPS 0x00
@ -442,7 +459,12 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
uint8_t *data = samples + MPU6000_SAMPLE_SIZE * i;
Vector3f accel, gyro;
float temp;
bool fsync_set = false;
#if MPU6000_EXT_SYNC_ENABLE
fsync_set = (int16_val(data, 2) & 1U) != 0;
#endif
accel = Vector3f(int16_val(data, 1),
int16_val(data, 0),
-int16_val(data, 2));
@ -474,7 +496,7 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
_rotate_and_correct_accel(_accel_instance, accel);
_rotate_and_correct_gyro(_gyro_instance, gyro);
_notify_new_accel_raw_sample(_accel_instance, accel);
_notify_new_accel_raw_sample(_accel_instance, accel, AP_HAL::micros64(), fsync_set);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
_temp_filtered = _temp_filter.apply(temp);
@ -579,6 +601,12 @@ void AP_InertialSensor_MPU6000::_set_filter_register(uint16_t filter_hz)
} else {
filter = BITS_DLPF_CFG_256HZ_NOLPF2;
}
#if MPU6000_EXT_SYNC_ENABLE
// add in EXT_SYNC bit if enabled
filter |= (MPUREG_CONFIG_EXT_SYNC_AZ << MPUREG_CONFIG_EXT_SYNC_SHIFT);
#endif
_register_write(MPUREG_CONFIG, filter);
}