altb_pmic / Mbed OS IndNav_QK3_T265

branch for tests with T265

Dependencies:   Lib_Cntrl AHRS Lib_Misc

Xtra_Sensors/TFmini.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 "TFmini.h"
altb2 2:e7874762cc25 2
altb2 2:e7874762cc25 3 using namespace std;
altb2 2:e7874762cc25 4
altb2 2:e7874762cc25 5 TFmini::TFmini(RawSerial& serial) : serial(serial) {
altb2 2:e7874762cc25 6
altb2 2:e7874762cc25 7 serial.baud(115200);
altb2 2:e7874762cc25 8 dist = 0;
altb2 2:e7874762cc25 9 strength = 0;
altb2 2:e7874762cc25 10 checksum = 0;
altb2 2:e7874762cc25 11 cnt = 0;
altb2 2:e7874762cc25 12 serial.attach(callback(this, &TFmini::receive), RawSerial::RxIrq);
altb2 2:e7874762cc25 13
altb2 2:e7874762cc25 14 }
altb2 2:e7874762cc25 15
altb2 2:e7874762cc25 16 TFmini::~TFmini() {
altb2 2:e7874762cc25 17 }
altb2 2:e7874762cc25 18
altb2 2:e7874762cc25 19 float TFmini::readDistance() {
altb2 2:e7874762cc25 20 return (float)dist/1000.0f;
altb2 2:e7874762cc25 21 }
altb2 2:e7874762cc25 22
altb2 2:e7874762cc25 23 float TFmini::readStrength() {
altb2 2:e7874762cc25 24 return strength;
altb2 2:e7874762cc25 25 }
altb2 2:e7874762cc25 26
altb2 2:e7874762cc25 27 void TFmini::receive()
altb2 2:e7874762cc25 28 {
altb2 2:e7874762cc25 29 if(serial.readable()) {
altb2 2:e7874762cc25 30 uartData[cnt] = serial.getc();
altb2 2:e7874762cc25 31 checksum += uartData[cnt];
altb2 2:e7874762cc25 32 if( uartData[0] != HEADER ) {
altb2 2:e7874762cc25 33 cnt = 0;
altb2 2:e7874762cc25 34 checksum = 0;
altb2 2:e7874762cc25 35 } else if( cnt == 1 && uartData[1] != HEADER ) {
altb2 2:e7874762cc25 36 cnt = 0;
altb2 2:e7874762cc25 37 checksum = 0;
altb2 2:e7874762cc25 38 } else if( cnt == 8 ) {
altb2 2:e7874762cc25 39 checksum -= uartData[cnt];
altb2 2:e7874762cc25 40 if( uartData[8] == (checksum & 0xff) ) {
altb2 2:e7874762cc25 41 dist = (uartData[2] + uartData[3] * 256);
altb2 2:e7874762cc25 42 strength = uartData[4] + uartData[5] * 256;
altb2 2:e7874762cc25 43 }
altb2 2:e7874762cc25 44 cnt = 0;
altb2 2:e7874762cc25 45 checksum = 0;
altb2 2:e7874762cc25 46 } else {
altb2 2:e7874762cc25 47 cnt++;
altb2 2:e7874762cc25 48 }
altb2 2:e7874762cc25 49 }
altb2 2:e7874762cc25 50 }