[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);
|
---|
[148] | 51 | delete desc;
|
---|
[7] | 52 |
|
---|
[15] | 53 | reglages_groupbox = new GroupBox(position, name);
|
---|
| 54 | deb_roll = new DoubleSpinBox(reglages_groupbox->NewRow(), "debattement roll",
|
---|
| 55 | " deg", -45, 45, 1, 0);
|
---|
| 56 | deb_pitch = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
|
---|
| 57 | "debattement pitch", " deg", -45, 45, 1, 0);
|
---|
| 58 | deb_wz = new DoubleSpinBox(reglages_groupbox->NewRow(), "debattement wz",
|
---|
| 59 | " deg/s", -180, 180, 1, 0);
|
---|
| 60 | deb_dz = new DoubleSpinBox(reglages_groupbox->LastRowLastCol(),
|
---|
| 61 | "debattement dz", " m/s", -2, 2, 0.1, 1);
|
---|
| 62 | trim = new DoubleSpinBox(reglages_groupbox->NewRow(), "trim", -1, 1, 0.01);
|
---|
| 63 | label_roll = new Label(reglages_groupbox->NewRow(), "trim roll");
|
---|
| 64 | button_roll =
|
---|
| 65 | new PushButton(reglages_groupbox->LastRowLastCol(), "reset roll trim");
|
---|
| 66 | label_pitch = new Label(reglages_groupbox->NewRow(), "trim pitch");
|
---|
| 67 | button_pitch =
|
---|
| 68 | new PushButton(reglages_groupbox->LastRowLastCol(), "reset pitch trim");
|
---|
[7] | 69 |
|
---|
[15] | 70 | z_ref = 0;
|
---|
| 71 | previous_time = 0;
|
---|
| 72 | trim_roll = 0;
|
---|
| 73 | trim_pitch = 0;
|
---|
[7] | 74 |
|
---|
[15] | 75 | label_roll->SetText("trim roll: %.2f", trim_roll);
|
---|
| 76 | label_pitch->SetText("trim pitch: %.2f", trim_pitch);
|
---|
[7] | 77 | }
|
---|
| 78 |
|
---|
[15] | 79 | JoyReference_impl::~JoyReference_impl(void) {}
|
---|
[7] | 80 |
|
---|
| 81 | void JoyReference_impl::SetRollAxis(float value) {
|
---|
[15] | 82 | input->SetValue(0, 0, value);
|
---|
[7] | 83 | }
|
---|
| 84 |
|
---|
| 85 | void JoyReference_impl::SetPitchAxis(float value) {
|
---|
[15] | 86 | input->SetValue(1, 0, value);
|
---|
[7] | 87 | }
|
---|
| 88 |
|
---|
| 89 | void JoyReference_impl::SetYawAxis(float value) {
|
---|
[15] | 90 | input->SetValue(2, 0, value);
|
---|
[7] | 91 | }
|
---|
| 92 |
|
---|
| 93 | void JoyReference_impl::SetAltitudeAxis(float value) {
|
---|
[15] | 94 | input->SetValue(3, 0, value);
|
---|
[7] | 95 | }
|
---|
| 96 |
|
---|
| 97 | void JoyReference_impl::RollTrimUp(void) {
|
---|
[15] | 98 | trim_roll += trim->Value();
|
---|
| 99 | output->SetValue(2, 0, trim_roll);
|
---|
| 100 | label_roll->SetText("trim roll: %.2f", trim_roll);
|
---|
[7] | 101 | }
|
---|
| 102 |
|
---|
| 103 | void JoyReference_impl::RollTrimDown(void) {
|
---|
[15] | 104 | trim_roll -= trim->Value();
|
---|
| 105 | output->SetValue(2, 0, trim_roll);
|
---|
| 106 | label_roll->SetText("trim roll: %.2f", trim_roll);
|
---|
[7] | 107 | }
|
---|
| 108 |
|
---|
| 109 | void JoyReference_impl::PitchTrimUp(void) {
|
---|
[15] | 110 | trim_pitch += trim->Value();
|
---|
| 111 | output->SetValue(3, 0, trim_pitch);
|
---|
| 112 | label_pitch->SetText("trim pitch: %.2f", trim_pitch);
|
---|
[7] | 113 | }
|
---|
| 114 |
|
---|
| 115 | void JoyReference_impl::PitchTrimDown(void) {
|
---|
[15] | 116 | trim_pitch -= trim->Value();
|
---|
| 117 | output->SetValue(3, 0, trim_pitch);
|
---|
| 118 | label_pitch->SetText("trim pitch: %.2f", trim_pitch);
|
---|
[7] | 119 | }
|
---|
| 120 |
|
---|
| 121 | void JoyReference_impl::SetYawRef(float value) {
|
---|
[15] | 122 | Euler ref(0, 0, value);
|
---|
| 123 | input->GetMutex();
|
---|
| 124 | ref.ToQuaternion(q_z);
|
---|
| 125 | input->ReleaseMutex();
|
---|
[7] | 126 |
|
---|
[15] | 127 | Update(GetTime());
|
---|
[7] | 128 | }
|
---|
| 129 |
|
---|
| 130 | void JoyReference_impl::SetZRef(float value) {
|
---|
[15] | 131 | z_ref = value;
|
---|
| 132 | output->SetValue(0, 0, z_ref);
|
---|
[7] | 133 | }
|
---|
| 134 |
|
---|
[15] | 135 | float JoyReference_impl::ZRef(void) const { return output->Value(0, 0); }
|
---|
[7] | 136 |
|
---|
[15] | 137 | float JoyReference_impl::dZRef(void) const { return output->Value(1, 0); }
|
---|
[7] | 138 |
|
---|
[15] | 139 | float JoyReference_impl::RollTrim(void) const { return trim_roll; }
|
---|
[7] | 140 |
|
---|
[15] | 141 | float JoyReference_impl::PitchTrim(void) const { return trim_pitch; }
|
---|
[7] | 142 |
|
---|
| 143 | void JoyReference_impl::Update(Time time) {
|
---|
[15] | 144 | input->SetDataTime(time);
|
---|
| 145 | UpdateFrom(input);
|
---|
[7] | 146 | }
|
---|
| 147 |
|
---|
| 148 | void JoyReference_impl::UpdateFrom(const io_data *data) {
|
---|
[15] | 149 | cvmatrix *input = (cvmatrix *)data;
|
---|
[7] | 150 |
|
---|
[15] | 151 | if (previous_time == 0)
|
---|
| 152 | previous_time = data->DataTime(); // pour la premiere iteration
|
---|
| 153 | float delta_t = (float)(data->DataTime() - previous_time) / 1000000000.;
|
---|
| 154 | previous_time = data->DataTime();
|
---|
[7] | 155 |
|
---|
[15] | 156 | if (button_roll->Clicked() == true) {
|
---|
| 157 | trim_roll = 0;
|
---|
| 158 | output->SetValue(2, 0, 0);
|
---|
| 159 | label_roll->SetText("trim roll: %.2f", trim_roll);
|
---|
| 160 | }
|
---|
| 161 | if (button_pitch->Clicked() == true) {
|
---|
| 162 | trim_pitch = 0;
|
---|
| 163 | output->SetValue(3, 0, 0);
|
---|
| 164 | label_pitch->SetText("trim pitch: %.2f", trim_pitch);
|
---|
| 165 | }
|
---|
[7] | 166 |
|
---|
[15] | 167 | // les box sont en degrés
|
---|
| 168 | input->GetMutex();
|
---|
[7] | 169 |
|
---|
[167] | 170 | Vector3Df theta_xy(
|
---|
[15] | 171 | -Euler::ToRadian(input->ValueNoMutex(0, 0) * deb_roll->Value()),
|
---|
| 172 | -Euler::ToRadian(input->ValueNoMutex(1, 0) * deb_pitch->Value()), 0);
|
---|
[167] | 173 | Vector3Df e_bar = theta_xy;
|
---|
[15] | 174 | e_bar.Normalize();
|
---|
| 175 | Quaternion q_xy(cos(theta_xy.GetNorm() / 2.0f),
|
---|
| 176 | e_bar.x * sin(theta_xy.GetNorm() / 2.0f),
|
---|
| 177 | e_bar.y * sin(theta_xy.GetNorm() / 2.0f), 0);
|
---|
| 178 | q_xy.Normalize();
|
---|
[7] | 179 |
|
---|
[15] | 180 | float wz_ref = Euler::ToRadian(input->ValueNoMutex(2, 0) * deb_wz->Value());
|
---|
| 181 | Quaternion w_zd(1, 0, 0, wz_ref * delta_t / 2);
|
---|
| 182 | w_zd.Normalize();
|
---|
| 183 | q_z = q_z * w_zd;
|
---|
| 184 | q_z.Normalize();
|
---|
[7] | 185 |
|
---|
[15] | 186 | Quaternion q_ref = q_z * q_xy;
|
---|
| 187 | q_ref.Normalize();
|
---|
[7] | 188 |
|
---|
[15] | 189 | z_ref += input->ValueNoMutex(3, 0) * deb_dz->Value() * delta_t;
|
---|
| 190 | float dz_ref = input->ValueNoMutex(3, 0) * deb_dz->Value();
|
---|
[7] | 191 |
|
---|
[15] | 192 | input->ReleaseMutex();
|
---|
[7] | 193 |
|
---|
[167] | 194 | ahrsData->SetQuaternionAndAngularRates(q_ref, Vector3Df(0, 0, wz_ref));
|
---|
[7] | 195 |
|
---|
[15] | 196 | // ouput quaternion for control law
|
---|
| 197 | output->GetMutex();
|
---|
| 198 | output->SetValueNoMutex(0, 0, z_ref);
|
---|
| 199 | output->SetValueNoMutex(1, 0, dz_ref);
|
---|
| 200 | output->ReleaseMutex();
|
---|
[7] | 201 |
|
---|
[15] | 202 | output->SetDataTime(data->DataTime());
|
---|
[7] | 203 | }
|
---|