File content as of revision 3:bc24fee36ba3:

#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();
    }