altb_pmic
/
Test_optical_flow_PX4
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
Diff: PX4Flow.cpp
- Revision:
- 9:be40afb750d3
- Parent:
- 0:4b02060af95b
- Child:
- 11:d367224f2194
diff -r cb6d8bc54aaf -r be40afb750d3 PX4Flow.cpp
--- a/PX4Flow.cpp Fri Sep 06 09:32:37 2019 +0000
+++ b/PX4Flow.cpp Fri Sep 13 10:43:57 2019 +0000
@@ -1,6 +1,6 @@
#include "PX4Flow.h"
-PX4Flow::PX4Flow( I2C& i2c, Serial& pc ): i2c(i2c), pc(pc)
+PX4Flow::PX4Flow( I2C& i2c, Serial& pc ): i2c(i2c)
{
i2c_commands[0] = FRAME;
i2c_commands[1] = INTEGRAL_FRAME;
@@ -13,69 +13,54 @@
//send 0x0 to PX4FLOW module and receive back 22 Bytes data
if( i2c.write(PX4FLOW_ADDRESS, &i2c_commands[0], 1 ) == 0) {
if(i2c.read(PX4FLOW_ADDRESS, bufferS, 22 ) == 0 ) {
-
// assign the data
- frame.frame_count = static_cast<uint16_t>(read16(bufferS, FRAME_COUNT));
- frame.pixel_flow_x_sum = static_cast<int16_t>(read16(bufferS, PIXEL_FLOW_X_SUM));
- frame.pixel_flow_y_sum = static_cast<int16_t>(read16(bufferS, PIXEL_FLOW_Y_SUM));
- frame.flow_comp_m_x = static_cast<int16_t>(read16(bufferS, FLOW_COMP_M_X));
- frame.flow_comp_m_y = static_cast<int16_t>(read16(bufferS, FLOW_COMP_M_Y));
- frame.qual = static_cast<int16_t>(read16(bufferS, QUAL));
- frame.gyro_x_rate = static_cast<int16_t>(read16(bufferS, GYRO_X_RATE));
- frame.gyro_y_rate = static_cast<int16_t>(read16(bufferS, GYRO_Y_RATE));
- frame.gyro_z_rate = static_cast<int16_t>(read16(bufferS, GYRO_Z_RATE));
- frame.gyro_range = static_cast<uint8_t>(read8(bufferS, GYRO_RANGE));
- frame.sonar_timestamp = static_cast<uint8_t>(read8(bufferS, SONAR_TIMESTAMP));
- frame.ground_distance = static_cast<uint16_t>(read16(bufferS, GROUND_DISTANCE));
-
+ frame.frame_count = (uint16_t)(read16(bufferS, FRAME_COUNT));
+ frame.pixel_flow_x_sum = (int16_t) (read16(bufferS, PIXEL_FLOW_X_SUM));
+ frame.pixel_flow_y_sum = (int16_t) (read16(bufferS, PIXEL_FLOW_Y_SUM));
+ frame.flow_comp_m_x = (int16_t) (read16(bufferS, FLOW_COMP_M_X));
+ frame.flow_comp_m_y = (int16_t) (read16(bufferS, FLOW_COMP_M_Y));
+ frame.qual = (int16_t) (read16(bufferS, QUAL));
+ frame.gyro_x_rate = (int16_t) (read16(bufferS, GYRO_X_RATE));
+ frame.gyro_y_rate = (int16_t) (read16(bufferS, GYRO_Y_RATE));
+ frame.gyro_z_rate = (int16_t) (read16(bufferS, GYRO_Z_RATE));
+ frame.gyro_range = (uint8_t) (read8(bufferS, GYRO_RANGE));
+ frame.sonar_timestamp = (uint8_t) (read8(bufferS, SONAR_TIMESTAMP));
+ frame.ground_distance = (uint16_t)(read16(bufferS, GROUND_DISTANCE));
return true;
-
} else {
- printf("Error: reading I2C\r\n");
return false;
}
} else {
- pc.printf("Error: writing I2C\r\n");
return false;
}
-
}
bool PX4Flow::update_integral()
{
-
//send 0x16 to PX4FLOW module and receive back 26 Bytes data
if( i2c.write(PX4FLOW_ADDRESS, &i2c_commands[1], 1 ) == 0 ) {
-
- int ack = i2c.read(PX4FLOW_ADDRESS, bufferI, 26 );
- if( ack == 0 ) {
-
+ if(i2c.read(PX4FLOW_ADDRESS, bufferI, 26 ) == 0 ) {
// assign the data
- iframe.frame_count_since_last_readout = static_cast<uint16_t>(read16(bufferI, FRAME_COUNT_SINCE_LAST_READOUT));
- iframe.pixel_flow_x_integral = static_cast<int16_t> (read16(bufferI, PIXEL_FLOW_X_INTEGRAL));
- iframe.pixel_flow_y_integral = static_cast<int16_t> (read16(bufferI, PIXEL_FLOW_Y_INTEGRAL));
- iframe.gyro_x_rate_integral = static_cast<int16_t> (read16(bufferI, GYRO_X_RATE_INTEGRAL));
- iframe.gyro_y_rate_integral = static_cast<int16_t> (read16(bufferI, GYRO_Y_RATE_INTEGRAL));
- iframe.gyro_z_rate_integral = static_cast<int16_t> (read16(bufferI, GYRO_Z_RATE_INTEGRAL));
- iframe.integration_timespan = static_cast<uint32_t>(read32(bufferI, INTEGRATION_TIMESPAN));
- iframe.sonar_timestamp = static_cast<uint32_t>(read32(bufferI, SONAR_TIMESTAMP_INTEGRAL));
- iframe.ground_distance = static_cast<int16_t> (read16(bufferI, GROUND_DISTANCE_INTEGRAL));
- iframe.gyro_temperature = static_cast<int16_t> (read16(bufferI, GYRO_TEMPERATURE));
- iframe.quality = static_cast<uint8_t> (read8(bufferI, QUALITY_INTEGRAL));
-
+ iframe.frame_count_since_last_readout = (uint16_t)(read16(bufferI, FRAME_COUNT_SINCE_LAST_READOUT));
+ iframe.pixel_flow_x_integral = (int16_t) (read16(bufferI, PIXEL_FLOW_X_INTEGRAL));
+ iframe.pixel_flow_y_integral = (int16_t) (read16(bufferI, PIXEL_FLOW_Y_INTEGRAL));
+ iframe.gyro_x_rate_integral = (int16_t) (read16(bufferI, GYRO_X_RATE_INTEGRAL));
+ iframe.gyro_y_rate_integral = (int16_t) (read16(bufferI, GYRO_Y_RATE_INTEGRAL));
+ iframe.gyro_z_rate_integral = (int16_t) (read16(bufferI, GYRO_Z_RATE_INTEGRAL));
+ iframe.integration_timespan = (uint32_t)(read32(bufferI, INTEGRATION_TIMESPAN));
+ iframe.sonar_timestamp = (uint32_t)(read32(bufferI, SONAR_TIMESTAMP_INTEGRAL));
+ iframe.ground_distance = (int16_t) (read16(bufferI, GROUND_DISTANCE_INTEGRAL));
+ iframe.gyro_temperature = (int16_t) (read16(bufferI, GYRO_TEMPERATURE));
+ iframe.quality = (uint8_t) (read8(bufferI, QUALITY_INTEGRAL));
return true;
-
} else {
- pc.printf("Error: reading I2C\r\n");
return false;
}
} else {
- pc.printf("Error: writing I2C\r\n");
return false;
}
}
-
// Methods to return the sensordata from datastructure
// Simple frame
uint16_t PX4Flow::frame_count()
@@ -138,7 +123,6 @@
return frame.ground_distance;
}
-
// Integral frame
uint16_t PX4Flow::frame_count_since_last_readout()
{
@@ -195,6 +179,73 @@
return iframe.quality;
}
+/* IndNav Git-branch modifications, start here */
+// Wrapper functions to read modified firmware data
+float PX4Flow::avg_flowx()
+{
+ return (float)iframe.pixel_flow_x_integral*0.3333333f; // avg flow x in mm/s
+}
+
+float PX4Flow::avg_flowy()
+{
+ return (float)iframe.pixel_flow_y_integral*0.3333333f; // avg flow y in mm/s
+}
+
+uint8_t PX4Flow::valid_frame_count()
+{
+ return (uint8_t)iframe.frame_count_since_last_readout; // nr. of valid frames (qual > 0) between i2c readings
+}
+
+uint8_t PX4Flow::all_frame_count()
+{
+ return (uint8_t)iframe.sonar_timestamp; // nr. of frames between i2c readings
+}
+
+float PX4Flow::update_fs()
+{
+ return (float)iframe.integration_timespan*0.001f; // i2c averaging update rate in Hz
+}
+
+float PX4Flow::readout_fs()
+{
+ return (float)iframe.ground_distance*0.001f; // i2c readout update rate in Hz
+}
+
+uint8_t PX4Flow::avg_quality()
+{
+ return iframe.quality; // avg 0-255 linear quality measurement 0=bad, 255=best
+}
+
+float PX4Flow::int_timespan()
+{
+ return (float)iframe.gyro_temperature*0.01f; // integration timespan in ms, now you can integrate flow values, if valid...
+}
+
+float PX4Flow::avg_gyro_x()
+{
+ return (float)iframe.gyro_x_rate_integral*0.00064516132f; // avg gyro x in rad/s
+}
+
+float PX4Flow::avg_gyro_y()
+{
+ return (float)iframe.gyro_y_rate_integral*0.00064516132f; // avg gyro y in rad/s
+}
+
+float PX4Flow::avg_gyro_z()
+{
+ return (float)iframe.gyro_z_rate_integral*0.00064516132f; // avg gyro z in rad/s
+}
+
+uint8_t PX4Flow::avg_scaled_quality(){
+
+ uint8_t avg_scaled_quality = 0;
+ /* valid_frame_count := iframe.frame_count_since_last_readout */
+ if(iframe.frame_count_since_last_readout > 0) {
+ avg_scaled_quality = (uint8_t)( (float)iframe.quality * ( (float)iframe.frame_count_since_last_readout / (float)iframe.sonar_timestamp ) ); // avg 0-255 linear quality measurement 0=bad, 255=best, scaled with N_valid/N_frames
+ }
+ return avg_scaled_quality;
+}
+/* IndNav Git-branch modifications, end here */
uint8_t PX4Flow::read8(char *buffer, const unsigned int& idx)
{