38 ScheduledWorkItem(MODULE_NAME,
px4::device_bus_to_wq(get_device_id())),
42 _yaw_rotation(yaw_rotation)
72 SPI::set_lockmode(LOCK_THREADS);
96 PX4_INFO(
"DEVICE_ID: %X", product_id);
104 PX4_INFO(
"REVISION_ID: %X", revision_id);
135 if (newMode !=
_mode) {
136 PX4_INFO(
"changing from mode %d -> %d", static_cast<int>(
_mode), static_cast<int>(newMode));
151 case Mode::SuperLowLight:
163 PX4_INFO(
"Resolution: %X", resolution);
164 PX4_INFO(
"Resolution is approx: %.3f", (
double)((resolution + 1.0
f) * (50.0
f / 8450.0
f)));
521 transfer(&cmd[0], &cmd[0],
sizeof(cmd));
532 transfer(&cmd[0],
nullptr,
sizeof(cmd));
541 struct TransferBuffer {
546 static_assert(
sizeof(buf) == (12 + 1));
550 if (transfer((uint8_t *)&buf, (uint8_t *)&buf,
sizeof(buf)) != PX4_OK) {
561 _previous_collect_timestamp = timestamp_sample;
576 const int16_t delta_x_raw = ((int16_t)buf.data.Delta_X_H << 8) | buf.data.Delta_X_L;
577 const int16_t delta_y_raw = ((int16_t)buf.data.Delta_Y_H << 8) | buf.data.Delta_Y_L;
584 const uint16_t shutter = (buf.data.Shutter_Upper << 8) | buf.data.Shutter_Lower;
586 if ((buf.data.SQUAL < 0x19) && (shutter >= 0x0BC0)) {
587 PX4_ERR(
"false motion report, discarding");
594 if ((shutter >= 0x1FFE) && (buf.data.RawData_Sum < 0x3C)) {
602 if ((shutter >= 0x1FFE) && (buf.data.RawData_Sum < 0x5A)) {
606 }
else if ((shutter >= 0x0BB8)) {
613 case Mode::SuperLowLight:
616 if ((shutter >= 0x03E8)) {
621 if (shutter >= 0x01F4) {
647 report.pixel_flow_x_integral = (float)
_flow_sum_x / 500.0
f;
648 report.pixel_flow_y_integral = (float)
_flow_sum_y / 500.0
f;
651 float zeroval = 0.0f;
652 rotate_3f(_yaw_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
657 report.sensor_id = 0;
658 report.quality = buf.data.SQUAL;
661 report.gyro_x_rate_integral = NAN;
662 report.gyro_y_rate_integral = NAN;
663 report.gyro_z_rate_integral = NAN;
666 report.max_flow_rate = 5.0f;
667 report.min_ground_distance = 0.08f;
668 report.max_ground_distance = 30.0f;
#define PARAM_INVALID
Handle returned when a parameter cannot be found.
static constexpr uint64_t T_SRR
uint64_t _previous_collect_timestamp
void registerWrite(uint8_t reg, uint8_t data)
static constexpr uint64_t T_SWW
static constexpr uint32_t SAMPLE_INTERVAL_MODE_2
measure the time elapsed performing an event
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
perf_counter_t _sample_perf
__EXPORT void rotate_3f(enum Rotation rot, float &x, float &y, float &z)
rotate a 3 element float vector in-place
static constexpr uint32_t SAMPLE_INTERVAL_MODE_0
static constexpr uint32_t SAMPLE_INTERVAL_MODE_1
uint64_t _flow_dt_sum_usec
perf_counter_t _comms_errors
bool changeMode(Mode newMode)
count the number of times an event occurs
uORB::PublicationMulti< optical_flow_s > _optical_flow_pub
perf_counter_t _dupe_count_perf
#define PAW3902_SPI_BUS_SPEED
void perf_count(perf_counter_t handle)
Count a performance event.
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
Rotation
Enum for board and external compass rotations.
unsigned _frame_count_since_last
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
void perf_end(perf_counter_t handle)
End a performance event.
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
PAW3902(int bus=PAW3902_BUS, enum Rotation yaw_rotation=ROTATION_NONE)
static constexpr uint8_t PRODUCT_ID
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
bool publish(const T &data)
Publish the struct.
uint8_t registerRead(uint8_t reg)
static constexpr uint64_t _collect_time
void perf_begin(perf_counter_t handle)
Begin a performance event.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uint32_t param_t
Parameter handle.
static constexpr uint8_t REVISION_ID