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@12:19fe4f6a8b6b, 2020-04-05 (annotated)

Committer:
pmic
Date:
Sun Apr 05 08:47:25 2020 +0000
Revision:
12:19fe4f6a8b6b
Parent:
11:d367224f2194
Child:
13:89b65bfe6dda
Commit before implementing driver update (reduce ammount of data transmitted). i2c driver has to be updated on both software sides (F446RE and px4flow)!

Who changed what in which revision?

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