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

Last change on this file was 318, checked in by Sanahuja Guillaume, 2 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.