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

Last change on this file since 312 was 236, checked in by Sanahuja Guillaume, 5 years ago

change build directories for new architecture

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