[10] | 1 | // %flair:license{
|
---|
[15] | 2 | // This file is part of the Flair framework distributed under the
|
---|
| 3 | // CECILL-C License, Version 1.0.
|
---|
[10] | 4 | // %flair:license}
|
---|
[7] | 5 | // created: 2012/08/29
|
---|
| 6 | // filename: JoyReference_impl.cpp
|
---|
| 7 | //
|
---|
| 8 | // author: Guillaume Sanahuja
|
---|
| 9 | // Copyright Heudiasyc UMR UTC/CNRS 7253
|
---|
| 10 | //
|
---|
| 11 | // version: $Id: $
|
---|
| 12 | //
|
---|
| 13 | // purpose: generation de consignes a partir joystick
|
---|
| 14 | //
|
---|
| 15 | //
|
---|
| 16 | /*********************************************************************/
|
---|
| 17 |
|
---|
| 18 | #include "JoyReference_impl.h"
|
---|
| 19 | #include "JoyReference.h"
|
---|
| 20 | #include <AhrsData.h>
|
---|
| 21 | #include <Euler.h>
|
---|
| 22 | #include <cvmatrix.h>
|
---|
| 23 | #include <Layout.h>
|
---|
| 24 | #include <GroupBox.h>
|
---|
| 25 | #include <DoubleSpinBox.h>
|
---|
| 26 | #include <SpinBox.h>
|
---|
| 27 | #include <Label.h>
|
---|
| 28 | #include <PushButton.h>
|
---|
| 29 | #include <math.h>
|
---|
| 30 |
|
---|
| 31 | using std::string;
|
---|
| 32 | using namespace flair::core;
|
---|
| 33 | using namespace flair::gui;
|
---|
| 34 | using namespace flair::filter;
|
---|
| 35 |
|
---|
[15] | 36 | JoyReference_impl::JoyReference_impl(JoyReference *inSelf,
|
---|
| 37 | const LayoutPosition *position,
|
---|
| 38 | string name)
|
---|
| 39 | : self(inSelf) {
|
---|
[7] | 40 |
|
---|
[15] | 41 | ahrsData = new AhrsData(self);
|
---|
| 42 | input = new cvmatrix(self, 4, 1, floatType, name);
|
---|
[7] | 43 |
|
---|
[15] | 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);
|
---|
[7] | 51 |
|
---|
[15] | 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");
|
---|
[7] | 68 |
|
---|
[15] | 69 | z_ref = 0;
|
---|
| 70 | previous_time = 0;
|
---|
| 71 | trim_roll = 0;
|
---|
| 72 | trim_pitch = 0;
|
---|
[7] | 73 |
|
---|
[15] | 74 | label_roll->SetText("trim roll: %.2f", trim_roll);
|
---|
| 75 | label_pitch->SetText("trim pitch: %.2f", trim_pitch);
|
---|
[7] | 76 | }
|
---|
| 77 |
|
---|
[15] | 78 | JoyReference_impl::~JoyReference_impl(void) {}
|
---|
[7] | 79 |
|
---|
| 80 | void JoyReference_impl::SetRollAxis(float value) {
|
---|
[15] | 81 | input->SetValue(0, 0, value);
|
---|
[7] | 82 | }
|
---|
| 83 |
|
---|
| 84 | void JoyReference_impl::SetPitchAxis(float value) {
|
---|
[15] | 85 | input->SetValue(1, 0, value);
|
---|
[7] | 86 | }
|
---|
| 87 |
|
---|
| 88 | void JoyReference_impl::SetYawAxis(float value) {
|
---|
[15] | 89 | input->SetValue(2, 0, value);
|
---|
[7] | 90 | }
|
---|
| 91 |
|
---|
| 92 | void JoyReference_impl::SetAltitudeAxis(float value) {
|
---|
[15] | 93 | input->SetValue(3, 0, value);
|
---|
[7] | 94 | }
|
---|
| 95 |
|
---|
| 96 | void JoyReference_impl::RollTrimUp(void) {
|
---|
[15] | 97 | trim_roll += trim->Value();
|
---|
| 98 | output->SetValue(2, 0, trim_roll);
|
---|
| 99 | label_roll->SetText("trim roll: %.2f", trim_roll);
|
---|
[7] | 100 | }
|
---|
| 101 |
|
---|
| 102 | void JoyReference_impl::RollTrimDown(void) {
|
---|
[15] | 103 | trim_roll -= trim->Value();
|
---|
| 104 | output->SetValue(2, 0, trim_roll);
|
---|
| 105 | label_roll->SetText("trim roll: %.2f", trim_roll);
|
---|
[7] | 106 | }
|
---|
| 107 |
|
---|
| 108 | void JoyReference_impl::PitchTrimUp(void) {
|
---|
[15] | 109 | trim_pitch += trim->Value();
|
---|
| 110 | output->SetValue(3, 0, trim_pitch);
|
---|
| 111 | label_pitch->SetText("trim pitch: %.2f", trim_pitch);
|
---|
[7] | 112 | }
|
---|
| 113 |
|
---|
| 114 | void JoyReference_impl::PitchTrimDown(void) {
|
---|
[15] | 115 | trim_pitch -= trim->Value();
|
---|
| 116 | output->SetValue(3, 0, trim_pitch);
|
---|
| 117 | label_pitch->SetText("trim pitch: %.2f", trim_pitch);
|
---|
[7] | 118 | }
|
---|
| 119 |
|
---|
| 120 | void JoyReference_impl::SetYawRef(float value) {
|
---|
[15] | 121 | Euler ref(0, 0, value);
|
---|
| 122 | input->GetMutex();
|
---|
| 123 | ref.ToQuaternion(q_z);
|
---|
| 124 | input->ReleaseMutex();
|
---|
[7] | 125 |
|
---|
[15] | 126 | Update(GetTime());
|
---|
[7] | 127 | }
|
---|
| 128 |
|
---|
| 129 | void JoyReference_impl::SetZRef(float value) {
|
---|
[15] | 130 | z_ref = value;
|
---|
| 131 | output->SetValue(0, 0, z_ref);
|
---|
[7] | 132 | }
|
---|
| 133 |
|
---|
[15] | 134 | float JoyReference_impl::ZRef(void) const { return output->Value(0, 0); }
|
---|
[7] | 135 |
|
---|
[15] | 136 | float JoyReference_impl::dZRef(void) const { return output->Value(1, 0); }
|
---|
[7] | 137 |
|
---|
[15] | 138 | float JoyReference_impl::RollTrim(void) const { return trim_roll; }
|
---|
[7] | 139 |
|
---|
[15] | 140 | float JoyReference_impl::PitchTrim(void) const { return trim_pitch; }
|
---|
[7] | 141 |
|
---|
| 142 | void JoyReference_impl::Update(Time time) {
|
---|
[15] | 143 | input->SetDataTime(time);
|
---|
| 144 | UpdateFrom(input);
|
---|
[7] | 145 | }
|
---|
| 146 |
|
---|
| 147 | void JoyReference_impl::UpdateFrom(const io_data *data) {
|
---|
[15] | 148 | cvmatrix *input = (cvmatrix *)data;
|
---|
[7] | 149 |
|
---|
[15] | 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();
|
---|
[7] | 154 |
|
---|
[15] | 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 | }
|
---|
[7] | 165 |
|
---|
[15] | 166 | // les box sont en degrés
|
---|
| 167 | input->GetMutex();
|
---|
[7] | 168 |
|
---|
[15] | 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();
|
---|
[7] | 178 |
|
---|
[15] | 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();
|
---|
[7] | 184 |
|
---|
[15] | 185 | Quaternion q_ref = q_z * q_xy;
|
---|
| 186 | q_ref.Normalize();
|
---|
[7] | 187 |
|
---|
[15] | 188 | z_ref += input->ValueNoMutex(3, 0) * deb_dz->Value() * delta_t;
|
---|
| 189 | float dz_ref = input->ValueNoMutex(3, 0) * deb_dz->Value();
|
---|
[7] | 190 |
|
---|
[15] | 191 | input->ReleaseMutex();
|
---|
[7] | 192 |
|
---|
[15] | 193 | ahrsData->SetQuaternionAndAngularRates(q_ref, Vector3D(0, 0, wz_ref));
|
---|
[7] | 194 |
|
---|
[15] | 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();
|
---|
[7] | 200 |
|
---|
[15] | 201 | output->SetDataTime(data->DataTime());
|
---|
[7] | 202 | }
|
---|