30#include "generated/modules.h"
39#ifndef OPTICFLOW_PMW3901_SENSOR_ANGLE
40#define OPTICFLOW_PMW3901_SENSOR_ANGLE 270
43#ifndef OPTICFLOW_PMW3901_SUBPIXEL_FACTOR
44#define OPTICFLOW_PMW3901_SUBPIXEL_FACTOR 100
47#ifndef OPTICFLOW_PMW3901_STD_PX
48#define OPTICFLOW_PMW3901_STD_PX 50
51#ifndef OPTICFLOW_PMW3901_AGL_ID
52#define OPTICFLOW_PMW3901_AGL_ID ABI_BROADCAST
56#ifndef OPTICFLOW_PMW3901_AGL_TIMEOUT_US
57#define OPTICFLOW_PMW3901_AGL_TIMEOUT_US 500000
85 float dt = (ts_usec - prev_ts_usec) / 1.0e6;
86 if (prev_ts_usec == 0) {
87 dt = OPTICFLOW_PMW3901_PERIODIC_PERIOD;
89 prev_ts_usec = ts_usec;
91 float c = cosf(RadOfDeg(
of_pmw.sensor_angle));
92 float s = sinf(RadOfDeg(
of_pmw.sensor_angle));
94 float flow_x = (c * delta_x -
s * delta_y) / dt;
95 float flow_y = (
s * delta_x + c * delta_y) / dt;
100 float flow_dy_p = rates->
p /
of_pmw.pmw.rad_per_px;
101 float flow_dx_q = -rates->
q /
of_pmw.pmw.rad_per_px;
102 float flow_der_x = flow_x - flow_dx_q;
103 float flow_der_y = flow_y - flow_dy_p;
107 static float vel_x = 0;
108 static float vel_y = 0;
109 static float noise = 0;
143#if SENSOR_SYNC_SEND_OPTICFLOW_PMW3901
147 float fps = 1.f / dt;
169 pmw3901_init(&
of_pmw.pmw, &OPTICFLOW_PMW3901_SPI_DEV, OPTICFLOW_PMW3901_SPI_SLAVE_IDX);
175#ifdef OPTICFLOW_PMW3901_RAD_PER_PX
176 of_pmw.pmw.rad_per_px = OPTICFLOW_PMW3901_RAD_PER_PX;
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
#define FLOW_OPTICFLOW_PMW3901_ID
#define VEL_OPTICFLOW_PMW3901_ID
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
void pprz_bsem_signal(pprz_bsem_t *bsem)
Common code for AP and FBW telemetry.
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance)
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
static abi_event agl_ev
The altitude ABI event.
#define OPTICFLOW_PMW3901_SENSOR_ANGLE
#define OPTICFLOW_PMW3901_SUBPIXEL_FACTOR
void opticflow_pmw3901_init(void)
struct opticflow_pmw3901_t of_pmw
#define OPTICFLOW_PMW3901_STD_PX
void opticflow_pmw3901_periodic(void)
static void opticflow_pmw3901_publish(int16_t delta_x, int16_t delta_y, uint32_t ts_usec)
static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance)
#define OPTICFLOW_PMW3901_AGL_TIMEOUT_US
void opticflow_pmw3901_event(void)
static bool agl_valid(uint32_t at_ts)
#define OPTICFLOW_PMW3901_AGL_ID
void pmw3901_init(struct pmw3901_t *pmw, struct spi_periph *periph, uint8_t slave_idx)
bool pmw3901_get_data(struct pmw3901_t *pmw, int16_t *delta_x, int16_t *delta_y)
API to get/set the generic vehicle states.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.