altb_pmic / Mbed OS IndNav_QK3_T265

branch for tests with T265

Dependencies:   Lib_Cntrl AHRS Lib_Misc

Threads_and_main/Read_Xtern_Sensors.cpp@2:e7874762cc25, 2019-10-21 (annotated)

Committer:
altb2
Date:
Mon Oct 21 17:16:11 2019 +0000
Revision:
2:e7874762cc25
Parent:
Sources/Read_Xtern_Sensors.cpp@1:d8c9f6b16279
Child:
3:bc24fee36ba3
Added additional Ekfs, tested AltHold (still some bugs, Problems at high Lift Rates)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 1:d8c9f6b16279 1 #include "Read_Xtern_Sensors.h"
altb2 1:d8c9f6b16279 2
altb2 1:d8c9f6b16279 3 using namespace std;
altb2 1:d8c9f6b16279 4
altb2 1:d8c9f6b16279 5
altb2 2:e7874762cc25 6 Read_Xtern_Sensors::Read_Xtern_Sensors(float Ts, RawSerial *uart4lidar, I2C *i2c) : optical_flow(*i2c), tfmini_1(*uart4lidar), dzdt(2.0f*Ts,Ts), thread(osPriorityNormal, 4096)
altb2 1:d8c9f6b16279 7 {
altb2 1:d8c9f6b16279 8 this->Ts = Ts;
altb2 1:d8c9f6b16279 9 }
altb2 1:d8c9f6b16279 10
altb2 1:d8c9f6b16279 11
altb2 1:d8c9f6b16279 12 void Read_Xtern_Sensors::loop(void){
altb2 1:d8c9f6b16279 13
altb2 1:d8c9f6b16279 14 while(1){
altb2 1:d8c9f6b16279 15 thread.signal_wait(signal);
altb2 1:d8c9f6b16279 16 if(optical_flow.update_integral()) {
altb2 1:d8c9f6b16279 17 data.sens_OF[0] = optical_flow.avg_flowx()*0.001f;
altb2 1:d8c9f6b16279 18 data.sens_OF[1] = optical_flow.avg_flowy()*0.001f;
altb2 1:d8c9f6b16279 19 data.sens_OF[2] = 0.0f; // later: Ultrasonic distance from OF
altb2 1:d8c9f6b16279 20 data.sens_OF[3] = (float)optical_flow.avg_scaled_quality();
altb2 2:e7874762cc25 21 data.sens_OF[4] = optical_flow.avg_gyro_x();
altb2 2:e7874762cc25 22 data.sens_OF[5] = optical_flow.avg_gyro_y();
altb2 1:d8c9f6b16279 23 }
altb2 2:e7874762cc25 24 data.sens_Lidar[0] = tfmini_1();
altb2 2:e7874762cc25 25 data.est_xyz[2] = data.sens_Lidar[0];
altb2 2:e7874762cc25 26 if(data.est_xyz[2] >= 0)
altb2 2:e7874762cc25 27 {
altb2 2:e7874762cc25 28 data.est_Vxyz[2] = dzdt(data.est_xyz[2]);
altb2 2:e7874762cc25 29 Lidar_1_is_valid = true;
altb2 2:e7874762cc25 30 }
altb2 2:e7874762cc25 31 else
altb2 2:e7874762cc25 32 Lidar_1_is_valid = false;
altb2 2:e7874762cc25 33 //pc.printf("OF: %1.3f %1.3f, L: %1.3f\r\n",data.sens_OF[0],data.sens_OF[1],data.est_xyz[2]);
altb2 2:e7874762cc25 34
altb2 1:d8c9f6b16279 35 }
altb2 1:d8c9f6b16279 36 }
altb2 1:d8c9f6b16279 37
altb2 1:d8c9f6b16279 38
altb2 1:d8c9f6b16279 39 // ------------------- start controllers ----------------
altb2 1:d8c9f6b16279 40 void Read_Xtern_Sensors::start_loop(void){
altb2 1:d8c9f6b16279 41 printf("Read Xternal Sensors Thread started\r\n");
altb2 1:d8c9f6b16279 42 thread.start(callback(this, &Read_Xtern_Sensors::loop));
altb2 1:d8c9f6b16279 43 ticker.attach(callback(this, &Read_Xtern_Sensors::sendSignal), Ts);
altb2 1:d8c9f6b16279 44 }
altb2 1:d8c9f6b16279 45
altb2 1:d8c9f6b16279 46 // this is for realtime OS
altb2 1:d8c9f6b16279 47 void Read_Xtern_Sensors::sendSignal() {
altb2 1:d8c9f6b16279 48 thread.signal_set(signal);
altb2 1:d8c9f6b16279 49 }
altb2 1:d8c9f6b16279 50 // ------------------- destructor -----------------
altb2 1:d8c9f6b16279 51 Read_Xtern_Sensors::~Read_Xtern_Sensors() {
altb2 1:d8c9f6b16279 52 ticker.detach();
altb2 1:d8c9f6b16279 53 }