source: flair-src/trunk/lib/FlairFilter/src/PlaneMultiplex.cpp@ 461

// %flair:license{

// This file is part of the Flair framework distributed under the

// CECILL-C License, Version 1.0.

// %flair:license}

//  created:    2022/01/05

//  filename:   PlaneMultiplex.cpp

//

//  author:     Guillaume Sanahuja

//              Copyright Heudiasyc UMR UTC/CNRS 7253

//

//  version:    $Id: $

//

//  purpose:    Class defining plane multiplexing

//

//

/*********************************************************************/



#include "PlaneMultiplex.h"

#include <Matrix.h>

#include <Tab.h>

#include <GridLayout.h>

#include <GroupBox.h>

#include <ComboBox.h>

#include <DoubleSpinBox.h>

#include <DataPlot1D.h>

#include <math.h>





using std::string;

using namespace flair::core;

using namespace flair::gui;

using namespace flair::filter;



namespace flair {

namespace filter {

class PlaneMultiplexBldc : public IODevice {

 

public:

  PlaneMultiplexBldc(PlaneMultiplex *parent, string name):IODevice(parent,name) {

    output = new Matrix(this, 1,1, floatType);

  }

  ~PlaneMultiplexBldc(){};





private:

  Matrix *output;

  void UpdateFrom(const core::io_data *data) {

    const Matrix* input = dynamic_cast<const Matrix*>(data);

  

    if (!input) {

        Warn("casting %s to Matrix failed\n",data->ObjectName().c_str());

        return;

    }

    output->SetValue(0, 0, input->Value(0,0));



    output->SetDataTime(data->DataTime());



    ProcessUpdate(output);

  }



};



class PlaneMultiplexServos : public IODevice {

 

public:



  

  PlaneMultiplexServos(PlaneMultiplex *parent, string name):IODevice(parent,name) {

      output = new Matrix(this, 2,1, floatType);

  }

  ~PlaneMultiplexServos(){};





private:

  Matrix *output;

  void UpdateFrom(const core::io_data *data) {

    const Matrix* input = dynamic_cast<const Matrix*>(data);

  

    if (!input) {

        Warn("casting %s to Matrix failed\n",data->ObjectName().c_str());

        return;

    }

    input->GetMutex();

    output->GetMutex();

    for (int i = 0; i < 2; i++) output->SetValueNoMutex(i, 0, input->ValueNoMutex(1+i,0));

    output->ReleaseMutex();

    input->ReleaseMutex();



    output->SetDataTime(data->DataTime());



    ProcessUpdate(output);

  }





};



} // end namespace filter

} // end namespace flair



PlaneMultiplex::PlaneMultiplex(string name) :UavMultiplex(name){

  

  bldc=new PlaneMultiplexBldc(this,"bldc_multiplex");

  servos=new PlaneMultiplexServos(this,"servos_multiplex");

 

  MatrixDescriptor *desc = new MatrixDescriptor(6, 1);

  for (int i = 0; i < 3; i++) {

    desc->SetElementName(i, 0, ActuatorName(i));

  }



  output = new Matrix(this, desc, floatType);

  delete desc;

  AddDataToLog(output);

}



PlaneMultiplex::~PlaneMultiplex(void) {}



IODevice* PlaneMultiplex::GetBldcMultiplex(void) const {

  return bldc;

}



IODevice* PlaneMultiplex::GetServosMultiplex(void) const {

  return servos;

}





void PlaneMultiplex::UseDefaultPlot(void) {

  Tab *plot_tab = new Tab(GetTabWidget(), "Values");

  plots[0] = new DataPlot1D(plot_tab->NewRow(), ActuatorName(0), 0, 1);

  plots[1] = new DataPlot1D(plot_tab->LastRowLastCol(), ActuatorName(1), 0, 1);

  plots[2] = new DataPlot1D(plot_tab->NewRow(), ActuatorName(2), 0, 1);



  for (int i = 0; i < 3; i++) {

    plots[i]->AddCurve(output->Element(i));

  }

}



void PlaneMultiplex::UpdateFrom(const io_data *data) {

  float u_roll, u_pitch, u_yaw, u_thrust;

  float trim_roll, trim_pitch, trim_yaw;

  float value[3];



  const Matrix* input = dynamic_cast<const Matrix*>(data);

  

  if (!input) {

      Warn("casting %s to Matrix failed\n",data->ObjectName().c_str());

      return;

  }



  // on prend une fois pour toute le mutex et on fait des accès directs

  input->GetMutex();



  u_roll = input->ValueNoMutex(0, 0);

  u_pitch = input->ValueNoMutex(1, 0);

  u_yaw = input->ValueNoMutex(2, 0);

  u_thrust = input->ValueNoMutex(3, 0);

  trim_roll = input->ValueNoMutex(4, 0);

  trim_pitch = input->ValueNoMutex(5, 0);

  trim_yaw = input->ValueNoMutex(6, 0);



  input->ReleaseMutex();

  

  //font motor

  value[0]=0;

  

  //aileron right

  value[1]=0;

 

  //aileron left

  value[2]=0;

 

  // on prend une fois pour toute le mutex et on fait des accès directs

  output->GetMutex();

  for (int i = 0; i < 3; i++)

    output->SetValueNoMutex(i, 0, value[i]);

  output->ReleaseMutex();



  output->SetDataTime(data->DataTime());



  ProcessUpdate(output);

}



string PlaneMultiplex::ActuatorName(int index) {

 

    switch (index) {

    case 0:

      return "front motor";

    case 1:

      return "right aileron";

    case 2:

      return "left aileron";

    default:

      return "unammed actuator";

    }

}



uint8_t PlaneMultiplex::MotorsCount(void) const { return 3; }