altb_pmic / Mbed OS IndNav_QK3_T265

branch for tests with T265

Dependencies:   Lib_Cntrl AHRS Lib_Misc

Xtra_Sensors/PX4Flow.cpp@4:dc844fde64d7, 2019-11-22 (annotated)

Committer:
Kiwicjam
Date:
Fri Nov 22 08:40:26 2019 +0000
Revision:
4:dc844fde64d7
Parent:
2:e7874762cc25 
Workin set, not running,

Who changed what in which revision?

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