source: flair-src/trunk/demos/PidStandalone/uav/src/PidUav.cpp

Last change on this file was 318, checked in by Sanahuja Guillaume, 3 years ago
File size: 3.8 KB
Line 
1// created: 2016/06/01
2// filename: PidUav.cpp
3//
4// author: Gildas Bayard
5// Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7// version: $Id: $
8//
9// purpose: Uav class showing the incidence of pid parameters choice
10//
11//
12/*********************************************************************/
13
14#include "PidUav.h"
15#include <TargetController.h>
16#include <GridLayout.h>
17#include <DataPlot1D.h>
18#include <Tab.h>
19#include <TabWidget.h>
20#include <GroupBox.h>
21#include <DoubleSpinBox.h>
22#include <FrameworkManager.h>
23#include <MetaDualShock3.h>
24#include <Pid.h>
25#include <AhrsData.h>
26
27#include <stdio.h>
28
29using namespace std;
30using namespace flair::core;
31using namespace flair::sensor;
32using namespace flair::gui;
33using namespace flair::filter;
34using namespace flair::meta;
35
36/*
37** ===================================================================
38** constructor and destructor
39**
40** ===================================================================
41*/
42PidUav::PidUav(Uav *uav, TargetController *controller)
43 : behaviourMode(BehaviourMode_t::Default),
44 UavStateMachine(controller) {
45 my_uRoll = new Pid(setupLawTab->At(1, 1), "custom uRoll");
46 my_uRoll->UseDefaultPlot(graphLawTab->LastRowLastCol());
47 my_uPitch = new Pid(setupLawTab->At(1, 0), "custom uPitch");
48 my_uPitch->UseDefaultPlot(graphLawTab->NewRow());
49}
50
51void PidUav::SignalEvent(Event_t event) {
52 UavStateMachine::SignalEvent(event);
53 switch (event) {
54 case Event_t::TakingOff:
55 // always take off in default mode
56 behaviourMode = BehaviourMode_t::Default;
57 break;
58 case Event_t::EnteringFailSafeMode:
59 // return to default mode
60 Thread::Info("Entering failSafe mode\n");
61 behaviourMode = BehaviourMode_t::Default;
62 break;
63 }
64}
65
66bool PidUav::StartCustomMode() {
67 // ask UavStateMachine to enter in custom torques
68 if (SetTorqueMode(TorqueMode_t::Custom)) {
69 Thread::Info("CustomTorques: start\n");
70 my_uPitch->Reset();
71 my_uRoll->Reset();
72 return true;
73 } else {
74 Thread::Warn("CustomTorques: could not start\n");
75 return false;
76 }
77}
78
79void PidUav::StartOscillatingMode() {
80 if (behaviourMode == BehaviourMode_t::Default) {
81 if (!StartCustomMode())
82 return;
83 }
84 behaviourMode = BehaviourMode_t::Oscillating;
85 Thread::Info("Entering oscillating mode\n");
86 // remove the 'D' effect with a strong 'P'
87}
88
89void PidUav::ExtraCheckJoystick(void) {
90 static bool wasOscillatingModeButtonPressed = false;
91 // controller button R1 enters optical flow mode
92 if (GetTargetController()->IsButtonPressed(9)) { // R1
93 if (!wasOscillatingModeButtonPressed) {
94 wasOscillatingModeButtonPressed = true;
95 if (behaviourMode != BehaviourMode_t::Oscillating)
96 StartOscillatingMode();
97 }
98 } else {
99 wasOscillatingModeButtonPressed = false;
100 }
101}
102
103void PidUav::ComputeCustomTorques(Euler &torques) {
104 ComputeDefaultTorques(torques);
105 // overload default torques calculation for pitch and roll
106 const AhrsData *refOrientation = GetDefaultReferenceOrientation();
107 Quaternion refQuaternion;
108 Vector3Df refAngularRates;
109 refOrientation->GetQuaternionAndAngularRates(refQuaternion, refAngularRates);
110 Euler refAngles = refQuaternion.ToEuler();
111
112 const AhrsData *currentOrientation = GetDefaultOrientation();
113 Quaternion currentQuaternion;
114 Vector3Df currentAngularRates;
115 currentOrientation->GetQuaternionAndAngularRates(currentQuaternion,
116 currentAngularRates);
117 Euler currentAngles = currentQuaternion.ToEuler();
118
119 my_uRoll->SetValues(currentAngles.roll - refAngles.roll,
120 currentAngularRates.x);
121 my_uRoll->Update(GetTime());
122 torques.roll = my_uRoll->Output();
123
124 my_uPitch->SetValues(currentAngles.pitch - refAngles.pitch,
125 currentAngularRates.y);
126 my_uPitch->Update(GetTime());
127 torques.pitch = my_uPitch->Output();
128}
129
130PidUav::~PidUav() {}
Note: See TracBrowser for help on using the repository browser.