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@0:4b02060af95b, 2019-08-02 (annotated)

Committer:
altb2
Date:
Fri Aug 02 14:15:07 2019 +0000
Revision:
0:4b02060af95b
Child:
9:be40afb750d3
PX4 Test on nuc L432KC (from hurc)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 0:4b02060af95b 1 #include "PX4Flow.h"
altb2 0:4b02060af95b 2
altb2 0:4b02060af95b 3 PX4Flow::PX4Flow( I2C& i2c, Serial& pc ): i2c(i2c), pc(pc)
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
altb2 0:4b02060af95b 17 // assign the data
altb2 0:4b02060af95b 18 frame.frame_count = static_cast<uint16_t>(read16(bufferS, FRAME_COUNT));
altb2 0:4b02060af95b 19 frame.pixel_flow_x_sum = static_cast<int16_t>(read16(bufferS, PIXEL_FLOW_X_SUM));
altb2 0:4b02060af95b 20 frame.pixel_flow_y_sum = static_cast<int16_t>(read16(bufferS, PIXEL_FLOW_Y_SUM));
altb2 0:4b02060af95b 21 frame.flow_comp_m_x = static_cast<int16_t>(read16(bufferS, FLOW_COMP_M_X));
altb2 0:4b02060af95b 22 frame.flow_comp_m_y = static_cast<int16_t>(read16(bufferS, FLOW_COMP_M_Y));
altb2 0:4b02060af95b 23 frame.qual = static_cast<int16_t>(read16(bufferS, QUAL));
altb2 0:4b02060af95b 24 frame.gyro_x_rate = static_cast<int16_t>(read16(bufferS, GYRO_X_RATE));
altb2 0:4b02060af95b 25 frame.gyro_y_rate = static_cast<int16_t>(read16(bufferS, GYRO_Y_RATE));
altb2 0:4b02060af95b 26 frame.gyro_z_rate = static_cast<int16_t>(read16(bufferS, GYRO_Z_RATE));
altb2 0:4b02060af95b 27 frame.gyro_range = static_cast<uint8_t>(read8(bufferS, GYRO_RANGE));
altb2 0:4b02060af95b 28 frame.sonar_timestamp = static_cast<uint8_t>(read8(bufferS, SONAR_TIMESTAMP));
altb2 0:4b02060af95b 29 frame.ground_distance = static_cast<uint16_t>(read16(bufferS, GROUND_DISTANCE));
altb2 0:4b02060af95b 30
altb2 0:4b02060af95b 31 return true;
altb2 0:4b02060af95b 32
altb2 0:4b02060af95b 33 } else {
altb2 0:4b02060af95b 34 printf("Error: reading I2C\r\n");
altb2 0:4b02060af95b 35 return false;
altb2 0:4b02060af95b 36 }
altb2 0:4b02060af95b 37 } else {
altb2 0:4b02060af95b 38 pc.printf("Error: writing I2C\r\n");
altb2 0:4b02060af95b 39 return false;
altb2 0:4b02060af95b 40 }
altb2 0:4b02060af95b 41
altb2 0:4b02060af95b 42 }
altb2 0:4b02060af95b 43
altb2 0:4b02060af95b 44 bool PX4Flow::update_integral()
altb2 0:4b02060af95b 45 {
altb2 0:4b02060af95b 46
altb2 0:4b02060af95b 47 //send 0x16 to PX4FLOW module and receive back 26 Bytes data
altb2 0:4b02060af95b 48 if( i2c.write(PX4FLOW_ADDRESS, &i2c_commands[1], 1 ) == 0 ) {
altb2 0:4b02060af95b 49
altb2 0:4b02060af95b 50 int ack = i2c.read(PX4FLOW_ADDRESS, bufferI, 26 );
altb2 0:4b02060af95b 51 if( ack == 0 ) {
altb2 0:4b02060af95b 52
altb2 0:4b02060af95b 53 // assign the data
altb2 0:4b02060af95b 54 iframe.frame_count_since_last_readout = static_cast<uint16_t>(read16(bufferI, FRAME_COUNT_SINCE_LAST_READOUT));
altb2 0:4b02060af95b 55 iframe.pixel_flow_x_integral = static_cast<int16_t> (read16(bufferI, PIXEL_FLOW_X_INTEGRAL));
altb2 0:4b02060af95b 56 iframe.pixel_flow_y_integral = static_cast<int16_t> (read16(bufferI, PIXEL_FLOW_Y_INTEGRAL));
altb2 0:4b02060af95b 57 iframe.gyro_x_rate_integral = static_cast<int16_t> (read16(bufferI, GYRO_X_RATE_INTEGRAL));
altb2 0:4b02060af95b 58 iframe.gyro_y_rate_integral = static_cast<int16_t> (read16(bufferI, GYRO_Y_RATE_INTEGRAL));
altb2 0:4b02060af95b 59 iframe.gyro_z_rate_integral = static_cast<int16_t> (read16(bufferI, GYRO_Z_RATE_INTEGRAL));
altb2 0:4b02060af95b 60 iframe.integration_timespan = static_cast<uint32_t>(read32(bufferI, INTEGRATION_TIMESPAN));
altb2 0:4b02060af95b 61 iframe.sonar_timestamp = static_cast<uint32_t>(read32(bufferI, SONAR_TIMESTAMP_INTEGRAL));
altb2 0:4b02060af95b 62 iframe.ground_distance = static_cast<int16_t> (read16(bufferI, GROUND_DISTANCE_INTEGRAL));
altb2 0:4b02060af95b 63 iframe.gyro_temperature = static_cast<int16_t> (read16(bufferI, GYRO_TEMPERATURE));
altb2 0:4b02060af95b 64 iframe.quality = static_cast<uint8_t> (read8(bufferI, QUALITY_INTEGRAL));
altb2 0:4b02060af95b 65
altb2 0:4b02060af95b 66 return true;
altb2 0:4b02060af95b 67
altb2 0:4b02060af95b 68 } else {
altb2 0:4b02060af95b 69 pc.printf("Error: reading I2C\r\n");
altb2 0:4b02060af95b 70 return false;
altb2 0:4b02060af95b 71 }
altb2 0:4b02060af95b 72 } else {
altb2 0:4b02060af95b 73 pc.printf("Error: writing I2C\r\n");
altb2 0:4b02060af95b 74 return false;
altb2 0:4b02060af95b 75 }
altb2 0:4b02060af95b 76 }
altb2 0:4b02060af95b 77
altb2 0:4b02060af95b 78
altb2 0:4b02060af95b 79 // Methods to return the sensordata from datastructure
altb2 0:4b02060af95b 80 // Simple frame
altb2 0:4b02060af95b 81 uint16_t PX4Flow::frame_count()
altb2 0:4b02060af95b 82 {
altb2 0:4b02060af95b 83 return frame.frame_count;
altb2 0:4b02060af95b 84 }
altb2 0:4b02060af95b 85
altb2 0:4b02060af95b 86 int16_t PX4Flow::pixel_flow_x_sum()
altb2 0:4b02060af95b 87 {
altb2 0:4b02060af95b 88 return frame.pixel_flow_x_sum;
altb2 0:4b02060af95b 89 }
altb2 0:4b02060af95b 90
altb2 0:4b02060af95b 91 int16_t PX4Flow::pixel_flow_y_sum()
altb2 0:4b02060af95b 92 {
altb2 0:4b02060af95b 93 return frame.pixel_flow_y_sum;
altb2 0:4b02060af95b 94 }
altb2 0:4b02060af95b 95
altb2 0:4b02060af95b 96 int16_t PX4Flow::flow_comp_m_x()
altb2 0:4b02060af95b 97 {
altb2 0:4b02060af95b 98 return frame.flow_comp_m_x;
altb2 0:4b02060af95b 99 }
altb2 0:4b02060af95b 100
altb2 0:4b02060af95b 101 int16_t PX4Flow::flow_comp_m_y()
altb2 0:4b02060af95b 102 {
altb2 0:4b02060af95b 103 return frame.flow_comp_m_y;
altb2 0:4b02060af95b 104 }
altb2 0:4b02060af95b 105
altb2 0:4b02060af95b 106 int16_t PX4Flow::qual()
altb2 0:4b02060af95b 107 {
altb2 0:4b02060af95b 108 return frame.qual;
altb2 0:4b02060af95b 109 }
altb2 0:4b02060af95b 110
altb2 0:4b02060af95b 111 int16_t PX4Flow::gyro_x_rate()
altb2 0:4b02060af95b 112 {
altb2 0:4b02060af95b 113 return frame.gyro_x_rate;
altb2 0:4b02060af95b 114 }
altb2 0:4b02060af95b 115
altb2 0:4b02060af95b 116 int16_t PX4Flow::gyro_y_rate()
altb2 0:4b02060af95b 117 {
altb2 0:4b02060af95b 118 return frame.gyro_y_rate;
altb2 0:4b02060af95b 119 }
altb2 0:4b02060af95b 120
altb2 0:4b02060af95b 121 int16_t PX4Flow::gyro_z_rate()
altb2 0:4b02060af95b 122 {
altb2 0:4b02060af95b 123 return frame.gyro_z_rate;
altb2 0:4b02060af95b 124 }
altb2 0:4b02060af95b 125
altb2 0:4b02060af95b 126 uint8_t PX4Flow::gyro_range()
altb2 0:4b02060af95b 127 {
altb2 0:4b02060af95b 128 return frame.gyro_range;
altb2 0:4b02060af95b 129 }
altb2 0:4b02060af95b 130
altb2 0:4b02060af95b 131 uint8_t PX4Flow::sonar_timestamp()
altb2 0:4b02060af95b 132 {
altb2 0:4b02060af95b 133 return frame.sonar_timestamp;
altb2 0:4b02060af95b 134 }
altb2 0:4b02060af95b 135
altb2 0:4b02060af95b 136 int16_t PX4Flow::ground_distance()
altb2 0:4b02060af95b 137 {
altb2 0:4b02060af95b 138 return frame.ground_distance;
altb2 0:4b02060af95b 139 }
altb2 0:4b02060af95b 140
altb2 0:4b02060af95b 141
altb2 0:4b02060af95b 142 // Integral frame
altb2 0:4b02060af95b 143 uint16_t PX4Flow::frame_count_since_last_readout()
altb2 0:4b02060af95b 144 {
altb2 0:4b02060af95b 145 return iframe.frame_count_since_last_readout;
altb2 0:4b02060af95b 146 }
altb2 0:4b02060af95b 147
altb2 0:4b02060af95b 148 int16_t PX4Flow::pixel_flow_x_integral()
altb2 0:4b02060af95b 149 {
altb2 0:4b02060af95b 150 return iframe.pixel_flow_x_integral;
altb2 0:4b02060af95b 151 }
altb2 0:4b02060af95b 152
altb2 0:4b02060af95b 153 int16_t PX4Flow::pixel_flow_y_integral()
altb2 0:4b02060af95b 154 {
altb2 0:4b02060af95b 155 return iframe.pixel_flow_y_integral;
altb2 0:4b02060af95b 156 }
altb2 0:4b02060af95b 157
altb2 0:4b02060af95b 158 int16_t PX4Flow::gyro_x_rate_integral()
altb2 0:4b02060af95b 159 {
altb2 0:4b02060af95b 160 return iframe.gyro_x_rate_integral;
altb2 0:4b02060af95b 161 }
altb2 0:4b02060af95b 162
altb2 0:4b02060af95b 163 int16_t PX4Flow::gyro_y_rate_integral()
altb2 0:4b02060af95b 164 {
altb2 0:4b02060af95b 165 return iframe.gyro_y_rate_integral;
altb2 0:4b02060af95b 166 }
altb2 0:4b02060af95b 167
altb2 0:4b02060af95b 168 int16_t PX4Flow::gyro_z_rate_integral()
altb2 0:4b02060af95b 169 {
altb2 0:4b02060af95b 170 return iframe.gyro_z_rate_integral;
altb2 0:4b02060af95b 171 }
altb2 0:4b02060af95b 172
altb2 0:4b02060af95b 173 uint32_t PX4Flow::integration_timespan()
altb2 0:4b02060af95b 174 {
altb2 0:4b02060af95b 175 return iframe.integration_timespan;
altb2 0:4b02060af95b 176 }
altb2 0:4b02060af95b 177
altb2 0:4b02060af95b 178 uint32_t PX4Flow::sonar_timestamp_integral()
altb2 0:4b02060af95b 179 {
altb2 0:4b02060af95b 180 return iframe.sonar_timestamp;
altb2 0:4b02060af95b 181 }
altb2 0:4b02060af95b 182
altb2 0:4b02060af95b 183 int16_t PX4Flow::ground_distance_integral()
altb2 0:4b02060af95b 184 {
altb2 0:4b02060af95b 185 return iframe.ground_distance;
altb2 0:4b02060af95b 186 }
altb2 0:4b02060af95b 187
altb2 0:4b02060af95b 188 int16_t PX4Flow::gyro_temperature()
altb2 0:4b02060af95b 189 {
altb2 0:4b02060af95b 190 return iframe.gyro_temperature;
altb2 0:4b02060af95b 191 }
altb2 0:4b02060af95b 192
altb2 0:4b02060af95b 193 uint8_t PX4Flow::quality_integral()
altb2 0:4b02060af95b 194 {
altb2 0:4b02060af95b 195 return iframe.quality;
altb2 0:4b02060af95b 196 }
altb2 0:4b02060af95b 197
altb2 0:4b02060af95b 198
altb2 0:4b02060af95b 199 uint8_t PX4Flow::read8(char *buffer, const unsigned int& idx)
altb2 0:4b02060af95b 200 {
altb2 0:4b02060af95b 201 return uint8_t( buffer[idx] );
altb2 0:4b02060af95b 202 }
altb2 0:4b02060af95b 203
altb2 0:4b02060af95b 204 uint16_t PX4Flow::read16(char *buffer, const unsigned int& idx)
altb2 0:4b02060af95b 205 {
altb2 0:4b02060af95b 206 return uint16_t( read8( buffer, idx ) | (read8(buffer, idx + 1 ) << 8 ) );
altb2 0:4b02060af95b 207 }
altb2 0:4b02060af95b 208
altb2 0:4b02060af95b 209 uint32_t PX4Flow::read32(char *buffer, const unsigned int& idx)
altb2 0:4b02060af95b 210 {
altb2 0:4b02060af95b 211 return uint32_t( (buffer[idx] << 0) | (buffer[idx + 1] << 8) | (buffer[idx+2] << 16) | (buffer[idx+3] << 24) );
altb2 0:4b02060af95b 212 }