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

Last change on this file since 98 was 44, checked in by Bayard Gildas, 8 years ago

Demo Pid et controller automatique scripté

File size: 3.9 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 <cvmatrix.h>
17#include <GridLayout.h>
18#include <DataPlot1D.h>
19#include <Tab.h>
20#include <TabWidget.h>
21#include <GroupBox.h>
22#include <DoubleSpinBox.h>
23#include <FrameworkManager.h>
24#include <MetaDualShock3.h>
25#include <Pid.h>
26#include <AhrsData.h>
27
28#include <stdio.h>
29
30using namespace std;
31using namespace framework::core;
32using namespace framework::sensor;
33using namespace framework::gui;
34using namespace framework::filter;
35using namespace framework::meta;
36
37/*
38** ===================================================================
39** constructor and destructor
40**
41** ===================================================================
42*/
43PidUav::PidUav(Uav* uav,TargetController *controller): behaviourMode(BehaviourMode_t::Default),UavStateMachine(uav,controller) {
44 my_uRoll=new Pid(setupLawTab->At(1,1),"custom uRoll");
45 my_uRoll->UseDefaultPlot(graphLawTab->LastRowLastCol());
46 my_uPitch=new Pid(setupLawTab->At(1,0),"custom uPitch");
47 my_uPitch->UseDefaultPlot(graphLawTab->NewRow());
48}
49
50void PidUav::SignalEvent(Event_t event) {
51 UavStateMachine::SignalEvent(event);
52 switch(event) {
53 case Event_t::TakingOff:
54 //always take off in default mode
55 behaviourMode=BehaviourMode_t::Default;
56 break;
57 case Event_t::EnteringFailSafeMode:
58 //return to default mode
59 Thread::Info("Entering failSafe mode\n");
60 behaviourMode=BehaviourMode_t::Default;
61 break;
62 }
63}
64
65bool PidUav::StartCustomMode() {
66 //ask UavStateMachine to enter in custom torques
67 if (SetTorqueMode(TorqueMode_t::Custom)) {
68 Thread::Info("CustomTorques: start\n");
69 my_uPitch->Reset();
70 my_uRoll->Reset();
71 return true;
72 } else {
73 Thread::Warn("CustomTorques: could not start\n");
74 return false;
75 }
76}
77
78void PidUav::StartOscillatingMode() {
79 if (behaviourMode==BehaviourMode_t::Default) {
80 if (!StartCustomMode()) return;
81 }
82 behaviourMode=BehaviourMode_t::Oscillating;
83 Thread::Info("Entering oscillating mode\n");
84 //remove the 'D' effect with a strong 'P'
85
86}
87
88void PidUav::ExtraCheckJoystick(void) {
89 static bool wasOscillatingModeButtonPressed=false;
90 // controller button R1 enters optical flow mode
91 if(GetJoystick()->IsButtonPressed(9)) { // R1
92 if (!wasOscillatingModeButtonPressed) {
93 wasOscillatingModeButtonPressed=true;
94 if (behaviourMode!=BehaviourMode_t::Oscillating) StartOscillatingMode();
95 }
96 } else {
97 wasOscillatingModeButtonPressed=false;
98 }
99}
100
101void PidUav::ComputeCustomTorques(Euler &torques) {
102 ComputeDefaultTorques(torques);
103 //overload default torques calculation for pitch and roll
104 const AhrsData *refOrientation=GetDefaultReferenceOrientation();
105 Quaternion refQuaternion;
106 Vector3D refAngularRates;
107 refOrientation->GetQuaternionAndAngularRates(refQuaternion,refAngularRates);
108 Euler refAngles=refQuaternion.ToEuler();
109
110 const AhrsData *currentOrientation=GetDefaultOrientation();
111 Quaternion currentQuaternion;
112 Vector3D currentAngularRates;
113 currentOrientation->GetQuaternionAndAngularRates(currentQuaternion,currentAngularRates);
114 Euler currentAngles=currentQuaternion.ToEuler();
115
116 my_uRoll->SetValues(currentAngles.roll-refAngles.roll,currentAngularRates.x);
117 my_uRoll->Update(GetTime());
118 torques.roll=my_uRoll->Output();
119
120 my_uPitch->SetValues(currentAngles.pitch-refAngles.pitch,currentAngularRates.y);
121 my_uPitch->Update(GetTime());
122 torques.pitch=my_uPitch->Output();
123}
124
125PidUav::~PidUav() {
126}
Note: See TracBrowser for help on using the repository browser.