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

Last change on this file since 314 was 314, checked in by Sanahuja Guillaume, 3 years ago

maj joystick

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 (GetTargetController()->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.