altb_pmic / Mbed 2 deprecated 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

PX4Flow.cpp@9:be40afb750d3, 2019-09-13 (annotated)

Committer:
pmic
Date:
Fri Sep 13 10:43:57 2019 +0000
Revision:
9:be40afb750d3
Parent:
0:4b02060af95b
Child:
11:d367224f2194
Implement wrapper functions for PX4Flow class according to adjusted PX4Flow firmware (Git-branch: IndNav). Use the PX4Flow Sensor not with a update frequency < 10 Hz. 20 Hz is recommended.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 0:4b02060af95b 1 #include "PX4Flow.h"
altb2 0:4b02060af95b 2
pmic 9:be40afb750d3 3 PX4Flow::PX4Flow( I2C& i2c, Serial& pc ): i2c(i2c)
altb2 0:4b02060af95b 4 {
altb2 0:4b02060af95b 5 i2c_commands[0] = FRAME;
altb2 0:4b02060af95b 6 i2c_commands[1] = INTEGRAL_FRAME;
altb2 0:4b02060af95b 7 }
altb2 0:4b02060af95b 8
altb2 0:4b02060af95b 9 PX4Flow::~PX4Flow() {}
altb2 0:4b02060af95b 10
altb2 0:4b02060af95b 11 bool PX4Flow::update()
altb2 0:4b02060af95b 12 {
altb2 0:4b02060af95b 13 //send 0x0 to PX4FLOW module and receive back 22 Bytes data
altb2 0:4b02060af95b 14 if( i2c.write(PX4FLOW_ADDRESS, &i2c_commands[0], 1 ) == 0) {
altb2 0:4b02060af95b 15 if(i2c.read(PX4FLOW_ADDRESS, bufferS, 22 ) == 0 ) {
altb2 0:4b02060af95b 16 // assign the data
pmic 9:be40afb750d3 17 frame.frame_count = (uint16_t)(read16(bufferS, FRAME_COUNT));
pmic 9:be40afb750d3 18 frame.pixel_flow_x_sum = (int16_t) (read16(bufferS, PIXEL_FLOW_X_SUM));
pmic 9:be40afb750d3 19 frame.pixel_flow_y_sum = (int16_t) (read16(bufferS, PIXEL_FLOW_Y_SUM));
pmic 9:be40afb750d3 20 frame.flow_comp_m_x = (int16_t) (read16(bufferS, FLOW_COMP_M_X));
pmic 9:be40afb750d3 21 frame.flow_comp_m_y = (int16_t) (read16(bufferS, FLOW_COMP_M_Y));
pmic 9:be40afb750d3 22 frame.qual = (int16_t) (read16(bufferS, QUAL));
pmic 9:be40afb750d3 23 frame.gyro_x_rate = (int16_t) (read16(bufferS, GYRO_X_RATE));
pmic 9:be40afb750d3 24 frame.gyro_y_rate = (int16_t) (read16(bufferS, GYRO_Y_RATE));
pmic 9:be40afb750d3 25 frame.gyro_z_rate = (int16_t) (read16(bufferS, GYRO_Z_RATE));
pmic 9:be40afb750d3 26 frame.gyro_range = (uint8_t) (read8(bufferS, GYRO_RANGE));
pmic 9:be40afb750d3 27 frame.sonar_timestamp = (uint8_t) (read8(bufferS, SONAR_TIMESTAMP));
pmic 9:be40afb750d3 28 frame.ground_distance = (uint16_t)(read16(bufferS, GROUND_DISTANCE));
altb2 0:4b02060af95b 29 return true;
altb2 0:4b02060af95b 30 } else {
altb2 0:4b02060af95b 31 return false;
altb2 0:4b02060af95b 32 }
altb2 0:4b02060af95b 33 } else {
altb2 0:4b02060af95b 34 return false;
altb2 0:4b02060af95b 35 }
altb2 0:4b02060af95b 36 }
altb2 0:4b02060af95b 37
altb2 0:4b02060af95b 38 bool PX4Flow::update_integral()
altb2 0:4b02060af95b 39 {
altb2 0:4b02060af95b 40 //send 0x16 to PX4FLOW module and receive back 26 Bytes data
altb2 0:4b02060af95b 41 if( i2c.write(PX4FLOW_ADDRESS, &i2c_commands[1], 1 ) == 0 ) {
pmic 9:be40afb750d3 42 if(i2c.read(PX4FLOW_ADDRESS, bufferI, 26 ) == 0 ) {
altb2 0:4b02060af95b 43 // assign the data
pmic 9:be40afb750d3 44 iframe.frame_count_since_last_readout = (uint16_t)(read16(bufferI, FRAME_COUNT_SINCE_LAST_READOUT));
pmic 9:be40afb750d3 45 iframe.pixel_flow_x_integral = (int16_t) (read16(bufferI, PIXEL_FLOW_X_INTEGRAL));
pmic 9:be40afb750d3 46 iframe.pixel_flow_y_integral = (int16_t) (read16(bufferI, PIXEL_FLOW_Y_INTEGRAL));
pmic 9:be40afb750d3 47 iframe.gyro_x_rate_integral = (int16_t) (read16(bufferI, GYRO_X_RATE_INTEGRAL));
pmic 9:be40afb750d3 48 iframe.gyro_y_rate_integral = (int16_t) (read16(bufferI, GYRO_Y_RATE_INTEGRAL));
pmic 9:be40afb750d3 49 iframe.gyro_z_rate_integral = (int16_t) (read16(bufferI, GYRO_Z_RATE_INTEGRAL));
pmic 9:be40afb750d3 50 iframe.integration_timespan = (uint32_t)(read32(bufferI, INTEGRATION_TIMESPAN));
pmic 9:be40afb750d3 51 iframe.sonar_timestamp = (uint32_t)(read32(bufferI, SONAR_TIMESTAMP_INTEGRAL));
pmic 9:be40afb750d3 52 iframe.ground_distance = (int16_t) (read16(bufferI, GROUND_DISTANCE_INTEGRAL));
pmic 9:be40afb750d3 53 iframe.gyro_temperature = (int16_t) (read16(bufferI, GYRO_TEMPERATURE));
pmic 9:be40afb750d3 54 iframe.quality = (uint8_t) (read8(bufferI, QUALITY_INTEGRAL));
altb2 0:4b02060af95b 55 return true;
altb2 0:4b02060af95b 56 } else {
altb2 0:4b02060af95b 57 return false;
altb2 0:4b02060af95b 58 }
altb2 0:4b02060af95b 59 } else {
altb2 0:4b02060af95b 60 return false;
altb2 0:4b02060af95b 61 }
altb2 0:4b02060af95b 62 }
altb2 0:4b02060af95b 63
altb2 0:4b02060af95b 64 // Methods to return the sensordata from datastructure
altb2 0:4b02060af95b 65 // Simple frame
altb2 0:4b02060af95b 66 uint16_t PX4Flow::frame_count()
altb2 0:4b02060af95b 67 {
altb2 0:4b02060af95b 68 return frame.frame_count;
altb2 0:4b02060af95b 69 }
altb2 0:4b02060af95b 70
altb2 0:4b02060af95b 71 int16_t PX4Flow::pixel_flow_x_sum()
altb2 0:4b02060af95b 72 {
altb2 0:4b02060af95b 73 return frame.pixel_flow_x_sum;
altb2 0:4b02060af95b 74 }
altb2 0:4b02060af95b 75
altb2 0:4b02060af95b 76 int16_t PX4Flow::pixel_flow_y_sum()
altb2 0:4b02060af95b 77 {
altb2 0:4b02060af95b 78 return frame.pixel_flow_y_sum;
altb2 0:4b02060af95b 79 }
altb2 0:4b02060af95b 80
altb2 0:4b02060af95b 81 int16_t PX4Flow::flow_comp_m_x()
altb2 0:4b02060af95b 82 {
altb2 0:4b02060af95b 83 return frame.flow_comp_m_x;
altb2 0:4b02060af95b 84 }
altb2 0:4b02060af95b 85
altb2 0:4b02060af95b 86 int16_t PX4Flow::flow_comp_m_y()
altb2 0:4b02060af95b 87 {
altb2 0:4b02060af95b 88 return frame.flow_comp_m_y;
altb2 0:4b02060af95b 89 }
altb2 0:4b02060af95b 90
altb2 0:4b02060af95b 91 int16_t PX4Flow::qual()
altb2 0:4b02060af95b 92 {
altb2 0:4b02060af95b 93 return frame.qual;
altb2 0:4b02060af95b 94 }
altb2 0:4b02060af95b 95
altb2 0:4b02060af95b 96 int16_t PX4Flow::gyro_x_rate()
altb2 0:4b02060af95b 97 {
altb2 0:4b02060af95b 98 return frame.gyro_x_rate;
altb2 0:4b02060af95b 99 }
altb2 0:4b02060af95b 100
altb2 0:4b02060af95b 101 int16_t PX4Flow::gyro_y_rate()
altb2 0:4b02060af95b 102 {
altb2 0:4b02060af95b 103 return frame.gyro_y_rate;
altb2 0:4b02060af95b 104 }
altb2 0:4b02060af95b 105
altb2 0:4b02060af95b 106 int16_t PX4Flow::gyro_z_rate()
altb2 0:4b02060af95b 107 {
altb2 0:4b02060af95b 108 return frame.gyro_z_rate;
altb2 0:4b02060af95b 109 }
altb2 0:4b02060af95b 110
altb2 0:4b02060af95b 111 uint8_t PX4Flow::gyro_range()
altb2 0:4b02060af95b 112 {
altb2 0:4b02060af95b 113 return frame.gyro_range;
altb2 0:4b02060af95b 114 }
altb2 0:4b02060af95b 115
altb2 0:4b02060af95b 116 uint8_t PX4Flow::sonar_timestamp()
altb2 0:4b02060af95b 117 {
altb2 0:4b02060af95b 118 return frame.sonar_timestamp;
altb2 0:4b02060af95b 119 }
altb2 0:4b02060af95b 120
altb2 0:4b02060af95b 121 int16_t PX4Flow::ground_distance()
altb2 0:4b02060af95b 122 {
altb2 0:4b02060af95b 123 return frame.ground_distance;
altb2 0:4b02060af95b 124 }
altb2 0:4b02060af95b 125
altb2 0:4b02060af95b 126 // Integral frame
altb2 0:4b02060af95b 127 uint16_t PX4Flow::frame_count_since_last_readout()
altb2 0:4b02060af95b 128 {
altb2 0:4b02060af95b 129 return iframe.frame_count_since_last_readout;
altb2 0:4b02060af95b 130 }
altb2 0:4b02060af95b 131
altb2 0:4b02060af95b 132 int16_t PX4Flow::pixel_flow_x_integral()
altb2 0:4b02060af95b 133 {
altb2 0:4b02060af95b 134 return iframe.pixel_flow_x_integral;
altb2 0:4b02060af95b 135 }
altb2 0:4b02060af95b 136
altb2 0:4b02060af95b 137 int16_t PX4Flow::pixel_flow_y_integral()
altb2 0:4b02060af95b 138 {
altb2 0:4b02060af95b 139 return iframe.pixel_flow_y_integral;
altb2 0:4b02060af95b 140 }
altb2 0:4b02060af95b 141
altb2 0:4b02060af95b 142 int16_t PX4Flow::gyro_x_rate_integral()
altb2 0:4b02060af95b 143 {
altb2 0:4b02060af95b 144 return iframe.gyro_x_rate_integral;
altb2 0:4b02060af95b 145 }
altb2 0:4b02060af95b 146
altb2 0:4b02060af95b 147 int16_t PX4Flow::gyro_y_rate_integral()
altb2 0:4b02060af95b 148 {
altb2 0:4b02060af95b 149 return iframe.gyro_y_rate_integral;
altb2 0:4b02060af95b 150 }
altb2 0:4b02060af95b 151
altb2 0:4b02060af95b 152 int16_t PX4Flow::gyro_z_rate_integral()
altb2 0:4b02060af95b 153 {
altb2 0:4b02060af95b 154 return iframe.gyro_z_rate_integral;
altb2 0:4b02060af95b 155 }
altb2 0:4b02060af95b 156
altb2 0:4b02060af95b 157 uint32_t PX4Flow::integration_timespan()
altb2 0:4b02060af95b 158 {
altb2 0:4b02060af95b 159 return iframe.integration_timespan;
altb2 0:4b02060af95b 160 }
altb2 0:4b02060af95b 161
altb2 0:4b02060af95b 162 uint32_t PX4Flow::sonar_timestamp_integral()
altb2 0:4b02060af95b 163 {
altb2 0:4b02060af95b 164 return iframe.sonar_timestamp;
altb2 0:4b02060af95b 165 }
altb2 0:4b02060af95b 166
altb2 0:4b02060af95b 167 int16_t PX4Flow::ground_distance_integral()
altb2 0:4b02060af95b 168 {
altb2 0:4b02060af95b 169 return iframe.ground_distance;
altb2 0:4b02060af95b 170 }
altb2 0:4b02060af95b 171
altb2 0:4b02060af95b 172 int16_t PX4Flow::gyro_temperature()
altb2 0:4b02060af95b 173 {
altb2 0:4b02060af95b 174 return iframe.gyro_temperature;
altb2 0:4b02060af95b 175 }
altb2 0:4b02060af95b 176
altb2 0:4b02060af95b 177 uint8_t PX4Flow::quality_integral()
altb2 0:4b02060af95b 178 {
altb2 0:4b02060af95b 179 return iframe.quality;
altb2 0:4b02060af95b 180 }
altb2 0:4b02060af95b 181
pmic 9:be40afb750d3 182 /* IndNav Git-branch modifications, start here */
pmic 9:be40afb750d3 183 // Wrapper functions to read modified firmware data
pmic 9:be40afb750d3 184 float PX4Flow::avg_flowx()
pmic 9:be40afb750d3 185 {
pmic 9:be40afb750d3 186 return (float)iframe.pixel_flow_x_integral*0.3333333f; // avg flow x in mm/s
pmic 9:be40afb750d3 187 }
pmic 9:be40afb750d3 188
pmic 9:be40afb750d3 189 float PX4Flow::avg_flowy()
pmic 9:be40afb750d3 190 {
pmic 9:be40afb750d3 191 return (float)iframe.pixel_flow_y_integral*0.3333333f; // avg flow y in mm/s
pmic 9:be40afb750d3 192 }
pmic 9:be40afb750d3 193
pmic 9:be40afb750d3 194 uint8_t PX4Flow::valid_frame_count()
pmic 9:be40afb750d3 195 {
pmic 9:be40afb750d3 196 return (uint8_t)iframe.frame_count_since_last_readout; // nr. of valid frames (qual > 0) between i2c readings
pmic 9:be40afb750d3 197 }
pmic 9:be40afb750d3 198
pmic 9:be40afb750d3 199 uint8_t PX4Flow::all_frame_count()
pmic 9:be40afb750d3 200 {
pmic 9:be40afb750d3 201 return (uint8_t)iframe.sonar_timestamp; // nr. of frames between i2c readings
pmic 9:be40afb750d3 202 }
pmic 9:be40afb750d3 203
pmic 9:be40afb750d3 204 float PX4Flow::update_fs()
pmic 9:be40afb750d3 205 {
pmic 9:be40afb750d3 206 return (float)iframe.integration_timespan*0.001f; // i2c averaging update rate in Hz
pmic 9:be40afb750d3 207 }
pmic 9:be40afb750d3 208
pmic 9:be40afb750d3 209 float PX4Flow::readout_fs()
pmic 9:be40afb750d3 210 {
pmic 9:be40afb750d3 211 return (float)iframe.ground_distance*0.001f; // i2c readout update rate in Hz
pmic 9:be40afb750d3 212 }
pmic 9:be40afb750d3 213
pmic 9:be40afb750d3 214 uint8_t PX4Flow::avg_quality()
pmic 9:be40afb750d3 215 {
pmic 9:be40afb750d3 216 return iframe.quality; // avg 0-255 linear quality measurement 0=bad, 255=best
pmic 9:be40afb750d3 217 }
pmic 9:be40afb750d3 218
pmic 9:be40afb750d3 219 float PX4Flow::int_timespan()
pmic 9:be40afb750d3 220 {
pmic 9:be40afb750d3 221 return (float)iframe.gyro_temperature*0.01f; // integration timespan in ms, now you can integrate flow values, if valid...
pmic 9:be40afb750d3 222 }
pmic 9:be40afb750d3 223
pmic 9:be40afb750d3 224 float PX4Flow::avg_gyro_x()
pmic 9:be40afb750d3 225 {
pmic 9:be40afb750d3 226 return (float)iframe.gyro_x_rate_integral*0.00064516132f; // avg gyro x in rad/s
pmic 9:be40afb750d3 227 }
pmic 9:be40afb750d3 228
pmic 9:be40afb750d3 229 float PX4Flow::avg_gyro_y()
pmic 9:be40afb750d3 230 {
pmic 9:be40afb750d3 231 return (float)iframe.gyro_y_rate_integral*0.00064516132f; // avg gyro y in rad/s
pmic 9:be40afb750d3 232 }
pmic 9:be40afb750d3 233
pmic 9:be40afb750d3 234 float PX4Flow::avg_gyro_z()
pmic 9:be40afb750d3 235 {
pmic 9:be40afb750d3 236 return (float)iframe.gyro_z_rate_integral*0.00064516132f; // avg gyro z in rad/s
pmic 9:be40afb750d3 237 }
pmic 9:be40afb750d3 238
pmic 9:be40afb750d3 239 uint8_t PX4Flow::avg_scaled_quality(){
pmic 9:be40afb750d3 240
pmic 9:be40afb750d3 241 uint8_t avg_scaled_quality = 0;
pmic 9:be40afb750d3 242 /* valid_frame_count := iframe.frame_count_since_last_readout */
pmic 9:be40afb750d3 243 if(iframe.frame_count_since_last_readout > 0) {
pmic 9:be40afb750d3 244 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
pmic 9:be40afb750d3 245 }
pmic 9:be40afb750d3 246 return avg_scaled_quality;
pmic 9:be40afb750d3 247 }
pmic 9:be40afb750d3 248 /* IndNav Git-branch modifications, end here */
altb2 0:4b02060af95b 249
altb2 0:4b02060af95b 250 uint8_t PX4Flow::read8(char *buffer, const unsigned int& idx)
altb2 0:4b02060af95b 251 {
altb2 0:4b02060af95b 252 return uint8_t( buffer[idx] );
altb2 0:4b02060af95b 253 }
altb2 0:4b02060af95b 254
altb2 0:4b02060af95b 255 uint16_t PX4Flow::read16(char *buffer, const unsigned int& idx)
altb2 0:4b02060af95b 256 {
altb2 0:4b02060af95b 257 return uint16_t( read8( buffer, idx ) | (read8(buffer, idx + 1 ) << 8 ) );
altb2 0:4b02060af95b 258 }
altb2 0:4b02060af95b 259
altb2 0:4b02060af95b 260 uint32_t PX4Flow::read32(char *buffer, const unsigned int& idx)
altb2 0:4b02060af95b 261 {
altb2 0:4b02060af95b 262 return uint32_t( (buffer[idx] << 0) | (buffer[idx + 1] << 8) | (buffer[idx+2] << 16) | (buffer[idx+3] << 24) );
altb2 0:4b02060af95b 263 }