source: flair-src/tags/latest/demos/Skeletons/CustomReferenceAngles/src/MyApp.cpp@ 133

Last change on this file since 133 was 43, checked in by Sanahuja Guillaume, 8 years ago

skeletons

File size: 3.3 KB
Line 
1// created: 2015/10/27
2// filename: MyApp.cpp
3//
4// author: Guillaume Sanahuja
5// Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7// version: $Id: $
8//
9// purpose: skeleton to use UavStateMachine with custom reference angles only
10//
11//
12/*********************************************************************/
13
14//include files, add yours
15#include "MyApp.h"
16#include <GridLayout.h>
17#include <PushButton.h>
18#include <MetaDualShock3.h>
19#include <FrameworkManager.h>
20#include <AhrsData.h>
21#include <Ahrs.h>
22#include <Uav.h>
23
24//namespaces, add others if necessary (filter, sensor, actuator)
25using namespace std;
26using namespace flair::core;
27using namespace flair::gui;
28using namespace flair::meta;
29using namespace flair::sensor;
30
31MyApp::MyApp(Uav* uav,TargetController *controller): UavStateMachine(uav,controller), behaviourMode(BehaviourMode_t::Default) {
32 start_CustomAngles=new PushButton(GetButtonsLayout()->NewRow(),"start CustomReferenceAngles");
33 stop_CustomAngles=new PushButton(GetButtonsLayout()->NewRow(),"stop CustomReferenceAngles");
34
35 customReferenceOrientation= new AhrsData(this,"reference");
36 uav->GetAhrs()->AddPlot(customReferenceOrientation,DataPlot::Yellow);
37 AddDataToControlLawLog(customReferenceOrientation);
38}
39
40MyApp::~MyApp() {
41}
42
43//this method is called by UavStateMachine::Run (main loop) when OrientationMode is Custom
44AhrsData *MyApp::GetReferenceOrientation(void) {
45 //compute the reference angles (for example depending on a position error)
46 //reference angular speed is set to 0
47
48 Euler refAngles;
49 //refAngles.roll=;
50 //refAngles.pitch=;
51 //refAngles.yaw=;
52
53 customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(),Vector3D(0,0,0));
54
55 return customReferenceOrientation;
56}
57
58void MyApp::SignalEvent(Event_t event) {
59 UavStateMachine::SignalEvent(event);
60 switch(event) {
61 case Event_t::TakingOff:
62 //always take off in default mode
63 behaviourMode=BehaviourMode_t::Default;
64 break;
65 case Event_t::EnteringFailSafeMode:
66 //return to default mode
67 Thread::Info("CustomReferenceAngles: stop\n");
68 behaviourMode=BehaviourMode_t::Default;
69 break;
70 }
71}
72
73void MyApp::ExtraCheckPushButton(void) {
74 if(start_CustomAngles->Clicked() && (behaviourMode!=BehaviourMode_t::CustomReferenceAngles)) {
75 StartCustomAngles();
76 }
77
78 if(stop_CustomAngles->Clicked() && (behaviourMode==BehaviourMode_t::CustomReferenceAngles)) {
79 StopCustomAngles();
80 }
81}
82
83void MyApp::ExtraCheckJoystick(void) {
84 //R1
85 if(GetJoystick()->IsButtonPressed(9) && (behaviourMode!=BehaviourMode_t::CustomReferenceAngles)) {
86 StartCustomAngles();
87 }
88
89 //stop is not managed here, it is done in UavStateMachine with cross button
90 //pushing cross button will enter fail safe mode and signal the EnteringFailSafeMode event
91}
92
93void MyApp::StartCustomAngles(void) {
94 //ask UavStateMachine to enter in custom orientation mode
95 if (SetOrientationMode(OrientationMode_t::Custom)) {
96 Thread::Info("CustomReferenceAngles: start\n");
97 } else {
98 Thread::Warn("CustomReferenceAngles: could not start\n");
99 return;
100 }
101
102 behaviourMode=BehaviourMode_t::CustomReferenceAngles;
103}
104
105void MyApp::StopCustomAngles(void) {
106 //just ask to enter fail safe mode
107 EnterFailSafeMode();
108}
Note: See TracBrowser for help on using the repository browser.