source: flair-src/trunk/demos/Skeletons/CustomReferenceAngles/src/MyApp.cpp @ 314

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

maj joystick

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(TargetController *controller): UavStateMachine(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    GetUav()->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(),Vector3Df(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(GetTargetController()->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.