fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-12 03:31:36 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
ad7b74847eb21a944659b72b0a02e4da75d1adda
PX4-Autopilot/src/drivers/optical_flow/paw3902/PAW3902.cpp
T

898 lines
24 KiB
C++
Raw Blame History

/****************************************************************************
*
* Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "PAW3902.hpp"
static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
{
return (msb << 8u) | lsb;
}
PAW3902::PAW3902(const I2CSPIDriverConfig &config) :
SPI(config),
I2CSPIDriver(config),
_drdy_gpio(config.drdy_gpio)
{
if (_drdy_gpio != 0) {
_no_motion_interrupt_perf = perf_alloc(PC_COUNT, MODULE_NAME": no motion interrupt");
}
float yaw_rotation_degrees = (float)config.custom1;
if (yaw_rotation_degrees >= 0.f) {
PX4_INFO("using yaw rotation %.3f degrees (%.3f radians)",
(double)yaw_rotation_degrees, (double)math::radians(yaw_rotation_degrees));
_rotation = matrix::Dcmf{matrix::Eulerf{0.f, 0.f, math::radians(yaw_rotation_degrees)}};
} else {
_rotation.identity();
}
}
PAW3902::~PAW3902()
{
perf_free(_bad_register_perf);
perf_free(_bad_transfer_perf);
perf_free(_reset_perf);
perf_free(_false_motion_perf);
perf_free(_mode_change_bright_perf);
perf_free(_mode_change_low_light_perf);
perf_free(_mode_change_super_low_light_perf);
perf_free(_no_motion_interrupt_perf);
}
int PAW3902::init()
{
int ret = SPI::init();
if (ret != PX4_OK) {
DEVICE_DEBUG("SPI::init failed (%i)", ret);
return ret;
}
return Reset() ? 0 : -1;
}
bool PAW3902::Reset()
{
_state = STATE::RESET;
DataReadyInterruptDisable();
_drdy_timestamp_sample.store(0);
ScheduleClear();
ScheduleNow();
return true;
}
void PAW3902::exit_and_cleanup()
{
DataReadyInterruptDisable();
I2CSPIDriverBase::exit_and_cleanup();
}
void PAW3902::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_bad_register_perf);
perf_print_counter(_bad_transfer_perf);
perf_print_counter(_reset_perf);
perf_print_counter(_false_motion_perf);
perf_print_counter(_mode_change_bright_perf);
perf_print_counter(_mode_change_low_light_perf);
perf_print_counter(_mode_change_super_low_light_perf);
perf_print_counter(_no_motion_interrupt_perf);
}
int PAW3902::probe()
{
for (int retry = 0; retry < 3; retry++) {
const uint8_t Product_ID = RegisterRead(Register::Product_ID);
const uint8_t Revision_ID = RegisterRead(Register::Revision_ID);
const uint8_t Inverse_Product_ID = RegisterRead(Register::Inverse_Product_ID);
if (Product_ID != PRODUCT_ID) {
DEVICE_DEBUG("unexpected Product_ID 0x%02x", Product_ID);
break;
}
if (Revision_ID != REVISION_ID) {
DEVICE_DEBUG("unexpected Revision_ID 0x%02x", Revision_ID);
break;
}
if (Inverse_Product_ID != PRODUCT_ID_INVERSE) {
DEVICE_DEBUG("unexpected Inverse_Product_ID 0x%02x", Inverse_Product_ID);
break;
}
return PX4_OK;
}
return PX4_ERROR;
}
void PAW3902::RunImpl()
{
const hrt_abstime now = hrt_absolute_time();
switch (_state) {
case STATE::RESET:
// Issue a soft reset
RegisterWrite(Register::Power_Up_Reset, 0x5A);
_bright_to_low_counter = 0;
_low_to_superlow_counter = 0;
_low_to_bright_counter = 0;
_superlow_to_low_counter = 0;
_reset_timestamp = now;
_failure_count = 0;
_state = STATE::WAIT_FOR_RESET;
perf_count(_reset_perf);
ScheduleDelayed(1_ms);
break;
case STATE::WAIT_FOR_RESET:
if (RegisterRead(Register::Product_ID) == PRODUCT_ID) {
// Read from registers 0x02, 0x03, 0x04, 0x05 and 0x06 one time regardless of the motion pin state.
RegisterRead(0x02);
RegisterRead(0x03);
RegisterRead(0x04);
RegisterRead(0x05);
RegisterRead(0x06);
_discard_reading = 3;
// if reset succeeded then configure
_state = STATE::CONFIGURE;
ScheduleNow();
} else {
// RESET not complete
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Reset failed, retrying");
_state = STATE::RESET;
ScheduleDelayed(100_ms);
} else {
PX4_DEBUG("Reset not complete, check again in 100 ms");
ScheduleDelayed(100_ms);
}
}
break;
case STATE::CONFIGURE:
if (Configure()) {
// if configure succeeded then start measurement cycle
_state = STATE::READ;
if (DataReadyInterruptConfigure()) {
_data_ready_interrupt_enabled = true;
// backup schedule as a watchdog timeout
ScheduleDelayed(1_s);
} else {
_data_ready_interrupt_enabled = false;
ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
}
} else {
// CONFIGURE not complete
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Configure failed, resetting");
_state = STATE::RESET;
} else {
PX4_DEBUG("Configure failed, retrying");
}
ScheduleDelayed(100_ms);
}
break;
case STATE::READ: {
hrt_abstime timestamp_sample = now;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
if (now < drdy_timestamp_sample + _scheduled_interval_us) {
timestamp_sample = drdy_timestamp_sample;
} else {
perf_count(_no_motion_interrupt_perf);
}
// push backup schedule back
ScheduleDelayed(kBackupScheduleIntervalUs);
}
struct TransferBuffer {
uint8_t cmd = Register::Motion_Burst;
BURST_TRANSFER data{};
} buffer{};
static_assert(sizeof(buffer) == (12 + 1));
bool success = false;
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) == 0) {
hrt_store_absolute_time(&_last_read_time);
if (_discard_reading > 0) {
_discard_reading--;
}
if (buffer.data.RawData_Sum > 0x98) {
perf_count(_bad_register_perf);
PX4_ERR("invalid RawData_Sum > 0x98");
}
// Number of Features = SQUAL * 4
// RawData_Sum maximum register value is 0x98
bool data_valid = (buffer.data.SQUAL > 0)
&& (buffer.data.RawData_Sum <= 0x98)
&& (_discard_reading == 0);
// publish sensor_optical_flow
sensor_optical_flow_s sensor_optical_flow{};
sensor_optical_flow.timestamp_sample = timestamp_sample;
sensor_optical_flow.device_id = get_device_id();
sensor_optical_flow.error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf);
// set specs according to datasheet
sensor_optical_flow.max_flow_rate = 7.4f; // Datasheet: 7.4 rad/s
sensor_optical_flow.min_ground_distance = 0.08f; // Datasheet: 80mm
sensor_optical_flow.max_ground_distance = INFINITY; // Datasheet: infinity
// check SQUAL & Shutter values
// To suppress false motion reports, discard Delta X and Delta Y values if the SQUAL and Shutter values meet the condition
// Bright Mode, SQUAL < 0x19, Shutter ≥ 0x1FF0
// Low Light Mode, SQUAL < 0x46, Shutter ≥ 0x1FF0
// Super Low Light Mode, SQUAL < 0x55, Shutter ≥ 0x0BC0
// 13-bit Shutter register
const uint8_t shutter_upper = buffer.data.Shutter_Upper & (Bit4 | Bit3 | Bit2 | Bit1 | Bit0);
const uint8_t shutter_lower = buffer.data.Shutter_Lower;
const uint16_t shutter = (shutter_upper << 8) | shutter_lower;
switch (_mode) {
case Mode::Bright:
sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_0;
sensor_optical_flow.mode = sensor_optical_flow_s::MODE_BRIGHT;
// quality < 25 (0x19) and shutter >= 8176 (0x1FF0)
if ((buffer.data.SQUAL < 0x19) && (shutter >= 0x1FF0)) {
// false motion report, discarding
data_valid = false;
perf_count(_false_motion_perf);
}
// shutter >= 8190 (0x1FFE), raw data sum < 60 (0x3C)
if ((shutter >= 0x1FFE) && (buffer.data.RawData_Sum < 0x3C)) {
// Bright -> LowLight
_bright_to_low_counter++;
if (_bright_to_low_counter >= 10) {
_mode = Mode::LowLight;
Reset();
}
} else {
_bright_to_low_counter = 0;
}
break;
case Mode::LowLight:
sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_1;
sensor_optical_flow.mode = sensor_optical_flow_s::MODE_LOWLIGHT;
// quality < 70 (0x46) and shutter >= 8176 (0x1FF0)
if ((buffer.data.SQUAL < 0x46) && (shutter >= 0x1FF0)) {
// false motion report, discarding
data_valid = false;
perf_count(_false_motion_perf);
}
// shutter >= 8190 (0x1FFE) and raw data sum < 90 (0x5A)
if ((shutter >= 0x1FFE) && (buffer.data.RawData_Sum < 0x5A)) {
// LowLight -> SuperLowLight
_low_to_bright_counter = 0;
_low_to_superlow_counter++;
if (_low_to_superlow_counter >= 10) {
_mode = Mode::SuperLowLight;
Reset();
}
} else if (shutter < 0x0BB8) {
// LowLight -> Bright
// shutter < 0x0BB8 (3000)
_low_to_bright_counter++;
_low_to_superlow_counter = 0;
if (_low_to_bright_counter >= 10) {
_mode = Mode::Bright;
Reset();
}
} else {
_low_to_bright_counter = 0;
_low_to_superlow_counter = 0;
}
break;
case Mode::SuperLowLight:
sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_2;
sensor_optical_flow.mode = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT;
// quality < 85 (0x55) and shutter >= 3008 (0x0BC0)
if ((buffer.data.SQUAL < 0x55) && (shutter >= 0x0BC0)) {
// false motion report, discarding
data_valid = false;
perf_count(_false_motion_perf);
}
// shutter < 500 (0x01F4)
if (shutter < 0x01F4) {
// should not operate with Shutter < 0x01F4 in Mode 2
_superlow_to_low_counter++;
data_valid = false;
} else if (shutter < 0x03E8) {
// SuperLowLight -> LowLight
// shutter < 1000 (0x03E8)
_superlow_to_low_counter++;
}
if (_superlow_to_low_counter >= 10) {
_mode = Mode::LowLight;
Reset();
}
break;
}
// motion in burst transfer
const bool motion_reported = (buffer.data.Motion & Motion_Bit::MOT);
if (data_valid) {
if (motion_reported) {
// only populate flow if data valid (motion and quality > 0)
const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L);
const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L);
// rotate measurements in yaw from sensor frame to body frame
const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{(float)delta_x_raw, (float)delta_y_raw, 0.f};
// datasheet provides 11.914 CPI (count per inch) scaling per meter of height
static constexpr float PIXART_RESOLUTION = 11.914f; // counts per inch (CPI) per meter (from surface)
static constexpr float INCHES_PER_METER = 39.3701f;
// CPI/m -> radians
static constexpr float SCALE = 1.f / (PIXART_RESOLUTION * INCHES_PER_METER);
sensor_optical_flow.pixel_flow[0] = pixel_flow_rotated(0) * SCALE;
sensor_optical_flow.pixel_flow[1] = pixel_flow_rotated(1) * SCALE;
sensor_optical_flow.quality = buffer.data.SQUAL;
}
// only publish when there's motion or at least every second
if (motion_reported || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) {
sensor_optical_flow.timestamp = hrt_absolute_time();
_sensor_optical_flow_pub.publish(sensor_optical_flow);
_last_publish = sensor_optical_flow.timestamp;
}
success = true;
if (_failure_count > 0) {
_failure_count--;
}
}
} else {
perf_count(_bad_transfer_perf);
}
if (!success) {
_failure_count++;
// full reset if things are failing consistently
if (_failure_count > 10) {
Reset();
}
}
}
break;
}
}
bool PAW3902::Configure()
{
switch (_mode) {
case Mode::Bright:
ConfigureModeBright();
_scheduled_interval_us = SAMPLE_INTERVAL_MODE_0 / 2;
perf_count(_mode_change_bright_perf);
break;
case Mode::LowLight:
ConfigureModeLowLight();
_scheduled_interval_us = SAMPLE_INTERVAL_MODE_1 / 2;
perf_count(_mode_change_low_light_perf);
break;
case Mode::SuperLowLight:
ConfigureModeSuperLowLight();
_scheduled_interval_us = SAMPLE_INTERVAL_MODE_2 / 2;
perf_count(_mode_change_super_low_light_perf);
break;
}
EnableLed();
return true;
}
void PAW3902::ConfigureModeBright()
{
// Mode 0: Bright (126 fps) 60 Lux typical
// set performance optimization registers
RegisterWrite(0x7F, 0x00);
RegisterWrite(0x55, 0x01);
RegisterWrite(0x50, 0x07);
RegisterWrite(0x7f, 0x0e);
RegisterWrite(0x43, 0x10);
RegisterWrite(0x48, 0x02);
RegisterWrite(0x7F, 0x00);
RegisterWrite(0x51, 0x7b);
RegisterWrite(0x50, 0x00);
RegisterWrite(0x55, 0x00);
RegisterWrite(0x7F, 0x00);
RegisterWrite(0x61, 0xAD);
RegisterWrite(0x7F, 0x03);
RegisterWrite(0x40, 0x00);
RegisterWrite(0x7F, 0x05);
RegisterWrite(0x41, 0xB3);
RegisterWrite(0x43, 0xF1);
RegisterWrite(0x45, 0x14);
RegisterWrite(0x5F, 0x34);
RegisterWrite(0x7B, 0x08);
RegisterWrite(0x5e, 0x34);
RegisterWrite(0x5b, 0x32);
RegisterWrite(0x6d, 0x32);
RegisterWrite(0x45, 0x17);
RegisterWrite(0x70, 0xe5);
RegisterWrite(0x71, 0xe5);
RegisterWrite(0x7F, 0x06);
RegisterWrite(0x44, 0x1B);
RegisterWrite(0x40, 0xBF);
RegisterWrite(0x4E, 0x3F);
RegisterWrite(0x7F, 0x08);
RegisterWrite(0x66, 0x44);
RegisterWrite(0x65, 0x20);
RegisterWrite(0x6a, 0x3a);
RegisterWrite(0x61, 0x05);
RegisterWrite(0x62, 0x05);
RegisterWrite(0x7F, 0x09);
RegisterWrite(0x4F, 0xAF);
RegisterWrite(0x48, 0x80);
RegisterWrite(0x49, 0x80);
RegisterWrite(0x57, 0x77);
RegisterWrite(0x5F, 0x40);
RegisterWrite(0x60, 0x78);
RegisterWrite(0x61, 0x78);
RegisterWrite(0x62, 0x08);
RegisterWrite(0x63, 0x50);
RegisterWrite(0x7F, 0x0A);
RegisterWrite(0x45, 0x60);
RegisterWrite(0x7F, 0x00);
RegisterWrite(0x4D, 0x11);
RegisterWrite(0x55, 0x80);
RegisterWrite(0x74, 0x21);
RegisterWrite(0x75, 0x1F);
RegisterWrite(0x4A, 0x78);
RegisterWrite(0x4B, 0x78);
RegisterWrite(0x44, 0x08);
RegisterWrite(0x45, 0x50);
RegisterWrite(0x64, 0xFE);
RegisterWrite(0x65, 0x1F);
RegisterWrite(0x72, 0x0A);
RegisterWrite(0x73, 0x00);
RegisterWrite(0x7F, 0x14);
RegisterWrite(0x44, 0x84);
RegisterWrite(0x65, 0x47);
RegisterWrite(0x66, 0x18);
RegisterWrite(0x63, 0x70);
RegisterWrite(0x6f, 0x2c);
RegisterWrite(0x7F, 0x15);
RegisterWrite(0x48, 0x48);
RegisterWrite(0x7F, 0x07);
RegisterWrite(0x41, 0x0D);
RegisterWrite(0x43, 0x14);
RegisterWrite(0x4B, 0x0E);
RegisterWrite(0x45, 0x0F);
RegisterWrite(0x44, 0x42);
RegisterWrite(0x4C, 0x80);
RegisterWrite(0x7F, 0x10);
RegisterWrite(0x5B, 0x03);
RegisterWrite(0x7F, 0x07);
RegisterWrite(0x40, 0x41);
px4_usleep(10'000); // delay 10ms
RegisterWrite(0x7F, 0x00);
RegisterWrite(0x32, 0x00);
RegisterWrite(0x7F, 0x07);
RegisterWrite(0x40, 0x40);
RegisterWrite(0x7F, 0x06);
RegisterWrite(0x68, 0x70);
RegisterWrite(0x69, 0x01);
RegisterWrite(0x7F, 0x0D);
RegisterWrite(0x48, 0xC0);
RegisterWrite(0x6F, 0xD5);
RegisterWrite(0x7F, 0x00);
RegisterWrite(0x5B, 0xA0);
RegisterWrite(0x4E, 0xA8);
RegisterWrite(0x5A, 0x50);
RegisterWrite(0x40, 0x80);
RegisterWrite(0x73, 0x1f);
px4_usleep(10'000); // delay 10ms
RegisterWrite(0x73, 0x00);
}
void PAW3902::ConfigureModeLowLight()
{
// Mode 1: Low Light (126 fps) 30 Lux typical
// low light and low speed motion tracking
// set performance optimization registers
RegisterWrite(0x7F, 0x00);
RegisterWrite(0x55, 0x01);
RegisterWrite(0x50, 0x07);
RegisterWrite(0x7f, 0x0e);
RegisterWrite(0x43, 0x10);
RegisterWrite(0x48, 0x02);
RegisterWrite(0x7F, 0x00);
RegisterWrite(0x51, 0x7b);
RegisterWrite(0x50, 0x00);
RegisterWrite(0x55, 0x00);
RegisterWrite(0x7F, 0x00);
RegisterWrite(0x61, 0xAD);
RegisterWrite(0x7F, 0x03);
RegisterWrite(0x40, 0x00);
RegisterWrite(0x7F, 0x05);
RegisterWrite(0x41, 0xB3);
RegisterWrite(0x43, 0xF1);
RegisterWrite(0x45, 0x14);
RegisterWrite(0x5F, 0x34);
RegisterWrite(0x7B, 0x08);
RegisterWrite(0x5e, 0x34);
RegisterWrite(0x5b, 0x65);
RegisterWrite(0x6d, 0x65);
RegisterWrite(0x45, 0x17);
RegisterWrite(0x70, 0xe5);
RegisterWrite(0x71, 0xe5);
RegisterWrite(0x7F, 0x06);
RegisterWrite(0x44, 0x1B);
RegisterWrite(0x40, 0xBF);
RegisterWrite(0x4E, 0x3F);
RegisterWrite(0x7F, 0x08);
RegisterWrite(0x66, 0x44);
RegisterWrite(0x65, 0x20);
RegisterWrite(0x6a, 0x3a);
RegisterWrite(0x61, 0x05);
RegisterWrite(0x62, 0x05);
RegisterWrite(0x7F, 0x09);
RegisterWrite(0x4F, 0xAF);
RegisterWrite(0x48, 0x80);
RegisterWrite(0x49, 0x80);
RegisterWrite(0x57, 0x77);
RegisterWrite(0x5F, 0x40);
RegisterWrite(0x60, 0x78);
RegisterWrite(0x61, 0x78);
RegisterWrite(0x62, 0x08);
RegisterWrite(0x63, 0x50);
RegisterWrite(0x7F, 0x0A);
RegisterWrite(0x45, 0x60);
RegisterWrite(0x7F, 0x00);
RegisterWrite(0x4D, 0x11);
RegisterWrite(0x55, 0x80);
RegisterWrite(0x74, 0x21);
RegisterWrite(0x75, 0x1F);
RegisterWrite(0x4A, 0x78);
RegisterWrite(0x4B, 0x78);
RegisterWrite(0x44, 0x08);
RegisterWrite(0x45, 0x50);
RegisterWrite(0x64, 0xFE);
RegisterWrite(0x65, 0x1F);
RegisterWrite(0x72, 0x0A);
RegisterWrite(0x73, 0x00);
RegisterWrite(0x7F, 0x14);
RegisterWrite(0x44, 0x84);
RegisterWrite(0x65, 0x67);
RegisterWrite(0x66, 0x18);
RegisterWrite(0x63, 0x70);
RegisterWrite(0x6f, 0x2c);
RegisterWrite(0x7F, 0x15);
RegisterWrite(0x48, 0x48);
RegisterWrite(0x7F, 0x07);
RegisterWrite(0x41, 0x0D);
RegisterWrite(0x43, 0x14);
RegisterWrite(0x4B, 0x0E);
RegisterWrite(0x45, 0x0F);
RegisterWrite(0x44, 0x42);
RegisterWrite(0x4C, 0x80);
RegisterWrite(0x7F, 0x10);
RegisterWrite(0x5B, 0x03);
RegisterWrite(0x7F, 0x07);
RegisterWrite(0x40, 0x41);
px4_usleep(10'000); // delay 10ms
RegisterWrite(0x7F, 0x00);
RegisterWrite(0x32, 0x00);
RegisterWrite(0x7F, 0x07);
RegisterWrite(0x40, 0x40);
RegisterWrite(0x7F, 0x06);
RegisterWrite(0x68, 0x70);
RegisterWrite(0x69, 0x01);
RegisterWrite(0x7F, 0x0D);
RegisterWrite(0x48, 0xC0);
RegisterWrite(0x6F, 0xD5);
RegisterWrite(0x7F, 0x00);
RegisterWrite(0x5B, 0xA0);
RegisterWrite(0x4E, 0xA8);
RegisterWrite(0x5A, 0x50);
RegisterWrite(0x40, 0x80);
RegisterWrite(0x73, 0x1f);
px4_usleep(10'000); // delay 10ms
RegisterWrite(0x73, 0x00);
}
void PAW3902::ConfigureModeSuperLowLight()
{
// Mode 2: Super Low Light (50 fps) 9 Lux typical
// super low light and low speed motion tracking
// set performance optimization registers
RegisterWrite(0x7F, 0x00);
RegisterWrite(0x55, 0x01);
RegisterWrite(0x50, 0x07);
RegisterWrite(0x7f, 0x0e);
RegisterWrite(0x43, 0x10);
RegisterWrite(0x48, 0x04);
RegisterWrite(0x7F, 0x00);
RegisterWrite(0x51, 0x7b);
RegisterWrite(0x50, 0x00);
RegisterWrite(0x55, 0x00);
RegisterWrite(0x7F, 0x00);
RegisterWrite(0x61, 0xAD);
RegisterWrite(0x7F, 0x03);
RegisterWrite(0x40, 0x00);
RegisterWrite(0x7F, 0x05);
RegisterWrite(0x41, 0xB3);
RegisterWrite(0x43, 0xF1);
RegisterWrite(0x45, 0x14);
RegisterWrite(0x5F, 0x34);
RegisterWrite(0x7B, 0x08);
RegisterWrite(0x5E, 0x34);
RegisterWrite(0x5B, 0x32);
RegisterWrite(0x6D, 0x32);
RegisterWrite(0x45, 0x17);
RegisterWrite(0x70, 0xE5);
RegisterWrite(0x71, 0xE5);
RegisterWrite(0x7F, 0x06);
RegisterWrite(0x44, 0x1B);
RegisterWrite(0x40, 0xBF);
RegisterWrite(0x4E, 0x3F);
RegisterWrite(0x7F, 0x08);
RegisterWrite(0x66, 0x44);
RegisterWrite(0x65, 0x20);
RegisterWrite(0x6A, 0x3a);
RegisterWrite(0x61, 0x05);
RegisterWrite(0x62, 0x05);
RegisterWrite(0x7F, 0x09);
RegisterWrite(0x4F, 0xAF);
RegisterWrite(0x48, 0x80);
RegisterWrite(0x49, 0x80);
RegisterWrite(0x57, 0x77);
RegisterWrite(0x5F, 0x40);
RegisterWrite(0x60, 0x78);
RegisterWrite(0x61, 0x78);
RegisterWrite(0x62, 0x08);
RegisterWrite(0x63, 0x50);
RegisterWrite(0x7F, 0x0A);
RegisterWrite(0x45, 0x60);
RegisterWrite(0x7F, 0x00);
RegisterWrite(0x4D, 0x11);
RegisterWrite(0x55, 0x80);
RegisterWrite(0x74, 0x21);
RegisterWrite(0x75, 0x1F);
RegisterWrite(0x4A, 0x78);
RegisterWrite(0x4B, 0x78);
RegisterWrite(0x44, 0x08);
RegisterWrite(0x45, 0x50);
RegisterWrite(0x64, 0xCE);
RegisterWrite(0x65, 0x0B);
RegisterWrite(0x72, 0x0A);
RegisterWrite(0x73, 0x00);
RegisterWrite(0x7F, 0x14);
RegisterWrite(0x44, 0x84);
RegisterWrite(0x65, 0x67);
RegisterWrite(0x66, 0x18);
RegisterWrite(0x63, 0x70);
RegisterWrite(0x6f, 0x2c);
RegisterWrite(0x7F, 0x15);
RegisterWrite(0x48, 0x48);
RegisterWrite(0x7F, 0x07);
RegisterWrite(0x41, 0x0D);
RegisterWrite(0x43, 0x14);
RegisterWrite(0x4B, 0x0E);
RegisterWrite(0x45, 0x0F);
RegisterWrite(0x44, 0x42);
RegisterWrite(0x4C, 0x80);
RegisterWrite(0x7F, 0x10);
RegisterWrite(0x5B, 0x02);
RegisterWrite(0x7F, 0x07);
RegisterWrite(0x40, 0x41);
px4_usleep(25'000); // delay 25ms
RegisterWrite(0x7F, 0x00);
RegisterWrite(0x32, 0x44);
RegisterWrite(0x7F, 0x07);
RegisterWrite(0x40, 0x40);
RegisterWrite(0x7F, 0x06);
RegisterWrite(0x68, 0x40);
RegisterWrite(0x69, 0x02);
RegisterWrite(0x7F, 0x0D);
RegisterWrite(0x48, 0xC0);
RegisterWrite(0x6F, 0xD5);
RegisterWrite(0x7F, 0x00);
RegisterWrite(0x5B, 0xA0);
RegisterWrite(0x4E, 0xA8);
RegisterWrite(0x5A, 0x50);
RegisterWrite(0x40, 0x80);
RegisterWrite(0x73, 0x0B);
px4_usleep(25'000); // delay 25ms
RegisterWrite(0x73, 0x00);
}
void PAW3902::EnableLed()
{
// Enable LED_N controls
RegisterWrite(0x7F, 0x14);
RegisterWrite(0x6F, 0x1c);
RegisterWrite(0x7F, 0x00);
}
int PAW3902::DataReadyInterruptCallback(int irq, void *context, void *arg)
{
static_cast<PAW3902 *>(arg)->DataReady();
return 0;
}
void PAW3902::DataReady()
{
_drdy_timestamp_sample.store(hrt_absolute_time());
ScheduleNow();
}
bool PAW3902::DataReadyInterruptConfigure()
{
if (_drdy_gpio == 0) {
return false;
}
// Setup data ready on falling edge
return (px4_arch_gpiosetevent(_drdy_gpio, false, true, false, &DataReadyInterruptCallback, this) == 0);
}
bool PAW3902::DataReadyInterruptDisable()
{
if (_drdy_gpio == 0) {
return false;
}
return (px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0);
}
uint8_t PAW3902::RegisterRead(uint8_t reg)
{
// tSWR SPI Time Between Write And Read Commands
const hrt_abstime elapsed_last_write = hrt_elapsed_time(&_last_write_time);
if (elapsed_last_write < TIME_TSWR_us) {
px4_udelay(TIME_TSWR_us - elapsed_last_write);
}
// tSRW/tSRR SPI Time Between Read And Subsequent Commands
const hrt_abstime elapsed_last_read = hrt_elapsed_time(&_last_read_time);
if (elapsed_last_write < TIME_TSRW_TSRR_us) {
px4_udelay(TIME_TSRW_TSRR_us - elapsed_last_read);
}
uint8_t cmd[2];
cmd[0] = DIR_READ(reg);
cmd[1] = 0;
transfer(&cmd[0], &cmd[0], sizeof(cmd));
hrt_store_absolute_time(&_last_read_time);
return cmd[1];
}
void PAW3902::RegisterWrite(uint8_t reg, uint8_t data)
{
// tSWW SPI Time Between Write Commands
const hrt_abstime elapsed_last_write = hrt_elapsed_time(&_last_write_time);
if (elapsed_last_write < TIME_TSWW_us) {
px4_udelay(TIME_TSWW_us - elapsed_last_write);
}
uint8_t cmd[2];
cmd[0] = DIR_WRITE(reg);
cmd[1] = data;
transfer(&cmd[0], nullptr, sizeof(cmd));
hrt_store_absolute_time(&_last_write_time);
}
Reference in New Issue View Git Blame Copy Permalink