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@13:89b65bfe6dda, 2020-04-06 (annotated)

Committer:
pmic
Date:
Mon Apr 06 05:57:56 2020 +0000
Revision:
13:89b65bfe6dda
Parent:
12:19fe4f6a8b6b 
Change receiving driver and firmware. Data latency now 2.4 ms faster.

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 {
pmic 13:89b65bfe6dda 5 i2c_commands = INTEGRAL_FRAME;
pmic 13:89b65bfe6dda 6
pmic 13:89b65bfe6dda 7 iframe.avg_flow_x = 0;
pmic 13:89b65bfe6dda 8 iframe.avg_flow_y = 0;
pmic 13:89b65bfe6dda 9 iframe.avg_qual = 0;
pmic 13:89b65bfe6dda 10 iframe.valid_frame_count = 0;
pmic 13:89b65bfe6dda 11 iframe.frame_count = 0;
altb2 0:4b02060af95b 12 }
altb2 0:4b02060af95b 13
altb2 0:4b02060af95b 14 PX4Flow::~PX4Flow() {}
altb2 0:4b02060af95b 15
altb2 0:4b02060af95b 16 bool PX4Flow::update_integral()
altb2 0:4b02060af95b 17 {
pmic 13:89b65bfe6dda 18 // send 0x16 to PX4FLOW module and receive back 7 Bytes data
pmic 13:89b65bfe6dda 19 int b1 = i2c.write(PX4FLOW_ADDRESS, &i2c_commands, 1);
pmic 12:19fe4f6a8b6b 20 if( b1 == 0 ) {
pmic 13:89b65bfe6dda 21 // dout1.write(1);
pmic 13:89b65bfe6dda 22 b1 = i2c.read(PX4FLOW_ADDRESS, bufferI, 7);
pmic 13:89b65bfe6dda 23 // dout1.write(0);
pmic 12:19fe4f6a8b6b 24 if(b1 == 0 ) {
altb2 0:4b02060af95b 25 // assign the data
pmic 13:89b65bfe6dda 26 iframe.avg_flow_x = (int16_t)(read16(bufferI, AVG_FLOW_X ));
pmic 13:89b65bfe6dda 27 iframe.avg_flow_y = (int16_t)(read16(bufferI, AVG_FLOW_Y ));
pmic 13:89b65bfe6dda 28 iframe.avg_qual = (uint8_t)( read8(bufferI, AVG_QUAL ));
pmic 13:89b65bfe6dda 29 iframe.valid_frame_count = (uint8_t)( read8(bufferI, VALID_FRAME_COUNT));
pmic 13:89b65bfe6dda 30 iframe.frame_count = (uint8_t)( read8(bufferI, FRAME_COUNT_ ));
altb2 0:4b02060af95b 31 return true;
altb2 0:4b02060af95b 32 } else {
altb2 0:4b02060af95b 33 return false;
altb2 0:4b02060af95b 34 }
altb2 0:4b02060af95b 35 } else {
altb2 0:4b02060af95b 36 return false;
altb2 0:4b02060af95b 37 }
altb2 0:4b02060af95b 38 }
altb2 0:4b02060af95b 39
pmic 13:89b65bfe6dda 40 float PX4Flow::avg_flow_x()
altb2 0:4b02060af95b 41 {
pmic 13:89b65bfe6dda 42 return (float)iframe.avg_flow_x*0.3333333f;
altb2 0:4b02060af95b 43 }
pmic 13:89b65bfe6dda 44
pmic 13:89b65bfe6dda 45 float PX4Flow::avg_flow_y()
altb2 0:4b02060af95b 46 {
pmic 13:89b65bfe6dda 47 return (float)iframe.avg_flow_y*0.3333333f;
altb2 0:4b02060af95b 48 }
altb2 0:4b02060af95b 49
pmic 13:89b65bfe6dda 50 uint8_t PX4Flow::avg_qual()
altb2 0:4b02060af95b 51 {
pmic 13:89b65bfe6dda 52 return iframe.avg_qual;
altb2 0:4b02060af95b 53 }
altb2 0:4b02060af95b 54
pmic 9:be40afb750d3 55 uint8_t PX4Flow::valid_frame_count()
pmic 9:be40afb750d3 56 {
pmic 13:89b65bfe6dda 57 return iframe.valid_frame_count;
pmic 9:be40afb750d3 58 }
pmic 9:be40afb750d3 59
pmic 13:89b65bfe6dda 60 uint8_t PX4Flow::frame_count()
pmic 9:be40afb750d3 61 {
pmic 13:89b65bfe6dda 62 return iframe.frame_count;
pmic 9:be40afb750d3 63 }
pmic 9:be40afb750d3 64
pmic 13:89b65bfe6dda 65 uint8_t PX4Flow::avg_qual_scaled(){
pmic 9:be40afb750d3 66
pmic 13:89b65bfe6dda 67 uint8_t avg_qual_scaled = 0;
pmic 13:89b65bfe6dda 68 if(iframe.frame_count > 0) {
pmic 13:89b65bfe6dda 69 avg_qual_scaled = (uint8_t)( (float)iframe.avg_qual * ( (float)iframe.valid_frame_count / (float)iframe.frame_count ) );
pmic 13:89b65bfe6dda 70 }
pmic 13:89b65bfe6dda 71 return avg_qual_scaled;
pmic 9:be40afb750d3 72 }
altb2 0:4b02060af95b 73
altb2 0:4b02060af95b 74 uint8_t PX4Flow::read8(char *buffer, const unsigned int& idx)
altb2 0:4b02060af95b 75 {
altb2 0:4b02060af95b 76 return uint8_t( buffer[idx] );
altb2 0:4b02060af95b 77 }
altb2 0:4b02060af95b 78
altb2 0:4b02060af95b 79 uint16_t PX4Flow::read16(char *buffer, const unsigned int& idx)
altb2 0:4b02060af95b 80 {
altb2 0:4b02060af95b 81 return uint16_t( read8( buffer, idx ) | (read8(buffer, idx + 1 ) << 8 ) );
altb2 0:4b02060af95b 82 }
altb2 0:4b02060af95b 83
altb2 0:4b02060af95b 84 uint32_t PX4Flow::read32(char *buffer, const unsigned int& idx)
altb2 0:4b02060af95b 85 {
altb2 0:4b02060af95b 86 return uint32_t( (buffer[idx] << 0) | (buffer[idx + 1] << 8) | (buffer[idx+2] << 16) | (buffer[idx+3] << 24) );
altb2 0:4b02060af95b 87 }