branch for tests with T265
Dependencies: Lib_Cntrl AHRS Lib_Misc
Threads_and_main/Read_Xtern_Sensors.cpp
- Committer:
- Kiwicjam
- Date:
- 2019-11-22
- Revision:
- 4:dc844fde64d7
- Parent:
- 3:bc24fee36ba3
File content as of revision 4:dc844fde64d7:
#include "Read_Xtern_Sensors.h" using namespace std; 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) { this->Ts = Ts; } void Read_Xtern_Sensors::loop(void){ while(1){ thread.signal_wait(signal); if(optical_flow.update_integral()) { data.sens_OF[0] = optical_flow.avg_flowx()*0.001f; data.sens_OF[1] = optical_flow.avg_flowy()*0.001f; data.sens_OF[2] = (float)optical_flow.avg_scaled_quality(); data.sens_OF[3] = optical_flow.avg_gyro_x(); data.sens_OF[4] = optical_flow.avg_gyro_y(); data.sens_OF[5] = optical_flow.avg_gyro_z(); data.est_Vxyz[0]= (-data.sens_OF[0] + data.sens_gyr[1] * data.sens_Lidar[2]); data.est_Vxyz[1]= ( data.sens_OF[1] - data.sens_gyr[0] * data.sens_Lidar[2]); } data.sens_Lidar[0] = tfmini_1(); data.est_xyz[2] = data.sens_Lidar[0]; if(data.est_xyz[2] >= 0) { data.est_Vxyz[2] = dzdt(data.est_xyz[2]); Lidar_1_is_valid = true; } else Lidar_1_is_valid = false; //pc.printf("OF: %1.3f %1.3f, L: %1.3f\r\n",data.sens_OF[0],data.sens_OF[1],data.est_xyz[2]); } } // ------------------- start controllers ---------------- void Read_Xtern_Sensors::start_loop(void){ printf("Read Xternal Sensors Thread started\r\n"); thread.start(callback(this, &Read_Xtern_Sensors::loop)); ticker.attach(callback(this, &Read_Xtern_Sensors::sendSignal), Ts); } // this is for realtime OS void Read_Xtern_Sensors::sendSignal() { thread.signal_set(signal); } // ------------------- destructor ----------------- Read_Xtern_Sensors::~Read_Xtern_Sensors() { ticker.detach(); }