Ignore:
Timestamp:
04/08/16 15:40:57 (6 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairFilter/src/JoyReference_impl.cpp

    r10 r15  
    3434using namespace flair::filter;
    3535
    36 JoyReference_impl::JoyReference_impl(JoyReference *inSelf,const LayoutPosition* position,string name): self(inSelf) {
    37 
    38     ahrsData= new AhrsData(self);
    39         input=new cvmatrix(self,4,1,floatType,name);
    40 
    41         cvmatrix_descriptor* desc=new cvmatrix_descriptor(4,1);
    42         desc->SetElementName(0,0,"z");;
    43         desc->SetElementName(1,0,"dz");
    44     desc->SetElementName(2,0,"trim_roll");
    45     desc->SetElementName(3,0,"trim_pitch");
    46     output=new cvmatrix(self,desc,floatType,name);
    47 
    48         reglages_groupbox=new GroupBox(position,name);
    49     deb_roll=new DoubleSpinBox(reglages_groupbox->NewRow(),"debattement roll"," deg",-45,45,1,0);
    50     deb_pitch=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"debattement pitch"," deg",-45,45,1,0);
    51     deb_wz=new DoubleSpinBox(reglages_groupbox->NewRow(),"debattement wz"," deg/s",-180,180,1,0);
    52     deb_dz=new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),"debattement dz"," m/s",-2,2,0.1,1);
    53     trim=new DoubleSpinBox(reglages_groupbox->NewRow(),"trim",-1,1,0.01);
    54     label_roll=new Label(reglages_groupbox->NewRow(),"trim roll");
    55     button_roll=new PushButton(reglages_groupbox->LastRowLastCol(),"reset roll trim");
    56     label_pitch=new Label(reglages_groupbox->NewRow(),"trim pitch");
    57     button_pitch=new PushButton(reglages_groupbox->LastRowLastCol(),"reset pitch trim");
    58 
    59     z_ref=0;
    60     previous_time=0;
    61     trim_roll=0;
    62     trim_pitch=0;
    63 
    64     label_roll->SetText("trim roll: %.2f",trim_roll);
    65     label_pitch->SetText("trim pitch: %.2f",trim_pitch);
    66 }
    67 
    68 JoyReference_impl::~JoyReference_impl(void) {
    69 }
     36JoyReference_impl::JoyReference_impl(JoyReference *inSelf,
     37                                     const LayoutPosition *position,
     38                                     string name)
     39    : self(inSelf) {
     40
     41  ahrsData = new AhrsData(self);
     42  input = new cvmatrix(self, 4, 1, floatType, name);
     43
     44  cvmatrix_descriptor *desc = new cvmatrix_descriptor(4, 1);
     45  desc->SetElementName(0, 0, "z");
     46  ;
     47  desc->SetElementName(1, 0, "dz");
     48  desc->SetElementName(2, 0, "trim_roll");
     49  desc->SetElementName(3, 0, "trim_pitch");
     50  output = new cvmatrix(self, desc, floatType, name);
     51
     52  reglages_groupbox = new GroupBox(position, name);
     53  deb_roll = new DoubleSpinBox(reglages_groupbox->NewRow(), "debattement roll",
     54                               " deg", -45, 45, 1, 0);
     55  deb_pitch = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
     56                                "debattement pitch", " deg", -45, 45, 1, 0);
     57  deb_wz = new DoubleSpinBox(reglages_groupbox->NewRow(), "debattement wz",
     58                             " deg/s", -180, 180, 1, 0);
     59  deb_dz = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
     60                             "debattement dz", " m/s", -2, 2, 0.1, 1);
     61  trim = new DoubleSpinBox(reglages_groupbox->NewRow(), "trim", -1, 1, 0.01);
     62  label_roll = new Label(reglages_groupbox->NewRow(), "trim roll");
     63  button_roll =
     64      new PushButton(reglages_groupbox->LastRowLastCol(), "reset roll trim");
     65  label_pitch = new Label(reglages_groupbox->NewRow(), "trim pitch");
     66  button_pitch =
     67      new PushButton(reglages_groupbox->LastRowLastCol(), "reset pitch trim");
     68
     69  z_ref = 0;
     70  previous_time = 0;
     71  trim_roll = 0;
     72  trim_pitch = 0;
     73
     74  label_roll->SetText("trim roll: %.2f", trim_roll);
     75  label_pitch->SetText("trim pitch: %.2f", trim_pitch);
     76}
     77
     78JoyReference_impl::~JoyReference_impl(void) {}
    7079
    7180void JoyReference_impl::SetRollAxis(float value) {
    72     input->SetValue(0,0,value);
     81  input->SetValue(0, 0, value);
    7382}
    7483
    7584void JoyReference_impl::SetPitchAxis(float value) {
    76     input->SetValue(1,0,value);
     85  input->SetValue(1, 0, value);
    7786}
    7887
    7988void JoyReference_impl::SetYawAxis(float value) {
    80     input->SetValue(2,0,value);
     89  input->SetValue(2, 0, value);
    8190}
    8291
    8392void JoyReference_impl::SetAltitudeAxis(float value) {
    84     input->SetValue(3,0,value);
     93  input->SetValue(3, 0, value);
    8594}
    8695
    8796void JoyReference_impl::RollTrimUp(void) {
    88     trim_roll+=trim->Value();
    89     output->SetValue(2,0,trim_roll);
    90     label_roll->SetText("trim roll: %.2f",trim_roll);
     97  trim_roll += trim->Value();
     98  output->SetValue(2, 0, trim_roll);
     99  label_roll->SetText("trim roll: %.2f", trim_roll);
    91100}
    92101
    93102void JoyReference_impl::RollTrimDown(void) {
    94     trim_roll-=trim->Value();
    95     output->SetValue(2,0,trim_roll);
    96     label_roll->SetText("trim roll: %.2f",trim_roll);
     103  trim_roll -= trim->Value();
     104  output->SetValue(2, 0, trim_roll);
     105  label_roll->SetText("trim roll: %.2f", trim_roll);
    97106}
    98107
    99108void JoyReference_impl::PitchTrimUp(void) {
    100     trim_pitch+=trim->Value();
    101     output->SetValue(3,0,trim_pitch);
    102     label_pitch->SetText("trim pitch: %.2f",trim_pitch);
     109  trim_pitch += trim->Value();
     110  output->SetValue(3, 0, trim_pitch);
     111  label_pitch->SetText("trim pitch: %.2f", trim_pitch);
    103112}
    104113
    105114void JoyReference_impl::PitchTrimDown(void) {
    106     trim_pitch-=trim->Value();
    107     output->SetValue(3,0,trim_pitch);
    108     label_pitch->SetText("trim pitch: %.2f",trim_pitch);
     115  trim_pitch -= trim->Value();
     116  output->SetValue(3, 0, trim_pitch);
     117  label_pitch->SetText("trim pitch: %.2f", trim_pitch);
    109118}
    110119
    111120void JoyReference_impl::SetYawRef(float value) {
    112     Euler ref(0,0,value);
    113     input->GetMutex();
    114     ref.ToQuaternion(q_z);
    115     input->ReleaseMutex();
    116 
    117     Update(GetTime());
     121  Euler ref(0, 0, value);
     122  input->GetMutex();
     123  ref.ToQuaternion(q_z);
     124  input->ReleaseMutex();
     125
     126  Update(GetTime());
    118127}
    119128
    120129void JoyReference_impl::SetZRef(float value) {
    121     z_ref=value;
    122     output->SetValue(0,0,z_ref);
    123 }
    124 
    125 float JoyReference_impl::ZRef(void) const {
    126     return output->Value(0,0);
    127 }
    128 
    129 float JoyReference_impl::dZRef(void) const {
    130    return output->Value(1,0);
    131 }
    132 
    133 float JoyReference_impl::RollTrim(void) const {
    134     return trim_roll;
    135 }
    136 
    137 float JoyReference_impl::PitchTrim(void) const {
    138     return trim_pitch;
    139 }
     130  z_ref = value;
     131  output->SetValue(0, 0, z_ref);
     132}
     133
     134float JoyReference_impl::ZRef(void) const { return output->Value(0, 0); }
     135
     136float JoyReference_impl::dZRef(void) const { return output->Value(1, 0); }
     137
     138float JoyReference_impl::RollTrim(void) const { return trim_roll; }
     139
     140float JoyReference_impl::PitchTrim(void) const { return trim_pitch; }
    140141
    141142void JoyReference_impl::Update(Time time) {
    142     input->SetDataTime(time);
    143     UpdateFrom(input);
     143  input->SetDataTime(time);
     144  UpdateFrom(input);
    144145}
    145146
    146147void JoyReference_impl::UpdateFrom(const io_data *data) {
    147     cvmatrix *input=(cvmatrix*)data;
    148 
    149     if(previous_time==0) previous_time=data->DataTime();//pour la premiere iteration
    150     float delta_t=(float)(data->DataTime()-previous_time)/1000000000.;
    151     previous_time=data->DataTime();
    152 
    153     if(button_roll->Clicked()==true) {
    154         trim_roll=0;
    155         output->SetValue(2,0,0);
    156         label_roll->SetText("trim roll: %.2f",trim_roll);
    157     }
    158     if(button_pitch->Clicked()==true) {
    159         trim_pitch=0;
    160         output->SetValue(3,0,0);
    161         label_pitch->SetText("trim pitch: %.2f",trim_pitch);
    162     }
    163 
    164     //les box sont en degrés
    165     input->GetMutex();
    166 
    167     Vector3D theta_xy(-Euler::ToRadian(input->ValueNoMutex(0,0)*deb_roll->Value()),
    168                       -Euler::ToRadian(input->ValueNoMutex(1,0)*deb_pitch->Value()),
    169                       0);
    170     Vector3D e_bar=theta_xy;
    171     e_bar.Normalize();
    172     Quaternion q_xy(cos(theta_xy.GetNorm()/2.0f),
    173                     e_bar.x*sin(theta_xy.GetNorm()/2.0f),
    174                     e_bar.y*sin(theta_xy.GetNorm()/2.0f),
    175                     0);
    176     q_xy.Normalize();
    177 
    178     float wz_ref=Euler::ToRadian(input->ValueNoMutex(2,0)*deb_wz->Value());
    179         Quaternion w_zd(1,0,0,wz_ref*delta_t/2);
    180         w_zd.Normalize();
    181         q_z = q_z*w_zd;
    182         q_z.Normalize();
    183 
    184         Quaternion q_ref = q_z*q_xy;
    185         q_ref.Normalize();
    186 
    187     z_ref+=input->ValueNoMutex(3,0)*deb_dz->Value()*delta_t;
    188     float dz_ref=input->ValueNoMutex(3,0)*deb_dz->Value();
    189 
    190     input->ReleaseMutex();
    191 
    192     ahrsData->SetQuaternionAndAngularRates(q_ref,Vector3D(0,0,wz_ref));
    193 
    194     //ouput quaternion for control law
    195     output->GetMutex();
    196     output->SetValueNoMutex(0,0,z_ref);
    197     output->SetValueNoMutex(1,0,dz_ref);
    198     output->ReleaseMutex();
    199 
    200     output->SetDataTime(data->DataTime());
    201 }
    202 
     148  cvmatrix *input = (cvmatrix *)data;
     149
     150  if (previous_time == 0)
     151    previous_time = data->DataTime(); // pour la premiere iteration
     152  float delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
     153  previous_time = data->DataTime();
     154
     155  if (button_roll->Clicked() == true) {
     156    trim_roll = 0;
     157    output->SetValue(2, 0, 0);
     158    label_roll->SetText("trim roll: %.2f", trim_roll);
     159  }
     160  if (button_pitch->Clicked() == true) {
     161    trim_pitch = 0;
     162    output->SetValue(3, 0, 0);
     163    label_pitch->SetText("trim pitch: %.2f", trim_pitch);
     164  }
     165
     166  // les box sont en degrés
     167  input->GetMutex();
     168
     169  Vector3D theta_xy(
     170      -Euler::ToRadian(input->ValueNoMutex(0, 0) * deb_roll->Value()),
     171      -Euler::ToRadian(input->ValueNoMutex(1, 0) * deb_pitch->Value()), 0);
     172  Vector3D e_bar = theta_xy;
     173  e_bar.Normalize();
     174  Quaternion q_xy(cos(theta_xy.GetNorm() / 2.0f),
     175                  e_bar.x * sin(theta_xy.GetNorm() / 2.0f),
     176                  e_bar.y * sin(theta_xy.GetNorm() / 2.0f), 0);
     177  q_xy.Normalize();
     178
     179  float wz_ref = Euler::ToRadian(input->ValueNoMutex(2, 0) * deb_wz->Value());
     180  Quaternion w_zd(1, 0, 0, wz_ref * delta_t / 2);
     181  w_zd.Normalize();
     182  q_z = q_z * w_zd;
     183  q_z.Normalize();
     184
     185  Quaternion q_ref = q_z * q_xy;
     186  q_ref.Normalize();
     187
     188  z_ref += input->ValueNoMutex(3, 0) * deb_dz->Value() * delta_t;
     189  float dz_ref = input->ValueNoMutex(3, 0) * deb_dz->Value();
     190
     191  input->ReleaseMutex();
     192
     193  ahrsData->SetQuaternionAndAngularRates(q_ref, Vector3D(0, 0, wz_ref));
     194
     195  // ouput quaternion for control law
     196  output->GetMutex();
     197  output->SetValueNoMutex(0, 0, z_ref);
     198  output->SetValueNoMutex(1, 0, dz_ref);
     199  output->ReleaseMutex();
     200
     201  output->SetDataTime(data->DataTime());
     202}
Note: See TracChangeset for help on using the changeset viewer.