[43] | 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)
|
---|
| 25 | using namespace std;
|
---|
| 26 | using namespace flair::core;
|
---|
| 27 | using namespace flair::gui;
|
---|
| 28 | using namespace flair::meta;
|
---|
| 29 | using namespace flair::sensor;
|
---|
| 30 |
|
---|
| 31 | MyApp::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 |
|
---|
| 40 | MyApp::~MyApp() {
|
---|
| 41 | }
|
---|
| 42 |
|
---|
| 43 | //this method is called by UavStateMachine::Run (main loop) when OrientationMode is Custom
|
---|
| 44 | AhrsData *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 |
|
---|
| 58 | void 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 |
|
---|
| 73 | void 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 |
|
---|
| 83 | void 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 |
|
---|
| 93 | void 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 |
|
---|
| 105 | void MyApp::StopCustomAngles(void) {
|
---|
| 106 | //just ask to enter fail safe mode
|
---|
| 107 | EnterFailSafeMode();
|
---|
| 108 | }
|
---|