38#include "pprzlink/messages.h"
48#ifndef PX4FLOW_QUALITY_THRESHOLD
49#define PX4FLOW_QUALITY_THRESHOLD 0.1
56#define PX4FLOW_I2C_FRAME 0x0
57#define PX4FLOW_I2C_INTEGRAL_FRAME 0x16
58#define PX4FLOW_I2C_FRAME_LENGTH 22
59#define PX4FLOW_I2C_INTEGRAL_FRAME_LENGTH 25
60#define PX4FLOW_I2C_ID 0x4D
67 static float quality = 0;
68 static float noise = 0;
70 quality = ((float)
px4flow.i2c_frame.qual) / 255.0;
72 noise = noise * noise;
74 static float timestamp = 0;
77 static float flow_comp_m_x = 0.0;
78 static float flow_comp_m_y = 0.0;
82 time_usec = (
uint32_t)(timestamp * 1e6);
84 flow_comp_m_x = ((float)
px4flow.i2c_frame.flow_comp_m_x) / 1000.0;
85 flow_comp_m_y = ((float)
px4flow.i2c_frame.flow_comp_m_y) / 1000.0;
100 static int32_t ground_distance = 0;
101 static float ground_distance_float = 0.0;
105 ground_distance_float = ((float)ground_distance) / 1000.0;
108 if (
px4flow.compensate_rotation) {
111 float gain = (float)fabs( (
double) (cosf(phi) * cosf(theta)));
112 ground_distance_float = ground_distance_float / gain;
137 px4flow.addr = PX4FLOW_I2C_ADDR;
143 px4flow.update_agl = USE_PX4FLOW_AGL;
144 px4flow.compensate_rotation = PX4FLOW_COMPENSATE_ROTATION;
145 px4flow.stddev = PX4FLOW_NOISE_STDDEV;
270 static uint8_t distance_quality = 0;
276 float flow_comp_m_x = 0.0;
277 float flow_comp_m_y = 0.0;
280 float ground_distance = ((float)
px4flow.i2c_int_frame.ground_distance) / 1000.0;
286 float flow_comp_m_x = ((float)
px4flow.i2c_frame.flow_comp_m_x) / 1000.0;
287 float flow_comp_m_y = ((float)
px4flow.i2c_frame.flow_comp_m_y) / 1000.0;
290 float ground_distance = ((float)
px4flow.i2c_frame.ground_distance) / 1000.0;
Main include for ABI (AirBorneInterface).
#define AGL_SONAR_PX4FLOW_ID
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Common code for AP and FBW telemetry.
bool i2c_transmit(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len)
Submit a write only transaction.
bool i2c_transceive(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len_w, uint16_t len_r)
Submit a write/read transaction.
@ I2CTransSuccess
transaction successfully finished by I2C driver
@ I2CTransDone
transaction set to done by user level
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
#define PX4FLOW_QUALITY_THRESHOLD
void px4flow_i2c_periodic(void)
Poll px4flow for data 152 i2c frames are created per second, so the PX4FLOW can be polled at up to 15...
#define PX4FLOW_I2C_INTEGRAL_FRAME_LENGTH
struct MedianFilterInt sonar_filter
#define PX4FLOW_I2C_INTEGRAL_FRAME
void px4flow_i2c_downlink(void)
Downlink message for debug Copy volatile variables from the px4_i2c_frame for safety.
void px4flow_i2c_init(void)
Initialization function.
#define PX4FLOW_I2C_FRAME_LENGTH
struct px4flow_data px4flow
static void px4flow_i2c_int_frame_cb(void)
Propagate itegral frame.
static void px4flow_i2c_frame_cb(void)
Propagate optical flow information.
#define PX4FLOW_I2C_FRAME
@ PX4FLOW_INT_FRAME_REC_OK
API to get/set the generic vehicle states.
static float get_sys_time_float(void)
Get the time in seconds since startup.
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.