From 18c3eba82906bf0cc2b3480032cac89c459ce1d9 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 24 Nov 2018 12:21:15 +0900 Subject: [PATCH] AP_OpticalFlow: minor format fix to Pixart driver --- .../AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp | 22 ++++++++----------- 1 file changed, 9 insertions(+), 13 deletions(-) diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp index 7994291e6f..da297678e8 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp @@ -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();