Paparazzi UAS
v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
#include "modules/optical_flow/px4flow_i2c.h"
#include "modules/core/abi.h"
#include "filters/median_filter.h"
#include "state.h"
#include "pprzlink/messages.h"
#include "modules/datalink/downlink.h"
Go to the source code of this file.
Macros | |
#define | PX4FLOW_QUALITY_THRESHOLD 0.1 |
#define | PX4FLOW_I2C_FRAME 0x0 |
#define | PX4FLOW_I2C_INTEGRAL_FRAME 0x16 |
#define | PX4FLOW_I2C_FRAME_LENGTH 22 |
#define | PX4FLOW_I2C_INTEGRAL_FRAME_LENGTH 25 |
#define | PX4FLOW_I2C_ID 0x4D |
Functions | |
static void | px4flow_i2c_frame_cb (void) |
Propagate optical flow information. More... | |
static void | px4flow_i2c_int_frame_cb (void) |
Propagate itegral frame. More... | |
void | px4flow_i2c_init (void) |
Initialization function. More... | |
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 150Hz (faster rate won't bring any finer resolution) More... | |
void | px4flow_i2c_downlink (void) |
Downlink message for debug Copy volatile variables from the px4_i2c_frame for safety. More... | |
Variables | |
struct px4flow_data | px4flow |
struct MedianFilterInt | sonar_filter |
#define PX4FLOW_I2C_FRAME 0x0 |
Definition at line 56 of file px4flow_i2c.c.
#define PX4FLOW_I2C_FRAME_LENGTH 22 |
Definition at line 58 of file px4flow_i2c.c.
#define PX4FLOW_I2C_ID 0x4D |
Definition at line 60 of file px4flow_i2c.c.
#define PX4FLOW_I2C_INTEGRAL_FRAME 0x16 |
Definition at line 57 of file px4flow_i2c.c.
#define PX4FLOW_I2C_INTEGRAL_FRAME_LENGTH 25 |
Definition at line 59 of file px4flow_i2c.c.
#define PX4FLOW_QUALITY_THRESHOLD 0.1 |
Definition at line 49 of file px4flow_i2c.c.
void px4flow_i2c_downlink | ( | void | ) |
Downlink message for debug Copy volatile variables from the px4_i2c_frame for safety.
Definition at line 264 of file px4flow_i2c.c.
References DefaultChannel, DefaultDevice, px4flow_i2c_frame::flow_comp_m_x, px4flow_i2c_frame::flow_comp_m_y, get_sys_time_float(), px4flow_i2c_frame::ground_distance, px4flow_i2c_integral_frame::ground_distance, px4flow_data::i2c_frame, px4flow_data::i2c_int_frame, px4flow_i2c_integral_frame::pixel_flow_x_integral, px4flow_i2c_frame::pixel_flow_x_sum, px4flow_i2c_integral_frame::pixel_flow_y_integral, px4flow_i2c_frame::pixel_flow_y_sum, px4flow, PX4FLOW_I2C_ID, px4flow_i2c_frame::qual, and px4flow_i2c_integral_frame::qual.
|
inlinestatic |
Propagate optical flow information.
Definition at line 65 of file px4flow_i2c.c.
References AGL_SONAR_PX4FLOW_ID, px4flow_data::compensate_rotation, px4flow_i2c_frame::flow_comp_m_x, px4flow_i2c_frame::flow_comp_m_y, get_sys_time_float(), get_sys_time_usec(), px4flow_i2c_frame::ground_distance, px4flow_data::i2c_frame, if(), FloatEulers::phi, px4flow, PX4FLOW_QUALITY_THRESHOLD, px4flow_i2c_frame::qual, sonar_filter, stateGetNedToBodyEulers_f(), px4flow_data::stddev, FloatEulers::theta, px4flow_data::update_agl, update_median_filter_i(), and VEL_PX4FLOW_ID.
Referenced by px4flow_i2c_periodic().
void px4flow_i2c_init | ( | void | ) |
Initialization function.
Definition at line 134 of file px4flow_i2c.c.
References px4flow_data::addr, px4flow_data::compensate_rotation, I2CTransDone, init_median_filter_i(), px4flow, PX4FLOW_FRAME_REQ, PX4FLOW_INT_FRAME_REQ, sonar_filter, i2c_transaction::status, px4flow_data::status, px4flow_data::stddev, px4flow_data::trans, and px4flow_data::update_agl.
|
inlinestatic |
Propagate itegral frame.
Definition at line 125 of file px4flow_i2c.c.
Referenced by px4flow_i2c_periodic().
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 150Hz (faster rate won't bring any finer resolution)
Definition at line 155 of file px4flow_i2c.c.
References px4flow_data::addr, i2c_transaction::buf, px4flow_i2c_frame::flow_comp_m_x, px4flow_i2c_frame::flow_comp_m_y, px4flow_i2c_frame::frame_count, px4flow_i2c_integral_frame::frame_count_since_last_readout, px4flow_i2c_frame::ground_distance, px4flow_i2c_integral_frame::ground_distance, px4flow_i2c_frame::gyro_range, px4flow_i2c_integral_frame::gyro_temperature, px4flow_i2c_frame::gyro_x_rate, px4flow_i2c_integral_frame::gyro_x_rate_integral, px4flow_i2c_frame::gyro_y_rate, px4flow_i2c_integral_frame::gyro_y_rate_integral, px4flow_i2c_frame::gyro_z_rate, px4flow_i2c_integral_frame::gyro_z_rate_integral, px4flow_data::i2c_frame, px4flow_data::i2c_int_frame, i2c_transceive(), i2c_transmit(), I2CTransSuccess, idx, px4flow_i2c_integral_frame::integration_timespan, px4flow_i2c_integral_frame::pixel_flow_x_integral, px4flow_i2c_frame::pixel_flow_x_sum, px4flow_i2c_integral_frame::pixel_flow_y_integral, px4flow_i2c_frame::pixel_flow_y_sum, px4flow, PX4FLOW_FRAME_REC, PX4FLOW_FRAME_REQ, PX4FLOW_I2C_FRAME, px4flow_i2c_frame_cb(), PX4FLOW_I2C_FRAME_LENGTH, px4flow_i2c_int_frame_cb(), PX4FLOW_I2C_INTEGRAL_FRAME, PX4FLOW_I2C_INTEGRAL_FRAME_LENGTH, PX4FLOW_INT_FRAME_REC, PX4FLOW_INT_FRAME_REC_OK, PX4FLOW_INT_FRAME_REQ, px4flow_i2c_frame::qual, px4flow_i2c_integral_frame::qual, px4flow_i2c_frame::sonar_timestamp, px4flow_i2c_integral_frame::sonar_timestamp, i2c_transaction::status, px4flow_data::status, and px4flow_data::trans.
struct px4flow_data px4flow |
Definition at line 1 of file px4flow_i2c.c.
Referenced by px4flow_i2c_downlink(), px4flow_i2c_frame_cb(), px4flow_i2c_init(), and px4flow_i2c_periodic().
struct MedianFilterInt sonar_filter |
Definition at line 1 of file px4flow_i2c.c.
Referenced by px4flow_i2c_frame_cb(), and px4flow_i2c_init().