dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

crazyflie: add probe() to vl53lxx driver and set imu sampling rate to 200 Hz for this board

This commit is contained in:
DanielePettenuzzo 2018-05-14 18:26:27 +02:00 committed by Beat Küng
parent ffbb53454e
commit 533f42adb8
6 changed files with 57 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
cmake/configs/nuttx_px4fmu-v4pro_default.cmake
View File

@ -25,7 +25,6 @@ set(config_module_list
drivers/imu/mpu6000
drivers/imu/mpu9250
drivers/oreoled
drivers/pmw3901
drivers/pwm_input
drivers/pwm_out_sim
drivers/px4flow

1
cmake/configs/nuttx_px4fmu-v5_default.cmake
View File

@ -25,7 +25,6 @@ set(config_module_list
drivers/imu/mpu6000
drivers/imu/mpu9250
drivers/oreoled
drivers/pmw3901
drivers/pwm_input
drivers/pwm_out_sim
drivers/px4flow

2
src/drivers/boards/crazyflie/board_config.h
View File

@ -118,7 +118,7 @@
#define PX4_FLOW_BUS_CS_GPIO { GPIO_SPI1_CS0_EXT, GPIO_SPI1_CS1_EXT, GPIO_SPI1_CS2_EXT }
/* SPI1 Devices */
#define PX4_SPIDEV_EXPANSION_1 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXPANSION, 0)
#define PX4_SPIDEV_EXPANSION_1 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXPANSION, 0) // SD CARD BREAKOUT
#define PX4_SPIDEV_EXPANSION_2 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXPANSION, 1) // OPTICAL FLOW BREAKOUT
#define PX4_SPIDEV_EXPANSION_3 PX4_MK_SPI_SEL(PX4_SPI_BUS_EXPANSION, 2)

63
src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp
View File

@ -107,6 +107,9 @@
#define VL53LXX_MAX_RANGING_DISTANCE 2.0f
#define VL53LXX_MIN_RANGING_DISTANCE 0.0f
#define VL53LXX_RA_IDENTIFICATION_MODEL_ID 0xC0
#define VL53LXX_IDENTIFICATION_MODEL_ID 0xEEAA
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
@ -130,6 +133,9 @@ public:
*/
void print_info();
protected:
virtual int probe();
private:
uint8_t _rotation;
work_s _work;
@ -253,24 +259,35 @@ int
VL53LXX::sensorInit()
{
uint8_t val = 0;
int ret;
int ret = OK;
float rate_limit;
uint8_t rate_limit_split[2];
// I2C at 2.8V on sensor side of level shifter
readRegister(VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG, val);
writeRegister(VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG, val | 0x01);
ret |= readRegister(VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG, val);
if (ret != OK) {
ret = PX4_ERROR;
return ret;
}
ret |= writeRegister(VHV_CONFIG_PAD_SCL_SDA_EXTSUP_HW_REG, val | 0x01);
// set I2C to standard mode
writeRegister(0x88, 0x00);
ret |= writeRegister(0x88, 0x00);
writeRegister(0x80, 0x01);
writeRegister(0xFF, 0x01);
writeRegister(0x00, 0x00);
readRegister(0x91, val);
writeRegister(0x00, 0x01);
writeRegister(0xFF, 0x00);
writeRegister(0x80, 0x00);
ret |= writeRegister(0x80, 0x01);
ret |= writeRegister(0xFF, 0x01);
ret |= writeRegister(0x00, 0x00);
ret |= readRegister(0x91, val);
ret |= writeRegister(0x00, 0x01);
ret |= writeRegister(0xFF, 0x00);
ret |= writeRegister(0x80, 0x00);
if (ret != OK) {
ret = PX4_ERROR;
return ret;
}
stop_variable_ = val;
@ -306,8 +323,6 @@ VL53LXX::init()
goto out;
}
sensorInit();
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
@ -339,6 +354,16 @@ out:
return ret;
}
int
VL53LXX::probe()
{
if (sensorInit() == OK) {
return OK;
}
// not found on any address
return -EIO;
}
int
VL53LXX::ioctl(device::file_t *filp, int cmd, unsigned long arg)
@ -602,7 +627,7 @@ VL53LXX::measure()
readRegister(SYSRANGE_START_REG, system_start);
if ((system_start & 0x01) == 1) {
work_queue(HPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this,
work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this,
1000); // reschedule every 1 ms until measurement is ready
ret = OK;
return ret;
@ -618,7 +643,7 @@ VL53LXX::measure()
readRegister(SYSRANGE_START_REG, system_start);
if ((system_start & 0x01) == 1) {
work_queue(HPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this,
work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this,
1000); // reschedule every 1 ms until measurement is ready
ret = OK;
return ret;
@ -631,7 +656,7 @@ VL53LXX::measure()
readRegister(RESULT_INTERRUPT_STATUS_REG, wait_for_measurement);
if ((wait_for_measurement & 0x07) == 0) {
work_queue(HPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this,
work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this,
1000); // reschedule every 1 ms until measurement is ready
ret = OK;
return ret;
@ -716,7 +741,7 @@ VL53LXX::start()
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, 1000);
work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, 1000);
/* notify about state change */
struct subsystem_info_s info = {};
@ -741,7 +766,7 @@ VL53LXX::start()
void
VL53LXX::stop()
{
work_cancel(HPWORK, &_work);
work_cancel(LPWORK, &_work);
}
@ -766,7 +791,7 @@ VL53LXX::cycle()
collect();
work_queue(HPWORK,
work_queue(LPWORK,
&_work,
(worker_t)&VL53LXX::cycle_trampoline,
this,

15
src/drivers/imu/mpu9250/mpu9250.cpp
View File

@ -139,12 +139,11 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const
_gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f),
_dlpf_freq(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ),
// #ifdef CRAZYFLIE
// _sample_rate(250),
// #else
// _sample_rate(1000),
// #endif
_sample_rate(250),
#ifdef CONFIG_ARCH_BOARD_CRAZYFLIE
_sample_rate(200),
#else
_sample_rate(1000),
#endif
_accel_reads(perf_alloc(PC_COUNT, "mpu9250_acc_read")),
_gyro_reads(perf_alloc(PC_COUNT, "mpu9250_gyro_read")),
_sample_perf(perf_alloc(PC_ELAPSED, "mpu9250_read")),
@ -161,8 +160,8 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const
_gyro_filter_x(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_y(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_z(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_accel_int(1000000 / MPU9250_ACCEL_MAX_OUTPUT_RATE / 4),
_gyro_int(1000000 / MPU9250_GYRO_MAX_OUTPUT_RATE / 4, true),
_accel_int(1000000 / MPU9250_ACCEL_MAX_OUTPUT_RATE),
_gyro_int(1000000 / MPU9250_GYRO_MAX_OUTPUT_RATE, true),
_rotation(rotation),
_checked_next(0),
_last_temperature(0),

11
src/drivers/imu/mpu9250/mpu9250.h
View File

@ -181,12 +181,11 @@
#define MPU_WHOAMI_6500 0x70
#define MPU9250_ACCEL_DEFAULT_RATE 1000
// #ifdef CRAZYFLIE
// #define MPU9250_ACCEL_MAX_OUTPUT_RATE 4*280
// #else
// #define MPU9250_ACCEL_MAX_OUTPUT_RATE 280
// #endif
#define MPU9250_ACCEL_MAX_OUTPUT_RATE 280 // remove this when using the previous #ifdef
#ifdef CONFIG_ARCH_BOARD_CRAZYFLIE
#define MPU9250_ACCEL_MAX_OUTPUT_RATE 1000
#else
#define MPU9250_ACCEL_MAX_OUTPUT_RATE 280
#endif
#define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
#define MPU9250_GYRO_DEFAULT_RATE 1000
/* rates need to be the same between accel and gyro */