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

AP_OpticalFlow: minor format fix to Pixart driver

Browse Source
This commit is contained in:
Randy Mackay 2018-11-24 12:21:15 +09:00
parent e629845f00
commit 18c3eba829
1 changed files with 9 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp
View File

@ -86,7 +86,6 @@ AP_OpticalFlow_Pixart::AP_OpticalFlow_Pixart(const char *devname, OpticalFlow &_
_dev = std::move(hal.spi->get_device(devname));
}
// detect the device
AP_OpticalFlow_Pixart *AP_OpticalFlow_Pixart::detect(const char *devname, OpticalFlow &_frontend)
{
@ -142,10 +141,10 @@ bool AP_OpticalFlow_Pixart::setup_sensor(void)
debug("Pixart: bad SROM ID: 0x%02x\n", id);
return false;
}
reg_write(PIXART_REG_SROM_EN, 0x15);
hal.scheduler->delay(10);
crc = reg_read16u(PIXART_REG_DOUT_L);
if (crc != 0xBEEF) {
debug("Pixart: bad SROM CRC: 0x%04x\n", crc);
@ -171,7 +170,6 @@ bool AP_OpticalFlow_Pixart::setup_sensor(void)
return true;
}
// write an 8 bit register
void AP_OpticalFlow_Pixart::reg_write(uint8_t reg, uint8_t value)
{
@ -211,7 +209,6 @@ int16_t AP_OpticalFlow_Pixart::reg_read16s(uint8_t reg)
return (int16_t)reg_read16u(reg);
}
void AP_OpticalFlow_Pixart::srom_download(void)
{
reg_write(0x39, 0x02);
@ -254,14 +251,13 @@ void AP_OpticalFlow_Pixart::load_configuration(const RegData *init_data, uint16_
}
}
void AP_OpticalFlow_Pixart::motion_burst(void)
{
uint8_t *b = (uint8_t *)&burst;
burst.delta_x = 0;
burst.delta_y = 0;
_dev->set_chip_select(true);
uint8_t reg = model==PIXART_3900?PIXART_REG_MOT_BURST:PIXART_REG_MOT_BURST2;
@ -300,7 +296,7 @@ void AP_OpticalFlow_Pixart::timer(void)
integral.last_frame_us = last_burst_us;
integral.gyro += Vector2f(gyro.x, gyro.y) * dt;
}
#if 0
static uint32_t last_print_ms;
static int fd = -1;
@ -312,7 +308,7 @@ void AP_OpticalFlow_Pixart::timer(void)
static int32_t sum_y;
sum_x += burst.delta_x;
sum_y += burst.delta_y;
uint32_t now = AP_HAL::millis();
if (now - last_print_ms >= 100 && (sum_x != 0 || sum_y != 0)) {
last_print_ms = now;
@ -332,11 +328,11 @@ void AP_OpticalFlow_Pixart::update(void)
return;
}
last_update_ms = now;
struct OpticalFlow::OpticalFlow_state state;
state.device_id = 1;
state.surface_quality = burst.squal;
if (integral.sum_us > 0) {
WITH_SEMAPHORE(_sem);
@ -344,14 +340,14 @@ void AP_OpticalFlow_Pixart::update(void)
float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
float dt = integral.sum_us * 1.0e-6;
state.flowRate = Vector2f(integral.sum.x * flowScaleFactorX,
integral.sum.y * flowScaleFactorY);
state.flowRate *= flow_pixel_scaling / dt;
// we only apply yaw to flowRate as body rate comes from AHRS
_applyYaw(state.flowRate);
state.bodyRate = integral.gyro / dt;
integral.sum.zero();