 |
Paparazzi UAS
v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
Go to the documentation of this file.
30 #include "generated/modules.h"
39 #ifndef OPTICFLOW_PMW3901_SENSOR_ANGLE
40 #define OPTICFLOW_PMW3901_SENSOR_ANGLE 270 // [deg] Sensor rotation around body z axis (down). 0 rad = sensor x forward, y right.
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 // [px] standard deviation of flow measurement
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;
94 float flow_x = (c * delta_x -
s * delta_y) / dt;
95 float flow_y = (
s * delta_x + c * delta_y) / dt;
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;
175 #ifdef OPTICFLOW_PMW3901_RAD_PER_PX
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
#define VEL_OPTICFLOW_PMW3901_ID
static bool agl_valid(uint32_t at_ts)
Event structure to store callbacks in a linked list.
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
bool pmw3901_get_data(struct pmw3901_t *pmw, int16_t *delta_x, int16_t *delta_y)
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
short int16_t
Typedef defining 16 bit short type.
bool pmw3901_data_available(struct pmw3901_t *pmw)
bool pmw3901_is_idle(struct pmw3901_t *pmw)
void pmw3901_start_read(struct pmw3901_t *pmw)
#define FLOW_OPTICFLOW_PMW3901_ID
void pmw3901_event(struct pmw3901_t *pmw)
#define OPTICFLOW_PMW3901_SUBPIXEL_FACTOR
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Common code for AP and FBW telemetry.
void pmw3901_init(struct pmw3901_t *pmw, struct spi_periph *periph, uint8_t slave_idx)
#define OPTICFLOW_PMW3901_AGL_ID
void opticflow_pmw3901_event(void)
void opticflow_pmw3901_periodic(void)
static void opticflow_pmw3901_publish(int16_t delta_x, int16_t delta_y, uint32_t ts_usec)
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
struct opticflow_pmw3901_t of_pmw
#define OPTICFLOW_PMW3901_AGL_TIMEOUT_US
#define OPTICFLOW_PMW3901_STD_PX
#define OPTICFLOW_PMW3901_SENSOR_ANGLE
static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance)
void opticflow_pmw3901_init(void)